From 725287a284b09f4b25bf7c3260267c83639bd56b Mon Sep 17 00:00:00 2001 From: mirage54321 Date: Tue, 18 Nov 2025 15:39:59 -0800 Subject: [PATCH] Initial commit --- .gitattributes | 2 + .github/workflows/gradle.yml | 23 + .gitignore | 187 +++++++ .vscode/launch.json | 27 + .vscode/settings.json | 62 +++ .wpilib/wpilib_preferences.json | 6 + README.md | 2 + WPILib-License.md | 24 + build.gradle | 124 +++++ gradle/wrapper/gradle-wrapper.jar | Bin 0 -> 43583 bytes gradle/wrapper/gradle-wrapper.properties | 7 + gradlew | 252 +++++++++ gradlew.bat | 94 ++++ networktables.json | 1 + settings.gradle | 30 ++ simgui-ds.json | 112 ++++ src/main/deploy/example.txt | 3 + src/main/java/frc4388/robot/Main.java | 29 ++ src/main/java/frc4388/robot/Robot.java | 264 ++++++++++ .../java/frc4388/robot/RobotContainer.java | 221 ++++++++ src/main/java/frc4388/robot/RobotMap.java | 175 +++++++ .../Autos/neo AutoRecoding format.txt | 20 + .../commands/Autos/neoPlaybackChooser.java | 108 ++++ .../robot/commands/MoveForTimeCommand.java | 49 ++ .../robot/commands/MoveUntilSuply.java | 45 ++ src/main/java/frc4388/robot/commands/PID.java | 60 +++ .../robot/commands/Swerve/RotateToAngle.java | 35 ++ .../commands/Swerve/neoJoystickPlayback.java | 197 +++++++ .../commands/Swerve/neoJoystickRecorder.java | 129 +++++ .../robot/commands/WhileTrueCommand.java | 104 ++++ .../robot/commands/alignment/DriveToReef.java | 188 +++++++ .../commands/alignment/DriveUntilLiDAR.java | 48 ++ .../robot/commands/alignment/LidarAlign.java | 107 ++++ .../commands/wait/waitElevatorRefrence.java | 36 ++ .../commands/wait/waitEndefectorRefrence.java | 36 ++ .../robot/commands/wait/waitFeedCoral.java | 36 ++ .../robot/commands/wait/waitSupplier.java | 36 ++ .../robot/constants/BuildConstants.java | 19 + .../frc4388/robot/constants/Constants.java | 146 ++++++ .../java/frc4388/robot/subsystems/LED.java | 75 +++ .../frc4388/robot/subsystems/lidar/LiDAR.java | 58 +++ .../robot/subsystems/lidar/LidarIO.java | 13 + .../robot/subsystems/lidar/LidarReal.java | 27 + .../robot/subsystems/swerve/SwerveDrive.java | 467 +++++++++++++++++ .../swerve/SwerveDriveConstants.java | 246 +++++++++ .../robot/subsystems/swerve/SwerveIO.java | 33 ++ .../robot/subsystems/swerve/SwerveReal.java | 63 +++ .../robot/subsystems/vision/Vision.java | 95 ++++ .../robot/subsystems/vision/VisionIO.java | 22 + .../robot/subsystems/vision/VisionReal.java | 158 ++++++ .../java/frc4388/utility/DeferredBlock.java | 41 ++ .../frc4388/utility/compute/DataUtils.java | 35 ++ .../utility/compute/ReefPositionHelper.java | 106 ++++ .../frc4388/utility/compute/RobotTime.java | 79 +++ .../frc4388/utility/compute/RobotUnits.java | 27 + .../utility/compute/TimesNegativeOne.java | 51 ++ .../java/frc4388/utility/compute/Trim.java | 145 ++++++ .../configurable/ConfigurableDouble.java | 23 + .../configurable/ConfigurableString.java | 23 + .../frc4388/utility/controller/ButtonBox.java | 19 + .../controller/DeadbandedXboxController.java | 28 + .../utility/controller/IHandController.java | 21 + .../utility/controller/VirtualController.java | 145 ++++++ .../utility/controller/XboxController.java | 218 ++++++++ .../utility/controller/XboxTriggerButton.java | 68 +++ .../java/frc4388/utility/status/Alerts.java | 10 + .../frc4388/utility/status/CanDevice.java | 53 ++ .../frc4388/utility/status/FaultCANCoder.java | 76 +++ .../utility/status/FaultPhotonCamera.java | 35 ++ .../frc4388/utility/status/FaultPidgeon2.java | 168 ++++++ .../frc4388/utility/status/FaultReporter.java | 133 +++++ .../frc4388/utility/status/FaultTalonFX.java | 154 ++++++ .../frc4388/utility/status/QueryUtils.java | 13 + .../frc4388/utility/status/Queryable.java | 8 + .../java/frc4388/utility/status/Status.java | 85 ++++ .../java/frc4388/utility/structs/Gains.java | 83 +++ .../frc4388/utility/structs/LEDPatterns.java | 45 ++ .../utility/structs/UtilityStructs.java | 26 + .../java/frc4388/mocks/MockPigeonIMU.java.old | 54 ++ .../robot/subsystems/LEDSubsystemTest.old | 59 +++ .../frc4388/utility/RobotGyroUtilityTest.old | 184 +++++++ .../frc4388/utility/RobotTimeUtilityTest.old | 104 ++++ vendordeps/AdvantageKit.json | 35 ++ vendordeps/NavX.json | 40 ++ vendordeps/PathplannerLib-2025.2.7.json | 38 ++ vendordeps/Phoenix6-frc2025-latest.json | 479 ++++++++++++++++++ vendordeps/WPILibNewCommands.json | 38 ++ vendordeps/photonlib.json | 71 +++ 88 files changed, 7321 insertions(+) create mode 100644 .gitattributes create mode 100644 .github/workflows/gradle.yml create mode 100644 .gitignore create mode 100644 .vscode/launch.json create mode 100644 .vscode/settings.json create mode 100644 .wpilib/wpilib_preferences.json create mode 100644 README.md create mode 100644 WPILib-License.md create mode 100644 build.gradle create mode 100644 gradle/wrapper/gradle-wrapper.jar create mode 100644 gradle/wrapper/gradle-wrapper.properties create mode 100755 gradlew create mode 100644 gradlew.bat create mode 100644 networktables.json create mode 100644 settings.gradle create mode 100644 simgui-ds.json create mode 100644 src/main/deploy/example.txt create mode 100644 src/main/java/frc4388/robot/Main.java create mode 100644 src/main/java/frc4388/robot/Robot.java create mode 100644 src/main/java/frc4388/robot/RobotContainer.java create mode 100644 src/main/java/frc4388/robot/RobotMap.java create mode 100644 src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt create mode 100644 src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java create mode 100644 src/main/java/frc4388/robot/commands/MoveForTimeCommand.java create mode 100644 src/main/java/frc4388/robot/commands/MoveUntilSuply.java create mode 100644 src/main/java/frc4388/robot/commands/PID.java create mode 100644 src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java create mode 100644 src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java create mode 100644 src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java create mode 100644 src/main/java/frc4388/robot/commands/WhileTrueCommand.java create mode 100644 src/main/java/frc4388/robot/commands/alignment/DriveToReef.java create mode 100644 src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java create mode 100644 src/main/java/frc4388/robot/commands/alignment/LidarAlign.java create mode 100644 src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java create mode 100644 src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java create mode 100644 src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java create mode 100644 src/main/java/frc4388/robot/commands/wait/waitSupplier.java create mode 100644 src/main/java/frc4388/robot/constants/BuildConstants.java create mode 100644 src/main/java/frc4388/robot/constants/Constants.java create mode 100644 src/main/java/frc4388/robot/subsystems/LED.java create mode 100644 src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java create mode 100644 src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java create mode 100644 src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java create mode 100644 src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java create mode 100644 src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java create mode 100644 src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java create mode 100644 src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java create mode 100644 src/main/java/frc4388/robot/subsystems/vision/Vision.java create mode 100644 src/main/java/frc4388/robot/subsystems/vision/VisionIO.java create mode 100644 src/main/java/frc4388/robot/subsystems/vision/VisionReal.java create mode 100644 src/main/java/frc4388/utility/DeferredBlock.java create mode 100644 src/main/java/frc4388/utility/compute/DataUtils.java create mode 100644 src/main/java/frc4388/utility/compute/ReefPositionHelper.java create mode 100644 src/main/java/frc4388/utility/compute/RobotTime.java create mode 100644 src/main/java/frc4388/utility/compute/RobotUnits.java create mode 100644 src/main/java/frc4388/utility/compute/TimesNegativeOne.java create mode 100644 src/main/java/frc4388/utility/compute/Trim.java create mode 100644 src/main/java/frc4388/utility/configurable/ConfigurableDouble.java create mode 100644 src/main/java/frc4388/utility/configurable/ConfigurableString.java create mode 100644 src/main/java/frc4388/utility/controller/ButtonBox.java create mode 100644 src/main/java/frc4388/utility/controller/DeadbandedXboxController.java create mode 100644 src/main/java/frc4388/utility/controller/IHandController.java create mode 100644 src/main/java/frc4388/utility/controller/VirtualController.java create mode 100644 src/main/java/frc4388/utility/controller/XboxController.java create mode 100644 src/main/java/frc4388/utility/controller/XboxTriggerButton.java create mode 100644 src/main/java/frc4388/utility/status/Alerts.java create mode 100644 src/main/java/frc4388/utility/status/CanDevice.java create mode 100644 src/main/java/frc4388/utility/status/FaultCANCoder.java create mode 100644 src/main/java/frc4388/utility/status/FaultPhotonCamera.java create mode 100644 src/main/java/frc4388/utility/status/FaultPidgeon2.java create mode 100644 src/main/java/frc4388/utility/status/FaultReporter.java create mode 100644 src/main/java/frc4388/utility/status/FaultTalonFX.java create mode 100644 src/main/java/frc4388/utility/status/QueryUtils.java create mode 100644 src/main/java/frc4388/utility/status/Queryable.java create mode 100644 src/main/java/frc4388/utility/status/Status.java create mode 100644 src/main/java/frc4388/utility/structs/Gains.java create mode 100644 src/main/java/frc4388/utility/structs/LEDPatterns.java create mode 100644 src/main/java/frc4388/utility/structs/UtilityStructs.java create mode 100644 src/test/java/frc4388/mocks/MockPigeonIMU.java.old create mode 100644 src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.old create mode 100644 src/test/java/frc4388/utility/RobotGyroUtilityTest.old create mode 100644 src/test/java/frc4388/utility/RobotTimeUtilityTest.old create mode 100644 vendordeps/AdvantageKit.json create mode 100644 vendordeps/NavX.json create mode 100644 vendordeps/PathplannerLib-2025.2.7.json create mode 100644 vendordeps/Phoenix6-frc2025-latest.json create mode 100644 vendordeps/WPILibNewCommands.json create mode 100644 vendordeps/photonlib.json diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..dfe0770 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,2 @@ +# Auto detect text files and perform LF normalization +* text=auto diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml new file mode 100644 index 0000000..7d89e58 --- /dev/null +++ b/.github/workflows/gradle.yml @@ -0,0 +1,23 @@ +name: Java CI + +on: + push: + branches: + - master + pull_request: + branches: + - master + +jobs: + build: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v1 + - name: Set up JDK 17 + uses: actions/setup-java@v1 + with: + java-version: 17 + - name: Change wrapper permissions + run: chmod +x ./gradlew + - name: Build with Gradle + run: ./gradlew build diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..9a9ca7b --- /dev/null +++ b/.gitignore @@ -0,0 +1,187 @@ +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +# DO NOT REMOVE .classpath and .project +.classpath +.project +.settings/ +bin/ + +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + +# Simulation GUI and other tools window save file +networktables.json +simgui.json +*-window.json + +# Simulation data log directory +logs/ + +# Folder that has CTRE Phoenix Sim device config storage +ctre_sim/ + +# clangd +/.cache +compile_commands.json + +# Eclipse generated file for annotation processors +.factorypath diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..fb77e69 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,27 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + { + "type": "java", + "name": "Main", + "request": "launch", + "mainClass": "frc4388.robot.Main", + "projectName": "2025RidgeScape" + }, + { + "type": "wpilib", + "name": "WPILib Desktop Debug", + "request": "launch", + "desktop": true + }, + { + "type": "wpilib", + "name": "WPILib roboRIO Debug", + "request": "launch", + "desktop": false + } + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..ef24bd2 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,62 @@ +{ + "java.configuration.updateBuildConfiguration": "automatic", + "java.server.launchMode": "Standard", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true, + "**/*~": true + }, + "java.test.config": [ + { + "name": "WPIlibUnitTests", + "workingDirectory": "${workspaceFolder}/build/jni/release", + "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], + "env": { + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } + }, + ], + "java.test.defaultConfig": "WPIlibUnitTests", + "java.import.gradle.annotationProcessing.enabled": false, + "java.completion.favoriteStaticMembers": [ + "org.junit.Assert.*", + "org.junit.Assume.*", + "org.junit.jupiter.api.Assertions.*", + "org.junit.jupiter.api.Assumptions.*", + "org.junit.jupiter.api.DynamicContainer.*", + "org.junit.jupiter.api.DynamicTest.*", + "org.mockito.Mockito.*", + "org.mockito.ArgumentMatchers.*", + "org.mockito.Answers.*", + "edu.wpi.first.units.Units.*" + ], + "java.completion.filteredTypes": [ + "java.awt.*", + "com.sun.*", + "sun.*", + "jdk.*", + "org.graalvm.*", + "io.micrometer.shaded.*", + "java.beans.*", + "java.util.Base64.*", + "java.util.Timer", + "java.sql.*", + "javax.swing.*", + "javax.management.*", + "javax.smartcardio.*", + "edu.wpi.first.math.proto.*", + "edu.wpi.first.math.**.proto.*", + "edu.wpi.first.math.**.struct.*", + ], + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable", + "wpilib.autoStartRioLog": false +} diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..60cf2c2 --- /dev/null +++ b/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2025", + "teamNumber": 4388 +} \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..b659de7 --- /dev/null +++ b/README.md @@ -0,0 +1,2 @@ +# Robot-Essentials + Basic code for any Ridgebotics robot project \ No newline at end of file diff --git a/WPILib-License.md b/WPILib-License.md new file mode 100644 index 0000000..e7cd597 --- /dev/null +++ b/WPILib-License.md @@ -0,0 +1,24 @@ +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 new file mode 100644 index 0000000..e8723ce --- /dev/null +++ b/build.gradle @@ -0,0 +1,124 @@ +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2025.3.2" + id "com.peterabeles.gversion" version "1.10" +} + +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} + +def ROBOT_MAIN_CLASS = "frc4388.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' + 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() + + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" +} + +test { + 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' +} + +task(replayWatch, type: JavaExec) { + mainClass = "org.littletonrobotics.junction.ReplayWatch" + classpath = sourceSets.main.runtimeClasspath +} + +// Create version file +project.compileJava.dependsOn(createVersionFile) +gversion { + srcDir = "src/main/java/" + classPackage = "frc4388.robot.constants" + className = "BuildConstants" + dateFormat = "yyyy-MM-dd HH:mm:ss z" + timeZone = "America/Denver" + indent = " " +} diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000000000000000000000000000000000000..a4b76b9530d66f5e68d973ea569d8e19de379189 GIT binary patch literal 43583 zcma&N1CXTcmMvW9vTb(Rwr$&4wr$(C?dmSu>@vG-+vuvg^_??!{yS%8zW-#zn-LkA z5&1^$^{lnmUON?}LBF8_K|(?T0Ra(xUH{($5eN!MR#ZihR#HxkUPe+_R8Cn`RRs(P z_^*#_XlXmGv7!4;*Y%p4nw?{bNp@UZHv1?Um8r6)Fei3p@ClJn0ECfg1hkeuUU@Or zDaPa;U3fE=3L}DooL;8f;P0ipPt0Z~9P0)lbStMS)ag54=uL9ia-Lm3nh|@(Y?B`; zx_#arJIpXH!U{fbCbI^17}6Ri*H<>OLR%c|^mh8+)*h~K8Z!9)DPf zR2h?lbDZQ`p9P;&DQ4F0sur@TMa!Y}S8irn(%d-gi0*WxxCSk*A?3lGh=gcYN?FGl z7D=Js!i~0=u3rox^eO3i@$0=n{K1lPNU zwmfjRVmLOCRfe=seV&P*1Iq=^i`502keY8Uy-WNPwVNNtJFx?IwAyRPZo2Wo1+S(xF37LJZ~%i)kpFQ3Fw=mXfd@>%+)RpYQLnr}B~~zoof(JVm^^&f zxKV^+3D3$A1G;qh4gPVjhrC8e(VYUHv#dy^)(RoUFM?o%W-EHxufuWf(l*@-l+7vt z=l`qmR56K~F|v<^Pd*p~1_y^P0P^aPC##d8+HqX4IR1gu+7w#~TBFphJxF)T$2WEa zxa?H&6=Qe7d(#tha?_1uQys2KtHQ{)Qco)qwGjrdNL7thd^G5i8Os)CHqc>iOidS} z%nFEDdm=GXBw=yXe1W-ShHHFb?Cc70+$W~z_+}nAoHFYI1MV1wZegw*0y^tC*s%3h zhD3tN8b=Gv&rj}!SUM6|ajSPp*58KR7MPpI{oAJCtY~JECm)*m_x>AZEu>DFgUcby z1Qaw8lU4jZpQ_$;*7RME+gq1KySGG#Wql>aL~k9tLrSO()LWn*q&YxHEuzmwd1?aAtI zBJ>P=&$=l1efe1CDU;`Fd+_;&wI07?V0aAIgc(!{a z0Jg6Y=inXc3^n!U0Atk`iCFIQooHqcWhO(qrieUOW8X(x?(RD}iYDLMjSwffH2~tB z)oDgNBLB^AJBM1M^c5HdRx6fBfka`(LD-qrlh5jqH~);#nw|iyp)()xVYak3;Ybik z0j`(+69aK*B>)e_p%=wu8XC&9e{AO4c~O1U`5X9}?0mrd*m$_EUek{R?DNSh(=br# z#Q61gBzEpmy`$pA*6!87 zSDD+=@fTY7<4A?GLqpA?Pb2z$pbCc4B4zL{BeZ?F-8`s$?>*lXXtn*NC61>|*w7J* z$?!iB{6R-0=KFmyp1nnEmLsA-H0a6l+1uaH^g%c(p{iT&YFrbQ$&PRb8Up#X3@Zsk zD^^&LK~111%cqlP%!_gFNa^dTYT?rhkGl}5=fL{a`UViaXWI$k-UcHJwmaH1s=S$4 z%4)PdWJX;hh5UoK?6aWoyLxX&NhNRqKam7tcOkLh{%j3K^4Mgx1@i|Pi&}<^5>hs5 zm8?uOS>%)NzT(%PjVPGa?X%`N2TQCKbeH2l;cTnHiHppPSJ<7y-yEIiC!P*ikl&!B z%+?>VttCOQM@ShFguHVjxX^?mHX^hSaO_;pnyh^v9EumqSZTi+#f&_Vaija0Q-e*| z7ulQj6Fs*bbmsWp{`auM04gGwsYYdNNZcg|ph0OgD>7O}Asn7^Z=eI>`$2*v78;sj-}oMoEj&@)9+ycEOo92xSyY344^ z11Hb8^kdOvbf^GNAK++bYioknrpdN>+u8R?JxG=!2Kd9r=YWCOJYXYuM0cOq^FhEd zBg2puKy__7VT3-r*dG4c62Wgxi52EMCQ`bKgf*#*ou(D4-ZN$+mg&7$u!! z-^+Z%;-3IDwqZ|K=ah85OLwkO zKxNBh+4QHh)u9D?MFtpbl)us}9+V!D%w9jfAMYEb>%$A;u)rrI zuBudh;5PN}_6J_}l55P3l_)&RMlH{m!)ai-i$g)&*M`eN$XQMw{v^r@-125^RRCF0 z^2>|DxhQw(mtNEI2Kj(;KblC7x=JlK$@78`O~>V!`|1Lm-^JR$-5pUANAnb(5}B}JGjBsliK4& zk6y(;$e&h)lh2)L=bvZKbvh@>vLlreBdH8No2>$#%_Wp1U0N7Ank!6$dFSi#xzh|( zRi{Uw%-4W!{IXZ)fWx@XX6;&(m_F%c6~X8hx=BN1&q}*( zoaNjWabE{oUPb!Bt$eyd#$5j9rItB-h*5JiNi(v^e|XKAj*8(k<5-2$&ZBR5fF|JA z9&m4fbzNQnAU}r8ab>fFV%J0z5awe#UZ|bz?Ur)U9bCIKWEzi2%A+5CLqh?}K4JHi z4vtM;+uPsVz{Lfr;78W78gC;z*yTch~4YkLr&m-7%-xc ztw6Mh2d>_iO*$Rd8(-Cr1_V8EO1f*^@wRoSozS) zy1UoC@pruAaC8Z_7~_w4Q6n*&B0AjOmMWa;sIav&gu z|J5&|{=a@vR!~k-OjKEgPFCzcJ>#A1uL&7xTDn;{XBdeM}V=l3B8fE1--DHjSaxoSjNKEM9|U9#m2<3>n{Iuo`r3UZp;>GkT2YBNAh|b z^jTq-hJp(ebZh#Lk8hVBP%qXwv-@vbvoREX$TqRGTgEi$%_F9tZES@z8Bx}$#5eeG zk^UsLBH{bc2VBW)*EdS({yw=?qmevwi?BL6*=12k9zM5gJv1>y#ML4!)iiPzVaH9% zgSImetD@dam~e>{LvVh!phhzpW+iFvWpGT#CVE5TQ40n%F|p(sP5mXxna+Ev7PDwA zamaV4m*^~*xV+&p;W749xhb_X=$|LD;FHuB&JL5?*Y2-oIT(wYY2;73<^#46S~Gx| z^cez%V7x$81}UWqS13Gz80379Rj;6~WdiXWOSsdmzY39L;Hg3MH43o*y8ibNBBH`(av4|u;YPq%{R;IuYow<+GEsf@R?=@tT@!}?#>zIIn0CoyV!hq3mw zHj>OOjfJM3F{RG#6ujzo?y32m^tgSXf@v=J$ELdJ+=5j|=F-~hP$G&}tDZsZE?5rX ztGj`!S>)CFmdkccxM9eGIcGnS2AfK#gXwj%esuIBNJQP1WV~b~+D7PJTmWGTSDrR` zEAu4B8l>NPuhsk5a`rReSya2nfV1EK01+G!x8aBdTs3Io$u5!6n6KX%uv@DxAp3F@{4UYg4SWJtQ-W~0MDb|j-$lwVn znAm*Pl!?Ps&3wO=R115RWKb*JKoexo*)uhhHBncEDMSVa_PyA>k{Zm2(wMQ(5NM3# z)jkza|GoWEQo4^s*wE(gHz?Xsg4`}HUAcs42cM1-qq_=+=!Gk^y710j=66(cSWqUe zklbm8+zB_syQv5A2rj!Vbw8;|$@C!vfNmNV!yJIWDQ>{+2x zKjuFX`~~HKG~^6h5FntRpnnHt=D&rq0>IJ9#F0eM)Y-)GpRjiN7gkA8wvnG#K=q{q z9dBn8_~wm4J<3J_vl|9H{7q6u2A!cW{bp#r*-f{gOV^e=8S{nc1DxMHFwuM$;aVI^ zz6A*}m8N-&x8;aunp1w7_vtB*pa+OYBw=TMc6QK=mbA-|Cf* zvyh8D4LRJImooUaSb7t*fVfih<97Gf@VE0|z>NcBwBQze);Rh!k3K_sfunToZY;f2 z^HmC4KjHRVg+eKYj;PRN^|E0>Gj_zagfRbrki68I^#~6-HaHg3BUW%+clM1xQEdPYt_g<2K+z!$>*$9nQ>; zf9Bei{?zY^-e{q_*|W#2rJG`2fy@{%6u0i_VEWTq$*(ZN37|8lFFFt)nCG({r!q#9 z5VK_kkSJ3?zOH)OezMT{!YkCuSSn!K#-Rhl$uUM(bq*jY? zi1xbMVthJ`E>d>(f3)~fozjg^@eheMF6<)I`oeJYx4*+M&%c9VArn(OM-wp%M<-`x z7sLP1&3^%Nld9Dhm@$3f2}87!quhI@nwd@3~fZl_3LYW-B?Ia>ui`ELg z&Qfe!7m6ze=mZ`Ia9$z|ARSw|IdMpooY4YiPN8K z4B(ts3p%2i(Td=tgEHX z0UQ_>URBtG+-?0E;E7Ld^dyZ;jjw0}XZ(}-QzC6+NN=40oDb2^v!L1g9xRvE#@IBR zO!b-2N7wVfLV;mhEaXQ9XAU+>=XVA6f&T4Z-@AX!leJ8obP^P^wP0aICND?~w&NykJ#54x3_@r7IDMdRNy4Hh;h*!u(Ol(#0bJdwEo$5437-UBjQ+j=Ic>Q2z` zJNDf0yO6@mr6y1#n3)s(W|$iE_i8r@Gd@!DWDqZ7J&~gAm1#~maIGJ1sls^gxL9LLG_NhU!pTGty!TbhzQnu)I*S^54U6Yu%ZeCg`R>Q zhBv$n5j0v%O_j{QYWG!R9W?5_b&67KB$t}&e2LdMvd(PxN6Ir!H4>PNlerpBL>Zvyy!yw z-SOo8caEpDt(}|gKPBd$qND5#a5nju^O>V&;f890?yEOfkSG^HQVmEbM3Ugzu+UtH zC(INPDdraBN?P%kE;*Ae%Wto&sgw(crfZ#Qy(<4nk;S|hD3j{IQRI6Yq|f^basLY; z-HB&Je%Gg}Jt@={_C{L$!RM;$$|iD6vu#3w?v?*;&()uB|I-XqEKqZPS!reW9JkLewLb!70T7n`i!gNtb1%vN- zySZj{8-1>6E%H&=V}LM#xmt`J3XQoaD|@XygXjdZ1+P77-=;=eYpoEQ01B@L*a(uW zrZeZz?HJsw_4g0vhUgkg@VF8<-X$B8pOqCuWAl28uB|@r`19DTUQQsb^pfqB6QtiT z*`_UZ`fT}vtUY#%sq2{rchyfu*pCg;uec2$-$N_xgjZcoumE5vSI{+s@iLWoz^Mf; zuI8kDP{!XY6OP~q5}%1&L}CtfH^N<3o4L@J@zg1-mt{9L`s^z$Vgb|mr{@WiwAqKg zp#t-lhrU>F8o0s1q_9y`gQNf~Vb!F%70f}$>i7o4ho$`uciNf=xgJ>&!gSt0g;M>*x4-`U)ysFW&Vs^Vk6m%?iuWU+o&m(2Jm26Y(3%TL; zA7T)BP{WS!&xmxNw%J=$MPfn(9*^*TV;$JwRy8Zl*yUZi8jWYF>==j~&S|Xinsb%c z2?B+kpet*muEW7@AzjBA^wAJBY8i|#C{WtO_or&Nj2{=6JTTX05}|H>N2B|Wf!*3_ z7hW*j6p3TvpghEc6-wufFiY!%-GvOx*bZrhZu+7?iSrZL5q9}igiF^*R3%DE4aCHZ zqu>xS8LkW+Auv%z-<1Xs92u23R$nk@Pk}MU5!gT|c7vGlEA%G^2th&Q*zfg%-D^=f z&J_}jskj|Q;73NP4<4k*Y%pXPU2Thoqr+5uH1yEYM|VtBPW6lXaetokD0u z9qVek6Q&wk)tFbQ8(^HGf3Wp16gKmr>G;#G(HRBx?F`9AIRboK+;OfHaLJ(P>IP0w zyTbTkx_THEOs%Q&aPrxbZrJlio+hCC_HK<4%f3ZoSAyG7Dn`=X=&h@m*|UYO-4Hq0 z-Bq&+Ie!S##4A6OGoC~>ZW`Y5J)*ouaFl_e9GA*VSL!O_@xGiBw!AF}1{tB)z(w%c zS1Hmrb9OC8>0a_$BzeiN?rkPLc9%&;1CZW*4}CDDNr2gcl_3z+WC15&H1Zc2{o~i) z)LLW=WQ{?ricmC`G1GfJ0Yp4Dy~Ba;j6ZV4r{8xRs`13{dD!xXmr^Aga|C=iSmor% z8hi|pTXH)5Yf&v~exp3o+sY4B^^b*eYkkCYl*T{*=-0HniSA_1F53eCb{x~1k3*`W zr~};p1A`k{1DV9=UPnLDgz{aJH=-LQo<5%+Em!DNN252xwIf*wF_zS^!(XSm(9eoj z=*dXG&n0>)_)N5oc6v!>-bd(2ragD8O=M|wGW z!xJQS<)u70m&6OmrF0WSsr@I%T*c#Qo#Ha4d3COcX+9}hM5!7JIGF>7<~C(Ear^Sn zm^ZFkV6~Ula6+8S?oOROOA6$C&q&dp`>oR-2Ym3(HT@O7Sd5c~+kjrmM)YmgPH*tL zX+znN>`tv;5eOfX?h{AuX^LK~V#gPCu=)Tigtq9&?7Xh$qN|%A$?V*v=&-2F$zTUv z`C#WyIrChS5|Kgm_GeudCFf;)!WH7FI60j^0o#65o6`w*S7R@)88n$1nrgU(oU0M9 zx+EuMkC>(4j1;m6NoGqEkpJYJ?vc|B zOlwT3t&UgL!pX_P*6g36`ZXQ; z9~Cv}ANFnJGp(;ZhS(@FT;3e)0)Kp;h^x;$*xZn*k0U6-&FwI=uOGaODdrsp-!K$Ac32^c{+FhI-HkYd5v=`PGsg%6I`4d9Jy)uW0y%) zm&j^9WBAp*P8#kGJUhB!L?a%h$hJgQrx!6KCB_TRo%9{t0J7KW8!o1B!NC)VGLM5! zpZy5Jc{`r{1e(jd%jsG7k%I+m#CGS*BPA65ZVW~fLYw0dA-H_}O zrkGFL&P1PG9p2(%QiEWm6x;U-U&I#;Em$nx-_I^wtgw3xUPVVu zqSuKnx&dIT-XT+T10p;yjo1Y)z(x1fb8Dzfn8e yu?e%!_ptzGB|8GrCfu%p?(_ zQccdaaVK$5bz;*rnyK{_SQYM>;aES6Qs^lj9lEs6_J+%nIiuQC*fN;z8md>r_~Mfl zU%p5Dt_YT>gQqfr@`cR!$NWr~+`CZb%dn;WtzrAOI>P_JtsB76PYe*<%H(y>qx-`Kq!X_; z<{RpAqYhE=L1r*M)gNF3B8r(<%8mo*SR2hu zccLRZwGARt)Hlo1euqTyM>^!HK*!Q2P;4UYrysje@;(<|$&%vQekbn|0Ruu_Io(w4#%p6ld2Yp7tlA`Y$cciThP zKzNGIMPXX%&Ud0uQh!uQZz|FB`4KGD?3!ND?wQt6!n*f4EmCoJUh&b?;B{|lxs#F- z31~HQ`SF4x$&v00@(P+j1pAaj5!s`)b2RDBp*PB=2IB>oBF!*6vwr7Dp%zpAx*dPr zb@Zjq^XjN?O4QcZ*O+8>)|HlrR>oD*?WQl5ri3R#2?*W6iJ>>kH%KnnME&TT@ZzrHS$Q%LC?n|e>V+D+8D zYc4)QddFz7I8#}y#Wj6>4P%34dZH~OUDb?uP%-E zwjXM(?Sg~1!|wI(RVuxbu)-rH+O=igSho_pDCw(c6b=P zKk4ATlB?bj9+HHlh<_!&z0rx13K3ZrAR8W)!@Y}o`?a*JJsD+twZIv`W)@Y?Amu_u zz``@-e2X}27$i(2=9rvIu5uTUOVhzwu%mNazS|lZb&PT;XE2|B&W1>=B58#*!~D&) zfVmJGg8UdP*fx(>Cj^?yS^zH#o-$Q-*$SnK(ZVFkw+er=>N^7!)FtP3y~Xxnu^nzY zikgB>Nj0%;WOltWIob|}%lo?_C7<``a5hEkx&1ku$|)i>Rh6@3h*`slY=9U}(Ql_< zaNG*J8vb&@zpdhAvv`?{=zDedJ23TD&Zg__snRAH4eh~^oawdYi6A3w8<Ozh@Kw)#bdktM^GVb zrG08?0bG?|NG+w^&JvD*7LAbjED{_Zkc`3H!My>0u5Q}m!+6VokMLXxl`Mkd=g&Xx z-a>m*#G3SLlhbKB!)tnzfWOBV;u;ftU}S!NdD5+YtOjLg?X}dl>7m^gOpihrf1;PY zvll&>dIuUGs{Qnd- zwIR3oIrct8Va^Tm0t#(bJD7c$Z7DO9*7NnRZorrSm`b`cxz>OIC;jSE3DO8`hX955ui`s%||YQtt2 z5DNA&pG-V+4oI2s*x^>-$6J?p=I>C|9wZF8z;VjR??Icg?1w2v5Me+FgAeGGa8(3S z4vg*$>zC-WIVZtJ7}o9{D-7d>zCe|z#<9>CFve-OPAYsneTb^JH!Enaza#j}^mXy1 z+ULn^10+rWLF6j2>Ya@@Kq?26>AqK{A_| zQKb*~F1>sE*=d?A?W7N2j?L09_7n+HGi{VY;MoTGr_)G9)ot$p!-UY5zZ2Xtbm=t z@dpPSGwgH=QtIcEulQNI>S-#ifbnO5EWkI;$A|pxJd885oM+ zGZ0_0gDvG8q2xebj+fbCHYfAXuZStH2j~|d^sBAzo46(K8n59+T6rzBwK)^rfPT+B zyIFw)9YC-V^rhtK`!3jrhmW-sTmM+tPH+;nwjL#-SjQPUZ53L@A>y*rt(#M(qsiB2 zx6B)dI}6Wlsw%bJ8h|(lhkJVogQZA&n{?Vgs6gNSXzuZpEyu*xySy8ro07QZ7Vk1!3tJphN_5V7qOiyK8p z#@jcDD8nmtYi1^l8ml;AF<#IPK?!pqf9D4moYk>d99Im}Jtwj6c#+A;f)CQ*f-hZ< z=p_T86jog%!p)D&5g9taSwYi&eP z#JuEK%+NULWus;0w32-SYFku#i}d~+{Pkho&^{;RxzP&0!RCm3-9K6`>KZpnzS6?L z^H^V*s!8<>x8bomvD%rh>Zp3>Db%kyin;qtl+jAv8Oo~1g~mqGAC&Qi_wy|xEt2iz zWAJEfTV%cl2Cs<1L&DLRVVH05EDq`pH7Oh7sR`NNkL%wi}8n>IXcO40hp+J+sC!W?!krJf!GJNE8uj zg-y~Ns-<~D?yqbzVRB}G>0A^f0!^N7l=$m0OdZuqAOQqLc zX?AEGr1Ht+inZ-Qiwnl@Z0qukd__a!C*CKuGdy5#nD7VUBM^6OCpxCa2A(X;e0&V4 zM&WR8+wErQ7UIc6LY~Q9x%Sn*Tn>>P`^t&idaOEnOd(Ufw#>NoR^1QdhJ8s`h^|R_ zXX`c5*O~Xdvh%q;7L!_!ohf$NfEBmCde|#uVZvEo>OfEq%+Ns7&_f$OR9xsihRpBb z+cjk8LyDm@U{YN>+r46?nn{7Gh(;WhFw6GAxtcKD+YWV?uge>;+q#Xx4!GpRkVZYu zzsF}1)7$?%s9g9CH=Zs+B%M_)+~*j3L0&Q9u7!|+T`^O{xE6qvAP?XWv9_MrZKdo& z%IyU)$Q95AB4!#hT!_dA>4e@zjOBD*Y=XjtMm)V|+IXzjuM;(l+8aA5#Kaz_$rR6! zj>#&^DidYD$nUY(D$mH`9eb|dtV0b{S>H6FBfq>t5`;OxA4Nn{J(+XihF(stSche7$es&~N$epi&PDM_N`As;*9D^L==2Q7Z2zD+CiU(|+-kL*VG+&9!Yb3LgPy?A zm7Z&^qRG_JIxK7-FBzZI3Q<;{`DIxtc48k> zc|0dmX;Z=W$+)qE)~`yn6MdoJ4co;%!`ddy+FV538Y)j(vg}5*k(WK)KWZ3WaOG!8 z!syGn=s{H$odtpqFrT#JGM*utN7B((abXnpDM6w56nhw}OY}0TiTG1#f*VFZr+^-g zbP10`$LPq_;PvrA1XXlyx2uM^mrjTzX}w{yuLo-cOClE8MMk47T25G8M!9Z5ypOSV zAJUBGEg5L2fY)ZGJb^E34R2zJ?}Vf>{~gB!8=5Z) z9y$>5c)=;o0HeHHSuE4U)#vG&KF|I%-cF6f$~pdYJWk_dD}iOA>iA$O$+4%@>JU08 zS`ep)$XLPJ+n0_i@PkF#ri6T8?ZeAot$6JIYHm&P6EB=BiaNY|aA$W0I+nz*zkz_z zkEru!tj!QUffq%)8y0y`T&`fuus-1p>=^hnBiBqD^hXrPs`PY9tU3m0np~rISY09> z`P3s=-kt_cYcxWd{de@}TwSqg*xVhp;E9zCsnXo6z z?f&Sv^U7n4`xr=mXle94HzOdN!2kB~4=%)u&N!+2;z6UYKUDqi-s6AZ!haB;@&B`? z_TRX0%@suz^TRdCb?!vNJYPY8L_}&07uySH9%W^Tc&1pia6y1q#?*Drf}GjGbPjBS zbOPcUY#*$3sL2x4v_i*Y=N7E$mR}J%|GUI(>WEr+28+V z%v5{#e!UF*6~G&%;l*q*$V?&r$Pp^sE^i-0$+RH3ERUUdQ0>rAq2(2QAbG}$y{de( z>{qD~GGuOk559Y@%$?N^1ApVL_a704>8OD%8Y%8B;FCt%AoPu8*D1 zLB5X>b}Syz81pn;xnB}%0FnwazlWfUV)Z-~rZg6~b z6!9J$EcE&sEbzcy?CI~=boWA&eeIa%z(7SE^qgVLz??1Vbc1*aRvc%Mri)AJaAG!p z$X!_9Ds;Zz)f+;%s&dRcJt2==P{^j3bf0M=nJd&xwUGlUFn?H=2W(*2I2Gdu zv!gYCwM10aeus)`RIZSrCK=&oKaO_Ry~D1B5!y0R=%!i2*KfXGYX&gNv_u+n9wiR5 z*e$Zjju&ODRW3phN925%S(jL+bCHv6rZtc?!*`1TyYXT6%Ju=|X;6D@lq$8T zW{Y|e39ioPez(pBH%k)HzFITXHvnD6hw^lIoUMA;qAJ^CU?top1fo@s7xT13Fvn1H z6JWa-6+FJF#x>~+A;D~;VDs26>^oH0EI`IYT2iagy23?nyJ==i{g4%HrAf1-*v zK1)~@&(KkwR7TL}L(A@C_S0G;-GMDy=MJn2$FP5s<%wC)4jC5PXoxrQBFZ_k0P{{s@sz+gX`-!=T8rcB(=7vW}^K6oLWMmp(rwDh}b zwaGGd>yEy6fHv%jM$yJXo5oMAQ>c9j`**}F?MCry;T@47@r?&sKHgVe$MCqk#Z_3S z1GZI~nOEN*P~+UaFGnj{{Jo@16`(qVNtbU>O0Hf57-P>x8Jikp=`s8xWs^dAJ9lCQ z)GFm+=OV%AMVqVATtN@|vp61VVAHRn87}%PC^RAzJ%JngmZTasWBAWsoAqBU+8L8u z4A&Pe?fmTm0?mK-BL9t+{y7o(7jm+RpOhL9KnY#E&qu^}B6=K_dB}*VlSEiC9fn)+V=J;OnN)Ta5v66ic1rG+dGAJ1 z1%Zb_+!$=tQ~lxQrzv3x#CPb?CekEkA}0MYSgx$Jdd}q8+R=ma$|&1a#)TQ=l$1tQ z=tL9&_^vJ)Pk}EDO-va`UCT1m#Uty1{v^A3P~83_#v^ozH}6*9mIjIr;t3Uv%@VeW zGL6(CwCUp)Jq%G0bIG%?{_*Y#5IHf*5M@wPo6A{$Um++Co$wLC=J1aoG93&T7Ho}P z=mGEPP7GbvoG!uD$k(H3A$Z))+i{Hy?QHdk>3xSBXR0j!11O^mEe9RHmw!pvzv?Ua~2_l2Yh~_!s1qS`|0~0)YsbHSz8!mG)WiJE| z2f($6TQtt6L_f~ApQYQKSb=`053LgrQq7G@98#igV>y#i==-nEjQ!XNu9 z~;mE+gtj4IDDNQJ~JVk5Ux6&LCSFL!y=>79kE9=V}J7tD==Ga+IW zX)r7>VZ9dY=V&}DR))xUoV!u(Z|%3ciQi_2jl}3=$Agc(`RPb z8kEBpvY>1FGQ9W$n>Cq=DIpski};nE)`p3IUw1Oz0|wxll^)4dq3;CCY@RyJgFgc# zKouFh!`?Xuo{IMz^xi-h=StCis_M7yq$u) z?XHvw*HP0VgR+KR6wI)jEMX|ssqYvSf*_3W8zVTQzD?3>H!#>InzpSO)@SC8q*ii- z%%h}_#0{4JG;Jm`4zg};BPTGkYamx$Xo#O~lBirRY)q=5M45n{GCfV7h9qwyu1NxOMoP4)jjZMxmT|IQQh0U7C$EbnMN<3)Kk?fFHYq$d|ICu>KbY_hO zTZM+uKHe(cIZfEqyzyYSUBZa8;Fcut-GN!HSA9ius`ltNebF46ZX_BbZNU}}ZOm{M2&nANL9@0qvih15(|`S~z}m&h!u4x~(%MAO$jHRWNfuxWF#B)E&g3ghSQ9|> z(MFaLQj)NE0lowyjvg8z0#m6FIuKE9lDO~Glg}nSb7`~^&#(Lw{}GVOS>U)m8bF}x zVjbXljBm34Cs-yM6TVusr+3kYFjr28STT3g056y3cH5Tmge~ASxBj z%|yb>$eF;WgrcOZf569sDZOVwoo%8>XO>XQOX1OyN9I-SQgrm;U;+#3OI(zrWyow3 zk==|{lt2xrQ%FIXOTejR>;wv(Pb8u8}BUpx?yd(Abh6? zsoO3VYWkeLnF43&@*#MQ9-i-d0t*xN-UEyNKeyNMHw|A(k(_6QKO=nKMCxD(W(Yop zsRQ)QeL4X3Lxp^L%wzi2-WVSsf61dqliPUM7srDB?Wm6Lzn0&{*}|IsKQW;02(Y&| zaTKv|`U(pSzuvR6Rduu$wzK_W-Y-7>7s?G$)U}&uK;<>vU}^^ns@Z!p+9?St1s)dG zK%y6xkPyyS1$~&6v{kl?Md6gwM|>mt6Upm>oa8RLD^8T{0?HC!Z>;(Bob7el(DV6x zi`I)$&E&ngwFS@bi4^xFLAn`=fzTC;aimE^!cMI2n@Vo%Ae-ne`RF((&5y6xsjjAZ zVguVoQ?Z9uk$2ON;ersE%PU*xGO@T*;j1BO5#TuZKEf(mB7|g7pcEA=nYJ{s3vlbg zd4-DUlD{*6o%Gc^N!Nptgay>j6E5;3psI+C3Q!1ZIbeCubW%w4pq9)MSDyB{HLm|k zxv-{$$A*pS@csolri$Ge<4VZ}e~78JOL-EVyrbxKra^d{?|NnPp86!q>t<&IP07?Z z^>~IK^k#OEKgRH+LjllZXk7iA>2cfH6+(e&9ku5poo~6y{GC5>(bRK7hwjiurqAiZ zg*DmtgY}v83IjE&AbiWgMyFbaRUPZ{lYiz$U^&Zt2YjG<%m((&_JUbZcfJ22(>bi5 z!J?<7AySj0JZ&<-qXX;mcV!f~>G=sB0KnjWca4}vrtunD^1TrpfeS^4dvFr!65knK zZh`d;*VOkPs4*-9kL>$GP0`(M!j~B;#x?Ba~&s6CopvO86oM?-? zOw#dIRc;6A6T?B`Qp%^<U5 z19x(ywSH$_N+Io!6;e?`tWaM$`=Db!gzx|lQ${DG!zb1Zl&|{kX0y6xvO1o z220r<-oaS^^R2pEyY;=Qllqpmue|5yI~D|iI!IGt@iod{Opz@*ml^w2bNs)p`M(Io z|E;;m*Xpjd9l)4G#KaWfV(t8YUn@A;nK^#xgv=LtnArX|vWQVuw3}B${h+frU2>9^ z!l6)!Uo4`5k`<<;E(ido7M6lKTgWezNLq>U*=uz&s=cc$1%>VrAeOoUtA|T6gO4>UNqsdK=NF*8|~*sl&wI=x9-EGiq*aqV!(VVXA57 zw9*o6Ir8Lj1npUXvlevtn(_+^X5rzdR>#(}4YcB9O50q97%rW2me5_L=%ffYPUSRc z!vv?Kv>dH994Qi>U(a<0KF6NH5b16enCp+mw^Hb3Xs1^tThFpz!3QuN#}KBbww`(h z7GO)1olDqy6?T$()R7y%NYx*B0k_2IBiZ14&8|JPFxeMF{vW>HF-Vi3+ZOI=+qP}n zw(+!WcTd~4ZJX1!ZM&y!+uyt=&i!+~d(V%GjH;-NsEEv6nS1TERt|RHh!0>W4+4pp z1-*EzAM~i`+1f(VEHI8So`S`akPfPTfq*`l{Fz`hS%k#JS0cjT2mS0#QLGf=J?1`he3W*;m4)ce8*WFq1sdP=~$5RlH1EdWm|~dCvKOi4*I_96{^95p#B<(n!d?B z=o`0{t+&OMwKcxiBECznJcfH!fL(z3OvmxP#oWd48|mMjpE||zdiTBdWelj8&Qosv zZFp@&UgXuvJw5y=q6*28AtxZzo-UUpkRW%ne+Ylf!V-0+uQXBW=5S1o#6LXNtY5!I z%Rkz#(S8Pjz*P7bqB6L|M#Er{|QLae-Y{KA>`^} z@lPjeX>90X|34S-7}ZVXe{wEei1<{*e8T-Nbj8JmD4iwcE+Hg_zhkPVm#=@b$;)h6 z<<6y`nPa`f3I6`!28d@kdM{uJOgM%`EvlQ5B2bL)Sl=|y@YB3KeOzz=9cUW3clPAU z^sYc}xf9{4Oj?L5MOlYxR{+>w=vJjvbyO5}ptT(o6dR|ygO$)nVCvNGnq(6;bHlBd zl?w-|plD8spjDF03g5ip;W3Z z><0{BCq!Dw;h5~#1BuQilq*TwEu)qy50@+BE4bX28+7erX{BD4H)N+7U`AVEuREE8 z;X?~fyhF-x_sRfHIj~6f(+^@H)D=ngP;mwJjxhQUbUdzk8f94Ab%59-eRIq?ZKrwD z(BFI=)xrUlgu(b|hAysqK<}8bslmNNeD=#JW*}^~Nrswn^xw*nL@Tx!49bfJecV&KC2G4q5a!NSv)06A_5N3Y?veAz;Gv+@U3R% z)~UA8-0LvVE{}8LVDOHzp~2twReqf}ODIyXMM6=W>kL|OHcx9P%+aJGYi_Om)b!xe zF40Vntn0+VP>o<$AtP&JANjXBn7$}C@{+@3I@cqlwR2MdwGhVPxlTIcRVu@Ho-wO` z_~Or~IMG)A_`6-p)KPS@cT9mu9RGA>dVh5wY$NM9-^c@N=hcNaw4ITjm;iWSP^ZX| z)_XpaI61<+La+U&&%2a z0za$)-wZP@mwSELo#3!PGTt$uy0C(nTT@9NX*r3Ctw6J~7A(m#8fE)0RBd`TdKfAT zCf@$MAxjP`O(u9s@c0Fd@|}UQ6qp)O5Q5DPCeE6mSIh|Rj{$cAVIWsA=xPKVKxdhg zLzPZ`3CS+KIO;T}0Ip!fAUaNU>++ZJZRk@I(h<)RsJUhZ&Ru9*!4Ptn;gX^~4E8W^TSR&~3BAZc#HquXn)OW|TJ`CTahk+{qe`5+ixON^zA9IFd8)kc%*!AiLu z>`SFoZ5bW-%7}xZ>gpJcx_hpF$2l+533{gW{a7ce^B9sIdmLrI0)4yivZ^(Vh@-1q zFT!NQK$Iz^xu%|EOK=n>ug;(7J4OnS$;yWmq>A;hsD_0oAbLYhW^1Vdt9>;(JIYjf zdb+&f&D4@4AS?!*XpH>8egQvSVX`36jMd>$+RgI|pEg))^djhGSo&#lhS~9%NuWfX zDDH;3T*GzRT@5=7ibO>N-6_XPBYxno@mD_3I#rDD?iADxX`! zh*v8^i*JEMzyN#bGEBz7;UYXki*Xr(9xXax(_1qVW=Ml)kSuvK$coq2A(5ZGhs_pF z$*w}FbN6+QDseuB9=fdp_MTs)nQf!2SlROQ!gBJBCXD&@-VurqHj0wm@LWX-TDmS= z71M__vAok|@!qgi#H&H%Vg-((ZfxPAL8AI{x|VV!9)ZE}_l>iWk8UPTGHs*?u7RfP z5MC&=c6X;XlUzrz5q?(!eO@~* zoh2I*%J7dF!!_!vXoSIn5o|wj1#_>K*&CIn{qSaRc&iFVxt*^20ngCL;QonIS>I5^ zMw8HXm>W0PGd*}Ko)f|~dDd%;Wu_RWI_d;&2g6R3S63Uzjd7dn%Svu-OKpx*o|N>F zZg=-~qLb~VRLpv`k zWSdfHh@?dp=s_X`{yxOlxE$4iuyS;Z-x!*E6eqmEm*j2bE@=ZI0YZ5%Yj29!5+J$4h{s($nakA`xgbO8w zi=*r}PWz#lTL_DSAu1?f%-2OjD}NHXp4pXOsCW;DS@BC3h-q4_l`<))8WgzkdXg3! zs1WMt32kS2E#L0p_|x+x**TFV=gn`m9BWlzF{b%6j-odf4{7a4y4Uaef@YaeuPhU8 zHBvRqN^;$Jizy+ z=zW{E5<>2gp$pH{M@S*!sJVQU)b*J5*bX4h>5VJve#Q6ga}cQ&iL#=(u+KroWrxa%8&~p{WEUF0il=db;-$=A;&9M{Rq`ouZ5m%BHT6%st%saGsD6)fQgLN}x@d3q>FC;=f%O3Cyg=Ke@Gh`XW za@RajqOE9UB6eE=zhG%|dYS)IW)&y&Id2n7r)6p_)vlRP7NJL(x4UbhlcFXWT8?K=%s7;z?Vjts?y2+r|uk8Wt(DM*73^W%pAkZa1Jd zNoE)8FvQA>Z`eR5Z@Ig6kS5?0h;`Y&OL2D&xnnAUzQz{YSdh0k zB3exx%A2TyI)M*EM6htrxSlep!Kk(P(VP`$p0G~f$smld6W1r_Z+o?=IB@^weq>5VYsYZZR@` z&XJFxd5{|KPZmVOSxc@^%71C@;z}}WhbF9p!%yLj3j%YOlPL5s>7I3vj25 z@xmf=*z%Wb4;Va6SDk9cv|r*lhZ`(y_*M@>q;wrn)oQx%B(2A$9(74>;$zmQ!4fN; z>XurIk-7@wZys<+7XL@0Fhe-f%*=(weaQEdR9Eh6>Kl-EcI({qoZqyzziGwpg-GM#251sK_ z=3|kitS!j%;fpc@oWn65SEL73^N&t>Ix37xgs= zYG%eQDJc|rqHFia0!_sm7`@lvcv)gfy(+KXA@E{3t1DaZ$DijWAcA)E0@X?2ziJ{v z&KOYZ|DdkM{}t+@{@*6ge}m%xfjIxi%qh`=^2Rwz@w0cCvZ&Tc#UmCDbVwABrON^x zEBK43FO@weA8s7zggCOWhMvGGE`baZ62cC)VHyy!5Zbt%ieH+XN|OLbAFPZWyC6)p z4P3%8sq9HdS3=ih^0OOlqTPbKuzQ?lBEI{w^ReUO{V?@`ARsL|S*%yOS=Z%sF)>-y z(LAQdhgAcuF6LQjRYfdbD1g4o%tV4EiK&ElLB&^VZHbrV1K>tHTO{#XTo>)2UMm`2 z^t4s;vnMQgf-njU-RVBRw0P0-m#d-u`(kq7NL&2T)TjI_@iKuPAK-@oH(J8?%(e!0Ir$yG32@CGUPn5w4)+9@8c&pGx z+K3GKESI4*`tYlmMHt@br;jBWTei&(a=iYslc^c#RU3Q&sYp zSG){)V<(g7+8W!Wxeb5zJb4XE{I|&Y4UrFWr%LHkdQ;~XU zgy^dH-Z3lmY+0G~?DrC_S4@=>0oM8Isw%g(id10gWkoz2Q%7W$bFk@mIzTCcIB(K8 zc<5h&ZzCdT=9n-D>&a8vl+=ZF*`uTvQviG_bLde*k>{^)&0o*b05x$MO3gVLUx`xZ z43j+>!u?XV)Yp@MmG%Y`+COH2?nQcMrQ%k~6#O%PeD_WvFO~Kct za4XoCM_X!c5vhRkIdV=xUB3xI2NNStK*8_Zl!cFjOvp-AY=D;5{uXj}GV{LK1~IE2 z|KffUiBaStRr;10R~K2VVtf{TzM7FaPm;Y(zQjILn+tIPSrJh&EMf6evaBKIvi42-WYU9Vhj~3< zZSM-B;E`g_o8_XTM9IzEL=9Lb^SPhe(f(-`Yh=X6O7+6ALXnTcUFpI>ekl6v)ZQeNCg2 z^H|{SKXHU*%nBQ@I3It0m^h+6tvI@FS=MYS$ZpBaG7j#V@P2ZuYySbp@hA# ze(kc;P4i_-_UDP?%<6>%tTRih6VBgScKU^BV6Aoeg6Uh(W^#J^V$Xo^4#Ekp ztqQVK^g9gKMTHvV7nb64UU7p~!B?>Y0oFH5T7#BSW#YfSB@5PtE~#SCCg3p^o=NkMk$<8- z6PT*yIKGrvne7+y3}_!AC8NNeI?iTY(&nakN>>U-zT0wzZf-RuyZk^X9H-DT_*wk= z;&0}6LsGtfVa1q)CEUPlx#(ED@-?H<1_FrHU#z5^P3lEB|qsxEyn%FOpjx z3S?~gvoXy~L(Q{Jh6*i~=f%9kM1>RGjBzQh_SaIDfSU_9!<>*Pm>l)cJD@wlyxpBV z4Fmhc2q=R_wHCEK69<*wG%}mgD1=FHi4h!98B-*vMu4ZGW~%IrYSLGU{^TuseqVgV zLP<%wirIL`VLyJv9XG_p8w@Q4HzNt-o;U@Au{7%Ji;53!7V8Rv0^Lu^Vf*sL>R(;c zQG_ZuFl)Mh-xEIkGu}?_(HwkB2jS;HdPLSxVU&Jxy9*XRG~^HY(f0g8Q}iqnVmgjI zfd=``2&8GsycjR?M%(zMjn;tn9agcq;&rR!Hp z$B*gzHsQ~aXw8c|a(L^LW(|`yGc!qOnV(ZjU_Q-4z1&0;jG&vAKuNG=F|H?@m5^N@ zq{E!1n;)kNTJ>|Hb2ODt-7U~-MOIFo%9I)_@7fnX+eMMNh>)V$IXesJpBn|uo8f~#aOFytCT zf9&%MCLf8mp4kwHTcojWmM3LU=#|{3L>E}SKwOd?%{HogCZ_Z1BSA}P#O(%H$;z7XyJ^sjGX;j5 zrzp>|Ud;*&VAU3x#f{CKwY7Vc{%TKKqmB@oTHA9;>?!nvMA;8+Jh=cambHz#J18x~ zs!dF>$*AnsQ{{82r5Aw&^7eRCdvcgyxH?*DV5(I$qXh^zS>us*I66_MbL8y4d3ULj z{S(ipo+T3Ag!+5`NU2sc+@*m{_X|&p#O-SAqF&g_n7ObB82~$p%fXA5GLHMC+#qqL zdt`sJC&6C2)=juQ_!NeD>U8lDVpAOkW*khf7MCcs$A(wiIl#B9HM%~GtQ^}yBPjT@ z+E=|A!Z?A(rwzZ;T}o6pOVqHzTr*i;Wrc%&36kc@jXq~+w8kVrs;%=IFdACoLAcCAmhFNpbP8;s`zG|HC2Gv?I~w4ITy=g$`0qMQdkijLSOtX6xW%Z9Nw<;M- zMN`c7=$QxN00DiSjbVt9Mi6-pjv*j(_8PyV-il8Q-&TwBwH1gz1uoxs6~uU}PrgWB zIAE_I-a1EqlIaGQNbcp@iI8W1sm9fBBNOk(k&iLBe%MCo#?xI$%ZmGA?=)M9D=0t7 zc)Q0LnI)kCy{`jCGy9lYX%mUsDWwsY`;jE(;Us@gmWPqjmXL+Hu#^;k%eT>{nMtzj zsV`Iy6leTA8-PndszF;N^X@CJrTw5IIm!GPeu)H2#FQitR{1p;MasQVAG3*+=9FYK zw*k!HT(YQorfQj+1*mCV458(T5=fH`um$gS38hw(OqVMyunQ;rW5aPbF##A3fGH6h z@W)i9Uff?qz`YbK4c}JzQpuxuE3pcQO)%xBRZp{zJ^-*|oryTxJ-rR+MXJ)!f=+pp z10H|DdGd2exhi+hftcYbM0_}C0ZI-2vh+$fU1acsB-YXid7O|=9L!3e@$H*6?G*Zp z%qFB(sgl=FcC=E4CYGp4CN>=M8#5r!RU!u+FJVlH6=gI5xHVD&k;Ta*M28BsxfMV~ zLz+@6TxnfLhF@5=yQo^1&S}cmTN@m!7*c6z;}~*!hNBjuE>NLVl2EwN!F+)0$R1S! zR|lF%n!9fkZ@gPW|x|B={V6x3`=jS*$Pu0+5OWf?wnIy>Y1MbbGSncpKO0qE(qO=ts z!~@&!N`10S593pVQu4FzpOh!tvg}p%zCU(aV5=~K#bKi zHdJ1>tQSrhW%KOky;iW+O_n;`l9~omqM%sdxdLtI`TrJzN6BQz+7xOl*rM>xVI2~# z)7FJ^Dc{DC<%~VS?@WXzuOG$YPLC;>#vUJ^MmtbSL`_yXtNKa$Hk+l-c!aC7gn(Cg ze?YPYZ(2Jw{SF6MiO5(%_pTo7j@&DHNW`|lD`~{iH+_eSTS&OC*2WTT*a`?|9w1dh zh1nh@$a}T#WE5$7Od~NvSEU)T(W$p$s5fe^GpG+7fdJ9=enRT9$wEk+ZaB>G3$KQO zgq?-rZZnIv!p#>Ty~}c*Lb_jxJg$eGM*XwHUwuQ|o^}b3^T6Bxx{!?va8aC@-xK*H ztJBFvFfsSWu89%@b^l3-B~O!CXs)I6Y}y#0C0U0R0WG zybjroj$io0j}3%P7zADXOwHwafT#uu*zfM!oD$6aJx7+WL%t-@6^rD_a_M?S^>c;z zMK580bZXo1f*L$CuMeM4Mp!;P@}b~$cd(s5*q~FP+NHSq;nw3fbWyH)i2)-;gQl{S zZO!T}A}fC}vUdskGSq&{`oxt~0i?0xhr6I47_tBc`fqaSrMOzR4>0H^;A zF)hX1nfHs)%Zb-(YGX;=#2R6C{BG;k=?FfP?9{_uFLri~-~AJ;jw({4MU7e*d)?P@ zXX*GkNY9ItFjhwgAIWq7Y!ksbMzfqpG)IrqKx9q{zu%Mdl+{Dis#p9q`02pr1LG8R z@As?eG!>IoROgS!@J*to<27coFc1zpkh?w=)h9CbYe%^Q!Ui46Y*HO0mr% zEff-*$ndMNw}H2a5@BsGj5oFfd!T(F&0$<{GO!Qdd?McKkorh=5{EIjDTHU`So>8V zBA-fqVLb2;u7UhDV1xMI?y>fe3~4urv3%PX)lDw+HYa;HFkaLqi4c~VtCm&Ca+9C~ zge+67hp#R9`+Euq59WhHX&7~RlXn=--m8$iZ~~1C8cv^2(qO#X0?vl91gzUKBeR1J z^p4!!&7)3#@@X&2aF2-)1Ffcc^F8r|RtdL2X%HgN&XU-KH2SLCbpw?J5xJ*!F-ypZ zMG%AJ!Pr&}`LW?E!K~=(NJxuSVTRCGJ$2a*Ao=uUDSys!OFYu!Vs2IT;xQ6EubLIl z+?+nMGeQQhh~??0!s4iQ#gm3!BpMpnY?04kK375e((Uc7B3RMj;wE?BCoQGu=UlZt!EZ1Q*auI)dj3Jj{Ujgt zW5hd~-HWBLI_3HuO) zNrb^XzPsTIb=*a69wAAA3J6AAZZ1VsYbIG}a`=d6?PjM)3EPaDpW2YP$|GrBX{q*! z$KBHNif)OKMBCFP5>!1d=DK>8u+Upm-{hj5o|Wn$vh1&K!lVfDB&47lw$tJ?d5|=B z^(_9=(1T3Fte)z^>|3**n}mIX;mMN5v2F#l(q*CvU{Ga`@VMp#%rQkDBy7kYbmb-q z<5!4iuB#Q_lLZ8}h|hPODI^U6`gzLJre9u3k3c#%86IKI*^H-@I48Bi*@avYm4v!n0+v zWu{M{&F8#p9cx+gF0yTB_<2QUrjMPo9*7^-uP#~gGW~y3nfPAoV%amgr>PSyVAd@l)}8#X zR5zV6t*uKJZL}?NYvPVK6J0v4iVpwiN|>+t3aYiZSp;m0!(1`bHO}TEtWR1tY%BPB z(W!0DmXbZAsT$iC13p4f>u*ZAy@JoLAkJhzFf1#4;#1deO8#8d&89}en&z!W&A3++^1(;>0SB1*54d@y&9Pn;^IAf3GiXbfT`_>{R+Xv; zQvgL>+0#8-laO!j#-WB~(I>l0NCMt_;@Gp_f0#^c)t?&#Xh1-7RR0@zPyBz!U#0Av zT?}n({(p?p7!4S2ZBw)#KdCG)uPnZe+U|0{BW!m)9 zi_9$F?m<`2!`JNFv+w8MK_K)qJ^aO@7-Ig>cM4-r0bi=>?B_2mFNJ}aE3<+QCzRr*NA!QjHw# z`1OsvcoD0?%jq{*7b!l|L1+Tw0TTAM4XMq7*ntc-Ived>Sj_ZtS|uVdpfg1_I9knY z2{GM_j5sDC7(W&}#s{jqbybqJWyn?{PW*&cQIU|*v8YGOKKlGl@?c#TCnmnAkAzV- zmK={|1G90zz=YUvC}+fMqts0d4vgA%t6Jhjv?d;(Z}(Ep8fTZfHA9``fdUHkA+z3+ zhh{ohP%Bj?T~{i0sYCQ}uC#5BwN`skI7`|c%kqkyWIQ;!ysvA8H`b-t()n6>GJj6xlYDu~8qX{AFo$Cm3d|XFL=4uvc?Keb zzb0ZmMoXca6Mob>JqkNuoP>B2Z>D`Q(TvrG6m`j}-1rGP!g|qoL=$FVQYxJQjFn33lODt3Wb1j8VR zlR++vIT6^DtYxAv_hxupbLLN3e0%A%a+hWTKDV3!Fjr^cWJ{scsAdfhpI)`Bms^M6 zQG$waKgFr=c|p9Piug=fcJvZ1ThMnNhQvBAg-8~b1?6wL*WyqXhtj^g(Ke}mEfZVM zJuLNTUVh#WsE*a6uqiz`b#9ZYg3+2%=C(6AvZGc=u&<6??!slB1a9K)=VL zY9EL^mfyKnD zSJyYBc_>G;5RRnrNgzJz#Rkn3S1`mZgO`(r5;Hw6MveN(URf_XS-r58Cn80K)ArH4 z#Rrd~LG1W&@ttw85cjp8xV&>$b%nSXH_*W}7Ch2pg$$c0BdEo-HWRTZcxngIBJad> z;C>b{jIXjb_9Jis?NZJsdm^EG}e*pR&DAy0EaSGi3XWTa(>C%tz1n$u?5Fb z1qtl?;_yjYo)(gB^iQq?=jusF%kywm?CJP~zEHi0NbZ);$(H$w(Hy@{i>$wcVRD_X|w-~(0Z9BJyh zhNh;+eQ9BEIs;tPz%jSVnfCP!3L&9YtEP;svoj_bNzeGSQIAjd zBss@A;)R^WAu-37RQrM%{DfBNRx>v!G31Z}8-El9IOJlb_MSoMu2}GDYycNaf>uny z+8xykD-7ONCM!APry_Lw6-yT>5!tR}W;W`C)1>pxSs5o1z#j7%m=&=7O4hz+Lsqm` z*>{+xsabZPr&X=}G@obTb{nPTkccJX8w3CG7X+1+t{JcMabv~UNv+G?txRqXib~c^Mo}`q{$`;EBNJ;#F*{gvS12kV?AZ%O0SFB$^ zn+}!HbmEj}w{Vq(G)OGAzH}R~kS^;(-s&=ectz8vN!_)Yl$$U@HNTI-pV`LSj7Opu zTZ5zZ)-S_{GcEQPIQXLQ#oMS`HPu{`SQiAZ)m1at*Hy%3xma|>o`h%E%8BEbi9p0r zVjcsh<{NBKQ4eKlXU|}@XJ#@uQw*$4BxKn6#W~I4T<^f99~(=}a`&3(ur8R9t+|AQ zWkQx7l}wa48-jO@ft2h+7qn%SJtL%~890FG0s5g*kNbL3I&@brh&f6)TlM`K^(bhr zJWM6N6x3flOw$@|C@kPi7yP&SP?bzP-E|HSXQXG>7gk|R9BTj`e=4de9C6+H7H7n# z#GJeVs1mtHhLDmVO?LkYRQc`DVOJ_vdl8VUihO-j#t=0T3%Fc1f9F73ufJz*adn*p zc%&vi(4NqHu^R>sAT_0EDjVR8bc%wTz#$;%NU-kbDyL_dg0%TFafZwZ?5KZpcuaO54Z9hX zD$u>q!-9`U6-D`E#`W~fIfiIF5_m6{fvM)b1NG3xf4Auw;Go~Fu7cth#DlUn{@~yu z=B;RT*dp?bO}o%4x7k9v{r=Y@^YQ^UUm(Qmliw8brO^=NP+UOohLYiaEB3^DB56&V zK?4jV61B|1Uj_5fBKW;8LdwOFZKWp)g{B%7g1~DgO&N& z#lisxf?R~Z@?3E$Mms$$JK8oe@X`5m98V*aV6Ua}8Xs2#A!{x?IP|N(%nxsH?^c{& z@vY&R1QmQs83BW28qAmJfS7MYi=h(YK??@EhjL-t*5W!p z^gYX!Q6-vBqcv~ruw@oMaU&qp0Fb(dbVzm5xJN%0o_^@fWq$oa3X?9s%+b)x4w-q5Koe(@j6Ez7V@~NRFvd zfBH~)U5!ix3isg`6be__wBJp=1@yfsCMw1C@y+9WYD9_C%{Q~7^0AF2KFryfLlUP# zwrtJEcH)jm48!6tUcxiurAMaiD04C&tPe6DI0#aoqz#Bt0_7_*X*TsF7u*zv(iEfA z;$@?XVu~oX#1YXtceQL{dSneL&*nDug^OW$DSLF0M1Im|sSX8R26&)<0Fbh^*l6!5wfSu8MpMoh=2l z^^0Sr$UpZp*9oqa23fcCfm7`ya2<4wzJ`Axt7e4jJrRFVf?nY~2&tRL* zd;6_njcz01c>$IvN=?K}9ie%Z(BO@JG2J}fT#BJQ+f5LFSgup7i!xWRKw6)iITjZU z%l6hPZia>R!`aZjwCp}I zg)%20;}f+&@t;(%5;RHL>K_&7MH^S+7<|(SZH!u zznW|jz$uA`P9@ZWtJgv$EFp>)K&Gt+4C6#*khZQXS*S~6N%JDT$r`aJDs9|uXWdbg zBwho$phWx}x!qy8&}6y5Vr$G{yGSE*r$^r{}pw zVTZKvikRZ`J_IJrjc=X1uw?estdwm&bEahku&D04HD+0Bm~q#YGS6gp!KLf$A{%Qd z&&yX@Hp>~(wU{|(#U&Bf92+1i&Q*-S+=y=3pSZy$#8Uc$#7oiJUuO{cE6=tsPhwPe| zxQpK>`Dbka`V)$}e6_OXKLB%i76~4N*zA?X+PrhH<&)}prET;kel24kW%+9))G^JI zsq7L{P}^#QsZViX%KgxBvEugr>ZmFqe^oAg?{EI=&_O#e)F3V#rc z8$4}0Zr19qd3tE4#$3_f=Bbx9oV6VO!d3(R===i-7p=Vj`520w0D3W6lQfY48}!D* z&)lZMG;~er2qBoI2gsX+Ts-hnpS~NYRDtPd^FPzn!^&yxRy#CSz(b&E*tL|jIkq|l zf%>)7Dtu>jCf`-7R#*GhGn4FkYf;B$+9IxmqH|lf6$4irg{0ept__%)V*R_OK=T06 zyT_m-o@Kp6U{l5h>W1hGq*X#8*y@<;vsOFqEjTQXFEotR+{3}ODDnj;o0@!bB5x=N z394FojuGOtVKBlVRLtHp%EJv_G5q=AgF)SKyRN5=cGBjDWv4LDn$IL`*=~J7u&Dy5 zrMc83y+w^F&{?X(KOOAl-sWZDb{9X9#jrQtmrEXD?;h-}SYT7yM(X_6qksM=K_a;Z z3u0qT0TtaNvDER_8x*rxXw&C^|h{P1qxK|@pS7vdlZ#P z7PdB7MmC2}%sdzAxt>;WM1s0??`1983O4nFK|hVAbHcZ3x{PzytQLkCVk7hA!Lo` zEJH?4qw|}WH{dc4z%aB=0XqsFW?^p=X}4xnCJXK%c#ItOSjdSO`UXJyuc8bh^Cf}8 z@Ht|vXd^6{Fgai8*tmyRGmD_s_nv~r^Fy7j`Bu`6=G)5H$i7Q7lvQnmea&TGvJp9a|qOrUymZ$6G|Ly z#zOCg++$3iB$!6!>215A4!iryregKuUT344X)jQb3|9qY>c0LO{6Vby05n~VFzd?q zgGZv&FGlkiH*`fTurp>B8v&nSxNz)=5IF$=@rgND4d`!AaaX;_lK~)-U8la_Wa8i?NJC@BURO*sUW)E9oyv3RG^YGfN%BmxzjlT)bp*$<| zX3tt?EAy<&K+bhIuMs-g#=d1}N_?isY)6Ay$mDOKRh z4v1asEGWoAp=srraLW^h&_Uw|6O+r;wns=uwYm=JN4Q!quD8SQRSeEcGh|Eb5Jg8m zOT}u;N|x@aq)=&;wufCc^#)5U^VcZw;d_wwaoh9$p@Xrc{DD6GZUqZ ziC6OT^zSq@-lhbgR8B+e;7_Giv;DK5gn^$bs<6~SUadiosfewWDJu`XsBfOd1|p=q zE>m=zF}!lObA%ePey~gqU8S6h-^J2Y?>7)L2+%8kV}Gp=h`Xm_}rlm)SyUS=`=S7msKu zC|T!gPiI1rWGb1z$Md?0YJQ;%>uPLOXf1Z>N~`~JHJ!^@D5kSXQ4ugnFZ>^`zH8CAiZmp z6Ms|#2gcGsQ{{u7+Nb9sA?U>(0e$5V1|WVwY`Kn)rsnnZ4=1u=7u!4WexZD^IQ1Jk zfF#NLe>W$3m&C^ULjdw+5|)-BSHwpegdyt9NYC{3@QtMfd8GrIWDu`gd0nv-3LpGCh@wgBaG z176tikL!_NXM+Bv#7q^cyn9$XSeZR6#!B4JE@GVH zoobHZN_*RF#@_SVYKkQ_igme-Y5U}cV(hkR#k1c{bQNMji zU7aE`?dHyx=1`kOYZo_8U7?3-7vHOp`Qe%Z*i+FX!s?6huNp0iCEW-Z7E&jRWmUW_ z67j>)Ew!yq)hhG4o?^z}HWH-e=es#xJUhDRc4B51M4~E-l5VZ!&zQq`gWe`?}#b~7w1LH4Xa-UCT5LXkXQWheBa2YJYbyQ zl1pXR%b(KCXMO0OsXgl0P0Og<{(@&z1aokU-Pq`eQq*JYgt8xdFQ6S z6Z3IFSua8W&M#`~*L#r>Jfd6*BzJ?JFdBR#bDv$_0N!_5vnmo@!>vULcDm`MFU823 zpG9pqjqz^FE5zMDoGqhs5OMmC{Y3iVcl>F}5Rs24Y5B^mYQ;1T&ks@pIApHOdrzXF z-SdX}Hf{X;TaSxG_T$0~#RhqKISGKNK47}0*x&nRIPtmdwxc&QT3$8&!3fWu1eZ_P zJveQj^hJL#Sn!*4k`3}(d(aasl&7G0j0-*_2xtAnoX1@9+h zO#c>YQg60Z;o{Bi=3i7S`Ic+ZE>K{(u|#)9y}q*j8uKQ1^>+(BI}m%1v3$=4ojGBc zm+o1*!T&b}-lVvZqIUBc8V}QyFEgm#oyIuC{8WqUNV{Toz`oxhYpP!_p2oHHh5P@iB*NVo~2=GQm+8Yrkm2Xjc_VyHg1c0>+o~@>*Qzo zHVBJS>$$}$_4EniTI;b1WShX<5-p#TPB&!;lP!lBVBbLOOxh6FuYloD%m;n{r|;MU3!q4AVkua~fieeWu2 zQAQ$ue(IklX6+V;F1vCu-&V?I3d42FgWgsb_e^29ol}HYft?{SLf>DrmOp9o!t>I^ zY7fBCk+E8n_|apgM|-;^=#B?6RnFKlN`oR)`e$+;D=yO-(U^jV;rft^G_zl`n7qnM zL z*-Y4Phq+ZI1$j$F-f;`CD#|`-T~OM5Q>x}a>B~Gb3-+9i>Lfr|Ca6S^8g*{*?_5!x zH_N!SoRP=gX1?)q%>QTY!r77e2j9W(I!uAz{T`NdNmPBBUzi2{`XMB^zJGGwFWeA9 z{fk33#*9SO0)DjROug+(M)I-pKA!CX;IY(#gE!UxXVsa)X!UftIN98{pt#4MJHOhY zM$_l}-TJlxY?LS6Nuz1T<44m<4i^8k@D$zuCPrkmz@sdv+{ciyFJG2Zwy&%c7;atIeTdh!a(R^QXnu1Oq1b42*OQFWnyQ zWeQrdvP|w_idy53Wa<{QH^lFmEd+VlJkyiC>6B#s)F;w-{c;aKIm;Kp50HnA-o3lY z9B~F$gJ@yYE#g#X&3ADx&tO+P_@mnQTz9gv30_sTsaGXkfNYXY{$(>*PEN3QL>I!k zp)KibPhrfX3%Z$H6SY`rXGYS~143wZrG2;=FLj50+VM6soI~up_>fU(2Wl@{BRsMi zO%sL3x?2l1cXTF)k&moNsHfQrQ+wu(gBt{sk#CU=UhrvJIncy@tJX5klLjgMn>~h= zg|FR&;@eh|C7`>s_9c~0-{IAPV){l|Ts`i=)AW;d9&KPc3fMeoTS%8@V~D8*h;&(^>yjT84MM}=%#LS7shLAuuj(0VAYoozhWjq z4LEr?wUe2^WGwdTIgWBkDUJa>YP@5d9^Rs$kCXmMRxuF*YMVrn?0NFyPl}>`&dqZb z<5eqR=ZG3>n2{6v6BvJ`YBZeeTtB88TAY(x0a58EWyuf>+^|x8Qa6wA|1Nb_p|nA zWWa}|z8a)--Wj`LqyFk_a3gN2>5{Rl_wbW?#by7&i*^hRknK%jwIH6=dQ8*-_{*x0j^DUfMX0`|K@6C<|1cgZ~D(e5vBFFm;HTZF(!vT8=T$K+|F)x3kqzBV4-=p1V(lzi(s7jdu0>LD#N=$Lk#3HkG!a zIF<7>%B7sRNzJ66KrFV76J<2bdYhxll0y2^_rdG=I%AgW4~)1Nvz=$1UkE^J%BxLo z+lUci`UcU062os*=`-j4IfSQA{w@y|3}Vk?i;&SSdh8n+$iHA#%ERL{;EpXl6u&8@ zzg}?hkEOUOJt?ZL=pWZFJ19mI1@P=$U5*Im1e_8Z${JsM>Ov?nh8Z zP5QvI!{Jy@&BP48%P2{Jr_VgzW;P@7)M9n|lDT|Ep#}7C$&ud&6>C^5ZiwKIg2McPU(4jhM!BD@@L(Gd*Nu$ji(ljZ<{FIeW_1Mmf;76{LU z-ywN~=uNN)Xi6$<12A9y)K%X|(W0p|&>>4OXB?IiYr||WKDOJPxiSe01NSV-h24^L z_>m$;|C+q!Mj**-qQ$L-*++en(g|hw;M!^%_h-iDjFHLo-n3JpB;p?+o2;`*jpvJU zLY^lt)Un4joij^^)O(CKs@7E%*!w>!HA4Q?0}oBJ7Nr8NQ7QmY^4~jvf0-`%waOLn zdNjAPaC0_7c|RVhw)+71NWjRi!y>C+Bl;Z`NiL^zn2*0kmj5gyhCLCxts*cWCdRI| zjsd=sT5BVJc^$GxP~YF$-U{-?kW6r@^vHXB%{CqYzU@1>dzf#3SYedJG-Rm6^RB7s zGM5PR(yKPKR)>?~vpUIeTP7A1sc8-knnJk*9)3t^e%izbdm>Y=W{$wm(cy1RB-19i za#828DMBY+ps#7Y8^6t)=Ea@%Nkt)O6JCx|ybC;Ap}Z@Zw~*}3P>MZLPb4Enxz9Wf zssobT^(R@KuShj8>@!1M7tm|2%-pYYDxz-5`rCbaTCG5{;Uxm z*g=+H1X8{NUvFGzz~wXa%Eo};I;~`37*WrRU&K0dPSB$yk(Z*@K&+mFal^?c zurbqB-+|Kb5|sznT;?Pj!+kgFY1#Dr;_%A(GIQC{3ct|{*Bji%FNa6c-thbpBkA;U zURV!Dr&X{0J}iht#-Qp2=xzuh(fM>zRoiGrYl5ttw2#r34gC41CCOC31m~^UPTK@s z6;A@)7O7_%C)>bnAXerYuAHdE93>j2N}H${zEc6&SbZ|-fiG*-qtGuy-qDelH(|u$ zorf8_T6Zqe#Ub!+e3oSyrskt_HyW_^5lrWt#30l)tHk|j$@YyEkXUOV;6B51L;M@=NIWZXU;GrAa(LGxO%|im%7F<-6N;en0Cr zLH>l*y?pMwt`1*cH~LdBPFY_l;~`N!Clyfr;7w<^X;&(ZiVdF1S5e(+Q%60zgh)s4 zn2yj$+mE=miVERP(g8}G4<85^-5f@qxh2ec?n+$A_`?qN=iyT1?U@t?V6DM~BIlBB z>u~eXm-aE>R0sQy!-I4xtCNi!!qh?R1!kKf6BoH2GG{L4%PAz0{Sh6xpuyI%*~u)s z%rLuFl)uQUCBQAtMyN;%)zFMx4loh7uTfKeB2Xif`lN?2gq6NhWhfz0u5WP9J>=V2 zo{mLtSy&BA!mSzs&CrKWq^y40JF5a&GSXIi2= z{EYb59J4}VwikL4P=>+mc6{($FNE@e=VUwG+KV21;<@lrN`mnz5jYGASyvz7BOG_6(p^eTxD-4O#lROgon;R35=|nj#eHIfJBYPWG>H>`dHKCDZ3`R{-?HO0mE~(5_WYcFmp8sU?wr*UkAQiNDGc6T zA%}GOLXlOWqL?WwfHO8MB#8M8*~Y*gz;1rWWoVSXP&IbKxbQ8+s%4Jnt?kDsq7btI zCDr0PZ)b;B%!lu&CT#RJzm{l{2fq|BcY85`w~3LSK<><@(2EdzFLt9Y_`;WXL6x`0 zDoQ?=?I@Hbr;*VVll1Gmd8*%tiXggMK81a+T(5Gx6;eNb8=uYn z5BG-0g>pP21NPn>$ntBh>`*})Fl|38oC^9Qz>~MAazH%3Q~Qb!ALMf$srexgPZ2@&c~+hxRi1;}+)-06)!#Mq<6GhP z-Q?qmgo${aFBApb5p}$1OJKTClfi8%PpnczyVKkoHw7Ml9e7ikrF0d~UB}i3vizos zXW4DN$SiEV9{faLt5bHy2a>33K%7Td-n5C*N;f&ZqAg#2hIqEb(y<&f4u5BWJ>2^4 z414GosL=Aom#m&=x_v<0-fp1r%oVJ{T-(xnomNJ(Dryv zh?vj+%=II_nV+@NR+(!fZZVM&(W6{6%9cm+o+Z6}KqzLw{(>E86uA1`_K$HqINlb1 zKelh3-jr2I9V?ych`{hta9wQ2c9=MM`2cC{m6^MhlL2{DLv7C^j z$xXBCnDl_;l|bPGMX@*tV)B!c|4oZyftUlP*?$YU9C_eAsuVHJ58?)zpbr30P*C`T z7y#ao`uE-SOG(Pi+`$=e^mle~)pRrdwL5)N;o{gpW21of(QE#U6w%*C~`v-z0QqBML!!5EeYA5IQB0 z^l01c;L6E(iytN!LhL}wfwP7W9PNAkb+)Cst?qg#$n;z41O4&v+8-zPs+XNb-q zIeeBCh#ivnFLUCwfS;p{LC0O7tm+Sf9Jn)~b%uwP{%69;QC)Ok0t%*a5M+=;y8j=v z#!*pp$9@!x;UMIs4~hP#pnfVc!%-D<+wsG@R2+J&%73lK|2G!EQC)O05TCV=&3g)C!lT=czLpZ@Sa%TYuoE?v8T8`V;e$#Zf2_Nj6nvBgh1)2 GZ~q4|mN%#X literal 0 HcmV?d00001 diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000..8e975a5 --- /dev/null +++ b/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,7 @@ +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 new file mode 100755 index 0000000..f5feea6 --- /dev/null +++ b/gradlew @@ -0,0 +1,252 @@ +#!/bin/sh + +# +# Copyright © 2015-2021 the original authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 +# + +############################################################################## +# +# Gradle start up script for POSIX generated by Gradle. +# +# Important for running: +# +# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is +# noncompliant, but you have some other compliant shell such as ksh or +# bash, then to run this script, type that shell name before the whole +# command line, like: +# +# ksh Gradle +# +# Busybox and similar reduced shells will NOT work, because this script +# requires all of these POSIX shell features: +# * functions; +# * expansions «$var», «${var}», «${var:-default}», «${var+SET}», +# «${var#prefix}», «${var%suffix}», and «$( cmd )»; +# * compound commands having a testable exit status, especially «case»; +# * various built-in commands including «command», «set», and «ulimit». +# +# Important for patching: +# +# (2) This script targets any POSIX shell, so it avoids extensions provided +# by Bash, Ksh, etc; in particular arrays are avoided. +# +# The "traditional" practice of packing multiple parameters into a +# space-separated string is a well documented source of bugs and security +# problems, so this is (mostly) avoided, by progressively accumulating +# options in "$@", and eventually passing that to Java. +# +# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS, +# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly; +# see the in-line comments for details. +# +# There are tweaks for specific operating systems such as AIX, CygWin, +# Darwin, MinGW, and NonStop. +# +# (3) This script is generated from the Groovy template +# https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# within the Gradle project. +# +# You can find Gradle at https://github.com/gradle/gradle/. +# +############################################################################## + +# Attempt to set APP_HOME + +# Resolve links: $0 may be a link +app_path=$0 + +# Need this for daisy-chained symlinks. +while + APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path + [ -h "$app_path" ] +do + ls=$( ls -ld "$app_path" ) + link=${ls#*' -> '} + case $link in #( + /*) app_path=$link ;; #( + *) app_path=$APP_HOME$link ;; + esac +done + +# This is normally unused +# shellcheck disable=SC2034 +APP_BASE_NAME=${0##*/} +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s +' "$PWD" ) || exit + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD=maximum + +warn () { + echo "$*" +} >&2 + +die () { + echo + echo "$*" + echo + exit 1 +} >&2 + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "$( uname )" in #( + CYGWIN* ) cygwin=true ;; #( + Darwin* ) darwin=true ;; #( + MSYS* | MINGW* ) msys=true ;; #( + NONSTOP* ) nonstop=true ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD=$JAVA_HOME/jre/sh/java + else + JAVACMD=$JAVA_HOME/bin/java + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD=java + if ! command -v java >/dev/null 2>&1 + then + die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +fi + +# Increase the maximum file descriptors if we can. +if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + ulimit -n "$MAX_FD" || + warn "Could not set maximum file descriptor limit to $MAX_FD" + esac +fi + +# Collect all arguments for the java command, stacking in reverse order: +# * args from the command line +# * the main class name +# * -classpath +# * -D...appname settings +# * --module-path (only if needed) +# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables. + +# For Cygwin or MSYS, switch paths to Windows format before running java +if "$cygwin" || "$msys" ; then + APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) + CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) + + JAVACMD=$( cygpath --unix "$JAVACMD" ) + + # Now convert the arguments - kludge to limit ourselves to /bin/sh + for arg do + if + case $arg in #( + -*) false ;; # don't mess with options #( + /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath + [ -e "$t" ] ;; #( + *) false ;; + esac + then + arg=$( cygpath --path --ignore --mixed "$arg" ) + fi + # Roll the args list around exactly as many times as the number of + # args, so each arg winds up back in the position where it started, but + # possibly modified. + # + # NB: a `for` loop captures its iteration list before it begins, so + # changing the positional parameters here affects neither the number of + # iterations, nor the values presented in `arg`. + shift # remove old arg + set -- "$@" "$arg" # push replacement arg + done +fi + + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Collect all arguments for the java command: +# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# and any embedded shellness will be escaped. +# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be +# treated as '${Hostname}' itself on the command line. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Stop when "xargs" is not available. +if ! command -v xargs >/dev/null 2>&1 +then + die "xargs is not available" +fi + +# Use "xargs" to parse quoted args. +# +# With -n1 it outputs one arg per line, with the quotes and backslashes removed. +# +# In Bash we could simply go: +# +# readarray ARGS < <( xargs -n1 <<<"$var" ) && +# set -- "${ARGS[@]}" "$@" +# +# but POSIX shell has neither arrays nor command substitution, so instead we +# post-process each arg (as a line of input to sed) to backslash-escape any +# character that might be a shell metacharacter, then use eval to reverse +# that process (while maintaining the separation between arguments), and wrap +# the whole thing up as a single "set" statement. +# +# This will of course break if any of these variables contains a newline or +# an unmatched quote. +# + +eval "set -- $( + printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | + xargs -n1 | + sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | + tr '\n' ' ' + )" '"$@"' + +exec "$JAVACMD" "$@" diff --git a/gradlew.bat b/gradlew.bat new file mode 100644 index 0000000..9b42019 --- /dev/null +++ b/gradlew.bat @@ -0,0 +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 +@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/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/settings.gradle b/settings.gradle new file mode 100644 index 0000000..7cab49b --- /dev/null +++ b/settings.gradle @@ -0,0 +1,30 @@ +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/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..47517dc --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,112 @@ +{ + "System Joysticks": { + "window": { + "enabled": false + } + }, + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "030000005e040000ea0200000b050000", + "useGamepad": true + }, + { + "useGamepad": true + }, + {}, + {}, + {}, + { + "useGamepad": true + } + ] +} diff --git a/src/main/deploy/example.txt b/src/main/deploy/example.txt new file mode 100644 index 0000000..70c79b6 --- /dev/null +++ b/src/main/deploy/example.txt @@ -0,0 +1,3 @@ +Files placed in this directory will be deployed to the RoboRIO into the +'deploy' directory in the home folder. Use the 'FileUtilities.getFilePath' wpilib function +to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Main.java b/src/main/java/frc4388/robot/Main.java new file mode 100644 index 0000000..ad2d494 --- /dev/null +++ b/src/main/java/frc4388/robot/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what you are doing, do not modify this file except to + * change the parameter class to the startRobot call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java new file mode 100644 index 0000000..c147692 --- /dev/null +++ b/src/main/java/frc4388/robot/Robot.java @@ -0,0 +1,264 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot; + +// Advantagekit +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; + +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc4388.robot.constants.BuildConstants; +import frc4388.robot.constants.Constants.SimConstants; +import frc4388.utility.DeferredBlock; +import frc4388.utility.compute.RobotTime; +import frc4388.utility.compute.Trim; +import frc4388.utility.status.FaultReporter; + +//import frc4388.robot.subsystems.LED; +/** + * The VM is configured to automatically run this class, and to call the + * functions corresponding to each mode, as described in the TimedRobot + * documentation. If you change the name of this class or the package after + * creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends LoggedRobot { + Command m_autonomousCommand; + + private RobotTime m_robotTime = RobotTime.getInstance(); + private RobotContainer m_robotContainer; + //private LED mled = new LED(); + /** + * This function is run when the robot is first started up and should be + * used for any initialization code. + */ + @Override + public void robotInit() { + // Start logging with AdvantageKit + startLogging(); + + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + + + // // Create a shuffleboard update thread, that will periodically update the values on shuffleboard + // FaultReporter.startThread(); + } + + + + /** + * This function is called every robot packet, no matter the mode. Use + * this for items like diagnostics that you want ran during disabled, + * autonomous, teleoperated and test.doubl + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + m_robotTime.updateTimes(); + // SmartDashboard.putNumber("Time", System.currentTimeMillis()); + + m_robotContainer.m_robotLED.update(); + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); + } + /** + * This function is called once each time the robot enters Disabled mode. + * You can use it to reset any subsystem information you want to clear when + * the robot is disabled. + */ + @Override + public void disabledInit() { + m_robotTime.endMatchTime(); + } + + @Override + public void disabledPeriodic() { + } + + @Override + public void disabledExit() { + DeferredBlock.execute(); + } + + /** + * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. + */ + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + /* + * String autoSelected = SmartDashboard.getString("Auto Selector", + * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand + * = new MyAutoCommand(); break; case "Default Auto": default: + * autonomousCommand = new ExampleCommand(); break; } + */ + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + m_robotTime.startMatchTime(); + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + } + + + @Override + public void teleopInit() { + m_robotContainer.stop(); + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + + if (m_autonomousCommand != null) { + CommandScheduler.getInstance().cancel(m_autonomousCommand); + m_autonomousCommand.cancel(); + m_autonomousCommand.end(true); + + } + m_robotTime.startMatchTime(); + } + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + // m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft()); + } + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopExit() { // the only OTHER mode that teleop can enter into is disabled. + Trim.dumpAll(); + } + + @Override + public void testInit() { + FaultReporter.printReport(); + } + + + + + + + + // VisionSystemSim visionSim; + // RobotMapSim robotMapSim; + + // @Override + // public void simulationInit() { + // visionSim = new VisionSystemSim("main"); + // robotMapSim = m_robotContainer.m_robotMap.configureSim(); + + + // // A 0.5 x 0.25 meter rectangular target + // TargetModel targetModel = new TargetModel(0.5, 0.25); + // // The pose of where the target is on the field. + // // Its rotation determines where "forward" or the target x-axis points. + // // Let's say this target is flat against the far wall center, facing the blue driver stations. + // Pose3d targetPose = new Pose3d(16, 4, 2, new Rotation3d(0, 0, Math.PI)); + // // The given target model at the given pose + // VisionTargetSim visionTarget = new VisionTargetSim(targetPose, targetModel); + + // // Add this vision target to the vision system simulation to make it visible + // visionSim.addVisionTargets(visionTarget); + + // // The layout of AprilTags which we want to add to the vision system + // AprilTagFieldLayout tagLayout = Constants.FieldConstants.kTagLayout; + + // visionSim.addAprilTags(tagLayout); + + + // visionSim.addCamera(robotMapSim.leftCamera, Constants.VisionConstants.LEFT_CAMERA_POS); + // visionSim.addCamera(robotMapSim.rightCamera, Constants.VisionConstants.RIGHT_CAMERA_POS); + + // SmartDashboard.putData(visionSim.getDebugField()); + // } + + + // @Override + // public void simulationPeriodic() { + // m_robotContainer.m_robotSwerveDrive.updateSim(RobotController.getBatteryVoltage()); + // // visionSim.update(m_robotContainer.m_robotSwerveDrive.getPose2d()); + + // // m_robotContainer.m_robotSwerveDrive. + // } + + + +// Start AdvantageKit logging / replay and record metadata +// Refrence: https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/template_projects/sources/skeleton/src/main/java/frc/robot/Robot.java + public void startLogging() { + // Record metadata + Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("Git+SHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; + } + + // Set up data receivers & replay source + switch (SimConstants.currentMode) { + case REAL: + // Running on a real robot, log to a USB stick ("/U/logs") + Logger.addDataReceiver(new WPILOGWriter()); + Logger.addDataReceiver(new NT4Publisher()); + break; + + case SIM: + // Running a physics simulator, log to NT + Logger.addDataReceiver(new NT4Publisher()); + break; + + case REPLAY: + // Replaying a log, set up replay source + setUseTiming(false); // Run as fast as possible + String logPath = LogFileUtil.findReplayLog(); + Logger.setReplaySource(new WPILOGReader(logPath)); + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); + break; + } + + // Start AdvantageKit logger + Logger.start(); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java new file mode 100644 index 0000000..4d81a20 --- /dev/null +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -0,0 +1,221 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot; + +// Drive Systems +import edu.wpi.first.wpilibj.DriverStation; + +import java.io.File; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.utility.controller.XboxController; +import frc4388.utility.controller.ButtonBox; +import frc4388.utility.controller.DeadbandedXboxController; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +// Commands +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +// Autos +import frc4388.utility.controller.VirtualController; +import frc4388.robot.commands.MoveForTimeCommand; +import frc4388.robot.commands.MoveUntilSuply; +import frc4388.robot.commands.alignment.DriveToReef; +import frc4388.robot.commands.alignment.DriveUntilLiDAR; +import frc4388.robot.commands.alignment.LidarAlign; +import frc4388.robot.commands.wait.waitElevatorRefrence; +import frc4388.robot.commands.wait.waitEndefectorRefrence; +import frc4388.robot.commands.wait.waitFeedCoral; +import frc4388.robot.commands.wait.waitSupplier; +import frc4388.robot.constants.Constants; +import frc4388.robot.constants.Constants.AutoConstants; +import frc4388.robot.constants.Constants.LiDARConstants; +import frc4388.robot.constants.Constants.OIConstants; +import frc4388.robot.constants.Constants.SimConstants.Mode; + +import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.commands.PathPlannerAuto; + +// Subsystems +import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.elevator.Elevator; +import frc4388.robot.subsystems.elevator.Elevator.CoordinationState; +import frc4388.robot.subsystems.lidar.LiDAR; +import frc4388.robot.subsystems.swerve.SwerveDrive; +import frc4388.robot.subsystems.vision.Vision; +// Utilites +import frc4388.utility.DeferredBlock; +import frc4388.utility.compute.TimesNegativeOne; +import frc4388.utility.compute.ReefPositionHelper.Side; + +/** + * This class is where the bulk of the robot should be declared. Since + * Command-based is a "declarative" paradigm, very little robot logic should + * actually be handled in the {@link Robot} periodic methods (other than the + * scheduler calls). Instead, the structure of the robot (2including subsystems, + * commands, and button mappings) should be declared here. + */ +public class RobotContainer { + /* RobotMap */ + + public final RobotMap m_robotMap = new RobotMap(Mode.REAL); + + /* Subsystems */ + public final LED m_robotLED = new LED(); + public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); + public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED); + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); + // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); + + public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef"); + public final LiDAR reverseLidar = new LiDAR(m_robotMap.reverseLidar, "Reverse"); + + + /* Controllers */ + private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); + private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID); + + // public List subsystems = new ArrayList<>(); + + // ! Teleop Commands + public void stop() { + new InstantCommand(()->{}, m_robotSwerveDrive).schedule(); + m_robotSwerveDrive.stopModules(); + Constants.AutoConstants.Y_OFFSET_TRIM.set(0); + } + + // ! /* Autos */ + private SendableChooser autoChooser; + private Command autoCommand; + + + /** + * This method is used to replcate {@link Trigger Triggers} for {@link VirtualController Virtual Controllers}.

