diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index f523e9c..e05703c 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2024", + "projectYear": "2026", "teamNumber": 4388 -} \ No newline at end of file +} diff --git a/autos/final_1note_stationary.auto b/autos/final_1note_stationary.auto deleted file mode 100644 index f83851c..0000000 Binary files a/autos/final_1note_stationary.auto and /dev/null differ diff --git a/autos/final_red_center_4note_taxi.auto b/autos/final_red_center_4note_taxi.auto deleted file mode 100644 index b20ab84..0000000 Binary files a/autos/final_red_center_4note_taxi.auto and /dev/null differ diff --git a/build.gradle b/build.gradle index 9638f90..489a591 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,7 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.2" + id "edu.wpi.first.GradleRIO" version "2026.2.1" + id "com.peterabeles.gversion" version "1.10" } java { @@ -33,6 +34,8 @@ deploy { frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' + deleteOldFiles = false // Change to true to delete files on roboRIO that no + // longer exist in deploy directory of this project } } } @@ -41,15 +44,17 @@ deploy { def deployArtifact = deploy.targets.roborio.artifacts.frcJava -// Set to true to use debug for JNI. +// Set to true to use debug for all targets including JNI, which will drastically impact +// performance. wpi.java.debugJni = false // Set this to true to enable desktop support. -def includeDesktopSupport = false +def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. dependencies { + annotationProcessor wpi.java.deps.wpilibAnnotations() implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() @@ -67,14 +72,20 @@ dependencies { nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() - //testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' - //testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + + implementation("com.fazecast:jSerialComm:2.4.1") + + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" } -// test { -// useJUnitPlatform() -// systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' -//} +test { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' +} // Simulation configuration (e.g. environment variables). wpi.sim.addGui().defaultEnabled = true @@ -85,7 +96,9 @@ wpi.sim.addDriverstation() // knows where to look for our Robot Class. jar { from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } - from sourceSets.main.allSource + from('src') { into 'backup/src' } + from('vendordeps') { into 'backup/vendordeps' } + from('build.gradle') { into 'backup' } manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE } @@ -98,4 +111,20 @@ wpi.java.configureTestTasks(test) // Configure string concat to always inline compile tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' +} + +task(replayWatch, type: JavaExec) { + mainClass = "org.littletonrobotics.junction.ReplayWatch" + classpath = sourceSets.main.runtimeClasspath +} + +// Create version file +project.compileJava.dependsOn(createVersionFile) +gversion { + srcDir = "src/main/java/" + classPackage = "frc4388.robot.constants" + className = "BuildConstants" + dateFormat = "yyyy-MM-dd HH:mm:ss z" + timeZone = "America/Denver" + indent = " " } \ No newline at end of file diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/settings.gradle b/settings.gradle index d94f73c..25f6f6e 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2024' + String frcYear = '2026' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') @@ -20,8 +20,8 @@ pluginManagement { } def frcHomeMaven = new File(frcHome, 'maven') maven { - name 'frcHome' - url frcHomeMaven + name = 'frcHome' + url = frcHomeMaven } } } diff --git a/src/main/deploy/pathplanner/autos/Center-Shoot.auto b/src/main/deploy/pathplanner/autos/Center-Shoot.auto new file mode 100644 index 0000000..03bbe7f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center-Shoot.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "path", + "data": { + "pathName": "Center-Hub-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Pikes Peak Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto b/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto new file mode 100644 index 0000000..a537fa2 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto @@ -0,0 +1,44 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Shoot driving across" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot Driving" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": "Spring Break Tests", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HubCenter-Score-PushLeft.auto b/src/main/deploy/pathplanner/autos/HubCenter-Score-PushLeft.auto new file mode 100644 index 0000000..82db52e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HubCenter-Score-PushLeft.auto @@ -0,0 +1,67 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "path", + "data": { + "pathName": "Center-Hub-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "path", + "data": { + "pathName": "Center-NeutralL" + } + }, + { + "type": "path", + "data": { + "pathName": "HubFarLeft-NeutralZone 1-2" + } + }, + { + "type": "named", + "data": { + "name": "Intake Retracted" + } + }, + { + "type": "path", + "data": { + "pathName": "HubFarLeft-NeutralZone 2-2" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Pikes Peak Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HubCenter-Score-PushRight.auto b/src/main/deploy/pathplanner/autos/HubCenter-Score-PushRight.auto new file mode 100644 index 0000000..e0aeb16 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HubCenter-Score-PushRight.auto @@ -0,0 +1,67 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "path", + "data": { + "pathName": "Center-Hub-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "path", + "data": { + "pathName": "Center-NeutralR" + } + }, + { + "type": "path", + "data": { + "pathName": "HubFarRight-NeutralZone 1-2" + } + }, + { + "type": "named", + "data": { + "name": "Intake Retracted" + } + }, + { + "type": "path", + "data": { + "pathName": "HubFarRight-NeutralZone 2-2" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Pikes Peak Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HubCenter-Station-Score.auto b/src/main/deploy/pathplanner/autos/HubCenter-Station-Score.auto new file mode 100644 index 0000000..6056557 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HubCenter-Station-Score.auto @@ -0,0 +1,56 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "HubCenter-PlayerStation" + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "path", + "data": { + "pathName": "CPlayerStation-CShoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Pikes Peak Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HubFarLeft-Score-Push.auto b/src/main/deploy/pathplanner/autos/HubFarLeft-Score-Push.auto new file mode 100644 index 0000000..b4ace59 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HubFarLeft-Score-Push.auto @@ -0,0 +1,62 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.15 + } + }, + { + "type": "path", + "data": { + "pathName": "HubFarLeft-NeutralZone 1-2" + } + }, + { + "type": "path", + "data": { + "pathName": "HubFarLeft-NeutralZone 2-2" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "HubFarRight-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Pikes Peak Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HubFarRight-Score-Push.auto b/src/main/deploy/pathplanner/autos/HubFarRight-Score-Push.auto new file mode 100644 index 0000000..8bebec6 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HubFarRight-Score-Push.auto @@ -0,0 +1,62 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "path", + "data": { + "pathName": "Copy of HubFarRight-NeutralZone 1-2" + } + }, + { + "type": "path", + "data": { + "pathName": "HubFarRight-NeutralZone 2-2" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "HubFarLeft-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Pikes Peak Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HubLeft-Score.auto b/src/main/deploy/pathplanner/autos/HubLeft-Score.auto new file mode 100644 index 0000000..cc4a42e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HubLeft-Score.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "path", + "data": { + "pathName": "HubLeft-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Pikes Peak Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HubRight-Score.auto b/src/main/deploy/pathplanner/autos/HubRight-Score.auto new file mode 100644 index 0000000..9dd2e72 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HubRight-Score.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "path", + "data": { + "pathName": "HubRight-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Pikes Peak Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HubRight-Station-Score-Push.auto b/src/main/deploy/pathplanner/autos/HubRight-Station-Score-Push.auto new file mode 100644 index 0000000..e6aae55 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HubRight-Station-Score-Push.auto @@ -0,0 +1,80 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "HubRight-PlayerStation" + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "path", + "data": { + "pathName": "PlayerStation-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "path", + "data": { + "pathName": "ReadyPush" + } + }, + { + "type": "path", + "data": { + "pathName": "HubFarRight-NeutralZone 1-2" + } + }, + { + "type": "path", + "data": { + "pathName": "HubFarRight-NeutralZone 2-2" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Pikes Peak Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HubRight-Station-Score.auto b/src/main/deploy/pathplanner/autos/HubRight-Station-Score.auto new file mode 100644 index 0000000..820b779 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HubRight-Station-Score.auto @@ -0,0 +1,56 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "HubRight-PlayerStation" + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "path", + "data": { + "pathName": "PlayerStation-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Pikes Peak Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ShootPreloadBump.auto b/src/main/deploy/pathplanner/autos/ShootPreloadBump.auto new file mode 100644 index 0000000..bc6176b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ShootPreloadBump.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "path", + "data": { + "pathName": "Bump test" + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetForward" + } + }, + { + "type": "path", + "data": { + "pathName": "BumpToCenter" + } + }, + { + "type": "path", + "data": { + "pathName": "HubFarLeft-NeutralZone 2-2" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Spring Break Tests", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test-LClimb.auto b/src/main/deploy/pathplanner/autos/Test-LClimb.auto new file mode 100644 index 0000000..d4084c8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Test-LClimb.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Climber Extended" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "path", + "data": { + "pathName": "LeftTower" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Climber Retracted" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Spring Break Tests", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test-RClimb.auto b/src/main/deploy/pathplanner/autos/Test-RClimb.auto new file mode 100644 index 0000000..69de5c5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Test-RClimb.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Climber Extended" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "path", + "data": { + "pathName": "RightTower" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Climber Retracted" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Spring Break Tests", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/X. LeftRamp-Neutral-Shoot-Neutral-Shoot.auto b/src/main/deploy/pathplanner/autos/X. LeftRamp-Neutral-Shoot-Neutral-Shoot.auto new file mode 100644 index 0000000..9c3c686 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/X. LeftRamp-Neutral-Shoot-Neutral-Shoot.auto @@ -0,0 +1,123 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeftBump" + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetForward" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftBump-Neutral-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetBackward" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftRamp-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeftBump2" + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetForward" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftBump2-Neutral2-Shoot2" + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetBackward" + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftRamp-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/X. LeftRamp-Neutral-Shoot.auto b/src/main/deploy/pathplanner/autos/X. LeftRamp-Neutral-Shoot.auto new file mode 100644 index 0000000..ed38e29 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/X. LeftRamp-Neutral-Shoot.auto @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeftBump" + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetForward" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftBump-Neutral-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetBackward" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftRamp-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Shoot.auto b/src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Shoot.auto new file mode 100644 index 0000000..35e60ec --- /dev/null +++ b/src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Shoot.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "named", + "data": { + "name": "Intake Reference" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftTrench-Neutral-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetBackward" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftRamp-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Trench-Shoot-Neutral-Shoot.auto b/src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Trench-Shoot-Neutral-Shoot.auto new file mode 100644 index 0000000..5f64882 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Trench-Shoot-Neutral-Shoot.auto @@ -0,0 +1,149 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "named", + "data": { + "name": "Intake Reference" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeftTrench-Neutral-Trench-Shoot" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "Labubu Growl" + } + }, + { + "type": "wait", + "data": { + "waitTime": 4.2 + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "named", + "data": { + "name": "Intake Reference" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ShotL-Neutral" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "Labubu Growl" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Trench-Shoot.auto b/src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Trench-Shoot.auto new file mode 100644 index 0000000..77820b1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Trench-Shoot.auto @@ -0,0 +1,93 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Zero Encoder" + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "named", + "data": { + "name": "Intake Reference" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeftTrench-Neutral-Trench-Shoot" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "Labubu Growl" + } + }, + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/X. RightRamp-Neutral-Shoot-Neutral-Shoot.auto b/src/main/deploy/pathplanner/autos/X. RightRamp-Neutral-Shoot-Neutral-Shoot.auto new file mode 100644 index 0000000..7597fbd --- /dev/null +++ b/src/main/deploy/pathplanner/autos/X. RightRamp-Neutral-Shoot-Neutral-Shoot.auto @@ -0,0 +1,123 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RightBump" + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetForward" + } + }, + { + "type": "path", + "data": { + "pathName": "RightBump-Neutral-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetBackward" + } + }, + { + "type": "path", + "data": { + "pathName": "RightRamp-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RightBump2" + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetForward" + } + }, + { + "type": "path", + "data": { + "pathName": "RightBump2-Neutral2-Shoot2" + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetBackward" + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "path", + "data": { + "pathName": "RightRamp-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/X. RightRamp-Neutral-Shoot.auto b/src/main/deploy/pathplanner/autos/X. RightRamp-Neutral-Shoot.auto new file mode 100644 index 0000000..bdc9076 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/X. RightRamp-Neutral-Shoot.auto @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RightBump" + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetForward" + } + }, + { + "type": "path", + "data": { + "pathName": "RightBump-Neutral-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetBackward" + } + }, + { + "type": "path", + "data": { + "pathName": "RightRamp-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Ramp-Shoot-PlayerStation-Shoot.auto b/src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Ramp-Shoot-PlayerStation-Shoot.auto new file mode 100644 index 0000000..cd0a6ee --- /dev/null +++ b/src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Ramp-Shoot-PlayerStation-Shoot.auto @@ -0,0 +1,136 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "named", + "data": { + "name": "Intake Reference" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RightTrench-Neutral-Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "RightRamp-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetBackward" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "Labubu Growl" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.8499999999999996 + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "path", + "data": { + "pathName": "ShotRightR-PlayerStation" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "path", + "data": { + "pathName": "PlayerStation-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Shoot.auto b/src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Shoot.auto new file mode 100644 index 0000000..51730f9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Shoot.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "named", + "data": { + "name": "Intake Reference" + } + }, + { + "type": "path", + "data": { + "pathName": "RightTrench-Neutral-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetBackward" + } + }, + { + "type": "path", + "data": { + "pathName": "RightRamp-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Trench-Shoot-PlayerStation-Shoot.auto b/src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Trench-Shoot-PlayerStation-Shoot.auto new file mode 100644 index 0000000..d993021 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Trench-Shoot-PlayerStation-Shoot.auto @@ -0,0 +1,123 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Zero Encoder" + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "named", + "data": { + "name": "Intake Reference" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RightTrench-Neutral-Trench-Shoot" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "Labubu Growl" + } + }, + { + "type": "wait", + "data": { + "waitTime": 4.5 + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "path", + "data": { + "pathName": "ShotRightT-PlayerStation" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "path", + "data": { + "pathName": "PlayerStation-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/X. Sierra.auto b/src/main/deploy/pathplanner/autos/X. Sierra.auto new file mode 100644 index 0000000..fe0ddb4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/X. Sierra.auto @@ -0,0 +1,99 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Zero Encoder" + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "named", + "data": { + "name": "Intake Reference" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SierraOne" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "Labubu Growl" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "path", + "data": { + "pathName": "SierraTwo" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "path", + "data": { + "pathName": "PlayerStation-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/X. zawg.auto b/src/main/deploy/pathplanner/autos/X. zawg.auto new file mode 100644 index 0000000..ffb6fe8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/X. zawg.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "named", + "data": { + "name": "Intake Reference" + } + }, + { + "type": "path", + "data": { + "pathName": "RightTrench-FirstSwipe" + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..95d2e43 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bump test.path b/src/main/deploy/pathplanner/paths/Bump test.path new file mode 100644 index 0000000..d6340e2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bump test.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4908076923076936, + "y": 6.633345195729537 + }, + "prevControl": null, + "nextControl": { + "x": 2.3977564102564113, + "y": 6.0868195547038955 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.4908076923076936, + "y": 5.6745769230769225 + }, + "prevControl": { + "x": 2.6330609248104158, + "y": 5.743196664476699 + }, + "nextControl": { + "x": 3.7815128205128214, + "y": 5.651320512820514 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.967615384615385, + "y": 5.709461538461539 + }, + "prevControl": { + "x": 4.746653846153847, + "y": 5.430384615384616 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.5211640211640187, + "rotationDegrees": -112.5 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -91.24536426676825 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.59775645089275 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BumpToCenter.path b/src/main/deploy/pathplanner/paths/BumpToCenter.path new file mode 100644 index 0000000..0721fbf --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BumpToCenter.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.935255041518388, + "y": 5.6650177935943065 + }, + "prevControl": null, + "nextControl": { + "x": 6.073930090574693, + "y": 5.873030367178768 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.559893238434164, + "y": 7.547876631079477 + }, + "prevControl": { + "x": 7.1940806642941855, + "y": 7.182064056939502 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.98776039963968 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -91.19348942398209 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path b/src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path new file mode 100644 index 0000000..7325176 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.45, + "y": 1.0 + }, + "prevControl": null, + "nextControl": { + "x": 0.8305284220032643, + "y": 0.9200002390150002 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.4998005698005699, + "y": 1.140868945868947 + }, + "prevControl": { + "x": 1.2334625881261518, + "y": 0.9559732945163617 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.6115541, + "y": 4.0213534 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.4, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.4, + "y": 5.5 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.2749379652605452, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -137.5 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center-Hub-Shoot.path b/src/main/deploy/pathplanner/paths/Center-Hub-Shoot.path new file mode 100644 index 0000000..5b245a0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center-Hub-Shoot.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.35, + "y": 4.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5, + "y": 4.0 + }, + "prevControl": { + "x": 2.75, + "y": 4.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center-NeutralL.path b/src/main/deploy/pathplanner/paths/Center-NeutralL.path new file mode 100644 index 0000000..a708e11 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center-NeutralL.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.5, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.5298452380952394, + "y": 4.83402380952381 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 7.4 + }, + "prevControl": { + "x": 2.013238095238095, + "y": 7.648345238095238 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center-NeutralR.path b/src/main/deploy/pathplanner/paths/Center-NeutralR.path new file mode 100644 index 0000000..8099573 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center-NeutralR.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.5, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 4.177702380952382, + "y": 3.2791666666666672 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 0.666 + }, + "prevControl": { + "x": 1.9210000000000012, + "y": 0.590559523809524 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center-ToDepot.path b/src/main/deploy/pathplanner/paths/Center-ToDepot.path new file mode 100644 index 0000000..a5205d5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center-ToDepot.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.5, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.490418422348576, + "y": 4.249816319262193 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.115, + "y": 6.0 + }, + "prevControl": { + "x": 2.1584120444167327, + "y": 5.753798061746948 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -178.85423716182484 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Copy of HubFarRight-NeutralZone 1-2.path b/src/main/deploy/pathplanner/paths/Copy of HubFarRight-NeutralZone 1-2.path new file mode 100644 index 0000000..9f98845 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Copy of HubFarRight-NeutralZone 1-2.path @@ -0,0 +1,65 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6162261904761905, + "y": 0.6661428571428576 + }, + "prevControl": null, + "nextControl": { + "x": 4.6685357142857145, + "y": 0.6015000000000011 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.663, + "y": 0.8712976190476196 + }, + "prevControl": { + "x": 7.713544609817508, + "y": 0.5725975891884018 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 9.0, + "y": 0.5 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.025631067961170227, + "maxWaypointRelativePos": 0.6920388349514595, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 2.6, + "rotation": 90.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/paths/Depot-Intake-Shoot (side).path b/src/main/deploy/pathplanner/paths/Depot-Intake-Shoot (side).path new file mode 100644 index 0000000..b0fb526 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Depot-Intake-Shoot (side).path @@ -0,0 +1,107 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.115, + "y": 6.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.9425952380952385, + "y": 6.874773809523809 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.52, + "y": 6.972 + }, + "prevControl": { + "x": 0.48937971516014256, + "y": 7.437592646883011 + }, + "nextControl": { + "x": 0.5506202848398575, + "y": 6.50640735311699 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.485, + "y": 4.985190476190476 + }, + "prevControl": { + "x": 0.554559516916223, + "y": 4.745062440329642 + }, + "nextControl": { + "x": 0.36614285714285816, + "y": 5.395499999999999 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3960952380952394, + "y": 5.179547619047619 + }, + "prevControl": { + "x": 2.1460952380952394, + "y": 5.179547619047619 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 0.5, + "y": 4.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.2, + "maxWaypointRelativePos": 2.3115107913668993, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 4.61155415, + "y": 4.0213534 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 2.673664122137403, + "maxWaypointRelativePos": 3.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.4, + "maxAcceleration": 1.0, + "maxAngularVelocity": 500.0, + "maxAngularAcceleration": 700.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 147.26477372789242 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path b/src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path new file mode 100644 index 0000000..cbd5c03 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path @@ -0,0 +1,121 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5541168091168096, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.8047435897435893, + "y": 3.957478632478633 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3637023809523816, + "y": 3.0632142857142854 + }, + "prevControl": { + "x": 2.6426055148555156, + "y": 3.382459299959301 + }, + "nextControl": { + "x": 1.8588436027530886, + "y": 2.4853303244177742 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5754761904761914, + "y": 1.0 + }, + "prevControl": { + "x": 2.2272563574654534, + "y": 0.9727386663284083 + }, + "nextControl": { + "x": 0.9236960234869294, + "y": 1.0272613336715914 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.3, + "y": 0.8999999999999999 + }, + "prevControl": { + "x": 0.5497421350529089, + "y": 0.8886480847776855 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.771712158808938, + "maxWaypointRelativePos": 3.0, + "constraints": { + "maxVelocity": 0.7, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 0.0, + "y": 0.7 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 1.0272952853597963, + "maxWaypointRelativePos": 1.95, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.0, + "y": 4.0 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.44367245657569376, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -95.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/paths/HubFarLeft-NeutralZone 1-2.path b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path new file mode 100644 index 0000000..cf8bda0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path @@ -0,0 +1,65 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.594630952380953, + "y": 7.425452380952382 + }, + "prevControl": null, + "nextControl": { + "x": 4.690130952380953, + "y": 7.504892857142858 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.535761904761905, + "y": 7.54422619047619 + }, + "prevControl": { + "x": 7.654535714285714, + "y": 7.187904761904762 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 9.0, + "y": 7.5 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.025631067961170227, + "maxWaypointRelativePos": 0.6920388349514595, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.5, + "rotation": -90.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/paths/HubFarLeft-NeutralZone 2-2.path b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path new file mode 100644 index 0000000..2cab886 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path @@ -0,0 +1,105 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.535761904761905, + "y": 7.54422619047619 + }, + "prevControl": null, + "nextControl": { + "x": 7.374699934066787, + "y": 8.027412102561545 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.535761904761905, + "y": 0.666 + }, + "prevControl": { + "x": 7.809145597850186, + "y": 0.6686447716595759 + }, + "nextControl": { + "x": 6.434404761904762, + "y": 0.6553452380952387 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 0.666 + }, + "prevControl": { + "x": 4.246995139395343, + "y": 0.7046445483228254 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.2679900744416821, + "maxWaypointRelativePos": 0.8992555831265578, + "constraints": { + "maxVelocity": 1.2, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 6.75, + "y": 0.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.9091262135922324, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.4, + "y": 0.5 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.9728155339805904, + "maxWaypointRelativePos": 1.5499999999999998, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 3.0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HubFarLeft-Shoot.path b/src/main/deploy/pathplanner/paths/HubFarLeft-Shoot.path new file mode 100644 index 0000000..aa3eec5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HubFarLeft-Shoot.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 7.4 + }, + "prevControl": null, + "nextControl": { + "x": 2.7848095238095247, + "y": 7.393059523809524 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.514869047619049, + "y": 5.805809523809524 + }, + "prevControl": { + "x": 2.638903782208257, + "y": 6.0228703093406395 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.61155415, + "y": 4.0213534 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.40595533498758746, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.0, + "y": 7.4 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.32258064516129836, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 139.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 1-2.path b/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 1-2.path new file mode 100644 index 0000000..4e9caae --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 1-2.path @@ -0,0 +1,65 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 0.666 + }, + "prevControl": null, + "nextControl": { + "x": 5.052309523809524, + "y": 0.6013571428571436 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.663, + "y": 0.8712976190476196 + }, + "prevControl": { + "x": 7.713544609817508, + "y": 0.5725975891884018 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 9.0, + "y": 0.5 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.025631067961170227, + "maxWaypointRelativePos": 0.6920388349514595, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.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/paths/HubFarRight-NeutralZone 2-2.path b/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path new file mode 100644 index 0000000..ddc66c0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path @@ -0,0 +1,105 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.663, + "y": 0.8712976190476196 + }, + "prevControl": null, + "nextControl": { + "x": 7.578952380952382, + "y": 1.367988095238095 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.53, + "y": 7.4 + }, + "prevControl": { + "x": 7.779738811179202, + "y": 7.388575194058578 + }, + "nextControl": { + "x": 6.471233966716803, + "y": 7.448435388999068 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 7.4 + }, + "prevControl": { + "x": 4.246995139395343, + "y": 7.438644548322825 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.95483870967742, + "constraints": { + "maxVelocity": 1.2, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 6.75, + "y": 8.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.9091262135922324, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.4, + "y": 7.4 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.9728155339805904, + "maxWaypointRelativePos": 1.5499999999999998, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 3.0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HubFarRight-Shoot.path b/src/main/deploy/pathplanner/paths/HubFarRight-Shoot.path new file mode 100644 index 0000000..068d796 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HubFarRight-Shoot.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 0.666 + }, + "prevControl": null, + "nextControl": { + "x": 3.17352380952381, + "y": 0.5905595238095237 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.61204761904762, + "y": 1.7351071428571436 + }, + "prevControl": { + "x": 2.8495119047619064, + "y": 1.4542857142857142 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.61155415, + "y": 4.0213534 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.40595533498758746, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.0, + "y": 0.5 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.32258064516129836, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -129.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HubLeft-Shoot.path b/src/main/deploy/pathplanner/paths/HubLeft-Shoot.path new file mode 100644 index 0000000..b535ae3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HubLeft-Shoot.path @@ -0,0 +1,65 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.508573466476462, + "y": 6.473937232524964 + }, + "prevControl": null, + "nextControl": { + "x": 3.258573466476462, + "y": 6.473937232524964 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.029843081312411, + "y": 5.412967189728959 + }, + "prevControl": { + "x": 3.133352353780315, + "y": 5.8916975748930085 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.612, + "y": 4.0213534 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.18893129770992434, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 139.3987053549956 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path b/src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path new file mode 100644 index 0000000..0d931a0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5838333333333345, + "y": 1.5515476190476192 + }, + "prevControl": null, + "nextControl": { + "x": 3.3365992451162603, + "y": 1.5886327322801814 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5754761904761914, + "y": 1.0 + }, + "prevControl": { + "x": 2.2272563574654534, + "y": 0.9727386663284083 + }, + "nextControl": { + "x": 0.9236960234869294, + "y": 1.0272613336715914 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.325, + "y": 0.95 + }, + "prevControl": { + "x": 0.5747421350529089, + "y": 0.9386480847776856 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 0.0, + "y": 0.7 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.6, + "maxWaypointRelativePos": 1.0956834532374118, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -95.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/paths/HubRight-Shoot.path b/src/main/deploy/pathplanner/paths/HubRight-Shoot.path new file mode 100644 index 0000000..11b30f3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HubRight-Shoot.path @@ -0,0 +1,65 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.573035714285715, + "y": 1.5623452380952383 + }, + "prevControl": null, + "nextControl": { + "x": 3.323035714285715, + "y": 1.5623452380952383 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.299, + "y": 2.599 + }, + "prevControl": { + "x": 2.5364642857142865, + "y": 2.3181785714285708 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.61155415, + "y": 4.0213534 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.18893129770992434, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -148.82 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LShoot-Tower.path b/src/main/deploy/pathplanner/paths/LShoot-Tower.path new file mode 100644 index 0000000..6ea3d76 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LShoot-Tower.path @@ -0,0 +1,65 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.029843081312411, + "y": 5.412967189728959 + }, + "prevControl": null, + "nextControl": { + "x": 1.5430833333333345, + "y": 5.40629761904762 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1003809523809533, + "y": 4.72604761904762 + }, + "prevControl": { + "x": -0.15214285714285625, + "y": 4.8772142857142855 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.0, + "y": 5.0 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.10038167938931303, + "maxWaypointRelativePos": 0.9064885496183247, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 139.399 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftBump-Neutral-Shoot.path b/src/main/deploy/pathplanner/paths/LeftBump-Neutral-Shoot.path new file mode 100644 index 0000000..094ce2f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftBump-Neutral-Shoot.path @@ -0,0 +1,128 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.968, + "y": 5.709 + }, + "prevControl": null, + "nextControl": { + "x": 6.105960476887348, + "y": 5.9174871862178 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.781615384615385, + "y": 6.976935897435897 + }, + "prevControl": { + "x": 7.478867821173717, + "y": 7.355370351737978 + }, + "nextControl": { + "x": 7.9377891465039925, + "y": 6.78171869507514 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.781615384615385, + "y": 4.837346153846154 + }, + "prevControl": { + "x": 7.804249321126013, + "y": 5.08631945546308 + }, + "nextControl": { + "x": 7.758981448104757, + "y": 4.588372852229227 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.572282051282052, + "y": 5.511782051282052 + }, + "prevControl": { + "x": 6.795888849032032, + "y": 5.623585450157039 + }, + "nextControl": { + "x": 6.348675253532072, + "y": 5.399978652407064 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.3977820512820527, + "y": 5.511782051282052 + }, + "prevControl": { + "x": 3.6429272202047827, + "y": 5.560811085066598 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 3.4736842105263164, + "rotationDegrees": 22.5 + } + ], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 7.0, + "y": 0.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.02657807308969639, + "maxWaypointRelativePos": 2.1705426356589252, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 7.0, + "y": 0.0 + }, + "rotationOffset": 100.0, + "minWaypointRelativePos": 2.4983388704318887, + "maxWaypointRelativePos": 3.2956810631229274, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 23.07 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftBump.path b/src/main/deploy/pathplanner/paths/LeftBump.path new file mode 100644 index 0000000..9a95768 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftBump.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4559230769230784, + "y": 5.709461538461539 + }, + "prevControl": null, + "nextControl": { + "x": 3.7466282051282063, + "y": 5.68620512820513 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.967615384615385, + "y": 5.709461538461539 + }, + "prevControl": { + "x": 4.746653846153847, + "y": 5.430384615384616 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5211640211640187, + "rotationDegrees": -112.5 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -91.24536426676825 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -147.0305960965379 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftBump2-Neutral2-Shoot2.path b/src/main/deploy/pathplanner/paths/LeftBump2-Neutral2-Shoot2.path new file mode 100644 index 0000000..2cba6eb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftBump2-Neutral2-Shoot2.path @@ -0,0 +1,148 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.049, + "y": 5.651320512820514 + }, + "prevControl": null, + "nextControl": { + "x": 6.049027667109173, + "y": 5.401320514351452 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.7931923076864935, + "y": 3.453589743589744 + }, + "prevControl": { + "x": 5.724512025720087, + "y": 3.693970730502218 + }, + "nextControl": { + "x": 5.90029192050797, + "y": 3.0787410986677135 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.316487179487182, + "y": 4.128025641025641 + }, + "prevControl": { + "x": 7.1826016731476, + "y": 3.9168984964132205 + }, + "nextControl": { + "x": 7.618820512820513, + "y": 4.604782051282052 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.4908846153846165, + "y": 5.511782051282052 + }, + "prevControl": { + "x": 6.716022524475761, + "y": 5.620469317739846 + }, + "nextControl": { + "x": 6.153666666666667, + "y": 5.348987179487179 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.3977820512820527, + "y": 5.511782051282052 + }, + "prevControl": { + "x": 3.6429272202047827, + "y": 5.560811085066598 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 3.4736842105263164, + "rotationDegrees": 22.5 + } + ], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 6.0, + "y": 0.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.02657807308969639, + "maxWaypointRelativePos": 0.9302325581395484, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 8.0, + "y": 5.5 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 1.2846068660022045, + "maxWaypointRelativePos": 1.7718715393134012, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 5.0, + "y": 8.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 1.8870431893687745, + "maxWaypointRelativePos": 2.2768549280177193, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 12.0, + "y": 8.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 2.5603543743078774, + "maxWaypointRelativePos": 3.0121816168327573, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 23.069999999999993 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftBump2.path b/src/main/deploy/pathplanner/paths/LeftBump2.path new file mode 100644 index 0000000..bd9e0bb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftBump2.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.2815000000000016, + "y": 5.511782051282052 + }, + "prevControl": null, + "nextControl": { + "x": 3.735000000000001, + "y": 5.593179487179486 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.049, + "y": 5.616435897435898 + }, + "prevControl": { + "x": 4.804782051282053, + "y": 5.616435897435898 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.45263157894736444, + "rotationDegrees": 112.5 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 91.245 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 117.25532837494306 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftRamp-Shoot.path b/src/main/deploy/pathplanner/paths/LeftRamp-Shoot.path new file mode 100644 index 0000000..7af0087 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftRamp-Shoot.path @@ -0,0 +1,65 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.398, + "y": 5.512 + }, + "prevControl": null, + "nextControl": { + "x": 3.2417373766296596, + "y": 5.316853920004989 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.23498717948718, + "y": 5.407128205128206 + }, + "prevControl": { + "x": 3.369206052535471, + "y": 5.618043577061233 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.612, + "y": 4.021 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 133.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 113.07 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftTower.path b/src/main/deploy/pathplanner/paths/LeftTower.path new file mode 100644 index 0000000..ba289a4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftTower.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.0522864367238562, + "y": 4.953628205128206 + }, + "prevControl": null, + "nextControl": { + "x": 1.0522864367238562, + "y": 4.66062130060539 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0522864367238562, + "y": 4.662923076923078 + }, + "prevControl": { + "x": 1.0522864367238562, + "y": 4.912923076923078 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 0.0, + "y": 4.67 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.7513725490196074, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.0, + "y": 5.0 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.36705882352941094, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.1, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Shoot.path b/src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Shoot.path new file mode 100644 index 0000000..ef47089 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Shoot.path @@ -0,0 +1,152 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5373205128205143, + "y": 7.430435897435898 + }, + "prevControl": null, + "nextControl": { + "x": 3.7870116206619695, + "y": 7.418012103131955 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.642076923076923, + "y": 7.069961538461539 + }, + "prevControl": { + "x": 7.178228149431453, + "y": 7.5205380189223625 + }, + "nextControl": { + "x": 8.216115793872147, + "y": 6.51234787481454 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.781615384615385, + "y": 4.837346153846154 + }, + "prevControl": { + "x": 7.786585532087434, + "y": 5.241663699788379 + }, + "nextControl": { + "x": 7.778542446002628, + "y": 4.5873650404629975 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.572282051282052, + "y": 5.511782051282052 + }, + "prevControl": { + "x": 6.957648225000392, + "y": 5.601152104386376 + }, + "nextControl": { + "x": 6.204516600433754, + "y": 5.426493771942126 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.3977820512820527, + "y": 5.511782051282052 + }, + "prevControl": { + "x": 3.7826390804558825, + "y": 5.585258005415613 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 3.4736842105263164, + "rotationDegrees": 22.5 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 2.374307862679942, + "maxWaypointRelativePos": 4.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 10.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 7.0, + "y": 0.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.9302325581395282, + "maxWaypointRelativePos": 2.1705426356589252, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 8.0, + "y": 7.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 2.4983388704318887, + "maxWaypointRelativePos": 3.2956810631229274, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.0, + "y": 7.4 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.6821705426356636, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 5.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 11.706999999999994 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Trench-Shoot.path b/src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Trench-Shoot.path new file mode 100644 index 0000000..929a6dc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Trench-Shoot.path @@ -0,0 +1,173 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.594630952380953, + "y": 7.414654761904762 + }, + "prevControl": null, + "nextControl": { + "x": 4.156107142857143, + "y": 7.457845238095238 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.460178571428572, + "y": 7.198702380952382 + }, + "prevControl": { + "x": 6.952690476190477, + "y": 7.522630952380953 + }, + "nextControl": { + "x": 7.89244195981076, + "y": 6.922789579857368 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.632940476190477, + "y": 4.71525 + }, + "prevControl": { + "x": 7.6161014126744035, + "y": 4.964682247193305 + }, + "nextControl": { + "x": 7.682415698697642, + "y": 3.9823876339562227 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.402011904761906, + "y": 6.561642857142857 + }, + "prevControl": { + "x": 6.45205334133696, + "y": 6.141294789912411 + }, + "nextControl": { + "x": 6.240047619047619, + "y": 7.922142857142857 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.594630952380953, + "y": 7.414654761904762 + }, + "prevControl": { + "x": 3.8777698932069846, + "y": 7.374206341786758 + }, + "nextControl": { + "x": 3.2690412087912097, + "y": 7.461167582417582 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.61204761904762, + "y": 6.216119047619048 + }, + "prevControl": { + "x": 3.0007619047619056, + "y": 6.4644642857142856 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.275434243176181, + "maxWaypointRelativePos": 1.8709677419354975, + "constraints": { + "maxVelocity": 1.2, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 7.0, + "y": 0.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 1.3067552602436252, + "maxWaypointRelativePos": 2.0198511166253303, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.0, + "y": 7.4 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.6910299003322272, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.0, + "y": 7.4 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 2.5913621262458837, + "maxWaypointRelativePos": 4.28, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 4.612, + "y": 4.021 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 4.529346622369888, + "maxWaypointRelativePos": 5.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 133.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/No Move.path b/src/main/deploy/pathplanner/paths/No Move.path new file mode 100644 index 0000000..2d956b3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/No Move.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.5, + "y": 2.867 + }, + "prevControl": null, + "nextControl": { + "x": 2.7320383800418035, + "y": 2.77395060350773 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5, + "y": 2.867 + }, + "prevControl": { + "x": 2.25, + "y": 2.867 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.1, + "maxAcceleration": 0.1, + "maxAngularVelocity": 0.1, + "maxAngularAcceleration": 0.1, + "nominalVoltage": 6.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -144.898 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -144.898 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path b/src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path new file mode 100644 index 0000000..f3f2e7c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.45, + "y": 1.0 + }, + "prevControl": null, + "nextControl": { + "x": 0.8305284220032643, + "y": 0.9200002390150002 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.435, + "y": 1.6487261904761907 + }, + "prevControl": { + "x": 1.168662018325582, + "y": 1.4638305391236057 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.612, + "y": 4.0213534 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.4, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.4, + "y": 5.5 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.2749379652605452, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -142.9 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RShoot-Tower (Matt).path b/src/main/deploy/pathplanner/paths/RShoot-Tower (Matt).path new file mode 100644 index 0000000..17a9501 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RShoot-Tower (Matt).path @@ -0,0 +1,97 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.299, + "y": 2.599 + }, + "prevControl": null, + "nextControl": { + "x": 2.1477500000000003, + "y": 2.836464285714286 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.37, + "y": 2.6 + }, + "prevControl": { + "x": 1.3866297526309428, + "y": 2.3505537105358476 + }, + "nextControl": { + "x": 1.3533702473690576, + "y": 2.8494462894641526 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.37, + "y": 2.8 + }, + "prevControl": { + "x": 1.12, + "y": 2.8 + }, + "nextControl": { + "x": 1.62, + "y": 2.8 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0522864367238562, + "y": 2.8 + }, + "prevControl": { + "x": 1.3005335052725924, + "y": 2.829553222446277 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 0.0, + "y": 2.7 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.7209923664121958, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.1, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 770.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -148.815 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RShoot-Tower.path b/src/main/deploy/pathplanner/paths/RShoot-Tower.path new file mode 100644 index 0000000..08f3e22 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RShoot-Tower.path @@ -0,0 +1,65 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.299, + "y": 2.599 + }, + "prevControl": null, + "nextControl": { + "x": 2.1477500000000003, + "y": 2.836464285714286 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0032023809523818, + "y": 2.7824761904761908 + }, + "prevControl": { + "x": 1.5106904761904771, + "y": 2.7716785714285708 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 0.0, + "y": 3.0 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.30343511450381533, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 770.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -148.815 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReadyPush.path b/src/main/deploy/pathplanner/paths/ReadyPush.path new file mode 100644 index 0000000..3c8bced --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ReadyPush.path @@ -0,0 +1,65 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.435, + "y": 1.649 + }, + "prevControl": null, + "nextControl": { + "x": 1.607678571428572, + "y": 1.3897738095238092 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 0.666 + }, + "prevControl": { + "x": 2.871190476190477, + "y": 0.5689642857142869 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 0.0, + "y": 1.0 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.5, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 2.5, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -148.82 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RightBump-Neutral-Shoot.path b/src/main/deploy/pathplanner/paths/RightBump-Neutral-Shoot.path new file mode 100644 index 0000000..63d6dd2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RightBump-Neutral-Shoot.path @@ -0,0 +1,128 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.968, + "y": 2.7089999999999996 + }, + "prevControl": null, + "nextControl": { + "x": 5.900563478191006, + "y": 2.468267128280525 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.723474358974359, + "y": 0.9767820512820515 + }, + "prevControl": { + "x": 7.037410256410256, + "y": 1.0930641025641024 + }, + "nextControl": { + "x": 7.969958998382976, + "y": 0.9350049937551674 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.863012820512822, + "y": 3.4419615384615385 + }, + "prevControl": { + "x": 7.88564675702345, + "y": 3.690934840078465 + }, + "nextControl": { + "x": 7.8403788840021935, + "y": 3.192988236844612 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.572282051282052, + "y": 2.5119999999999996 + }, + "prevControl": { + "x": 6.795888849032032, + "y": 2.623803398874987 + }, + "nextControl": { + "x": 6.348675253532072, + "y": 2.400196601125012 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.3977820512820527, + "y": 2.512 + }, + "prevControl": { + "x": 3.6429272202047827, + "y": 2.5610290337845467 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 3.4736842105263164, + "rotationDegrees": -22.5 + } + ], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 7.0, + "y": 8.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.02657807308969639, + "maxWaypointRelativePos": 2.1705426356589252, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 7.0, + "y": 8.0 + }, + "rotationOffset": -100.0, + "minWaypointRelativePos": 2.4983388704318887, + "maxWaypointRelativePos": 3.2956810631229274, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -23.069999999999993 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 88.32335186314447 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RightBump.path b/src/main/deploy/pathplanner/paths/RightBump.path new file mode 100644 index 0000000..916396a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RightBump.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4559230769230784, + "y": 2.5465897435897444 + }, + "prevControl": null, + "nextControl": { + "x": 3.7466282051282063, + "y": 2.523333333333337 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.967615384615385, + "y": 2.7089999999999996 + }, + "prevControl": { + "x": 4.746653846153847, + "y": 2.4299230769230773 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5211640211640187, + "rotationDegrees": 112.5 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 91.245 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 127.62823656328939 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RightBump2-Neutral2-Shoot2.path b/src/main/deploy/pathplanner/paths/RightBump2-Neutral2-Shoot2.path new file mode 100644 index 0000000..712b6ab --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RightBump2-Neutral2-Shoot2.path @@ -0,0 +1,142 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.049, + "y": 2.5119999999999996 + }, + "prevControl": null, + "nextControl": { + "x": 6.049027667109173, + "y": 2.2620000015309376 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.049, + "y": 4.8140897435897445 + }, + "prevControl": { + "x": 5.8000264938666355, + "y": 4.791458056872672 + }, + "nextControl": { + "x": 6.297973506133364, + "y": 4.836721430306816 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.374628205128206, + "y": 4.267564102564102 + }, + "prevControl": { + "x": 7.209222795856494, + "y": 4.455023566405375 + }, + "nextControl": { + "x": 7.549051282051284, + "y": 4.069884615384615 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.7118205128205135, + "y": 2.779153846153846 + }, + "prevControl": { + "x": 6.936958421911658, + "y": 2.8878411126116403 + }, + "nextControl": { + "x": 6.374602564102564, + "y": 2.6163589743589735 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.3977820512820527, + "y": 2.5119999999999996 + }, + "prevControl": { + "x": 3.642927220204783, + "y": 2.5610290337845445 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.18947368421052632, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.4736842105263164, + "rotationDegrees": -22.5 + } + ], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 6.0, + "y": 8.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.23034330011073328, + "maxWaypointRelativePos": 0.868217054263568, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 8.0, + "y": 2.5 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 1.2846068660022045, + "maxWaypointRelativePos": 1.7718715393134012, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 5.0, + "y": 0.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 2.1085271317829366, + "maxWaypointRelativePos": 3.1539313399778472, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -23.069999999999993 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RightBump2.path b/src/main/deploy/pathplanner/paths/RightBump2.path new file mode 100644 index 0000000..129f1b8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RightBump2.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5256923076923092, + "y": 2.6159999999999997 + }, + "prevControl": null, + "nextControl": { + "x": 3.8279615384615413, + "y": 1.9764871794871786 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.049, + "y": 2.6159999999999997 + }, + "prevControl": { + "x": 5.002474358974361, + "y": 2.488448717948718 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5211640211640187, + "rotationDegrees": -112.5 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -91.245 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": -108.98741511036879 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RightRamp-Shoot.path b/src/main/deploy/pathplanner/paths/RightRamp-Shoot.path new file mode 100644 index 0000000..1515e1e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RightRamp-Shoot.path @@ -0,0 +1,65 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.398, + "y": 2.5119999999999996 + }, + "prevControl": null, + "nextControl": { + "x": 3.160642585150445, + "y": 2.590494952805491 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.398, + "y": 2.5119999999999996 + }, + "prevControl": { + "x": 3.595338054344081, + "y": 2.3585148466212686 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.612, + "y": 4.021 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 0.1, + "maxAngularAcceleration": 0.1, + "nominalVoltage": 6.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -129.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": -23.07 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RightTower.path b/src/main/deploy/pathplanner/paths/RightTower.path new file mode 100644 index 0000000..248e5a3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RightTower.path @@ -0,0 +1,65 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.0522864367238562, + "y": 2.4768205128205136 + }, + "prevControl": null, + "nextControl": { + "x": 1.0771623664791057, + "y": 2.725579810373011 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0522864367238562, + "y": 2.8 + }, + "prevControl": { + "x": 1.0473432640619955, + "y": 2.5500488746894003 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 0.0, + "y": 2.7 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.3999999999999999, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.1, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RightTrench-FirstSwipe.path b/src/main/deploy/pathplanner/paths/RightTrench-FirstSwipe.path new file mode 100644 index 0000000..86dad1d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RightTrench-FirstSwipe.path @@ -0,0 +1,131 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.8969642857142865, + "y": 0.6757407407407425 + }, + "prevControl": null, + "nextControl": { + "x": 5.1494880952380955, + "y": 0.6013571428571436 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.237607142857144, + "y": 1.4759642857142854 + }, + "prevControl": { + "x": 7.558649163266191, + "y": 0.2980245529994656 + }, + "nextControl": { + "x": 8.584042208300819, + "y": 2.0770024068622486 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.237607142857144, + "y": 3.8190476190476197 + }, + "prevControl": { + "x": 8.310074297364315, + "y": 3.5787466468913385 + }, + "nextControl": { + "x": 8.165425863946698, + "y": 4.058400628167732 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.237607142857144, + "y": 1.0872500000000003 + }, + "prevControl": { + "x": 9.520930827264253, + "y": 2.0312275985936155 + }, + "nextControl": { + "x": 7.304441714310592, + "y": 0.40083918172158384 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.4434642857142865, + "y": 0.6757407407407425 + }, + "prevControl": { + "x": 4.9917133086412, + "y": 0.5083004760390194 + }, + "nextControl": { + "x": 2.6336428571428576, + "y": 0.7633214285714289 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.61204761904762, + "y": 1.918666666666668 + }, + "prevControl": { + "x": 2.502511133859423, + "y": 1.6751404175218187 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "end of RIght trench first swipe" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.9709543568464838, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.4315352697095514, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 3.991228070175485, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -133.33166255170664 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RightTrench-Neutral-Shoot.path b/src/main/deploy/pathplanner/paths/RightTrench-Neutral-Shoot.path new file mode 100644 index 0000000..a4d89f8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RightTrench-Neutral-Shoot.path @@ -0,0 +1,152 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6070897435897447, + "y": 0.6046794871794885 + }, + "prevControl": null, + "nextControl": { + "x": 3.8833980386504074, + "y": 0.5273348158056345 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.595564102564104, + "y": 0.9884102564102579 + }, + "prevControl": { + "x": 7.151835398129718, + "y": 0.5429530943613806 + }, + "nextControl": { + "x": 7.93843114627953, + "y": 1.332612871042205 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.793243589743589, + "y": 3.4070769230769233 + }, + "prevControl": { + "x": 7.74538518514461, + "y": 3.9526634447090423 + }, + "nextControl": { + "x": 7.8344917146104915, + "y": 2.936847688346859 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.572282051282052, + "y": 2.5119999999999996 + }, + "prevControl": { + "x": 7.050706524183416, + "y": 2.443033938208306 + }, + "nextControl": { + "x": 6.24953920807272, + "y": 2.5585241728388555 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.3977820512820527, + "y": 2.512 + }, + "prevControl": { + "x": 3.7815128205128214, + "y": 2.5582179487179486 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 3.4736842105263164, + "rotationDegrees": -22.5 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 2.2059800664451794, + "maxWaypointRelativePos": 4.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 10.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 7.0, + "y": 8.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.859357696566985, + "maxWaypointRelativePos": 2.1705426356589252, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.0, + "y": 5.0 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 2.4983388704318887, + "maxWaypointRelativePos": 3.2956810631229274, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.0, + "y": 0.6 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.6910299003322272, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -23.07 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RightTrench-Neutral-Trench-Shoot.path b/src/main/deploy/pathplanner/paths/RightTrench-Neutral-Trench-Shoot.path new file mode 100644 index 0000000..cd28585 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RightTrench-Neutral-Trench-Shoot.path @@ -0,0 +1,173 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6070897435897447, + "y": 0.6046794871794885 + }, + "prevControl": null, + "nextControl": { + "x": 4.115752584653353, + "y": 0.5635297641960387 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.470976190476191, + "y": 0.9884102564102579 + }, + "prevControl": { + "x": 7.2683607377306005, + "y": 0.7987129676949314 + }, + "nextControl": { + "x": 7.845328732990856, + "y": 1.3388951842695742 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.595564102564104, + "y": 3.3979404761904766 + }, + "prevControl": { + "x": 7.578725039048031, + "y": 3.6473727233837807 + }, + "nextControl": { + "x": 7.645039325071269, + "y": 2.665078110146699 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.3306071428571435, + "y": 0.9884102564102579 + }, + "prevControl": { + "x": 7.602572796768329, + "y": 1.2858726903756175 + }, + "nextControl": { + "x": 6.958504578754579, + "y": 0.5814230769230777 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.711743589743591, + "y": 0.6046794871794885 + }, + "prevControl": { + "x": 3.9948825305696225, + "y": 0.5642310670614844 + }, + "nextControl": { + "x": 3.3861538461538476, + "y": 0.6511923076923085 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.4442692307692324, + "y": 1.302 + }, + "prevControl": { + "x": 3.3396410256410265, + "y": 1.1395769230769235 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "END-RightTrnech-Neutral-Trench-Shoot" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.255583126550872, + "maxWaypointRelativePos": 1.7617866004962806, + "constraints": { + "maxVelocity": 1.2, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 7.0, + "y": 8.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 1.3067552602436252, + "maxWaypointRelativePos": 2.015503875969026, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.0, + "y": 0.6 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.6910299003322272, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.0, + "y": 0.7 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 2.5913621262458837, + "maxWaypointRelativePos": 4.28, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 4.612, + "y": 4.021 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 4.529346622369888, + "maxWaypointRelativePos": 5.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -130.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot driving across.path b/src/main/deploy/pathplanner/paths/Shoot driving across.path new file mode 100644 index 0000000..4af7874 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot driving across.path @@ -0,0 +1,102 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.9326282051282067, + "y": 6.5815769230769225 + }, + "prevControl": null, + "nextControl": { + "x": 1.9932621113872893, + "y": 6.3390412980405895 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9326282051282067, + "y": 1.6628461538461545 + }, + "prevControl": { + "x": 1.936134248900721, + "y": 1.9128215679513527 + }, + "nextControl": { + "x": 1.9291221613556924, + "y": 1.4128707397409563 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9326282051282067, + "y": 6.5815769230769225 + }, + "prevControl": { + "x": 1.9198835421653975, + "y": 6.331901987278464 + }, + "nextControl": { + "x": 1.9453728680910158, + "y": 6.8312518588753814 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9326282051282067, + "y": 1.6628461538461545 + }, + "prevControl": { + "x": 1.9337461053582308, + "y": 1.912843654431812 + }, + "nextControl": { + "x": 1.9315103048981825, + "y": 1.4128486532604971 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9326282051282067, + "y": 6.5815769230769225 + }, + "prevControl": { + "x": 1.8878500514131944, + "y": 6.335619778537727 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.8, + "maxAcceleration": 10.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ShotL-Neutral.path b/src/main/deploy/pathplanner/paths/ShotL-Neutral.path new file mode 100644 index 0000000..5ffabf4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ShotL-Neutral.path @@ -0,0 +1,215 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.612, + "y": 6.216 + }, + "prevControl": null, + "nextControl": { + "x": 2.958273419839204, + "y": 6.2585504440271835 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.595, + "y": 7.415 + }, + "prevControl": { + "x": 2.756519548465237, + "y": 7.377436791520136 + }, + "nextControl": { + "x": 4.203016743276786, + "y": 7.4422386310797695 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.786547619047619, + "y": 7.285083333333334 + }, + "prevControl": { + "x": 5.572245332631391, + "y": 7.447981654135905 + }, + "nextControl": { + "x": 6.56590740520863, + "y": 6.692665946707327 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.786547619047619, + "y": 3.959416666666667 + }, + "prevControl": { + "x": 5.556087373096201, + "y": 4.228987726737335 + }, + "nextControl": { + "x": 6.036389134520053, + "y": 3.6671751917974453 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.58557142857143, + "y": 4.27254761904762 + }, + "prevControl": { + "x": 6.494472316478745, + "y": 3.9148971897508758 + }, + "nextControl": { + "x": 6.681350061848797, + "y": 4.648569607870673 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.315630952380953, + "y": 7.285083333333334 + }, + "prevControl": { + "x": 6.842509529997394, + "y": 7.164317956805786 + }, + "nextControl": { + "x": 5.287977497592714, + "y": 7.520630880111148 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.594630952380953, + "y": 7.414654761904762 + }, + "prevControl": { + "x": 3.844619709717091, + "y": 7.417025670071149 + }, + "nextControl": { + "x": 3.0970699426307586, + "y": 7.409935863847653 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.612, + "y": 6.216 + }, + "prevControl": { + "x": 2.844455287324918, + "y": 6.424977756526983 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 2.650124069478908, + "maxWaypointRelativePos": 2.8709677419354973, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 6.0, + "y": 0.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 2.306755260243625, + "maxWaypointRelativePos": 3.3975186104218427, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.0, + "y": 7.4 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.4, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.0, + "y": 7.4 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 4.689826302729521, + "maxWaypointRelativePos": 6.28, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 4.612, + "y": 4.021 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 6.529346622369888, + "maxWaypointRelativePos": 7.0, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 6.0, + "y": 0.0 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 3.7, + "maxWaypointRelativePos": 4.5786600496277865, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 133.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 133.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ShotRightR-PlayerStation.path b/src/main/deploy/pathplanner/paths/ShotRightR-PlayerStation.path new file mode 100644 index 0000000..4fe709e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ShotRightR-PlayerStation.path @@ -0,0 +1,95 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.398, + "y": 2.512 + }, + "prevControl": null, + "nextControl": { + "x": 3.2508028935074877, + "y": 2.309928201274319 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5754761904761914, + "y": 1.0 + }, + "prevControl": { + "x": 2.2272563574654534, + "y": 0.9727386663284083 + }, + "nextControl": { + "x": 0.9236960234869294, + "y": 1.0272613336715914 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.325, + "y": 0.95 + }, + "prevControl": { + "x": 0.5747421350529089, + "y": 0.9386480847776856 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 10.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 0.0, + "y": 0.7 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.6, + "maxWaypointRelativePos": 1.0956834532374118, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -95.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -106.71386741074437 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ShotRightT-PlayerStation.path b/src/main/deploy/pathplanner/paths/ShotRightT-PlayerStation.path new file mode 100644 index 0000000..d901b4f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ShotRightT-PlayerStation.path @@ -0,0 +1,95 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.4442692307692324, + "y": 1.302 + }, + "prevControl": null, + "nextControl": { + "x": 2.29707212427672, + "y": 1.0999282012743195 + }, + "isLocked": false, + "linkedName": "END-RightTrnech-Neutral-Trench-Shoot" + }, + { + "anchor": { + "x": 1.5754761904761914, + "y": 1.0 + }, + "prevControl": { + "x": 2.2272563574654534, + "y": 0.9727386663284083 + }, + "nextControl": { + "x": 0.9236960234869294, + "y": 1.0272613336715914 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.325, + "y": 0.95 + }, + "prevControl": { + "x": 0.5747421350529089, + "y": 0.9386480847776856 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Playerstation" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 10.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 0.0, + "y": 0.7 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.6, + "maxWaypointRelativePos": 1.0956834532374118, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -95.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -130.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SierraOne.path b/src/main/deploy/pathplanner/paths/SierraOne.path new file mode 100644 index 0000000..b059bc2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SierraOne.path @@ -0,0 +1,163 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6070897435897447, + "y": 0.6046794871794885 + }, + "prevControl": null, + "nextControl": { + "x": 4.115752584653353, + "y": 0.5635297641960387 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.470976190476191, + "y": 0.9884102564102579 + }, + "prevControl": { + "x": 7.2683607377306005, + "y": 0.7987129676949314 + }, + "nextControl": { + "x": 7.845328732990856, + "y": 1.3388951842695742 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.595564102564104, + "y": 3.3979404761904766 + }, + "prevControl": { + "x": 7.578725039048031, + "y": 3.6473727233837807 + }, + "nextControl": { + "x": 7.645039325071269, + "y": 2.665078110146699 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.3306071428571435, + "y": 0.9884102564102579 + }, + "prevControl": { + "x": 7.602572796768329, + "y": 1.2858726903756175 + }, + "nextControl": { + "x": 6.958504578754579, + "y": 0.5814230769230777 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.712, + "y": 0.6046794871794885 + }, + "prevControl": { + "x": 3.997989714851531, + "y": 0.6083696770485412 + }, + "nextControl": { + "x": 3.4545183150183143, + "y": 0.6013571428571436 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.472, + "y": 0.6046794871794885 + }, + "prevControl": { + "x": 2.721109806922687, + "y": 0.6257580093037158 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.255583126550872, + "maxWaypointRelativePos": 1.7617866004962806, + "constraints": { + "maxVelocity": 1.2, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 7.0, + "y": 8.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 1.3067552602436252, + "maxWaypointRelativePos": 2.015503875969026, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.0, + "y": 0.6 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.6910299003322272, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 0.0, + "y": 0.7 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 2.5913621262458837, + "maxWaypointRelativePos": 5.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 3.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SierraTwo.path b/src/main/deploy/pathplanner/paths/SierraTwo.path new file mode 100644 index 0000000..a81aa9c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SierraTwo.path @@ -0,0 +1,95 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.472, + "y": 0.605 + }, + "prevControl": null, + "nextControl": { + "x": 2.2708330107691865, + "y": 0.7534312717853275 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5754761904761914, + "y": 1.0 + }, + "prevControl": { + "x": 2.2272563574654534, + "y": 0.9727386663284083 + }, + "nextControl": { + "x": 0.9236960234869294, + "y": 1.0272613336715914 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.325, + "y": 0.95 + }, + "prevControl": { + "x": 0.5747421350529089, + "y": 0.9386480847776856 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 10.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 0.0, + "y": 0.7 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.6, + "maxWaypointRelativePos": 1.0956834532374118, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -95.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 3.0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Stupidauto.path b/src/main/deploy/pathplanner/paths/Stupidauto.path new file mode 100644 index 0000000..2a8ee7c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Stupidauto.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 0.5365714285714285 + }, + "prevControl": null, + "nextControl": { + "x": 3.119535714285715, + "y": 0.7633214285714292 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.547261904761905, + "y": 3.160392857142857 + }, + "prevControl": { + "x": 2.320511904761905, + "y": 2.696095238095238 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -159.67686317033716 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 177.94339863785623 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..ba45b6f --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,35 @@ +{ + "robotWidth": 0.889, + "robotLength": 0.889, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [ + "Spring Break Tests", + "Pikes Peak Autos" + ], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 600.0, + "defaultMaxAngAccel": 750.0, + "defaultNominalVoltage": 12.0, + "robotMass": 74.088, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.048, + "driveGearing": 5.143, + "maxDriveSpeed": 5.45, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java deleted file mode 100644 index 7027b5e..0000000 --- a/src/main/java/frc4388/robot/Constants.java +++ /dev/null @@ -1,188 +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 edu.wpi.first.math.geometry.Translation2d; -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.36962890625 + 0.5; - public static final double FRONT_RIGHT_ROT_OFFSET = 0.61474609375 + 0.5; - public static final double BACK_LEFT_ROT_OFFSET = -0.227294921875 + 0.5; - public static final double BACK_RIGHT_ROT_OFFSET = 0.60595703125 + 0.5; - } - - public static final class IDs { - 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 = 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 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 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 double APRIL_HEIGHT = -1.0; // TODO: find actual value - - public static final Translation2d RedSpeakerCenter = new Translation2d(7.914259, -3.221609); - public static final Translation2d BlueSpeakerCenter = new Translation2d(-8.308975, 1.442593); - - public static final double SpeakerBubbleDistance = 3; - public static final double targetPosDistance = 1.5; - - } - - 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 ShooterConstants { - public static final int LEFT_SHOOTER_ID = 15; // final - public static final int RIGHT_SHOOTER_ID = 16; // final - public static final double SHOOTER_SPEED = 0.50; // final - public static final double SHOOTER_IDLE = 0.20; // final - public static final double SHOOTER_IDLE_LIMELIGHT = 0.20; - } - - public static final class IntakeConstants { - public static final int INTAKE_MOTOR_ID = 18; - public static final int PIVOT_MOTOR_ID = 17; - public static final double INTAKE_SPEED = 0.75; - public static final double INTAKE_OUT_SPEED_UNPRESSED = 1.0; - public static final double INTAKE_OUT_SPEED_PRESSED = 0.5; - public static final double HANDOFF_SPEED = 0.4; - public static final double PIVOT_SPEED = 0.2; - - public static final class ArmPID { - public static final Gains INTAKE_GAINS = new Gains(0.05, 0, 0, 0, 0, 1.0); - } - } - - public static final class ClimbConstants { - public static final int CLIMB_MOTOR_ID = 19; - public static final double CLIMB_IN_SPEED = -1.0; - public static final double CLIMB_OUT_SPEED = 0.87; - } - - 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.1; - - } -} diff --git a/src/main/java/frc4388/robot/Main.java b/src/main/java/frc4388/robot/Main.java index ad2d494..b2e3ea4 100644 --- a/src/main/java/frc4388/robot/Main.java +++ b/src/main/java/frc4388/robot/Main.java @@ -25,5 +25,6 @@ public final class Main { */ public static void main(String... args) { RobotBase.startRobot(Robot::new); + } } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 58adaea..a286a66 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,11 +7,28 @@ package frc4388.robot; -import edu.wpi.first.wpilibj.TimedRobot; +// 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 com.ctre.phoenix6.SignalLogger; + +import edu.wpi.first.wpilibj.GenericHID.RumbleType; 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.HubShiftTimer; +import frc4388.utility.compute.HubShiftTimer.ShiftInfo; +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 @@ -20,7 +37,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(); @@ -32,12 +49,23 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { + // Start logging with AdvantageKit + startLogging(); + + com.revrobotics.util.StatusLogger.disableAutoLogging(); + SignalLogger.enableAutoLogging(false); // 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, @@ -49,15 +77,13 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { m_robotTime.updateTimes(); - //System.out.println(m_robotContainer.limelight.isNearSpeaker()); - //mled.updateLED(); // 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 @@ -96,6 +122,7 @@ public class Robot extends TimedRobot { m_autonomousCommand.schedule(); } m_robotTime.startMatchTime(); + HubShiftTimer.initializeAuto(); } /** @@ -105,16 +132,23 @@ 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(); + HubShiftTimer.initializeTeleop(); } /** @@ -122,13 +156,121 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - // m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft()); + var info = HubShiftTimer.getShiftInfo(); + + double rumble = (info.remainingInShift() < 5. && info.remainingInShift() > 0.1) ? 1 : 0; + + // m_robotContainer.getDeadbandedDriverController().setRumble(RumbleType.kBothRumble, rumble); + // m_robotContainer.getDeadbandedOperatorController().setRumble(RumbleType.kBothRumble, rumble); } /** - * 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 10caf89..fbb663b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -5,303 +5,306 @@ /* the project. */ /*----------------------------------------------------------------------------*/ +// 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; -// Drive Systems -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.wpilibj.GenericHID; -import frc4388.utility.controller.XboxController; -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; +import java.io.File; +import com.pathplanner.lib.commands.PathPlannerAuto; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import 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; // Commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -// Autos -import frc4388.robot.commands.Intake.ArmIntakeIn; -import frc4388.utility.controller.VirtualController; -import frc4388.robot.commands.Swerve.neoJoystickPlayback; -import frc4388.robot.commands.Swerve.neoJoystickRecorder; - -// Subsystems -import frc4388.robot.subsystems.Climber; -import frc4388.robot.subsystems.Intake; -import frc4388.robot.subsystems.Limelight; -import frc4388.robot.subsystems.Shooter; -// import frc4388.robot.subsystems.LED; -import frc4388.robot.subsystems.SwerveDrive; - -// Utilites +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc4388.robot.commands.Swerve.StayInPosition; +import frc4388.robot.constants.Constants; +import frc4388.robot.constants.Constants.OIConstants; +import frc4388.robot.constants.Constants.SimConstants.Mode; +import frc4388.robot.subsystems.swerve.SimpleSwerveSim; +import frc4388.robot.subsystems.swerve.SwerveDrive; +import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; -import frc4388.utility.configurable.ConfigurableString; +import frc4388.utility.compute.FieldPositions; +import frc4388.utility.compute.TimesNegativeOne; +import frc4388.utility.controller.DeadbandedXboxController; +// Autos +import frc4388.utility.controller.VirtualController; +import frc4388.utility.controller.XboxController; /** * 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 = new RobotMap(RobotBase.isReal() ? Mode.REAL : Mode.SIM); + + /*Limit Switch */ + // public final DigitalInput m_armLimitSwitch = new DigitalInput(9); + /* Subsystems */ - // private final LED m_robotLED = new LED(); - - private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); - - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, - m_robotMap.rightFront, - m_robotMap.leftBack, - m_robotMap.rightBack, - - m_robotMap.gyro); + // public final Lidar m_lidar = new Lidar(); + // public final LED m_robotLED = new LED(Constants.LEDConstants.LED_SPARK_ID); + public final SimpleSwerveSim m_robotSwerveSIM = new SimpleSwerveSim(); + //Testing of Colors + public final Vision m_vision = new Vision(); + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); + // public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO, m_robotSwerveDrive); + // public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO, m_robotSwerveDrive, m_robotIntake, m_robotLED); + /* 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); + + // public List subsystems = new ArrayList<>(); + private final StayInPosition m_stayInPosition = new StayInPosition(m_robotSwerveDrive); - private final Limelight limelight = new Limelight(); - - private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter, limelight); - - private final Climber m_robotClimber = new Climber(m_robotMap.climbMotor); - - /* Virtual Controllers */ - private final VirtualController m_virtualDriver = new VirtualController(0); - private final VirtualController m_virtualOperator = new VirtualController(1); - - private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); - private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter); - - // ! Teleop Commands - - private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.PIDIn()), - new InstantCommand(() -> m_robotShooter.idle()) - ); + private Pose2d currentPose = new Pose2d(0, 0, new Rotation2d()); + // ! Teleop Commands + public void stop() { + new InstantCommand(()->{}, m_robotSwerveDrive).schedule(); + m_robotSwerveDrive.stopModules(); + Constants.AutoConstants.Y_OFFSET_TRIM.set(0); + } - private SequentialCommandGroup intakeNotePullInIdle = new SequentialCommandGroup( - intakeToShootStuff, intakeToShoot, - new InstantCommand(() -> m_robotShooter.idle()) - ); + // ! /* Autos */ + private SendableChooser autoChooser; + private Command autoCommand; + - private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter) - // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) - ); - - private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup( - interrupt, - new InstantCommand(() -> m_robotIntake.PIDIn(), m_robotIntake), - new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake) - ); - - // ! /* Autos */ - private String lastAutoName = "four_note_taxi_kracken.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); + - private neoJoystickPlayback ampShoot = new neoJoystickPlayback(m_robotSwerveDrive, "Amp_shoot.auto", - new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, - false, true); - - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer() { - configureButtonBindings(); - configureVirtualButtonBindings(); - new DeferredBlock(() -> m_robotSwerveDrive.resetGyroFlip()); - 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 - m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), - getDeadbandedDriverController().getRight(), - true); - }, m_robotSwerveDrive) - .withName("SwerveDrive DefaultCommand")); - m_robotSwerveDrive.setToSlow(); - - // ! Swerve Drive One Module Test - // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - // m_robotMap.rightFront.go(getDeadbandedDriverController().getLeft()); - // } - - // ! Swerve Drive Default Command (Orientation Rotation) - // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - // m_robotSwerveDrive.driveWithInputOrientation(getDeadbandedDriverController().getLeft(), - // getDeadbandedDriverController().getRightX(), - // getDeadbandedDriverController().getRightY(), - // true); - // }, m_robotSwerveDrive)) - // .withName("SwerveDrive OrientationCommand")); - // continually sends updates to the Blinkin LED controller to keep the lights on - // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - - // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - // m_robotSwerveDrive.driveWithInput( - // getDeadbandedDriverController().getLeft(), - // getDeadbandedDriverController().getRight(), - // true); - // }, m_robotSwerveDrive)); - - - - - } - - /** - * Use this method to define your button->command mappings. Buttons can be - * created by instantiating a {@link GenericHID} or one of its subclasses - * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then - * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - - // ? /* Driver Buttons */ - - DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip())); + + public RobotContainer() { - // ! /* Speed */ + configureSINGLEBindings(); + + // Called on first robot enable + DeferredBlock.addBlock(() -> { + m_robotSwerveDrive.resetGyro(); + }, false); + + // Called on every robot enable + DeferredBlock.addBlock(() -> { + // m_robotIntake.setMode(IntakeMode.Idle); + // m_robotShooter.spinUpIdle(); + // m_robotIntake.io.updateGains(); + TimesNegativeOne.update(); + FieldPositions.update(); + // m_robotShooter.io.updateGains(); + }, true); + + DriverStation.silenceJoystickConnectionWarning(true); + + // Drive normally + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + m_robotSwerveDrive.driveWithInput( + getDeadbandedDriverController().getLeft(), + getDeadbandedDriverController().getRight(),true); + + }, m_robotSwerveDrive) + .withName("SwerveDrive DefaultCommand")); + + m_robotSwerveDrive.setToSlow(); + + makeAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); + + } + + + + private boolean lt_down() { + return getDeadbandedDriverController().getLeftTriggerAxis() > 0.8; + } + + private boolean rt_down() { + return getDeadbandedDriverController().getRightTriggerAxis() > 0.8; + } + + private void configureSINGLEBindings() { + + String controllerInstructions = "" + + "Single Controller:\n" + + + // Driver controls. + "- Sticks: Field oriented controls\n" + + "- Right Bumper: Shift Up\n" + + "- Left Bumper: Shift Down\n" + + "- BACK (left small btn): Reset Gyro\n" + + "- DPAD: Fine Alignment\n" + + + // Operator normal buttons + "- X (press): Roller Down\n" + + "- X (hold): Roller Spin\n" + + "- B (hold): Roller Expel + Arm Stop\n" + + "- Y : Roller Off + Arm Up \n" + + + // Operator override buttons + "- LT (hold): Switch over to OP override mode\n" + + "- LS Left+Right Override: Manually move intake\n" + + "- RS Up+Down Override: Manually move climber"; + + SmartDashboard.putString("Controller Binds", controllerInstructions); + + // Driver controls + new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); + 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 */ - - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.PIDIn())) - .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); - - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.PIDOut())) - .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); - - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.handoff())) - .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors())); - - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON) - .onTrue(emergencyRetract); - - - // Override Intake Position encoder: out - new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-55), m_robotIntake)); - - // Override Intake Position encoder: in - new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-6.2), m_robotIntake)); - - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotShooter.spin(0.5), m_robotShooter)) - .onFalse(turnOffShoot); - - - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(intakeNotePullInIdle) - .onFalse(new InstantCommand(() -> m_robotIntake.PIDIn())); - - // Spins up shooter, no wind down - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)); - - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) - .onTrue(emergencyRetract); - - new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5) - .onTrue(new InstantCommand(() -> m_robotClimber.climbOut())) - .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); - - new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5) - .onTrue(new InstantCommand(() -> m_robotClimber.climbIn())) - .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); - - // new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) - // .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5))); + // Fine Alignment + new Trigger(() -> getDeadbandedDriverController().getPOV() != -1) + .whileTrue(new RunCommand( + () -> m_robotSwerveDrive.driveFine( + new Translation2d( + 1, + Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV()) + ), + getDeadbandedDriverController().getRight(), 0.15 + ), m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); - new Trigger(() -> getDeadbandedOperatorController().getPOV() != -1) - .onTrue(ampShoot) - .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter, m_robotSwerveDrive)); - // ? /* Programer Buttons (Controller 3)*/ - // * /* Auto Recording */ - new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) - .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, - new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, - () -> autoplaybackName.get())) - .onFalse(new InstantCommand()); - new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, - () -> autoplaybackName.get(), - new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, - true, false)) - .onFalse(new InstantCommand()); - } - - /** - * This method is used to replcate {@link Trigger Triggers} for {@link VirtualController Virtual Controllers}.

- * Please use {@link RobotContainer#DualJoystickButton} in {@link RobotContainer#configureButtonBindings} for standard buttons. - */ - private void configureVirtualButtonBindings() { + // Operator controls (Non-override, LT UP) - // ? /* Driver Buttons */ + + // Arm down + new Trigger(() -> !lt_down() && getDeadbandedDriverController().getXButton()) + .onTrue(new InstantCommand(() -> { + m_robotMap.m_robotIntake.PIDIn(); + m_robotMap.m_robotIntake.spinIntakeMotor(); + }, m_robotMap.m_robotIntake)) + .onFalse(new InstantCommand(() -> m_robotMap.m_robotIntake.stopArmMotor(), m_robotMap.m_robotIntake)); - /* Notice: the following buttons have not been replicated - * Swerve Drive Slow and Fast mode Gear Shifts : Fast mode is known to cause drift, so we disable that feature in Autoplayback - * Swerve Drive Rotation Gear Shifts : Same reason as Slow and Fast mode. - * Auto Recording controls : We don't want an Null Ouroboros for an auto. - */ - - // ? /* Operator Buttons */ - - /* Notice: the following buttons have not been replicated - * Override Intake Position Encoder : It's an emergancy overide, for when the position of intake when the robot boots, the intake is not inside the robot. - * We don't need it in an auto. - * Climbing controls : We don't need to climb in auto. - */ + // Arm up + new Trigger(() -> !lt_down() && getDeadbandedDriverController().getYButton()) + .onTrue(new InstantCommand(() -> { + m_robotMap.m_robotIntake.PIDIn(); + m_robotMap.m_robotIntake.stopIntakeMotors(); + }, m_robotMap.m_robotIntake)) + .onFalse(new InstantCommand(() -> m_robotMap.m_robotIntake.stopArmMotor(), m_robotMap.m_robotIntake)); + + // Handoff / spit out + new Trigger(() -> !lt_down() && getDeadbandedDriverController().getAButton()) + .onTrue(new InstantCommand(() -> { + m_robotMap.m_robotIntake.PIDIn(); + m_robotMap.m_robotIntake.stopIntakeMotors(); + }, m_robotMap.m_robotIntake)) + .onFalse(new InstantCommand(() -> m_robotMap.m_robotIntake.stopArmMotor(), m_robotMap.m_robotIntake)); + + // Shoot + new Trigger(() -> !lt_down() && rt_down()) + .onTrue(new InstantCommand(() -> m_robotMap.m_robotShooter.spin(0.5), m_robotMap.m_robotShooter)) + .onFalse(new InstantCommand(() -> m_robotMap.m_robotShooter.stop(), m_robotMap.m_robotShooter)); + + + + // OP Override - // ? Notice: the Programer Buttons are not to be replicated because they are designed for debuging the robot, and do not need to be replicated in auto. } +//.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT))); + /** * Use this to pass the autonomous command to the main {@link Robot} class. * * @return the command to run in autonomous */ public Command getAutonomousCommand() { - //no auto - return autoPlayback; - //return playbackChooser.getCommand(); - // return new Command() {}; + + + //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(); + autoChooser.setDefaultOption("None", "None"); + 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("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2026KPopRobotHunters\\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 == null || filename.equals("None")) { + autoCommand = null; + return; + } + // 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); + + //---- + PathPlannerAuto auto = new PathPlannerAuto(filename); + m_robotSwerveDrive.setInitalPose(auto.getStartingPose()); + //----- + }); + SmartDashboard.putData(autoChooser); + } /** @@ -322,11 +325,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; + // } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 4d1869f..6c26de6 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -8,79 +8,219 @@ package frc4388.robot; import com.ctre.phoenix6.hardware.TalonFX; -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.ShooterConstants; -import frc4388.robot.Constants.ClimbConstants; -import frc4388.robot.Constants.IntakeConstants; -import frc4388.robot.subsystems.SwerveModule; -import frc4388.utility.RobotGyro; +import org.photonvision.PhotonCamera; + +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import com.revrobotics.spark.SparkMax; + +import edu.wpi.first.wpilibj.DigitalInput; +import frc4388.robot.constants.ClimbConstants; +import frc4388.robot.constants.Constants; +//import frc4388.robot.constants.Constants.ElevatorConstants; +import frc4388.robot.constants.Constants.SimConstants; +import frc4388.robot.constants.IntakeConstants; +import frc4388.robot.constants.ShooterConstants; +import frc4388.robot.subsystems.Climber; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.Shooter; +// import frc4388.robot.constants.Constants.VisionConstants; +// import frc4388.robot.subsystems.intake.IntakeConstants; +// import frc4388.robot.subsystems.intake.IntakeIO; +// import frc4388.robot.subsystems.intake.IntakeReal; +// import frc4388.robot.subsystems.shooter.ShooterConstants; +// import frc4388.robot.subsystems.shooter.ShooterIO; +// import frc4388.robot.subsystems.shooter.ShooterReal; +import frc4388.robot.subsystems.swerve.SimpleSwerveSim; +// import frc4388.robot.subsystems.elevator.ElevatorIO; +// import frc4388.robot.subsystems.elevator.ElevatorReal; +import frc4388.robot.subsystems.swerve.SwerveDriveConstants; +import frc4388.robot.subsystems.swerve.SwerveIO; +import frc4388.robot.subsystems.swerve.SwerveReal; +import frc4388.robot.subsystems.vision.VisionIO; +import frc4388.robot.subsystems.vision.VisionReal; +import frc4388.utility.compute.JankCoder; +import frc4388.utility.status.FaultCANCoder; +import frc4388.utility.status.FaultPhotonCamera; +import frc4388.utility.status.FaultPidgeon2; +import frc4388.utility.status.FaultSparkMax; +import frc4388.utility.status.FaultTalonFX; /** * Defines and holds all I/O objects on the Roborio. This is useful for unit * testing and modularization. */ public class RobotMap { - private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON_ID); - public RobotGyro gyro = new RobotGyro(m_pigeon2); - - public 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 final VisionIO leftCamera; + // public final VisionIO rightCamera; + + // public final LiDAR lidar = new + + // public final LidarIO reefLidar; + // public final LidarIO reverseLidar; - public RobotMap() { - configureLEDMotorControllers(); - configureDriveMotorControllers(); - } /* 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 SwerveIO swerveDrivetrain; - 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); + public final Intake m_robotIntake; + public final Shooter m_robotShooter; + public final Climber m_robotClimber; - /* Shooter Subsystem */ - public final TalonFX leftShooter = new TalonFX(ShooterConstants.LEFT_SHOOTER_ID); - public final TalonFX rightShooter = new TalonFX(ShooterConstants.RIGHT_SHOOTER_ID); - /* Intake Subsystem */ - public final TalonFX intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID); - public final TalonFX pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID); + public RobotMap(SimConstants.Mode mode) { + + /* Shooter Subsystem */ + final TalonFX leftShooter = new TalonFX(ShooterConstants.LEFT_SHOOTER_ID.id); + final TalonFX rightShooter = new TalonFX(ShooterConstants.RIGHT_SHOOTER_ID.id); - /* Climber Subsystem */ - public final TalonFX climbMotor = new TalonFX(ClimbConstants.CLIMB_MOTOR_ID); + /* Intake Subsystem */ + final TalonFX intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID.id); + final TalonFX pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID.id); - void configureLEDMotorControllers() { + /* Climber Subsystem */ + final TalonFX climbMotor = new TalonFX(ClimbConstants.CLIMB_MOTOR_ID.id); + + m_robotIntake = new Intake(intakeMotor, pivotMotor); + m_robotShooter = new Shooter(leftShooter, rightShooter); + m_robotClimber = new Climber(climbMotor); + + FaultTalonFX.addDevice(leftShooter, "Left Shooter"); + FaultTalonFX.addDevice(rightShooter, "Right Shooter"); + FaultTalonFX.addDevice(intakeMotor, "Intake Motor"); + FaultTalonFX.addDevice(pivotMotor, "Pivot Motor"); + FaultTalonFX.addDevice(climbMotor, "Climb Motor"); + + + switch (mode) { + case REAL: + // // Configure cameras + // PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); + // PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); + + // leftCamera = new VisionReal(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ; + // rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS); + + // FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera"); + // FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera"); + + // // Configure LiDAR + // reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL); + // reverseLidar = new LidarReal(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL); + // DigitalInput armLimitSwitch = new DigitalInput(IntakeConstants.ARM_LIMIT_SWITCH_CHANNEL); + + // Configure swerve drive train + SwerveDrivetrain swerveDrivetrainReal = new SwerveDrivetrain (TalonFX::new, TalonFX::new, CANcoder::new, + SwerveDriveConstants.DrivetrainConstants, + SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT, + SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT + ); + + swerveDrivetrain = new SwerveReal(swerveDrivetrainReal); + + // Configure Shooter 22,23,24 + // TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id, Constants.CANIVORE_CANBUS); + // TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id, Constants.CANIVORE_CANBUS); + // TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id, Constants.CANIVORE_CANBUS); + + //Configure Intake 20,21 + // SparkMax arm = new SparkMax(IntakeConstants.ARM_ID.id, com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless); + // TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id, Constants.RIO_CANBUS); + // DigitalInput armLimitSwitch = new DigitalInput(IntakeConstants.ARM_LIMIT_SWITCH_CHANNEL); + // DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); + // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); + // DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); + + // shooterIO = new ShooterReal(shooter1, shooter2, indexer); + // JankCoder armEncoder = new JankCoder(0, IntakeConstants.ARM_ENCODER_OFFSET); + + // intakeIO = new IntakeReal(armLimitSwitch, arm, roller, armEncoder); + // Fault + FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); + + + // FaultTalonFX.addDevice(shooter1, "Shooter1"); + // FaultTalonFX.addDevice(shooter2, "Shooter2"); + // FaultTalonFX.addDevice(indexer, "Indexer"); + // FaultSparkMax.addDevice(arm, "Arm"); + // FaultTalonFX.addDevice(roller, "Roller"); + + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getDriveMotor(), "Module 1 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getSteerMotor(), "Module 1 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(1).getEncoder(), "Module 1 CANCoder"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getDriveMotor(), "Module 2 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getSteerMotor(), "Module 2 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(2).getEncoder(), "Module 2 CANCoder"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getDriveMotor(), "Module 3 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getSteerMotor(), "Module 3 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder"); + + break; + case SIM: + // leftCamera = new VisionIO() {}; + // rightCamera = new VisionIO() {}; + + swerveDrivetrain = new SimpleSwerveSim() {}; + + // shooterIO = new ShooterIO() {}; + // intakeIO = new IntakeIO() {}; + break; + default: + // leftCamera = new VisionIO() {}; + // rightCamera = new VisionIO() {}; + swerveDrivetrain = new SwerveIO() {}; + // intakeIO = new IntakeIO() {}; + // shooterIO = new ShooterIO() {}; + break; + } } - void configureIntakeMotorControllers() { - } + // public class RobotMapSim { + // public PhotonCameraSim leftCamera; + // public PhotonCameraSim rightCamera; + // } - 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); - } -} + // public RobotMapSim configureSim() { + // RobotMapSim sim = new RobotMapSim(); + + // // The simulated camera properties + // SimCameraProperties cameraProp = new SimCameraProperties(); + // // A 640 x 480 camera with a 100 degree diagonal FOV. + // cameraProp.setCalibration(640, 480, Rotation2d.fromDegrees(100)); + // // Approximate detection noise with average and standard deviation error in pixels. + // cameraProp.setCalibError(0.25, 0.08); + // // Set the camera image capture framerate (Note: this is limited by robot loop rate). + // cameraProp.setFPS(20); + // // The average and standard deviation in milliseconds of image data latency. + // cameraProp.setAvgLatencyMs(35); + // cameraProp.setLatencyStdDevMs(5); + + // // sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp); + // // sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp); + + + // sim.leftCamera.enableRawStream(true); + // sim.leftCamera.enableProcessedStream(true); + // sim.leftCamera.enableDrawWireframe(true); + + + // sim.rightCamera.enableRawStream(true); + // sim.rightCamera.enableProcessedStream(true); + // sim.rightCamera.enableDrawWireframe(true); + + // return sim; + + // } + +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java deleted file mode 100644 index 58eb3ed..0000000 --- a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java +++ /dev/null @@ -1,208 +0,0 @@ -// package frc4388.robot.commands.Autos; -// import frc4388.robot.subsystems.Limelight; -// import frc4388.robot.subsystems.SwerveDrive; -// import frc4388.robot.Constants.AutoAlignConstants; -// import frc4388.robot.Constants.VisionConstants; -// import edu.wpi.first.wpilibj2.command.Command; - -// import edu.wpi.first.math.geometry.Translation2d; -// import edu.wpi.first.math.geometry.Rotation2d; - -// import java.util.Optional; - -// import org.opencv.core.RotatedRect; - -// import edu.wpi.first.math.geometry.Pose2d; - -// import edu.wpi.first.wpilibj.DriverStation; -// import edu.wpi.first.wpilibj.DriverStation.Alliance; - -// public class AutoAlign extends Command { -// private SwerveDrive swerve; -// private Limelight limelight; -// private Pose2d pose; -// private Translation2d targetPos; -// private Rotation2d targetRot; - -// private Optional alliance; -// private boolean isRed; - -// private boolean isFinished; -// private boolean isReverseFinished; - -// private boolean reverseAfterFinish; -// private Translation2d[] moveStickReplayArr; -// private Translation2d[] rotStickReplayArr; -// private int replayIndex; - -// // PID Stuff -// private double prevError; -// private double cumError; - -// public AutoAlign(SwerveDrive swerve, Limelight limelight) { -// this.swerve = swerve; -// this.limelight = limelight; -// } - -// // Calc the closest point on a circle, to the center of the speaker -// private Translation2d calcTargetPos(){ -// final double R = VisionConstants.targetPosDistance; -// final double cX; -// final double cY; -// if(isRed){ -// cX = VisionConstants.RedSpeakerCenter.getX(); -// cY = VisionConstants.RedSpeakerCenter.getY(); -// }else{ -// cX = VisionConstants.BlueSpeakerCenter.getX(); -// cY = VisionConstants.BlueSpeakerCenter.getY(); -// } -// final double pX = pose.getX(); -// final double pY = pose.getY(); - -// // Code is from https://stackoverflow.com/questions/300871/best-way-to-find-a-point-on-a-circle-closest-to-a-given-point -// double vX = pX - cX; -// double vY = pY - cY; -// double magV = Math.sqrt(vX*vX + vY*vY); -// double aX = cX + vX / magV * R; -// double aY = cY + vY / magV * R; - -// return new Translation2d(aX, aY); -// } - -// // Calculate the angle facing the center, at the target rot -// private Rotation2d calcTargetRot() { -// final double R = VisionConstants.targetPosDistance; -// final double cX; -// final double cY; -// if(isRed){ -// cX = VisionConstants.RedSpeakerCenter.getX(); -// cY = VisionConstants.RedSpeakerCenter.getY(); -// }else{ -// cX = VisionConstants.BlueSpeakerCenter.getX(); -// cY = VisionConstants.BlueSpeakerCenter.getY(); -// } -// final double pX = pose.getX(); -// final double pY = pose.getY(); - -// final double dX = pX - cX; -// final double dY = pY - cY; - -// final double yaw = ((Math.atan2(dX, dY)*360/Math.PI) % 360); - -// return Rotation2d.fromDegrees(yaw); -// } - -// // Clamp to a circle, like an xbox controller -// private Translation2d clamp(double oldX, double oldY){ -// // Code is from https://stackoverflow.com/questions/74329985/how-can-i-clamp-a-position-to-a-circley -// final double alpha = (Math.atan2(oldX, -oldY) * 180 / Math.PI + 360) % 360; -// final double maxX = Math.abs(Math.cos(alpha / 180 * Math.PI)); -// final double maxY = Math.abs(Math.sin(alpha / 180 * Math.PI)); - -// final double newX = Math.max(-maxX, Math.min(maxX, oldX)); -// final double newY = Math.max(-maxY, Math.min(maxY, oldY)); - -// return new Translation2d(newX, newY); -// } - -// private Translation2d calcMoveStick(){ -// final double curX = pose.getX(); -// final double curY = pose.getY(); - -// // I think this might work, assuming the constants are good -// double stickX = -(curX - targetPos.getX()) * AutoAlignConstants.MoveSpeed; -// double stickY = -(curY - targetPos.getY()) * AutoAlignConstants.MoveSpeed; - -// return clamp(stickX, stickY); -// } - -// private Translation2d calcRotStick(){ -// double error = pose.getRotation().getDegrees() - targetRot.getDegrees(); -// cumError += error * .02; // 20 ms -// double delta = error - prevError; - -// final double kP = 4; -// final double kI = 4; -// final double kD = 4; -// final double kF = 4; - -// double output = error * kP; -// output += cumError * kI; -// output += delta * kD; -// output += kF; - -// prevError = error; -// return clamp(output, 0); -// } - -// public void reverse() { -// this.reverseAfterFinish = true; -// } - -// // Called when the command is initially scheduled. -// @Override -// public final void initialize() { -// isRed = alliance.get() == DriverStation.Alliance.Red; -// if(reverseAfterFinish){ -// // isReverseFinished = false; -// replayIndex = 0; -// }else{ -// moveStickReplayArr = new Translation2d[]{}; -// rotStickReplayArr = new Translation2d[]{}; -// } -// } - -// // Called every time the scheduler runs while the command is scheduled. -// @Override -// public void execute() { -// // Update limelight pos -// pose = limelight.getPose(); - -// // These must be 0, or it will error -// Translation2d moveStick = new Translation2d(0, 0); -// Translation2d rotStick = new Translation2d(0, 0); - -// // Regular replay -// if(!isFinished){ -// targetPos = calcTargetPos(); -// targetRot = calcTargetRot(); - -// moveStick = calcMoveStick(); -// rotStick = calcRotStick(); - -// // This is a way of appending... -// moveStickReplayArr[moveStickReplayArr.length] = moveStick; -// rotStickReplayArr[rotStickReplayArr.length] = rotStick; - -// // if(isFinished != limelight.isNearSpeaker() && isReverseFinished){ -// // replayIndex -// // } -// isFinished = limelight.isNearSpeaker(); - -// // If reverseAfterFinish, then loop back over and replay in reverse -// }else if(reverseAfterFinish && !isReverseFinished){ -// // Get reverse direction -// moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1]; -// rotStick = rotStickReplayArr[replayIndex-rotStickReplayArr.length-1]; - -// // Invert sticks -// moveStick = new Translation2d(moveStick.getX()*-1, moveStick.getY()*-1); -// rotStick = new Translation2d(rotStick.getX()*-1, rotStick.getY()*-1); - -// replayIndex++; - -// if(replayIndex >= moveStickReplayArr.length){ -// reverseAfterFinish = true; -// } -// } - -// // This would greatly benifit from having feild Relative implemented. -// swerve.driveWithInput(moveStick, rotStick, true); -// } - -// // Returns true when the command should end. -// @Override -// public final boolean isFinished() { -// return isFinished && (isReverseFinished || !reverseAfterFinish); -// } -// } \ 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/neo AutoRecoding format.txt b/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt deleted file mode 100644 index a65aea9..0000000 --- a/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt +++ /dev/null @@ -1,20 +0,0 @@ -AUTO file format - -HEADER static size 0x5 -0x00 BYTE NUM AXES: defualts to 6 -0x01 BYTE NUM POV: defualts to 1 -0x02 BYTE NUM CONTROLLERS: defualts to 2 -0x03 SHORT FRAMES: any value greator or equal than one. - -FRAME PER CONTROLLER: defualt size 0x34 -0x00 DOUBLE AXES[NUM AXES] -0x30 SHORT BUTTONS -0x32 SHORT POVs[NUM POV] - -FRAME: size varrys -FRAME PER CONTROLLER[NUM CONTROLLERS] -INT UNIXTIMESTAMP - -FILE: -HEADER -FRAME[FRAMES] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java deleted file mode 100644 index 86bc7b2..0000000 --- a/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java +++ /dev/null @@ -1,107 +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.commands.Swerve.neoJoystickPlayback; -// import frc4388.robot.subsystems.SwerveDrive; -// import frc4388.utility.controller.VirtualController; - -// public class neoPlaybackChooser { -// private final SendableChooser m_teamChosser = new SendableChooser(); -// private final SendableChooser m_possitionChosser = new SendableChooser(); -// private final SendableChooser m_autoNameChosser = new SendableChooser(); - -// private final SwerveDrive m_swerve; -// private final VirtualController[] m_controllers; -// // private final ArrayList> m_choosers = new ArrayList<>(); -// // private SendableChooser m_playback = null; -// private final ArrayList m_widgets = new ArrayList<>(); -// // private final HashMap m_commandPool = new HashMap<>(); - -// // private final File m_dir = new File("/home/lvuser/autos/"); -// // private int m_cmdNum = 0; - -// // commands -// private Command m_noAuto = new InstantCommand(); - -// public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) { -// m_teamChosser.addOption("Red", "red"); -// m_teamChosser.setDefaultOption("Blue", "blue"); -// m_teamChosser.addOption("Nuetral", "nuetral"); -// m_possitionChosser.addOption("AMP", "amp"); -// m_possitionChosser.setDefaultOption("Center", "center"); -// m_possitionChosser.addOption("Source", "source"); -// m_swerve = swerve; -// m_controllers = controllers; -// } - -// public neoPlaybackChooser addOption(String name, String option) { -// m_autoNameChosser.addOption(name, option); -// return this; -// } - -// // public PlaybackChooser buildDisplay() { -// // for (int i = 0; i < 10; i++) { -// // appendCommand(); -// // } -// // m_playback = m_choosers.get(0); -// // nextChooser(); - -// // // ! This does not work, why? -// // Shuffleboard.getTab("Auto Chooser") -// // .add("Add Sequence", new InstantCommand(() -> nextChooser())) -// // .withPosition(4, 0); -// // return this; -// // } - -// // This will be bound to a button for the time being -// public void render() { -// // var chooser = new SendableChooser(); -// // // chooser.setDefaultOption("No Auto", m_noAuto); - -// // m_choosers.add(chooser); -// ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser") -// .add("Command: " + m_choosers.size(), chooser) -// .withSize(4, 1) -// .withPosition(0, m_choosers.size() - 1) -// .withWidget(BuiltInWidgets.kSplitButtonChooser) -// .withWidget(BuiltInWidgets.kComboBoxChooser); - - -// m_widgets.add(widget); -// } - -// // public void nextChooser() { -// // SendableChooser chooser = m_choosers.get(m_cmdNum++); - -// // String[] dirs = m_dir.list(); - -// // if(dirs != null){ // Fix funny error -// // for (String auto : dirs) { -// // chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); -// // } -// // } - - -// // for (var cmd_name : m_commandPool.keySet()) { -// // chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); -// // } -// // } - -// public String autoName() { -// return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto"; -// } - -// public Command getCommand() { -// return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true); -// } -// } diff --git a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java deleted file mode 100644 index 5fb161a..0000000 --- a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java +++ /dev/null @@ -1,52 +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.Intake; - -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.Intake; -import frc4388.robot.subsystems.Shooter; - -public class ArmIntakeIn extends Command { - /** Creates a new ArmIntakeIn. */ - private Intake robotIntake; - private Shooter robotShooter; - - public ArmIntakeIn(Intake robotIntake, Shooter robotShooter) { - // Use addRequirements() here to declare subsystem dependencies. - this.robotIntake = robotIntake; - this.robotShooter = robotShooter; - - addRequirements(this.robotIntake, this.robotShooter); - } - - // 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() { - robotIntake.PIDOut(); - robotIntake.spinIntakeMotor(); - } - - // 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 robotIntake.getIntakeLimitSwitchState(); - // if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0))) - // { - // return !true==true; - // } - // else - // { - // return !false==!(!(true)); - // } - } -} 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 deleted file mode 100644 index a2945c0..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java +++ /dev/null @@ -1,35 +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 edu.wpi.first.math.geometry.Translation2d; -import frc4388.robot.commands.PID; -import frc4388.robot.subsystems.SwerveDrive; - -public class RotateToAngle extends PID { - - SwerveDrive drive; - double targetAngle; - - /** Creates a new RotateToAngle. */ - public RotateToAngle(SwerveDrive drive, double targetAngle) { - super(0.3, 0.0, 0.0, 0.0, 1); - - this.drive = drive; - this.targetAngle = targetAngle; - - addRequirements(drive); - } - - @Override - public double getError() { - return targetAngle - drive.getGyroAngle(); - } - - @Override - public void runWithOutput(double output) { - drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true); - } -} diff --git a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java new file mode 100644 index 0000000..7b826a7 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java @@ -0,0 +1,54 @@ +// 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 org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.AutoLogOutput; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.commands.PID; +import frc4388.robot.subsystems.swerve.SwerveDrive; + +public class StayInPosition extends PID { + SwerveDrive drive; + Translation2d driveTranslation = new Translation2d(); + + public StayInPosition(SwerveDrive drive) { + super(0.3, 0.0, 0.0, 0.0, 1); + this.drive = drive; + addRequirements(drive); + } + + public void goToTargetPose(Pose2d targetPose) { + Pose2d currentPose = drive.getCurrentPose(); + double translationX = targetPose.getX() - currentPose.getX(); + double translationY = targetPose.getY() - currentPose.getY(); + if (translationX > 0.2){ + //If below is changed make this change too so it never goes over 1 + translationX = 0.2; + } + if (translationY > 0.2){ + //Same here + translationY = 0.2; + } + if (Math.abs(translationX) < 0.08 && Math.abs(translationY) < 0.08) { + driveTranslation = new Translation2d(); + } else { + //TODO: Tweak for best corrector against another bot + driveTranslation = new Translation2d(translationX * 4.5, translationY * 4.5); + } + + drive.driveFieldAngleSIP(driveTranslation, targetPose.getRotation()); + } + + @Override + public double getError() { + return 0; + } + + @Override + public void runWithOutput(double output) {} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java deleted file mode 100644 index 8b5afdf..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java +++ /dev/null @@ -1,197 +0,0 @@ -package frc4388.robot.commands.Swerve; - -import java.io.FileInputStream; -import java.util.ArrayList; -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.DataUtils; -import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; -import frc4388.utility.UtilityStructs.AutoRecordingFrame; -import frc4388.utility.controller.VirtualController; - - -/** - * The NEO autonomus playback system, designed based the old {@link JoystickPlayback} System but with {@link VirtualController}s - * @author Zachary Wilke - */ -public class neoJoystickPlayback extends Command { - private final SwerveDrive swerve; - private final VirtualController[] controllers; - private final ArrayList frames = new ArrayList<>(); - private final Supplier filenameGetter; - private String filename; - private int frame_index = 0; - private long startTime = 0; - private long playbackTime = 0; - private boolean m_finished = false; // ! There is no better way. - private boolean m_shouldfree = false; // should free memory on ending - - private byte m_numAxes = 0; - private byte m_numPOVs = 0; - private byte m_numControllers = 0; - private short m_numFrames = -1; - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param shouldfree Unloads the auto on compleation or intruption. - * @param instantload Load the auto on object instantiation - */ - public neoJoystickPlayback(SwerveDrive swerve, Supplier filenameGetter, VirtualController[] controllers, boolean shouldfree, boolean instantload) { - this.swerve = swerve; - this.filenameGetter = filenameGetter; - this.controllers = controllers; - this.m_shouldfree = shouldfree; - - if (instantload) loadAuto(); - addRequirements(this.swerve); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filename a String containing the name of the auto file you wish to playback. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param shouldfree unloads the auto on compleation or intruption. - * @param instantload load the auto on object instantiation - */ - public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) { - this(swerve, () -> filename, controllers, shouldfree, instantload); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - */ - public neoJoystickPlayback(SwerveDrive swerve, Supplier filenameGetter, VirtualController[] controllers) { - this(swerve, filenameGetter, controllers, true, false); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filename a String containing the name of the auto file you wish to playback. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - */ - public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) { - this(swerve, () -> filename, controllers, true, false); - } - - /** - * Load the auto file from disk into memory - * @return Returns true if loading was successful, else wise; return false - * @implNote if the auto is already loaded, it will return true. - */ - public boolean loadAuto() { - filename = filenameGetter.get(); - try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) { - if (m_numFrames != -1 && m_numFrames == frames.size()) { - System.out.println("AUTOPLAYBACK: Auto Already loaded."); - return true; - } - - m_numAxes = stream.readNBytes(1)[0]; - m_numPOVs = stream.readNBytes(1)[0]; - m_numControllers = stream.readNBytes(1)[0]; - m_numFrames = DataUtils.byteArrayToShort(stream.readNBytes(2)); - - if (m_numControllers > controllers.length) { - System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers - + " virtual controllers but only " + controllers.length + " were given"); - return false; - } - - for (int i = 0; i < m_numFrames; i++) { - AutoRecordingFrame frame = new AutoRecordingFrame(); - for (int j = 0; j < m_numControllers; j++) { - AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); - double[] axes = new double[m_numAxes]; - for (int k = 0; k < m_numAxes; k++) { // we love third level for loops. - axes[k] = DataUtils.byteArrayToDouble(stream.readNBytes(8)); - } - short button = DataUtils.byteArrayToShort(stream.readNBytes(2)); - short[] POV = new short[m_numPOVs]; - for (int k = 0; k < m_numPOVs; k++) { - POV[k] = DataUtils.byteArrayToShort(stream.readNBytes(2)); - } - controllerFrame.axes = axes; - controllerFrame.button = button; - controllerFrame.POV = POV; - frame.controllerFrames[j] = controllerFrame; - } - frame.timeStamp = DataUtils.byteArrayToInt(stream.readNBytes(4)); - frames.add(frame); - } - - System.out.println("AUTOPLAYBACK: Read Auto `" + filename + "` that is " + m_numFrames + " frames long"); - return true; - - } catch (Exception e) { - e.printStackTrace(); - System.out.println("AUTOPLAYBACK: Unable to read auto file `" + filename + '`'); - return false; - } - } - - /** - * Unloads the auto. - */ - public void unloadAuto() { - System.out.println("AUTOPLAYBACK: Auto unloaded"); - frames.clear(); - } - - @Override - public void initialize() { - startTime = System.currentTimeMillis(); - playbackTime = 0; - frame_index = 0; - - m_finished = !loadAuto(); - } - - @Override - public void execute() { - if (frame_index >= m_numFrames) m_finished = true; - if (m_finished) return; - - // if (frame_index == 0) { - // startTime = System.currentTimeMillis(); - // playbackTime = 0; - // } else { - // playbackTime = System.currentTimeMillis() - startTime; - // } - - AutoRecordingFrame frame = frames.get(frame_index); - for (int i = 0; i < controllers.length; i++) { - AutoRecordingControllerFrame controllerFrame = frame.controllerFrames[i]; - controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV); - if (i == 0) { - this.swerve.driveWithInput( - new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)), - new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)), - true); - } - } - frame_index++; - } - - @Override - public void end(boolean interrupted) { - for (VirtualController controller : controllers) controller.zeroControls(); - swerve.stopModules(); - if (m_shouldfree) unloadAuto(); - } - - @Override - public boolean isFinished() { - return m_finished; - } -} diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java deleted file mode 100644 index 7f48a6c..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java +++ /dev/null @@ -1,129 +0,0 @@ -package frc4388.robot.commands.Swerve; - -import java.io.FileOutputStream; -import java.util.ArrayList; -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.DataUtils; -import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; -import frc4388.utility.UtilityStructs.AutoRecordingFrame; -import frc4388.utility.controller.DeadbandedXboxController; - -/** - * The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s - * @author Zachary Wilke - */ -public class neoJoystickRecorder extends Command { - private final SwerveDrive swerve; - private final XboxController[] controllers; - private String filename; - private final Supplier filenameGetter; - private long startTime = -1; - private final ArrayList frames = new ArrayList<>(); - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. - */ - public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, Supplier filenameGetter) { - this.swerve = swerve; - this.controllers = controllers; - this.filenameGetter = filenameGetter; - this.filename = ""; - - addRequirements(this.swerve); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param filename a String containing the name of the auto file you wish to playback. - */ - public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) { - this(swerve, controllers, () -> filename); - } - - @Override - public void initialize() { - frames.clear(); - - this.startTime = System.currentTimeMillis(); - AutoRecordingFrame frame = new AutoRecordingFrame(); - frame.controllerFrames = new AutoRecordingControllerFrame[] {new AutoRecordingControllerFrame(), new AutoRecordingControllerFrame()}; - frames.add(frame); - this.filename = this.filenameGetter.get(); - } - - - @Override - public void execute() { - System.out.println("AUTORECORD: RECORDING"); - AutoRecordingFrame frame = new AutoRecordingFrame(); - frame.timeStamp = (int) (System.currentTimeMillis() - startTime); - for (int i = 0; i < controllers.length; i++) { - XboxController controller = controllers[i]; - AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); - double[] axes = {controller.getLeftX(), controller.getLeftY(), - controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(), - controller.getRightX(), controller.getRightY()}; - short button = 0; - for (int j = 0; j < 10; j++) - if (controller.getRawButton(j+1)) - button |= 1 << j; - short[] POV = {(short)(controller.getPOV())}; - controllerFrame.axes = axes; - controllerFrame.button = button; - controllerFrame.POV = POV; - frame.controllerFrames[i] = controllerFrame; - } - - frames.add(frame); - - swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]), - new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]), - true); // Really jank way of doing this. - - } - @Override - public void end(boolean interrupted) { - try (FileOutputStream stream = new FileOutputStream("/home/lvuser/autos/" + filename)) { - // header: size of 0x5 - // byte Number of axes per controller - // byte Number of POVs per controller - // byte Number of controllers - // short Number of frames - stream.write(new byte[]{6, 1, (byte) controllers.length}); - stream.write(DataUtils.shortToByteArray((short) frames.size())); - - // frame - // controller frame * number of controllers - // int unix time stamp. - for (AutoRecordingFrame frame : frames) { - // controller frame - // double axis * Number of axes per controller - // short button states - // short POV * Number of POVs per controller - for (AutoRecordingControllerFrame controllerFrame: frame.controllerFrames) { - for (double axis: controllerFrame.axes) { - stream.write(DataUtils.doubleToByteArray(axis)); - } - stream.write(DataUtils.shortToByteArray(controllerFrame.button)); - for (short POV: controllerFrame.POV) { - stream.write(DataUtils.shortToByteArray(POV)); - } - } - stream.write(DataUtils.intToByteArray(frame.timeStamp)); - } - System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long."); - } catch (Exception e) { - e.printStackTrace(); - } - } -} diff --git a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java new file mode 100644 index 0000000..5a1d2d3 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java @@ -0,0 +1,119 @@ +package frc4388.robot.commands.alignment; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.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.Lidar; +import frc4388.robot.subsystems.vision.Vision; +import frc4388.utility.compute.FieldPositions; +import frc4388.utility.structs.Gains; + +public class AutoAlign extends Command { + + private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0); + private Lidar lidar; + private boolean isLidar; + + private Pose2d targetpos; + private double targetRotation; + + SwerveDrive swerveDrive; + Vision vision; + + public AutoAlign(SwerveDrive swerveDrive, Vision vision, Lidar lidar, boolean isLidar) { + this.swerveDrive = swerveDrive; + this.vision = vision; + this.lidar = lidar; + this.isLidar = isLidar; + addRequirements(swerveDrive); + } + + @Override + public void initialize() { + rotPID.initialize(); + this.targetRotation = swerveDrive.getPose2d().getRotation().getDegrees(); + //this.targetpos = new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)); + } + + + double roterr; + + double rotoutput; + + @Override + public void execute() { + double desiredHeading; + // Pose2d robotPose = vision.getPose2d(); + targetpos = new Pose2d(lidar.getClosestBall(), new Rotation2d(0)); + // if (robotPose == null) return; + if (targetpos == null) return; + if (targetpos.getTranslation() == null) return; + + + double dx = targetpos.getX();// - robotPose.getX(); + double dy = targetpos.getY();// - robotPose.getY(); + + if (!isLidar){ + desiredHeading = (Math.atan2(dy, dx)+ Math.PI) * (180. / Math.PI) + 180; + }else{ + desiredHeading = (Math.atan2(dx, dy)) * (180. / Math.PI);// + 180; + } + + + targetRotation = swerveDrive.getPose2d().getRotation().getDegrees() - desiredHeading; + + // roterr = desiredHeading.getDegrees() - robotPose.getRotation().getDegrees(); + // if (roterr > 180) roterr -= 360; + // if (roterr < -180) roterr += 360; + + SmartDashboard.putNumber("Target Rotation!", targetRotation); + // System.out.println("Target: " + targetpos + "Heading: " + desiredHeading + "Error: " + roterr); + swerveDrive.driveRelativeAngle(new Translation2d(0.0, 0.0), Rotation2d.fromDegrees(targetRotation)); + } + + @Override + public final boolean isFinished() { + // boolean finished = Math.abs(roterr) < AutoConstants.ROT_TOLERANCE; + // if (finished) { + // swerveDrive.softStop(); + // } + // return finished; + return false; + } + + 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/waitSupplier.java b/src/main/java/frc4388/robot/commands/waitSupplier.java new file mode 100644 index 0000000..f4cd5a6 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/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; + +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(); + } +} \ No newline at end of file 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..2fec569 --- /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 = "2024AcrossTheRidgebotiverse"; + public static final String VERSION = "unspecified"; + public static final int GIT_REVISION = 21; + public static final String GIT_SHA = "f74be4b547fafc94c234cef14666422f4c4dd8e8"; + public static final String GIT_DATE = "2024-10-26 12:45:10 MDT"; + public static final String GIT_BRANCH = "2026-postseason-demo"; + public static final String BUILD_DATE = "2026-04-28 14:57:26 MDT"; + public static final long BUILD_UNIX_TIME = 1777409846756L; + public static final int DIRTY = 1; + + private BuildConstants(){} +} diff --git a/src/main/java/frc4388/robot/constants/ClimbConstants.java b/src/main/java/frc4388/robot/constants/ClimbConstants.java new file mode 100644 index 0000000..fbd6022 --- /dev/null +++ b/src/main/java/frc4388/robot/constants/ClimbConstants.java @@ -0,0 +1,21 @@ +package frc4388.robot.constants; + +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import frc4388.utility.status.CanDevice; + +public class ClimbConstants { + public static final CanDevice CLIMB_MOTOR_ID = new CanDevice("Climb Motor", 19); + public static final double CLIMB_IN_SPEED = -1.0; + public static final double CLIMB_OUT_SPEED = 0.87; + + public static final TalonFXConfiguration CLIMB_MOTOR_CONFIG = new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withInverted(InvertedValue.CounterClockwise_Positive) + ); +} 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..c7adfca --- /dev/null +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -0,0 +1,110 @@ +/*----------------------------------------------------------------------------*/ +/* 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.CANBus; + +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.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 CANBus RIO_CANBUS = CANBus.roboRIO(); + public static final CANBus CANIVORE_CANBUS = new CANBus("drivetrain"); + + // 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 ROT_GAINS = new Gains(8,0,0.0); + // public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP); + // public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI); + // public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD); + // public static final Gains XY_GAINS = new Gains(3,0.3,0.0); + + // public static final Gains ROT_GAINS = new Gains(0.05,0,0.007); + // public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); + + public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0); + // public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 1.5); + public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0); + + public static final double XY_TOLERANCE = 0.07; // Meters + public static final double ROT_TOLERANCE = 10; // Degrees + + public static final double MIN_XY_PID_OUTPUT = 0.0; + public static final double MIN_ROT_PID_OUTPUT = 0.0; + + public static final double VELOCITY_THRESHHOLD = 0.01; + + + public static final double STOP_VELOCITY = 0.; + } + + + + public static final class LEDConstants { + public static final int LED_SPARK_ID = 8; + + public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_RAINBOW; + } + + public static final class OIConstants { + public static final int XBOX_DRIVER_ID = 0; + public static final int XBOX_OPERATOR_ID = 1; + public static final int BUTTONBOX_ID = 2; + public static final int XBOX_PROGRAMMER_ID = 3; + public static final double LEFT_AXIS_DEADBAND = 0.1; + + } + + + + // Logging constants + public static class SimConstants { + public static final Mode simMode = Mode.SIM; + public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; + + public static enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + + /** Replaying from a log file. */ + REPLAY + } + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/constants/FieldConstants.java b/src/main/java/frc4388/robot/constants/FieldConstants.java new file mode 100644 index 0000000..f54067b --- /dev/null +++ b/src/main/java/frc4388/robot/constants/FieldConstants.java @@ -0,0 +1,44 @@ +package frc4388.robot.constants; + +import static edu.wpi.first.units.Units.Meters; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; + +public final class FieldConstants { + public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark); + + public static final double BUMP_OFFSET = 0.4; + public static final Transform2d BUMP_OFFSET_RED_FRONT = new Transform2d( + Meters.of(BUMP_OFFSET), + Meters.of(0), + new Rotation2d() + ); + public static final Transform2d BUMP_OFFSET_BLUE_FRONT = new Transform2d( + Meters.of(-BUMP_OFFSET), + Meters.of(0), + new Rotation2d() + ); + + public static final Transform2d BUMP_OFFSET_RED_BACK = new Transform2d( + Meters.of(-BUMP_OFFSET), + Meters.of(0), + new Rotation2d() + ); + public static final Transform2d BUMP_OFFSET_BLUE_BACK = new Transform2d( + Meters.of(BUMP_OFFSET), + Meters.of(0), + new Rotation2d() + ); + + // Test april tag field layout + // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( + // Arrays.asList(new AprilTag[] { + // new AprilTag(0, new Pose3d( + // new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.) + // )), + // }), 100, 100); + +} diff --git a/src/main/java/frc4388/robot/constants/IntakeConstants.java b/src/main/java/frc4388/robot/constants/IntakeConstants.java new file mode 100644 index 0000000..f6f499f --- /dev/null +++ b/src/main/java/frc4388/robot/constants/IntakeConstants.java @@ -0,0 +1,22 @@ +package frc4388.robot.constants; + +import frc4388.utility.status.CanDevice; +import frc4388.utility.structs.Gains; + +public class IntakeConstants { + // public static final int INTAKE_MOTOR_ID = 18; + // public static final int PIVOT_MOTOR_ID = 17; + public static final CanDevice INTAKE_MOTOR_ID = new CanDevice("Intake Motor", 18); + public static final CanDevice PIVOT_MOTOR_ID = new CanDevice("Pivot Motor", 17); + + + public static final double INTAKE_SPEED = 0.75; + public static final double INTAKE_OUT_SPEED_UNPRESSED = 1.0; + public static final double INTAKE_OUT_SPEED_PRESSED = 0.5; + public static final double HANDOFF_SPEED = 0.4; + public static final double PIVOT_SPEED = 0.2; + + public static final class ArmPID { + public static final Gains INTAKE_GAINS = new Gains(0.05, 0, 0, 0, 0, 1.0); + } +} diff --git a/src/main/java/frc4388/robot/constants/ShooterConstants.java b/src/main/java/frc4388/robot/constants/ShooterConstants.java new file mode 100644 index 0000000..e6727ea --- /dev/null +++ b/src/main/java/frc4388/robot/constants/ShooterConstants.java @@ -0,0 +1,17 @@ +package frc4388.robot.constants; + +import frc4388.utility.status.CanDevice; + +public class ShooterConstants { + // public static final int LEFT_SHOOTER_ID = 15; // final + // public static final int RIGHT_SHOOTER_ID = 16; // final + + public static final CanDevice LEFT_SHOOTER_ID = new CanDevice("Left Shooter Motor", 15); + public static final CanDevice RIGHT_SHOOTER_ID = new CanDevice("Right Shooter Motor", 16); + + + + public static final double SHOOTER_SPEED = 0.50; // final + public static final double SHOOTER_IDLE = 0.20; // final + public static final double SHOOTER_IDLE_LIMELIGHT = 0.20; +} diff --git a/src/main/java/frc4388/robot/constants/VisionConstants.java b/src/main/java/frc4388/robot/constants/VisionConstants.java new file mode 100644 index 0000000..db99be2 --- /dev/null +++ b/src/main/java/frc4388/robot/constants/VisionConstants.java @@ -0,0 +1,32 @@ +package frc4388.robot.constants; + +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; + +public 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(-13-9.134), -Units.inchesToMeters(10.75), Units.inchesToMeters(9.5) + ), new Rotation3d(0,25.*(Math.PI/180.),Math.PI)); + public static final Transform3d RIGHT_CAMERA_POS = new Transform3d( + new Translation3d(Units.inchesToMeters(-13-9.134), Units.inchesToMeters(10.75), Units.inchesToMeters(9.5)), + new Rotation3d(0,25.*(Math.PI/180.),Math.PI) + ); + + public static final double MIN_ESTIMATION_DISTANCE = 5; // 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); +} \ No newline at end of file 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/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 5ed1472..595c595 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -5,6 +5,7 @@ package frc4388.robot.subsystems; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -12,7 +13,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.ClimbConstants; +import frc4388.robot.constants.ClimbConstants; public class Climber extends SubsystemBase { /** Creates a new Climber. */ @@ -20,9 +21,10 @@ public class Climber extends SubsystemBase { public Climber(TalonFX climbMotor) { this.climbMotor = climbMotor; - this.climbMotor.setInverted(true); + climbMotor.getConfigurator().apply(ClimbConstants.CLIMB_MOTOR_CONFIG); + // this.climbMotor.(true); - climbMotor.setNeutralMode(NeutralModeValue.Brake); + // climbMotor.setNeutralMode(NeutralModeValue.Brake); var slot0Configs = new Slot0Configs(); slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output @@ -54,4 +56,4 @@ public class Climber extends SubsystemBase { // This method will be called once per scheduler run //SmartDashboard.putNumber("climber pos", climbMotor.getPosition().getValue()); } -} +} \ No newline at end of file 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 91de2e9..0000000 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ /dev/null @@ -1,88 +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 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/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 5ffa72a..f75f227 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -4,14 +4,16 @@ package frc4388.robot.subsystems; +import static edu.wpi.first.units.Units.Degrees; + import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.IntakeConstants; -import frc4388.utility.Gains; +import frc4388.robot.constants.IntakeConstants; +import frc4388.utility.structs.Gains; public class Intake extends SubsystemBase { private TalonFX intakeMotor; @@ -86,7 +88,8 @@ public class Intake extends SubsystemBase { } public double getArmPos() { - return pivotMotor.getPosition().getValue(); + // return pivotMotor.getPosition().getValue(); + return pivotMotor.getPosition().getValue().in(Degrees); } public void resetArmPosition() { diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java deleted file mode 100644 index e9e070c..0000000 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ /dev/null @@ -1,90 +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 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 frc4388.robot.Constants.LEDConstants; -import frc4388.utility.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 static LED getInstance() { - if(m_self == null) - m_self = new LED(); - return m_self; - } - @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()); - } - - /** - * 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 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()); - } - /** - * Add your docs here. - * @return - */ - public AddressableLEDBuffer getBuffer() { - return m_ledBuffer; - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java deleted file mode 100644 index 2200b07..0000000 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ /dev/null @@ -1,82 +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.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import frc4388.robot.Constants.VisionConstants; - -// Look at vvv for networktables stuff -// https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#apriltag-and-3d-data - -public class Limelight extends SubsystemBase { - - // // [X, Y, Z, Roll, Pitch, Yaw] - // private double[] cameraPose; - // private boolean isTag; - - // private Pose2d pose; - // private boolean isNearSpeaker; - - // public boolean getIsTag() { - // return isTag; - // } - - // private void update() { - // SmartDashboard.putBoolean("Apriltag", isTag); - // if(!isTag){ - // return; - // } - - // double x = cameraPose[0]; - // double y = cameraPose[1]; - // double yaw = cameraPose[5]; - - // Rotation2d rot = Rotation2d.fromDegrees(yaw); - - // pose = new Pose2d(x, y, rot); - - // boolean isRed = DriverStation.getAlliance().get() == Alliance.Red; - - // double distance; - - // if(isRed){ - // distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter); - // }else{ - // distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter); - // } - - // isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance; - - // //SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker); - // //SmartDashboard.putNumber("speakerDistance", distance); - // } - - // public Pose2d getPose() { - // return pose; - // } - - // public boolean isNearSpeaker() { - // return isNearSpeaker; - // } - - // @Override - // public void periodic() { - // // This method will be called once per scheduler run - - // //isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0; - // //double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]); - - // //if(newPose != cameraPose){ - // // cameraPose = newPose; - // //update(); - // //} - // } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index fb80e19..2136e04 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -8,10 +8,8 @@ import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.IntakeConstants; -import frc4388.robot.Constants.ShooterConstants; - -import frc4388.robot.subsystems.Limelight; +import frc4388.robot.constants.IntakeConstants; +import frc4388.robot.constants.ShooterConstants; // import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix6.hardware.TalonFX; @@ -24,7 +22,7 @@ public class Shooter extends SubsystemBase { private TalonFX leftShooter; private TalonFX rightShooter; - private Limelight limelight; +// private Limelight limelight; private int spinMode = 0; // 0 = Stop / Coast @@ -36,11 +34,11 @@ public class Shooter extends SubsystemBase { private double smartDashboardShooterSpeed; /** Creates a new Shooter. */ - public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight tmplimelight) { + public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX) { leftShooter = leftTalonFX; rightShooter = rightTalonFX; - limelight = tmplimelight; + // limelight = tmplimelight; leftShooter.setNeutralMode(NeutralModeValue.Coast); rightShooter.setNeutralMode(NeutralModeValue.Coast); @@ -85,7 +83,7 @@ public class Shooter extends SubsystemBase { spinMode = 0; } - public void idle() { + public void to_idle() { spin(ShooterConstants.SHOOTER_IDLE); spinMode = 1; } @@ -111,4 +109,4 @@ public class Shooter extends SubsystemBase { // idle(); // } } -} +} \ No newline at end of file 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 b9895fb..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()); - } - - 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() * - 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/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/swerve/SimpleSwerveSim.java b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java new file mode 100644 index 0000000..44937da --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java @@ -0,0 +1,204 @@ +package frc4388.robot.subsystems.swerve; + +import java.util.List; + +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.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; +import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; + +public class SimpleSwerveSim implements SwerveIO { + private Pose2d pose = new Pose2d(); + private Pose2d lastPose = pose; + private double vx = 0.0; + private double vy = 0.0; + private double omega = 0.0; + + private long lastTimeNs = System.nanoTime(); + + public SimpleSwerveSim() { + } + + public synchronized void setControl(SwerveRequest ctrl) { + if (ctrl == null) return; + + if (ctrl instanceof SwerveRequest.FieldCentricFacingAngle facingAngle) { + vx = facingAngle.VelocityX; + vy = facingAngle.VelocityY; + + double currentAngle = pose.getRotation().getRadians(); + double targetAngle = facingAngle.TargetDirection.getRadians(); + double error = targetAngle - currentAngle; + + error = Math.atan2(Math.sin(error), Math.cos(error)); + + double kP = 5.0; + omega = error * kP; + return; + } + + if (ctrl instanceof SwerveRequest.FieldCentric fc) { + vx = fc.VelocityX; + vy = fc.VelocityY; + omega = fc.RotationalRate; + return; + } + + if (ctrl instanceof SwerveRequest.RobotCentric rc) { + double cos = pose.getRotation().getCos(); + double sin = pose.getRotation().getSin(); + double vxRobot = rc.VelocityX; + double vyRobot = rc.VelocityY; + vx = vxRobot * cos - vyRobot * sin; + vy = vxRobot * sin + vyRobot * cos; + omega = rc.RotationalRate; + return; + } + + if (ctrl instanceof SwerveRequest.SwerveDriveBrake) { + vx = 0; vy = 0; omega = 0; + return; + } + + ChassisSpeeds cs = tryGetSpeedsField(ctrl); + if (cs != null) { + vx = cs.vxMetersPerSecond; + vy = cs.vyMetersPerSecond; + omega = cs.omegaRadiansPerSecond; + } + } + + private ChassisSpeeds tryGetSpeedsField(SwerveRequest ctrl) { + try { + java.lang.reflect.Field f = ctrl.getClass().getDeclaredField("Speeds"); + f.setAccessible(true); + Object o = f.get(ctrl); + if (o instanceof ChassisSpeeds) { + return (ChassisSpeeds) o; + } + } catch (NoSuchFieldException nsf) { + } catch (IllegalAccessException iae) { + } catch (SecurityException se) { + } + return null; + } + + private double tryGetDoubleField(Object obj, Class cls, String... names) { + for (String n : names) { + try { + java.lang.reflect.Field f = cls.getDeclaredField(n); + f.setAccessible(true); + Object val = f.get(obj); + if (val instanceof Number) { + return ((Number) val).doubleValue(); + } + } catch (NoSuchFieldException nsf) { + } catch (IllegalAccessException iae) { + } catch (Throwable t) { + } + } + return 0.0; + } + + + public synchronized void pointAtXY(double x, double y) { + Translation2d target = new Translation2d(x, y); + Translation2d toTarget = target.minus(pose.getTranslation()); + if (toTarget.getNorm() < 1e-9) return; + Rotation2d desired = toTarget.getAngle(); + pose = new Pose2d(pose.getTranslation(), desired); + lastPose = pose; + vx = 0.0; + vy = 0.0; + omega = 0.0; + } + + @Override + public synchronized void updateInputs(SwerveState state) { + if (state == null) return; + + long now = System.nanoTime(); + double dt = Math.max(1e-6, (now - lastTimeNs) / 1.0e9); + lastTimeNs = now; + + lastPose = pose; + double dxField; + double dyField; + + if (DriverStation.isAutonomous()) { + double dxRobot = vx * dt; + double dyRobot = vy * dt; + + Rotation2d r = pose.getRotation(); + double cos = r.getCos(); + double sin = r.getSin(); + + dxField = dxRobot * cos - dyRobot * sin; + dyField = dxRobot * sin + dyRobot * cos; + + } else { + dxField = vx * dt; + dyField = vy * dt; + + } + + + double dTheta = omega * dt; + + Translation2d newTrans = pose.getTranslation().plus(new Translation2d(dxField, dyField)); + Rotation2d newRot = pose.getRotation().plus(Rotation2d.fromRadians(dTheta)); + pose = new Pose2d(newTrans, newRot); + + state.lastPose = lastPose; + state.currentPose = pose; + state.speeds = new ChassisSpeeds(vx, vy, omega); + state.odometryRate = dt; + } + + @Override + public synchronized void resetPose(Pose2d p) { + if (p == null) return; + pose = p; + lastPose = p; + lastTimeNs = System.nanoTime(); + } + + @Override + public synchronized void tareEverything() { + pose = new Pose2d(); + lastPose = pose; + vx = 0.0; + vy = 0.0; + omega = 0.0; + lastTimeNs = System.nanoTime(); + } + + @Override + public synchronized void addVisionMeasurement(List poses) { + if (poses == null || poses.isEmpty()) return; + Pose2d visPose = poses.get(poses.size() - 1).pose(); + if (visPose != null) { + pose = visPose; + lastPose = visPose; + } + } + + public synchronized void pointAt(Translation2d target) { + if (target == null) return; + Translation2d toTarget = target.minus(pose.getTranslation()); + if (toTarget.getNorm() < 1e-9) return; + Rotation2d desired = toTarget.getAngle(); + pose = new Pose2d(pose.getTranslation(), desired); + lastPose = pose; + omega = 0.0; + } + + @Override + public synchronized void setLimits(double limitInAmps) { + } +} 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..5523eec --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -0,0 +1,707 @@ +// 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 static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import java.util.Optional; +import java.util.function.Supplier; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.trajectory.PathPlannerTrajectory; +import com.pathplanner.lib.util.PathPlannerLogging; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +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.Constants.AutoConstants; +import frc4388.robot.subsystems.vision.Vision; +import frc4388.utility.compute.TimesNegativeOne; +import frc4388.utility.status.FaultReporter; +import frc4388.utility.status.Queryable; +import frc4388.utility.status.Status; + +public class SwerveDrive extends SubsystemBase implements Queryable { + // private SwerveDrivetrain swerveDriveTrain; + + private SwerveIO io; + private SwerveStateAutoLogged state; + + private Vision vision; + + + + public int gear_index = SwerveDriveConstants.STARTING_GEAR; + public boolean stopped = false; + public boolean robotKnowsWhereItIs = false; + + public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index]; + public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED; + public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to + // 25% + + public double lastOdomSpeed; + + public Pose2d initalPose2d = new Pose2d(); + + + + public double rotTarget = 0.0; + public Rotation2d orientRotTarget = new Rotation2d(); + public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + + private final PIDController m_rotationOverridePID = new PIDController( + SwerveDriveConstants.PIDConstants.AIM_kP.get(), + SwerveDriveConstants.PIDConstants.AIM_kI.get(), + SwerveDriveConstants.PIDConstants.AIM_kD.get() + ); + private boolean m_useRotationOverride = false; + private Translation2d m_rotationOverrideTarget = new Translation2d(); + + /** Creates a new SwerveDrive. */ + public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) { + // public SwerveDrive(SwerveDrivetrain + // swerveDriveTrain) { + FaultReporter.register(this); + + this.io = swerveDriveTrain; + this.state = new SwerveStateAutoLogged(); + + this.vision = vision; + + RobotConfig config; + try { + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + // Handle exception as needed + config = null; + } + + PPHolonomicDriveController driveController = new PPHolonomicDriveController( + new PIDConstants(5.0, 0.0, 0.0), // Translation PID + new PIDConstants(5.0, 0.0, 0.0) // Rotation PID (used when override is OFF) + ); + driveController.setRotationTargetOverride(() -> { + if (!m_useRotationOverride) return Optional.empty(); + Rotation2d targetAngle = getPose2d() + .getTranslation() + .minus(m_rotationOverrideTarget) + .getAngle(); + return Optional.of(targetAngle); + }); + + // DoubleSupplier a = () -> 1.d; + AutoBuilder.configure( + () -> { + return getPose2d(); + }, // Robot pose supplier + this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting + // pose) + () -> state.speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speeds, feedforwards) -> io.setControl(new SwerveRequest.ApplyRobotSpeeds() + .withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. + // Also optionally outputs individual module feedforwards + driveController, // <-- use the variable, not inline new PPHolonomicDriveController(...) + config, // The robot configuration + () -> { + // Boolean supplier that controls when the path will be mirrored for the red + // alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + // var alliance = DriverStation.getAlliance(); + // if (alliance.isPresent()) { + // return alliance.get() == DriverStation.Alliance.Red; + // } + return TimesNegativeOne.isRed; + }, + this // Reference to this subsystem to set requirements + ); + + PathPlannerLogging.setLogActivePathCallback( + (activePath) -> { + Logger.recordOutput( + "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + }); + + PathPlannerLogging.setLogTargetPoseCallback( + (targetPose) -> { + Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + }); + + + + // // Configure SysId + // sysId = + // new SysIdRoutine( + // new SysIdRoutine.Config( + // null, + // null, + // null, + // (state) -> Logger.recordOutput("Drive/SysIdState", toString())), + // new SysIdRoutine.Mechanism( + // (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); + + } + + public void setOdoPose(Pose2d pose) { + if (pose == null) return; + initalPose2d = pose; + io.resetPose(initalPose2d); + } + + public void setInitalPose(Pose2d startingAutoPose){ + initalPose2d = startingAutoPose; + } + + + // 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 double chassisXSpeeds(){ + if (TimesNegativeOne.isRed) { + return chassisSpeeds.vxMetersPerSecond; + } else { + return -chassisSpeeds.vxMetersPerSecond; + } + } + + public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: + stopModules(); // stop the swerve + + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput + return; // don't bother doing swerve drive math and return early. + + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); + + stopped = false; + if (fieldRelative) { + + leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); + rightStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis); + + // ! drift correction + if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) { + + rotTarget = state.currentPose.getRotation().getDegrees(); + + io.setControl(new SwerveRequest.FieldCentric() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + + // .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective)); + SmartDashboard.putBoolean("drift correction", false); + } else { + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(Rotation2d.fromDegrees(rotTarget)); + ctrl.HeadingController.setPID( + SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP, + SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI, + SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD + ); + io.setControl(ctrl); + SmartDashboard.putBoolean("drift correction", true); + } + + + } else { // Create robot-relative speeds. + io.setControl(new SwerveRequest.RobotCentric() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(-leftStick.getY() * speedAdjust) + .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + } + } + + public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) { + stopped = false; + // Create robot-relative speeds. + if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0); + io.setControl(new SwerveRequest.RobotCentric() + .withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) + .withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) + .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + + } + + + + public void aimAtPosition(Translation2d fieldPos, double aimLeadTime) { + Translation2d robotSpeed = new Translation2d( + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond + ); + Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos); + Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle(); + + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(chassisSpeeds.vxMetersPerSecond) + .withVelocityY(chassisSpeeds.vyMetersPerSecond) + .withTargetDirection(ang); + ctrl.HeadingController.setPID( + SwerveDriveConstants.PIDConstants.AIM_kP.get(), + SwerveDriveConstants.PIDConstants.AIM_kI.get(), + SwerveDriveConstants.PIDConstants.AIM_kD.get() + ); + io.setControl(ctrl); + } + + 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); + + if(!TimesNegativeOne.isRed) { + leftStick.rotateBy(new Rotation2d(Math.PI/2.)); + } + + io.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(rightStick.getAngle())); + } + + public void driveRelativeAngle(Translation2d leftStick, Rotation2d heading) { + + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); + leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(heading); + ctrl.HeadingController.setPID( + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD + ); + io.setControl(ctrl); + } + + // Drive with a specific velocity and heading + public void driveFieldAngle(Translation2d leftStick, Rotation2d heading) { + if (leftStick.getNorm() < 0.05) // if no imput and the swerve drive is still going: + stopModules(); // stop the swerve + + + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); + leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); + + rotTarget = heading.getDegrees(); + + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(heading); + ctrl.HeadingController.setPID( + SwerveDriveConstants.PIDConstants.AIM_kP.get(), + SwerveDriveConstants.PIDConstants.AIM_kI.get(), + SwerveDriveConstants.PIDConstants.AIM_kD.get() + // SwerveDriveConstants.PIDConstants.AIM_GAINS.kP, + // SwerveDriveConstants.PIDConstants.AIM_GAINS.kI, + // SwerveDriveConstants.PIDConstants.AIM_GAINS.kD + ); + io.setControl(ctrl); + // SmartDashboard.putBoolean("drift correction", true); + } + + public void driveFieldAngleSIP(Translation2d leftStick, Rotation2d heading) { + + rotTarget = heading.getDegrees(); + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(heading); + ctrl.HeadingController.setPID( + SwerveDriveConstants.PIDConstants.AIM_kP.get(), + SwerveDriveConstants.PIDConstants.AIM_kI.get(), + SwerveDriveConstants.PIDConstants.AIM_kD.get() + ); + io.setControl(ctrl); + } + + public void driveIntake(Translation2d leftStick, boolean invertRotation){ + // if (invert){ + // Translation2d stick = new Translation2d(-leftStick.getX(), -leftStick.getY()); + // driveFieldAngle(stick, heading); + + // } else{ + // driveFieldAngle(leftStick, heading); + // } + double speed = leftStick.getNorm(); + + if(speed < 0.3) { + driveWithInput(leftStick, new Translation2d(), true); + } else { + + + + Rotation2d heading = new Rotation2d(leftStick.getX(), -leftStick.getY());//.r otateBy(Rotation2d.fromDegrees(90)); + + heading = heading.rotateBy(Rotation2d.fromDegrees(270)); + + driveFieldAngle(leftStick, heading); + } + } + + public void driveIntakeOrientation(Translation2d leftStick, Translation2d rightStick){ + // if (invert){ + // Translation2d stick = new Translation2d(-leftStick.getX(), -leftStick.getY()); + // driveFieldAngle(stick, heading); + + // } else{ + // driveFieldAngle(leftStick, heading); + // } + double speed = rightStick.getNorm(); + + if(speed < 0.3) { + driveWithInput(leftStick, new Translation2d(), true); + } else { + + + + Rotation2d heading = new Rotation2d(rightStick.getX(), rightStick.getY());//.r otateBy(Rotation2d.fromDegrees(90)); + + if(TimesNegativeOne.isRed) { + heading = heading.rotateBy(Rotation2d.fromDegrees(-90)); + } else { + heading = heading.rotateBy(Rotation2d.fromDegrees(90)); + } + + rotTarget = heading.getDegrees(); + + driveFieldAngle(leftStick, heading); + } + } + + + // Drive with the robot facing towards a specific position + public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime) { + Translation2d robotSpeedYOnly = new Translation2d(0, chassisSpeeds.vyMetersPerSecond); + double yDistance = Math.abs(getPose2d().getTranslation().getY() - fieldPos.getY()); + if ((chassisSpeeds.vyMetersPerSecond >0 &&getPose2d().getTranslation().getY() >4)||(chassisSpeeds.vyMetersPerSecond <0 &&getPose2d().getTranslation().getY() <4)){ + if (Math.abs(chassisSpeeds.vyMetersPerSecond) > 0.2) { + if (TimesNegativeOne.isRed){ + robotSpeedYOnly = new Translation2d(-SwerveDriveConstants.FAR_OFFSET.get() * yDistance * (getPose2d().getTranslation().getX()-7.28989525), chassisSpeeds.vyMetersPerSecond); + } else { + robotSpeedYOnly = new Translation2d((getPose2d().getTranslation().getX())* yDistance* SwerveDriveConstants.FAR_OFFSET.get(), chassisSpeeds.vyMetersPerSecond); + } + } } + Translation2d fieldPosLead = robotSpeedYOnly.times(aimLeadTime).plus(fieldPos); + Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle(); + Pose2d fieldPosLeadLog = new Pose2d(fieldPosLead, new Rotation2d()); + Logger.recordOutput("Lead Aim", fieldPosLeadLog); + driveFieldAngle(leftStick, ang); + } + + public void offsetOdoPosition(Transform2d offset) { + // Manually performing an addittion on the pose + // WHY doesn't WPILIB have the ability to not transform poses + Pose2d new_pose = new Pose2d( + new Translation2d( + state.currentPose.getX() + offset.getX(), + state.currentPose.getY() + offset.getY() + ), + state.currentPose.getRotation() + ); + this.io.resetPose(new_pose); + } + + public void defenseXPosition(){ + io.setControl(new SwerveRequest.SwerveDriveBrake()); + } + + public void stopDefenseXPosition(){ + stopModules(); + } + + public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) { + // Calculate the angle between the current position and the lead position + //Rotation2d ang = getPose2d().getTranslation().minus(fieldPos).getAngle(); + Rotation2d ang = new Rotation2d(0,1); + System.out.println(ang); + + driveFieldAngle(leftStick, ang); + } + + public Pose2d getCurrentPose(){ + return state.currentPose; + } + + public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) { + leftStick = leftStick.rotateBy(heading); + + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(heading); + // ctrl.HeadingController.setPID( + // DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, + // DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, + // DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD + // ); + io.setControl(ctrl); + } + + public void activateLuigiMode() { + io.setLimits(20); + } + + public void deactivateLuigiMode() { + io.setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT); + } + + public boolean rotateToTarget(double angle) { + io.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(0) + .withVelocityY(0) + .withTargetDirection(Rotation2d.fromDegrees(angle))); + + if (Math.abs(angle - getGyroAngle()) < 5.0) { + return true; + } + + return false; + } + + public boolean isStopped() { + return lastOdomSpeed < AutoConstants.STOP_VELOCITY; + } + + public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) { + + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); + + io.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * -speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(rot)); + } + + public double getGyroAngle() { + return getPose2d().getRotation().getRadians(); + } + + public Pose2d getPose2d() { + if(state.currentPose == null) + return initalPose2d; + return state.currentPose; + } + + public Supplier getPoseSupplier() { + return () -> this.getPose2d(); + } + + public void resetGyro() { + io.tareEverything(); + robotKnowsWhereItIs = false; + rotTarget = 0; + // vision.resetRotations(); + } + + + public void softStop() { + stopped = true; + io.setControl(new SwerveRequest.FieldCentric() + .withVelocityX(0) + .withVelocityY(0) + .withRotationalRate(0) + ); // stop the modules without breaking + } + + public void stopModules() { + // stopped = true; + // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); + softStop(); + } + + public void enableRotationOverride(Translation2d fieldTarget, double aimLeadTime, Translation2d fieldPos) { + Translation2d robotSpeedYOnly = new Translation2d(0, chassisSpeeds.vyMetersPerSecond); + + Translation2d fieldPosLead = robotSpeedYOnly.times(aimLeadTime).plus(fieldPos); + m_rotationOverrideTarget = fieldPosLead; + m_useRotationOverride = true; + } + + public void disableRotationOverride() { + m_useRotationOverride = false; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run\ + SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI); + SmartDashboard.putNumber("RotTartget", rotTarget); + + io.updateInputs(state); + Logger.processInputs("SwerveDrive", state); + + vision.setLastOdomPose(state.currentPose); + setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate); + + if (state.speeds != null) { + this.chassisSpeeds = state.speeds; + } else { + this.chassisSpeeds = new ChassisSpeeds(); + } + + if (vision.isTag()) { + Pose2d pose = vision.getPose2d(); + if (!robotKnowsWhereItIs) { + robotKnowsWhereItIs = true; + Pose2d curPose = getPose2d(); + rotTarget += pose.getRotation().getDegrees() - curPose.getRotation().getDegrees(); + } + + io.addVisionMeasurement(vision.getPosesToAdd()); + io.updateInputs(state); + Logger.processInputs("SwerveDrive", state); + + vision.setLastOdomPose(state.currentPose); + setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate); + } + + // if(e.isPresent()) + } + + private void reset_index() { + gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the + // robot start in?) + } + + public void shiftDown() { + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) + reset_index(); // If outof bounds: reset index + int i = gear_index - 1; + if (i == -1) + i = 0; + setPercentOutput(SwerveDriveConstants.GEARS[i]); + gear_index = i; + } + + public void shiftUp() { + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) + reset_index(); // If outof bounds: reset index + int i = gear_index + 1; + if (i == SwerveDriveConstants.GEARS.length) + i = SwerveDriveConstants.GEARS.length - 1; + setPercentOutput(SwerveDriveConstants.GEARS[i]); + gear_index = i; + } + + + public void setPercentOutput(double speed) { + speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; + gear_index = -1; + } + + public void setToSlow() { + setPercentOutput(SwerveDriveConstants.SLOW_SPEED); + gear_index = 0; + } + + public void setToFast() { + setPercentOutput(SwerveDriveConstants.FAST_SPEED); + gear_index = 1; + } + + public void setToTurbo() { + setPercentOutput(SwerveDriveConstants.TURBO_SPEED); + gear_index = 2; + } + + public void shiftUpRot() { + rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED; + } + + public void shiftDownRot() { + rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; + } + + private int tmp_gear_index = SwerveDriveConstants.STARTING_GEAR; + + public void startSlowPeriod() { + tmp_gear_index = gear_index; + setToSlow(); + } + + public void startTurboPeriod() { + tmp_gear_index = gear_index; + setToTurbo(); + } + + public void endSlowPeriod() { + setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]); + gear_index = tmp_gear_index; + } + + + + public void setLastOdomSpeed(Pose2d curPose, Pose2d lastPose, double freq){ + if(curPose != null && lastPose != null){ + lastOdomSpeed = curPose.getTranslation().getDistance(lastPose.getTranslation())/freq; + } + } + + @AutoLogOutput(key="SwerveDrive/speed ") + public double getOdometrySpeed() { + return lastOdomSpeed; + } + + + + @Override + public String getName() { + return "Swerve Drive Controller"; + } + + @Override + public Status diagnosticStatus() { + Status status = new Status(); + + // status.addReport(ReportLevel.INFO, + // "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there."); + + return status; + } +} \ No newline at end of file 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..fbd84ef --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -0,0 +1,263 @@ +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.Distance; +import edu.wpi.first.units.measure.Angle; +import frc4388.robot.constants.Constants; +import frc4388.utility.configurable.ConfigurableDouble; +//import edu.wpi.first.units.measure.measure.Distance; +import frc4388.utility.status.CanDevice; +import frc4388.utility.structs.Gains; + + +// No mans land +// Beware, there be dragons. +public final class SwerveDriveConstants { + // Duct tape. + public static final double ROT_SPEED = 3.5; + public static final double ROTATION_SPEED = ROT_SPEED; + public static final double MAX_ROT_SPEED = ROT_SPEED; + + // 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 ConfigurableDouble FAR_OFFSET = new ConfigurableDouble("Far Offset", 0.05); // degrees to add to the angle when we are far away, to account for camera misalignment. TODO: find value. + + 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); + 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); + 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); + 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); + 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); + + + + // TODO: Replace this with a static constant + public static final ConfigurableDouble AIM_kP = new ConfigurableDouble("Aim kP", 12); + public static final ConfigurableDouble AIM_kI = new ConfigurableDouble("Aim kI", 0); + public static final ConfigurableDouble AIM_kD = new ConfigurableDouble("Aim kD", 0.1); + public static final ConfigurableDouble HOLD_POSITION_kP = new ConfigurableDouble("Hold Position kP", 15); + // public static final Gains AIM_GAINS = new Gains(2.5, 0, 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).withCANBusName(Constants.CANIVORE_CANBUS.getName()); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() // holy verbosity batman. + .withDriveMotorGearRatio(DRIVE_RATIO) + .withSteerMotorGearRatio(STEER_RATIO) + .withCouplingGearRatio(COUPLE_RATIO) + .withWheelRadius(WHEEL_RADIUS) + .withSteerMotorGains(PIDConstants.PREPROVIDED_STEER_GAINS) // ? + .withDriveMotorGains(PIDConstants.PREPROVIDED_DRIVE_GAINS) // ? + .withSteerMotorClosedLoopOutput(ClosedLoopOutputType.Voltage) + .withDriveMotorClosedLoopOutput(ClosedLoopOutputType.Voltage) + .withSlipCurrent(Configurations.SLIP_CURRENT) + .withSpeedAt12Volts(MAX_SPEED_MEETERS_PER_SEC) + .withDriveMotorType(DriveMotorArrangement.TalonFX_Integrated) + .withSteerMotorType(SteerMotorArrangement.TalonFX_Integrated) + .withFeedbackSource(SteerFeedbackType.RemoteCANcoder) + .withDriveMotorInitialConfigs(Configurations.DRIVE_MOTOR_CONFIGS) + .withSteerMotorInitialConfigs(Configurations.STEER_MOTOR_CONFIGS); + + public static final SwerveModuleConstants FRONT_LEFT = + ConstantCreator.createModuleConstants( + IDs.LEFT_FRONT_STEER.id, IDs.LEFT_FRONT_WHEEL.id, IDs.LEFT_FRONT_ENCODER.id, ModuleSpecificConstants.FRONT_LEFT_ENCODER_OFFSET, + ModuleSpecificConstants.FRONT_LEFT_XPOS, ModuleSpecificConstants.FRONT_LEFT_YPOS, + ModuleSpecificConstants.FRONT_LEFT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_LEFT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_LEFT_ENCODER_INVERTED + ); + public static final SwerveModuleConstants FRONT_RIGHT = + ConstantCreator.createModuleConstants( + IDs.RIGHT_FRONT_STEER.id, IDs.RIGHT_FRONT_WHEEL.id, IDs.RIGHT_FRONT_ENCODER.id, ModuleSpecificConstants.FRONT_RIGHT_ENCODER_OFFSET, + ModuleSpecificConstants.FRONT_RIGHT_XPOS, ModuleSpecificConstants.FRONT_RIGHT_YPOS, + ModuleSpecificConstants.FRONT_RIGHT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_RIGHT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_RIGHT_ENCODER_INVERTED + ); + public static final SwerveModuleConstants BACK_LEFT = + ConstantCreator.createModuleConstants( + IDs.LEFT_BACK_STEER.id, IDs.LEFT_BACK_WHEEL.id, IDs.LEFT_BACK_ENCODER.id, ModuleSpecificConstants.BACK_LEFT_ENCODER_OFFSET, + ModuleSpecificConstants.BACK_LEFT_XPOS, ModuleSpecificConstants.BACK_LEFT_YPOS, + ModuleSpecificConstants.BACK_LEFT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.BACK_LEFT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.BACK_LEFT_ENCODER_INVERTED + ); + public static final SwerveModuleConstants BACK_RIGHT = + ConstantCreator.createModuleConstants( + IDs.RIGHT_BACK_STEER.id, IDs.RIGHT_BACK_WHEEL.id, IDs.RIGHT_BACK_ENCODER.id, ModuleSpecificConstants.BACK_RIGHT_ENCODER_OFFSET, + ModuleSpecificConstants.BACK_RIGHT_XPOS, ModuleSpecificConstants.BACK_RIGHT_YPOS, + ModuleSpecificConstants.BACK_RIGHT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.BACK_RIGHT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.BACK_RIGHT_ENCODER_INVERTED + ); + + // misc + public static final int TIMEOUT_MS = 30; + public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; +} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java new file mode 100644 index 0000000..25db223 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java @@ -0,0 +1,35 @@ +package frc4388.robot.subsystems.swerve; + +import java.util.List; + +import org.littletonrobotics.junction.AutoLog; + +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; +import frc4388.utility.status.CanDevice; + +public interface SwerveIO { + @AutoLog + public class SwerveState { + public Pose2d currentPose = null; + public Pose2d lastPose = null; + public ChassisSpeeds speeds = null; + public double odometryRate = 1; + } + + public default void setControl(SwerveRequest ctrl) {} + + public default void setLimits(double limitInAmps) {} + + public default void tareEverything() {} + + public default void resetPose(Pose2d pose) {} + + public default void addVisionMeasurement(List poses) {} + + public default void updateInputs(SwerveState state) {} +} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java new file mode 100644 index 0000000..9574699 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java @@ -0,0 +1,80 @@ +package frc4388.robot.subsystems.swerve; + +import java.util.List; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.swerve.SwerveModule; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import frc4388.robot.subsystems.vision.Vision; +import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; +import frc4388.utility.status.CanDevice; + +public class SwerveReal implements SwerveIO { + SwerveDrivetrain swerveDriveTrain; + + public SwerveReal(SwerveDrivetrain swerveDriveTrain) { + this.swerveDriveTrain = swerveDriveTrain; + swerveDriveTrain.getOdometryFrequency(); + } + + @Override + public void updateInputs(SwerveState state) { + double time = Vision.getTime(); + state.odometryRate = 1 / swerveDriveTrain.getOdometryFrequency(); + state.currentPose = swerveDriveTrain.samplePoseAt(time).orElse(null); + state.lastPose = swerveDriveTrain.samplePoseAt(time - state.odometryRate).orElse(null); + state.speeds = swerveDriveTrain.getState().Speeds; + } + + @Override + public void setControl(SwerveRequest ctrl) { + swerveDriveTrain.setControl(ctrl); + } + + @Override + public void tareEverything() { + swerveDriveTrain.tareEverything(); + } + + @Override + public void resetPose(Pose2d pose) { + if (pose == null) return; + try { + // Preferred: ask the drivetrain to reset its odometry directly + System.out.println("!"+pose); + swerveDriveTrain.resetPose(pose); + } catch (NoSuchMethodError | RuntimeException e) { + // Fallback: tare sensors then add a timed vision measurement so odometry is seeded + swerveDriveTrain.tareEverything(); + swerveDriveTrain.addVisionMeasurement(pose, Utils.fpgaToCurrentTime(Vision.getTime())); + } + } + + @Override + public void setLimits(double limitInAmps) { + for (SwerveModule module : swerveDriveTrain.getModules()) { + var talonFXConfigurator = module.getDriveMotor().getConfigurator(); + var talonFXConfigs = new TalonFXConfiguration(); + + talonFXConfigurator.refresh(talonFXConfigs); + talonFXConfigs.CurrentLimits.StatorCurrentLimit = limitInAmps; + talonFXConfigs.CurrentLimits.SupplyCurrentLimit = limitInAmps+10; + talonFXConfigurator.apply(talonFXConfigs); + } + } + + @Override + public void addVisionMeasurement(List poses) { + for(PoseObservation pose : poses) { + swerveDriveTrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp())); + } + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/vision/Lidar.java b/src/main/java/frc4388/robot/subsystems/vision/Lidar.java new file mode 100644 index 0000000..d859102 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/vision/Lidar.java @@ -0,0 +1,313 @@ +package frc4388.robot.subsystems.vision; + +import java.util.ArrayList; +import java.util.LinkedList; +import java.util.List; +import java.util.Queue; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.core.Scalar; +import org.opencv.highgui.HighGui; +import org.opencv.imgproc.Imgproc; + +import edu.wpi.first.cscore.OpenCvLoader; +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 edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.subsystems.vision.RPLidarA1.PolarPoint; +import frc4388.robot.subsystems.vision.RPLidarA1.ScanListener; +import frc4388.utility.configurable.ConfigurableDouble; +import frc4388.utility.status.FaultA1M8; + + + +public class Lidar extends SubsystemBase implements ScanListener { + // private final Spark m_motor; + private final RPLidarA1 lidar; + + private ConfigurableDouble speed = new ConfigurableDouble("LiDAR speed", 0.2); + + static + { + // This is so libopencv_javaVERSION.so (where version is the 3-digit opencv + // version) gets loaded. + try { + OpenCvLoader.forceLoad(); + } + catch (Exception e) { + e.printStackTrace(); + } + } + + // private static final double m_Scan = 0.1; + public Lidar() { + // Spark motor = new Spark(0); + // this.m_motor = motor; + this.lidar = new RPLidarA1(); + this.lidar.setListener(this); + + + // Thread processThread = new Thread(this::pointLoop); + // processThread.setDaemon(true); + // processThread.setName("RPLidar-Calc"); + // processThread.start(); + + FaultA1M8.addDevice(lidar, "A1M8"); + } + + public Rotation2d getLatestBallAngle() { + return latestBallAngleDeg; + } + + public boolean outOfBounds(Translation2d closestBall){ + // Make sure robot doesn't go off the earth + return true; + } + + + @Override + public void periodic() { + this.lidar.setSpeed(speed.get()); + SmartDashboard.putString("lidar state", this.lidar.getStatus().toString()); + } + + // Detection constriants: cluster detection + private static final double ANG_MAX_GAP = 3.; // Degrees + private static final double DIST_MAX_GAP = 0.04; // Meters + + // Detection constraints: Circle detection + private static final double RADIUS_X_COEFF = Units.inchesToMeters(0); + private static final double RADIUS_Y_COEFF = Units.inchesToMeters(0); + private static final double RADIUS_OFFSET = Units.inchesToMeters(3); + private static final double RADIUS_TOLERANCE = Units.inchesToMeters(3); + + private static boolean radiusInTolerance(double x, double y, double radius) { + double rad_at_position = RADIUS_X_COEFF*x + RADIUS_Y_COEFF*y + RADIUS_OFFSET; + + return Math.abs(rad_at_position - radius) <= RADIUS_TOLERANCE; + } + + // Window constants + private static final int WIDTH = 512; + private static final int HEIGHT = 512; + private static final int POINT_RAD = 2; + + + Translation2d closestBall; + Translation2d closestBallPrior = null; + + @AutoLogOutput + public Translation2d getClosestBall() { + return closestBall; + } + + + + private List point_group = new ArrayList<>(); + private double last_ang = 0; + private double last_dist = 0; + private Rotation2d latestBallAngleDeg= new Rotation2d(); + private boolean last_color = false; + + Point LIDAR = new Point(WIDTH/2,WIDTH/2); + + @Override + public void onScanComplete(List scan) { + + System.out.println("SCAN: " + scan.size()); + + double scale = 0.009; + + List circlePoints = new ArrayList<>(); + + // Mat mat = Mat.zeros(WIDTH, HEIGHT, CvType.CV_8UC3); + + for(PolarPoint point_polar : scan) { + if(!(point_polar.angle < 30 || point_polar.angle > 330)) { + continue; + } + + double ang_rad = Math.toRadians(point_polar.angle); + double x = point_polar.distance * Math.cos(ang_rad); + double y = point_polar.distance * Math.sin(ang_rad); + + // Point point_xy = new Point((WIDTH/2) + (x/scale), (HEIGHT/2) + (y/scale)); + Point point_xy = new Point(x, y); + + if( + Math.abs(last_ang - point_polar.angle) > ANG_MAX_GAP || + Math.abs(last_dist - point_polar.distance) > DIST_MAX_GAP + ) { + last_color = !last_color; + + if ( + point_group.size() >= 3 + // point_group.size() <= POINT_MAX.get() + ) { + // Get points + Point p1 = point_group.get(0); + Point p2 = point_group.get(point_group.size()/2); + Point p3 = point_group.get(point_group.size()-1); + + // Simplify to var + double dx23 = p2.x - p3.x; + double dy23 = p2.y - p3.y; + + double dx13 = p1.x - p3.x; + double dy13 = p1.y - p3.y; + + double dx12 = p1.x - p2.x; + double dy12 = p1.y - p2.y; + + // Calc Determinite + double D = p1.x*dy23 - p1.y*dx23 + (p2.x*p3.y - p3.x*p2.y); + + // The points are in a straight line. + if(D == 0) { + continue; + } + + // Square distances between each set of 2 points + double a_sq = dx23*dx23 + dy23*dy23; + double b_sq = dx13*dx13 + dy13*dy13; + double c_sq = dx12*dx12 + dy12*dy12; + + // Calculate the radius + double radius = Math.sqrt(a_sq*b_sq*c_sq) / (2 * Math.abs(D)); + + // Square distances between each point and origin + double d1 = p1.x*p1.x + p1.y*p1.y; + double d2 = p2.x*p2.x + p2.y*p2.y; + double d3 = p3.x*p3.x + p3.y*p3.y; + + // Calculate X and Y + double cx = (d1*dy23 - d2*dy13 + d3*dy12)/(2*D); + double cy = -(d1*dx23 - d2*dx13 + d3*dx12)/(2*D); + + + + if(radiusInTolerance(cx, cy, radius)) { + circlePoints.add(new Translation2d(cx, cy)); + } + + + + + // FitResult result = TaubinCircleFitter.fit(point_group, Lidar::getRadius); + + // if(result.rmsError < ERROR_BOUND.get()) { + // circlePoints.add(result.center); + // Imgproc.circle(mat, result.center, (int) (result.radius/scale), new Scalar(255,255,255)); + // } + } + + point_group.clear(); + } + + point_group.add(point_xy); + + last_ang = point_polar.angle; + last_dist = point_polar.distance; + + // Point scaledPoint = new Point((WIDTH/2) + (point_xy.x / scale), (WIDTH/2) + (point_xy.y / scale)); + // Imgproc.circle(mat, scaledPoint, POINT_RAD, new Scalar(127,127,127)); + } + + // for(Translation2d circle : circlePoints) { + // Point scaledPoint = new Point( (WIDTH/2) + (circle.getX() / scale), (WIDTH/2) + (circle.getY() / scale)); + // Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(0,255,255)); + // // System.out.println(circle.x + " - " + circle.y); + // } + + + + closestBall = new Translation2d(); + + if (circlePoints.isEmpty()) { + closestBall = new Translation2d(Double.NaN, Double.NaN); + } else { + double minDist = Double.POSITIVE_INFINITY; + Translation2d best = null; + for (Translation2d circle : circlePoints) { + double dist = circle.getSquaredNorm(); // distance from 0,0 + if (dist < minDist) { + minDist = dist; + best = circle; + } + } + + closestBall = best; + } + + if (closestBallPrior != null) { + if (closestBall.getDistance(closestBallPrior) < 0.1 && outOfBounds(closestBall)){ + + // Point scaledPoint = new Point( (WIDTH/2) + (closestBall.getX() / scale), (WIDTH/2) + (closestBall.getY() / scale)); + // Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(200, 25, 52), -1); + latestBallAngleDeg = new Rotation2d(Math.atan((closestBall.getY())/(closestBall.getX()))/Math.PI*180); + } else { + // Point scaledPoint = new Point( (WIDTH/2) + (closestBallPrior.getX() / scale), (WIDTH/2) + (closestBallPrior.getY() / scale)); + // Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(200, 25, 52), -1); + } + } + + closestBallPrior = closestBall; + + + + // Imgproc.circle(mat, LIDAR, (int) (RADIUS_OFFSET / scale), new Scalar(255,255,255), -1); + + // System.o + + + // showWindow(mat); + } + + private static void showWindow(Mat img) { + // Display the image in a window titled "Original Image" + HighGui.imshow("Original Image", img); + + + // Wait for a key press to close the window + HighGui.waitKey(1); + } + + // XYZ Position of the LiDAR on the robot + private static final Translation2d LiDAR_POS = new Translation2d(1, 0); + + // Angle of the lidar unit + private static final double LiDAR_PITCH = 0; // Radians + private static final double LiDAR_ROLL = 0; // Radians + + // Convert a LiDAR ball position to a field position + public static Translation2d lidarPosToField(Translation2d p, Pose2d pose) { + // Project the point tilted plane on to the XY plane + // Point should be relative to the XY plane, with (0,0) centered at the centerpoint of the lidar + double x = p.getX() * Math.cos(LiDAR_ROLL) + p.getY() * Math.sin(LiDAR_PITCH) * Math.sin(LiDAR_ROLL); + double y = p.getY() * Math.cos(LiDAR_PITCH); + + // Translate the ball position to relative to the center of the robot + // Point should be relative to robot, wth (0,0) centered at center of robot + x -= LiDAR_POS.getX(); + y -= LiDAR_POS.getY(); + + // Rotate the point by the robot's rotation + // Point should now be relative to robot, but rotated relative to the field. + double ang = -pose.getRotation().getRadians(); + x = x*Math.cos(ang) - y*Math.sin(ang); + y = x*Math.sin(ang) + y*Math.cos(ang); + + // Translate the point to the robot's field position + // Point should be relative to field. (0,0) should be relative to the field. + x += pose.getX(); + y += pose.getY(); + + return new Translation2d(x, y); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/vision/RPLidarA1.java b/src/main/java/frc4388/robot/subsystems/vision/RPLidarA1.java new file mode 100644 index 0000000..41ad20a --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/vision/RPLidarA1.java @@ -0,0 +1,431 @@ +package frc4388.robot.subsystems.vision; + +import com.fazecast.jSerialComm.SerialPort; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import java.io.InputStream; +import java.io.OutputStream; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import java.util.concurrent.atomic.AtomicReference; + +/** + * Robust RPLidar A1 / R1M8 Driver for FRC. + * Implements standard protocol with auto-reconnection and state monitoring. + */ +public class RPLidarA1 { + + // --- Data Types --- + + public static class PolarPoint { + public final double angle; // Degrees 0-360 + public final double distance; // Meters + + public PolarPoint(double angle, double distance) { + this.angle = angle; + this.distance = distance; + } + } + + @FunctionalInterface + public interface ScanListener { + void onScanComplete(List scan); + } + + public enum ConnectionStatus { + DISCONNECTED, // Port not found or closed + CONNECTING, // Attempting to open serial port + CONNECTED_IDLE, // Port open, but scan not started / no data yet + CONNECTED_DISABLED,// Robot is disabled, but sensor is connected + RECEIVING_DATA, // Actively receiving valid scan points + ERROR // Communication failure or timeout + } + + // --- Protocol Constants --- + private static final byte SYNC_BYTE = (byte) 0xA5; + private static final byte SYNC_BYTE2 = (byte) 0x5A; + private static final byte CMD_STOP = (byte) 0x25; + private static final byte CMD_RESET = (byte) 0x40; + private static final byte CMD_SCAN = (byte) 0x20; + private static final byte CMD_GET_HEALTH = (byte) 0x52; + + private static final int DESCRIPTOR_LEN = 7; + private static final int SCAN_PACKET_LEN = 5; + + // --- Settings --- + private static final String PORT_DESC = "CP2102 USB to UART Bridge Controller"; + private static final double WATCHDOG_TIMEOUT = 2.5; // Seconds before assuming link is dead + + // --- Members --- + private final AtomicReference mStatus = new AtomicReference<>(ConnectionStatus.DISCONNECTED); + private SerialPort mSerialPort; + private InputStream mIn; + private OutputStream mOut; + private ScanListener mListener; + + private final List mCurrentScan = new ArrayList<>(); + private double mLastDataTimestamp = 0; + // private boolean mScanningActive = false; + + public RPLidarA1() { + Thread driverThread = new Thread(this::runLoop); + driverThread.setDaemon(true); + driverThread.setName("RPLidar-Driver-Thread"); + driverThread.start(); + + Thread pwmThread = new Thread(this::funnyDTR_PWM); + pwmThread.setDaemon(true); + pwmThread.setName("RPLidar-Driver-PWM"); + pwmThread.start(); + } + + /** Sets the function to call whenever a full 360-degree rotation is parsed. */ + public void setListener(ScanListener listener) { + this.mListener = listener; + } + + // Set Speed of motor between 0 - 1 + public void setSpeed(double speed) { + this.motor_percentage = speed; + } + + public ConnectionStatus getStatus() { + return mStatus.get(); + } + + /** Signals the Lidar to stop the motor and laser. */ + private void stop_motor() { + sendCmd(CMD_RESET); + Timer.delay(0.02); + sendCmd(CMD_STOP); + mSerialPort.setDTR(); + } + + + private final static double TOGGLE_DELAY = 10; + private double motor_percentage = 0.5; + private boolean is_dtr = false; + + // Control the speed of the motor like a PWM through the DTR serial pin + // This is "PWM", like we control the speed through the percentage. + // The rate of toggles is the resolution + private void funnyDTR_PWM() { + while (!Thread.interrupted()) { + try { + ConnectionStatus status = mStatus.get(); + if (status == ConnectionStatus.RECEIVING_DATA) { + + // If the motor is at full speed + if (motor_percentage >= 1) { + // Set the motor to on + mSerialPort.clearDTR(); + // check again in a little bit + Thread.sleep(100); + } + + + // If the motor is at zero speed + if (motor_percentage <= 0) { + // Set the motor to on + mSerialPort.setDTR(); + // check again in a little bit + Thread.sleep(100); + } + + if (is_dtr) { + mSerialPort.clearDTR(); + // Sleep for main part of motor pulse + Thread.sleep((long) (TOGGLE_DELAY * motor_percentage)); + } else { + mSerialPort.setDTR(); + // Sleep for gap of motor pulse + Thread.sleep((long) (TOGGLE_DELAY * (1 - motor_percentage))); + } + + is_dtr = !is_dtr; + + } else if(status == ConnectionStatus.CONNECTED_DISABLED) { + // Stop the motor + mSerialPort.setDTR(); + + + // Sleep until we can check again + Thread.sleep(100); + + } else { // When the motor is not ready + // Sleep until we can check again + Thread.sleep(100); + } + } catch (Exception e) { + continue; + } + } + + } + + private void runLoop() { + while (!Thread.interrupted()) { + + try { + ConnectionStatus current = mStatus.get(); + + boolean robotEnabled = DriverStation.isEnabled(); + + + switch (current) { + case DISCONNECTED: + case ERROR: + attemptConnection(); + break; + + case CONNECTING: + // Handled by attemptConnection + break; + + case CONNECTED_DISABLED: + if (robotEnabled) { + mStatus.set(ConnectionStatus.CONNECTED_IDLE); + // On enable, set the last data time to now to avoid watchdog error + mLastDataTimestamp = Timer.getFPGATimestamp(); + break; + } + + // We have to check the health seperately because + // the connection check only ever occurs when + // the robot is recieving data + if (!getHealth()) { + mStatus.set(ConnectionStatus.ERROR); + } + + break; + + case CONNECTED_IDLE: + if (!robotEnabled) { + mStatus.set(ConnectionStatus.CONNECTED_DISABLED); + // On enable, set the last data time to now to avoid watchdog error + mLastDataTimestamp = Timer.getFPGATimestamp(); + break; + } + + if (initiateScanMode()) { + mStatus.set(ConnectionStatus.RECEIVING_DATA); + mLastDataTimestamp = Timer.getFPGATimestamp(); + } else { + + mStatus.set(ConnectionStatus.ERROR); + } + + break; + + case RECEIVING_DATA: + if (!robotEnabled) { + mStatus.set(ConnectionStatus.CONNECTED_DISABLED); + break; + } + + processIncomingData(); + + + checkWatchdog(); + break; + } + + Thread.sleep(200); + + } catch (Exception e) { + continue; + } + } + } + + private void attemptConnection() { + if (mSerialPort != null && mSerialPort.isOpen()) { + mSerialPort.closePort(); + } + + mStatus.set(ConnectionStatus.CONNECTING); + + SerialPort[] ports = SerialPort.getCommPorts(); + for (SerialPort p : ports) { + if (p.getPortDescription().contains(PORT_DESC)) { + mSerialPort = p; + break; + } + } + + if (mSerialPort != null) { + mSerialPort.setComPortParameters(115200, 8, SerialPort.ONE_STOP_BIT, SerialPort.NO_PARITY); + mSerialPort.setFlowControl(SerialPort.FLOW_CONTROL_DISABLED); + if (mSerialPort.openPort()) { + mIn = mSerialPort.getInputStream(); + mOut = mSerialPort.getOutputStream(); + + if (DriverStation.isEnabled()) { + mStatus.set(ConnectionStatus.CONNECTED_IDLE); + // On start, set the last data time to now to avoid watchdog error + mLastDataTimestamp = Timer.getFPGATimestamp(); + } else { + mStatus.set(ConnectionStatus.CONNECTED_DISABLED); + stop_motor(); + } + + mStatus.set(ConnectionStatus.CONNECTED_IDLE); + + // For A1: DTR False starts motor, DTR True stops it. + // mSerialPort.setDTR(); + return; + } + } + + mStatus.set(ConnectionStatus.DISCONNECTED); + Timer.delay(1.0); // Wait before retry + } + + private boolean initiateScanMode() { + try { + // Clear buffer before starting + while (mIn.available() > 0) mIn.read(); + + mSerialPort.clearDTR(); // Start Motor + + Thread.sleep(100); + + sendCmd(CMD_SCAN); + + // Wait for 7-byte descriptor + byte[] descriptor = new byte[DESCRIPTOR_LEN]; + long start = System.currentTimeMillis(); + while (mIn.available() < DESCRIPTOR_LEN) { + if (System.currentTimeMillis() - start > 1000) return false; + Timer.delay(0.01); + } + + mIn.read(descriptor); + + return descriptor[0] == SYNC_BYTE && descriptor[1] == SYNC_BYTE2; + } catch (Exception e) { + return false; + } + } + + private void processIncomingData() { + try { + while (mIn.available() >= SCAN_PACKET_LEN) { + byte[] packet = new byte[SCAN_PACKET_LEN]; + mIn.read(packet); + + // Protocol validation based on provided Python logic + boolean newScan = (packet[0] & 0x1) != 0; + boolean invNewScan = ((packet[0] >> 1) & 0x1) != 0; + int checkBit = (packet[1] & 0x1); + + if (newScan == invNewScan || checkBit != 1) { + // Out of sync - skip one byte to try and find sync again + return; + } + + mLastDataTimestamp = Timer.getFPGATimestamp(); + + // Python logic: ((raw[1] >> 1) + (raw[2] << 7)) / 64. + int angleRaw = ((packet[1] & 0xFF) >> 1) + ((packet[2] & 0xFF) << 7); + double angle = angleRaw / 64.0; + + // Python logic: (raw[3] + (raw[4] << 8)) / 4. (in mm) + int distRaw = (packet[3] & 0xFF) + ((packet[4] & 0xFF) << 8); + double distanceMeters = distRaw / 4000.0; + + if (newScan && !mCurrentScan.isEmpty()) { + if (mListener != null) { + mListener.onScanComplete(new ArrayList<>(mCurrentScan)); + } + mCurrentScan.clear(); + } + + if (distanceMeters > 0) { + mCurrentScan.add(new PolarPoint(angle, distanceMeters)); + } + } + } catch (Exception e) { + mStatus.set(ConnectionStatus.ERROR); + } + } + + private void checkWatchdog() { + if (Timer.getFPGATimestamp() - mLastDataTimestamp > WATCHDOG_TIMEOUT) { + + // // + // stop_motor(); + + mStatus.set(ConnectionStatus.CONNECTED_IDLE); + + + // We have to check the health seperately because + // the connection check only ever occurs when + // the robot is recieving data + // if (!getHealth()) { + + // DriverStation.reportWarning("RPLidar A1: Data timeout. Reconnecting...", false); + // mStatus.set(ConnectionStatus.ERROR); + + // } + } + } + + private void sendCmd(byte cmd) { + try { + if (mOut != null) { + mOut.write(new byte[]{SYNC_BYTE, cmd}); + mOut.flush(); + } + } catch (Exception e) { + mStatus.set(ConnectionStatus.ERROR); + } + } + + /** + * Queries the device health status. + * @return true if the device is connected and returns a 'Good' health status, false otherwise. + */ + public boolean getHealth() { + if (mStatus.get() == ConnectionStatus.DISCONNECTED || mOut == null || mIn == null) { + return false; + } + + try { + // Ensure the buffer is clear before sending request + while (mIn.available() > 0) mIn.read(); + + sendCmd(CMD_GET_HEALTH); + + // Read 7-byte Descriptor + byte[] descriptor = new byte[DESCRIPTOR_LEN]; + long startTime = System.currentTimeMillis(); + while (mIn.available() < DESCRIPTOR_LEN) { + if (System.currentTimeMillis() - startTime > 500) return false; + Timer.delay(0.01); + } + mIn.read(descriptor); + + return true; + + // // Check if descriptor is valid and data type matches HEALTH (0x06) + // if (descriptor[0] != SYNC_BYTE || descriptor[1] != SYNC_BYTE2 || descriptor[6] != 0x06) { + // return false; + // } + + // // Read 3-byte Health Payload + // byte[] healthPayload = new byte[3]; + // while (mIn.available() < 3) { + // if (System.currentTimeMillis() - startTime > 1000) return false; + // Timer.delay(0.01); + // } + // mIn.read(healthPayload); + + // Byte 0 is status: 0x00 = Good, 0x01 = Warning, 0x02 = Error + // return healthPayload[0] == 0; + } catch (Exception e) { + return false; + } + } +} \ No newline at end of file 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..bc6aae9 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/vision/Vision.java @@ -0,0 +1,102 @@ +package frc4388.robot.subsystems.vision; + +import java.util.ArrayList; +import java.util.List; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.Utils; + +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; +import frc4388.utility.structs.LEDPatterns; + +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]); + } + Logger.recordOutput("Vision/isTagDectected", isTag()); + + // if (isTag()){ + // m_robotLED.setMode(LEDPatterns.SOLID_GREEN_DARK); + // } + } + + public List getPosesToAdd(){ + List poses = new ArrayList<>(); + for(int i = 0; i < state.length; i++) { + if(state[i].lastEstimatedPose != null) { + poses.add(state[i].lastEstimatedPose); + } + } + + return poses; + } + + public void setLastOdomPose(Pose2d pose){ + if(pose != null) + lastPhysOdomPose = pose; + } + + public boolean isTag(){ + for(int i = 0; i < state.length; i++){ + if(state[i].isTagDetected && state[i].isTagProcessed) + return true; + } + return false; + } + + @AutoLogOutput + public Pose2d getPose2d() { + if(lastPhysOdomPose != null) + return lastPhysOdomPose; + + // if(lastVisionPose != null) + // return lastVisionPose; + return new Pose2d(); + + } + + public static double getTime() { + return Utils.getCurrentTimeSeconds(); + } + + + @Override + public Status diagnosticStatus() { + return new Status(); + // // TODO Auto-generated method stub + // throw new UnsupportedOperationException("Unimplemented method 'diagnosticStatus'"); + } + + // Simple LED helper class for compilation and basic usage; replace with real implementation if available. + private static class LED { + public void setMode(LEDPatterns mode) { + // no-op stub for compilation; integrate with hardware driver as needed + } + } +} diff --git a/src/main/java/frc4388/robot/subsystems/vision/VisionIO.java b/src/main/java/frc4388/robot/subsystems/vision/VisionIO.java new file mode 100644 index 0000000..f979135 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/vision/VisionIO.java @@ -0,0 +1,22 @@ +package frc4388.robot.subsystems.vision; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.geometry.Pose2d; + +public interface VisionIO { + @AutoLog + public class VisionState { + public boolean isTagDetected = false; + public boolean isTagProcessed = false; + // public double latency = 0; + public PoseObservation lastEstimatedPose = null; + } + + public static record PoseObservation( + Pose2d pose, + double timestamp + ) {} + + public default void updateInputs(VisionState state) {} +} diff --git a/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java new file mode 100644 index 0000000..1ff346f --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java @@ -0,0 +1,158 @@ +package frc4388.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Transform3d; +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; +import frc4388.robot.constants.FieldConstants; +import frc4388.robot.constants.VisionConstants; + +public class VisionReal implements VisionIO { + // private PhotonCamera[] cameras; + // private PhotonPoseEstimator[] estimators; + + private PhotonCamera camera; + private PhotonPoseEstimator estimator; + + // public List poses = new ArrayList<>(); + + + public VisionReal(PhotonCamera camera, Transform3d position){ + this.camera = camera; + estimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, position); + estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + } + + // private Instant lastVisionTime = null; + + + public void updateInputs(VisionState state) { + state.isTagProcessed = false; + state.isTagDetected = false; + + state.lastEstimatedPose = null; + + var results = camera.getAllUnreadResults(); + + // If there are no more updates from the camera + if (results.size() == 0) + return; + + + var result = results.get(results.size()-1); + + // If there are no tags + if(!result.hasTargets()) + return; + + state.isTagDetected = result.hasTargets(); + + 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/DualJoystickButton.java b/src/main/java/frc4388/utility/DualJoystickButton.java deleted file mode 100644 index e4ef5ed..0000000 --- a/src/main/java/frc4388/utility/DualJoystickButton.java +++ /dev/null @@ -1,22 +0,0 @@ -package frc4388.utility; - -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj2.command.button.Trigger; - - -/** - * A button binding for two controllers, preferably an {@link frc4388.utility.controller.DeadbandedXboxController Xbox Controller} and {@link frc4388.utility.controller.VirtualController Virtual Xbox Controller} - * @author Zachary Wilke - */ -public class DualJoystickButton extends Trigger { - - /** - * Creates an Button binding on two controllers - * @param joystickA A controller - * @param joystickB A controller - * @param buttonNumber The button to bind to - */ - public DualJoystickButton(GenericHID joystickA, GenericHID joystickB, int buttonNumber) { - super(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber))); - } -} diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java deleted file mode 100644 index 966d2e0..0000000 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ /dev/null @@ -1,269 +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; - -/** - * 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 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/FieldPositions.java b/src/main/java/frc4388/utility/compute/FieldPositions.java new file mode 100644 index 0000000..e48ebfb --- /dev/null +++ b/src/main/java/frc4388/utility/compute/FieldPositions.java @@ -0,0 +1,24 @@ +package frc4388.utility.compute; + +import edu.wpi.first.math.geometry.Translation2d; + +public class FieldPositions { + // public static final Translation2d RED_HUB_POSITION = new Translation2d(0, 0); + // public static final Translation2d BLUE_HUB_POSITION = new Translation2d(0, 0); + public static final Translation2d RED_HUB_POSITION = new Translation2d(11.9014494, 4.0213534);// + 0.3); + public static final Translation2d BLUE_HUB_POSITION = new Translation2d(4.61155415, 4.0213534);// + 0.3); + + + // We set the default position to one just in case it doesn't update + // Setting to null could cause a null pointer, and setting to (0,0) could cause problems + // I would rather have the 50/50 chance than a code error + public static Translation2d HUB_POSITION = BLUE_HUB_POSITION; + + public static void update() { + if(TimesNegativeOne.isRed) { + HUB_POSITION = RED_HUB_POSITION; + } else { + HUB_POSITION = BLUE_HUB_POSITION; + } + } +} diff --git a/src/main/java/frc4388/utility/compute/HubShiftTimer.java b/src/main/java/frc4388/utility/compute/HubShiftTimer.java new file mode 100644 index 0000000..6f91284 --- /dev/null +++ b/src/main/java/frc4388/utility/compute/HubShiftTimer.java @@ -0,0 +1,154 @@ + +package frc4388.utility.compute; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; +public class HubShiftTimer { + + public enum ShiftPhase { + DISABLED, + AUTO, + TRANSITION, // 0 – 10 s + SHIFT1, // 10 – 35 s + SHIFT2, // 35 – 60 s + SHIFT3, // 60 – 85 s + SHIFT4, // 85 – 110 s + ENDGAME // 110 – 140 s + } + + public record ShiftInfo( + ShiftPhase phase, + double elapsedInShift, + double remainingInShift, + boolean isActive) {} +//total teleop time + public static final double TELEOP_DURATION = 140.0; +//total auto time + public static final double AUTO_DURATION = 20.0; + +//shift start and end times for calculations + private static final double[] SHIFT_STARTS = {0.0, 10.0, 35.0, 60.0, 85.0, 110.0}; + private static final double[] SHIFT_ENDS = {10.0, 35.0, 60.0, 85.0, 110.0, 140.0}; + +//hub active schedule, true is active and false is inactive +//starts always as active becasue transition is first and is active, but then is inactive for winner or active for loser + private static final boolean[] WINNER_SCHEDULE = {true, false, true, false, true, true}; + private static final boolean[] LOSER_SCHEDULE = {true, true, false, true, false, true}; + +//shift phase names during teleop + private static final ShiftPhase[] SHIFT_PHASES = { + ShiftPhase.TRANSITION, + ShiftPhase.SHIFT1, + ShiftPhase.SHIFT2, + ShiftPhase.SHIFT3, + ShiftPhase.SHIFT4, + ShiftPhase.ENDGAME + }; + +//timer to track time + private static final Timer teleopTimer = new Timer(); + private static double timerOffset = 0.0; + +//fms syncing idk other team did it too + private static final double RESYNC_THRESHOLD = 3.0; + + +//call at start of auto to start timer + public static void initializeAuto() { + teleopTimer.restart(); + } + +//call at start of teleop to start timer again because sometimes delay between auto and telop + public static void initializeTeleop() { + timerOffset = 0.0; + teleopTimer.restart(); + } + + + +//returns the updated shift info based on the winner of auto + public static ShiftInfo getShiftInfo() { + if (!DriverStation.isEnabled()) { + return new ShiftInfo(ShiftPhase.DISABLED, 0.0, 0.0, false); + } + if (DriverStation.isAutonomousEnabled()) { + double autoElapsed = teleopTimer.get(); // timer restarts in initialize() + return new ShiftInfo( + ShiftPhase.AUTO, + autoElapsed, + Math.max(0.0, AUTO_DURATION - teleopTimer.get()), + true); + } + return computeTeleopShift(); + } + +//find auto winner, R = red wins, B = blue wins + public static Alliance autoWinnerAlliance() { + String msg = DriverStation.getGameSpecificMessage(); + if (msg != null && msg.length() > 0) { + char c = msg.charAt(0); + if (c == 'R') return Alliance.Red; + if (c == 'B') return Alliance.Blue; + } + // backup if no msg, returns auto winner as opposite of our alliance. if we red -> blue wins auto + Alliance ours = DriverStation.getAlliance().orElse(Alliance.Blue); + return (ours == Alliance.Blue) ? Alliance.Red : Alliance.Blue; + } + + + //return our schedule for the shifts + private static boolean[] getSchedule() { + Alliance ours = DriverStation.getAlliance().orElse(Alliance.Blue); + Alliance winner = autoWinnerAlliance(); + return (ours == winner) ? WINNER_SCHEDULE : LOSER_SCHEDULE; + } + +//time since start of teleop + private static double getTeleopElapsed() { + double localTime = teleopTimer.get() - timerOffset; + + // Re-sync to FMS time if we drift too far (only when FMS is attached) + if (DriverStation.isFMSAttached()) { + double fmsElapsed = TELEOP_DURATION - DriverStation.getMatchTime(); + if (fmsElapsed <= TELEOP_DURATION - 5.0 // ignore the first few seconds of jitter + && Math.abs(localTime - fmsElapsed) >= RESYNC_THRESHOLD) { + timerOffset += localTime - fmsElapsed; + localTime = fmsElapsed; + } + } + return Math.max(0.0, Math.min(TELEOP_DURATION, localTime)); + } + + private static ShiftInfo computeTeleopShift() { + boolean[] schedule = getSchedule(); + double elapsed = getTeleopElapsed(); + + // Find which shift we're in + int phaseIndex = SHIFT_STARTS.length - 1; // default to last shift if past all bounds + for (int i = 0; i < SHIFT_STARTS.length; i++) { + if (elapsed >= SHIFT_STARTS[i] && elapsed < SHIFT_ENDS[i]) { + phaseIndex = i; + break; + } + } + + double shiftElapsed = elapsed - SHIFT_STARTS[phaseIndex]; + double shiftRemaining = SHIFT_ENDS[phaseIndex] - elapsed; + + // merge time for elapsed if same active/inactive + if (phaseIndex > 0 && schedule[phaseIndex] == schedule[phaseIndex - 1]) { + shiftElapsed = elapsed - SHIFT_STARTS[phaseIndex - 1]; + } + + // merge time for remaining time if same active/inactive status + if (phaseIndex < SHIFT_ENDS.length - 1 && schedule[phaseIndex] == schedule[phaseIndex + 1]) { + shiftRemaining = SHIFT_ENDS[phaseIndex + 1] - elapsed; + } + + return new ShiftInfo( + SHIFT_PHASES[phaseIndex], + shiftElapsed, + shiftRemaining, + schedule[phaseIndex]); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/compute/JankCoder copy.java.old b/src/main/java/frc4388/utility/compute/JankCoder copy.java.old new file mode 100644 index 0000000..8abe01f --- /dev/null +++ b/src/main/java/frc4388/utility/compute/JankCoder copy.java.old @@ -0,0 +1,100 @@ +package frc4388.utility.compute; + +import static edu.wpi.first.units.Units.Rotations; + +import java.io.FileInputStream; +import java.io.FileOutputStream; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.DutyCycleEncoder; + +public class JankCoder { + DutyCycleEncoder m_encoder; + + boolean loaded_rotations = false; + double lastRotation = 0; + double offset = 0; + + public JankCoder(int dio_channel) { + m_encoder = new DutyCycleEncoder(dio_channel); + loadRotations(); + } + public JankCoder(int dio_channel, double _offset) { + m_encoder = new DutyCycleEncoder(dio_channel); + loadRotations(); + } + + public void update() { + if(!m_encoder.isConnected()) { + return; + } + + if(!loaded_rotations) { + loadRotations(); + } else { + double curRot = m_encoder.get(); + if(lastRotation - curRot > 0.5) { + offset += 1; + saveRotations(); + } else if (curRot - lastRotation > 0.5) { + offset -= 1; + saveRotations(); + } + + lastRotation = curRot; + } + } + + public double get() { + return (double) offset + m_encoder.get(); + } + + public Angle getRotations() { + return Rotations.of(get()); + } + + public int getRotationCount() { + return (int) offset; + } + + public void resetRotation() { + setRotation(0); + } + + public void setRotation(double rotation) { + offset = rotation - m_encoder.get(); + saveRotations(); + } + + private void saveRotations() { + try (FileOutputStream stream = new FileOutputStream("/home/lvuser/encoder" + m_encoder.getSourceChannel())) { + stream.write(DataUtils.doubleToByteArray(offset)); + } catch (Exception e) { + // e.printStackTrace(); + System.out.println("ENCODER: Unable to write to trim file `" + m_encoder.getSourceChannel() + "`!?!"); + } + } + + private boolean loadRotations() { + lastRotation = m_encoder.get(); + this.loaded_rotations = true; + + try (FileInputStream stream = new FileInputStream("/home/lvuser/encoder" + m_encoder.getSourceChannel())) { + offset = DataUtils.byteArrayToDouble(stream.readNBytes(4)); + + // 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("ENCODER: Unable to read encoder `" + m_encoder.getSourceChannel() + "`, using current value..."); + return false; + } + + } +} diff --git a/src/main/java/frc4388/utility/compute/JankCoder.java b/src/main/java/frc4388/utility/compute/JankCoder.java new file mode 100644 index 0000000..63a23d8 --- /dev/null +++ b/src/main/java/frc4388/utility/compute/JankCoder.java @@ -0,0 +1,105 @@ +package frc4388.utility.compute; + +import static edu.wpi.first.units.Units.Rotations; + +import java.io.FileInputStream; +import java.io.FileOutputStream; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import frc4388.utility.configurable.ConfigurableDouble; + +public class JankCoder { + DutyCycleEncoder m_encoder; + + boolean loaded_rotations = false; + int rotations = 0; + double lastRotation = 0; + final ConfigurableDouble offset; + + public JankCoder(int dio_channel, ConfigurableDouble offset) { + this.offset = offset; + m_encoder = new DutyCycleEncoder(dio_channel); + loadRotations(); + } + + public void update() { + // if(!m_encoder.isConnected()) { + // return; + // } + + if(!loaded_rotations) { + loadRotations(); + } else { + double curRot = m_encoder.get(); + if(lastRotation - curRot > 0.5) { + rotations += 1; + saveRotations(); + } else if (curRot - lastRotation > 0.5) { + rotations -= 1; + saveRotations(); + } + + lastRotation = curRot; + } + } + + public double get() { + return (double) rotations + m_encoder.get() + offset.get(); + } + + public Angle getRotations() { + return Rotations.of(get()); + } + + public int getRotationCount() { + return rotations; + } + + public void resetRotations() { + offset.set(-m_encoder.get()); + setRotations(0); + } + + public void setRotations(int rotation) { + rotations = rotation; + saveRotations(); + } + + public boolean isConnected() { + return m_encoder.isConnected(); + } + + public void saveRotations() { + try (FileOutputStream stream = new FileOutputStream("/home/lvuser/encoder" + m_encoder.getSourceChannel())) { + stream.write(DataUtils.intToByteArray(rotations)); + } catch (Exception e) { + // e.printStackTrace(); + System.out.println("ENCODER: Unable to write to trim file `" + m_encoder.getSourceChannel() + "`!?!"); + } + } + + public boolean loadRotations() { + lastRotation = m_encoder.get(); + this.loaded_rotations = true; + + try (FileInputStream stream = new FileInputStream("/home/lvuser/encoder" + m_encoder.getSourceChannel())) { + int fileValue = DataUtils.byteArrayToInt(stream.readNBytes(4)); + + rotations = 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("ENCODER: Unable to read encoder `" + m_encoder.getSourceChannel() + "`, using current value..."); + return false; + } + + } +} 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/configurable/ConfigurableDouble.java b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java index c0384db..a6023bb 100644 --- a/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java +++ b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java @@ -20,4 +20,8 @@ public class ConfigurableDouble { public double get() { return SmartDashboard.getNumber(name, defualtValue); } + + public void set(double value) { + SmartDashboard.putNumber(name, value); + } } diff --git a/src/main/java/frc4388/utility/configurable/ConfigurableString.java b/src/main/java/frc4388/utility/configurable/ConfigurableString.java index 34c0290..69b144b 100644 --- a/src/main/java/frc4388/utility/configurable/ConfigurableString.java +++ b/src/main/java/frc4388/utility/configurable/ConfigurableString.java @@ -11,6 +11,7 @@ public class ConfigurableString { * @param name the name of the Smart Dashboard key. * @param defualtValue the initilization value */ + public ConfigurableString(String name, String defualtValue) { this.name = name; this.defualtValue = defualtValue; @@ -20,4 +21,5 @@ public class ConfigurableString { public String get() { return SmartDashboard.getString(name, defualtValue); } + } diff --git a/src/main/java/frc4388/utility/controller/ButtonBox.java b/src/main/java/frc4388/utility/controller/ButtonBox.java new file mode 100644 index 0000000..33c7744 --- /dev/null +++ b/src/main/java/frc4388/utility/controller/ButtonBox.java @@ -0,0 +1,19 @@ +package frc4388.utility.controller; + +import edu.wpi.first.wpilibj.GenericHID; + +public class ButtonBox extends GenericHID { + public static final int White = 1; + public static final int One = 2; + public static final int Two = 3; + public static final int Three = 4; + public static final int Four = 5; + public static final int Five = 6; + public static final int Six = 7; + public static final int Seven = 8; + public static final int Eight = 9; + + public ButtonBox(int ID){ + super(ID); + } +} diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java 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..351dda9 --- /dev/null +++ b/src/main/java/frc4388/utility/status/Alerts.java @@ -0,0 +1,9 @@ +package frc4388.utility.status; + +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; + +// Class to update a series of WPILIB Alerts +public class Alerts { + public static Alert no_auto = new Alert("No auto has been selected!", AlertType.kError); +} diff --git a/src/main/java/frc4388/utility/status/CanDevice.java b/src/main/java/frc4388/utility/status/CanDevice.java new file mode 100644 index 0000000..ed49c4f --- /dev/null +++ b/src/main/java/frc4388/utility/status/CanDevice.java @@ -0,0 +1,58 @@ +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; + } + + + public Object getId() { + return id; + } + + +} diff --git a/src/main/java/frc4388/utility/status/FaultA1M8.java b/src/main/java/frc4388/utility/status/FaultA1M8.java new file mode 100644 index 0000000..be9fc06 --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultA1M8.java @@ -0,0 +1,39 @@ +package frc4388.utility.status; + +import frc4388.robot.subsystems.vision.RPLidarA1; +import frc4388.robot.subsystems.vision.RPLidarA1.ConnectionStatus; +import frc4388.utility.status.Status.ReportLevel; + +// Fault reporter for the RPLidar A1M8 Revolving lidar sensor +public class FaultA1M8 implements Queryable { + private String name; + private RPLidarA1 cam; + + public static void addDevice(RPLidarA1 cam, String name) { + FaultA1M8 p = new FaultA1M8(); + + p.name = name; + p.cam = cam; + + FaultReporter.register(p); + } + + @Override + public String getName() { + return name; + } + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + + ConnectionStatus cam_ConnectionStatus = cam.getStatus(); + + if(cam_ConnectionStatus != ConnectionStatus.RECEIVING_DATA) + s.addReport(ReportLevel.ERROR, "Not Connected! Status = " + cam_ConnectionStatus); + + s.addReport(ReportLevel.INFO, cam.getStatus().toString()); + + return s; + } +} \ No newline at end of file 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..732bef3 --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultReporter.java @@ -0,0 +1,131 @@ +package frc4388.utility.status; + +import java.util.ArrayList; +import java.util.List; + +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/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/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..b4e470d --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,35 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "26.0.2", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2026", + "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": "26.0.2" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "26.0.2", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json index e978a5f..92ed2f1 100644 --- a/vendordeps/NavX.json +++ b/vendordeps/NavX.json @@ -3,7 +3,7 @@ "name": "NavX", "version": "2024.1.0", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "frcYear": "2024", + "frcYear": "2026", "mavenUrls": [ "https://dev.studica.com/maven/release/2024/" ], @@ -37,4 +37,4 @@ ] } ] -} \ No newline at end of file +} diff --git a/vendordeps/PathplannerLib-2026.1.2.json b/vendordeps/PathplannerLib-2026.1.2.json new file mode 100644 index 0000000..f72fa41 --- /dev/null +++ b/vendordeps/PathplannerLib-2026.1.2.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib-2026.1.2.json", + "name": "PathplannerLib", + "version": "2026.1.2", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2026", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2026.1.2" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2026.1.2", + "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/Phoenix5-5.36.0.json b/vendordeps/Phoenix5-5.36.0.json new file mode 100644 index 0000000..9a27e47 --- /dev/null +++ b/vendordeps/Phoenix5-5.36.0.json @@ -0,0 +1,171 @@ +{ + "fileName": "Phoenix5-5.36.0.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.36.0", + "frcYear": "2026", + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2026-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-frc2026-latest.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-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-frc2026-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-frc2026-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.36.0" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.36.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.36.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.36.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.36.0", + "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.36.0", + "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.36.0", + "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.36.0", + "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.36.0", + "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.36.0", + "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-26.1.3.json similarity index 65% rename from vendordeps/Phoenix6.json rename to vendordeps/Phoenix6-26.1.3.json index 0322385..d5bc4a2 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6-26.1.3.json @@ -1,50 +1,80 @@ { - "fileName": "Phoenix6.json", + "fileName": "Phoenix6-26.1.3.json", "name": "CTRE-Phoenix (v6)", - "version": "24.3.0", - "frcYear": 2024, + "version": "26.1.3", + "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-latest.json", "conflictsWith": [ { - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", - "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", - "offlineFileName": "Phoenix6And5.json" + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2026-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.3.0" + "version": "26.1.3" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "24.3.0", + "artifactId": "api-cpp", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.3.0", + "artifactId": "api-cpp-sim", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -52,25 +82,13 @@ { "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", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -78,12 +96,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.3.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -91,25 +110,13 @@ { "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", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -117,12 +124,27 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.3.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -130,12 +152,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.3.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -143,12 +166,55 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.3.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -158,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.3.0", + "version": "26.1.3", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -166,6 +232,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -173,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.3.0", + "version": "26.1.3", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -181,6 +248,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -188,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.3.0", + "version": "26.1.3", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -196,6 +264,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -203,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.3.0", + "version": "26.1.3", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -211,6 +280,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -218,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.3.0", + "version": "26.1.3", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,21 +296,7 @@ "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", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -248,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.3.0", + "version": "26.1.3", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,6 +312,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -263,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.3.0", + "version": "26.1.3", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -271,21 +328,7 @@ "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", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -293,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.3.0", + "version": "26.1.3", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -301,6 +344,23 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "26.1.3", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -308,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.3.0", + "version": "26.1.3", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -316,6 +376,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -323,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.3.0", + "version": "26.1.3", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -331,6 +392,55 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "26.1.3", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "26.1.3", + "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": "26.1.3", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..c6b1b9e --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,133 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2026.0.5", + "frcYear": "2026", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2026.0.5" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2026.0.5", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibBackendDriver", + "version": "2026.0.5", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibWpiBackendDriver", + "version": "2026.0.5", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2026.0.5", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2026.0.5", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibBackendDriver", + "version": "2026.0.5", + "libName": "BackendDriver", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibWpiBackendDriver", + "version": "2026.0.5", + "libName": "REVLibWpi", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index 67bf389..d90630e 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -3,7 +3,7 @@ "name": "WPILib-New-Commands", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "frcYear": "2024", + "frcYear": "2026", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [ @@ -25,6 +25,7 @@ "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ + "linuxsystemcore", "linuxathena", "linuxarm32", "linuxarm64", diff --git a/vendordeps/navx_frc.json b/vendordeps/navx_frc.json deleted file mode 100644 index dca1d82..0000000 --- a/vendordeps/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/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..a4ec272 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2026.3.2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2026", + "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": "v2026.3.2", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2026.3.2", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2026.3.2", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2026.3.2" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2026.3.2" + } + ] +} \ No newline at end of file