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 index 375359c..34cbaac 100644 --- a/.gitignore +++ b/.gitignore @@ -182,3 +182,6 @@ ctre_sim/ # clangd /.cache compile_commands.json + +# Eclipse generated file for annotation processors +.factorypath diff --git a/.vscode/launch.json b/.vscode/launch.json index c9c9713..0a05d87 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,18 +4,24 @@ // 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, + "desktop": true }, { "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", - "desktop": false, + "desktop": false } ] } diff --git a/.vscode/settings.json b/.vscode/settings.json index 4ed293b..2d59e50 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -25,5 +25,38 @@ } }, ], - "java.test.defaultConfig": "WPIlibUnitTests" + "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 index acfab82..60cf2c2 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2025beta", + "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/build.gradle b/build.gradle index 351eaf0..b30e48b 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,7 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2" + id "edu.wpi.first.GradleRIO" version "2025.3.2" + id "com.peterabeles.gversion" version "1.10" } java { @@ -34,7 +35,7 @@ deploy { 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 on roboRIO + // longer exist in deploy directory of this project } } } @@ -72,6 +73,9 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" } test { @@ -102,3 +106,19 @@ wpi.java.configureTestTasks(test) 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/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/pathplanner/autos/1 Coral Center Barge Left.auto b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Left.auto new file mode 100644 index 0000000..3dd88fc --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Left.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef 4 Left" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "named", + "data": { + "name": "lower-algae-removal" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 Coral", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1 Coral Center Barge Right.auto b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Right.auto new file mode 100644 index 0000000..e0d6f82 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Right.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef 4 Right" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "named", + "data": { + "name": "lower-algae-removal" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 Coral", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto new file mode 100644 index 0000000..ae18da3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 1 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 5 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef 6" + } + }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Coral", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto new file mode 100644 index 0000000..42d1639 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 2 to Reef 5" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 5 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef 6" + } + }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Coral", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto new file mode 100644 index 0000000..72db1a3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 3 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 5 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef 6" + } + }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Coral", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 4.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 4.auto new file mode 100644 index 0000000..3df4f64 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 4.auto @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 4 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 3 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Coral", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto new file mode 100644 index 0000000..0a519ac --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 5 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 3 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Coral", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto new file mode 100644 index 0000000..68d7700 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 6 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 3 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Coral", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Center Barge Left.auto b/src/main/deploy/pathplanner/autos/2 Coral Center Barge Left.auto new file mode 100644 index 0000000..8351dab --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 Coral Center Barge Left.auto @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef 4 Right" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 4 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef 6" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Coral", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Coral Center Barge Right.auto b/src/main/deploy/pathplanner/autos/2 Coral Center Barge Right.auto new file mode 100644 index 0000000..0628011 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 Coral Center Barge Right.auto @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef 4 Left" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 4 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Coral", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 2.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 2.auto new file mode 100644 index 0000000..218687a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 2.auto @@ -0,0 +1,109 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 2 to Reef 5" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 5 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef 6" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef 6" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 6 to Top Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto new file mode 100644 index 0000000..a09d168 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto @@ -0,0 +1,121 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 5 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 3 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Coral Center Barge Bottom.auto b/src/main/deploy/pathplanner/autos/3 Coral Center Barge Bottom.auto new file mode 100644 index 0000000..88b79f7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 Coral Center Barge Bottom.auto @@ -0,0 +1,109 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef 4 Left" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 4 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef 2" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef 2 to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "named", + "data": { + "name": "feed-driveback" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto index 268147b..9333c35 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -7,7 +7,49 @@ { "type": "path", "data": { - "pathName": "New Path" + "pathName": "Cage 1 to Reef" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 2 to Reef 5" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 3 to Reef" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 4 to Reef" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 5 to Reef" + } + }, + { + "type": "path", + "data": { + "pathName": "Cage 6 to Reef" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef 4 Left" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef 4 Right" } } ] diff --git a/src/main/deploy/pathplanner/autos/Taxi.auto b/src/main/deploy/pathplanner/autos/Taxi.auto new file mode 100644 index 0000000..a717089 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Taxi.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "taxi" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/test.auto b/src/main/deploy/pathplanner/autos/test.auto new file mode 100644 index 0000000..4a33be6 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/test.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "test" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index bab0da9..23e0db9 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1 +1 @@ -{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path new file mode 100644 index 0000000..943d856 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.3040625, + "y": 1.3559375 + }, + "prevControl": null, + "nextControl": { + "x": 2.892340424329296, + "y": 2.067688275398428 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5221875, + "y": 2.3675 + }, + "prevControl": { + "x": 2.589485733053583, + "y": 1.921600370835535 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 59.28109573597083 + }, + "reversed": false, + "folder": "Bottom Feed to Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 53.426969021480645 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cage 1 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 1 to Reef.path new file mode 100644 index 0000000..710c3b1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Cage 1 to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.9834375, + "y": 7.1084375 + }, + "prevControl": null, + "nextControl": { + "x": 6.292598684210526, + "y": 6.699835526315789 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.590213815789473, + "y": 5.622203947368421 + }, + "prevControl": { + "x": 5.846151315789473, + "y": 6.024391447368421 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -118.81079374297312 + }, + "reversed": false, + "folder": "Barge to Reef", + "idealStartingState": { + "velocity": 0, + "rotation": -115.9743939624313 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path b/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path new file mode 100644 index 0000000..77f25d6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path @@ -0,0 +1,65 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.0078125, + "y": 6.0725 + }, + "prevControl": null, + "nextControl": { + "x": 6.902067334527623, + "y": 5.78751784036735 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.590213815789473, + "y": 5.593338815789473 + }, + "prevControl": { + "x": 5.872115174798128, + "y": 6.170888760790882 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 5.15, + "y": 4.85 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -120.52970589993409 + }, + "reversed": false, + "folder": "Barge to Reef", + "idealStartingState": { + "velocity": 0, + "rotation": -123.4533094540727 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cage 3 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 3 to Reef.path new file mode 100644 index 0000000..5e34982 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Cage 3 to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.9590625, + "y": 4.9878124999999995 + }, + "prevControl": null, + "nextControl": { + "x": 6.709257689548163, + "y": 4.997689578268469 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.619078947368421, + "y": 5.593338815789473 + }, + "prevControl": { + "x": 6.094391447368421, + "y": 5.5202138157894725 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -121.26879518614867 + }, + "reversed": false, + "folder": "Barge to Reef", + "idealStartingState": { + "velocity": 0, + "rotation": -124.91940201245771 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path new file mode 100644 index 0000000..c5ef9d6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.97125, + "y": 3.1474999999999995 + }, + "prevControl": null, + "nextControl": { + "x": 6.166875, + "y": 2.8184375 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.55172697368421, + "y": 2.4470394736842103 + }, + "prevControl": { + "x": 6.3804769736842095, + "y": 2.7395394736842107 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 120.96375653207355 + }, + "reversed": false, + "folder": "Barge to Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 118.9091836511478 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path new file mode 100644 index 0000000..452dec9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.016603535353535, + "y": 1.9718434343434341 + }, + "prevControl": null, + "nextControl": { + "x": 6.670197826108406, + "y": 2.0396270358261894 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.5805921052631575, + "y": 2.4374177631578946 + }, + "prevControl": { + "x": 5.9030178398827875, + "y": 2.377301864673819 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.0, + "rotation": 119.99999999999999 + }, + "reversed": false, + "folder": "Barge to Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 123.5866498700801 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path new file mode 100644 index 0000000..27ace9e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.057260101010102, + "y": 0.8537878787878788 + }, + "prevControl": null, + "nextControl": { + "x": 6.315277777777778, + "y": 1.5144570707070704 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.561348684210526, + "y": 2.456661184210526 + }, + "prevControl": { + "x": 5.883902345826687, + "y": 2.038842371079213 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 120.98817835541992 + }, + "reversed": false, + "folder": "Barge to Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 132.5633517531898 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Left.path b/src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Left.path new file mode 100644 index 0000000..a30c744 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Left.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.1296875, + "y": 4.0493749999999995 + }, + "prevControl": null, + "nextControl": { + "x": 6.833510101010102, + "y": 4.06893308080808 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.3496875, + "y": 3.8421874999999996 + }, + "prevControl": { + "x": 6.599301094934041, + "y": 3.8282931405651617 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -179.96096735022735 + }, + "reversed": false, + "folder": "Barge to Reef", + "idealStartingState": { + "velocity": 0, + "rotation": -179.2890804030095 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Right.path b/src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Right.path new file mode 100644 index 0000000..b662f54 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Right.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.1053125, + "y": 4.061562499999999 + }, + "prevControl": null, + "nextControl": { + "x": 6.809135101010102, + "y": 4.081120580808079 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.3496875, + "y": 4.195625 + }, + "prevControl": { + "x": 6.599301094934041, + "y": 4.181730640565162 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -179.96096735022735 + }, + "reversed": false, + "folder": "Barge to Reef", + "idealStartingState": { + "velocity": 0, + "rotation": -179.2890804030095 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index efc54d4..bde395e 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -3,45 +3,29 @@ "waypoints": [ { "anchor": { - "x": 13.16336824764045, - "y": 1.4493814298363055 + "x": 4.0, + "y": 6.0 }, "prevControl": null, "nextControl": { - "x": 14.079377311296996, - "y": 1.379811121204163 + "x": 4.2495147804709745, + "y": 5.984431624152738 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 13.16336824764045, - "y": 2.6204816251440413 + "x": 4.0, + "y": 6.0 }, "prevControl": { - "x": 14.160542671367828, - "y": 2.550911316511899 - }, - "nextControl": { - "x": 12.223879630793721, - "y": 2.686027342598464 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 13.16336824764045, - "y": 1.4493814298363055 - }, - "prevControl": { - "x": 12.200978978229143, - "y": 1.518951738468449 + "x": 4.249520880858856, + "y": 5.984529705386749 }, "nextControl": null, "isLocked": false, - "linkedName": "mid" + "linkedName": null } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path new file mode 100644 index 0000000..dbd1a43 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5221875, + "y": 2.4040625 + }, + "prevControl": null, + "nextControl": { + "x": 2.9564891755960816, + "y": 2.1114702488693777 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3284375, + "y": 1.3803125 + }, + "prevControl": { + "x": 2.0502196323692035, + "y": 1.7292616486680126 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.5, + "maxAcceleration": 5.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 55.37584492005105 + }, + "reversed": false, + "folder": "Reef to Bottom Feed", + "idealStartingState": { + "velocity": 0, + "rotation": 58.10920819815426 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path new file mode 100644 index 0000000..46f8cb1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.426644736842105, + "y": 2.851151315789474 + }, + "prevControl": null, + "nextControl": { + "x": 4.23151774045974, + "y": 2.4081021765718877 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.7415296052631577, + "y": 1.5618421052631584 + }, + "prevControl": { + "x": 3.369389020277641, + "y": 1.8741228567589594 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 55.05578949900953 + }, + "reversed": false, + "folder": "Reef to Bottom Feed", + "idealStartingState": { + "velocity": 0, + "rotation": 121.42956561483854 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path new file mode 100644 index 0000000..7fa944f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.9750822368421055, + "y": 3.9865131578947364 + }, + "prevControl": null, + "nextControl": { + "x": 7.0046052631578934, + "y": 1.6388157894736843 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3040625, + "y": 1.368125 + }, + "prevControl": { + "x": 1.7992598684210526, + "y": 2.302713815789474 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 52.857102599919905 + }, + "reversed": false, + "folder": "Reef to Bottom Feed", + "idealStartingState": { + "velocity": 0, + "rotation": -179.46454101443553 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path b/src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path new file mode 100644 index 0000000..ef11e1e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.946217105263157, + "y": 4.178947368421053 + }, + "prevControl": null, + "nextControl": { + "x": 6.604342105263157, + "y": 6.518947368421053 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.452878289473684, + "y": 6.565131578947369 + }, + "prevControl": { + "x": 2.0499142743221688, + "y": 5.561413144603935 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -53.93780932479242 + }, + "reversed": false, + "folder": "Reef to Top Feed", + "idealStartingState": { + "velocity": 0, + "rotation": 178.9390883097358 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path b/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path new file mode 100644 index 0000000..519045a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.41702302631579, + "y": 5.256578947368421 + }, + "prevControl": null, + "nextControl": { + "x": 4.4044191919191915, + "y": 5.9460227272727275 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5076388888888888, + "y": 6.484722222222222 + }, + "prevControl": { + "x": 3.184818220656944, + "y": 6.004734862812045 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.5, + "maxAcceleration": 5.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -54.83470564502973 + }, + "reversed": false, + "folder": "Reef to Top Feed", + "idealStartingState": { + "velocity": 0, + "rotation": -121.0370223454415 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path b/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path new file mode 100644 index 0000000..77ed74f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5099747474747476, + "y": 5.742739898989899 + }, + "prevControl": null, + "nextControl": { + "x": 2.36577546257746, + "y": 6.2120596924932325 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.4143914473684207, + "y": 6.584375 + }, + "prevControl": { + "x": 2.7066049155713703, + "y": 6.056626627340928 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.5, + "maxAcceleration": 5.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -53.93780932479242 + }, + "reversed": false, + "folder": "Reef to Top Feed", + "idealStartingState": { + "velocity": 0, + "rotation": -58.96173664449721 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Feed to Reef 6.path b/src/main/deploy/pathplanner/paths/Top Feed to Reef 6.path new file mode 100644 index 0000000..74f69ec --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top Feed to Reef 6.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.31625, + "y": 6.730625 + }, + "prevControl": null, + "nextControl": { + "x": 2.5451175614850996, + "y": 6.277541643530879 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.540467171717172, + "y": 5.7630681818181815 + }, + "prevControl": { + "x": 2.590802656557517, + "y": 6.105344244914677 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.239047023115106 + }, + "reversed": false, + "folder": "Top Feed to Reef", + "idealStartingState": { + "velocity": 0, + "rotation": -52.93323259086456 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/test.path b/src/main/deploy/pathplanner/paths/test.path new file mode 100644 index 0000000..20bb18a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/test.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.1457991803278684, + "y": 5.193801229508197 + }, + "prevControl": null, + "nextControl": { + "x": 3.1457991803278684, + "y": 5.193801229508197 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.1457991803278684, + "y": 5.745235655737704 + }, + "prevControl": { + "x": 3.8039959016393414, + "y": 5.682274590163933 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 7def6c7..f932c1d 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -2,18 +2,28 @@ "robotWidth": 0.9, "robotLength": 0.9, "holonomicMode": false, - "pathFolders": [], - "autoFolders": [], + "pathFolders": [ + "Barge to Reef", + "Bottom Feed to Reef", + "Reef to Top Feed", + "Reef to Bottom Feed", + "Top Feed to Reef" + ], + "autoFolders": [ + "1 Coral", + "2 Coral", + "3 Coral" + ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 74.088, + "robotMass": 58.18, "robotMOI": 6.883, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.048, - "driveGearing": 5.143, + "driveWheelRadius": 0.0508, + "driveGearing": 6.12, "maxDriveSpeed": 5.45, "driveMotorType": "krakenX60", "driveCurrentLimit": 60.0, diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java deleted file mode 100644 index ee735f9..0000000 --- a/src/main/java/frc4388/robot/Constants.java +++ /dev/null @@ -1,200 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* 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.pathplanner.lib.config.ModuleConfig; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPLTVController; -import com.pathplanner.lib.controllers.PathFollowingController; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.system.plant.DCMotor; - -// import com.pathplanner.l; - -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import frc4388.utility.Gains; -import frc4388.utility.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 class SwerveDriveConstants { - - public static final double MAX_ROT_SPEED = 3.5; - 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 String CANBUS_NAME = "IDK"; - - public static final double CORRECTION_MIN = 10; - public static final double CORRECTION_MAX = 50; - - public static final double[] GEARS = {0.25, 0.5, 1.0}; - - 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 class DefaultSwerveRotOffsets { - public static final double FRONT_LEFT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets. - public static final double FRONT_RIGHT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets. - public static final double BACK_LEFT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets. - public static final double BACK_RIGHT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets. - } - - public static final class IDs { - public static final int FR_ID = 22; - public static final int FL_ID = 20; - public static final int BL_ID = 21; - public static final int BR_ID = 23; - // public static final int RIGHT_FRONT_WHEEL_ID = 2; - // public static final int RIGHT_FRONT_STEER_ID = 3; - // public static final int RIGHT_FRONT_ENCODER_ID = 10; - - // public static final int LEFT_FRONT_WHEEL_ID = 4; - // public static final int LEFT_FRONT_STEER_ID = 5; - // public static final int LEFT_FRONT_ENCODER_ID = 11; - - // public static final int LEFT_BACK_WHEEL_ID = 6; - // public static final int LEFT_BACK_STEER_ID = 7; - // public static final int LEFT_BACK_ENCODER_ID = 12; - - // public static final int RIGHT_BACK_WHEEL_ID = 8; - // public static final int RIGHT_BACK_STEER_ID = 9; - // public static final int RIGHT_BACK_ENCODER_ID = 13; - - public static final int DRIVE_PIGEON_ID = 10; - } - - 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 Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); - - } - - public static final class AutoConstants { - public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0); - public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0); - public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0); - public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune - - public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value - public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value - - public static final double MASS = 10; // KG - public static final double MOI = 10; // Moment of inertia - - public static final double WHEEL_RADIUS = 0.05; // Meters - public static final double MAX_DRIVE_VELOCITY = 0.5; // Meters per second - public static final double MAX_ROT_SPEED = 0.2; // Rotations per second - - - public static final double COEFFICENT_OF_FRICTION = 1.0; // Between 0 and 1 - public static final DCMotor TALON_SRX_MOTOR = DCMotor.getVex775Pro(1); //TODO: Get actual motor constants - public static final double DRIVE_CURRENT_LIMIT = 100000; //TODO: Get actual value - - - public static final ModuleConfig MODULE_CONFIG = new ModuleConfig( - WHEEL_RADIUS, - MAX_DRIVE_VELOCITY, - COEFFICENT_OF_FRICTION, - TALON_SRX_MOTOR, - DRIVE_CURRENT_LIMIT, - 2); - - public static final RobotConfig PP_ROBOT_CONFIG = new RobotConfig( - MASS, - MOI, - MODULE_CONFIG, - new Translation2d[] { - new Translation2d(), - new Translation2d(), - new Translation2d(), - new Translation2d(), - }); - - - public static final double ROBOT_LOOP_TIME = 0.02; // Time it takes for the robot to run a loop - public static final PathFollowingController PP_PATH_FOLLOWING_CONTROLLER = new PPLTVController(ROBOT_LOOP_TIME); - } - - public static final class Conversions { - public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 6.22; - public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = JOYSTICK_TO_METERS_PER_SECOND_FAST * 0.5; - - public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; - public static final double MOTOR_REV_PER_STEER_REV = 12.8; - - public static final double TICKS_PER_MOTOR_REV = 0.5; - public static final double WHEEL_DIAMETER_INCHES = 3.9; - public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; - - public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; - public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV; - public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV; - public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH; - - public static final double TICK_TIME_TO_SECONDS = 10; - public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS; - } - - public static final class Configurations { - public static final double OPEN_LOOP_RAMP_RATE = 0.2; - public static final double CLOSED_LOOP_RAMP_RATE = 0.2; - public static final double NEUTRAL_DEADBAND = 0.04; - } - - public static final double MAX_SPEED_FEET_PER_SECOND = 20.4; - public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; - - // dimensions - public static final double WIDTH = 18.5; - public static final double HEIGHT = 18.5; - public static final double HALF_WIDTH = WIDTH / 2.d; - public static final double HALF_HEIGHT = HEIGHT / 2.d; - - // misc - public static final int TIMEOUT_MS = 30; - public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; - } - - public static final class VisionConstants { - public static final String CAMERA_NAME = "Camera_Module_v1"; - } - - public static final class DriveConstants { - public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; - } - - public static final class LEDConstants { - public static final int LED_SPARK_ID = 0; - - public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_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 XBOX_PROGRAMMER_ID = 2; - public static final double LEFT_AXIS_DEADBAND = 0.20; - - } -} diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 573ce52..dc49322 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,14 +7,24 @@ package frc4388.robot; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// 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.RobotTime; +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 @@ -23,7 +33,7 @@ import frc4388.utility.RobotTime; * creating this project, you must also update the build.gradle file in the * project. */ -public class Robot extends TimedRobot { +public class Robot extends LoggedRobot { Command m_autonomousCommand; private RobotTime m_robotTime = RobotTime.getInstance(); @@ -35,12 +45,20 @@ public class Robot extends TimedRobot { */ @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, @@ -52,15 +70,15 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { m_robotTime.updateTimes(); - //System.out.println(m_robotContainer.limelight.isNearSpeaker()); - //mled.updateLED(); + // 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 @@ -108,14 +126,20 @@ public class Robot extends TimedRobot { 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(); } @@ -129,9 +153,112 @@ public class Robot extends TimedRobot { } /** - * This function is called periodically during test mode. + * This function is called periodically during operator control. */ @Override - public void testPeriodic() { + 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 index cf83f7f..3704390 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,17 +7,17 @@ package frc4388.robot; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; - // Drive Systems import edu.wpi.first.wpilibj.DriverStation; + +import java.io.File; 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 frc4388.robot.Constants.OIConstants; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -25,88 +25,94 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; - // Autos import frc4388.utility.controller.VirtualController; -import frc4388.robot.commands.Swerve.neoJoystickPlayback; -import frc4388.robot.commands.Swerve.neoJoystickRecorder; -import frc4388.robot.subsystems.RobotLocalizer; -// Subsystems -// import frc4388.robot.subsystems.LED; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.robot.subsystems.TankDrive; +import frc4388.robot.commands.wait.waitSupplier; +import frc4388.robot.constants.Constants.OIConstants; +import frc4388.robot.subsystems.differential.DiffDrive; + +import com.pathplanner.lib.commands.PathPlannerAuto; + // Utilites import frc4388.utility.DeferredBlock; -import frc4388.utility.configurable.ConfigurableString; +import frc4388.utility.compute.TimesNegativeOne; /** * This class is where the bulk of the robot should be declared. Since * Command-based is a "declarative" paradigm, very little robot logic should * actually be handled in the {@link Robot} periodic methods (other than the - * scheduler calls). Instead, the structure of the robot (including subsystems, + * 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(); + + public final RobotMap m_robotMap = RobotMap.configureReal(); /* Subsystems */ - // private final LED m_robotLED = new LED(); + public final DiffDrive m_DiffDrive = new DiffDrive(m_robotMap.m_DiffDrive, m_robotMap.m_gyro); + // 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.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.IRIntakeBeam, 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 SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, - // m_robotMap.rightFront, - // m_robotMap.leftBack, - // m_robotMap.rightBack, - - // m_robotMap.gyro); - - - // ! /* Autos */ - private final RobotLocalizer robotLocalizer = new RobotLocalizer(m_robotMap.gyro, m_robotMap.cam); - private final SendableChooser autoChooser; - - - private final TankDrive tankDrive = new TankDrive(m_robotMap.FR, m_robotMap.FL, m_robotMap.BL, m_robotMap.BR, robotLocalizer); /* 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 DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(OIConstants.XBOX_PROGRAMMER_ID); + private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID); - /* Virtual Controllers */ - private final VirtualController m_virtualDriver = new VirtualController(0); - private final VirtualController m_virtualOperator = new VirtualController(1); + // 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; + + // private Command waitFeedStation = new waitSupplier(m_robotElevator::readyToMove); + // private Command waitDebuger = new waitSupplier(m_driverXbox::getYButtonPressed); + // private Command waitDebugerManual = new waitSupplier(m_driverXbox::getYButtonPressed); + private Command waitDebuger = new waitSupplier(() -> true); - // private String lastAutoName = "defualt.auto"; - // private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); - // private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, - // () -> autoplaybackName.get(), // lastAutoName - // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, - // true, false); - /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { configureButtonBindings(); configureVirtualButtonBindings(); - // new DeferredBlock(() -> m_robotSwerveDrive.resetGyroFlip()); + + DeferredBlock.addBlock(() -> { // Called on first robot enable + // m_robotSwerveDrive.resetGyro(); + }, false); + DeferredBlock.addBlock(() -> { // Called on every robot enable + TimesNegativeOne.update(); + }, true); + DriverStation.silenceJoystickConnectionWarning(true); // CameraServer.startAutomaticCapture(); /* Default Commands */ // ! Swerve Drive Default Command (Regular Rotation) // drives the robot with a two-axis input from the driver controller - tankDrive.setDefaultCommand(new RunCommand(() -> { - tankDrive.driveWithInput(getDeadbandedDriverController().getLeft(), - getDeadbandedDriverController().getRight(), - true); - }, tankDrive) + m_DiffDrive.setDefaultCommand(new RunCommand(() -> { + m_DiffDrive.arcadeDrive(m_driverXbox.getLeft(), m_driverXbox.getRight()); + }, m_DiffDrive) .withName("SwerveDrive DefaultCommand")); - // m_robotMap.tankDrive.setToSlow(); + + makeAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); + // this.subsystems.add(m_robotSwerveDrive); + // this.subsystems.add(m_robotMap.leftFront); + // this.subsystems.add(m_robotMap.rightFront); + // this.subsystems.add(m_robotMap.rightBack); + // this.subsystems.add(m_robotMap.leftBack); // ! Swerve Drive One Module Test // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { @@ -133,8 +139,7 @@ public class RobotContainer { - autoChooser = AutoBuilder.buildAutoChooser(); - SmartDashboard.putData("Auto Chooser", autoChooser); + } /** @@ -145,26 +150,10 @@ public class RobotContainer { */ private void configureButtonBindings() { - // ? /* Driver Buttons */ + new JoystickButton(m_driverXbox, XboxController.A_BUTTON).onTrue(new InstantCommand(() -> { + m_DiffDrive.resetOdometry(); + })); - // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip())); - - // // ! /* Speed */ - // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); - - // new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot())); - - // new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); - - // ? /* Operator Buttons */ - // ? /* Programer Buttons (Controller 3)*/ // * /* Auto Recording */ @@ -180,6 +169,7 @@ public class RobotContainer { // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, // true, false)) // .onFalse(new InstantCommand()); + } /** @@ -214,8 +204,62 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // return autoPlayback; - return autoChooser.getSelected(); + + + //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); + } /** @@ -236,11 +280,7 @@ public class RobotContainer { return this.m_operatorXbox; } - public VirtualController getVirtualDriverController() { - return m_virtualDriver; - } - - public VirtualController getVirtualOperatorController() { - return m_virtualOperator; + 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 index 6a74795..9186e8d 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -7,76 +7,43 @@ package frc4388.robot; -import com.ctre.phoenix6.hardware.TalonFX; -import com.pathplanner.lib.commands.PathPlannerAuto; - -import edu.wpi.first.wpilibj2.command.Command; - -import org.photonvision.PhotonCamera; - import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.Pigeon2; -// import edu.wpi.first.wpilibj.motorcontrol.Spark; -// import frc4388.robot.Constants.LEDConstants; -import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.Constants.VisionConstants; -import frc4388.robot.subsystems.SwerveModule; -import frc4388.robot.subsystems.TankDrive; -import frc4388.utility.RobotGyro; +import frc4388.robot.subsystems.differential.DiffConstants; +import frc4388.robot.subsystems.differential.DiffIO; +import frc4388.robot.subsystems.differential.DiffTalonSRX; +import frc4388.robot.subsystems.differential.GyroIO; +import frc4388.robot.subsystems.differential.GyroPigeon2; /** * 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 PhotonCamera cam = new PhotonCamera(VisionConstants.CAMERA_NAME); - - // public SwerveModule leftFront; - // public SwerveModule rightFront; - // public SwerveModule leftBack; - // public SwerveModule rightBack; + // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id); + // public RobotGyro gyro = new RobotGyro(m_pigeon2); - public RobotMap() { - configureDriveMotorControllers(); + public DiffIO m_DiffDrive; + public GyroIO m_gyro; + + public static RobotMap configureReal() { + RobotMap map = new RobotMap(); + + TalonSRX m_leftFront = new TalonSRX(DiffConstants.IDs.FRONT_LEFT_MOTOR.id); + TalonSRX m_rightFront = new TalonSRX(DiffConstants.IDs.FRONT_RIGHT_MOTOR.id); + TalonSRX m_leftRear = new TalonSRX(DiffConstants.IDs.REAR_LEFT_MOTOR.id); + TalonSRX m_rightRear = new TalonSRX(DiffConstants.IDs.REAR_RIGHT_MOTOR.id); + + map.m_DiffDrive = new DiffTalonSRX( + m_leftFront, m_rightFront, + m_leftRear, m_rightRear + ); + + Pigeon2 pigeon2 = new Pigeon2(10); + map.m_gyro = new GyroPigeon2(pigeon2); + + return map; } - - public final TalonSRX FR = new TalonSRX(SwerveDriveConstants.IDs.FR_ID); - public final TalonSRX FL = new TalonSRX(SwerveDriveConstants.IDs.FL_ID); - public final TalonSRX BL = new TalonSRX(SwerveDriveConstants.IDs.BL_ID); - public final TalonSRX BR = new TalonSRX(SwerveDriveConstants.IDs.BR_ID); - - public TankDrive tankDrive; - - /* LED Subsystem */ - // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); - - /* Swreve Drive Subsystem */ - // public final TalonFX leftFrontWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); - // public final TalonFX leftFrontSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); - // public final CANcoder leftFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID); - - - // public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); - // public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); - // public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); - - // public final TalonFX leftBackWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); - // public final TalonFX leftBackSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); - // public final CANcoder leftBackEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); - - // public final TalonFX rightBackWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); - // public final TalonFX rightBackSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); - // public final CANcoder rightBackEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); - - void configureDriveMotorControllers() { - // initialize SwerveModules - // this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); - // this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET); - // this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET); - // this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET); - } -} + +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java deleted file mode 100644 index f3d636d..0000000 --- a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java +++ /dev/null @@ -1,98 +0,0 @@ -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.subsystems.SwerveDrive; - -public class PlaybackChooser { - 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; - private final SwerveDrive m_swerve; - - // commands - private Command m_noAuto = new InstantCommand(); - - public PlaybackChooser(SwerveDrive swerve, Object... pool) { - m_swerve = swerve; - } - - public PlaybackChooser addOption(String name, Command option) { - m_commandPool.put(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 appendCommand() { - var chooser = new SendableChooser(); - chooser.setDefaultOption("No Auto", m_noAuto); - - m_choosers.add(chooser); - ComplexWidget widget = Shuffleboard.getTab("Auto Chooser") - .add("Command: " + m_choosers.size(), chooser) - .withSize(4, 1) - .withPosition(0, m_choosers.size() - 1) - .withWidget(BuiltInWidgets.kSplitButtonChooser); - - 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 Command getCommand() { - Command command = m_playback.getSelected(); - command = command == null ? m_noAuto : command.asProxy(); - - Command[] commands = new Command[m_cmdNum - 1]; - for (int i = 0; i < m_cmdNum - 1; i++) { - Command command2 = m_choosers.get(i + 1).getSelected(); - command2 = command2 == null ? m_noAuto : command2.asProxy(); - - commands[i] = command2.asProxy(); - } - - return command.andThen(commands); - } -} diff --git a/src/main/java/frc4388/robot/commands/Autos/Taxi.txt b/src/main/java/frc4388/robot/commands/Autos/Taxi.txt deleted file mode 100644 index c99ee2c..0000000 --- a/src/main/java/frc4388/robot/commands/Autos/Taxi.txt +++ /dev/null @@ -1,225 +0,0 @@ -0.0,0.0,0.0,0.0,0 -0.0,0.0,0.0,0.0,0 -0.0,0.0,0.0,0.0,12 -0.0,0.0,0.0,0.0,26 -0.0,0.0,0.0,0.0,37 -0.0,0.0,0.0,0.0,50 -0.0,0.0,0.0,0.0,62 -0.0,0.0,0.0,0.0,73 -0.0,0.0,0.0,0.0,88 -0.0,0.0,0.0,0.0,103 -0.0,0.0,0.0,0.0,116 -0.0,0.0,0.0,0.0,160 -0.0,0.0,0.0,0.0,173 -0.0,0.0,0.0,0.0,185 -0.0,0.0,0.0,0.0,200 -0.0,0.0,0.0,0.0,211 -0.0,0.0,0.0,0.0,223 -0.0,0.0,0.0,0.0,235 -0.0,0.0,0.0,0.0,247 -0.0,0.0,0.0,0.0,263 -0.0,0.0,0.0,0.0,283 -0.0,0.0,0.0,0.0,303 -0.0,-0.109375,0.0,0.0,323 -0.0,-0.1484375,0.0,0.0,343 -0.0,-0.2109375,0.0,0.0,363 -0.0,-0.3671875,0.0,0.0,398 -0.0,-0.4140625,0.0,0.0,411 -0.0,-0.4765625,0.0,0.0,425 -0.0,-0.5078125,0.0,0.0,443 -0.0,-0.5078125,0.0,0.0,463 -0.0,-0.53125,0.0,0.0,483 -0.0,-0.5546875,0.0,0.0,504 -0.0,-0.5625,0.0,0.0,523 -0.0,-0.5625,0.0,0.0,544 -0.0,-0.5703125,0.0,0.0,563 -0.0,-0.5859375,0.0,0.0,584 -0.0,-0.5859375,0.0,0.0,603 -0.0,-0.5859375,0.0,0.0,640 -0.0,-0.59375,0.0,0.0,657 -0.0,-0.6015625,0.0,0.0,672 -0.0,-0.6015625,0.0,0.0,685 -0.0,-0.6015625,0.0,0.0,703 -0.0,-0.6015625,0.0,0.0,723 -0.0,-0.6015625,0.0,0.0,743 -0.0,-0.6015625,0.0,0.0,763 -0.0,-0.6015625,0.0,0.0,783 -0.0,-0.6015625,0.0,0.0,803 -0.0,-0.6015625,0.0,0.0,823 -0.0,-0.6015625,0.0,0.0,844 -0.0,-0.6015625,0.0,0.0,878 -0.0,-0.6015625,0.0,0.0,893 -0.0,-0.6015625,0.0,0.0,907 -0.0,-0.6015625,0.0,0.0,924 -0.0,-0.609375,0.0,0.0,943 -0.0,-0.609375,0.0,0.0,963 -0.0,-0.609375,0.0,0.0,983 -0.0,-0.609375,0.0,0.0,1004 -0.0,-0.609375,0.0,0.0,1023 -0.0,-0.609375,0.0,0.0,1043 -0.0,-0.609375,0.0,0.0,1064 -0.0,-0.609375,0.0,0.0,1083 -0.0,-0.609375,0.0,0.0,1156 -0.0,-0.609375,0.0,0.0,1172 -0.0,-0.609375,0.0,0.0,1185 -0.0,-0.609375,0.0,0.0,1200 -0.0,-0.609375,0.0,0.0,1215 -0.0,-0.609375,0.0,0.0,1225 -0.0,-0.609375,0.0,0.0,1236 -0.0,-0.609375,0.0,0.0,1249 -0.0,-0.609375,0.0,0.0,1263 -0.0,-0.609375,0.0,0.0,1283 -0.0,-0.609375,0.0,0.0,1303 -0.0,-0.609375,0.0,0.0,1323 -0.0,-0.609375,0.0,0.0,1363 -0.0,-0.6015625,0.0,0.0,1376 -0.0,-0.6015625,0.0,0.0,1394 -0.0,-0.6015625,0.0,0.0,1405 -0.0,-0.6015625,0.0,0.0,1423 -0.0,-0.6015625,0.0,0.0,1443 -0.0,-0.6015625,0.0,0.0,1463 -0.0,-0.6015625,0.0,0.0,1483 -0.0,-0.6015625,0.0,0.0,1503 -0.0,-0.6015625,0.0,0.0,1523 -0.0,-0.6015625,0.0,0.0,1543 -0.0,-0.6015625,0.0,0.0,1563 -0.0,-0.6015625,0.0,0.0,1597 -0.0,-0.6015625,0.0,0.0,1608 -0.0,-0.6015625,0.0,0.0,1624 -0.0,-0.6015625,0.0,0.0,1643 -0.0,-0.6015625,0.0,0.0,1664 -0.0,-0.5859375,0.0,0.0,1683 -0.0,-0.5859375,0.0,0.0,1703 -0.0,-0.5625,0.0,0.0,1723 -0.0,-0.5625,0.0,0.0,1743 -0.0,-0.5625,0.0,0.0,1763 -0.0,-0.5625,0.0,0.0,1783 -0.0,-0.5625,0.0,0.0,1803 -0.0,-0.5625,0.0,0.0,1843 -0.0,-0.5625,0.0,0.0,1855 -0.0,-0.5625,0.0,0.0,1868 -0.0,-0.5625,0.0,0.0,1883 -0.0,-0.5625,0.0,0.0,1903 -0.0,-0.5625,0.0,0.0,1923 -0.0,-0.5625,0.0,0.0,1943 -0.0,-0.5625,0.0,0.0,1963 -0.0,-0.5625,0.0,0.0,1983 -0.0,-0.5625,0.0,0.0,2003 -0.0,-0.5625,0.0,0.0,2024 -0.0,-0.5625,0.0,0.0,2043 -0.0,-0.5625,0.0,0.0,2081 -0.0,-0.5625,0.0,0.0,2093 -0.0,-0.5625,0.0,0.0,2105 -0.0,-0.5625,0.0,0.0,2123 -0.0,-0.5625,0.0,0.0,2143 -0.0,-0.5625,0.0,0.0,2163 -0.0,-0.5625,0.0,0.0,2183 -0.0,-0.5625,0.0,0.0,2203 -0.0,-0.5625,0.0,0.0,2223 -0.0,-0.5625,0.0,0.0,2243 -0.0,-0.5625,0.0,0.0,2263 -0.0,-0.5625,0.0,0.0,2283 -0.0,-0.5625,0.0,0.0,2366 -0.0,-0.5625,0.0,0.0,2377 -0.0,-0.5625,0.0,0.0,2394 -0.0,-0.5703125,0.0,0.0,2405 -0.0,-0.5703125,0.0,0.0,2418 -0.0,-0.5703125,0.0,0.0,2431 -0.0,-0.5703125,0.0,0.0,2444 -0.0,-0.5703125,0.0,0.0,2458 -0.0,-0.5703125,0.0,0.0,2470 -0.0,-0.5703125,0.0,0.0,2485 -0.0,-0.5703125,0.0,0.0,2503 -0.0,-0.5703125,0.0,0.0,2523 -0.0,-0.5703125,0.0,0.0,2563 -0.0,-0.5703125,0.0,0.0,2577 -0.0,-0.5703125,0.0,0.0,2591 -0.0,-0.5703125,0.0,0.0,2608 -0.0,-0.5703125,0.0,0.0,2624 -0.0,-0.5703125,0.0,0.0,2643 -0.0,-0.5703125,0.0,0.0,2677 -0.0,-0.5703125,0.0,0.0,2698 -0.0,-0.5703125,0.0,0.0,2711 -0.0,-0.5703125,0.0,0.0,2725 -0.0,-0.5703125,0.0,0.0,2743 -0.0,-0.5703125,0.0,0.0,2764 -0.0,-0.5703125,0.0,0.0,2810 -0.0,-0.5703125,0.0,0.0,2820 -0.0,-0.5703125,0.0,0.0,2833 -0.0,-0.5703125,0.0,0.0,2845 -0.0,-0.5703125,0.0,0.0,2863 -0.0,-0.5703125,0.0,0.0,2883 -0.0,-0.5703125,0.0,0.0,2904 -0.0,-0.5703125,0.0,0.0,2924 -0.0,-0.5703125,0.0,0.0,2943 -0.0,-0.5703125,0.0,0.0,2963 -0.0,-0.5703125,0.0,0.0,2983 -0.0,-0.5703125,0.0,0.0,3003 -0.0,-0.5703125,0.0,0.0,3033 -0.0,-0.5703125,0.0,0.0,3050 -0.0,-0.5703125,0.0,0.0,3065 -0.0,-0.5703125,0.0,0.0,3083 -0.0,-0.5703125,0.0,0.0,3103 -0.0,-0.5703125,0.0,0.0,3123 -0.0,-0.5703125,0.0,0.0,3144 -0.0,-0.5703125,0.0,0.0,3164 -0.0,-0.5703125,0.0,0.0,3184 -0.0,-0.5703125,0.0,0.0,3203 -0.0,-0.5703125,0.0,0.0,3223 -0.0,-0.5703125,0.0,0.0,3243 -0.0,-0.5703125,0.0,0.0,3272 -0.0,-0.5703125,0.0,0.0,3289 -0.0,-0.5703125,0.0,0.0,3303 -0.0,-0.5703125,0.0,0.0,3323 -0.0,-0.5703125,0.0,0.0,3343 -0.0,-0.5703125,0.0,0.0,3363 -0.0,-0.5703125,0.0,0.0,3383 -0.0,-0.5703125,0.0,0.0,3403 -0.0,-0.5703125,0.0,0.0,3423 -0.0,-0.5703125,0.0,0.0,3443 -0.0,-0.5703125,0.0,0.0,3463 -0.0,-0.5703125,0.0,0.0,3483 -0.0,-0.5703125,0.0,0.0,3566 -0.0,-0.5703125,0.0,0.0,3578 -0.0,-0.5703125,0.0,0.0,3596 -0.0,-0.5703125,0.0,0.0,3610 -0.0,-0.5703125,0.0,0.0,3623 -0.0,-0.5703125,0.0,0.0,3640 -0.0,-0.5703125,0.0,0.0,3651 -0.0,-0.5703125,0.0,0.0,3663 -0.0,-0.5703125,0.0,0.0,3678 -0.0,-0.5703125,0.0,0.0,3691 -0.0,-0.5703125,0.0,0.0,3706 -0.0,-0.5703125,0.0,0.0,3723 -0.0,-0.5703125,0.0,0.0,3766 -0.0,-0.5703125,0.0,0.0,3778 -0.0,-0.5703125,0.0,0.0,3792 -0.0,-0.5703125,0.0,0.0,3807 -0.0,-0.5703125,0.0,0.0,3823 -0.0,-0.5703125,0.0,0.0,3843 -0.0,-0.53125,0.0,0.0,3863 -0.0,-0.53125,0.0,0.0,3884 -0.0,-0.421875,0.0,0.0,3904 -0.0,0.0,0.0,0.0,3924 -0.0,0.0,0.0,0.0,3944 -0.0,0.0,0.0,0.0,3963 -0.0,0.0,0.0,0.0,3999 -0.0,0.0,0.0,0.0,4010 -0.0,0.0,0.0,0.0,4025 -0.0,0.0,0.0,0.0,4043 -0.0,0.0,0.0,0.0,4063 -0.0,0.0,0.0,0.0,4083 -0.0,0.0,0.0,0.0,4103 -0.0,0.0,0.0,0.0,4123 -0.0,0.0,0.0,0.0,4143 -0.0,0.0,0.0,0.0,4163 -0.0,0.0,0.0,0.0,4183 -0.0,0.0,0.0,0.0,4203 -0.0,0.0,0.0,0.0,4236 -0.0,0.0,0.0,0.0,4247 -0.0,0.0,0.0,0.0,4264 -0.0,0.0,0.0,0.0,4284 -0.0,0.0,0.0,0.0,4304 -0.0,0.0,0.0,0.0,4324 -0.0,0.0,0.0,0.0,4343 -0.0,0.0,0.0,0.0,4363 \ 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 index 86bc7b2..a069786 100644 --- a/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java @@ -1,3 +1,4 @@ +package frc4388.robot.commands.autos; // package frc4388.robot.commands.Autos; // import java.io.File; 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..d3ad236 --- /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.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..8e2d5a3 --- /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.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 index 97233f8..c95c9fd 100644 --- a/src/main/java/frc4388/robot/commands/PID.java +++ b/src/main/java/frc4388/robot/commands/PID.java @@ -5,7 +5,7 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.utility.Gains; +import frc4388.utility.structs.Gains; public abstract class PID extends Command { protected Gains gains; diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java deleted file mode 100644 index ae054ed..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java +++ /dev/null @@ -1,145 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands.Swerve; - -import java.io.File; -import java.io.FileNotFoundException; -import java.util.ArrayList; -import java.util.Scanner; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.UtilityStructs.TimedOutput; - -public class JoystickPlayback extends Command { - private final SwerveDrive swerve; - private String filename; - private int mult = 1; - private Scanner input; - private final ArrayList outputs = new ArrayList<>(); - private int counter = 0; - private long startTime = 0; - private long playbackTime = 0; - private int lastIndex; - private boolean m_finished = false; // ! find a better way - - /** Creates a new JoystickPlayback. */ - public JoystickPlayback(SwerveDrive swerve, String filename, int mult) { - this.swerve = swerve; - this.filename = filename; - this.mult = mult; - - addRequirements(this.swerve); - } - - /** Creates a new JoystickPlayback. */ - public JoystickPlayback(SwerveDrive swerve, String filename) { - this(swerve, filename, 1); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - outputs.clear(); - m_finished = false; - - startTime = System.currentTimeMillis(); - playbackTime = 0; - lastIndex = 0; - try { - input = new Scanner(new File("/home/lvuser/autos/" + filename)); - - String line = ""; - while (input.hasNextLine()) { - line = input.nextLine(); - - if (line.isEmpty() || line.isBlank() || line.equals("\n")) { - continue; - } - - String[] values = line.split(","); - System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]); - - var out = new TimedOutput(); - out.leftX = Double.parseDouble(values[0]) * mult; - out.leftY = Double.parseDouble(values[1]); - out.rightX = Double.parseDouble(values[2]); - out.rightY = Double.parseDouble(values[3]); - - out.timedOffset = Long.parseLong(values[4]); - - outputs.add(out); - } - - input.close(); - } catch (FileNotFoundException e) { - e.printStackTrace(); - } - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if (counter == 0) { - startTime = System.currentTimeMillis(); - playbackTime = 0; - } else { - playbackTime = System.currentTimeMillis() - startTime; - } - - // skip to reasonable time frame - // too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing - { - int i = lastIndex == 0 ? 1 : lastIndex; - while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) { - i++; - } - - if (i >= outputs.size()) { - m_finished = true; // ! kind of a hack - return; - } - lastIndex = i; - } - - TimedOutput lastOut = outputs.get(lastIndex - 1); - TimedOutput out = outputs.get(lastIndex); - - double deltaTime = out.timedOffset - lastOut.timedOffset; - double playbackDelta = playbackTime - lastOut.timedOffset; - - double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime); - double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime); - double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime); - double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime); - - // this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY), - // new Translation2d(out.rightX, out.rightY), - // true); - - // this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY), - // new Translation2d(lerpRX, lerpRY), - // true); - - this.swerve.playbackDriveWithInput( new Translation2d(lerpLX, lerpLY), - new Translation2d(lerpRX, lerpRY), - true); - - counter++; - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - input.close(); - swerve.stopModules(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return m_finished; - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java deleted file mode 100644 index 0224fcf..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java +++ /dev/null @@ -1,97 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands.Swerve; - -import java.io.File; -import java.io.IOException; -import java.io.PrintWriter; -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.SwerveDrive; -import frc4388.utility.UtilityStructs.TimedOutput; - -public class JoystickRecorder extends Command { - public final SwerveDrive swerve; - - public final Supplier leftX; - public final Supplier leftY; - public final Supplier rightX; - public final Supplier rightY; - private String filename; - public final ArrayList outputs = new ArrayList<>(); - private long startTime = -1; - - - /** Creates a new JoystickRecorder. */ - public JoystickRecorder(SwerveDrive swerve, Supplier leftX, Supplier leftY, - Supplier rightX, Supplier rightY, - String filename) - { - this.swerve = swerve; - this.leftX = leftX; - this.leftY = leftY; - this.rightX = rightX; - this.rightY = rightY; - this.filename = filename; - - addRequirements(this.swerve); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - outputs.clear(); - - this.startTime = System.currentTimeMillis(); - - outputs.add(new TimedOutput()); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - var inputs = new TimedOutput(); - inputs.leftX = leftX.get(); - inputs.leftY = leftY.get(); - inputs.rightX = rightX.get(); - inputs.rightY = rightY.get(); - inputs.timedOffset = System.currentTimeMillis() - startTime; - - outputs.add(inputs); - - swerve.playbackDriveWithInput(new Translation2d(inputs.leftX, inputs.leftY), - new Translation2d(inputs.rightX, inputs.rightY), - true); - - //System.out.println("RECORDING"); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - File output = new File("/home/lvuser/autos/" + filename); - - try (PrintWriter writer = new PrintWriter(output)) { - for (var input : outputs) { - writer.println( input.leftX + "," + input.leftY + "," + - input.rightX + "," + input.rightY + "," + - input.timedOffset); - } - - writer.close(); - } catch (IOException e) { - e.printStackTrace(); - } - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java index a2945c0..50e616c 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java +++ b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java @@ -2,11 +2,11 @@ // 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; +package frc4388.robot.commands.swerve; import edu.wpi.first.math.geometry.Translation2d; import frc4388.robot.commands.PID; -import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.swerve.SwerveDrive; public class RotateToAngle extends PID { diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java index 8b5afdf..bff5105 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java @@ -1,4 +1,4 @@ -package frc4388.robot.commands.Swerve; +package frc4388.robot.commands.swerve; import java.io.FileInputStream; import java.util.ArrayList; @@ -6,11 +6,11 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.DataUtils; -import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; -import frc4388.utility.UtilityStructs.AutoRecordingFrame; +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; /** @@ -24,8 +24,8 @@ public class neoJoystickPlayback extends Command { private final Supplier filenameGetter; private String filename; private int frame_index = 0; - private long startTime = 0; - private long playbackTime = 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 @@ -150,8 +150,8 @@ public class neoJoystickPlayback extends Command { @Override public void initialize() { - startTime = System.currentTimeMillis(); - playbackTime = 0; + // startTime = System.currentTimeMillis(); + // playbackTime = 0; frame_index = 0; m_finished = !loadAuto(); diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java index 7f48a6c..4cf3159 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java @@ -1,4 +1,4 @@ -package frc4388.robot.commands.Swerve; +package frc4388.robot.commands.swerve; import java.io.FileOutputStream; import java.util.ArrayList; @@ -7,11 +7,11 @@ 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.SwerveDrive; -import frc4388.utility.DataUtils; -import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; -import frc4388.utility.UtilityStructs.AutoRecordingFrame; +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 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..7108de5 --- /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; + +/* 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..73fd893 --- /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; + +/* 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..2f66710 --- /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; + +/* 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..e81a70e --- /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 = "Vision-Minibot"; + public static final String VERSION = "unspecified"; + public static final int GIT_REVISION = 5; + public static final String GIT_SHA = "a9e45fcf9ba356f63af7c567d51115d255bb269b"; + public static final String GIT_DATE = "2024-12-16 00:39:59 MST"; + public static final String GIT_BRANCH = "master"; + public static final String BUILD_DATE = "2025-07-16 14:12:56 MDT"; + public static final long BUILD_UNIX_TIME = 1752696776235L; + 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..6e8733d --- /dev/null +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -0,0 +1,234 @@ +/*----------------------------------------------------------------------------*/ +/* 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 Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0); + public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 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; + + // X is tangent to reef side + // Y is normal to reef side + public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5+1); // This is from the field + public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16+1); + public static final double HALF_ROBOT_SIZE = Units.inchesToMeters(18); + + public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15); + public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP; + // public static final double L4_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(4.5); + + public static final double L3_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15); + public static final double L3_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(5+1); + + public static final double L2_PREP_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(6); + public static final double L2_SCORE_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(0.5-2); + + public static final double ALGAE_REMOVAL_DISTANCE = HALF_ROBOT_SIZE; + + // public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); + // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5); + // // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5); + + // public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); + // public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1); + + // public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); + // public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5); + + // public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); + + public static final int L4_DRIVE_TIME = 250; //Milliseconds + // public static final int L3_DRIVE_TIME = 500; + public static final int L2_DRIVE_TIME = 250; //Milliseconds + public static final int ALGAE_DRIVE_TIME = 500; + public static final double STOP_VELOCITY = 0.0; + } + + 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; + + } + + public static final class ElevatorConstants { + public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15); + public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16); + + public static final double SAFETY_ENDEFFECTOR_MAX_TORQUE = 75; + public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 20; + + // public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND + + public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND + public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND + public static final int INTAKE_LIMIT_SWITCH = 6; // TODO: FIND + + + public static final double GEAR_RATIO_ELEVATOR = -9.0; + //Max for elevator = 50% + + public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR; + public static final double WAITING_POSITION_ELEVATOR = -9.5; // TODO: find 2-4 in off the pipe + public static final double HOVERING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe + public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe + public static final double SCORING_THREE_ELEVATOR = -9.25; + public static final double DEALGAE_L2_ELEVATOR = -4; + public static final double DEALGAE_L3_ELEVATOR = -26.5; + public static final double MAX_POSITION_ELEVATOR = 4.5 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position + + public static final double GEAR_RATIO_ENDEFECTOR = -100.0; + public static final double ENDEFECTOR_DRIVE_SLOW = -0.08; + //Max for endefector = 60% + public static final double L2_SCORE_ELEVATOR = -5; + public static final double L2_LEAVE_ELEVATOR = -11; + + public static final double L2_SCORE_ENDEFFECTOR = -19; + + public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR; + public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR; + public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR; + public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; + public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; + public static final double PRIMED_FOUR_ENDEFFECTOR = 0.44 * GEAR_RATIO_ENDEFECTOR; + public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR; + + public static final Slot0Configs ELEVATOR_PID = new Slot0Configs() + .withKP(1) + .withKI(0) + .withKD(0); + + public static final Slot0Configs ENDEFFECTOR_PID = new Slot0Configs() + .withKP(1) + .withKI(0) + .withKD(0); + } + + // 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 + } + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Apriltags.java b/src/main/java/frc4388/robot/subsystems/Apriltags.java deleted file mode 100644 index c6062e8..0000000 --- a/src/main/java/frc4388/robot/subsystems/Apriltags.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc4388.robot.subsystems; - -//import edu.wpi.first.apriltag.AprilTag; -//import edu.wpi.first.math.geometry.Pose3d; -//import edu.wpi.first.math.geometry.Rotation3d; -//import edu.wpi.first.networktables.NetworkTable; -//import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; - -public class Apriltags { - public static class Tag { - public boolean visible = true; - public double x, y, z = 0; - public double ry, rp, rr = 0; - } - - public Tag getTagPosRot() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - - final Tag tag = new Tag(); - tag.visible = isAprilTag(); - tag.x = tagTable.getEntry("TagPosX").getDouble(0); - tag.y = tagTable.getEntry("TagPosY").getDouble(0); - tag.z = tagTable.getEntry("TagPosZ").getDouble(0); - tag.ry = tagTable.getEntry("TagRotY").getDouble(0); - tag.rp = tagTable.getEntry("TagRotP").getDouble(0); - tag.rr = tagTable.getEntry("TagRotR").getDouble(0); - - return tag; - } - - public boolean isAprilTag() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - return tagTable.getEntry("IsTag").getBoolean(false); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java deleted file mode 100644 index b4db7ed..0000000 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ /dev/null @@ -1,89 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* 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 com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.hardware.core.CorePigeon2; - -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import frc4388.robot.Constants.DriveConstants; -import frc4388.utility.RobotGyro; -import frc4388.utility.RobotTime; - -/** - * Add your docs here. - */ -public class DiffDrive extends SubsystemBase { - // Put methods for controlling this subsystem - // here. Call these from Commands. - - private RobotTime m_robotTime = RobotTime.getInstance(); - - private TalonFX m_leftFrontMotor; - private TalonFX m_rightFrontMotor; - private TalonFX m_leftBackMotor; - private TalonFX m_rightBackMotor; - private DifferentialDrive m_driveTrain; - private RobotGyro m_gyro; - - /** - * Add your docs here. - */ - public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor, - TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) { - - m_leftFrontMotor = leftFrontMotor; - m_rightFrontMotor = rightFrontMotor; - m_leftBackMotor = leftBackMotor; - m_rightBackMotor = rightBackMotor; - m_leftBackMotor .setControl(new Follower(m_leftFrontMotor.getDeviceID(), false)); - m_rightBackMotor.setControl(new Follower(m_rightBackMotor.getDeviceID(), false)); - m_driveTrain = driveTrain; - m_gyro = gyro; - } - - @Override - public void periodic() { - m_gyro.updatePigeonDeltas(); - - if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) { - updateSmartDashboard(); - } - } - - /** - * Add your docs here. - */ - public void driveWithInput(double move, double steer) { - m_driveTrain.arcadeDrive(move, steer); - } - - /** - * Add your docs here. - */ - public void tankDriveWithInput(double leftMove, double rightMove) { - m_leftFrontMotor.set(leftMove); - m_rightFrontMotor.set(rightMove); - } - - /** - * Add your docs here. - */ - private void updateSmartDashboard() { - - // Examples of the functionality of RobotGyro - SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon); - SmartDashboard.putNumber("Turn Rate", m_gyro.getRate()); - SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch()); - //SmartDashboard.putData(m_gyro); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java new file mode 100644 index 0000000..7ac9418 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -0,0 +1,397 @@ +// 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; + +import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.constants.Constants.AutoConstants; +import frc4388.robot.constants.Constants.ElevatorConstants; +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; + +public class Elevator extends SubsystemBase implements Queryable { + /** Creates a new Elevator. */ + private TalonFX elevatorMotor; + private TalonFX endeffectorMotor; + private LED led; + + // @AutoLog + // private class ElevatorState { + @SuppressWarnings("unused") + public long wait = 0; + public long maxWait = 1000; + + public double elevatorRefrence = 0; + public double endeffectorRefrence = 0; + + public boolean elevatorManualStop = true; + public boolean endefectorManualStop = true; + + public boolean disableAutoIntake = false; + + public boolean seededZeroEndefector = false; + public boolean seededZeroElevator = false; + + public DigitalInput basinBeamBreak; + public DigitalInput endeffectorLimitSwitch; + public DigitalInput intakeIR; + // } + + // private ElevatorState state = new ElevatorState(); + + public enum CoordinationState { + Waiting, // for coral into the though + WatingBeamTripped, //once the beam break trips + Ready, // Has coral in endefector + Hovering, // Has coral in endefector + L2Score, + L2ScoreLeave, + PrimedThree, // Arm and elevator Waiting to score in the level 3 position + ScoringThree, // Arm and elevator in the level three position + PrimedFour, // Arm and elevator Waiting to score in the level 4 position + ScoringFour, // Arm and elevator in the level four position + BallRemoverL2Primed, // Arm and elevator ready to remove the ball in the level 2 reef. + BallRemoverL2Go, // Arm and elevator removing the ball in the level 2 reef. + BallRemoverL3Primed, // Arm and elevator ready to remove the ball in the level 3 reef. + BallRemoverL3Go, // Arm and elevator removing the ball in the level 3 reef. + } + + private CoordinationState currentState; + + // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) { + public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) { + elevatorMotor = elevatorTalonFX; + endeffectorMotor = endeffectorTalonFX; + this.led = led; + + this.basinBeamBreak = basinLimitSwitch; + this.endeffectorLimitSwitch = endeffectorLimitSwitch; + this.intakeIR = intakeDigitalInput; + + elevatorMotor.setNeutralMode(NeutralModeValue.Brake); + endeffectorMotor.setNeutralMode(NeutralModeValue.Brake); + + elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID); + endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID); + currentState = CoordinationState.Ready; + + FaultReporter.register(this); + } + + //PID methods + + private void PIDPosition(TalonFX motor, double position) { + if (motor == elevatorMotor) elevatorRefrence = position; + else endeffectorRefrence = position; + + var request = new PositionDutyCycle(position); + motor.setControl(request); + } + + public void elevatorStop() { + elevatorMotor.set(0); + } + + public void endeffectorStop() { + endeffectorMotor.set(0); + } + + + public void transitionState(CoordinationState state) { + // elevatorMotor.enable(); + + + currentState = state; + switch (currentState) { + case Waiting: { + wait = System.currentTimeMillis() + maxWait; + PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); + PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0)); + led.setMode(LEDConstants.WAITING_PATTERN); + break; + } + + case WatingBeamTripped: { + PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR); + PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); + led.setMode(LEDConstants.DOWN_PATTERN); + break; + } + + case Ready: { + PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0)); + PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); + led.setMode(LEDConstants.DOWN_PATTERN); + break; + } + + case Hovering: { + PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR); + PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); + led.setMode(LEDConstants.READY_PATTERN); + break; + } + + case L2Score: { + PIDPosition(elevatorMotor, ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } + + case L2ScoreLeave: { + PIDPosition(elevatorMotor, ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } + + case PrimedFour: { + PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } + + case ScoringFour: { + PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } + + case PrimedThree: { + PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } + + case ScoringThree: { + PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } + + case BallRemoverL2Primed: { + PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } + + case BallRemoverL2Go: { + PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } + + case BallRemoverL3Primed: { + PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); + break; + } + + case BallRemoverL3Go: { + PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } + + default: { + assert false; + } + } + + } + + public void togggleAutoIntaking() { + disableAutoIntake = !disableAutoIntake; + } + + public boolean elevatorAtReference() { + // double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble(); + double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble(); + double diffrence = elevatorRefrence - elevatorPosition; + + boolean headedUp = diffrence < 0; + boolean forwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0; + boolean reverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0; + + return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp)); + } + + public boolean endeffectorAtReference() { + // double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble(); + double endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble(); + double diffrence = endeffectorRefrence - endeffectorPosition; + + boolean headedUp = diffrence < 0; + boolean forwardLimit = endeffectorMotor.getForwardLimit().asSupplier().get().value == 0; + boolean reverseLimit = endeffectorMotor.getReverseLimit().asSupplier().get().value == 0; + + return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp)); + } + // public void driveElevatorStick(Translation2d stick) { + // if (stick.getNorm() > 0.05) { + // elevatorMotor.set(stick.getY()); + // } + // } + + public boolean getEndeffectorLimit() { + return endeffectorLimitSwitch.get(); + } + + private void periodicWaiting() { + if (!basinBeamBreak.get()) + transitionState(CoordinationState.Ready); + // if(!endeffectorLimitSwitch.get()) + // transitionState(CoordinationState.Hovering); + } + + // private void periodicWaitingTripped() { + // if (!basinBeamBreak.get() && System.currentTimeMillis() > wait) + // transitionState(CoordinationState.Ready); + // } + + private void periodicReady() { + if (elevatorAtReference() && !endeffectorLimitSwitch.get()) + transitionState(CoordinationState.Hovering); + if(elevatorAtReference() && endeffectorLimitSwitch.get()) + transitionState(CoordinationState.Hovering); + } + + @SuppressWarnings("unused") + private void periodicScoring() { + if (!endeffectorLimitSwitch.get()) + transitionState(CoordinationState.Waiting); + } + + public void manualElevatorVel(double velocity) { + if (Math.abs(velocity) > 0.1) { + elevatorMotor.set(velocity); + elevatorManualStop = false; + return; + } + if (!elevatorManualStop) { + elevatorManualStop = true; + elevatorMotor.set(0); + } + } + + public void manualEndeffectorVel(double velocity) { + if (Math.abs(velocity) > 0.1) { + endeffectorMotor.set(velocity); + endefectorManualStop = false; + return; + } + if (!endefectorManualStop) { + endefectorManualStop = true; + endeffectorMotor.set(0); + } + } + + @Override + public void periodic() { + + // double elevatorVelocity = elevatorMotor.getVelocity().getValueAsDouble(); + // double elevatorTorque = elevatorMotor.getTorqueCurrent().getValueAsDouble(); + // double endeffectorVelocity = endeffectorMotor.getVelocity().getValueAsDouble(); + // double endeffectorTorque = endeffectorMotor.getTorqueCurrent().getValueAsDouble(); + + + // if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){ + // PIDPosition(endeffectorMotor, endeffectorMotor.getPosition().getValueAsDouble()); + // } + + // This method will be called once per scheduler run + // SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity); + // SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque); + SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0); + SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0); + SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0); + SmartDashboard.putString("State", currentState.toString()); + + if (!seededZeroEndefector && endeffectorMotor.getForwardLimit().asSupplier().get().value == 0) { + endeffectorMotor.setPosition(0); + seededZeroEndefector = !seededZeroEndefector; + } + + if (!seededZeroElevator && elevatorMotor.getForwardLimit().asSupplier().get().value == 0) { + elevatorMotor.setPosition(0); + seededZeroElevator = !seededZeroElevator; + } + + if (disableAutoIntake) return; + + if (currentState == CoordinationState.Waiting) { + periodicWaiting(); + } else if (currentState == CoordinationState.WatingBeamTripped) { + // periodicWaitingTripped(); + } else if (currentState == CoordinationState.Ready) { + periodicReady(); + } + + if(!intakeIR.get()){ + led.setMode(LEDConstants.DOWN_PATTERN); + } + + + // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) { + // periodicScoring(); + // } + } + + public boolean isL4Primed() { + return currentState == CoordinationState.PrimedFour; + } + + public boolean isL3Primed() { + return currentState == CoordinationState.PrimedThree; + } + + public boolean hasCoral() { + return elevatorAtReference() && currentState == CoordinationState.Hovering || !endeffectorLimitSwitch.get(); + } + + public boolean readyToMove() { + return !intakeIR.get() || hasCoral() || !endeffectorLimitSwitch.get(); + // return hasCoral(); + } + + public void armShuffle(){ + if(!basinBeamBreak.get()){ + //shuffle the coral with the arm until coral hits beam break + } + } + + @Override + public String getName() { + return "Elevator"; + } + + // @Override + // public void queryStatus() {} + + @Override + public Status diagnosticStatus() { + Status status = new Status(); + + status.addReport(ReportLevel.INFO, "Elevator Mode: " + currentState.name()); + + return status; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index e9e070c..c050278 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -7,84 +7,69 @@ package frc4388.robot.subsystems; -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.motorcontrol.Spark; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.AutoLogOutput; -import frc4388.robot.Constants.LEDConstants; -import frc4388.utility.LEDPatterns; +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 { - - static AddressableLED m_led; - static AddressableLEDBuffer m_ledBuffer; - static LED m_self; - /** - * Add your docs here. - */ - - public LED(){ - if(m_self != null) - return; - m_led = new AddressableLED(9); - m_ledBuffer = new AddressableLEDBuffer(10); - m_led.setLength(m_ledBuffer.getLength()); - m_led.setData(m_ledBuffer); - m_led.start(); - System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); +public class LED extends SubsystemBase implements Queryable { + public LED() { + FaultReporter.register(this); } - public static LED getInstance() { - if(m_self == null) - m_self = new LED(); - return m_self; + + 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(){ - //gamermode(); - //SmartDashboard.putNumber("LED", m_currentPattern.getValue()); - return; - } - static int firstcolor = 0; - static void gamermode() { - for(int i = 0; i < m_ledBuffer.getLength(); i++) { - final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180; - setLEDHSV(i, hue, 255, 128); - } - firstcolor +=3; - firstcolor %= 180; - } - /** - * Add your docs here. - */ - public static void updateLED (){ - gamermode(); - // m_LEDController.set(m_currentPattern.getValue()); + public void periodic() { + update(); } - /** - * Add your docs here. - */ - public static void setLEDRGB(int lednum, int r, int g, int b){ - m_ledBuffer.setRGB(lednum, r, g, b); - //m_currentPattern = pattern; - // m_LEDController.set(m_currentPattern.getValue()); + public void update() { + if(!LEDController.isAlive() || LEDController.isSafetyEnabled()) return; + + if(DriverStation.isDisabled()){ + LEDController.set(LEDConstants.DEFAULT_PATTERN.getValue()); + }else + LEDController.set(mode.getValue()); } - public static void setLEDHSV(int lednum, int hue, int sat, int val){ - m_ledBuffer.setRGB(lednum, hue, sat, val); - //m_currentPattern = pattern; - // m_LEDController.set(m_currentPattern.getValue()); + + @AutoLogOutput + public String state() { + return mode.getClass().toString(); } - /** - * Add your docs here. - * @return - */ - public AddressableLEDBuffer getBuffer() { - return m_ledBuffer; + + @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/RobotLocalizer.java b/src/main/java/frc4388/robot/subsystems/RobotLocalizer.java deleted file mode 100644 index 7e884c2..0000000 --- a/src/main/java/frc4388/robot/subsystems/RobotLocalizer.java +++ /dev/null @@ -1,106 +0,0 @@ -package frc4388.robot.subsystems; - -import org.photonvision.PhotonCamera; -import org.photonvision.targeting.PhotonTrackedTarget; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.util.DriveFeedforwards; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.utility.RobotGyro; - -public class RobotLocalizer extends SubsystemBase { - private RobotGyro gyro; - private Pose2d lastPose2d = new Pose2d(); - private PhotonCamera camera; - - - public RobotLocalizer(RobotGyro gyro, PhotonCamera cam) { - this.gyro = gyro; - this.camera = cam; - } - - @Override - public void periodic() { - // time - Translation3d accel = gyro.getAcceleration3d(); - - SmartDashboard.putNumber("Accel X", accel.getX()); - SmartDashboard.putNumber("Accel Y", accel.getY()); - SmartDashboard.putNumber("Accel Z", accel.getZ()); - - Rotation3d rot = gyro.getRotation3d(); - - SmartDashboard.putNumber("Rot X", rot.getX()); - SmartDashboard.putNumber("Rot Y", rot.getY()); - SmartDashboard.putNumber("Rot Z", rot.getZ()); - - // boolean tagExists = SmartDashboard.getBoolean("photonvision/Camera_Module_v1/hasTarget", false); - - Translation2d pos; - - var result = camera.getAllUnreadResults(); - // if (result.hasTargets()) { - // PhotonTrackedTarget target = result.getBestTarget(); - // Transform3d pos3d = target.getBestCameraToTarget(); - // pos = new Translation2d(pos3d.getX(), pos3d.getY()); - // } else { - pos = lastPose2d.getTranslation(); - // } - - // results. - // if (!results.isEmpty()) { - - // double totalX = 0; - // double totalY = 0; - - // // Camera processed a new frame since last - // // Get the last one in the list. - // var result = results.get(results.size() - 1); - // // PhotonTrackedTarget targets = result.getMultiTagResult(); - - // if (result.hasTargets()) { - // PhotonTrackedTarget target = result.getBestTarget(); - // Transform3d pos3d = target.getBestCameraToTarget(); - // pos = new Translation2d(pos3d.getX(), pos3d.getY()); - // } else { - // pos = lastPose2d.getTranslation(); - // } - // }else { - // pos = lastPose2d.getTranslation(); - // } - - - lastPose2d = new Pose2d( - pos, - gyro.getRotation2d() - ); - } - - // Pathplanner function - public Pose2d getPose(){ - return lastPose2d; - } - - // PathPlanner func - public void resetPose(Pose2d pose){ - lastPose2d = pose; - } - - // PathPlanner - public ChassisSpeeds getChassisSpeeds() { - return new ChassisSpeeds( - 0, - 0, - Units.rotationsToRadians(gyro.getAngularVelocity()) - ); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java deleted file mode 100644 index bd35742..0000000 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ /dev/null @@ -1,333 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.subsystems; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.Constants.SwerveDriveConstants.Conversions; -import frc4388.utility.RobotGyro; -import frc4388.utility.RobotUnits; - -public class SwerveDrive extends SubsystemBase { - - private SwerveModule leftFront; - private SwerveModule rightFront; - private SwerveModule leftBack; - private SwerveModule rightBack; - - private SwerveModule[] modules; - - private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d rightFrontLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d leftBackLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); - - private RobotGyro gyro; - - private int gear_index; - private boolean stopped = false; - - public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED; - public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; - - public double rotTarget = 0.0; - public Rotation2d orientRotTarget = new Rotation2d(); - public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); - - /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { - this.leftFront = leftFront; - this.rightFront = rightFront; - this.leftBack = leftBack; - this.rightBack = rightBack; - - this.gyro = gyro; - reset_index(); - this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; - } - - 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) { - - double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0; - SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction)); - - if (fieldRelative) { - - double rot = 0; - - // ! drift correction - if (rightStick.getNorm() > 0.05) { - rotTarget = gyro.getAngle(); - rot_correction = 0; - // rot = rightStick.getX(); - // SmartDashboard.putBoolean("drift correction", false); - stopped = false; - } else if(leftStick.getNorm() > 0.05) { - if (!stopped) { - stopModules(); - stopped = true; - } - - // SmartDashboard.putBoolean("drift correction", true); - - // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; - - } - - // Use the left joystick to set speed. Apply a cubic curve and the set max speed. - Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); - - // Convert field-relative speeds to robot-relative speeds. - // chassisSpeeds = chassisSpeeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); - } else { // Create robot-relative speeds. - chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); - } - setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); - } - - public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - if (fieldRelative) { - - double rot = 0; - - // ! drift correction - if (rightStick.getNorm() > 0.05) { - rotTarget = gyro.getAngle(); - rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; - // SmartDashboard.putBoolean("drift correction", false); - stopped = false; - } else if(leftStick.getNorm() > 0.05) { - if (!stopped) { - stopModules(); - stopped = true; - } - - // SmartDashboard.putBoolean("drift correction", true); - // double rot_correction = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; - - - } - - // Use the left joystick to set speed. Apply a cubic curve and the set max speed. - Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust); - // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); - - // Convert field-relative speeds to robot-relative speeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1)); - } else { // Create robot-relative speeds. - chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED); - } - // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); - } - - public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - - // Translation2d rightStick = new Translation2d(-rightX, rightY); - double rightX = rightStick.getX(); - double rightY = rightStick.getY(); - - double rot_correction = 0; - - // double rot_correction = ((rightStick.getAngle().getDegrees() - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; - - if(fieldRelative) { - double rot = 0; - if(rightStick.getNorm() > 0.5) { - orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1)); - - Rotation2d tmp = orientRotTarget.minus(gyro.getRotation2d().minus(new Rotation2d(Math.PI)).interpolate(orientRotTarget, 0.5)); - double min = tmp.getDegrees(); - min = Math.max(Math.abs(min), 2); - if(tmp.getDegrees() < 0) - min*=-1; - tmp = new Rotation2d(min * Math.PI / 180); - rot = tmp.getRadians(); // x x - y/x - } - - Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), ((-1 * rightStick.getX()) * SwerveDriveConstants.ROTATION_SPEED) + rot_correction, gyro.getRotation2d()).times(1); - } else { // Create robot-relative speeds. - chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); - } - // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); - } - - /** - * Set each module of the swerve drive to the corresponding desired state. - * @param desiredStates Array of module states to set. - */ - public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - for (int i = 0; i < desiredStates.length; i++) { - SwerveModule module = modules[i]; - SwerveModuleState state = desiredStates[i]; - module.setDesiredState(state); - } - } - - public boolean rotateToTarget(double angle) { - double currentAngle = getGyroAngle(); - double error = angle - currentAngle; - - driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true); - - if (Math.abs(angle - getGyroAngle()) < 5.0) { - return true; - } - - return false; - } - - public double getGyroAngle() { - return -gyro.getAngle(); - } - - public void add180() { - gyro.reset(gyro.getAngle()+180); - rotTarget = gyro.getAngle(); - - } - - public void resetGyro() { - gyro.reset(); - rotTarget = gyro.getAngle(); - } - - public void resetGyroFlip() { - gyro.resetFlip(); - rotTarget = gyro.getAngle(); - } - - public void resetGyroRightBlue() { - gyro.resetRightSideBlue(); - rotTarget = gyro.getAngle(); - } - - public void resetGyroRightAmp() { - gyro.resetAmpSide(); - rotTarget = gyro.getAngle(); - } - - public void stopModules() { - for (SwerveModule module : this.modules) { - module.stop(); - } - } - - public SwerveDriveKinematics getKinematics() { - return this.kinematics; - } - - public boolean getSpeedState() { - - return false; - } - - @Override - public void periodic() { - // This method will be called once per scheduler run\ - SmartDashboard.putNumber("Gyro", getGyroAngle()); - SmartDashboard.putNumber("RotTartget", rotTarget); - } - - private void reset_index() { - gear_index = 0; // 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 = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * speed; - gear_index = -1; - } - - public void setToSlow() { - this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.SLOW_SPEED; - System.out.println("SLOW"); - System.out.println("SLOW"); - System.out.println("SLOW"); - System.out.println("SLOW"); - System.out.println("SLOW"); - } - - public void setToFast() { - this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.FAST_SPEED; - System.out.println("FAST"); - System.out.println("FAST"); - System.out.println("FAST"); - System.out.println("FAST"); - System.out.println("FAST"); - } - - public void setToTurbo() { - this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.TURBO_SPEED; - System.out.println("TURBO"); - System.out.println("TURBO"); - System.out.println("TURBO"); - System.out.println("TURBO"); - System.out.println("TURBO"); - } - - public void toggleGear(double angle) { - if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) { - this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; - } else { - this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; - SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; - } - } - - public void shiftUpRot() { - rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED; - } - - public void shiftDownRot() { - rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; - } - - - -} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java deleted file mode 100644 index 02384ea..0000000 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ /dev/null @@ -1,242 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.subsystems; - -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.FeedbackConfigs; -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.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.ctre.phoenix6.hardware.CANcoder; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.utility.Gains; - -public class SwerveModule extends SubsystemBase { - private TalonFX driveMotor; - private TalonFX angleMotor; - private CANcoder encoder; - // private final StatusSignal cc_pos; - // private final StatusSignal cc_vel; - // private int selfid; - // private ConfigurableDouble offsetGetter; - private static int swerveId = 0; - public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; - - - /** Creates a new SwerveModule. */ - public SwerveModule(TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) { - this.driveMotor = driveMotor; - this.angleMotor = angleMotor; - this.encoder = encoder; - - var motorCfg = new TalonFXConfiguration() - .withOpenLoopRamps( - new OpenLoopRampsConfigs() - .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) - ).withClosedLoopRamps( - new ClosedLoopRampsConfigs() - .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) - ).withMotorOutput( - new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) - .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) - ).withCurrentLimits( - new CurrentLimitsConfigs() - .withStatorCurrentLimit(100) - .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(100) - .withSupplyCurrentLimitEnable(true) - ); - - driveMotor.getConfigurator().apply(motorCfg); - - TalonFXConfiguration angleConfig = new TalonFXConfiguration() - .withOpenLoopRamps( - new OpenLoopRampsConfigs() - .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) - ).withClosedLoopRamps( - new ClosedLoopRampsConfigs() - .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) - ).withMotorOutput( - new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) - .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) - ); - - angleConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - - angleConfig.Slot0.kP = swerveGains.kP; - angleConfig.Slot0.kI = swerveGains.kI; - angleConfig.Slot0.kD = swerveGains.kD; - - angleConfig.Feedback.FeedbackRemoteSensorID = encoder.getDeviceID(); - angleConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - angleMotor.getConfigurator().apply(angleConfig); - - CANcoderConfiguration canconfig = new CANcoderConfiguration(); - canconfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; - canconfig.MagnetSensor.MagnetOffset = offset; - encoder.getConfigurator().apply(canconfig); - - rotateToAngle(0); - } - - // public void go(double ang){ - // // double curang = this.encoder.getAbsolutePosition().getValue(); - // System.out.println(getAngle().getDegrees()); - // rotateToAngle(ang); - // } - - @Override - public void periodic() { - //encoder.configMagnetOffset(offsetGetter.get()); - //SmartDashboard.putString("Error Code: " + selfid, getstuff()); - // SmartDashboard.putNumber("Angular Position: " + selfid, getAngle().getDegrees()); - // SmartDashboard.putNumber("Angular Velocity: " + selfid, getAngularVel()); - // SmartDashboard.putNumber("Drive Position: " + selfid, getDrivePos()); - // SmartDashboard.putNumber("Drive Velocity: " + selfid, getDriveVel()); - } - /** - * Get the drive motor of the SwerveModule - * @return the drive motor of the SwerveModule - */ - public TalonFX getDriveMotor() { - return this.driveMotor; - } - - /** - * Get the angle motor of the SwerveModule - * @return the angle motor of the SwerveModule - */ - public TalonFX getAngleMotor() { - return this.angleMotor; - } - - /** - * Get the CANcoder of the SwerveModule - * @return the CANcoder of the SwerveModule - */ - public CANcoder getEncoder() { - return this.encoder; - } - - /** - * Get the angle of a SwerveModule as a Rotation2d - * @return the angle of a SwerveModule as a Rotation2d - */ - public Rotation2d getAngle() { - // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. - // return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); - return Rotation2d.fromRotations(encoder.getPosition().getValue().baseUnitMagnitude()); - } - - public double getAngularVel() { - // return this.angleMotor.getSelectedSensorVelocity(); - return angleMotor.getVelocity().getValueAsDouble(); - } - - public double getDrivePos() { - // return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV; - return driveMotor.getPosition().getValueAsDouble(); - } - - public double getDriveVel() { - // return this.driveMotor.getSelectedSensorVelocity(0); - return driveMotor.getVelocity().getValueAsDouble(); - } - - public void stop() { - driveMotor.set(0); - angleMotor.set(0); - } - - public void rotateToAngle(double angle) { - final PositionVoltage m_request = new PositionVoltage(angle); - angleMotor.setControl(m_request); - } - - /** - * Get state of swerve module - * @return speed in m/s and angle in degrees - */ - public SwerveModuleState getState() { - return new SwerveModuleState( - Units.inchesToMeters(driveMotor.getVelocity().getValue().baseUnitMagnitude() * - SwerveDriveConstants.Conversions.INCHES_PER_WHEEL_REV * - SwerveDriveConstants.Conversions.WHEEL_REV_PER_MOTOR_REV), - getAngle() - ); - } - - // private SwerveModuleState optimizeState(SwerveModuleState desiredState) { - // Rotation2d curRot = this.getAngle(); - - // } - - /** - * Returns the current position of the SwerveModule - * @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor. - // */ - // public SwerveModulePosition getPosition() { - // return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle()); - // } - - /** - * Set the speed and rotation of the SwerveModule from a SwerveModuleState object - * @param desiredState a SwerveModuleState representing the desired new state of the module - // */ - public void setDesiredState(SwerveModuleState desiredState) { - Rotation2d currentRotation = this.getAngle(); - - SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); - - // calculate the difference between our current rotational position and our new rotational position - Rotation2d rotationDelta = state.angle.minus(currentRotation); - - double speed = Units.metersToFeet(state.speedMetersPerSecond) / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND; - - rotateToAngle(rotationDelta.getRotations() + currentRotation.getRotations()); - - driveMotor.set(Math.max(Math.min(speed, 1.), -1.)); - } - - public void reset() { - // encoder.setPosition(0); - } - - // public double getCurrent() { - // return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent(); - // } - - // public double getVoltage() { - // return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); - // } - - // public String getstuff() { - // encoder.getPosition(); - // return "" + encoder.getLastError().value; - // } -} diff --git a/src/main/java/frc4388/robot/subsystems/TankDrive.java b/src/main/java/frc4388/robot/subsystems/TankDrive.java deleted file mode 100755 index 65efd06..0000000 --- a/src/main/java/frc4388/robot/subsystems/TankDrive.java +++ /dev/null @@ -1,115 +0,0 @@ -package frc4388.robot.subsystems; - -import java.util.function.BooleanSupplier; -import java.util.function.Function; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.controllers.PPLTVController; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.util.DriveFeedforwards; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; - -public class TankDrive extends SubsystemBase{ - private TalonSRX FR; - private TalonSRX FL; - private TalonSRX BL; - private TalonSRX BR; - private RobotLocalizer robotLocalizer; - - public TankDrive(TalonSRX FR,TalonSRX FL,TalonSRX BL, TalonSRX BR, RobotLocalizer robotLocalizer){ - this.FR = FR; - this.FL = FL; - this.BL = BL; - this.BR = BR; - this.robotLocalizer = robotLocalizer; - - - // Configure AutoBuilder last - AutoBuilder.configure( - robotLocalizer::getPose, - robotLocalizer::resetPose, - robotLocalizer::getChassisSpeeds, - this::setTargetChassisSpeeds, - AutoConstants.PP_PATH_FOLLOWING_CONTROLLER, - AutoConstants.PP_ROBOT_CONFIG, - () -> { - // 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 false; - }, - this - ); - } - - private static final ControlMode mode = ControlMode.PercentOutput; - - private static final double maxMoveSpeed = 0.3; - private static final double maxturnSpeed = 0.3; - - - private static final double[] turn = { //Right by default, motors are wired weirdly - maxturnSpeed, //FR - maxturnSpeed, //FL - maxturnSpeed, //BL - maxturnSpeed, //BR - }; - - private static final double[] move = { //forward by default, motors are wired weirdly - -maxMoveSpeed, //FR - maxMoveSpeed, //FL - maxMoveSpeed, //BL - -maxMoveSpeed, //BR - }; - - public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - - double percent_move = /* (1. - Math.abs(rightStick.getX())) */ leftStick.getY(); - - double FR_rot = move[0] * percent_move + turn[0] * rightStick.getX(); - double FL_rot = move[1] * percent_move + turn[1] * rightStick.getX(); - double BL_rot = move[2] * percent_move + turn[2] * rightStick.getX(); - double BR_rot = move[3] * percent_move + turn[3] * rightStick.getX(); - - FR.set(mode, FR_rot); - FL.set(mode, FL_rot); - BL.set(mode, BL_rot); - BR.set(mode, BR_rot); - - SmartDashboard.putNumber("FR", FR_rot); - SmartDashboard.putNumber("FL", FL_rot); - SmartDashboard.putNumber("BL", BL_rot); - SmartDashboard.putNumber("BR", BR_rot); - - } - - private double clamp(double x, double min, double max){ - return Math.max(Math.min(x, max), min); - } - - private void setTargetChassisSpeeds(ChassisSpeeds target, DriveFeedforwards idk) { - double move = clamp(target.vxMetersPerSecond/AutoConstants.MAX_DRIVE_VELOCITY, -1, 1); - double rot = clamp(Units.radiansToRotations(target.omegaRadiansPerSecond)/AutoConstants.MAX_ROT_SPEED, -1, 1); - - driveWithInput( - new Translation2d(0, move), - new Translation2d(rot, 0), - false); - - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java deleted file mode 100644 index 371f621..0000000 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ /dev/null @@ -1,38 +0,0 @@ -package frc4388.robot.subsystems; - -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; - -public class Vision { - private final NetworkTableEntry m_isTags; - private final NetworkTableEntry m_xPoses; - private final NetworkTableEntry m_yPoses; - private final NetworkTableEntry m_zPoses; - - public Vision() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - - m_isTags = tagTable.getEntry("IsTag"); - m_xPoses = tagTable.getEntry("TagPosX"); - m_yPoses = tagTable.getEntry("TagPosY"); - m_zPoses = tagTable.getEntry("TagPosZ"); - } - - public AprilTag[] getAprilTags() { - if (!m_isTags.getBoolean(false)) return new AprilTag[0]; - - double xarr[] = m_xPoses.getDoubleArray(new double[] {}); - double yarr[] = m_yPoses.getDoubleArray(new double[] {}); - double zarr[] = m_zPoses.getDoubleArray(new double[] {}); - - AprilTag tags[] = new AprilTag[xarr.length]; - for (int i = 0; i < tags.length; i++) { - tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d())); - } - - return tags; - } -} diff --git a/src/main/java/frc4388/robot/subsystems/differential/DiffConstants.java b/src/main/java/frc4388/robot/subsystems/differential/DiffConstants.java new file mode 100644 index 0000000..74fcf3e --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/differential/DiffConstants.java @@ -0,0 +1,29 @@ +package frc4388.robot.subsystems.differential; + +import static edu.wpi.first.units.Units.Inches; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Distance; +import frc4388.utility.status.CanDevice; +import frc4388.utility.structs.Gains; + +public class DiffConstants { + + public static final Distance WHEEL_RADIUS_TO_ARC = Inches.of(2.5).times(Math.PI * 2); //meters + public static final Distance TRACK_DISPLACEMENT = Inches.of(6.5); //meters + public static final Distance TRACK_WIDTH = TRACK_DISPLACEMENT.times(2); + + public static final Gains ROT_GAINS = new Gains(20, 0, 0); + + public static final class IDs { + public static final CanDevice FRONT_LEFT_MOTOR = new CanDevice("SRX_FRONT_LEFT", 4); + public static final CanDevice FRONT_RIGHT_MOTOR = new CanDevice("SRX_FRONT_RIGHT", 1); + public static final CanDevice REAR_LEFT_MOTOR = new CanDevice("SRX_REAR_LEFT", 2); + public static final CanDevice REAR_RIGHT_MOTOR = new CanDevice("SRX_REAR_RIGHT", 3); + + // public static final CanDevice FRONT_LEFT_MOTOR = new CanDevice("SRX_FRONT_LEFT", 3); + // public static final CanDevice FRONT_RIGHT_MOTOR = new CanDevice("SRX_FRONT_RIGHT", 2); + // public static final CanDevice REAR_LEFT_MOTOR = new CanDevice("SRX_REAR_LEFT", 1); + // public static final CanDevice REAR_RIGHT_MOTOR = new CanDevice("SRX_REAR_RIGHT", 4); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/differential/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/differential/DiffDrive.java new file mode 100644 index 0000000..76554dd --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/differential/DiffDrive.java @@ -0,0 +1,118 @@ +package frc4388.robot.subsystems.differential; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.hardware.Pigeon2; + +import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; +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.DifferentialDriveKinematics; +import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.utility.status.Queryable; +import frc4388.utility.status.Status; +import frc4388.utility.structs.Drivebase; + +public class DiffDrive extends SubsystemBase implements Drivebase, Queryable { + DiffIO io; + DiffStateAutoLogged state = new DiffStateAutoLogged(); + GyroIO gyroIO; + GyroStateAutoLogged gyroState = new GyroStateAutoLogged(); + + + DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(DiffConstants.TRACK_WIDTH); + DifferentialDrivePoseEstimator odometry = new DifferentialDrivePoseEstimator(kinematics, new Rotation2d(), 0, 0, new Pose2d()); + + public DiffDrive(DiffIO io, GyroIO gyroIO) { + this.io = io; + this.gyroIO = gyroIO; + } + + @Override + public void periodic() { + io.updateInputs(state); + Logger.processInputs(getName(), state); + gyroIO.updateInputs(gyroState); + Logger.processInputs("gyro", gyroState); + + var speeds = new DifferentialDriveWheelPositions( + DiffConstants.WHEEL_RADIUS_TO_ARC.times(state.leftOutputPosition), + DiffConstants.WHEEL_RADIUS_TO_ARC.times(state.rightOutputPosition) + ); + + odometry.update(gyroState.yaw, speeds); + } + + @Override + public void arcadeDrive(Translation2d left, Translation2d right) { + io.driveWithInput( + left.getY() + right.getX(), + left.getY() - right.getX() + ); + } + + private PID rotPid = new PID(DiffConstants.ROT_GAINS); + + @Override + public void driveFieldRelative(Translation2d left, Translation2d right) { + // In case the driver's stick is inside the deadband + if(right.getNorm() == 0) { + io.driveWithInput(0, 0); + return; + }; + + double targetAngle = right.getAngle().getRotations(); + double curAngle = gyroState.yaw.getRotations(); + + double error = targetAngle - curAngle - signedFloor(curAngle); + if(error > 0.5) { + error -= 1; + }else if(error < -0.5) { + error += 1; + } + + double move = 0; + double rot = rotPid.update(targetAngle - curAngle); + + io.driveWithInput( + move + rot, + move - rot + ); + } + + private double signedFloor(double x){ + if(x > 0) { + return x - Math.floor(x); + } else { + return Math.ceil(x) - x; + } + } + + @Override + public void resetOdometry() { + gyroIO.reset(); + odometry.resetPose(new Pose2d()); + } + + + + @AutoLogOutput + public Pose2d estimatedPose() { + return odometry.getEstimatedPosition(); + } + + + @Override + public String getName() { + return "Diff drive"; + } + + @Override + public Status diagnosticStatus() { + return new Status(); + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/differential/DiffIO.java b/src/main/java/frc4388/robot/subsystems/differential/DiffIO.java new file mode 100644 index 0000000..13e1fef --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/differential/DiffIO.java @@ -0,0 +1,20 @@ +package frc4388.robot.subsystems.differential; + +import org.littletonrobotics.junction.AutoLog; + +public interface DiffIO { + @AutoLog + public class DiffState { + public double leftOutputPosition = 0; + public double leftOutputVelocity = 0; + public double[] leftCurrent = new double[] {}; + + public double rightOutputPosition = 0; + public double rightOutputVelocity = 0; + public double[] rightCurrent = new double[] {}; + } + + public default void driveWithInput(double leftInput, double rightInput) {} + + public default void updateInputs(DiffState state) {} +} diff --git a/src/main/java/frc4388/robot/subsystems/differential/DiffTalonSRX.java b/src/main/java/frc4388/robot/subsystems/differential/DiffTalonSRX.java new file mode 100644 index 0000000..ffc412d --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/differential/DiffTalonSRX.java @@ -0,0 +1,52 @@ +package frc4388.robot.subsystems.differential; + +import org.littletonrobotics.junction.AutoLogOutput; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; + +public class DiffTalonSRX implements DiffIO { + TalonSRX m_leftFront; + TalonSRX m_rightFront; + TalonSRX m_leftRear; + TalonSRX m_rightRear; + + public DiffTalonSRX(TalonSRX m_leftFront, TalonSRX m_rightFront, TalonSRX m_leftRear, TalonSRX m_rightRear) { + this.m_leftFront = m_leftFront; + this.m_rightFront = m_rightFront; + this.m_leftRear = m_leftRear; + this.m_rightRear = m_rightRear; + + + m_leftRear.follow(m_leftFront); + m_rightRear.follow(m_rightFront); + + + this.m_rightFront.setInverted(true); + this.m_rightRear.setInverted(true); + } + + @Override + public void updateInputs(DiffState state) { + state.leftCurrent = new double[] {m_leftFront.getStatorCurrent(), m_leftRear.getStatorCurrent()}; + state.leftOutputPosition = (m_leftFront.getSelectedSensorPosition() + m_leftRear.getSelectedSensorPosition()) / 2 / 1100; + state.leftOutputVelocity = (m_leftFront.getSelectedSensorVelocity() + m_leftRear.getSelectedSensorVelocity()) / 2 / 1100; + + state.rightCurrent = new double[] {m_rightFront.getStatorCurrent(), m_rightRear.getStatorCurrent()}; + state.rightOutputPosition = (m_rightFront.getSelectedSensorPosition() + m_rightFront.getSelectedSensorPosition()) / 2 / 1100; + state.rightOutputVelocity = (m_rightFront.getSelectedSensorVelocity() + m_rightFront.getSelectedSensorVelocity()) / 2 / 1100; + } + + @Override + public void driveWithInput(double leftInput, double rightInput) { + m_leftFront.set(TalonSRXControlMode.PercentOutput, leftInput); + m_rightFront.set(TalonSRXControlMode.PercentOutput, rightInput); + } + + @AutoLogOutput + public double test() { + return m_leftFront.getSelectedSensorPosition(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/differential/GyroIO.java b/src/main/java/frc4388/robot/subsystems/differential/GyroIO.java new file mode 100644 index 0000000..a1ec1cd --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/differential/GyroIO.java @@ -0,0 +1,19 @@ +package frc4388.robot.subsystems.differential; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Voltage; + +public interface GyroIO { + @AutoLog + public class GyroState { + public boolean connected = false; + public Voltage voltage; + public Rotation2d yaw; + } + + public default void updateInputs(GyroState inputs) {} + + public default void reset() {} +} diff --git a/src/main/java/frc4388/robot/subsystems/differential/GyroPigeon2.java b/src/main/java/frc4388/robot/subsystems/differential/GyroPigeon2.java new file mode 100644 index 0000000..faf4a0b --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/differential/GyroPigeon2.java @@ -0,0 +1,36 @@ +package frc4388.robot.subsystems.differential; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.Pigeon2; + +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.Alert.AlertType; +import frc4388.utility.status.Alerts; + +public class GyroPigeon2 implements GyroIO { + Pigeon2 m_gyro; + + StatusSignal voltage; + + public GyroPigeon2(Pigeon2 m_gyro){ + this.m_gyro = m_gyro; + + voltage = m_gyro.getSupplyVoltage(); + } + + @Override + public void updateInputs(GyroState state) { + voltage.refresh(); + + state.connected = m_gyro.isConnected(); + state.voltage = voltage.getValue(); + state.yaw = m_gyro.getRotation2d(); + + Alerts.validate(!state.connected, "Gyro is not connected!", AlertType.kError); + } + + @Override + public void reset() { + m_gyro.reset(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/differential/PID.java b/src/main/java/frc4388/robot/subsystems/differential/PID.java new file mode 100644 index 0000000..facd2dd --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/differential/PID.java @@ -0,0 +1,29 @@ +package frc4388.robot.subsystems.differential; + +import frc4388.utility.structs.Gains; + +public class PID { + protected Gains gains; + private double output = 0; + + + /** Creates a new PelvicInflammatoryDisease. */ + public PID(Gains gains) { + this.gains = gains; + } + + 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; + } +} \ 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..dbbb024 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java @@ -0,0 +1,55 @@ +package frc4388.robot.subsystems.lidar; + +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); + } + + // @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/LidarLiteV2.java b/src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.java new file mode 100644 index 0000000..3851050 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.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 LidarLiteV2 implements LidarIO { + + + private Counter LidarPWM; + + public LidarLiteV2(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..40a91be --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -0,0 +1,484 @@ +// 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 java.util.Optional; + +import org.littletonrobotics.junction.Logger; + +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 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.structs.Drivebase; +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, Drivebase { + private SwerveDrivetrain swerveDriveTrain; + private Vision vision; + + // @AutoLog + // public class SwerveDriveState { + 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 = null; + + + public double rotTarget = 0.0; + public Rotation2d orientRotTarget = new Rotation2d(); + public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + // } + + // public SwerveDriveState state = new SwerveDriveState(); + + /** Creates a new SwerveDrive. */ + public SwerveDrive(SwerveDrivetrain swerveDriveTrain, Vision vision) { + // public SwerveDrive(SwerveDrivetrain + // swerveDriveTrain) { + FaultReporter.register(this); + + this.swerveDriveTrain = swerveDriveTrain; + 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 swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d); + }, // Robot pose supplier + this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting + // pose) + () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speeds, feedforwards) -> swerveDriveTrain.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; + swerveDriveTrain.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 = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees(); + swerveDriveTrain.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 + ); + swerveDriveTrain.setControl(ctrl); + SmartDashboard.putBoolean("drift correction", true); + } + + + } else { // Create robot-relative speeds. + swerveDriveTrain.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); + swerveDriveTrain.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); + + swerveDriveTrain.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 + ); + swerveDriveTrain.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 + // ); + swerveDriveTrain.setControl(ctrl); + } + + 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); + } + } + + public void activateLuigiMode() { + setLimits(20); + } + + public void deactivateLuigiMode() { + setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT); + } + + public boolean rotateToTarget(double angle) { + swerveDriveTrain.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); + + swerveDriveTrain.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() { + return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d); + } + + @Override + public void resetOdometry() { + swerveDriveTrain.tareEverything(); + robotKnowsWhereItIs = false; + rotTarget = 0; + // vision.resetRotations(); + } + + + public void softStop() { + stopped = true; + swerveDriveTrain.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); + + double time = Vision.getTime(); + double freq = swerveDriveTrain.getOdometryFrequency(); + + Optional curpose = swerveDriveTrain.samplePoseAt(time); + Optional lastPose = swerveDriveTrain.samplePoseAt(time - freq); + + vision.setLastOdomPose(curpose); + setLastOdomSpeed(curpose, lastPose, freq); + + if (vision.isTag()) { + Pose2d pose = vision.getPose2d(); + if (!robotKnowsWhereItIs) { + robotKnowsWhereItIs = true; + Pose2d curPose = getPose2d(); + rotTarget += pose.getRotation().getDegrees() - curPose.getRotation().getDegrees(); + } + + vision.addVisionMeasurement(swerveDriveTrain); + } + + // 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(Optional curPose, Optional lastPose, double freq){ + if(curPose.isPresent() && lastPose.isPresent()){ + lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; + } + } + + + + @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; + } + + + // Update CTRE simulation, if used. + public void updateSim(double voltage) { + swerveDriveTrain.updateSimState(0.02, voltage); + } +} + 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/vision/Vision.java b/src/main/java/frc4388/robot/subsystems/vision/Vision.java new file mode 100644 index 0000000..d24ee3f --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/vision/Vision.java @@ -0,0 +1,94 @@ +package frc4388.robot.subsystems.vision; + +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 void addVisionMeasurement(SwerveDrivetrain drivetrain){ + // for(EstimatedRobotPose pose : poses){ + // + // } + for(int i = 0; i < state.length; i++) { + if(state[i].lastEstimatedPose != null) { + PoseObservation pose = state[i].lastEstimatedPose; + drivetrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp())); + } + } + } + + public void setLastOdomPose(Optional pose){ + if(pose.isPresent()) + lastPhysOdomPose = pose.get(); + } + + 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/VisionPhotonvision.java b/src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.java new file mode 100644 index 0000000..888dc12 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.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 VisionPhotonvision implements VisionIO { + // private PhotonCamera[] cameras; + // private PhotonPoseEstimator[] estimators; + + private PhotonCamera camera; + private PhotonPoseEstimator estimator; + + // public List poses = new ArrayList<>(); + + + public VisionPhotonvision(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/AprilTag.java b/src/main/java/frc4388/utility/AprilTag.java deleted file mode 100644 index c2ad269..0000000 --- a/src/main/java/frc4388/utility/AprilTag.java +++ /dev/null @@ -1,13 +0,0 @@ -package frc4388.utility; - -// This is a seperate class in case I want to encode rotation or other -// information about the tag -public class AprilTag { - public final double x, y, z; - - public AprilTag(double _x, double _y, double _z) { - x = _x; - y = _y; - z = _z; - } -} diff --git a/src/main/java/frc4388/utility/DeferredBlock.java b/src/main/java/frc4388/utility/DeferredBlock.java index 20d3c57..c5a558e 100644 --- a/src/main/java/frc4388/utility/DeferredBlock.java +++ b/src/main/java/frc4388/utility/DeferredBlock.java @@ -2,22 +2,40 @@ 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 = new ArrayList<>(); + private static ArrayList m_blocks_norerun = new ArrayList<>(); + private static ArrayList m_blocks_rerun = new ArrayList<>(); private static boolean m_hasRun = false; - public DeferredBlock(Runnable block) { - m_blocks.add(block); + 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() { - if (m_hasRun) return; - for (Runnable block : m_blocks) { + // Run blocks that run multiple times. + for (Runnable block : m_blocks_rerun) { block.run(); } - m_blocks.clear(); // for garbage collection + // 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/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java deleted file mode 100644 index d0d65fd..0000000 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ /dev/null @@ -1,294 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* 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 com.ctre.phoenix.sensors.WPI_Pigeon2; -import com.ctre.phoenix6.hardware.Pigeon2; -// import com.kauailabs.navx.frc.AHRS; - -// import edu.wpi.first.wpilibj.GyroBase; -// import edu.wpi.first.wpilibj.interfaces.Gyro; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; - -/** - * Gyro class that allows for interchangeable use between a pigeon and a navX - */ -public class RobotGyro { - private RobotTime m_robotTime = RobotTime.getInstance(); - - private Pigeon2 m_pigeon = null; - // private AHRS m_navX = null; - public boolean m_isGyroAPigeon; //true if pigeon, false if navX - - private double m_lastPigeonAngle; - private double m_deltaPigeonAngle; - - private double pitchZero = 0; - private double rollZero = 0; - - /** - * Creates a Gyro based on a pigeon - * @param gyro the gyroscope to use for Gyro - */ - public RobotGyro(Pigeon2 gyro) { - m_pigeon = gyro; - m_isGyroAPigeon = true; - } - - /** - * Creates a Gyro based on a navX - * @param gyro the gyroscope to use for Gyro - */ - // public RobotGyro(AHRS gyro){ - // m_navX = gyro; - // m_isGyroAPigeon = false; - // } - - /** - * Resets yaw, pitch, and roll. - */ - public void resetZeroValues() { - if (!m_isGyroAPigeon) return; - - // pitchZero = m_pigeon.getPitch(); - // rollZero = m_pigeon.getRoll(); - } - - /** - * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note - * that the getRate() method for a navX will likely be much more accurate than for a pigeon. - */ - public void updatePigeonDeltas() { - double currentPigeonAngle = getAngle(); - m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle; - m_lastPigeonAngle = currentPigeonAngle; - } - - /** - *