+ * Please use {@link RobotContainer#DualJoystickButton} in {@link RobotContainer#configureButtonBindings} for standard buttons. + */ + private void configureVirtualButtonBindings() { + + // ? /* Driver Buttons */ + + /* Notice: the following buttons have not been replicated + * Swerve Drive Slow and Fast mode Gear Shifts : Fast mode is known to cause drift, so we disable that feature in Autoplayback + * Swerve Drive Rotation Gear Shifts : Same reason as Slow and Fast mode. + * Auto Recording controls : We don't want an Null Ouroboros for an auto. + */ + + // ? /* Operator Buttons */ + + /* Notice: the following buttons have not been replicated + * Override Intake Position Encoder : It's an emergancy overide, for when the position of intake when the robot boots, the intake is not inside the robot. + * We don't need it in an auto. + * Climbing controls : We don't need to climb in auto. + */ + + // ? Notice: the Programer Buttons are not to be replicated because they are designed for debuging the robot, and do not need to be replicated in auto. + + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + + + //return autoPlayback; + //return new GotoPositionCommand(m_robotSwerveDrive, m_vision) + //return autoChooser.getSelected(); + // try{ + // // // Load the path you want to follow using its name in the GUI + // return autoCommand; + // } catch (Exception e) { + // DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace()); + return autoCommand; + // } + // return new PathPlannerAuto("Line-up-no-arm"); + // zach told me to do the below comment + //return new GotoPositionCommand(m_robotSwerveDrive, m_vision); + // return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos); + } + + public boolean autoChooserUpdated = false; + public void makeAutoChooser() { + autoChooser = new SendableChooser(); + + File dir; + + if(RobotBase.isReal()) { + dir = new File("/home/lvuser/deploy/pathplanner/autos/"); + } else { + // dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\"); + dir = new File("/home/astatin3/Documents/GitHub/2025RidgeScape/src/main/deploy/pathplanner/autos"); + } + + String[] autos = dir.list(); + + if(autos == null) return; + + for (String auto : autos) { + if (auto.endsWith(".auto")) + autoChooser.addOption(auto.replaceAll(".auto", ""), auto.replaceAll(".auto", "")); + // System.out.println(auto); + } + + autoChooser.onChange((filename) -> { + autoChooserUpdated = true; + if (filename.equals("Taxi")) { + autoCommand = new SequentialCommandGroup( + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(0, -1), + new Translation2d(), 1000, true + ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)); + } else { + autoCommand = new PathPlannerAuto(filename); + } + System.out.println("Robot Auto Changed " + filename); + }); + // SmartDashboard.putData(autoChooser); + + } + + /** + * A button binding for two controllers, preferably an {@link DeadbandedXboxController Xbox Controller} and {@link VirtualController Virtual Xbox Controller} + * @param joystickA A controller + * @param joystickB A controller + * @param buttonNumber The button to bind to + */ + public Trigger DualJoystickButton(GenericHID joystickA, GenericHID joystickB, int buttonNumber) { + return new Trigger(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber))); + } + + public DeadbandedXboxController getDeadbandedDriverController() { + return this.m_driverXbox; + } + + public DeadbandedXboxController getDeadbandedOperatorController() { + return this.m_operatorXbox; + } + + public ButtonBox getButtonBox() { + return this.m_buttonBox; + } +} diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java new file mode 100644 index 0000000..01cdb68 --- /dev/null +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -0,0 +1,175 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot; + +import com.ctre.phoenix6.hardware.TalonFX; + +import org.photonvision.PhotonCamera; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; + +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DigitalInput; +import frc4388.robot.constants.Constants.ElevatorConstants; +import frc4388.robot.constants.Constants.LiDARConstants; +import frc4388.robot.constants.Constants.SimConstants; +import frc4388.robot.constants.Constants.VisionConstants; +import frc4388.robot.subsystems.elevator.ElevatorIO; +import frc4388.robot.subsystems.elevator.ElevatorReal; +import frc4388.robot.subsystems.lidar.LiDAR; +import frc4388.robot.subsystems.lidar.LidarIO; +import frc4388.robot.subsystems.lidar.LidarReal; +import frc4388.robot.subsystems.swerve.SwerveDriveConstants; +import frc4388.robot.subsystems.swerve.SwerveIO; +import frc4388.robot.subsystems.swerve.SwerveReal; +import frc4388.robot.subsystems.vision.VisionIO; +import frc4388.robot.subsystems.vision.VisionReal; +import frc4388.utility.status.FaultCANCoder; +import frc4388.utility.status.FaultPhotonCamera; +import frc4388.utility.status.FaultPidgeon2; +import frc4388.utility.status.FaultTalonFX; + +/** + * Defines and holds all I/O objects on the Roborio. This is useful for unit + * testing and modularization. + */ +public class RobotMap { + // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id); + // public RobotGyro gyro = new RobotGyro(m_pigeon2); + + public final VisionIO leftCamera; + public final VisionIO rightCamera; + + // public final LiDAR lidar = new + + public final LidarIO reefLidar; + public final LidarIO reverseLidar; + + + /* LED Subsystem */ + // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + + /* Swreve Drive Subsystem */ + public final SwerveIO swerveDrivetrain; + + /* Elevator Subsystem */ + public final ElevatorIO elevatorIO; + + public RobotMap(SimConstants.Mode mode) { + switch (mode) { + case REAL: + // Configure cameras + PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); + PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); + + leftCamera = new VisionReal(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ; + rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS); + + FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera"); + FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera"); + + // Configure LiDAR + reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL); + reverseLidar = new LidarReal(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL); + + // Configure swerve drive train + SwerveDrivetrain swerveDrivetrainReal = new SwerveDrivetrain (TalonFX::new, TalonFX::new, CANcoder::new, + SwerveDriveConstants.DrivetrainConstants, + SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT, + SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT + ); + + swerveDrivetrain = new SwerveReal(swerveDrivetrainReal); + + // Configure elevator + + TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id); + TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id); + + + DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); + DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); + DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); + + elevatorIO = new ElevatorReal(elevator, endeffector, basinLimitSwitch, endeffectorLimitSwitch, IRIntakeBeam); + + + + // Fault + FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); + + FaultTalonFX.addDevice(elevator, "Elevator"); + FaultTalonFX.addDevice(endeffector, "Endeffector"); + + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getDriveMotor(), "Module 1 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getSteerMotor(), "Module 1 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(1).getEncoder(), "Module 1 CANCoder"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getDriveMotor(), "Module 2 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getSteerMotor(), "Module 2 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(2).getEncoder(), "Module 2 CANCoder"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getDriveMotor(), "Module 3 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getSteerMotor(), "Module 3 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder"); + + break; + // case SIM: + // break; + default: + leftCamera = new VisionIO() {}; + rightCamera = new VisionIO() {}; + reefLidar = new LidarIO() {}; + reverseLidar = new LidarIO() {}; + swerveDrivetrain = new SwerveIO() {}; + elevatorIO = new ElevatorIO() {}; + break; + } + } + + // public class RobotMapSim { + // public PhotonCameraSim leftCamera; + // public PhotonCameraSim rightCamera; + // } + + // public RobotMapSim configureSim() { + // RobotMapSim sim = new RobotMapSim(); + + // // The simulated camera properties + // SimCameraProperties cameraProp = new SimCameraProperties(); + // // A 640 x 480 camera with a 100 degree diagonal FOV. + // cameraProp.setCalibration(640, 480, Rotation2d.fromDegrees(100)); + // // Approximate detection noise with average and standard deviation error in pixels. + // cameraProp.setCalibError(0.25, 0.08); + // // Set the camera image capture framerate (Note: this is limited by robot loop rate). + // cameraProp.setFPS(20); + // // The average and standard deviation in milliseconds of image data latency. + // cameraProp.setAvgLatencyMs(35); + // cameraProp.setLatencyStdDevMs(5); + + // // sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp); + // // sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp); + + + // sim.leftCamera.enableRawStream(true); + // sim.leftCamera.enableProcessedStream(true); + // sim.leftCamera.enableDrawWireframe(true); + + + // sim.rightCamera.enableRawStream(true); + // sim.rightCamera.enableProcessedStream(true); + // sim.rightCamera.enableDrawWireframe(true); + + // return sim; + + // } + +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt b/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt new file mode 100644 index 0000000..a65aea9 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt @@ -0,0 +1,20 @@ +AUTO file format + +HEADER static size 0x5 +0x00 BYTE NUM AXES: defualts to 6 +0x01 BYTE NUM POV: defualts to 1 +0x02 BYTE NUM CONTROLLERS: defualts to 2 +0x03 SHORT FRAMES: any value greator or equal than one. + +FRAME PER CONTROLLER: defualt size 0x34 +0x00 DOUBLE AXES[NUM AXES] +0x30 SHORT BUTTONS +0x32 SHORT POVs[NUM POV] + +FRAME: size varrys +FRAME PER CONTROLLER[NUM CONTROLLERS] +INT UNIXTIMESTAMP + +FILE: +HEADER +FRAME[FRAMES] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java new file mode 100644 index 0000000..a069786 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java @@ -0,0 +1,108 @@ +package frc4388.robot.commands.autos; +// package frc4388.robot.commands.Autos; + +// import java.io.File; +// import java.util.ArrayList; +// import java.util.HashMap; + +// import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +// import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; +// import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +// import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import frc4388.robot.commands.Swerve.JoystickPlayback; +// import frc4388.robot.commands.Swerve.neoJoystickPlayback; +// import frc4388.robot.subsystems.SwerveDrive; +// import frc4388.utility.controller.VirtualController; + +// public class neoPlaybackChooser { +// private final SendableChooser m_teamChosser = new SendableChooser(); +// private final SendableChooser m_possitionChosser = new SendableChooser(); +// private final SendableChooser m_autoNameChosser = new SendableChooser(); + +// private final SwerveDrive m_swerve; +// private final VirtualController[] m_controllers; +// // private final ArrayList> m_choosers = new ArrayList<>(); +// // private SendableChooser m_playback = null; +// private final ArrayList m_widgets = new ArrayList<>(); +// // private final HashMap m_commandPool = new HashMap<>(); + +// // private final File m_dir = new File("/home/lvuser/autos/"); +// // private int m_cmdNum = 0; + +// // commands +// private Command m_noAuto = new InstantCommand(); + +// public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) { +// m_teamChosser.addOption("Red", "red"); +// m_teamChosser.setDefaultOption("Blue", "blue"); +// m_teamChosser.addOption("Nuetral", "nuetral"); +// m_possitionChosser.addOption("AMP", "amp"); +// m_possitionChosser.setDefaultOption("Center", "center"); +// m_possitionChosser.addOption("Source", "source"); +// m_swerve = swerve; +// m_controllers = controllers; +// } + +// public neoPlaybackChooser addOption(String name, String option) { +// m_autoNameChosser.addOption(name, option); +// return this; +// } + +// // public PlaybackChooser buildDisplay() { +// // for (int i = 0; i < 10; i++) { +// // appendCommand(); +// // } +// // m_playback = m_choosers.get(0); +// // nextChooser(); + +// // // ! This does not work, why? +// // Shuffleboard.getTab("Auto Chooser") +// // .add("Add Sequence", new InstantCommand(() -> nextChooser())) +// // .withPosition(4, 0); +// // return this; +// // } + +// // This will be bound to a button for the time being +// public void render() { +// // var chooser = new SendableChooser(); +// // // chooser.setDefaultOption("No Auto", m_noAuto); + +// // m_choosers.add(chooser); +// ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser") +// .add("Command: " + m_choosers.size(), chooser) +// .withSize(4, 1) +// .withPosition(0, m_choosers.size() - 1) +// .withWidget(BuiltInWidgets.kSplitButtonChooser) +// .withWidget(BuiltInWidgets.kComboBoxChooser); + + +// m_widgets.add(widget); +// } + +// // public void nextChooser() { +// // SendableChooser chooser = m_choosers.get(m_cmdNum++); + +// // String[] dirs = m_dir.list(); + +// // if(dirs != null){ // Fix funny error +// // for (String auto : dirs) { +// // chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); +// // } +// // } + + +// // for (var cmd_name : m_commandPool.keySet()) { +// // chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); +// // } +// // } + +// public String autoName() { +// return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto"; +// } + +// public Command getCommand() { +// return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true); +// } +// } diff --git a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java new file mode 100644 index 0000000..3a4e043 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java @@ -0,0 +1,49 @@ +package frc4388.robot.commands; + +import java.time.Instant; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.swerve.SwerveDrive; + +// Command to repeat a joystick movement for a specific time. +public class MoveForTimeCommand extends Command { + private final SwerveDrive swerveDrive; + private final Translation2d leftStick; + private final Translation2d rightStick; + private final long duration; + private final boolean robotRelative; + + private Instant startTime; + + public MoveForTimeCommand( + SwerveDrive swerveDrive, + Translation2d leftStick, + Translation2d rightStick, + long millis, + boolean robotRelative) { + + addRequirements(swerveDrive); + + this.swerveDrive = swerveDrive; + this.leftStick = leftStick; + this.rightStick = rightStick; + this.duration = millis; + this.robotRelative = robotRelative; + } + + @Override + public void initialize() { + startTime = Instant.now(); + } + + @Override + public void execute() { + swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative); + } + + @Override + public boolean isFinished() { + return Math.abs(startTime.toEpochMilli() - Instant.now().toEpochMilli()) > duration; + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/MoveUntilSuply.java b/src/main/java/frc4388/robot/commands/MoveUntilSuply.java new file mode 100644 index 0000000..c5b5e53 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/MoveUntilSuply.java @@ -0,0 +1,45 @@ +package frc4388.robot.commands; + +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.swerve.SwerveDrive; + +// Command to repeat a joystick movement for a specific time. +public class MoveUntilSuply extends Command { + private final SwerveDrive swerveDrive; + private final Translation2d leftStick; + private final Translation2d rightStick; + private final Supplier truth; + private final boolean robotRelative; + + public MoveUntilSuply( + SwerveDrive swerveDrive, + Translation2d leftStick, + Translation2d rightStick, + Supplier truth, + boolean robotRelative) { + addRequirements(swerveDrive); + + this.swerveDrive = swerveDrive; + this.leftStick = leftStick; + this.rightStick = rightStick; + this.truth = truth; + this.robotRelative = robotRelative; + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative); + } + + @Override + public boolean isFinished() { + return truth.get(); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/PID.java b/src/main/java/frc4388/robot/commands/PID.java new file mode 100644 index 0000000..c95c9fd --- /dev/null +++ b/src/main/java/frc4388/robot/commands/PID.java @@ -0,0 +1,60 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.utility.structs.Gains; + +public abstract class PID extends Command { + protected Gains gains; + private double output = 0; + private double tolerance = 0; + + /** Creates a new PelvicInflammatoryDisease. */ + public PID(double kp, double ki, double kd, double kf, double tolerance) { + gains = new Gains(kp, ki, kd, kf, 0); + this.tolerance = tolerance; + } + + public PID(Gains gains, double tolerance) { + this.gains = gains; + this.tolerance = tolerance; + } + + /** produces the error from the setpoint */ + public abstract double getError(); + + /** todo: javadoc */ + public abstract void runWithOutput(double output); + + // Called when the command is initially scheduled. + @Override + public final void initialize() { + output = 0; + } + + private double prevError, cumError = 0; + + // Called every time the scheduler runs while the command is scheduled. + @Override + public final void execute() { + double error = getError(); + cumError += error * .02; // 20 ms + double delta = error - prevError; + + output = error * gains.kP; + output += cumError * gains.kI; + output += delta * gains.kD; + output += gains.kF; + + runWithOutput(output); + } + + // Returns true when the command should end. + @Override + public final boolean isFinished() { + return Math.abs(getError()) < tolerance; + } +} diff --git a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java new file mode 100644 index 0000000..50e616c --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java @@ -0,0 +1,35 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.swerve; + +import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.commands.PID; +import frc4388.robot.subsystems.swerve.SwerveDrive; + +public class RotateToAngle extends PID { + + SwerveDrive drive; + double targetAngle; + + /** Creates a new RotateToAngle. */ + public RotateToAngle(SwerveDrive drive, double targetAngle) { + super(0.3, 0.0, 0.0, 0.0, 1); + + this.drive = drive; + this.targetAngle = targetAngle; + + addRequirements(drive); + } + + @Override + public double getError() { + return targetAngle - drive.getGyroAngle(); + } + + @Override + public void runWithOutput(double output) { + drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true); + } +} diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java new file mode 100644 index 0000000..bff5105 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java @@ -0,0 +1,197 @@ +package frc4388.robot.commands.swerve; + +import java.io.FileInputStream; +import java.util.ArrayList; +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.swerve.SwerveDrive; +import frc4388.utility.compute.DataUtils; +import frc4388.utility.controller.VirtualController; +import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame; +import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame; + + +/** + * The NEO autonomus playback system, designed based the old {@link JoystickPlayback} System but with {@link VirtualController}s + * @author Zachary Wilke + */ +public class neoJoystickPlayback extends Command { + private final SwerveDrive swerve; + private final VirtualController[] controllers; + private final ArrayList frames = new ArrayList<>(); + private final Supplier filenameGetter; + private String filename; + private int frame_index = 0; + // private long startTime = 0; + // private long playbackTime = 0; + private boolean m_finished = false; // ! There is no better way. + private boolean m_shouldfree = false; // should free memory on ending + + private byte m_numAxes = 0; + private byte m_numPOVs = 0; + private byte m_numControllers = 0; + private short m_numFrames = -1; + + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + * @param shouldfree Unloads the auto on compleation or intruption. + * @param instantload Load the auto on object instantiation + */ + public neoJoystickPlayback(SwerveDrive swerve, Supplier filenameGetter, VirtualController[] controllers, boolean shouldfree, boolean instantload) { + this.swerve = swerve; + this.filenameGetter = filenameGetter; + this.controllers = controllers; + this.m_shouldfree = shouldfree; + + if (instantload) loadAuto(); + addRequirements(this.swerve); + } + + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param filename a String containing the name of the auto file you wish to playback. + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + * @param shouldfree unloads the auto on compleation or intruption. + * @param instantload load the auto on object instantiation + */ + public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) { + this(swerve, () -> filename, controllers, shouldfree, instantload); + } + + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + */ + public neoJoystickPlayback(SwerveDrive swerve, Supplier filenameGetter, VirtualController[] controllers) { + this(swerve, filenameGetter, controllers, true, false); + } + + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param filename a String containing the name of the auto file you wish to playback. + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + */ + public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) { + this(swerve, () -> filename, controllers, true, false); + } + + /** + * Load the auto file from disk into memory + * @return Returns true if loading was successful, else wise; return false + * @implNote if the auto is already loaded, it will return true. + */ + public boolean loadAuto() { + filename = filenameGetter.get(); + try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) { + if (m_numFrames != -1 && m_numFrames == frames.size()) { + System.out.println("AUTOPLAYBACK: Auto Already loaded."); + return true; + } + + m_numAxes = stream.readNBytes(1)[0]; + m_numPOVs = stream.readNBytes(1)[0]; + m_numControllers = stream.readNBytes(1)[0]; + m_numFrames = DataUtils.byteArrayToShort(stream.readNBytes(2)); + + if (m_numControllers > controllers.length) { + System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers + + " virtual controllers but only " + controllers.length + " were given"); + return false; + } + + for (int i = 0; i < m_numFrames; i++) { + AutoRecordingFrame frame = new AutoRecordingFrame(); + for (int j = 0; j < m_numControllers; j++) { + AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); + double[] axes = new double[m_numAxes]; + for (int k = 0; k < m_numAxes; k++) { // we love third level for loops. + axes[k] = DataUtils.byteArrayToDouble(stream.readNBytes(8)); + } + short button = DataUtils.byteArrayToShort(stream.readNBytes(2)); + short[] POV = new short[m_numPOVs]; + for (int k = 0; k < m_numPOVs; k++) { + POV[k] = DataUtils.byteArrayToShort(stream.readNBytes(2)); + } + controllerFrame.axes = axes; + controllerFrame.button = button; + controllerFrame.POV = POV; + frame.controllerFrames[j] = controllerFrame; + } + frame.timeStamp = DataUtils.byteArrayToInt(stream.readNBytes(4)); + frames.add(frame); + } + + System.out.println("AUTOPLAYBACK: Read Auto `" + filename + "` that is " + m_numFrames + " frames long"); + return true; + + } catch (Exception e) { + e.printStackTrace(); + System.out.println("AUTOPLAYBACK: Unable to read auto file `" + filename + '`'); + return false; + } + } + + /** + * Unloads the auto. + */ + public void unloadAuto() { + System.out.println("AUTOPLAYBACK: Auto unloaded"); + frames.clear(); + } + + @Override + public void initialize() { + // startTime = System.currentTimeMillis(); + // playbackTime = 0; + frame_index = 0; + + m_finished = !loadAuto(); + } + + @Override + public void execute() { + if (frame_index >= m_numFrames) m_finished = true; + if (m_finished) return; + + // if (frame_index == 0) { + // startTime = System.currentTimeMillis(); + // playbackTime = 0; + // } else { + // playbackTime = System.currentTimeMillis() - startTime; + // } + + AutoRecordingFrame frame = frames.get(frame_index); + for (int i = 0; i < controllers.length; i++) { + AutoRecordingControllerFrame controllerFrame = frame.controllerFrames[i]; + controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV); + if (i == 0) { + this.swerve.driveWithInput( + new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)), + new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)), + true); + } + } + frame_index++; + } + + @Override + public void end(boolean interrupted) { + for (VirtualController controller : controllers) controller.zeroControls(); + swerve.stopModules(); + if (m_shouldfree) unloadAuto(); + } + + @Override + public boolean isFinished() { + return m_finished; + } +} diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java new file mode 100644 index 0000000..4cf3159 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java @@ -0,0 +1,129 @@ +package frc4388.robot.commands.swerve; + +import java.io.FileOutputStream; +import java.util.ArrayList; +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.swerve.SwerveDrive; +import frc4388.utility.compute.DataUtils; +import frc4388.utility.controller.DeadbandedXboxController; +import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame; +import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame; + +/** + * The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s + * @author Zachary Wilke + */ +public class neoJoystickRecorder extends Command { + private final SwerveDrive swerve; + private final XboxController[] controllers; + private String filename; + private final Supplier filenameGetter; + private long startTime = -1; + private final ArrayList frames = new ArrayList<>(); + + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. + */ + public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, Supplier filenameGetter) { + this.swerve = swerve; + this.controllers = controllers; + this.filenameGetter = filenameGetter; + this.filename = ""; + + addRequirements(this.swerve); + } + + /** + * Creates an new NEO Joystick Playback with specifyed pramiters. + * @param swerve m_robotSwerveDrive + * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. + * @param filename a String containing the name of the auto file you wish to playback. + */ + public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) { + this(swerve, controllers, () -> filename); + } + + @Override + public void initialize() { + frames.clear(); + + this.startTime = System.currentTimeMillis(); + AutoRecordingFrame frame = new AutoRecordingFrame(); + frame.controllerFrames = new AutoRecordingControllerFrame[] {new AutoRecordingControllerFrame(), new AutoRecordingControllerFrame()}; + frames.add(frame); + this.filename = this.filenameGetter.get(); + } + + + @Override + public void execute() { + System.out.println("AUTORECORD: RECORDING"); + AutoRecordingFrame frame = new AutoRecordingFrame(); + frame.timeStamp = (int) (System.currentTimeMillis() - startTime); + for (int i = 0; i < controllers.length; i++) { + XboxController controller = controllers[i]; + AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); + double[] axes = {controller.getLeftX(), controller.getLeftY(), + controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(), + controller.getRightX(), controller.getRightY()}; + short button = 0; + for (int j = 0; j < 10; j++) + if (controller.getRawButton(j+1)) + button |= 1 << j; + short[] POV = {(short)(controller.getPOV())}; + controllerFrame.axes = axes; + controllerFrame.button = button; + controllerFrame.POV = POV; + frame.controllerFrames[i] = controllerFrame; + } + + frames.add(frame); + + swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]), + new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]), + true); // Really jank way of doing this. + + } + @Override + public void end(boolean interrupted) { + try (FileOutputStream stream = new FileOutputStream("/home/lvuser/autos/" + filename)) { + // header: size of 0x5 + // byte Number of axes per controller + // byte Number of POVs per controller + // byte Number of controllers + // short Number of frames + stream.write(new byte[]{6, 1, (byte) controllers.length}); + stream.write(DataUtils.shortToByteArray((short) frames.size())); + + // frame + // controller frame * number of controllers + // int unix time stamp. + for (AutoRecordingFrame frame : frames) { + // controller frame + // double axis * Number of axes per controller + // short button states + // short POV * Number of POVs per controller + for (AutoRecordingControllerFrame controllerFrame: frame.controllerFrames) { + for (double axis: controllerFrame.axes) { + stream.write(DataUtils.doubleToByteArray(axis)); + } + stream.write(DataUtils.shortToByteArray(controllerFrame.button)); + for (short POV: controllerFrame.POV) { + stream.write(DataUtils.shortToByteArray(POV)); + } + } + stream.write(DataUtils.intToByteArray(frame.timeStamp)); + } + System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long."); + } catch (Exception e) { + e.printStackTrace(); + } + } +} diff --git a/src/main/java/frc4388/robot/commands/WhileTrueCommand.java b/src/main/java/frc4388/robot/commands/WhileTrueCommand.java new file mode 100644 index 0000000..2a0d045 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/WhileTrueCommand.java @@ -0,0 +1,104 @@ +package frc4388.robot.commands; + +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj2.command.Command; + +import java.util.function.BooleanSupplier; + +/** + * A command composition that runs one of two commands, depending on the value of the given + * condition when this command is initialized. + * + *

The rules for command compositions apply: command instances that are passed to it cannot be + * added to any other composition or scheduled individually, and the composition requires all + * subsystems its components require. + * + *

This class is provided by the NewCommands VendorDep + */ +public class WhileTrueCommand extends Command { + private final Command m_whileTrue; + private final BooleanSupplier m_condition; + + /** + * Creates a new WhileTrueCommand. + * + * @param whileTrue the command to run while the condition is true + * @param condition the condition to determine which command to run + */ + @SuppressWarnings("this-escape") + public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) { + m_whileTrue = requireNonNullParam(whileTrue, "whileTrue", "WhileTrueCommand"); + m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand"); + + //CommandScheduler.getInstance().registerComposedCommands(whileTrue); + + // addRequirements(whileTrue.getRequirements()); + } + + @Override + public void initialize() { + if(m_condition.getAsBoolean()) + m_whileTrue.initialize(); + } + + @Override + public void execute() { + m_whileTrue.execute(); + + System.out.println("Loop, " + !m_whileTrue.isFinished() + ", " + m_condition.getAsBoolean()); + + if(!m_whileTrue.isFinished()) + return; + + if(m_condition.getAsBoolean()){ + m_whileTrue.end(false); + m_whileTrue.initialize(); + } + } + + @Override + public void end(boolean interrupted) { + m_whileTrue.end(interrupted); + } + + @Override + public boolean isFinished() { + return !m_condition.getAsBoolean() && m_whileTrue.isFinished(); + } + + @Override + public boolean runsWhenDisabled() { + return m_whileTrue.runsWhenDisabled(); + } + + @Override + public InterruptionBehavior getInterruptionBehavior() { + if (m_whileTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) { + return InterruptionBehavior.kCancelSelf; + } else { + return InterruptionBehavior.kCancelIncoming; + } + } + + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + builder.addStringProperty("whileTrue", m_whileTrue::getName, null); + builder.addStringProperty( + "selected", + () -> { + if (m_whileTrue == null) { + return "null"; + } else { + return m_whileTrue.getName(); + } + }, + null); + } +} diff --git a/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java b/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java new file mode 100644 index 0000000..6a11e85 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java @@ -0,0 +1,188 @@ +package frc4388.robot.commands.alignment; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.constants.Constants.AutoConstants; +import frc4388.robot.subsystems.swerve.SwerveDrive; +import frc4388.robot.subsystems.vision.Vision; +import frc4388.utility.compute.ReefPositionHelper; +import frc4388.utility.compute.TimesNegativeOne; +import frc4388.utility.compute.ReefPositionHelper.Side; +import frc4388.utility.structs.Gains; + +public class DriveToReef extends Command { + + + // private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998); + // private Translation2d translation2d= new Translation2d(16.579342-0.15,5.547867999999999); + + private PID xPID = new PID(AutoConstants.XY_GAINS, 0); + private PID yPID = new PID(AutoConstants.XY_GAINS, 0); + // private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0); + private Pose2d targetpos; + + SwerveDrive swerveDrive; + Vision vision; + double distance; + Side side; + boolean waitVelocityZero; + + /** + * Command to drive robot to position. + * @param SwerveDrive m_robotSwerveDrive + */ + + public DriveToReef(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) { + this.swerveDrive = swerveDrive; + this.vision = vision; + this.distance = distance; + this.side = side; + this.waitVelocityZero = waitVelocityZero && false; + addRequirements(swerveDrive); + } + + + public static double tagRelativeXError = -1; + private static void setTagRelativeXError(double val){ + tagRelativeXError = val; + } + + @Override + public void initialize() { + xPID.initialize(); + yPID.initialize(); + this.targetpos = ReefPositionHelper.getNearestPosition( + this.vision.getPose2d(), + side, + Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()), + distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get()) + ); + } + + double xerr; + double yerr; + double roterr; + + double xoutput; + double youtput; + double rotoutput; + + @Override + public void execute() { + xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis); + yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis); + // xerr = targetpos.getX() - vision.getPose2d().getX(); + // yerr = targetpos.getX() - vision.getPose2d().getY(); + + // roterr = TimesNegativeOne.invert(targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(), TimesNegativeOne.isRed); + + roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees())); + + if(roterr > 180){ + roterr -= 360; + }else if(roterr < -180){ + roterr += 360; + } + + // SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees()); + // SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees()); + // SmartDashboard.putNumber("Rotational PID error", roterr); + + SmartDashboard.putNumber("PID X Error", xerr); + SmartDashboard.putNumber("PID Y Error", yerr); + SmartDashboard.putNumber("PID Rot Error", roterr); + + xoutput = xPID.update(xerr); + youtput = yPID.update(yerr); + // rotoutput = rotPID.update(roterr); + + xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; + youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; + // rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1; + + + + Translation2d leftStick = new Translation2d( + Math.max(Math.min(-youtput, 1), -1), + Math.max(Math.min(-xoutput, 1), -1) + // 0,0 + ); + + // Translation2d rightStick = new Translation2d( + // Math.max(Math.min(rotoutput, 1), -1), + // 0 + // ); + + setTagRelativeXError( + new Translation2d(xerr, yerr).getAngle() + .rotateBy(targetpos.getRotation()) + .getCos()); + + swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation()); + // swerveDrive.driveWithInputOrientation(leftStick, rightStick, true); + } + + @Override + public final boolean isFinished() { + boolean finished = ( + (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && + (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && + (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) && + (!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD) + ); + // System.out.println(finished); + + if(finished) + swerveDrive.softStop(); + + return finished; + // this statement is a boolean in and of itself + } + + + + + + + + + + + + + + + private class PID { + protected Gains gains; + private double output = 0; + + + /** Creates a new PelvicInflammatoryDisease. */ + public PID(Gains gains, double tolerance) { + this.gains = gains; + } + + // Called when the command is initially scheduled. + public final void initialize() { + output = 0; + } + + private double prevError, cumError = 0; + + // Called every time the scheduler runs while the command is scheduled. + public double update(double error) { + cumError += error * .02; // 20 ms + double delta = error - prevError; + + output = error * gains.kP; + output += cumError * gains.kI; + output += delta * gains.kD; + output += gains.kF; + + return output; + } + } +} diff --git a/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java b/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java new file mode 100644 index 0000000..648a7d8 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java @@ -0,0 +1,48 @@ +package frc4388.robot.commands.alignment; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.lidar.LiDAR; +import frc4388.robot.subsystems.swerve.SwerveDrive; + +// Command to repeat a joystick movement for a specific time. +public class DriveUntilLiDAR extends Command { + private final SwerveDrive swerveDrive; + private final Translation2d leftStick; + private final Translation2d rightStick; + private final LiDAR m_lidar; + private final double mindistance; + + public DriveUntilLiDAR( + SwerveDrive swerveDrive, + Translation2d leftStick, + Translation2d rightStick, + LiDAR lidar, + double mindistance) { + addRequirements(swerveDrive); + + this.swerveDrive = swerveDrive; + this.leftStick = leftStick; + this.rightStick = rightStick; + this.m_lidar = lidar; + this.mindistance = mindistance; + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + swerveDrive.driveFine(leftStick, rightStick, 0.3); + } + + @Override + public boolean isFinished() { + if (Math.abs(m_lidar.getDistance()) < mindistance) { + swerveDrive.softStop(); + return true; + } + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java b/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java new file mode 100644 index 0000000..19efd85 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java @@ -0,0 +1,107 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.alignment; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.lidar.LiDAR; +import frc4388.robot.subsystems.swerve.SwerveDrive; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class LidarAlign extends Command { + private SwerveDrive swerveDrive; + private LiDAR lidar; + + private int currentFinderTick; + // private int tickFoundPipe; + private boolean foundReef; + private boolean headedRight; + private double speed; + private int bounces; + private double additionalDistance = 0; + // private final boolean constructedHeadedRight; + + /** Creates a new LidarAlign. */ + public LidarAlign(SwerveDrive swerveDrive, LiDAR lidar) {//, boolean headedRight) { + // Use addRequirements() here to declare subsystem dependencies. + + this.swerveDrive = swerveDrive; + this.lidar = lidar; + + addRequirements(swerveDrive, lidar); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.currentFinderTick = 0; + this.speed = 0.4; // TODO: find good speed for this + this.foundReef = false; + this.headedRight = (DriveToReef.tagRelativeXError < 0); + this.additionalDistance = 0; + this.bounces = 0; + } + + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (lidar.withinDistance()) { + swerveDrive.softStop(); + foundReef = true; + return; + } + + if (currentFinderTick > (15 + additionalDistance)) { //arbutrary threshhold for now. + headedRight = !headedRight; + currentFinderTick *= -1; + bounces++; + additionalDistance += 5; + if (bounces == 5) return; + } + double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI; + double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef + SmartDashboard.putNumber("Rel Angle", relAngle); + SmartDashboard.putNumber("heading", currentHeading); + if (!headedRight) { + swerveDrive.driveRelativeLockedAngle(new Translation2d(0, -speed), Rotation2d.fromDegrees(relAngle)); + } else { + swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle)); + } + + currentFinderTick++; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (lidar.getDistance() < 4) { + swerveDrive.stopModules(); + return true; + } else if (foundReef && lidar.withinDistance()) { // spot on + swerveDrive.stopModules(); + return true; + } else if (foundReef && !lidar.withinDistance()) { // over shot + speed = speed / 2; + headedRight = !headedRight; + currentFinderTick = 0; + foundReef = false; + return false; + } else if (bounces >= 3) { + swerveDrive.stopModules(); + return true; + } else { + return false; + } + } +} diff --git a/src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java b/src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java new file mode 100644 index 0000000..7d2ec77 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.wait; + +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.elevator.Elevator; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class waitElevatorRefrence extends Command { + /** Creates a new waitElevatorRefrence. */ + private Elevator elevator; + public waitElevatorRefrence(Elevator elevator) { + this.elevator = elevator; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return elevator.elevatorAtReference(); + } +} diff --git a/src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java b/src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java new file mode 100644 index 0000000..1ff48cd --- /dev/null +++ b/src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.wait; + +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.elevator.Elevator; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class waitEndefectorRefrence extends Command { + /** Creates a new waitElevatorRefrence. */ + private Elevator elevator; + public waitEndefectorRefrence(Elevator elevator) { + this.elevator = elevator; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return elevator.endeffectorAtReference(); + } +} diff --git a/src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java b/src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java new file mode 100644 index 0000000..992879a --- /dev/null +++ b/src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.wait; + +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.elevator.Elevator; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class waitFeedCoral extends Command { + /** Creates a new waitElevatorRefrence. */ + private Elevator elevator; + public waitFeedCoral(Elevator elevator) { + this.elevator = elevator; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return elevator.hasCoral(); + } +} diff --git a/src/main/java/frc4388/robot/commands/wait/waitSupplier.java b/src/main/java/frc4388/robot/commands/wait/waitSupplier.java new file mode 100644 index 0000000..a23db69 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/wait/waitSupplier.java @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.wait; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.Command; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class waitSupplier extends Command { + /** Creates a new waitSupplier. */ + private final Supplier truth; + public waitSupplier(Supplier truth) { + this.truth = truth; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return truth.get(); + } +} diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java new file mode 100644 index 0000000..5aeda6b --- /dev/null +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -0,0 +1,19 @@ +package frc4388.robot.constants; + +/** + * Automatically generated file containing build version information. + */ +public final class BuildConstants { + public static final String MAVEN_GROUP = ""; + public static final String MAVEN_NAME = "2025RidgeScape"; + public static final String VERSION = "unspecified"; + public static final int GIT_REVISION = 173; + public static final String GIT_SHA = "2b7bb1224195b9e001e60b895ee04d63abdfc513"; + public static final String GIT_DATE = "2025-07-17 09:15:19 MDT"; + public static final String GIT_BRANCH = "advantagekit"; + public static final String BUILD_DATE = "2025-07-17 11:55:31 MDT"; + public static final long BUILD_UNIX_TIME = 1752774931371L; + public static final int DIRTY = 1; + + private BuildConstants(){} +} diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java new file mode 100644 index 0000000..05bfe77 --- /dev/null +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -0,0 +1,146 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.constants; + +import com.ctre.phoenix6.configs.Slot0Configs; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotBase; +import frc4388.utility.compute.Trim; +import frc4388.utility.status.CanDevice; +import frc4388.utility.structs.Gains; +import frc4388.utility.structs.LEDPatterns; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be + * declared globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static final String CANBUS_NAME = "rio"; + + public static final class LiDARConstants { + public static final int REEF_LIDAR_DIO_CHANNEL = 7; + public static final int REVERSE_LIDAR_DIO_CHANNEL = 4; + + public static final int HUMAN_PLAYER_STATION_DISTANCE = 40; + + public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole + public static final int LIDAR_MICROS_TO_CM = 10; + public static final int SECONDS_TO_MICROS = 1000000; + } + + public static final class AutoConstants { + // public static final Gains XY_GAINS = new Gains(5,0.6,0.0); + public static final Gains XY_GAINS = new Gains(8,0,0.0); + // public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP); + // public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI); + // public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD); + // public static final Gains XY_GAINS = new Gains(3,0.3,0.0); + + // public static final Gains ROT_GAINS = new Gains(0.05,0,0.007); + // public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); + + public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0); + // public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 1.5); + public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0); + + public static final double XY_TOLERANCE = 0.07; // Meters + public static final double ROT_TOLERANCE = 5; // Degrees + + public static final double MIN_XY_PID_OUTPUT = 0.0; + public static final double MIN_ROT_PID_OUTPUT = 0.0; + + public static final double VELOCITY_THRESHHOLD = 0.01; + } + + + public static final class VisionConstants { + public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT"; + public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT"; + + public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0)); + public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0)); + + public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters + + // Photonvision thing + // The standard deviations of our vision estimated poses, which affect correction rate + // X, Y, Theta + // https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2 + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(0.5, 0.5, 4); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1); + } + + public static final class FieldConstants { + public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + + // Test april tag field layout + // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( + // Arrays.asList(new AprilTag[] { + // new AprilTag(1, new Pose3d( + // new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.) + // )), + // }), 100, 100); + + } + + public static final class LEDConstants { + public static final int LED_SPARK_ID = 9; + + public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; + + public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED; + public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW; + public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK; + public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW; + + public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES; + public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES; + } + + public static final class OIConstants { + public static final int XBOX_DRIVER_ID = 0; + public static final int XBOX_OPERATOR_ID = 1; + public static final int BUTTONBOX_ID = 2; + public static final int XBOX_PROGRAMMER_ID = 3; + public static final double LEFT_AXIS_DEADBAND = 0.1; + + } + + + + // Logging constants + public static class SimConstants { + public static final Mode simMode = Mode.SIM; + public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; + + public static enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + + /** Replaying from a log file. */ + REPLAY + } + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java new file mode 100644 index 0000000..c050278 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -0,0 +1,75 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.subsystems; + +import org.littletonrobotics.junction.AutoLogOutput; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.motorcontrol.Spark; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.constants.Constants.LEDConstants; +import frc4388.utility.status.Status; +import frc4388.utility.status.FaultReporter; +import frc4388.utility.status.Queryable; +import frc4388.utility.status.Status.ReportLevel; +import frc4388.utility.structs.LEDPatterns; + +/** + * Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED + * Driver + */ +public class LED extends SubsystemBase implements Queryable { + public LED() { + FaultReporter.register(this); + } + + private static Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + private LEDPatterns mode = LEDConstants.DEFAULT_PATTERN; + + public void setMode(LEDPatterns pattern){ + this.mode = pattern; + } + + @Override + public void periodic() { + update(); + } + + public void update() { + if(!LEDController.isAlive() || LEDController.isSafetyEnabled()) return; + + if(DriverStation.isDisabled()){ + LEDController.set(LEDConstants.DEFAULT_PATTERN.getValue()); + }else + LEDController.set(mode.getValue()); + } + + @AutoLogOutput + public String state() { + return mode.getClass().toString(); + } + + @Override + public String getName() { + return "LEDs"; + } + + @Override + public Status diagnosticStatus() { + Status status = new Status(); + + if(!LEDController.isAlive()) + status.addReport(ReportLevel.ERROR, "LED is DISCONNECTED"); + + status.addReport(ReportLevel.INFO, "LED Mode: " + mode.name()); + + return status; + } + + +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java b/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java new file mode 100644 index 0000000..2945c2d --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java @@ -0,0 +1,58 @@ +package frc4388.robot.subsystems.lidar; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.constants.Constants.LiDARConstants; +import frc4388.utility.status.Status; +import frc4388.utility.status.FaultReporter; +import frc4388.utility.status.Queryable; +import frc4388.utility.status.Status.ReportLevel; + +public class LiDAR extends SubsystemBase implements Queryable { + LidarIO io; + LidarStateAutoLogged state = new LidarStateAutoLogged(); + + private String name = "Lidar"; + + public LiDAR(LidarIO device, String name) { + FaultReporter.register(this); + + this.io = device; + this.name = name; + } + + @Override + public void periodic() { + io.updateInputs(state); + Logger.processInputs("LiDAR/"+name, state); + } + + // @AutoLogOutput(key = "Lidar/{name}") + public double getDistance(){ + return state.distance; + } + + public boolean withinDistance(){ + if(state.distance == -1) return false; + return state.distance < LiDARConstants.LIDAR_DETECT_DISTANCE; + } + + @Override + public String getName() { + return "Lidar " + name; + } + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + + if(state.distance == -1) + s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED"); + + s.addReport(ReportLevel.INFO, "LiDAR Distance: " + state.distance); + + return s; + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java b/src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java new file mode 100644 index 0000000..e3b4ebc --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java @@ -0,0 +1,13 @@ +package frc4388.robot.subsystems.lidar; + +import org.littletonrobotics.junction.AutoLog; + +public interface LidarIO { + @AutoLog + public class LidarState { + public boolean connected; + public double distance; + } + + public default void updateInputs(LidarState state) {} +} diff --git a/src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java b/src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java new file mode 100644 index 0000000..3bf33d5 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java @@ -0,0 +1,27 @@ +package frc4388.robot.subsystems.lidar; + +import edu.wpi.first.wpilibj.Counter; +import frc4388.robot.constants.Constants.LiDARConstants; + +// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface +public class LidarReal implements LidarIO { + + + private Counter LidarPWM; + + public LidarReal(int port) { + LidarPWM = new Counter(port); + LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured + LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement + LidarPWM.reset(); + } + + @Override + public void updateInputs(LidarState state) { + + if(LidarPWM.get() < 1) + state.distance = -1; + else + state.distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java new file mode 100644 index 0000000..bbc61e4 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -0,0 +1,467 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems.swerve; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.swerve.SwerveRequest; +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.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.constants.Constants.AutoConstants; +import frc4388.robot.subsystems.vision.Vision; +import frc4388.utility.compute.TimesNegativeOne; +import frc4388.utility.status.Status; +import frc4388.utility.status.FaultReporter; +import frc4388.utility.status.Queryable; + +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.util.PathPlannerLogging; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.RobotConfig; + +public class SwerveDrive extends SubsystemBase implements Queryable { + // private SwerveDrivetrain swerveDriveTrain; + + private SwerveIO io; + private SwerveStateAutoLogged state; + + private Vision vision; + + + + public int gear_index = SwerveDriveConstants.STARTING_GEAR; + public boolean stopped = false; + public boolean robotKnowsWhereItIs = false; + + public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index]; + public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED; + public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to + // 25% + + public double lastOdomSpeed; + + public Pose2d initalPose2d = new Pose2d(); + + + public double rotTarget = 0.0; + public Rotation2d orientRotTarget = new Rotation2d(); + public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + + /** Creates a new SwerveDrive. */ + public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) { + // public SwerveDrive(SwerveDrivetrain + // swerveDriveTrain) { + FaultReporter.register(this); + + this.io = swerveDriveTrain; + this.state = new SwerveStateAutoLogged(); + + this.vision = vision; + + RobotConfig config; + try { + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + // Handle exception as needed + config = null; + } + // DoubleSupplier a = () -> 1.d; + AutoBuilder.configure( + () -> { + return getPose2d(); + }, // Robot pose supplier + this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting + // pose) + () -> state.speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speeds, feedforwards) -> io.setControl(new SwerveRequest.ApplyRobotSpeeds() + .withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. + // Also optionally outputs individual module feedforwards + new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for + // holonomic drive trains + new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants + ), + config, // The robot configuration + () -> { + // Boolean supplier that controls when the path will be mirrored for the red + // alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + // var alliance = DriverStation.getAlliance(); + // if (alliance.isPresent()) { + // return alliance.get() == DriverStation.Alliance.Red; + // } + return TimesNegativeOne.isRed; + }, + this // Reference to this subsystem to set requirements + ); + + PathPlannerLogging.setLogActivePathCallback( + (activePath) -> { + Logger.recordOutput( + "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + }); + + PathPlannerLogging.setLogTargetPoseCallback( + (targetPose) -> { + Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + }); + + + + // // Configure SysId + // sysId = + // new SysIdRoutine( + // new SysIdRoutine.Config( + // null, + // null, + // null, + // (state) -> Logger.recordOutput("Drive/SysIdState", toString())), + // new SysIdRoutine.Mechanism( + // (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); + + } + + public void setOdoPose(Pose2d pose) { + if (pose == null) return; + initalPose2d = pose; + io.resetPose(pose); + } + + // public void oneModuleTest(SwerveModule module, Translation2d leftStick, + // Translation2d rightStick){ + // // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); + // // rightStick.getAngle() + // double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + + // Math.pow(leftStick.getY(), 2)); + // // System.out.println(ang); + // // module.go(ang); + // // Rotation2d rot = Rotation2d.fromRadians(ang); + // Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY()); + // SwerveModuleState state = new SwerveModuleState(speed, rot); + // module.setDesiredState(state); + // } + + public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: + stopModules(); // stop the swerve + + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput + return; // don't bother doing swerve drive math and return early. + + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); + + stopped = false; + if (fieldRelative) { + + leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); + rightStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis); + + // ! drift correction + if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) { + + rotTarget = state.currentPose.getRotation().getDegrees(); + + io.setControl(new SwerveRequest.FieldCentric() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + + // .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective)); + SmartDashboard.putBoolean("drift correction", false); + } else { + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(Rotation2d.fromDegrees(rotTarget)); + ctrl.HeadingController.setPID( + SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP, + SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI, + SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD + ); + io.setControl(ctrl); + SmartDashboard.putBoolean("drift correction", true); + } + + + } else { // Create robot-relative speeds. + io.setControl(new SwerveRequest.RobotCentric() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(-leftStick.getY() * speedAdjust) + .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + } + } + + public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) { + stopped = false; + // Create robot-relative speeds. + if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0); + io.setControl(new SwerveRequest.RobotCentric() + .withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) + .withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) + .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + + } + + + public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical + // reason to have a robot + // relitive version of + // this, and no pre + // provided version + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve + // drive is still going: + stopModules(); // stop the swerve + + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput + return; // don't bother doing swerve drive math and return early. + + leftStick.rotateBy(TimesNegativeOne.ForwardOffset); + + io.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(rightStick.getAngle())); + } + + public void driveRelativeAngle(Translation2d leftStick, Rotation2d heading) { + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); + leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(heading); + ctrl.HeadingController.setPID( + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD + ); + io.setControl(ctrl); + } + + public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) { + leftStick = leftStick.rotateBy(heading); + + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(heading); + // ctrl.HeadingController.setPID( + // DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, + // DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, + // DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD + // ); + io.setControl(ctrl); + } + + public void activateLuigiMode() { + io.setLimits(20); + } + + public void deactivateLuigiMode() { + io.setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT); + } + + public boolean rotateToTarget(double angle) { + io.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(0) + .withVelocityY(0) + .withTargetDirection(Rotation2d.fromDegrees(angle))); + + if (Math.abs(angle - getGyroAngle()) < 5.0) { + return true; + } + + return false; + } + + public boolean isStopped() { + return lastOdomSpeed < AutoConstants.STOP_VELOCITY; + } + + public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) { + // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the + // swerve drive is still going: + // stopModules(); // stop the swerve + + // if (leftStick.getNorm() < 0.05) //if no imput + // return; // don't bother doing swerve drive math and return early. + + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); + + io.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * -speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(rot)); + // double + } + + public double getGyroAngle() { + return getPose2d().getRotation().getRadians(); + } + + public Pose2d getPose2d() { + if(state.currentPose == null) + return initalPose2d; + return state.currentPose; + } + + public void resetGyro() { + io.tareEverything(); + robotKnowsWhereItIs = false; + rotTarget = 0; + // vision.resetRotations(); + } + + + public void softStop() { + stopped = true; + io.setControl(new SwerveRequest.FieldCentric() + .withVelocityX(0) + .withVelocityY(0) + .withRotationalRate(0) + ); // stop the modules without breaking + } + + public void stopModules() { + // stopped = true; + // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); + softStop(); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run\ + SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI); + SmartDashboard.putNumber("RotTartget", rotTarget); + + io.updateInputs(state); + Logger.processInputs("SwerveDrive", state); + + vision.setLastOdomPose(state.currentPose); + setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate); + + if (vision.isTag()) { + Pose2d pose = vision.getPose2d(); + if (!robotKnowsWhereItIs) { + robotKnowsWhereItIs = true; + Pose2d curPose = getPose2d(); + rotTarget += pose.getRotation().getDegrees() - curPose.getRotation().getDegrees(); + } + + io.addVisionMeasurement(vision.getPosesToAdd()); + } + + // if(e.isPresent()) + } + + private void reset_index() { + gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the + // robot start in?) + } + + public void shiftDown() { + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) + reset_index(); // If outof bounds: reset index + int i = gear_index - 1; + if (i == -1) + i = 0; + setPercentOutput(SwerveDriveConstants.GEARS[i]); + gear_index = i; + } + + public void shiftUp() { + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) + reset_index(); // If outof bounds: reset index + int i = gear_index + 1; + if (i == SwerveDriveConstants.GEARS.length) + i = SwerveDriveConstants.GEARS.length - 1; + setPercentOutput(SwerveDriveConstants.GEARS[i]); + gear_index = i; + } + + public void setPercentOutput(double speed) { + speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; + gear_index = -1; + } + + public void setToSlow() { + setPercentOutput(SwerveDriveConstants.SLOW_SPEED); + gear_index = 0; + } + + public void setToFast() { + setPercentOutput(SwerveDriveConstants.FAST_SPEED); + gear_index = 1; + } + + public void setToTurbo() { + setPercentOutput(SwerveDriveConstants.TURBO_SPEED); + gear_index = 2; + } + + public void shiftUpRot() { + rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED; + } + + public void shiftDownRot() { + rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; + } + + private int tmp_gear_index = SwerveDriveConstants.STARTING_GEAR; + + public void startSlowPeriod() { + tmp_gear_index = gear_index; + setToSlow(); + } + + public void startTurboPeriod() { + tmp_gear_index = gear_index; + setToTurbo(); + } + + public void endSlowPeriod() { + setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]); + gear_index = tmp_gear_index; + } + + + + public void setLastOdomSpeed(Pose2d curPose, Pose2d lastPose, double freq){ + if(curPose != null && lastPose != null){ + lastOdomSpeed = curPose.getTranslation().getDistance(lastPose.getTranslation())/freq; + } + } + + @AutoLogOutput(key="SwerveDrive/speed ") + public double getOdometrySpeed() { + return lastOdomSpeed; + } + + + + @Override + public String getName() { + return "Swerve Drive Controller"; + } + + @Override + public Status diagnosticStatus() { + Status status = new Status(); + + // status.addReport(ReportLevel.INFO, + // "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there."); + + return status; + } +} + diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java new file mode 100644 index 0000000..35b02db --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -0,0 +1,246 @@ +package frc4388.robot.subsystems.swerve; + +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Rotations; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import frc4388.utility.status.CanDevice; +import frc4388.utility.structs.Gains; + + +// No mans land +// Beware, there be dragons. +public final class SwerveDriveConstants { + public static final double MAX_ROT_SPEED = Math.PI * 2; + public static final double AUTO_MAX_ROT_SPEED = 1.5; + public static final double MIN_ROT_SPEED = 1.0; + public static double ROTATION_SPEED = MAX_ROT_SPEED; + public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED; + public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED; + + public static final double CORRECTION_MIN = 10; + public static final double CORRECTION_MAX = 50; + + public static final double SLOW_SPEED = 0.25; + public static final double FAST_SPEED = 0.5; + public static final double TURBO_SPEED = 1.0; + + public static final double[] GEARS = {SLOW_SPEED, FAST_SPEED, TURBO_SPEED}; + public static final int STARTING_GEAR = 0; + // Dimensions + public static final double WIDTH = 18.5; // TODO: validate + public static final double HEIGHT = 18.5; // TODO: validate + public static final double HALF_WIDTH = WIDTH / 2.d; + public static final double HALF_HEIGHT = HEIGHT / 2.d; + + // Mechanics + private static final double COUPLE_RATIO = 3; //todo: find + private static final double DRIVE_RATIO = 6.12; + private static final double STEER_RATIO = (150/7); + private static final Distance WHEEL_RADIUS = Inches.of(2); + + public static final double MAX_SPEED_MEETERS_PER_SEC = 6.22; // TODO: Validate + + public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084; + public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; + + // Operation + public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270 + + public static final boolean DRIFT_CORRECTION_ENABLED = true; + public static final boolean INVERT_X = false; + public static final boolean INVERT_Y = true; + public static final boolean INVERT_ROTATION = false; + + // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0); + + private static final class ModuleSpecificConstants { //2025 + //Front Left + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(-0.368896484375); + private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; + private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; + private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; + private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); + + //Front Right + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.011474609375); + private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true; + private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; + private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; + private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); + + //Back Left + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.333251953125+0.5); + private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; + private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; + private static final boolean BACK_LEFT_ENCODER_INVERTED = false; + private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH); + private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); + + //Back Right + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625+0.5); + private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; + private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; + private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; + private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH); + private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); + } + + public static final class IDs { + public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 4); + public static final CanDevice RIGHT_FRONT_STEER = new CanDevice("RIGHT_FRONT_STEER", 5); + public static final CanDevice RIGHT_FRONT_ENCODER = new CanDevice("RIGHT_FRONT_ENCODER", 11); + + public static final CanDevice LEFT_FRONT_WHEEL = new CanDevice("LEFT_FRONT_WHEEL", 2); + public static final CanDevice LEFT_FRONT_STEER = new CanDevice("LEFT_FRONT_STEER", 3); + public static final CanDevice LEFT_FRONT_ENCODER = new CanDevice("LEFT_FRONT_ENCODER", 10); + + public static final CanDevice LEFT_BACK_WHEEL = new CanDevice("LEFT_BACK_WHEEL", 6); + public static final CanDevice LEFT_BACK_STEER = new CanDevice("LEFT_BACK_STEER", 7); + public static final CanDevice LEFT_BACK_ENCODER = new CanDevice("LEFT_BACK_ENCODER", 12); + + public static final CanDevice RIGHT_BACK_WHEEL = new CanDevice("RIGHT_BACK_WHEEL", 8); + public static final CanDevice RIGHT_BACK_STEER = new CanDevice("RIGHT_BACK_STEER", 9); + public static final CanDevice RIGHT_BACK_ENCODER = new CanDevice("RIGHT_BACK_ENCODER", 13); + + public static final CanDevice DRIVE_PIGEON = new CanDevice("DRIVE_PIGEON", 14); + } + + public static final class PIDConstants { + public static final int SWERVE_SLOT_IDX = 0; + public static final int SWERVE_PID_LOOP_IDX = 1; + public static final Gains SWERVE_GAINS = new Gains(50, 0.0, 0.32, 0.0, 0, 0.0); + + public static final Slot0Configs CURRENT_SWERVE_ROT_GAINS = new Slot0Configs() + .withKP(50).withKI(0).withKD(0.32) + .withKS(0).withKV(0).withKA(0); + + public static final Slot0Configs TEST_SWERVE_ROT_GAINS = new Slot0Configs() + .withKP(10).withKI(0).withKD(0.3) + .withKS(0).withKV(0).withKA(0); + + public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + public static final Slot0Configs PREPROVIDED_STEER_GAINS = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.6) + .withKS(0.1).withKV(1.91).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + public static final Slot0Configs PREPROVIDED_DRIVE_GAINS = new Slot0Configs() + .withKP(0.1).withKI(0).withKD(0) + .withKS(0).withKV(0.124); + + public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1); + public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1); + } + + public static final class Configurations { + public static final double OPEN_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help. + public static final double CLOSED_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help. + public static final double NEUTRAL_DEADBAND = 0.04; + + // POWER! (limiting) + private static final TalonFXConfiguration DRIVE_MOTOR_CONFIGS = new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withDutyCycleNeutralDeadband(NEUTRAL_DEADBAND) + ).withOpenLoopRamps( + new OpenLoopRampsConfigs() + .withDutyCycleOpenLoopRampPeriod(OPEN_LOOP_RAMP_RATE) + ).withClosedLoopRamps( + new ClosedLoopRampsConfigs() + .withDutyCycleClosedLoopRampPeriod(CLOSED_LOOP_RAMP_RATE) + ); + private static final TalonFXConfiguration STEER_MOTOR_CONFIGS = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withDutyCycleNeutralDeadband(NEUTRAL_DEADBAND) + // ).withOpenLoopRamps( + // new OpenLoopRampsConfigs() + // .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) + // ).withClosedLoopRamps( + // new ClosedLoopRampsConfigs() + // .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) + ); + public static final double SLIP_CURRENT = 60; // TODO: Tune??? + } + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withPigeon2Id(IDs.DRIVE_PIGEON.id); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() // holy verbosity batman. + .withDriveMotorGearRatio(DRIVE_RATIO) + .withSteerMotorGearRatio(STEER_RATIO) + .withCouplingGearRatio(COUPLE_RATIO) + .withWheelRadius(WHEEL_RADIUS) + .withSteerMotorGains(PIDConstants.PREPROVIDED_STEER_GAINS) // ? + .withDriveMotorGains(PIDConstants.PREPROVIDED_DRIVE_GAINS) // ? + .withSteerMotorClosedLoopOutput(ClosedLoopOutputType.Voltage) + .withDriveMotorClosedLoopOutput(ClosedLoopOutputType.Voltage) + .withSlipCurrent(Configurations.SLIP_CURRENT) + .withSpeedAt12Volts(MAX_SPEED_MEETERS_PER_SEC) + .withDriveMotorType(DriveMotorArrangement.TalonFX_Integrated) + .withSteerMotorType(SteerMotorArrangement.TalonFX_Integrated) + .withFeedbackSource(SteerFeedbackType.RemoteCANcoder) + .withDriveMotorInitialConfigs(Configurations.DRIVE_MOTOR_CONFIGS) + .withSteerMotorInitialConfigs(Configurations.STEER_MOTOR_CONFIGS); + + public static final SwerveModuleConstants FRONT_LEFT = + ConstantCreator.createModuleConstants( + IDs.LEFT_FRONT_STEER.id, IDs.LEFT_FRONT_WHEEL.id, IDs.LEFT_FRONT_ENCODER.id, ModuleSpecificConstants.FRONT_LEFT_ENCODER_OFFSET, + ModuleSpecificConstants.FRONT_LEFT_XPOS, ModuleSpecificConstants.FRONT_LEFT_YPOS, + ModuleSpecificConstants.FRONT_LEFT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_LEFT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_LEFT_ENCODER_INVERTED + ); + public static final SwerveModuleConstants FRONT_RIGHT = + ConstantCreator.createModuleConstants( + IDs.RIGHT_FRONT_STEER.id, IDs.RIGHT_FRONT_WHEEL.id, IDs.RIGHT_FRONT_ENCODER.id, ModuleSpecificConstants.FRONT_RIGHT_ENCODER_OFFSET, + ModuleSpecificConstants.FRONT_RIGHT_XPOS, ModuleSpecificConstants.FRONT_RIGHT_YPOS, + ModuleSpecificConstants.FRONT_RIGHT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_RIGHT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_RIGHT_ENCODER_INVERTED + ); + public static final SwerveModuleConstants BACK_LEFT = + ConstantCreator.createModuleConstants( + IDs.LEFT_BACK_STEER.id, IDs.LEFT_BACK_WHEEL.id, IDs.LEFT_BACK_ENCODER.id, ModuleSpecificConstants.BACK_LEFT_ENCODER_OFFSET, + ModuleSpecificConstants.BACK_LEFT_XPOS, ModuleSpecificConstants.BACK_LEFT_YPOS, + ModuleSpecificConstants.BACK_LEFT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.BACK_LEFT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.BACK_LEFT_ENCODER_INVERTED + ); + public static final SwerveModuleConstants BACK_RIGHT = + ConstantCreator.createModuleConstants( + IDs.RIGHT_BACK_STEER.id, IDs.RIGHT_BACK_WHEEL.id, IDs.RIGHT_BACK_ENCODER.id, ModuleSpecificConstants.BACK_RIGHT_ENCODER_OFFSET, + ModuleSpecificConstants.BACK_RIGHT_XPOS, ModuleSpecificConstants.BACK_RIGHT_YPOS, + ModuleSpecificConstants.BACK_RIGHT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.BACK_RIGHT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.BACK_RIGHT_ENCODER_INVERTED + ); + + // misc + public static final int TIMEOUT_MS = 30; + public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; +} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java new file mode 100644 index 0000000..fa7de1a --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java @@ -0,0 +1,33 @@ +package frc4388.robot.subsystems.swerve; + +import java.util.List; + +import org.littletonrobotics.junction.AutoLog; + +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; + +public interface SwerveIO { + @AutoLog + public class SwerveState { + public Pose2d currentPose = null; + public Pose2d lastPose = null; + public ChassisSpeeds speeds = null; + public double odometryRate = 1; + } + + public default void setControl(SwerveRequest ctrl) {} + + public default void setLimits(double limitInAmps) {} + + public default void tareEverything() {} + + public default void resetPose(Pose2d pose) {} + + public default void addVisionMeasurement(List poses) {} + + public default void updateInputs(SwerveState state) {} +} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java new file mode 100644 index 0000000..05d9e8f --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java @@ -0,0 +1,63 @@ +package frc4388.robot.subsystems.swerve; + +import java.util.List; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.swerve.SwerveModule; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import frc4388.robot.subsystems.vision.Vision; +import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; + +public class SwerveReal implements SwerveIO { + SwerveDrivetrain swerveDriveTrain; + + public SwerveReal(SwerveDrivetrain swerveDriveTrain) { + this.swerveDriveTrain = swerveDriveTrain; + swerveDriveTrain.getOdometryFrequency(); + } + + @Override + public void updateInputs(SwerveState state) { + double time = Vision.getTime(); + state.odometryRate = 1 / swerveDriveTrain.getOdometryFrequency(); + state.currentPose = swerveDriveTrain.samplePoseAt(time).orElse(null); + state.lastPose = swerveDriveTrain.samplePoseAt(time - state.odometryRate).orElse(null); + state.speeds = swerveDriveTrain.getState().Speeds; + } + + @Override + public void setControl(SwerveRequest ctrl) { + swerveDriveTrain.setControl(ctrl); + } + + @Override + public void tareEverything() { + swerveDriveTrain.tareEverything(); + } + + @Override + public void setLimits(double limitInAmps) { + for (SwerveModule module : swerveDriveTrain.getModules()) { + var talonFXConfigurator = module.getDriveMotor().getConfigurator(); + var talonFXConfigs = new TalonFXConfiguration(); + + talonFXConfigurator.refresh(talonFXConfigs); + talonFXConfigs.CurrentLimits.StatorCurrentLimit = limitInAmps; + talonFXConfigs.CurrentLimits.SupplyCurrentLimit = limitInAmps+10; + talonFXConfigurator.apply(talonFXConfigs); + } + } + + @Override + public void addVisionMeasurement(List poses) { + for(PoseObservation pose : poses) { + swerveDriveTrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp())); + } + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/vision/Vision.java b/src/main/java/frc4388/robot/subsystems/vision/Vision.java new file mode 100644 index 0000000..a069af7 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/vision/Vision.java @@ -0,0 +1,95 @@ +package frc4388.robot.subsystems.vision; + +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; +import frc4388.utility.status.FaultReporter; +import frc4388.utility.status.Queryable; +import frc4388.utility.status.Status; + +public class Vision extends SubsystemBase implements Queryable { + VisionIO[] io; + VisionStateAutoLogged[] state; + + + public Pose2d lastVisionPose = new Pose2d(); + public Pose2d lastPhysOdomPose = new Pose2d(); + + public Vision(VisionIO... devices) { + FaultReporter.register(this); + io = devices; + state = new VisionStateAutoLogged[io.length]; + + for(int i = 0; i < io.length; i++) { + state[i] = new VisionStateAutoLogged(); + } + } + + @Override + public void periodic() { + for(int i = 0; i < io.length; i++) { + io[i].updateInputs(state[i]); + Logger.processInputs("Vision/Camera" + i , state[i]); + } + } + + public List getPosesToAdd(){ + List poses = new ArrayList<>(); + for(int i = 0; i < state.length; i++) { + if(state[i].lastEstimatedPose != null) { + poses.add(state[i].lastEstimatedPose); + } + } + + return poses; + } + + public void setLastOdomPose(Pose2d pose){ + if(pose != null) + lastPhysOdomPose = pose; + } + + public boolean isTag(){ + for(int i = 0; i < state.length; i++){ + if(state[i].isTagDetected && state[i].isTagProcessed) + return true; + } + return false; + } + + @AutoLogOutput + public Pose2d getPose2d() { + if(lastPhysOdomPose != null) + return lastPhysOdomPose; + + // if(lastVisionPose != null) + // return lastVisionPose; + return new Pose2d(); + + } + + public static double getTime() { + return Utils.getCurrentTimeSeconds(); + } + + + @Override + public Status diagnosticStatus() { + return new Status(); + // // TODO Auto-generated method stub + // throw new UnsupportedOperationException("Unimplemented method 'diagnosticStatus'"); + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/vision/VisionIO.java b/src/main/java/frc4388/robot/subsystems/vision/VisionIO.java new file mode 100644 index 0000000..f979135 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/vision/VisionIO.java @@ -0,0 +1,22 @@ +package frc4388.robot.subsystems.vision; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.geometry.Pose2d; + +public interface VisionIO { + @AutoLog + public class VisionState { + public boolean isTagDetected = false; + public boolean isTagProcessed = false; + // public double latency = 0; + public PoseObservation lastEstimatedPose = null; + } + + public static record PoseObservation( + Pose2d pose, + double timestamp + ) {} + + public default void updateInputs(VisionState state) {} +} diff --git a/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java new file mode 100644 index 0000000..1c12437 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java @@ -0,0 +1,158 @@ +package frc4388.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Transform3d; +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; +import frc4388.robot.constants.Constants.FieldConstants; +import frc4388.robot.constants.Constants.VisionConstants; + +public class VisionReal implements VisionIO { + // private PhotonCamera[] cameras; + // private PhotonPoseEstimator[] estimators; + + private PhotonCamera camera; + private PhotonPoseEstimator estimator; + + // public List poses = new ArrayList<>(); + + + public VisionReal(PhotonCamera camera, Transform3d position){ + this.camera = camera; + estimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, position); + estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + } + + // private Instant lastVisionTime = null; + + + public void updateInputs(VisionState state) { + state.isTagProcessed = false; + state.isTagDetected = false; + + state.lastEstimatedPose = null; + + var results = camera.getAllUnreadResults(); + + // If there are no more updates from the camera + if (results.size() == 0) + return; + + + var result = results.get(results.size()-1); + + state.isTagDetected = state.isTagDetected | result.hasTargets(); + + // If there are no tags + if(!result.hasTargets()) + return; + + Optional estimatedRobotPose = getEstimatedGlobalPose(result, estimator); + + // If the tag was failed to be processed + if(estimatedRobotPose.isEmpty()) + return; + + EstimatedRobotPose pose = estimatedRobotPose.get(); + + state.lastEstimatedPose = new PoseObservation(pose.estimatedPose.toPose2d(), pose.timestampSeconds); + + state.isTagProcessed = true; + } + + + /** + * The latest estimated robot pose on the field from vision data. This may be empty. This should + * only be called once per loop. + * + *

Also includes updates for the standard deviations, which can (optionally) be retrieved with + * {@link getEstimationStdDevs} + * + * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets + * used for estimation. + */ + public Optional getEstimatedGlobalPose(PhotonPipelineResult change, PhotonPoseEstimator estimator) { + Optional visionEst = Optional.empty(); + + var targets = change.getTargets(); + for(int i = targets.size()-1; i >= 0; i--){ + Transform3d pos = targets.get(i).getBestCameraToTarget(); + double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2)); + if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) { + change.targets.remove(i); + } + } + + if(targets.size() <= 0) + return visionEst; // Will be empty + + visionEst = estimator.update(change); + // updateEstimationStdDevs(visionEst, change.getTargets(), estimator); + + return visionEst; + } + + public String getName() { + return camera.getName(); + } + + + // /** + // * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard + // * deviations based on number of tags, estimation strategy, and distance from the tags. + // * + // * @param estimatedPose The estimated pose to guess standard deviations for. + // * @param targets All targets in this camera frame + // */ + // private void updateEstimationStdDevs( + // Optional estimatedPose, + // List targets, + // PhotonPoseEstimator estimator) { + // if (estimatedPose.isEmpty()) { + // // No pose input. Default to single-tag std devs + // curStdDevs = VisionConstants.kSingleTagStdDevs; + + // } else { + // // Pose present. Start running Heuristic + // var estStdDevs = VisionConstants.kSingleTagStdDevs; + // int numTags = 0; + // double avgDist = 0; + + // // Precalculation - see how many tags we found, and calculate an average-distance metric + // for (var tgt : targets) { + // var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId()); + // if (tagPose.isEmpty()) continue; + + // double distance = tagPose + // .get() + // .toPose2d() + // .getTranslation() + // .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + + // if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) { + // numTags++; + // avgDist += distance; + // } + // } + + // if (numTags == 0) { + // // No tags visible. Default to single-tag std devs + // curStdDevs = VisionConstants.kSingleTagStdDevs; + // } else { + // // One or more tags visible, run the full heuristic. + // avgDist /= numTags; + // // Decrease std devs if multiple targets are visible + // if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs; + // // Increase std devs based on (average) distance + // if (numTags == 1 && avgDist > 4) + // estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + // else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + // curStdDevs = estStdDevs; + // } + // } + // } +} diff --git a/src/main/java/frc4388/utility/DeferredBlock.java b/src/main/java/frc4388/utility/DeferredBlock.java new file mode 100644 index 0000000..c5a558e --- /dev/null +++ b/src/main/java/frc4388/utility/DeferredBlock.java @@ -0,0 +1,41 @@ +package frc4388.utility; + +import java.util.ArrayList; + +// Class for running code snippets whenever the robot is enabled. +public class DeferredBlock { + private static ArrayList m_blocks_norerun = new ArrayList<>(); + private static ArrayList m_blocks_rerun = new ArrayList<>(); + private static boolean m_hasRun = false; + + public static void addBlock(Runnable block) { + addBlock(block, false); + } + + + public static void addBlock(Runnable block, boolean rerun) { + if(rerun) { + m_blocks_rerun.add(block); + } else { + m_blocks_norerun.add(block); + } + } + + public static void execute() { + + // Run blocks that run multiple times. + for (Runnable block : m_blocks_rerun) { + block.run(); + } + + // Run blocks that only run once + if (m_hasRun) return; + + for (Runnable block : m_blocks_norerun) { + block.run(); + } + + m_blocks_norerun.clear(); // for garbage collection + m_hasRun = true; + } +} diff --git a/src/main/java/frc4388/utility/compute/DataUtils.java b/src/main/java/frc4388/utility/compute/DataUtils.java new file mode 100644 index 0000000..3b83190 --- /dev/null +++ b/src/main/java/frc4388/utility/compute/DataUtils.java @@ -0,0 +1,35 @@ +package frc4388.utility.compute; + +import java.nio.ByteBuffer; + +public class DataUtils { + public static byte[] doubleToByteArray(double value) { + byte[] bytes = new byte[8]; + ByteBuffer.wrap(bytes).putDouble(value); + return bytes; + } + + public static double byteArrayToDouble(byte[] bytes) { + return ByteBuffer.wrap(bytes).getDouble(); + } + + public static byte[] intToByteArray(int value) { + byte[] bytes = new byte[4]; + ByteBuffer.wrap(bytes).putInt(value); + return bytes; + } + + public static int byteArrayToInt(byte[] bytes) { + return ByteBuffer.wrap(bytes).getInt(); + } + + public static byte[] shortToByteArray(short value) { + byte[] bytes = new byte[2]; + ByteBuffer.wrap(bytes).putShort(value); + return bytes; + } + + public static short byteArrayToShort(byte[] bytes) { + return ByteBuffer.wrap(bytes).getShort(); + } +} diff --git a/src/main/java/frc4388/utility/compute/ReefPositionHelper.java b/src/main/java/frc4388/utility/compute/ReefPositionHelper.java new file mode 100644 index 0000000..73dc6a5 --- /dev/null +++ b/src/main/java/frc4388/utility/compute/ReefPositionHelper.java @@ -0,0 +1,106 @@ +package frc4388.utility.compute; + +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.util.Units; +import frc4388.robot.constants.Constants.AutoConstants; +import frc4388.robot.constants.Constants.FieldConstants; + +public class ReefPositionHelper { + public enum Side { + LEFT, + RIGHT, + CENTER, + FAR_LEFT + } + + public static final Pose2d[] RED_TAGS = { + FieldConstants.kTagLayout.getTagPose(6).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(7).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(8).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(9).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(10).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(11).get().toPose2d() + }; + + public static final Pose2d[] BLUE_TAGS = { + FieldConstants.kTagLayout.getTagPose(17).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(18).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(19).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(20).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(21).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(22).get().toPose2d() + }; + + public static double distanceTo(Pose2d first, Pose2d second){ + return Math.sqrt(Math.pow(first.getX() - second.getX(),2) + Math.pow(first.getY() - second.getY(),2)); + } + + + /* + * Function to loop through a list of tag locations to figure out closest one + */ + public static Pose2d getNearestTag(Pose2d[] locations, Pose2d position){ + if(locations.length <= 0) return new Pose2d(); + + Pose2d minPos = locations[0]; + double minDistance = distanceTo(position,minPos); + + for(int i = 1; i < locations.length; i++){ + double dist = distanceTo(locations[i],position); + if(dist < minDistance){ + minPos = locations[i]; + minDistance = dist; + } + } + + System.out.println(minPos.getRotation().getDegrees()); + + return minPos; + } + + /* + * Function to find closest tag location based on side + */ + public static Pose2d getNearestTag(Pose2d position) { + + if(TimesNegativeOne.isRed) + return getNearestTag(RED_TAGS, position); + else + return getNearestTag(BLUE_TAGS, position); + } + + public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) { + return offset(getNearestTag(position), + getSide(side) + xtrim, + ydistance); + } + + public static double getSide(Side side){ + switch(side) { + case LEFT: + return -(AutoConstants.X_SCORING_POSITION_OFFSET); + case FAR_LEFT: + return -(AutoConstants.X_SCORING_POSITION_OFFSET+Units.inchesToMeters(8)); + case RIGHT: + return (AutoConstants.X_SCORING_POSITION_OFFSET); + case CENTER: + return 0; + } + assert false; + return 0; + } + + + public static Pose2d offset(Pose2d oldPose, double xoffset, double yoffset){ + Translation2d oldTranslation = oldPose.getTranslation(); + + double rot = oldPose.getRotation().getRadians(); + + return new Pose2d(new Translation2d( + oldTranslation.getX() + Math.cos(rot + Math.PI/2) * xoffset + Math.cos(rot) * yoffset, + oldTranslation.getY() + Math.sin(rot + Math.PI/2) * xoffset + Math.sin(rot) * yoffset + ), oldPose.getRotation().rotateBy(Rotation2d.k180deg)); + } +} diff --git a/src/main/java/frc4388/utility/compute/RobotTime.java b/src/main/java/frc4388/utility/compute/RobotTime.java new file mode 100644 index 0000000..ebb43d8 --- /dev/null +++ b/src/main/java/frc4388/utility/compute/RobotTime.java @@ -0,0 +1,79 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.utility.compute; + +/** + *

Keeps track of Robot times like time passed, delta time, etc + *

All times are in milliseconds + */ +public class RobotTime { + private long m_currTime = System.currentTimeMillis(); + public long m_deltaTime = 0; + + private long m_startRobotTime = m_currTime; + public long m_robotTime = 0; + public long m_lastRobotTime = 0; + + private long m_startMatchTime = 0; + public long m_matchTime = 0; + public long m_lastMatchTime = 0; + + public long m_frameNumber = 0; + + /** + * Private constructor prevents other classes from instantiating + */ + private RobotTime(){} + + private static RobotTime instance = null; + + /** + * Gets the instance of Robot Time. If there is no instance running one will be created. + * @return instance of Robot Time + */ + public static RobotTime getInstance() { + if (instance == null) { + instance = new RobotTime(); + } + return instance; + } + + /** + * Call this once per periodic loop. + */ + public void updateTimes() { + m_lastRobotTime = m_robotTime; + m_lastMatchTime = m_matchTime; + + m_currTime = System.currentTimeMillis(); + m_robotTime = m_currTime - m_startRobotTime; + m_deltaTime = m_robotTime - m_lastRobotTime; + m_frameNumber++; + + if (m_startMatchTime != 0) { + m_matchTime = m_currTime - m_startMatchTime; + } + } + + /** + * Call this in both the auto and periodic inits + */ + public void startMatchTime() { + if (m_startMatchTime == 0) { + m_startMatchTime = m_currTime; + } + } + + /** + * Call this in disabled init + */ + public void endMatchTime() { + m_startMatchTime = 0; + m_matchTime = 0; + } +} diff --git a/src/main/java/frc4388/utility/compute/RobotUnits.java b/src/main/java/frc4388/utility/compute/RobotUnits.java new file mode 100644 index 0000000..0f76d06 --- /dev/null +++ b/src/main/java/frc4388/utility/compute/RobotUnits.java @@ -0,0 +1,27 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.utility.compute; + +/** Aarav's good units class (better than WPILib) + * @author Aarav Shah */ + +public class RobotUnits { + // constants + + // angle conversions + public static double degreesToRadians(final double degrees) {return degrees * Math.PI / 180;} + + public static double radiansToDegrees(final double radians) {return radians / Math.PI * 180;} + + // falcon conversions + public static double falconTicksToRotations(final double ticks) {return ticks / 2048;} + + public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;} + + // distance conversions + public static double metersToFeet(final double meters) {return meters * 3.28084;} + + public static double feetToMeters(final double feet) {return feet / 3.28084;} +} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/compute/TimesNegativeOne.java b/src/main/java/frc4388/utility/compute/TimesNegativeOne.java new file mode 100644 index 0000000..2ba0d55 --- /dev/null +++ b/src/main/java/frc4388/utility/compute/TimesNegativeOne.java @@ -0,0 +1,51 @@ +package frc4388.utility.compute; + +import java.util.Optional; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.robot.subsystems.swerve.SwerveDriveConstants; + +// Class that holds weather the drivers sticks should be inverted +public class TimesNegativeOne { + + public static boolean XAxis = SwerveDriveConstants.INVERT_X; + public static boolean YAxis = SwerveDriveConstants.INVERT_Y; + public static boolean RotAxis = SwerveDriveConstants.INVERT_ROTATION; + public static boolean isRed = false; + public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET); + + private static boolean isRed() { + Optional alliance = DriverStation.getAlliance(); + if(alliance.isEmpty()) return false; + return (alliance.get() == Alliance.Red); + } + + public static void update(){ + isRed = isRed(); + XAxis = SwerveDriveConstants.INVERT_X ^ isRed; + YAxis = SwerveDriveConstants.INVERT_Y ^ isRed; + RotAxis = SwerveDriveConstants.INVERT_ROTATION; + ForwardOffset = Rotation2d.fromDegrees((SwerveDriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0))); + SmartDashboard.putBoolean("Is red alliance", isRed); + } + + public static double invert(double num, boolean invert){ + return invert ? -num : num; + } + + public static Translation2d invert(Translation2d stick, boolean invertXY){ + if(invertXY) return stick.times(-1); + else return stick; + } + + public static Translation2d invert(Translation2d stick, boolean invertX, boolean invertY){ + return new Translation2d( + invert(stick.getX(), invertX), + invert(stick.getY(), invertY) + ); + } +} diff --git a/src/main/java/frc4388/utility/compute/Trim.java b/src/main/java/frc4388/utility/compute/Trim.java new file mode 100644 index 0000000..429d526 --- /dev/null +++ b/src/main/java/frc4388/utility/compute/Trim.java @@ -0,0 +1,145 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.utility.compute; + +import java.io.FileInputStream; +import java.io.FileOutputStream; +import java.util.ArrayList; + +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; + +/** + * Reboot persistant Trims. + * @author Zachary Wilke + */ +public class Trim { + private static ArrayList trims = new ArrayList(); + private static ShuffleboardTab trimTab = Shuffleboard.getTab("Trims"); + + private String trimName; + private double upperBound; + private double lowerBound; + private double step; + + private boolean modified = false; + private double currentValue; + private boolean persistant = false; + + private GenericEntry trimElement = null; + + /** + * Creates a variably Trim with a given name, upper and lower bounds, step size and intial value + * @param trimName please keep the trim name without special symbols + * @param upperBound the upper limit inclusive + * @param lowerBound the lower limit inclusive + * @param step the step size + * @param inital the inital value, will get overridden if the persistant trim exists on disk. + * @param persistnat Weather the trim is persistant or not + */ + public Trim(String trimName, double upperBound, double lowerBound, double step, double inital, boolean persistant) { + this.trimName = trimName; + this.upperBound = upperBound; + this.lowerBound = lowerBound; + this.step = step; + this.persistant = persistant; + currentValue = inital; + load(); + trimElement = trimTab.add(trimName, currentValue).getEntry(); + + trims.add(this); + } + + /** + * Creates a non-Trim with a given name, upper and lower bounds, step size and intial value + * @param trimName please keep the trim name without special symbols + * @param upperBound the upper limit inclusive + * @param lowerBound the lower limit inclusive + * @param step the step size + * @param inital the inital value, will get overridden if the persistant trim exists on disk. + */ + public Trim(String trimName, double upperBound, double lowerBound, double step, double inital) { + this.trimName = trimName; + this.upperBound = upperBound; + this.lowerBound = lowerBound; + this.step = step; + currentValue = inital; + load(); + trimElement = trimTab.add(trimName, currentValue).getEntry(); + + trims.add(this); + } + + private void clampModify() { + currentValue = Math.min(upperBound, Math.max(currentValue, lowerBound)); + if (trimElement != null) + trimElement.setValue(currentValue); + modified = true; + } + + public void stepUp() { + this.currentValue += step; + clampModify(); + } + + public void stepDown() { + this.currentValue -= step; + clampModify(); + } + + public void set(double value) { + this.currentValue = value; + clampModify(); + } + + public double get() { + return this.currentValue; + } + + public boolean isModified() { + return modified; + } + + public boolean load() { + if(!persistant) + return false; + + try (FileInputStream stream = new FileInputStream("/home/lvuser/trims/" + trimName)) { + double fileValue = DataUtils.byteArrayToDouble(stream.readNBytes(8)); + currentValue = fileValue; + clampModify(); + modified = false; + if (fileValue != currentValue) { + System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping..."); + modified = true; + } + return true; + } catch (Exception e) { + // e.printStackTrace(); + System.out.println("TRIMS: Unable to read trim file `" + trimName + "`, using current value..."); + return false; + } + + } + + public void dump() { + try (FileOutputStream stream = new FileOutputStream("/home/lvuser/trims/" + trimName)) { + stream.write(DataUtils.doubleToByteArray(currentValue)); + modified = false; + } catch (Exception e) { + // e.printStackTrace(); + System.out.println("TRIMS: Unable to write to trim file `" + trimName + "`!?!"); + } + } + + public static void dumpAll() { + for (int i = 0; i < trims.size(); i++) { + Trim trim = trims.get(i); + if (trim.isModified()) + trim.dump(); + } + } +} diff --git a/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java new file mode 100644 index 0000000..c0384db --- /dev/null +++ b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java @@ -0,0 +1,23 @@ +package frc4388.utility.configurable; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ConfigurableDouble { + private double defualtValue; + private String name; + + /** + * Creates an new ConfigurableDouble through Smart Dashboard. + * @param name the name of the Smart Dashboard key. + * @param defualtValue the initilization value + */ + public ConfigurableDouble(String name, double defualtValue) { + this.name = name; + this.defualtValue = defualtValue; + SmartDashboard.putNumber(name, defualtValue); + } + + public double get() { + return SmartDashboard.getNumber(name, defualtValue); + } +} diff --git a/src/main/java/frc4388/utility/configurable/ConfigurableString.java b/src/main/java/frc4388/utility/configurable/ConfigurableString.java new file mode 100644 index 0000000..34c0290 --- /dev/null +++ b/src/main/java/frc4388/utility/configurable/ConfigurableString.java @@ -0,0 +1,23 @@ +package frc4388.utility.configurable; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ConfigurableString { + private String defualtValue; + private String name; + + /** + * Creates an new ConfigurableString through Smart Dashboard. + * @param name the name of the Smart Dashboard key. + * @param defualtValue the initilization value + */ + public ConfigurableString(String name, String defualtValue) { + this.name = name; + this.defualtValue = defualtValue; + SmartDashboard.putString(name, defualtValue); + } + + public String get() { + return SmartDashboard.getString(name, defualtValue); + } +} diff --git a/src/main/java/frc4388/utility/controller/ButtonBox.java b/src/main/java/frc4388/utility/controller/ButtonBox.java new file mode 100644 index 0000000..33c7744 --- /dev/null +++ b/src/main/java/frc4388/utility/controller/ButtonBox.java @@ -0,0 +1,19 @@ +package frc4388.utility.controller; + +import edu.wpi.first.wpilibj.GenericHID; + +public class ButtonBox extends GenericHID { + public static final int White = 1; + public static final int One = 2; + public static final int Two = 3; + public static final int Three = 4; + public static final int Four = 5; + public static final int Five = 6; + public static final int Six = 7; + public static final int Seven = 8; + public static final int Eight = 9; + + public ButtonBox(int ID){ + super(ID); + } +} diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java new file mode 100644 index 0000000..ae4202b --- /dev/null +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -0,0 +1,28 @@ + +package frc4388.utility.controller; + +import static frc4388.robot.constants.Constants.OIConstants.LEFT_AXIS_DEADBAND; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.XboxController; + +public class DeadbandedXboxController extends XboxController { + public DeadbandedXboxController(int port) { super(port); } + + @Override public double getLeftX() { return getLeft().getX(); } + @Override public double getLeftY() { return getLeft().getY(); } + @Override public double getRightX() { return getRight().getX(); } + @Override public double getRightY() { return getRight().getY(); } + + public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); } + public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); } + + public static Translation2d skewToDeadzonedCircle(double x, double y) { + Translation2d translation2d = new Translation2d(x, y); + double magnitude = translation2d.getNorm(); + + if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); + + return translation2d; + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/controller/IHandController.java b/src/main/java/frc4388/utility/controller/IHandController.java new file mode 100644 index 0000000..13aa763 --- /dev/null +++ b/src/main/java/frc4388/utility/controller/IHandController.java @@ -0,0 +1,21 @@ +package frc4388.utility.controller; + +/** + * Add your docs here. + */ +public interface IHandController { + + public double getLeftXAxis(); + + public double getLeftYAxis(); + + public double getRightXAxis(); + + public double getRightYAxis(); + + public double getLeftTriggerAxis(); + + public double getRightTriggerAxis(); + + public int getDpadAngle(); +} diff --git a/src/main/java/frc4388/utility/controller/VirtualController.java b/src/main/java/frc4388/utility/controller/VirtualController.java new file mode 100644 index 0000000..85adb64 --- /dev/null +++ b/src/main/java/frc4388/utility/controller/VirtualController.java @@ -0,0 +1,145 @@ +package frc4388.utility.controller; + +import edu.wpi.first.wpilibj.GenericHID; + +/** + * A virtual controller that can be bound like an standard controller. + * @author Zachary Wilke + */ +public class VirtualController extends GenericHID { + private short m_buttonStates = 0; + private short m_buttonStatesLastFrame = 0; + private double[] m_axes = new double[6]; + private short[] m_pov = new short[1]; + + /** + * Create an virtual controller + * @param port virtual port (merely a formality). + */ + public VirtualController(int port) { + super(port); + } + + /** + * Set the curent inputs to the new frames. + * @param axes joystick axes, (i.e. joysticks and triggers). + * @param buttonFlags the bit packed button states. + * @param pov the array of dpads. + */ + public void setFrame(double[] axes, short buttonFlags, short[] pov) { + m_axes = axes; + setOutputs(buttonFlags); + m_pov = pov; + } + + /** + * Zero outs the controls. + */ + public void zeroControls() { + m_axes = new double[6]; + m_buttonStates = 0; + m_buttonStatesLastFrame = 0; + m_pov = new short[] {-1}; + } + + /** + * Gets the value of a bitflag from an int + * @param value int to search + * @param index index of bit + * @return if the bit is set + */ + public static boolean getFlag(int value, int index) { + return ((value & 1 << index) != 0); + } + + @Override + public boolean getRawButton(int button) { // man why are buttons indexed at 1. + return getFlag(m_buttonStates, button - 1); + } + + @Override + public boolean getRawButtonPressed(int button) { + return (!getFlag(m_buttonStatesLastFrame, button - 1) && getRawButton(button)); + } + + @Override + public boolean getRawButtonReleased(int button) { + return (getFlag(m_buttonStatesLastFrame, button - 1) && !getRawButton(button)); + } + + @Override + public double getRawAxis(int axis) { + return m_axes[axis]; + } + + @Override + public int getPOV(int pov) { + return m_pov[pov]; + } + + @Override + public int getAxisCount() { + return m_axes.length; + } + + @Override + public int getPOVCount() { + return m_pov.length; + } + + @Override + public int getButtonCount() { + return 10; + } + + @Override + public boolean isConnected() { + return true; + } + + @Override + public HIDType getType() { + return HIDType.kXInputGamepad; + } + + @Override + public String getName() { + return "Virtual Controller"; + } + + @Override + public int getAxisType(int axis) { + return 1; /* ! Warning, does not return accurate data. + Hopefully this isn't a problem */ + } + + /** + * Use {@link VirtualController#setFrame} or {@link VirtualController#setOutputs}. + * this is an no-op overide. + */ + @Override + public void setOutput(int outputNumber, boolean value) { + // do not use + //m_buttonStatesLastFrame[outputNumber - 1] = m_buttonStates[outputNumber - 1]; + //m_buttonStates[outputNumber - 1] = value; + } + + /** + * Set buttons from a packed int, if you want to set joysticks and dpad use {@link VirtualController#SetFrame} + */ + @Override + public void setOutputs(int value) { + m_buttonStatesLastFrame = m_buttonStates; + m_buttonStates = (short) value; + } + + /** + * Why are you Setting rumble on an virtual controller? + * @param type the rumble type (even though it won't do anything) + * @param value the rumble strength (always multiplyed by 0.0) + */ + @Override + public void setRumble(RumbleType type, double value) { + System.out.println("Why are you Setting rumble on an virtual controller?"); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/controller/XboxController.java b/src/main/java/frc4388/utility/controller/XboxController.java new file mode 100644 index 0000000..8b8f0f8 --- /dev/null +++ b/src/main/java/frc4388/utility/controller/XboxController.java @@ -0,0 +1,218 @@ +package frc4388.utility.controller; + +import edu.wpi.first.wpilibj.Joystick; + +/** + * This is a wrapper for the WPILib Joystick class that represents an XBox + * controller. + * @author frc1675 + */ +public class XboxController implements IHandController +{ + public static final int LEFT_X_AXIS = 0; + public static final int LEFT_Y_AXIS = 1; + public static final int LEFT_TRIGGER_AXIS = 2; + public static final int RIGHT_TRIGGER_AXIS = 3; + public static final int RIGHT_X_AXIS = 4; + public static final int RIGHT_Y_AXIS = 5; + public static final int LEFT_RIGHT_DPAD_AXIS = 6; + public static final int TOP_BOTTOM_DPAD_AXIS = 6; + + public static final int A_BUTTON = 1; + public static final int B_BUTTON = 2; + public static final int X_BUTTON = 3; + public static final int Y_BUTTON = 4; + public static final int LEFT_BUMPER_BUTTON = 5; + public static final int RIGHT_BUMPER_BUTTON = 6; + public static final int BACK_BUTTON = 7; + public static final int START_BUTTON = 8; + + public static final int LEFT_JOYSTICK_BUTTON = 9; + public static final int RIGHT_JOYSTICK_BUTTON = 10; + + private static final double LEFT_DPAD_TOLERANCE = -0.9; + private static final double RIGHT_DPAD_TOLERANCE = 0.9; + private static final double BOTTOM_DPAD_TOLERANCE = -0.9; + private static final double TOP_DPAD_TOLERANCE = 0.9; + + private static final double LEFT_TRIGGER_TOLERANCE = 0.5; + private static final double RIGHT_TRIGGER_TOLERANCE = 0.5; + + private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9; + private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9; + private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9; + private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9; + + private static final double LEFT_AXIS_UP_TOLERANCE = -0.9; + private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9; + private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9; + private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9; + + private static final double DEADZONE = 0.1; + + private Joystick m_stick; + + /** + * Add your docs here. + */ + public XboxController(int portNumber){ + m_stick = new Joystick(portNumber); + } + + /** + * Add your docs here. + */ + public Joystick getJoyStick() { + return m_stick; + } + + /** + * Checks if the input falls within the deadzone. + * @param input from an axis on the controller + * @return true if input falls in deadzone, false if input falls outside deadzone + */ + private boolean inDeadZone(double input){ + return (Math.abs(input) < DEADZONE); + } + + /** + * Updates an input to have a deadzone around the 0 position + * @param input from an axis on the controller + * @return updated input + */ + private double getAxisWithDeadZoneCheck(double input){ + if(inDeadZone(input)){ + return 0.0; + } else { + return input; + } + } + + public boolean getAButton(){ + return m_stick.getRawButton(A_BUTTON); + } + + public boolean getXButton(){ + return m_stick.getRawButton(X_BUTTON); + } + + public boolean getBButton(){ + return m_stick.getRawButton(B_BUTTON); + } + + public boolean getYButton(){ + return m_stick.getRawButton(Y_BUTTON); + } + + public boolean getBackButton(){ + return m_stick.getRawButton(BACK_BUTTON); + } + + public boolean getStartButton(){ + return m_stick.getRawButton(START_BUTTON); + } + + public boolean getLeftBumperButton(){ + return m_stick.getRawButton(LEFT_BUMPER_BUTTON); + } + + public boolean getRightBumperButton(){ + return m_stick.getRawButton(RIGHT_BUMPER_BUTTON); + } + + public boolean getLeftJoystickButton(){ + return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON); + } + + public boolean getRightJoystickButton(){ + return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON); + } + + public double getLeftXAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS)); + } + + public double getLeftYAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS)); + } + + public double getRightXAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS)); + } + + public double getRightYAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS)); + } + + public double getLeftTriggerAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS)); + } + + public double getRightTriggerAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS)); + } + + /** + * Get the angle input from the dpad. + * @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc. + */ + public int getDpadAngle() { + return m_stick.getPOV(0); + } + + public boolean getDPadLeft(){ + return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE); + } + + public boolean getDPadRight(){ + return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE); + } + + public boolean getDPadTop(){ + return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE); + } + + public boolean getDPadBottom(){ + return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE); + } + + public boolean getLeftTrigger(){ + return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE); + } + + public boolean getRightTrigger(){ + return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE); + } + + public boolean getRightAxisUpTrigger(){ + return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE); + } + + public boolean getRightAxisDownTrigger(){ + return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE); + } + + public boolean getRightAxisLeftTrigger(){ + return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE); + } + + public boolean getRightAxisRightTrigger(){ + return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE); + } + + public boolean getLeftAxisUpTrigger(){ + return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE); + } + + public boolean getLeftAxisDownTrigger(){ + return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE); + } + + public boolean getLeftAxisLeftTrigger(){ + return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE); + } + + public boolean getLeftAxisRightTrigger(){ + return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java new file mode 100644 index 0000000..0a56841 --- /dev/null +++ b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java @@ -0,0 +1,68 @@ +package frc4388.utility.controller; + +//import edu.wpi.first.wpilibj2.command.button.Trigger; + +/** + * Mapping for the Xbox controller triggers to allow triggers to be defined as + * buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger + * exceeds a tolerance defined in {@link XboxController}. + */ +public class XboxTriggerButton {//extends Trigger { + public static final int RIGHT_TRIGGER = 0; + public static final int LEFT_TRIGGER = 1; + public static final int RIGHT_AXIS_UP_TRIGGER = 2; + public static final int RIGHT_AXIS_DOWN_TRIGGER = 3; + public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4; + public static final int RIGHT_AXIS_LEFT_TRIGGER = 5; + public static final int LEFT_AXIS_UP_TRIGGER = 6; + public static final int LEFT_AXIS_DOWN_TRIGGER = 7; + public static final int LEFT_AXIS_RIGHT_TRIGGER = 8; + public static final int LEFT_AXIS_LEFT_TRIGGER = 9; + + private XboxController m_controller; + private int m_trigger; + + /** + * Creates a Trigger-Button mapped to a specific Xbox controller and trigger + */ + public XboxTriggerButton(XboxController controller, int trigger) { + m_controller = controller; + m_trigger = trigger; + } + + /** {@inheritDoc} */ + // @Override + public boolean get() { + if (m_trigger == RIGHT_TRIGGER) { + return m_controller.getRightTrigger(); + } + else if (m_trigger == LEFT_TRIGGER) { + return m_controller.getLeftTrigger(); + } + else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) { + return m_controller.getRightAxisUpTrigger(); + } + else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) { + return m_controller.getRightAxisDownTrigger(); + } + else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) { + return m_controller.getRightAxisRightTrigger(); + } + else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) { + return m_controller.getRightAxisLeftTrigger(); + } + else if (m_trigger == LEFT_AXIS_UP_TRIGGER) { + return m_controller.getLeftAxisUpTrigger(); + } + else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) { + return m_controller.getLeftAxisDownTrigger(); + } + else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) { + return m_controller.getLeftAxisRightTrigger(); + } + else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) { + return m_controller.getLeftAxisLeftTrigger(); + } + return false; + } +} diff --git a/src/main/java/frc4388/utility/status/Alerts.java b/src/main/java/frc4388/utility/status/Alerts.java new file mode 100644 index 0000000..4065586 --- /dev/null +++ b/src/main/java/frc4388/utility/status/Alerts.java @@ -0,0 +1,10 @@ +package frc4388.utility.status; + +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import frc4388.robot.RobotContainer; + +// Class to update a series of WPILIB Alerts +public class Alerts { + public static Alert no_auto = new Alert("No auto has been selected!", AlertType.kError); +} diff --git a/src/main/java/frc4388/utility/status/CanDevice.java b/src/main/java/frc4388/utility/status/CanDevice.java new file mode 100644 index 0000000..ef72647 --- /dev/null +++ b/src/main/java/frc4388/utility/status/CanDevice.java @@ -0,0 +1,53 @@ +package frc4388.utility.status; + +import java.util.ArrayList; +import java.util.List; + +import frc4388.utility.status.Status.ReportLevel; + +public class CanDevice { + public static List devices = new ArrayList<>(); + + public String name; + public int id; + + public CanDevice(String name, int id) { + this.name = name; + this.id = id; + + devices.add(this); + } + + + private boolean isAlive() { + return true; //TODO: Link this with Device Finder + } + + public String getName() { + return "CAN ID " + this.id + " ( " + this.name + " ) "; + } + + public void Log(String str){ + System.out.println(getName() + " - " + str); + } + + // public Status queryStatus() { + // Status s = new Status(); + + // s.addReport(ReportLevel.INFO, "TODO"); + + // return s; + // } + + public Status diagnosticStatus() { + Status s = new Status(); + //TODO + s.addReport(ReportLevel.INFO, "Add CAN magic here"); + boolean isAlive = isAlive(); + s.addReport(isAlive ? ReportLevel.INFO : ReportLevel.ERROR, "Is Alive: " + isAlive); + + return s; + } + + +} diff --git a/src/main/java/frc4388/utility/status/FaultCANCoder.java b/src/main/java/frc4388/utility/status/FaultCANCoder.java new file mode 100644 index 0000000..a1631fb --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultCANCoder.java @@ -0,0 +1,76 @@ +package frc4388.utility.status; + +import com.ctre.phoenix6.controls.EmptyControl; +import com.ctre.phoenix6.hardware.CANcoder; + +import frc4388.utility.status.Status.ReportLevel; + +public class FaultCANCoder implements Queryable { + private String name; + private CANcoder cancoder; + + public static void addDevice(CANcoder cancoder, String name) { + FaultCANCoder p = new FaultCANCoder(); + + p.name = name; + p.cancoder = cancoder; + + FaultReporter.register(p); + } + + @Override + public String getName() { + return name; + } + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + + boolean debounceBad = !QueryUtils.isDebounceOk(cancoder.getSupplyVoltage()); + boolean emptyControlBad = cancoder.setControl(new EmptyControl()).value != 0; + + if(debounceBad || emptyControlBad) { + s.addReport(ReportLevel.ERROR, "device is unreachable - Failed" + + (debounceBad ? " Failed debounce test" : "") + + (emptyControlBad ? " Failed empty control test" : "") + ); + } + + // faults + if (cancoder.getFault_Hardware().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Hardware fault detected"); + } + if (cancoder.getFault_BootDuringEnable().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Device booted while enabled"); + } + if (cancoder.getFault_BadMagnet().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Bad magnet"); + } + if (cancoder.getFault_Undervoltage().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Device supply voltage near brownout"); + } + if (cancoder.getFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Unlicensed feature in use"); + } + + // sticky faults + if (cancoder.getStickyFault_Hardware().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Hardware fault detected"); + } + if (cancoder.getStickyFault_BootDuringEnable().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Device booted while enabled"); + } + if (cancoder.getStickyFault_BadMagnet().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Bad magnet"); + } + if (cancoder.getStickyFault_Undervoltage().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Device supply voltage near brownout"); + } + if (cancoder.getStickyFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Unlicensed feature in use"); + } + + return s; + } +} diff --git a/src/main/java/frc4388/utility/status/FaultPhotonCamera.java b/src/main/java/frc4388/utility/status/FaultPhotonCamera.java new file mode 100644 index 0000000..5ea8b70 --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultPhotonCamera.java @@ -0,0 +1,35 @@ +package frc4388.utility.status; + +import org.photonvision.PhotonCamera; + +import frc4388.utility.status.Status.ReportLevel; + +public class FaultPhotonCamera implements Queryable { + private String name; + private PhotonCamera cam; + + public static void addDevice(PhotonCamera cam, String name) { + FaultPhotonCamera p = new FaultPhotonCamera(); + + p.name = name; + p.cam = cam; + + FaultReporter.register(p); + } + + @Override + public String getName() { + return name; + } + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + + if(!cam.isConnected()) + s.addReport(ReportLevel.ERROR, "Not Connected!"); + + return s; + } +} + diff --git a/src/main/java/frc4388/utility/status/FaultPidgeon2.java b/src/main/java/frc4388/utility/status/FaultPidgeon2.java new file mode 100644 index 0000000..1e0bb4f --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultPidgeon2.java @@ -0,0 +1,168 @@ +package frc4388.utility.status; + +import com.ctre.phoenix6.controls.EmptyControl; +import com.ctre.phoenix6.hardware.Pigeon2; + +import frc4388.utility.status.Status.ReportLevel; + +public class FaultPidgeon2 implements Queryable { + private String name; + private Pigeon2 pigeon2; + + public static void addDevice(Pigeon2 pigeon2, String name) { + FaultPidgeon2 p = new FaultPidgeon2(); + + p.name = name; + p.pigeon2 = pigeon2; + + FaultReporter.register(p); + } + + @Override + public String getName() { + return name; + } + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + + + + boolean debounceBad = !QueryUtils.isDebounceOk(pigeon2.getSupplyVoltage()); + boolean emptyControlBad = pigeon2.setControl(new EmptyControl()).value != 0; + + if(debounceBad || emptyControlBad) { + s.addReport(ReportLevel.ERROR, "device is unreachable - Failed" + + (debounceBad ? " Failed debounce test" : "") + + (emptyControlBad ? " Failed empty control test" : "") + ); + } + + + s.addReport(ReportLevel.INFO, "Voltage: " + pigeon2.getSupplyVoltage()); + + s.addReport(ReportLevel.INFO, "Pitch: " + pigeon2.getPitch()); + s.addReport(ReportLevel.INFO, "Yaw: " + pigeon2.getYaw()); + s.addReport(ReportLevel.INFO, "Roll: " + pigeon2.getRoll()); + + s.addReport(ReportLevel.INFO, "Acceleration X: " + pigeon2.getAccelerationX()); + s.addReport(ReportLevel.INFO, "Acceleration Y: " + pigeon2.getAccelerationY()); + s.addReport(ReportLevel.INFO, "Acceleration Z: " + pigeon2.getAccelerationZ()); + + s.addReport(ReportLevel.INFO, "Magnomiter X: " + pigeon2.getMagneticFieldX()); + s.addReport(ReportLevel.INFO, "Magnomiter Y: " + pigeon2.getMagneticFieldY()); + s.addReport(ReportLevel.INFO, "Magnomiter Z: " + pigeon2.getMagneticFieldZ()); + + + // faults + if (pigeon2.getFault_BootDuringEnable().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Device booted while enabled"); + } + if (pigeon2.getFault_BootIntoMotion().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Device booted while in motion"); + } + if (pigeon2.getFault_BootupAccelerometer().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Accelerometer fault detected"); + } + if (pigeon2.getFault_BootupGyroscope().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Gyro fault detected"); + } + if (pigeon2.getFault_BootupMagnetometer().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Magnetometer fault detected"); + } + if (pigeon2.getFault_DataAcquiredLate().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "Motion stack data acquisition slower than expected"); + } + if (pigeon2.getFault_Hardware().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Hardware fault detected"); + } + if (pigeon2.getFault_LoopTimeSlow().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "Motion stack loop time was slower than expected"); + } + if (pigeon2.getFault_SaturatedAccelerometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "Accelerometer values are saturated"); + } + if (pigeon2.getFault_SaturatedGyroscope().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Gyro values are saturated"); + } + if (pigeon2.getFault_SaturatedMagnetometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "Magnetometer values are saturated"); + } + if (pigeon2.getFault_Undervoltage().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "Device supply voltage near brownout"); + } + if (pigeon2.getFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Unlicensed feature in use"); + } + + // sticky faults + if (pigeon2.getStickyFault_BootDuringEnable().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Device booted while enabled"); + } + if (pigeon2.getStickyFault_BootIntoMotion().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Device booted while in motion"); + } + if (pigeon2.getStickyFault_BootupAccelerometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Accelerometer fault detected"); + } + if (pigeon2.getStickyFault_BootupGyroscope().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Gyro fault detected"); + } + if (pigeon2.getStickyFault_BootupMagnetometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Magnetometer fault detected"); + } + if (pigeon2.getStickyFault_DataAcquiredLate().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + String.format( + "[STICKY] Motion stack data acquisition slower than expected")); + } + if (pigeon2.getStickyFault_Hardware().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Hardware fault detected"); + } + if (pigeon2.getStickyFault_LoopTimeSlow().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + String.format( + "[STICKY] Motion stack loop time was slower than expected")); + } + if (pigeon2.getStickyFault_SaturatedAccelerometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "[STICKY] Accelerometer values are saturated"); + } + if (pigeon2.getStickyFault_SaturatedGyroscope().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Gyro values are saturated"); + } + if (pigeon2.getStickyFault_SaturatedMagnetometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "[STICKY] Magnetometer values are saturated"); + } + if (pigeon2.getStickyFault_Undervoltage().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "[STICKY] Device supply voltage near brownout"); + } + if (pigeon2.getStickyFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Unlicensed feature in use"); + } + + return s; + } +} diff --git a/src/main/java/frc4388/utility/status/FaultReporter.java b/src/main/java/frc4388/utility/status/FaultReporter.java new file mode 100644 index 0000000..afd4dd4 --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultReporter.java @@ -0,0 +1,133 @@ +package frc4388.utility.status; + +import java.util.ArrayList; +import java.util.List; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.CANBus.CANBusStatus; + +import frc4388.robot.constants.Constants; +import frc4388.utility.status.Status.Report; +import frc4388.utility.status.Status.ReportLevel; + +public class FaultReporter { + + private static final String REPORTS_HEADER = + "###############\n" + // + "#.............#\n" + // + "#...Reports...#\n" + // + "#.............#\n" + // + "###############\n"; + + + private static final String CAN_HEADER = + "###############\n" + // + "#.............#\n" + // + "#....CAN(t)...#\n" + // + "#.............#\n" + // + "###############\n"; + + private static final String ERROR_HEADER = + "###############\n" + // + "#.............#\n" + // + "#....ERRORS...#\n" + // + "#.............#\n" + // + "###############\n"; + + private static List queryables = new ArrayList<>(); + + // public static void startThread() { + // new Thread() { + // public void run() { + // try{ + // while(!this.isInterrupted() && this.isAlive()){ + // Thread.sleep(500); + // for(int i=0;i errors = new ArrayList<>(); + + // Subsystems header + System.out.println(REPORTS_HEADER); + + for(int i=0;i< queryables.size();i++){ + + Queryable q = queryables.get(i); + System.out.println("** Subsystem diagnostic report for " + q.getName() + ":"); + Status status = q.diagnosticStatus(); + + for(int a=0;a 0) { + // Errors header + System.out.println(ERROR_HEADER); + for(int i=0;i reports; + + public Status() { + this.reports = new ArrayList<>(); + } + + public void addReport(ReportLevel level, String description) { + Report r = new Report(); + r.reportLevel = level; + r.description = description; + this.reports.add(r); + } + + private String printStatusCode(StatusCode status){ + return status.getName() + " (" + status.value + ")"; + } + + public void diagnoseHardwareCTRE(String deviceName, TalonFX motor) { + addReport(ReportLevel.ERROR, deviceName + " Motor (TalonFX) Alive?: " + (motor.isAlive() ? "Alive." : "Dead!")); + + + + if (motor.isAlive()) addReport(ReportLevel.INFO, deviceName + " Motor (TalonFX) Alive?: Alive."); + else addReport(ReportLevel.ERROR, deviceName + " Motor (TalonFX) Alive?: Dead!"); + } + + public void diagnoseHardwareCTRE(String deviceName, CANcoder coder) { + // Because the Cancoder has no method to check its alive, we send it a empty control which it should return a zero when it gets the control. + // If its not zero, that means that most likely that it had some communication error, I.e. It actually is powered off or not connected at all. + // TODO: validate that a CANCoder can actually do `EmptyControl`s + StatusCode status = coder.setControl(new EmptyControl()); + if (status.value == 0) addReport(ReportLevel.INFO, deviceName + " Cancoder Alive?: Alive. " + printStatusCode(status)); + else addReport(ReportLevel.ERROR, deviceName + " Cancoder Alive?: Dead! " + printStatusCode(status)); + + + + // StatusSignal -> MagnetHealthValue -> int + int coderMagHealth = coder.getMagnetHealth().getValue().value; + if (coderMagHealth == 3) addReport(ReportLevel.INFO, deviceName + " Cancoder Magnet Strength?: Ideal."); // why is 3 the 'good value'? + if (coderMagHealth == 2) addReport(ReportLevel.WARNING, deviceName + " Cancoder Magnet Strength?: Subpar."); + if (coderMagHealth == 1) addReport(ReportLevel.ERROR, deviceName + " Cancoder Magnet Strength?: Too Close or Far!"); + if (coderMagHealth == 0) addReport(ReportLevel.ERROR, deviceName + " Cancoder Magnet Strength?: Unkown!"); + } + + public void diagnoseHardwareCTRE(String deviceName, Pigeon2 pigeon) { + // Because the Pigeon has no method to check its alive, we send it a empty control which it should return a zero when it gets the control. + // If its not zero, that means that most likely that it had some communication error, I.e. It actually is powered off or not connected at all. + // TODO: validate that a Pigeon2 can actually do `EmptyControl`s + StatusCode status = pigeon.setControl(new EmptyControl()); + if (status.value == 0) addReport(ReportLevel.INFO, deviceName + " Pigeon2 Alive?: Alive. " + printStatusCode(status)); + else addReport(ReportLevel.ERROR, deviceName + " Pigeon2 Alive?: Dead! " + printStatusCode(status)); + } + + public boolean hasReport() { + return reports.size() == 0; + } +} diff --git a/src/main/java/frc4388/utility/structs/Gains.java b/src/main/java/frc4388/utility/structs/Gains.java new file mode 100644 index 0000000..0d5b674 --- /dev/null +++ b/src/main/java/frc4388/utility/structs/Gains.java @@ -0,0 +1,83 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.utility.structs; + +/** Add your docs here. */ +public class Gains { + public double kP; + public double kI; + public double kD; + public double kF; + public int kIZone; + public double kPeakOutput; + public double kMaxOutput; + public double kMinOutput; + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIZone The zone of the I value. + * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0. + */ + public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIZone = kIZone; + this.kPeakOutput = kPeakOutput; + this.kMaxOutput = kPeakOutput; + this.kMinOutput = -kPeakOutput; + } + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIZone The zone of the I value. + */ + public Gains(double kP, double kI, double kD, double kF, int kIZone) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIZone = kIZone; + this.kPeakOutput = 1.0; + this.kMaxOutput = 1.0; + this.kMinOutput = -1.0; + } + + public Gains(double kP, double kI, double kD) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + } + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIZone The zone of the I value. + * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0. + * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0. + */ + public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIZone = kIZone; + this.kMaxOutput = kMaxOutput; + this.kMinOutput = kMinOutput; + this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/structs/LEDPatterns.java b/src/main/java/frc4388/utility/structs/LEDPatterns.java new file mode 100644 index 0000000..585dc43 --- /dev/null +++ b/src/main/java/frc4388/utility/structs/LEDPatterns.java @@ -0,0 +1,45 @@ +package frc4388.utility.structs; + +/** + * Add your docs here. + */ +public enum LEDPatterns { + /* PALLETTE PATTERNS */ + RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f), + RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f), + PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f), + PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f), + RAINBOW_TWINKLES(-0.55f), PARTY_TWINKLES(-0.53f), OCEAN_TWINKLES(-0.51f), LAVA_TWINKLES(-0.49f), FOREST_TWINKLES(-0.47f), + RAINBOW_WAVES(-0.45f), PARTY_WAVES(-0.43f), OCEAN_WAVES(-0.41f), LAVA_WAVES(-0.39f), FOREST_WAVES(-0.37f), + RED_SCANNER(-0.35f), GRAY_SCANNER(-0.33f), RED_CHASE(-0.31f), BLUE_CHASE(-0.29f), GRAY_CHASE(-0.27f), RED_HEARTBEAT(-0.25f), + BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f), + GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f), + + /* COLOR 1 PATTERNS */ + C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f), + C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f), + + /* COLOR 2 PATTERNS */ + C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f), + C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f), + + /* COLOR 1 AND 2 PATTERNS */ + C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f), + C1C2_WAVES(0.53f), C1C2_SINELON(0.55f), + + /* SOLID COLORS */ + SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f), + SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f), + SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f), + SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f), + SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f); + + /* GETTERS/SETTERS */ + private final float id; + LEDPatterns(float id) { + this.id = id; + } + public float getValue() { + return id; + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/structs/UtilityStructs.java b/src/main/java/frc4388/utility/structs/UtilityStructs.java new file mode 100644 index 0000000..2307f2f --- /dev/null +++ b/src/main/java/frc4388/utility/structs/UtilityStructs.java @@ -0,0 +1,26 @@ +package frc4388.utility.structs; + +public class UtilityStructs { + public static class TimedOutput { + public double leftX = 0.0; + public double leftY = 0.0; + public double rightX = 0.0; + public double rightY = 0.0; + + public boolean OPLB; + public boolean OPRB; + + + public long timedOffset = 0; + } + public static class AutoRecordingControllerFrame { + public double[] axes = new double[6]; + public short button = 0; + public short[] POV = new short[1]; + + } + public static class AutoRecordingFrame { + public AutoRecordingControllerFrame[] controllerFrames = new AutoRecordingControllerFrame[2]; + public int timeStamp; + } +} diff --git a/src/test/java/frc4388/mocks/MockPigeonIMU.java.old b/src/test/java/frc4388/mocks/MockPigeonIMU.java.old new file mode 100644 index 0000000..ecddde7 --- /dev/null +++ b/src/test/java/frc4388/mocks/MockPigeonIMU.java.old @@ -0,0 +1,54 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.mocks; + +import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix.sensors.PigeonIMU; + +/** + * Add your docs here. + */ +public class MockPigeonIMU extends PigeonIMU { + public int m_deviceNumber; + public double currentYaw; + public double currentPitch; + public double currentRoll; + + public MockPigeonIMU(int deviceNumber) { + super(deviceNumber); + m_deviceNumber = deviceNumber; + } + + @Override + public ErrorCode setYaw(double angleDeg) { + currentYaw = angleDeg; + return ErrorCode.OK; + } + + /** + * @param currentPitch the Pitch to set + */ + public void setCurrentPitch(double currentPitch) { + this.currentPitch = currentPitch; + } + + /** + * @param currentRoll the Roll to set + */ + public void setCurrentRoll(double currentRoll) { + this.currentRoll = currentRoll; + } + + @Override + public ErrorCode getYawPitchRoll(double[] ypr_deg) { + ypr_deg[0] = currentYaw; + ypr_deg[1] = currentPitch; + ypr_deg[2] = currentRoll; + return ErrorCode.OK; + } +} diff --git a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.old b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.old new file mode 100644 index 0000000..8fcbf53 --- /dev/null +++ b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.old @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.subsystems; + +import static org.junit.Assert.assertEquals; +import static org.mockito.Mockito.mock; + +import org.junit.Test; + +import edu.wpi.first.wpilibj.*; +import frc4388.robot.Constants.LEDConstants; +import frc4388.utility.LEDPatterns; + +/** + * Add your docs here. + */ +public class LEDSubsystemTest { + @Test + public void testConstructor() { + // Arrange + Spark ledController = mock(Spark.class); + + // Act + LED led = new LED(ledController); + + // Assert + assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001); + } + + @Test + public void testPatterns() { + // Arrange + Spark ledController = mock(Spark.class); + LED led = new LED(ledController); + + // Act + led.setPattern(LEDPatterns.RAINBOW_RAINBOW); + + // Assert + assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001); + + // Act + led.setPattern(LEDPatterns.BLUE_BREATH); + + // Assert + assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001); + + // Act + led.setPattern(LEDPatterns.SOLID_BLACK); + + // Assert + assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001); + } +} diff --git a/src/test/java/frc4388/utility/RobotGyroUtilityTest.old b/src/test/java/frc4388/utility/RobotGyroUtilityTest.old new file mode 100644 index 0000000..8c0b1e5 --- /dev/null +++ b/src/test/java/frc4388/utility/RobotGyroUtilityTest.old @@ -0,0 +1,184 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.utility; + +import static org.junit.Assert.*; +import static org.mockito.Mockito.*; + +import com.kauailabs.navx.frc.AHRS; + +import org.junit.*; + +import frc4388.mocks.MockPigeonIMU; +import frc4388.robot.Constants.DriveConstants; + +/** + * Add your docs here. + */ +public class RobotGyroUtilityTest { + // TODO UNTESTED: most functions for NavX + + private RobotGyro gyroPigeon; + private RobotGyro gyroNavX; + + @Test + public void testConstructor() { + // Arrange + MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID); + AHRS navX = mock(AHRS.class); + gyroPigeon = new RobotGyro(pigeon); + gyroNavX = new RobotGyro(navX); + + // Assert + assertEquals(true, gyroPigeon.m_isGyroAPigeon); + assertEquals(pigeon, gyroPigeon.getPigeon()); + assertEquals(null, gyroPigeon.getNavX()); + assertEquals(false, gyroNavX.m_isGyroAPigeon); + assertEquals(navX, gyroNavX.getNavX()); + assertEquals(null, gyroNavX.getPigeon()); + } + + @Test + public void testHeadingPigeon() { + // Arrange + MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID); + gyroPigeon = new RobotGyro(pigeon); + + // Act & Assert + assertEquals(-90, gyroPigeon.getHeading(270), 0.0001); + assertEquals(-45, gyroPigeon.getHeading(315), 0.0001); + assertEquals(-60, gyroPigeon.getHeading(-60), 0.0001); + assertEquals(30, gyroPigeon.getHeading(30), 0.0001); + assertEquals(0, gyroPigeon.getHeading(0), 0.0001); + assertEquals(180, gyroPigeon.getHeading(180), 0.0001); + assertEquals(-180, gyroPigeon.getHeading(-180), 0.0001); + assertEquals(97, gyroPigeon.getHeading(1537), 0.0001); + assertEquals(99, gyroPigeon.getHeading(-2781), 0.0001); + } + + @Test + public void testYawPitchRollPigeon() { + // Arrange + MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID); + gyroPigeon = new RobotGyro(pigeon); + + // Assert + assertEquals(0, gyroPigeon.getAngle(), 0.0001); + + // Act + pigeon.setYaw(40); + + // Assert + assertEquals(40, gyroPigeon.getAngle(), 0.0001); + + // Act + gyroPigeon.reset(); + + // Assert + assertEquals(0, gyroPigeon.getAngle(), 0.0001); + + // Act + pigeon.setYaw(-1457); + pigeon.setCurrentPitch(100); + pigeon.setCurrentRoll(100); + + // Assert + assertEquals(-1457, gyroPigeon.getAngle(), 0.0001); + assertEquals(90, gyroPigeon.getPitch(), 0.0001); + assertEquals(90, gyroPigeon.getRoll(), 0.0001); + + // Act + pigeon.setCurrentPitch(45); + pigeon.setCurrentRoll(45); + + // Assert + assertEquals(45, gyroPigeon.getPitch(), 0.0001); + assertEquals(45, gyroPigeon.getRoll(), 0.0001); + + // Act + pigeon.setCurrentPitch(0); + pigeon.setCurrentRoll(0); + + // Assert + assertEquals(0, gyroPigeon.getPitch(), 0.0001); + assertEquals(0, gyroPigeon.getRoll(), 0.0001); + + // Act + pigeon.setCurrentPitch(-60); + pigeon.setCurrentRoll(-60); + + // Assert + assertEquals(-60, gyroPigeon.getPitch(), 0.0001); + assertEquals(-60, gyroPigeon.getRoll(), 0.0001); + + // Act + pigeon.setCurrentPitch(-90); + pigeon.setCurrentRoll(-90); + + // Assert + assertEquals(-90, gyroPigeon.getPitch(), 0.0001); + assertEquals(-90, gyroPigeon.getRoll(), 0.0001); + + // Act + pigeon.setCurrentPitch(-100); + pigeon.setCurrentRoll(-100); + + // Assert + assertEquals(-90, gyroPigeon.getPitch(), 0.0001); + assertEquals(-90, gyroPigeon.getRoll(), 0.0001); + } + + @Test + public void testRatesPigeon() { + // Arrange + MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID); + gyroPigeon = new RobotGyro(pigeon); + RobotTime robotTime = RobotTime.getInstance(); + gyroPigeon.updatePigeonDeltas(); + + // Act + robotTime.m_deltaTime = 5; + pigeon.setYaw(0); + gyroPigeon.updatePigeonDeltas(); + + // Assert + assertEquals(0, gyroPigeon.getRate(), 1); + + // Act + robotTime.m_deltaTime = 5; + pigeon.setYaw(90); + gyroPigeon.updatePigeonDeltas(); + + // Assert + assertEquals(18000, gyroPigeon.getRate(), 0.001); + + // Act + robotTime.m_deltaTime = 5; + pigeon.setYaw(90); + gyroPigeon.updatePigeonDeltas(); + + // Assert + assertEquals(0, gyroPigeon.getRate(), 0.001); + + // Act + robotTime.m_deltaTime = 3; + pigeon.setYaw(-30); + gyroPigeon.updatePigeonDeltas(); + + // Assert + assertEquals(-40000, gyroPigeon.getRate(), 0.001); + + // Act + robotTime.m_deltaTime = 6; + pigeon.setYaw(690); + gyroPigeon.updatePigeonDeltas(); + + // Assert + assertEquals(120000, gyroPigeon.getRate(), 0.001); + } +} diff --git a/src/test/java/frc4388/utility/RobotTimeUtilityTest.old b/src/test/java/frc4388/utility/RobotTimeUtilityTest.old new file mode 100644 index 0000000..2c31a34 --- /dev/null +++ b/src/test/java/frc4388/utility/RobotTimeUtilityTest.old @@ -0,0 +1,104 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.utility; + +import static org.junit.Assert.*; + +import org.junit.*; + +/** + * Add your docs here. + */ +public class RobotTimeUtilityTest { + RobotTime robotTime = RobotTime.getInstance(); + + @Test + public void testUpdateTimes() { + // Arrange + long lastTime; + robotTime.m_deltaTime = 0; + robotTime.m_robotTime = 0; + robotTime.m_lastRobotTime = 0; + robotTime.m_frameNumber = 0; + robotTime.endMatchTime(); + robotTime.m_lastMatchTime = 0; + + // Assert + assertEquals(0, robotTime.m_deltaTime); + assertEquals(0, robotTime.m_robotTime); + assertEquals(0, robotTime.m_lastRobotTime); + assertEquals(0, robotTime.m_frameNumber); + lastTime = robotTime.m_robotTime; + + // Act + wait(1); + robotTime.updateTimes(); + + // Assert + assertEquals(true, robotTime.m_deltaTime > 0); + assertEquals(true, robotTime.m_robotTime > 0); + assertEquals(lastTime, robotTime.m_lastRobotTime); + assertEquals(1, robotTime.m_frameNumber); + lastTime = robotTime.m_robotTime; + + // Act + wait(1); + robotTime.updateTimes(); + + // Assert + assertEquals(true, robotTime.m_deltaTime > 0); + assertEquals(true, robotTime.m_robotTime > 0); + assertEquals(lastTime, robotTime.m_lastRobotTime); + assertEquals(2, robotTime.m_frameNumber); + } + + @Test + public void testMatchTime() { + // Arrange + long lastTime; + + // Assert + assertEquals(0, robotTime.m_matchTime); + assertEquals(0, robotTime.m_lastMatchTime); + lastTime = robotTime.m_matchTime; + + // Act + robotTime.startMatchTime(); + wait(1); + robotTime.updateTimes(); + + // Assert + assertEquals(true, robotTime.m_matchTime > 0); + assertEquals(lastTime, robotTime.m_lastMatchTime); + lastTime = robotTime.m_matchTime; + + // Act + wait(1); + robotTime.updateTimes(); + robotTime.endMatchTime(); + + // Assert + assertEquals(0, robotTime.m_matchTime); + assertEquals(lastTime, robotTime.m_lastMatchTime); + lastTime = robotTime.m_matchTime; + + // Act + wait(1); + robotTime.updateTimes(); + + // Assert + assertEquals(0, robotTime.m_matchTime); + assertEquals(lastTime, robotTime.m_lastMatchTime); + } + + private void wait(int millis) { + try { + Thread.sleep(millis); + } catch (Exception e) {} + } +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..bef4a15 --- /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 new file mode 100644 index 0000000..ecf8fa0 --- /dev/null +++ b/vendordeps/NavX.json @@ -0,0 +1,40 @@ +{ + "fileName": "NavX.json", + "name": "NavX", + "version": "2024.1.0", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2025", + "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" + ] + } + ] +} diff --git a/vendordeps/PathplannerLib-2025.2.7.json b/vendordeps/PathplannerLib-2025.2.7.json new file mode 100644 index 0000000..d3f84e5 --- /dev/null +++ b/vendordeps/PathplannerLib-2025.2.7.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib-2025.2.7.json", + "name": "PathplannerLib", + "version": "2025.2.7", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2025", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2025.2.7" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2025.2.7", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-frc2025-latest.json new file mode 100644 index 0000000..6f40c84 --- /dev/null +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -0,0 +1,479 @@ +{ + "fileName": "Phoenix6-frc2025-latest.json", + "name": "CTRE-Phoenix (v6)", + "version": "25.4.0", + "frcYear": "2025", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "25.4.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "25.4.0", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.4.0", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "25.4.0", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.4.0", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.4.0", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.4.0", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.4.0", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.4.0", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.4.0", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.4.0", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.4.0", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.4.0", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.4.0", + "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.4.0", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json new file mode 100644 index 0000000..3718e0a --- /dev/null +++ b/vendordeps/WPILibNewCommands.json @@ -0,0 +1,38 @@ +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "1.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "frcYear": "2025", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } + ] +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..2d7b1d8 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.3.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.3.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.3.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.3.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.3.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.3.1" + } + ] +} \ No newline at end of file