NavX: - *

Calibrate the gyro by running for a number of samples and computing the center value. Then use - * the center value as the Accumulator center value for subsequent measurements. It's important to - * make sure that the robot is not moving while the centering calculations are in progress, this - * is typically done when the robot is first turned on while it's sitting at rest before the - * competition starts. - * - *

Pigeon: - *

Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot - * into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon - * has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to - * make sure that the robot is not moving while the tempurature calculations are in progress, this - * is typically done when the robot is first turned on while it's sitting at rest before the - * competition starts. - */ - public void calibrate() { - return; - // if (m_isGyroAPigeon) { - // m_pigeon.calibrate(); - // } else { - // m_navX.calibrate(); - // } - } - - public void reset() { - resetZeroValues(); - - // if (m_isGyroAPigeon) { - m_pigeon.setYaw(0); - // } else { - // // m_navX.reset(); - // } - - } - - public void reset(double val) { - resetZeroValues(); - - // if (m_isGyroAPigeon) { - m_pigeon.setYaw(val); - // } else { - // // m_navX.reset(); - // } - - } - - public void resetFlip() { - resetZeroValues(); - - // if (m_isGyroAPigeon) { - m_pigeon.setYaw(180); - // } else { - // m_navX.reset(); - // } - - } - - public void resetNinety() { - resetZeroValues(); - - if (m_isGyroAPigeon) { - m_pigeon.setYaw(90); - } else { - // m_navX.reset(); - } - - } - - public void resetTwoSeventy() { - resetZeroValues(); - - if (m_isGyroAPigeon) { - m_pigeon.setYaw(270); - } else { - // m_navX.reset(); - } - - } - - public void resetRightSideBlue() { - resetZeroValues(); - - if (m_isGyroAPigeon) { - m_pigeon.setYaw(60); - } else { - // m_navX.reset(); - } - - } - - public void resetAmpSide() { - resetZeroValues(); - - if (m_isGyroAPigeon) { - m_pigeon.setYaw(-60); - } else { - // m_navX.reset(); - } - - } - - /** - * Get Yaw, Pitch, and Roll data. - * - * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data. - * Yaw is within [-368,640, +368,640] degrees. - * Pitch is within [-90,+90] degrees. - * Roll is within [-90,+90] degrees. - */ - private double[] getPigeonAngles() { - m_pigeon.getAngle(); - var rotation = m_pigeon.getRotation3d(); - - return new double[] {RobotUnits.radiansToDegrees(rotation.getX() - rollZero), RobotUnits.radiansToDegrees(rotation.getY() - pitchZero), RobotUnits.radiansToDegrees(rotation.getZ())}; - } - - public Rotation2d getRotation2d() { - return m_pigeon.getRotation2d(); - } - - public Rotation3d getRotation3d() { - return m_pigeon.getRotation3d(); - } - - public Translation2d getAcceleration2d() { - return new Translation2d( - m_pigeon.getAccelerationX().getValue().baseUnitMagnitude(), - m_pigeon.getAccelerationY().getValue().baseUnitMagnitude()); - } - - - public Translation3d getAcceleration3d() { - return new Translation3d( - m_pigeon.getAccelerationX().getValue().baseUnitMagnitude(), - m_pigeon.getAccelerationY().getValue().baseUnitMagnitude(), - m_pigeon.getAccelerationZ().getValue().baseUnitMagnitude()); - } - - public double getAngularVelocity() { - return m_pigeon.getAngularVelocityYDevice().getValueAsDouble(); - } - - public double getAngle() { - // if (m_isGyroAPigeon) { - return getPigeonAngles()[2]; - // } else { - // return m_navX.getAngle(); - // } - } - - public double getYaw() { - return this.getAngle(); - } - - /** - * Gets an absolute heading of the robot - * @return heading from -180 to 180 degrees - */ - public double getHeading() { - return getHeading(getAngle()); - } - - /** - * Gets an absolute heading of the robot - * @return heading from -180 to 180 degrees - */ - public double getHeading(double angle) { - return Math.IEEEremainder(angle, 360); - } - - /** - * Returns the current pitch value (in degrees, from -90 to 90) - * reported by the sensor. Pitch is a measure of rotation around - * the Y Axis. - * @return The current pitch value in degrees (-90 to 90). - */ - public double getPitch() { - // if (m_isGyroAPigeon) { - return MathUtil.clamp(getPigeonAngles()[1], -90, 90); - // } else { - // return MathUtil.clamp(m_navX.getPitch(), -90, 90); - // } - } - - /** - * Returns the current roll value (in degrees, from -90 to 90) - * reported by the sensor. Roll is a measure of rotation around - * the X Axis. - * @return The current roll value in degrees (-90 to 90). - */ - public double getRoll() { - // if (m_isGyroAPigeon) { - return MathUtil.clamp(getPigeonAngles()[2], -90, 90); - // } else { - // return MathUtil.clamp(m_navX.getRoll(), -90, 90); - // } - } - - public double getRate() { - // if (m_isGyroAPigeon) { - return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000; - // } else { - // // return m_navX.getRate(); - // } - } - - public Pigeon2 getPigeon(){ - return m_pigeon; - } - - // public AHRS getNavX(){ - // return m_navX; - // } - - public void close() throws Exception { - - } -} diff --git a/src/main/java/frc4388/utility/DataUtils.java b/src/main/java/frc4388/utility/compute/DataUtils.java similarity index 96% rename from src/main/java/frc4388/utility/DataUtils.java rename to src/main/java/frc4388/utility/compute/DataUtils.java index 3d998b6..3b83190 100644 --- a/src/main/java/frc4388/utility/DataUtils.java +++ b/src/main/java/frc4388/utility/compute/DataUtils.java @@ -1,4 +1,4 @@ -package frc4388.utility; +package frc4388.utility.compute; import java.nio.ByteBuffer; 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/RobotTime.java b/src/main/java/frc4388/utility/compute/RobotTime.java similarity index 98% rename from src/main/java/frc4388/utility/RobotTime.java rename to src/main/java/frc4388/utility/compute/RobotTime.java index 694f850..ebb43d8 100644 --- a/src/main/java/frc4388/utility/RobotTime.java +++ b/src/main/java/frc4388/utility/compute/RobotTime.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.utility; +package frc4388.utility.compute; /** *

Keeps track of Robot times like time passed, delta time, etc diff --git a/src/main/java/frc4388/utility/RobotUnits.java b/src/main/java/frc4388/utility/compute/RobotUnits.java similarity index 96% rename from src/main/java/frc4388/utility/RobotUnits.java rename to src/main/java/frc4388/utility/compute/RobotUnits.java index 9e07312..0f76d06 100644 --- a/src/main/java/frc4388/utility/RobotUnits.java +++ b/src/main/java/frc4388/utility/compute/RobotUnits.java @@ -2,7 +2,7 @@ // 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; +package frc4388.utility.compute; /** Aarav's good units class (better than WPILib) * @author Aarav Shah */ 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/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 index 4577a2e..ae4202b 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -1,6 +1,7 @@ + package frc4388.utility.controller; -import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; +import static frc4388.robot.constants.Constants.OIConstants.LEFT_AXIS_DEADBAND; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; 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..5271a78 --- /dev/null +++ b/src/main/java/frc4388/utility/status/Alerts.java @@ -0,0 +1,25 @@ +package frc4388.utility.status; + +import java.util.HashMap; + +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); + + private static HashMap validations = new HashMap<>(); + + public static void validate(boolean isTrue, String message, AlertType type) { + Alert currentAlert = validations.get(message); + + if(currentAlert == null) { + currentAlert = new Alert(message, type); + validations.put(message, currentAlert); + } + + currentAlert.set(isTrue); + } +} 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/Drivebase.java b/src/main/java/frc4388/utility/structs/Drivebase.java new file mode 100644 index 0000000..56699ce --- /dev/null +++ b/src/main/java/frc4388/utility/structs/Drivebase.java @@ -0,0 +1,20 @@ +package frc4388.utility.structs; + +import edu.wpi.first.math.geometry.Translation2d; + +// Abstract +public interface Drivebase { + + // Swerve drive - Drive relative to field. + public default void driveFieldRelative(Translation2d left, Translation2d right) {} + + // Swerve drive - Drive relative to robot; + public default void driveRobotOriented(Translation2d left, Translation2d right) {} + + // Diff drive - Arcade drive + // Left stick forward and back is forward and back movement + // Right stick left and right is turning + public default void arcadeDrive(Translation2d left, Translation2d right) {} + + public void resetOdometry(); +} diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/structs/Gains.java similarity index 98% rename from src/main/java/frc4388/utility/Gains.java rename to src/main/java/frc4388/utility/structs/Gains.java index 7a3a026..0d5b674 100644 --- a/src/main/java/frc4388/utility/Gains.java +++ b/src/main/java/frc4388/utility/structs/Gains.java @@ -2,7 +2,7 @@ // 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; +package frc4388.utility.structs; /** Add your docs here. */ public class Gains { diff --git a/src/main/java/frc4388/utility/LEDPatterns.java b/src/main/java/frc4388/utility/structs/LEDPatterns.java similarity index 98% rename from src/main/java/frc4388/utility/LEDPatterns.java rename to src/main/java/frc4388/utility/structs/LEDPatterns.java index 6df032c..585dc43 100644 --- a/src/main/java/frc4388/utility/LEDPatterns.java +++ b/src/main/java/frc4388/utility/structs/LEDPatterns.java @@ -1,4 +1,4 @@ -package frc4388.utility; +package frc4388.utility.structs; /** * Add your docs here. diff --git a/src/main/java/frc4388/utility/UtilityStructs.java b/src/main/java/frc4388/utility/structs/UtilityStructs.java similarity index 95% rename from src/main/java/frc4388/utility/UtilityStructs.java rename to src/main/java/frc4388/utility/structs/UtilityStructs.java index 935dbbe..2307f2f 100644 --- a/src/main/java/frc4388/utility/UtilityStructs.java +++ b/src/main/java/frc4388/utility/structs/UtilityStructs.java @@ -1,4 +1,4 @@ -package frc4388.utility; +package frc4388.utility.structs; public class UtilityStructs { public static class TimedOutput { diff --git a/vendordeps/2024/PathplannerLib2024.json b/vendordeps/2024/PathplannerLib2024.json deleted file mode 100644 index f112e62..0000000 --- a/vendordeps/2024/PathplannerLib2024.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "fileName": "PathplannerLib2024.json", - "name": "PathplannerLib", - "version": "2024.2.8", - "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2024", - "mavenUrls": [ - "https://3015rangerrobotics.github.io/pathplannerlib/repo" - ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib2024.json", - "javaDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-java", - "version": "2024.2.8" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-cpp", - "version": "2024.2.8", - "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/2024/Phoenix.json b/vendordeps/2024/Phoenix.json deleted file mode 100644 index ff7359e..0000000 --- a/vendordeps/2024/Phoenix.json +++ /dev/null @@ -1,151 +0,0 @@ -{ - "fileName": "Phoenix5.json", - "name": "CTRE-Phoenix (v5)", - "version": "5.33.1", - "frcYear": 2024, - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", - "requires": [ - { - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", - "offlineFileName": "Phoenix6.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.33.1" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.33.1" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.33.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.33.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-cpp", - "version": "5.33.1", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.33.1", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.33.1", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "5.33.1", - "libName": "CTRE_Phoenix_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "api-cpp-sim", - "version": "5.33.1", - "libName": "CTRE_PhoenixSim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.33.1", - "libName": "CTRE_PhoenixCCISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} \ No newline at end of file diff --git a/vendordeps/2024/Phoenix6.json b/vendordeps/2024/Phoenix6.json deleted file mode 100644 index 0322385..0000000 --- a/vendordeps/2024/Phoenix6.json +++ /dev/null @@ -1,339 +0,0 @@ -{ - "fileName": "Phoenix6.json", - "name": "CTRE-Phoenix (v6)", - "version": "24.3.0", - "frcYear": 2024, - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", - "conflictsWith": [ - { - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", - "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", - "offlineFileName": "Phoenix6And5.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-java", - "version": "24.3.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "24.3.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.3.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.3.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.3.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "24.3.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "24.3.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "24.3.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "24.3.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "24.3.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "24.3.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-cpp", - "version": "24.3.0", - "libName": "CTRE_Phoenix6_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "24.3.0", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "24.3.0", - "libName": "CTRE_Phoenix6_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.3.0", - "libName": "CTRE_PhoenixTools_Sim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.3.0", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.3.0", - "libName": "CTRE_SimTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "24.3.0", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "24.3.0", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "24.3.0", - "libName": "CTRE_SimCANCoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "24.3.0", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "24.3.0", - "libName": "CTRE_SimProCANcoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "24.3.0", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} \ No newline at end of file diff --git a/vendordeps/2024/WPILibNewCommands.json b/vendordeps/2024/WPILibNewCommands.json deleted file mode 100644 index 67bf389..0000000 --- a/vendordeps/2024/WPILibNewCommands.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "fileName": "WPILibNewCommands.json", - "name": "WPILib-New-Commands", - "version": "1.0.0", - "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "frcYear": "2024", - "mavenUrls": [], - "jsonUrl": "", - "javaDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-java", - "version": "wpilib" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-cpp", - "version": "wpilib", - "libName": "wpilibNewCommands", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxarm32", - "linuxarm64", - "windowsx86-64", - "windowsx86", - "linuxx86-64", - "osxuniversal" - ] - } - ] -} diff --git a/vendordeps/2024/navx_frc.json b/vendordeps/2024/navx_frc.json deleted file mode 100644 index dca1d82..0000000 --- a/vendordeps/2024/navx_frc.json +++ /dev/null @@ -1,35 +0,0 @@ -{ - "fileName": "navx_frc.json", - "name": "KauaiLabs_navX_FRC", - "version": "3.1.413", - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "mavenUrls": [ - "https://repo1.maven.org/maven2/" - ], - "jsonUrl": "https://www.kauailabs.com/dist/frc/2020/navx_frc.json", - "javaDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-java", - "version": "3.1.413" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-cpp", - "version": "3.1.413", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": false, - "libName": "navx_frc", - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxraspbian", - "windowsx86-64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/2024/photonlib-v2025.0.0-beta-6.json b/vendordeps/2024/photonlib-v2025.0.0-beta-6.json deleted file mode 100644 index c0a6513..0000000 --- a/vendordeps/2024/photonlib-v2025.0.0-beta-6.json +++ /dev/null @@ -1,71 +0,0 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2025.0.0-beta-6", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2024", - "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" - ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", - "jniDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-6", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-cpp", - "version": "v2025.0.0-beta-6", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-6", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2025.0.0-beta-6" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2025.0.0-beta-6" - } - ] -} 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/2024/NavX.json b/vendordeps/NavX.json similarity index 99% rename from vendordeps/2024/NavX.json rename to vendordeps/NavX.json index e01bab1..ecf8fa0 100644 --- a/vendordeps/2024/NavX.json +++ b/vendordeps/NavX.json @@ -37,4 +37,4 @@ ] } ] -} \ No newline at end of file +} diff --git a/vendordeps/PathplannerLib2024.json b/vendordeps/PathplannerLib-2025.2.7.json similarity index 83% rename from vendordeps/PathplannerLib2024.json rename to vendordeps/PathplannerLib-2025.2.7.json index e79fe1a..d3f84e5 100644 --- a/vendordeps/PathplannerLib2024.json +++ b/vendordeps/PathplannerLib-2025.2.7.json @@ -1,18 +1,18 @@ { - "fileName": "PathplannerLib-beta.json", + "fileName": "PathplannerLib-2025.2.7.json", "name": "PathplannerLib", - "version": "2025.0.0-beta-5", + "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-beta.json", + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", "javaDependencies": [ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.0.0-beta-5" + "version": "2025.2.7" } ], "jniDependencies": [], @@ -20,15 +20,15 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.0.0-beta-5", + "version": "2025.2.7", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "osxuniversal", "linuxx86-64", + "osxuniversal", "linuxathena", "linuxarm32", "linuxarm64" diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix5-5.35.1.json similarity index 83% rename from vendordeps/Phoenix.json rename to vendordeps/Phoenix5-5.35.1.json index ee205af..69df8b5 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix5-5.35.1.json @@ -1,341 +1,171 @@ { - - "fileName": "Phoenix5-frc2025-beta-latest.json", - + "fileName": "Phoenix5-5.35.1.json", "name": "CTRE-Phoenix (v5)", - - "version": "5.34.0-beta-4", - + "version": "5.35.1", "frcYear": "2025", - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-beta-latest.json", - + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json", "requires": [ - { - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", - - "offlineFileName": "Phoenix6-frc2025-beta-latest.json", - - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json" - + "offlineFileName": "Phoenix6-frc2025-latest.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json" } - ], - "conflictsWith": [ - { - "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", - "errorMessage": "Users must use the Phoenix 5 replay vendordep when using the Phoenix 6 replay vendordep.", - - "offlineFileName": "Phoenix6-replay-frc2025-beta-latest.json" - + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" }, - { - "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df", - "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.", - - "offlineFileName": "Phoenix5-replay-frc2025-beta-latest.json" - + "offlineFileName": "Phoenix5-replay-frc2025-latest.json" } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - - "version": "5.34.0-beta-4" - + "version": "5.35.1" }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - - "version": "5.34.0-beta-4" - + "version": "5.35.1" } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - - "version": "5.34.0-beta-4", - + "version": "5.35.1", "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - - "version": "5.34.0-beta-4", - + "version": "5.35.1", "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-cpp", - - "version": "5.34.0-beta-4", - + "version": "5.35.1", "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - - "version": "5.34.0-beta-4", - + "version": "5.35.1", "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - - "version": "5.34.0-beta-4", - + "version": "5.35.1", "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "wpiapi-cpp-sim", - - "version": "5.34.0-beta-4", - + "version": "5.35.1", "libName": "CTRE_Phoenix_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "api-cpp-sim", - - "version": "5.34.0-beta-4", - + "version": "5.35.1", "libName": "CTRE_PhoenixSim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - - "version": "5.34.0-beta-4", - + "version": "5.35.1", "libName": "CTRE_PhoenixCCISim", - "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/Phoenix6.json b/vendordeps/Phoenix6-frc2025-latest.json similarity index 74% rename from vendordeps/Phoenix6.json rename to vendordeps/Phoenix6-frc2025-latest.json index 0b786d1..6f40c84 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,32 +1,32 @@ { - "fileName": "Phoenix6-frc2025-beta-latest.json", + "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.0.0-beta-4", + "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-beta-latest.json", + "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-beta-latest.json" + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.0.0-beta-4" + "version": "25.4.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.0.0-beta-4", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.0.0-beta-4", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.0.0-beta-4", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.0.0-beta-4", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.0.0-beta-4", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.0.0-beta-4", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-4", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.0.0-beta-4", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.0.0-beta-4", + "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": [ @@ -152,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.0.0-beta-4", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.0.0-beta-4", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +194,35 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.0.0-beta-4", + "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": [ @@ -196,7 +238,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.0.0-beta-4", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -212,7 +254,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.0.0-beta-4", + "version": "25.4.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -228,7 +270,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.0.0-beta-4", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -244,7 +286,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.0.0-beta-4", + "version": "25.4.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -260,7 +302,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.0.0-beta-4", + "version": "25.4.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -276,7 +318,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.0.0-beta-4", + "version": "25.4.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -292,7 +334,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-4", + "version": "25.4.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +350,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.0.0-beta-4", + "version": "25.4.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -324,7 +366,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.0.0-beta-4", + "version": "25.4.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -337,10 +379,26 @@ ], "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.0.0-beta-4", + "version": "25.4.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -356,7 +414,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.0.0-beta-4", + "version": "25.4.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -372,7 +430,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.0.0-beta-4", + "version": "25.4.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,6 +442,38 @@ "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/photonlib-v2025.0.0-beta-6.json b/vendordeps/photonlib-v2025.0.0-beta-6.json deleted file mode 100644 index 6a23ba8..0000000 --- a/vendordeps/photonlib-v2025.0.0-beta-6.json +++ /dev/null @@ -1,71 +0,0 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2025.0.0-beta-6", - "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.0.0-beta-6", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-cpp", - "version": "v2025.0.0-beta-6", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-6", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2025.0.0-beta-6" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2025.0.0-beta-6" - } - ] -} 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