From b1246cb338935cca95fcbd2764a00bff90f74cbb Mon Sep 17 00:00:00 2001 From: EJainDev Date: Wed, 16 Sep 2026 21:49:32 -0400 Subject: [PATCH 01/15] update to 2027 alpha --- .run/Build Robot.run.xml | 6 +- .wpilib/wpilib_preferences.json | 2 +- build.gradle | 141 ++++++++++-------- gradle/wrapper/gradle-wrapper.properties | 2 +- vendordeps/AdvantageKit.json | 10 +- ...WPILibNewCommands.json => CommandsV2.json} | 24 ++- vendordeps/PathplannerLib.json | 15 +- vendordeps/Phoenix6.json | 78 +++++----- vendordeps/REVLib.json | 43 +++--- vendordeps/URCL.json | 65 -------- vendordeps/photonlib.json | 20 +-- 11 files changed, 178 insertions(+), 228 deletions(-) rename vendordeps/{WPILibNewCommands.json => CommandsV2.json} (52%) delete mode 100644 vendordeps/URCL.json diff --git a/.run/Build Robot.run.xml b/.run/Build Robot.run.xml index 41ebccc55..38e44441a 100644 --- a/.run/Build Robot.run.xml +++ b/.run/Build Robot.run.xml @@ -4,7 +4,7 @@ @@ -17,6 +17,7 @@ true true + false false + false + false + false \ No newline at end of file diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 53efa6e23..9727c0951 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2025", + "projectYear": "2027", "teamNumber": 467 } diff --git a/build.gradle b/build.gradle index dc8dc8c8c..bf50cdb02 100755 --- a/build.gradle +++ b/build.gradle @@ -1,76 +1,84 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2026.2.1" + id "org.wpilib.GradleRIO" version "2027.0.0-alpha-5" id "com.peterabeles.gversion" version "1.10" id "com.diffplug.spotless" version "6.25.0" } +wpi.maven.useLocal = false +wpi.maven.useDevelopment = true + +wpi.versions.wpilibVersion = providers.gradleProperty('wpilibVersion').orElse('2027.0.0-alpha-6').get() + java { - sourceCompatibility = JavaVersion.VERSION_17 - targetCompatibility = JavaVersion.VERSION_17 + sourceCompatibility = JavaVersion.VERSION_25 + targetCompatibility = JavaVersion.VERSION_25 } def ROBOT_MAIN_CLASS = "frc.robot.Main" // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. -deploy { - targets { - roborio(getTargetTypeClass('RoboRIO')) { - // Team number is loaded either from the .wpilib/wpilib_preferences.json - // or from command line. If not found an exception will be thrown. - // You can use getTeamOrDefault(team) instead of getTeamNumber if you - // want to store a team number in this file. - team = project.frc.getTeamNumber() - debug = project.frc.getDebugOrDefault(false) - - artifacts { - // First part is artifact name, 2nd is artifact type - // getTargetTypeClass is a shortcut to get the class type using a string - - frcJava(getArtifactTypeClass('FRCJavaArtifact')) { - jvmArgs.add("-XX:+UnlockExperimentalVMOptions") - jvmArgs.add("-XX:GCTimeRatio=5") - jvmArgs.add("-XX:+UseSerialGC") - jvmArgs.add("-XX:MaxGCPauseMillis=50") - - // Heap size is set dynamically in predeploy based on robot type - jvmArgs.add("-XX:+AlwaysPreTouch") - - predeploy << { ctx -> - def robotTypeFile = "/home/lvuser/robot" - def result = ctx.execute("cat " + robotTypeFile) - def robotType = result.result.trim() - println "Detected robot type: " + robotType - - def heapSize = 100 - if (robotType == "ROBOT_BRIEFCASE") { - heapSize = 10 - println "Using reduced heap size for ROBOT_BRIEFCASE: " + heapSize + "MB" - } else { - println "Using default heap size: " + heapSize + "MB" +def roborioTargetType = deploy.getTargetTypeClass('RoboRIO') +def deployArtifact = null +if (roborioTargetType != null) { + deploy { + targets { + roborio(roborioTargetType) { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = project.frc.getTeamNumber() + debug = project.frc.getDebugOrDefault(false) + + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string + + frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + jvmArgs.add("-XX:+UnlockExperimentalVMOptions") + jvmArgs.add("-XX:GCTimeRatio=5") + jvmArgs.add("-XX:+UseSerialGC") + jvmArgs.add("-XX:MaxGCPauseMillis=50") + + // Heap size is set dynamically in predeploy based on robot type + jvmArgs.add("-XX:+AlwaysPreTouch") + + predeploy << { ctx -> + def robotTypeFile = "/home/lvuser/robot" + def result = ctx.execute("cat " + robotTypeFile) + def robotType = result.result.trim() + println "Detected robot type: " + robotType + + def heapSize = 100 + if (robotType == "ROBOT_BRIEFCASE") { + heapSize = 10 + println "Using reduced heap size for ROBOT_BRIEFCASE: " + heapSize + "MB" + } else { + println "Using default heap size: " + heapSize + "MB" + } + + jvmArgs.add("-Xmx" + heapSize + "M") + jvmArgs.add("-Xms" + heapSize + "M") } - - jvmArgs.add("-Xmx" + heapSize + "M") - jvmArgs.add("-Xms" + heapSize + "M") } - } - // Static files artifact - frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { - files = project.fileTree('src/main/deploy') - directory = '/home/lvuser/deploy' - // Change to true to delete files on roboRIO that no - // longer exist in deploy directory on roboRIO - deleteOldFiles = false + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + // Change to true to delete files on roboRIO that no + // longer exist in deploy directory on roboRIO + deleteOldFiles = false + } } } } + deployArtifact = deploy.targets.roborio.artifacts.frcJava } } -def deployArtifact = deploy.targets.roborio.artifacts.frcJava - // Set to true to use debug for JNI. wpi.java.debugJni = false @@ -90,11 +98,13 @@ dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() - roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) - roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + if (roborioTargetType != null) { + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) + roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) - roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) - roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + } nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) @@ -104,15 +114,12 @@ dependencies { nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() - compileOnly 'org.projectlombok:lombok:1.18.30' - annotationProcessor 'org.projectlombok:lombok:1.18.30' + compileOnly 'org.projectlombok:lombok:1.18.44' + annotationProcessor 'org.projectlombok:lombok:1.18.44' testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' - compileOnly 'org.projectlombok:lombok:1.18.30' - annotationProcessor 'org.projectlombok:lombok:1.18.30' - def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" } @@ -141,12 +148,14 @@ jar { } } from sourceSets.main.allSource - manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) + manifest org.wpilib.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE } // Configure jar and deploy tasks -deployArtifact.jarTask = jar +if (deployArtifact != null) { + deployArtifact.jarTask = jar +} wpi.java.configureExecutableTasks(jar) wpi.java.configureTestTasks(test) @@ -160,7 +169,9 @@ task(checkRobot, dependsOn: "classes", type: JavaExec) { mainClass = "frc.robot.Constants" classpath = sourceSets.main.runtimeClasspath } -deployroborio.dependsOn(checkRobot) +if (deployArtifact != null) { + deployroborio.dependsOn(checkRobot) +} // Create version file project.compileJava.dependsOn(createVersionFile) @@ -206,8 +217,8 @@ task(eventDeploy) { } createVersionFile.dependsOn(eventDeploy) -// Spotless formatting -project.compileJava.dependsOn(spotlessApply) +// Spotless formatting (disabled for 2027 experimental) +// project.compileJava.dependsOn(spotlessApply) spotless { java { target fileTree(".") { diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 73fd75353..b5029cebe 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.10-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-9.7.1-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 2faa4db80..1a8abd54c 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,9 +1,9 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "26.0.0", + "version": "27.0.0-alpha-4", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", - "frcYear": "2026", + "wpilibYear": "2027_alpha5", "mavenUrls": [ "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" ], @@ -12,18 +12,18 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "26.0.0" + "version": "27.0.0-alpha-4" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "26.0.0", + "version": "27.0.0-alpha-4", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ - "linuxathena", + "linuxsystemcore", "linuxx86-64", "linuxarm64", "osxuniversal", diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/CommandsV2.json similarity index 52% rename from vendordeps/WPILibNewCommands.json rename to vendordeps/CommandsV2.json index c54ae11fa..8358b098e 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/CommandsV2.json @@ -1,30 +1,38 @@ { - "fileName": "WPILibNewCommands.json", - "name": "WPILib-New-Commands", + "fileName": "CommandsV2.json", + "name": "Commands V2", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "frcYear": "2026", + "wpilibYear": "2027_alpha5", "mavenUrls": [], "jsonUrl": "", + "conflictsWith": [ + { + "uuid": "4decdc05-a056-46cf-9561-39449bbb01306", + "errorMessage": "Users can not have both Commands v2 and Commands v3 vendordeps in their robot program.", + "offlineFileName": "CommandsV3.json" + } + ], "javaDependencies": [ { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-java", + "groupId": "org.wpilib.commandsv2", + "artifactId": "commandsv2-java", "version": "wpilib" } ], "jniDependencies": [], "cppDependencies": [ { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-cpp", + "groupId": "org.wpilib.commandsv2", + "artifactId": "commandsv2-cpp", "version": "wpilib", - "libName": "wpilibNewCommands", + "libName": "commandsv2", "headerClassifier": "headers", "sourcesClassifier": "sources", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ + "linuxsystemcore", "linuxathena", "linuxarm32", "linuxarm64", diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 6d78d24b4..9b36d2bf1 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,18 +1,18 @@ { - "fileName": "PathplannerLib.json", + "fileName": "PathplannerLibSystemCoreAlpha.json", "name": "PathplannerLib", - "version": "2026.1.2", + "version": "2027.0.0-alpha-3", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2026", + "wpilibYear": "2027_alpha5", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLibSystemCoreAlpha.json", "javaDependencies": [ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2026.1.2" + "version": "2027.0.0-alpha-3" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2026.1.2", + "version": "2027.0.0-alpha-3", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -29,8 +29,7 @@ "windowsx86-64", "linuxx86-64", "osxuniversal", - "linuxathena", - "linuxarm32", + "linuxsystemcore", "linuxarm64" ] } diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 328a963c2..40dd410c2 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,60 +1,60 @@ { - "fileName": "Phoenix6-frc2026-latest.json", + "fileName": "Phoenix6-26.50.0-alpha-1.json", "name": "CTRE-Phoenix (v6)", - "version": "26.1.0", - "frcYear": "2026", + "version": "26.50.0-alpha-1", + "wpilibYear": "2027_alpha5", "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-frc2026-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2027-latest.json", "conflictsWith": [ { "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", - "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", - "offlineFileName": "Phoenix6-replay-frc2026-latest.json" + "errorMessage": "Users cannot have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2027-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.0" + "version": "26.50.0-alpha-1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", "linuxarm64", - "linuxathena" + "linuxsystemcore" ], "simMode": "hwsim" }, { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", "linuxarm64", - "linuxathena" + "linuxsystemcore" ], "simMode": "hwsim" }, { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,14 +233,14 @@ "windowsx86-64", "linuxx86-64", "linuxarm64", - "linuxathena" + "linuxsystemcore" ], "simMode": "hwsim" }, { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -249,14 +249,14 @@ "windowsx86-64", "linuxx86-64", "linuxarm64", - "linuxathena" + "linuxsystemcore" ], "simMode": "hwsim" }, { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.50.0-alpha-1", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index bb613bf67..7efb9b64a 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,63 +1,60 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2026.0.1", - "frcYear": "2026", + "version": "2027.0.0-alpha-6", + "wpilibYear": "2027_alpha5", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "https://maven.revrobotics.com/" ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json", + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2027.json", "javaDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2026.0.1" + "version": "2027.0.0-alpha-6" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.1", + "version": "2027.0.0-alpha-6", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ "windowsx86-64", "linuxarm64", "linuxx86-64", - "linuxathena", - "linuxarm32", + "linuxsystemcore", "osxuniversal" ] }, { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", + "version": "2027.0.0-alpha-6", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ "windowsx86-64", "linuxarm64", "linuxx86-64", - "linuxathena", - "linuxarm32", + "linuxsystemcore", "osxuniversal" ] }, { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", + "version": "2027.0.0-alpha-6", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ "windowsx86-64", "linuxarm64", "linuxx86-64", - "linuxathena", - "linuxarm32", + "linuxsystemcore", "osxuniversal" ] } @@ -66,7 +63,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2026.0.1", + "version": "2027.0.0-alpha-6", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -75,15 +72,14 @@ "windowsx86-64", "linuxarm64", "linuxx86-64", - "linuxathena", - "linuxarm32", + "linuxsystemcore", "osxuniversal" ] }, { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.1", + "version": "2027.0.0-alpha-6", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -92,15 +88,14 @@ "windowsx86-64", "linuxarm64", "linuxx86-64", - "linuxathena", - "linuxarm32", + "linuxsystemcore", "osxuniversal" ] }, { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", + "version": "2027.0.0-alpha-6", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -108,15 +103,14 @@ "windowsx86-64", "linuxarm64", "linuxx86-64", - "linuxathena", - "linuxarm32", + "linuxsystemcore", "osxuniversal" ] }, { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", + "version": "2027.0.0-alpha-6", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -124,8 +118,7 @@ "windowsx86-64", "linuxarm64", "linuxx86-64", - "linuxathena", - "linuxarm32", + "linuxsystemcore", "osxuniversal" ] } diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json deleted file mode 100644 index f5b535e53..000000000 --- a/vendordeps/URCL.json +++ /dev/null @@ -1,65 +0,0 @@ -{ - "fileName": "URCL.json", - "name": "URCL", - "version": "2026.0.0", - "frcYear": "2026", - "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c", - "mavenUrls": [ - "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" - ], - "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/main/URCL.json", - "javaDependencies": [ - { - "groupId": "org.littletonrobotics.urcl", - "artifactId": "URCL-java", - "version": "2026.0.0" - } - ], - "jniDependencies": [ - { - "groupId": "org.littletonrobotics.urcl", - "artifactId": "URCL-driver", - "version": "2026.0.0", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "org.littletonrobotics.urcl", - "artifactId": "URCL-cpp", - "version": "2026.0.0", - "libName": "URCL", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena", - "osxuniversal" - ] - }, - { - "groupId": "org.littletonrobotics.urcl", - "artifactId": "URCL-driver", - "version": "2026.0.0", - "libName": "URCLDriver", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena", - "osxuniversal" - ] - } - ] -} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 9326447d4..3f6b31fca 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,9 +1,9 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.3.1", + "version": "v2027.0.0-alpha-2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2026", + "wpilibYear": "2027_alpha5", "mavenUrls": [ "https://maven.photonvision.org/repository/internal", "https://maven.photonvision.org/repository/snapshots" @@ -13,12 +13,12 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1-rc-2", + "version": "v2027.0.0-alpha-2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ "windowsx86-64", - "linuxathena", + "linuxsystemcore", "linuxx86-64", "osxuniversal" ] @@ -28,14 +28,14 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.1.1-rc-2", + "version": "v2027.0.0-alpha-2", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "linuxathena", + "linuxsystemcore", "linuxx86-64", "osxuniversal" ] @@ -43,14 +43,14 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1-rc-2", + "version": "v2027.0.0-alpha-2", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "linuxathena", + "linuxsystemcore", "linuxx86-64", "osxuniversal" ] @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.1.1-rc-2" + "version": "v2027.0.0-alpha-2" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.1.1-rc-2" + "version": "v2027.0.0-alpha-2" } ] } From 64aed533cf3fad78e806f45467b71e3a7875422b Mon Sep 17 00:00:00 2001 From: Ekansh Jain Date: Thu, 17 Sep 2026 17:06:59 +0000 Subject: [PATCH 02/15] move imports --- .../lib/characterization/SysIdFactory.java | 8 ++-- .../java/frc/lib/utils/AllianceFlipUtil.java | 14 +++--- src/main/java/frc/lib/utils/GeomUtils.java | 10 ++--- .../java/frc/lib/utils/LocalADStarAK.java | 4 +- src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/FieldConstants.java | 12 ++--- src/main/java/frc/robot/Main.java | 2 +- src/main/java/frc/robot/Orchestrator.java | 34 +++++++------- src/main/java/frc/robot/Robot.java | 16 +++---- src/main/java/frc/robot/RobotContainer.java | 30 ++++++------- src/main/java/frc/robot/RobotState.java | 6 +-- .../java/frc/robot/commands/auto/Autos.java | 10 ++--- .../frc/robot/commands/auto/DriveToPose.java | 20 ++++----- .../commands/auto/RotateToOrientation.java | 14 +++--- .../commands/auto/StraightDriveToPose.java | 20 ++++----- .../robot/commands/drive/DriveCommands.java | 30 ++++++------- .../robot/commands/drive/DriveWithDpad.java | 6 +-- .../frc/robot/subsystems/drive/Drive.java | 44 +++++++++---------- .../subsystems/drive/DriveConstants.java | 8 ++-- .../frc/robot/subsystems/drive/GyroIO.java | 2 +- .../robot/subsystems/drive/GyroIOPigeon2.java | 14 +++--- .../frc/robot/subsystems/drive/Module.java | 12 ++--- .../frc/robot/subsystems/drive/ModuleIO.java | 2 +- .../robot/subsystems/drive/ModuleIOSim.java | 12 ++--- .../robot/subsystems/drive/ModuleIOSpark.java | 10 ++--- .../subsystems/drive/ModuleIOTalonSpark.java | 16 +++---- .../subsystems/drive/OdometryThread.java | 4 +- .../frc/robot/subsystems/indexer/Indexer.java | 6 +-- .../frc/robot/subsystems/intake/Intake.java | 4 +- .../intake/extend/IntakeExtend.java | 10 ++--- .../intake/extend/IntakeExtendIOKraken.java | 2 +- .../intake/extend/IntakeExtendIOSparkMax.java | 2 +- .../intake/rollers/IntakeRollers.java | 6 +-- .../intake/rollers/IntakeRollersIOKraken.java | 4 +- .../robot/subsystems/leds/LedPatterns.java | 12 ++--- .../java/frc/robot/subsystems/leds/Leds.java | 26 +++++------ .../subsystems/magicCarpet/MagicCarpet.java | 6 +-- .../magicCarpet/MagicCarpetSparkMax.java | 2 +- .../frc/robot/subsystems/shooter/Shooter.java | 34 +++++++------- .../subsystems/shooter/ShooterConstants.java | 8 ++-- .../frc/robot/subsystems/vision/Vision.java | 20 ++++----- .../subsystems/vision/VisionConstants.java | 10 ++--- .../frc/robot/subsystems/vision/VisionIO.java | 4 +- .../vision/VisionIOPhotonVision.java | 6 +-- .../vision/VisionIOPhotonVisionSim.java | 4 +- .../java/frc/robot/util/CustomTriggers.java | 4 +- .../robot/util/ShooterLeadCompensator.java | 12 ++--- src/main/java/frc/robot/util/Zone.java | 2 +- 48 files changed, 274 insertions(+), 274 deletions(-) diff --git a/src/main/java/frc/lib/characterization/SysIdFactory.java b/src/main/java/frc/lib/characterization/SysIdFactory.java index a4567b34b..25828cef1 100644 --- a/src/main/java/frc/lib/characterization/SysIdFactory.java +++ b/src/main/java/frc/lib/characterization/SysIdFactory.java @@ -1,9 +1,9 @@ package frc.lib.characterization; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import org.wpilib.units.measure.Voltage; +import org.wpilib.wpilibj2.command.Command; +import org.wpilib.wpilibj2.command.Subsystem; +import org.wpilib.wpilibj2.command.sysid.SysIdRoutine; import java.util.function.Consumer; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/lib/utils/AllianceFlipUtil.java b/src/main/java/frc/lib/utils/AllianceFlipUtil.java index c5d1063c0..41d868bf4 100644 --- a/src/main/java/frc/lib/utils/AllianceFlipUtil.java +++ b/src/main/java/frc/lib/utils/AllianceFlipUtil.java @@ -1,12 +1,12 @@ package frc.lib.utils; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.DriverStation; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.wpilibj.DriverStation; import frc.robot.FieldConstants; /** diff --git a/src/main/java/frc/lib/utils/GeomUtils.java b/src/main/java/frc/lib/utils/GeomUtils.java index 3db5d0c94..9eef300ca 100644 --- a/src/main/java/frc/lib/utils/GeomUtils.java +++ b/src/main/java/frc/lib/utils/GeomUtils.java @@ -1,10 +1,10 @@ package frc.lib.utils; -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.geometry.Twist2d; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Transform2d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.geometry.Twist2d; public class GeomUtils { diff --git a/src/main/java/frc/lib/utils/LocalADStarAK.java b/src/main/java/frc/lib/utils/LocalADStarAK.java index c51f334e4..58c3954a7 100644 --- a/src/main/java/frc/lib/utils/LocalADStarAK.java +++ b/src/main/java/frc/lib/utils/LocalADStarAK.java @@ -6,8 +6,8 @@ import com.pathplanner.lib.path.PathPoint; import com.pathplanner.lib.pathfinding.LocalADStar; import com.pathplanner.lib.pathfinding.Pathfinder; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Translation2d; +import org.wpilib.math.Pair; +import org.wpilib.math.geometry.Translation2d; import java.util.ArrayList; import java.util.Collections; import java.util.List; diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 922aca595..05839f77a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,7 +1,7 @@ package frc.robot; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotBase; +import org.wpilib.wpilibj.DriverStation; +import org.wpilib.wpilibj.RobotBase; import java.io.BufferedReader; import java.io.FileReader; import java.io.IOException; diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index a2210db17..14581e930 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -1,12 +1,12 @@ package frc.robot; import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Filesystem; +import org.wpilib.apriltag.AprilTagFieldLayout; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.util.Units; +import org.wpilib.wpilibj.Filesystem; import java.io.IOException; import java.nio.file.Path; diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 8776e5dda..d8c6248f8 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -4,7 +4,7 @@ package frc.robot; -import edu.wpi.first.wpilibj.RobotBase; +import org.wpilib.wpilibj.RobotBase; /** * Do NOT add any static variables to this class, or any initialization at all. Unless you know what diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 37135b86b..048c6f20a 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -1,24 +1,24 @@ package frc.robot; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Minute; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Rotations; +import static org.wpilib.units.Units.Meters; +import static org.wpilib.units.Units.Minute; +import static org.wpilib.units.Units.RadiansPerSecond; +import static org.wpilib.units.Units.Rotations; import static frc.robot.subsystems.shooter.ShooterConstants.CLOSE_HUB_SHOOTER_RPM; -import edu.wpi.first.math.filter.LinearFilter; -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.util.Units; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.SelectCommand; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import org.wpilib.math.filter.LinearFilter; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Transform2d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.util.Units; +import org.wpilib.units.measure.Distance; +import org.wpilib.wpilibj.smartdashboard.SmartDashboard; +import org.wpilib.wpilibj2.command.Command; +import org.wpilib.wpilibj2.command.Commands; +import org.wpilib.wpilibj2.command.ConditionalCommand; +import org.wpilib.wpilibj2.command.SelectCommand; +import org.wpilib.wpilibj2.command.button.CommandXboxController; import frc.lib.utils.AllianceFlipUtil; import frc.robot.FieldConstants.Hub; import frc.robot.commands.auto.DriveToPose; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ef925d158..34e2f4adb 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,14 +4,14 @@ package frc.robot; -import edu.wpi.first.math.filter.LinearFilter; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; +import org.wpilib.math.filter.LinearFilter; +import org.wpilib.wpilibj.Alert; +import org.wpilib.wpilibj.Alert.AlertType; +import org.wpilib.wpilibj.DriverStation; +import org.wpilib.wpilibj.RobotController; +import org.wpilib.wpilibj.Timer; +import org.wpilib.wpilibj2.command.Command; +import org.wpilib.wpilibj2.command.CommandScheduler; import java.io.IOException; import java.util.HashMap; import java.util.Map; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a205ec02e..582b77791 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,7 +4,7 @@ package frc.robot; -import static edu.wpi.first.units.Units.*; +import static org.wpilib.units.Units.*; import static frc.robot.subsystems.drive.DriveConstants.ppConfig; import static frc.robot.subsystems.vision.VisionConstants.*; @@ -13,18 +13,18 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.PathPlannerLogging; -import edu.wpi.first.math.geometry.*; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.GenericHID.RumbleType; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import org.wpilib.math.geometry.*; +import org.wpilib.wpilibj.DriverStation; +import org.wpilib.wpilibj.DriverStation.Alliance; +import org.wpilib.wpilibj.GenericHID; +import org.wpilib.wpilibj.GenericHID.RumbleType; +import org.wpilib.wpilibj.XboxController; +import org.wpilib.wpilibj2.command.Command; +import org.wpilib.wpilibj2.command.Commands; +import org.wpilib.wpilibj2.command.button.CommandXboxController; +import org.wpilib.wpilibj2.command.button.Trigger; +import org.wpilib.wpilibj2.command.sysid.SysIdRoutine; +import org.wpilib.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.lib.utils.LocalADStarAK; import frc.robot.RobotState.IntakePosition; import frc.robot.commands.auto.Autos; @@ -269,8 +269,8 @@ public RobotContainer() { /** * 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}. + * org.wpilib.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link + * org.wpilib.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { indexer.setDefaultCommand(indexer.stop()); diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 7e91b29c8..f797de420 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -1,9 +1,9 @@ package frc.robot; -import static edu.wpi.first.units.Units.Percent; +import static org.wpilib.units.Units.Percent; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.LEDPattern; +import org.wpilib.wpilibj.DriverStation; +import org.wpilib.wpilibj.LEDPattern; import frc.robot.subsystems.leds.LedPatterns; import frc.robot.subsystems.leds.Leds.Sections; import org.littletonrobotics.junction.AutoLogOutput; diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index 06eb4f0a5..cc7fded40 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -1,10 +1,10 @@ package frc.robot.commands.auto; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.util.Units; +import org.wpilib.wpilibj2.command.Command; +import org.wpilib.wpilibj2.command.Commands; import frc.lib.utils.AllianceFlipUtil; import frc.robot.Orchestrator; import frc.robot.subsystems.drive.Drive; diff --git a/src/main/java/frc/robot/commands/auto/DriveToPose.java b/src/main/java/frc/robot/commands/auto/DriveToPose.java index 4b9eafb20..508ed8b77 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToPose.java +++ b/src/main/java/frc/robot/commands/auto/DriveToPose.java @@ -1,15 +1,15 @@ package frc.robot.commands.auto; -import edu.wpi.first.math.controller.ProfiledPIDController; -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.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.Command; +import org.wpilib.math.controller.ProfiledPIDController; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.trajectory.TrapezoidProfile; +import org.wpilib.math.util.Units; +import org.wpilib.wpilibj.DriverStation; +import org.wpilib.wpilibj.DriverStation.Alliance; +import org.wpilib.wpilibj2.command.Command; import frc.lib.utils.GeomUtils; import frc.lib.utils.TunableNumber; import frc.robot.Constants; diff --git a/src/main/java/frc/robot/commands/auto/RotateToOrientation.java b/src/main/java/frc/robot/commands/auto/RotateToOrientation.java index 9bdeec173..18d6a987f 100644 --- a/src/main/java/frc/robot/commands/auto/RotateToOrientation.java +++ b/src/main/java/frc/robot/commands/auto/RotateToOrientation.java @@ -1,12 +1,12 @@ package frc.robot.commands.auto; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; +import org.wpilib.math.controller.ProfiledPIDController; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.trajectory.TrapezoidProfile.Constraints; +import org.wpilib.math.util.Units; +import org.wpilib.wpilibj2.command.Command; import frc.robot.subsystems.drive.Drive; import java.util.Objects; import java.util.function.Supplier; diff --git a/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java b/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java index f0011c966..da0ff2bb7 100644 --- a/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java +++ b/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java @@ -1,15 +1,15 @@ package frc.robot.commands.auto; -import edu.wpi.first.math.controller.ProfiledPIDController; -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.math.trajectory.TrapezoidProfile.Constraints; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.Command; +import org.wpilib.math.controller.ProfiledPIDController; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.trajectory.TrapezoidProfile.Constraints; +import org.wpilib.math.util.Units; +import org.wpilib.wpilibj.DriverStation; +import org.wpilib.wpilibj.DriverStation.Alliance; +import org.wpilib.wpilibj2.command.Command; import frc.lib.utils.GeomUtils; import frc.robot.subsystems.drive.Drive; import java.util.function.Supplier; diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index bbba70117..673a202ce 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -1,20 +1,20 @@ package frc.robot.commands.drive; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import org.wpilib.math.MathUtil; +import org.wpilib.math.controller.ProfiledPIDController; +import org.wpilib.math.filter.SlewRateLimiter; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Transform2d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.trajectory.TrapezoidProfile; +import org.wpilib.math.util.Units; +import org.wpilib.wpilibj.DriverStation; +import org.wpilib.wpilibj.DriverStation.Alliance; +import org.wpilib.wpilibj.Timer; +import org.wpilib.wpilibj2.command.Command; +import org.wpilib.wpilibj2.command.Commands; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; import java.text.DecimalFormat; diff --git a/src/main/java/frc/robot/commands/drive/DriveWithDpad.java b/src/main/java/frc/robot/commands/drive/DriveWithDpad.java index 62d1f7a2a..734ba0a53 100644 --- a/src/main/java/frc/robot/commands/drive/DriveWithDpad.java +++ b/src/main/java/frc/robot/commands/drive/DriveWithDpad.java @@ -1,8 +1,8 @@ package frc.robot.commands.drive; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; +import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.util.Units; +import org.wpilib.wpilibj2.command.Command; import frc.robot.subsystems.drive.Drive; import java.util.function.Supplier; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 4d405d38d..ae75a7b03 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -1,31 +1,31 @@ package frc.robot.subsystems.drive; -import static edu.wpi.first.units.Units.*; +import static org.wpilib.units.Units.*; import static frc.robot.subsystems.drive.DriveConstants.*; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathPlannerPath; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import org.wpilib.math.Matrix; +import org.wpilib.math.controller.PIDController; +import org.wpilib.math.estimator.SwerveDrivePoseEstimator; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Twist2d; +import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.SwerveDriveKinematics; +import org.wpilib.math.kinematics.SwerveModulePosition; +import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.numbers.N1; +import org.wpilib.math.numbers.N3; +import org.wpilib.wpilibj.Alert; +import org.wpilib.wpilibj.Alert.AlertType; +import org.wpilib.wpilibj.DriverStation; +import org.wpilib.wpilibj2.command.Command; +import org.wpilib.wpilibj2.command.Commands; +import org.wpilib.wpilibj2.command.SubsystemBase; +import org.wpilib.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.Mode; import frc.robot.subsystems.vision.VisionConstants; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index a469e5c73..41aa5bf47 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -7,10 +7,10 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.RobotConfig; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.util.Units; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.system.plant.DCMotor; +import org.wpilib.math.util.Units; import frc.robot.Constants; public class DriveConstants { diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index b7a60e641..64e8d6608 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -1,6 +1,6 @@ package frc.robot.subsystems.drive; -import edu.wpi.first.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; public interface GyroIO { diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 5ffc26320..06f9cafc4 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -8,13 +8,13 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.LinearAcceleration; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.util.Units; +import org.wpilib.units.measure.Angle; +import org.wpilib.units.measure.AngularVelocity; +import org.wpilib.units.measure.LinearAcceleration; +import org.wpilib.wpilibj.Alert; +import org.wpilib.wpilibj.Alert.AlertType; import java.util.Queue; /** IO implementation for Pigeon 2. */ diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 16ba4889a..ce9682515 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -2,12 +2,12 @@ import static frc.robot.subsystems.drive.DriveConstants.*; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; +import org.wpilib.math.MathUtil; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.SwerveModulePosition; +import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.wpilibj.Alert; +import org.wpilib.wpilibj.Alert.AlertType; import org.littletonrobotics.junction.Logger; public class Module { diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 4e5705a54..ef0bd6ae1 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -1,6 +1,6 @@ package frc.robot.subsystems.drive; -import edu.wpi.first.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 98e945c51..7e6607e40 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -2,12 +2,12 @@ import static frc.robot.subsystems.drive.DriveConstants.*; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import org.wpilib.math.MathUtil; +import org.wpilib.math.controller.PIDController; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.system.plant.LinearSystemId; +import org.wpilib.wpilibj.Timer; +import org.wpilib.wpilibj.simulation.DCMotorSim; /** Physics sim implementation of module IO. */ public class ModuleIOSim implements ModuleIO { diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index dfa079eb2..8c8243bae 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -19,11 +19,11 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; +import org.wpilib.math.MathUtil; +import org.wpilib.math.filter.Debouncer; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.util.Units; +import org.wpilib.units.measure.Angle; import java.util.Queue; import java.util.function.DoubleSupplier; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonSpark.java index 6842806dd..3311cc156 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonSpark.java @@ -29,14 +29,14 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Voltage; +import org.wpilib.math.MathUtil; +import org.wpilib.math.filter.Debouncer; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.util.Units; +import org.wpilib.units.measure.Angle; +import org.wpilib.units.measure.AngularVelocity; +import org.wpilib.units.measure.Current; +import org.wpilib.units.measure.Voltage; import frc.lib.utils.PhoenixUtil; import java.util.Queue; import java.util.function.DoubleSupplier; diff --git a/src/main/java/frc/robot/subsystems/drive/OdometryThread.java b/src/main/java/frc/robot/subsystems/drive/OdometryThread.java index 9b3b7f31a..0345d663d 100644 --- a/src/main/java/frc/robot/subsystems/drive/OdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/OdometryThread.java @@ -5,8 +5,8 @@ import com.ctre.phoenix6.StatusSignal; import com.revrobotics.REVLibError; import com.revrobotics.spark.SparkBase; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.wpilibj.RobotController; +import org.wpilib.units.measure.Angle; +import org.wpilib.wpilibj.RobotController; import java.util.ArrayList; import java.util.List; import java.util.Queue; diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 6f43a6785..1ed297f22 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -2,9 +2,9 @@ import static frc.robot.subsystems.indexer.IndexerConstants.FEEDUP_VOLT; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.wpilib.wpilibj2.command.Command; +import org.wpilib.wpilibj2.command.Commands; +import org.wpilib.wpilibj2.command.SubsystemBase; import frc.robot.RobotState; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 4d41fba74..014ab894f 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -2,8 +2,8 @@ import static frc.robot.subsystems.intake.IntakeConstants.*; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import org.wpilib.wpilibj2.command.Command; +import org.wpilib.wpilibj2.command.Commands; import frc.robot.subsystems.intake.extend.IntakeExtend; import frc.robot.subsystems.intake.rollers.IntakeRollers; diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java index bbf147252..1a656fead 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java @@ -6,11 +6,11 @@ import static frc.robot.subsystems.intake.IntakeConstants.POSITION_TOLERANCE; import static frc.robot.subsystems.intake.IntakeConstants.STALL_VELOCITY; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.wpilib.wpilibj.Timer; +import org.wpilib.wpilibj2.command.Command; +import org.wpilib.wpilibj2.command.Commands; +import org.wpilib.wpilibj2.command.ConditionalCommand; +import org.wpilib.wpilibj2.command.SubsystemBase; import frc.robot.RobotState; import frc.robot.RobotState.IntakePosition; import java.util.function.BooleanSupplier; diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java index 814cfc72f..132cdf94b 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java @@ -22,7 +22,7 @@ import com.revrobotics.spark.config.EncoderConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.wpilibj.DigitalInput; +import org.wpilib.wpilibj.DigitalInput; public class IntakeExtendIOKraken implements IntakeExtendIO { private final SparkMax extendMotor; diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java index c21bc77d9..381c13fa7 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java @@ -20,7 +20,7 @@ import com.revrobotics.spark.config.EncoderConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.wpilibj.DigitalInput; +import org.wpilib.wpilibj.DigitalInput; public class IntakeExtendIOSparkMax implements IntakeExtendIO { diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java index 0cec68ba6..c1d543ac7 100644 --- a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java +++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java @@ -3,9 +3,9 @@ import static frc.robot.subsystems.intake.IntakeConstants.INTAKE_VOLTS; import static frc.robot.subsystems.intake.IntakeConstants.OUTTAKE_VOLTS; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.wpilib.wpilibj2.command.Command; +import org.wpilib.wpilibj2.command.Commands; +import org.wpilib.wpilibj2.command.SubsystemBase; import frc.robot.RobotState; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOKraken.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOKraken.java index cbe116e01..f782a9369 100644 --- a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOKraken.java +++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOKraken.java @@ -10,8 +10,8 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Voltage; +import org.wpilib.units.measure.Current; +import org.wpilib.units.measure.Voltage; public class IntakeRollersIOKraken implements IntakeRollersIO { diff --git a/src/main/java/frc/robot/subsystems/leds/LedPatterns.java b/src/main/java/frc/robot/subsystems/leds/LedPatterns.java index 21b4f6f7b..18192f674 100644 --- a/src/main/java/frc/robot/subsystems/leds/LedPatterns.java +++ b/src/main/java/frc/robot/subsystems/leds/LedPatterns.java @@ -1,12 +1,12 @@ package frc.robot.subsystems.leds; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Seconds; +import static org.wpilib.units.Units.Meters; +import static org.wpilib.units.Units.MetersPerSecond; +import static org.wpilib.units.Units.Seconds; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.LEDPattern; -import edu.wpi.first.wpilibj.util.Color; +import org.wpilib.units.measure.Distance; +import org.wpilib.wpilibj.LEDPattern; +import org.wpilib.wpilibj.util.Color; import java.util.Map; public enum LedPatterns { diff --git a/src/main/java/frc/robot/subsystems/leds/Leds.java b/src/main/java/frc/robot/subsystems/leds/Leds.java index c4f7d94e1..d14faf7cf 100644 --- a/src/main/java/frc/robot/subsystems/leds/Leds.java +++ b/src/main/java/frc/robot/subsystems/leds/Leds.java @@ -1,18 +1,18 @@ package frc.robot.subsystems.leds; -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLED.ColorOrder; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.AddressableLEDBufferView; -import edu.wpi.first.wpilibj.LEDPattern; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.wpilib.networktables.GenericEntry; +import org.wpilib.wpilibj.AddressableLED; +import org.wpilib.wpilibj.AddressableLED.ColorOrder; +import org.wpilib.wpilibj.AddressableLEDBuffer; +import org.wpilib.wpilibj.AddressableLEDBufferView; +import org.wpilib.wpilibj.LEDPattern; +import org.wpilib.wpilibj.shuffleboard.BuiltInLayouts; +import org.wpilib.wpilibj.shuffleboard.BuiltInWidgets; +import org.wpilib.wpilibj.shuffleboard.Shuffleboard; +import org.wpilib.wpilibj.shuffleboard.ShuffleboardLayout; +import org.wpilib.wpilibj.shuffleboard.ShuffleboardTab; +import org.wpilib.wpilibj.smartdashboard.SendableChooser; +import org.wpilib.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.RobotState; import frc.robot.RobotState.Mode; diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java index 3b49a1522..acb745f97 100644 --- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java +++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java @@ -1,8 +1,8 @@ package frc.robot.subsystems.magicCarpet; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.wpilib.wpilibj2.command.Command; +import org.wpilib.wpilibj2.command.Commands; +import org.wpilib.wpilibj2.command.SubsystemBase; import frc.robot.RobotState; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetSparkMax.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetSparkMax.java index f87337a63..01c075289 100644 --- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetSparkMax.java +++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetSparkMax.java @@ -10,7 +10,7 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.math.MathUtil; +import org.wpilib.math.MathUtil; import frc.robot.Schematic; public class MagicCarpetSparkMax implements MagicCarpetIO { diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 80c857bb0..a3f9c1791 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,28 +1,28 @@ package frc.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.*; -import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.units.Units.Volts; +import static org.wpilib.units.Units.*; +import static org.wpilib.units.Units.Second; +import static org.wpilib.units.Units.Volts; import static frc.robot.subsystems.shooter.ShooterConstants.FF_SCALAR; import static frc.robot.subsystems.shooter.ShooterConstants.KA; import static frc.robot.subsystems.shooter.ShooterConstants.KS; import static frc.robot.subsystems.shooter.ShooterConstants.KV; import static frc.robot.subsystems.shooter.ShooterConstants.TOLERANCE; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.units.*; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.Time; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; +import org.wpilib.math.controller.PIDController; +import org.wpilib.math.controller.SimpleMotorFeedforward; +import org.wpilib.math.filter.SlewRateLimiter; +import org.wpilib.units.*; +import org.wpilib.units.measure.AngularVelocity; +import org.wpilib.units.measure.Distance; +import org.wpilib.units.measure.Time; +import org.wpilib.wpilibj.smartdashboard.SmartDashboard; +import org.wpilib.wpilibj2.command.Command; +import org.wpilib.wpilibj2.command.Commands; +import org.wpilib.wpilibj2.command.SubsystemBase; +import org.wpilib.wpilibj2.command.sysid.SysIdRoutine; +import org.wpilib.wpilibj2.command.sysid.SysIdRoutine.Config; +import org.wpilib.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.RobotState; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index f784e5bea..98eff4b4c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -1,10 +1,10 @@ package frc.robot.subsystems.shooter; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -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.util.Units; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Transform2d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.util.Units; public class ShooterConstants { public static final double ENCODER_POSITION_CONVERSION = 1; diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 51b86e592..38c67bd6f 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -2,16 +2,16 @@ import static frc.robot.subsystems.vision.VisionConstants.*; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.wpilib.math.Matrix; +import org.wpilib.math.VecBuilder; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.numbers.N1; +import org.wpilib.math.numbers.N3; +import org.wpilib.wpilibj.Alert; +import org.wpilib.wpilibj.Alert.AlertType; +import org.wpilib.wpilibj2.command.SubsystemBase; import frc.robot.RobotState; import java.util.LinkedList; import java.util.List; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 463e6ce91..5a8d80b34 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -1,10 +1,10 @@ package frc.robot.subsystems.vision; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; +import org.wpilib.apriltag.AprilTagFieldLayout; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.util.Units; import frc.robot.FieldConstants; public class VisionConstants { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 64ca15120..3e2cf2c41 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -1,7 +1,7 @@ package frc.robot.subsystems.vision; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; public interface VisionIO { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index 0832ee9e1..9f3015d7b 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -2,9 +2,9 @@ import static frc.robot.subsystems.vision.VisionConstants.*; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform3d; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Transform3d; import java.util.HashSet; import java.util.LinkedList; import java.util.List; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java index 973025523..97a6eb365 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -2,8 +2,8 @@ import static frc.robot.subsystems.vision.VisionConstants.aprilTagLayout; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Transform3d; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Transform3d; import java.util.function.Supplier; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; diff --git a/src/main/java/frc/robot/util/CustomTriggers.java b/src/main/java/frc/robot/util/CustomTriggers.java index f1abc88d8..57829c228 100644 --- a/src/main/java/frc/robot/util/CustomTriggers.java +++ b/src/main/java/frc/robot/util/CustomTriggers.java @@ -1,7 +1,7 @@ package frc.robot.util; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.button.Trigger; +import org.wpilib.wpilibj2.command.CommandScheduler; +import org.wpilib.wpilibj2.command.button.Trigger; import java.util.HashMap; import java.util.Map; import java.util.function.BooleanSupplier; diff --git a/src/main/java/frc/robot/util/ShooterLeadCompensator.java b/src/main/java/frc/robot/util/ShooterLeadCompensator.java index ccaecb1dc..cbdf2f7d2 100644 --- a/src/main/java/frc/robot/util/ShooterLeadCompensator.java +++ b/src/main/java/frc/robot/util/ShooterLeadCompensator.java @@ -1,12 +1,12 @@ package frc.robot.util; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Seconds; +import static org.wpilib.units.Units.Meters; +import static org.wpilib.units.Units.Seconds; -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.units.measure.Time; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.units.measure.Time; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterConstants; diff --git a/src/main/java/frc/robot/util/Zone.java b/src/main/java/frc/robot/util/Zone.java index 3862daccd..140a482bf 100644 --- a/src/main/java/frc/robot/util/Zone.java +++ b/src/main/java/frc/robot/util/Zone.java @@ -1,6 +1,6 @@ package frc.robot.util; -import edu.wpi.first.math.geometry.Pose2d; +import org.wpilib.math.geometry.Pose2d; import frc.lib.utils.AllianceFlipUtil; import java.util.Objects; import java.util.function.Supplier; From 21e1096198c56d90a83240542dcd382dd398ceb7 Mon Sep 17 00:00:00 2001 From: EJainDev Date: Thu, 17 Sep 2026 15:48:35 -0400 Subject: [PATCH 03/15] fix every import --- .../lib/characterization/SysIdFactory.java | 6 ++--- .../java/frc/lib/utils/AllianceFlipUtil.java | 2 +- .../java/frc/lib/utils/LocalADStarAK.java | 2 +- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/FieldConstants.java | 4 ++-- src/main/java/frc/robot/Main.java | 2 +- src/main/java/frc/robot/Orchestrator.java | 12 +++++----- src/main/java/frc/robot/Robot.java | 16 ++++++------- src/main/java/frc/robot/RobotContainer.java | 23 +++++++++---------- src/main/java/frc/robot/RobotState.java | 4 ++-- .../java/frc/robot/commands/auto/Autos.java | 4 ++-- .../frc/robot/commands/auto/DriveToPose.java | 2 +- .../commands/auto/RotateToOrientation.java | 2 +- .../commands/auto/StraightDriveToPose.java | 2 +- .../robot/commands/drive/DriveCommands.java | 4 ++-- .../robot/commands/drive/DriveWithDpad.java | 2 +- .../frc/robot/subsystems/drive/Drive.java | 20 ++++++++-------- .../subsystems/drive/DriveConstants.java | 2 +- .../robot/subsystems/drive/GyroIOPigeon2.java | 4 ++-- .../frc/robot/subsystems/drive/Module.java | 8 +++---- .../robot/subsystems/drive/ModuleIOSim.java | 8 +++---- .../robot/subsystems/drive/ModuleIOSpark.java | 8 +++---- .../subsystems/drive/ModuleIOTalonSpark.java | 8 +++---- .../subsystems/drive/OdometryThread.java | 2 +- .../frc/robot/subsystems/indexer/Indexer.java | 6 ++--- .../subsystems/indexer/IndexerIOSparkMax.java | 4 ++-- .../frc/robot/subsystems/intake/Intake.java | 4 ++-- .../intake/extend/IntakeExtend.java | 10 ++++---- .../intake/extend/IntakeExtendIOKraken.java | 8 +++---- .../intake/extend/IntakeExtendIOSparkMax.java | 8 +++---- .../intake/rollers/IntakeRollers.java | 6 ++--- .../robot/subsystems/leds/LedPatterns.java | 4 ++-- .../java/frc/robot/subsystems/leds/Leds.java | 18 +++++++++------ .../subsystems/magicCarpet/MagicCarpet.java | 6 ++--- .../magicCarpet/MagicCarpetSparkMax.java | 6 ++--- .../frc/robot/subsystems/shooter/Shooter.java | 14 +++++------ .../subsystems/shooter/ShooterIOSparkMax.java | 4 ++-- .../frc/robot/subsystems/vision/Vision.java | 10 ++++---- .../subsystems/vision/VisionConstants.java | 2 +- .../java/frc/robot/util/CustomTriggers.java | 4 ++-- 40 files changed, 134 insertions(+), 131 deletions(-) diff --git a/src/main/java/frc/lib/characterization/SysIdFactory.java b/src/main/java/frc/lib/characterization/SysIdFactory.java index 25828cef1..fbe8298df 100644 --- a/src/main/java/frc/lib/characterization/SysIdFactory.java +++ b/src/main/java/frc/lib/characterization/SysIdFactory.java @@ -1,9 +1,9 @@ package frc.lib.characterization; import org.wpilib.units.measure.Voltage; -import org.wpilib.wpilibj2.command.Command; -import org.wpilib.wpilibj2.command.Subsystem; -import org.wpilib.wpilibj2.command.sysid.SysIdRoutine; +import org.wpilib.command2.Command; +import org.wpilib.command2.Subsystem; +import org.wpilib.command2.sysid.SysIdRoutine; import java.util.function.Consumer; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/lib/utils/AllianceFlipUtil.java b/src/main/java/frc/lib/utils/AllianceFlipUtil.java index 41d868bf4..bb60394b5 100644 --- a/src/main/java/frc/lib/utils/AllianceFlipUtil.java +++ b/src/main/java/frc/lib/utils/AllianceFlipUtil.java @@ -6,7 +6,7 @@ import org.wpilib.math.geometry.Rotation3d; import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.geometry.Translation3d; -import org.wpilib.wpilibj.DriverStation; +import org.wpilib.driverstation.DriverStation; import frc.robot.FieldConstants; /** diff --git a/src/main/java/frc/lib/utils/LocalADStarAK.java b/src/main/java/frc/lib/utils/LocalADStarAK.java index 58c3954a7..7dae8474c 100644 --- a/src/main/java/frc/lib/utils/LocalADStarAK.java +++ b/src/main/java/frc/lib/utils/LocalADStarAK.java @@ -6,7 +6,7 @@ import com.pathplanner.lib.path.PathPoint; import com.pathplanner.lib.pathfinding.LocalADStar; import com.pathplanner.lib.pathfinding.Pathfinder; -import org.wpilib.math.Pair; +import org.wpilib.math.util.Pair; import org.wpilib.math.geometry.Translation2d; import java.util.ArrayList; import java.util.Collections; diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 05839f77a..1f34a1380 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,7 +1,7 @@ package frc.robot; -import org.wpilib.wpilibj.DriverStation; -import org.wpilib.wpilibj.RobotBase; +import org.wpilib.driverstation.DriverStation; +import org.wpilib.framework.RobotBase; import java.io.BufferedReader; import java.io.FileReader; import java.io.IOException; diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index 14581e930..f21d247ca 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -1,12 +1,12 @@ package frc.robot; import com.fasterxml.jackson.databind.ObjectMapper; -import org.wpilib.apriltag.AprilTagFieldLayout; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.geometry.Translation3d; import org.wpilib.math.util.Units; -import org.wpilib.wpilibj.Filesystem; +import org.wpilib.system.Filesystem; import java.io.IOException; import java.nio.file.Path; diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index d8c6248f8..5a10f2147 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -4,7 +4,7 @@ package frc.robot; -import org.wpilib.wpilibj.RobotBase; +import org.wpilib.framework.RobotBase; /** * Do NOT add any static variables to this class, or any initialization at all. Unless you know what diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 048c6f20a..1259170e2 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -13,12 +13,12 @@ import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.util.Units; import org.wpilib.units.measure.Distance; -import org.wpilib.wpilibj.smartdashboard.SmartDashboard; -import org.wpilib.wpilibj2.command.Command; -import org.wpilib.wpilibj2.command.Commands; -import org.wpilib.wpilibj2.command.ConditionalCommand; -import org.wpilib.wpilibj2.command.SelectCommand; -import org.wpilib.wpilibj2.command.button.CommandXboxController; +import org.wpilib.smartdashboard.SmartDashboard; +import org.wpilib.command2.Command; +import org.wpilib.command2.Commands; +import org.wpilib.command2.ConditionalCommand; +import org.wpilib.command2.SelectCommand; +import org.wpilib.command2.button.CommandXboxController; // Removed import frc.lib.utils.AllianceFlipUtil; import frc.robot.FieldConstants.Hub; import frc.robot.commands.auto.DriveToPose; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 34e2f4adb..9c34ae8ad 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,13 +5,13 @@ package frc.robot; import org.wpilib.math.filter.LinearFilter; -import org.wpilib.wpilibj.Alert; -import org.wpilib.wpilibj.Alert.AlertType; -import org.wpilib.wpilibj.DriverStation; -import org.wpilib.wpilibj.RobotController; -import org.wpilib.wpilibj.Timer; -import org.wpilib.wpilibj2.command.Command; -import org.wpilib.wpilibj2.command.CommandScheduler; +import org.wpilib.driverstation.Alert; +import org.wpilib.wpilibj.Alert.AlertType; // Removed or renamed +import org.wpilib.driverstation.DriverStation; +import org.wpilib.system.RobotController; +import org.wpilib.system.Timer; +import org.wpilib.command2.Command; +import org.wpilib.command2.CommandScheduler; import java.io.IOException; import java.util.HashMap; import java.util.Map; @@ -22,7 +22,7 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; -import org.littletonrobotics.urcl.URCL; +import org.littletonrobotics.urcl.URCL; // Removed (must use native revrobotics logger) /** * The VM is configured to automatically run this class, and to call the functions corresponding to diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 582b77791..0a92d8764 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,18 +13,17 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.PathPlannerLogging; +import org.wpilib.driverstation.GenericHID.RumbleType; import org.wpilib.math.geometry.*; -import org.wpilib.wpilibj.DriverStation; -import org.wpilib.wpilibj.DriverStation.Alliance; -import org.wpilib.wpilibj.GenericHID; -import org.wpilib.wpilibj.GenericHID.RumbleType; -import org.wpilib.wpilibj.XboxController; -import org.wpilib.wpilibj2.command.Command; -import org.wpilib.wpilibj2.command.Commands; -import org.wpilib.wpilibj2.command.button.CommandXboxController; -import org.wpilib.wpilibj2.command.button.Trigger; -import org.wpilib.wpilibj2.command.sysid.SysIdRoutine; -import org.wpilib.wpilibj2.command.sysid.SysIdRoutine.Direction; +import org.wpilib.driverstation.DriverStation; +import org.wpilib.driverstation.Alliance; +import org.wpilib.wpilibj.XboxController; // Removed +import org.wpilib.command2.Command; +import org.wpilib.command2.Commands; +import org.wpilib.command2.button.CommandXboxController; // Removed +import org.wpilib.command2.button.Trigger; +import org.wpilib.command2.sysid.SysIdRoutine; +import org.wpilib.command2.sysid.SysIdRoutine.Direction; import frc.lib.utils.LocalADStarAK; import frc.robot.RobotState.IntakePosition; import frc.robot.commands.auto.Autos; @@ -270,7 +269,7 @@ public RobotContainer() { * Use this method to define your button->command mappings. Buttons can be created by * instantiating a {@link GenericHID} or one of its subclasses ({@link * org.wpilib.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link - * org.wpilib.wpilibj2.command.button.JoystickButton}. + * org.wpilib.command2.button.JoystickButton}. */ private void configureButtonBindings() { indexer.setDefaultCommand(indexer.stop()); diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index f797de420..c8cb24bd3 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -2,11 +2,11 @@ import static org.wpilib.units.Units.Percent; -import org.wpilib.wpilibj.DriverStation; -import org.wpilib.wpilibj.LEDPattern; +import org.wpilib.driverstation.DriverStation; import frc.robot.subsystems.leds.LedPatterns; import frc.robot.subsystems.leds.Leds.Sections; import org.littletonrobotics.junction.AutoLogOutput; +import org.wpilib.hardware.led.LEDPattern; /** * Robot state will hold all the global information that is shared across the subsystems, such as diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index cc7fded40..7d13b71b4 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -3,8 +3,8 @@ import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.util.Units; -import org.wpilib.wpilibj2.command.Command; -import org.wpilib.wpilibj2.command.Commands; +import org.wpilib.command2.Command; +import org.wpilib.command2.Commands; import frc.lib.utils.AllianceFlipUtil; import frc.robot.Orchestrator; import frc.robot.subsystems.drive.Drive; diff --git a/src/main/java/frc/robot/commands/auto/DriveToPose.java b/src/main/java/frc/robot/commands/auto/DriveToPose.java index 508ed8b77..3543b68d7 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToPose.java +++ b/src/main/java/frc/robot/commands/auto/DriveToPose.java @@ -9,7 +9,7 @@ import org.wpilib.math.util.Units; import org.wpilib.wpilibj.DriverStation; import org.wpilib.wpilibj.DriverStation.Alliance; -import org.wpilib.wpilibj2.command.Command; +import org.wpilib.command2.Command; import frc.lib.utils.GeomUtils; import frc.lib.utils.TunableNumber; import frc.robot.Constants; diff --git a/src/main/java/frc/robot/commands/auto/RotateToOrientation.java b/src/main/java/frc/robot/commands/auto/RotateToOrientation.java index 18d6a987f..d7cf5306b 100644 --- a/src/main/java/frc/robot/commands/auto/RotateToOrientation.java +++ b/src/main/java/frc/robot/commands/auto/RotateToOrientation.java @@ -6,7 +6,7 @@ import org.wpilib.math.kinematics.ChassisSpeeds; import org.wpilib.math.trajectory.TrapezoidProfile.Constraints; import org.wpilib.math.util.Units; -import org.wpilib.wpilibj2.command.Command; +import org.wpilib.command2.Command; import frc.robot.subsystems.drive.Drive; import java.util.Objects; import java.util.function.Supplier; diff --git a/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java b/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java index da0ff2bb7..c9f4a54ff 100644 --- a/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java +++ b/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java @@ -9,7 +9,7 @@ import org.wpilib.math.util.Units; import org.wpilib.wpilibj.DriverStation; import org.wpilib.wpilibj.DriverStation.Alliance; -import org.wpilib.wpilibj2.command.Command; +import org.wpilib.command2.Command; import frc.lib.utils.GeomUtils; import frc.robot.subsystems.drive.Drive; import java.util.function.Supplier; diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index 673a202ce..f8c864d6c 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -13,8 +13,8 @@ import org.wpilib.wpilibj.DriverStation; import org.wpilib.wpilibj.DriverStation.Alliance; import org.wpilib.wpilibj.Timer; -import org.wpilib.wpilibj2.command.Command; -import org.wpilib.wpilibj2.command.Commands; +import org.wpilib.command2.Command; +import org.wpilib.command2.Commands; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; import java.text.DecimalFormat; diff --git a/src/main/java/frc/robot/commands/drive/DriveWithDpad.java b/src/main/java/frc/robot/commands/drive/DriveWithDpad.java index 734ba0a53..16d46ac04 100644 --- a/src/main/java/frc/robot/commands/drive/DriveWithDpad.java +++ b/src/main/java/frc/robot/commands/drive/DriveWithDpad.java @@ -2,7 +2,7 @@ import org.wpilib.math.kinematics.ChassisSpeeds; import org.wpilib.math.util.Units; -import org.wpilib.wpilibj2.command.Command; +import org.wpilib.command2.Command; import frc.robot.subsystems.drive.Drive; import java.util.function.Supplier; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index ae75a7b03..5549e963c 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -5,7 +5,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathPlannerPath; -import org.wpilib.math.Matrix; +import org.wpilib.math.linalg.Matrix; import org.wpilib.math.controller.PIDController; import org.wpilib.math.estimator.SwerveDrivePoseEstimator; import org.wpilib.math.geometry.Pose2d; @@ -13,19 +13,19 @@ import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Rotation3d; import org.wpilib.math.geometry.Twist2d; -import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.ChassisSpeeds; // Removed or renamed (maybe Velocities) import org.wpilib.math.kinematics.SwerveDriveKinematics; import org.wpilib.math.kinematics.SwerveModulePosition; -import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.kinematics.SwerveModuleState; // Removed or renamed import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N3; -import org.wpilib.wpilibj.Alert; -import org.wpilib.wpilibj.Alert.AlertType; -import org.wpilib.wpilibj.DriverStation; -import org.wpilib.wpilibj2.command.Command; -import org.wpilib.wpilibj2.command.Commands; -import org.wpilib.wpilibj2.command.SubsystemBase; -import org.wpilib.wpilibj2.command.sysid.SysIdRoutine; +import org.wpilib.driverstation.Alert; +import org.wpilib.wpilibj.Alert.AlertType; // Removed or renamed +import org.wpilib.driverstation.DriverStation; +import org.wpilib.command2.Command; +import org.wpilib.command2.Commands; +import org.wpilib.command2.SubsystemBase; +import org.wpilib.command2.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.Mode; import frc.robot.subsystems.vision.VisionConstants; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 41aa5bf47..bdd04b1fd 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -9,7 +9,7 @@ import com.pathplanner.lib.config.RobotConfig; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; -import org.wpilib.math.system.plant.DCMotor; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.util.Units; import frc.robot.Constants; diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 06f9cafc4..dbbd6930d 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -13,8 +13,8 @@ import org.wpilib.units.measure.Angle; import org.wpilib.units.measure.AngularVelocity; import org.wpilib.units.measure.LinearAcceleration; -import org.wpilib.wpilibj.Alert; -import org.wpilib.wpilibj.Alert.AlertType; +import org.wpilib.driverstation.Alert; +import org.wpilib.wpilibj.Alert.AlertType; // Removed or renamed import java.util.Queue; /** IO implementation for Pigeon 2. */ diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index ce9682515..5a71dbd96 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -2,12 +2,12 @@ import static frc.robot.subsystems.drive.DriveConstants.*; -import org.wpilib.math.MathUtil; +import org.wpilib.math.util.MathUtil; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.kinematics.SwerveModulePosition; -import org.wpilib.math.kinematics.SwerveModuleState; -import org.wpilib.wpilibj.Alert; -import org.wpilib.wpilibj.Alert.AlertType; +import org.wpilib.math.kinematics.SwerveModuleState; // Removed or renamed (seems like it was broken down into parts) +import org.wpilib.driverstation.Alert; +import org.wpilib.wpilibj.Alert.AlertType; // Removed or renamed import org.littletonrobotics.junction.Logger; public class Module { diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 7e6607e40..fefb66dcb 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -2,12 +2,12 @@ import static frc.robot.subsystems.drive.DriveConstants.*; -import org.wpilib.math.MathUtil; +import org.wpilib.math.util.MathUtil; import org.wpilib.math.controller.PIDController; import org.wpilib.math.geometry.Rotation2d; -import org.wpilib.math.system.plant.LinearSystemId; -import org.wpilib.wpilibj.Timer; -import org.wpilib.wpilibj.simulation.DCMotorSim; +import org.wpilib.math.system.LinearSystemId; // Removed or renamed (likely LinearSystem now) +import org.wpilib.system.Timer; +import org.wpilib.simulation.DCMotorSim; /** Physics sim implementation of module IO. */ public class ModuleIOSim implements ModuleIO { diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 8c8243bae..1fb261dd0 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -12,14 +12,14 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.*; import com.revrobotics.spark.FeedbackSensor; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.ControlType; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import org.wpilib.math.MathUtil; +import org.wpilib.math.util.MathUtil; import org.wpilib.math.filter.Debouncer; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.util.Units; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonSpark.java index 3311cc156..0b36ee8ef 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonSpark.java @@ -21,15 +21,15 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.ControlType; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import org.wpilib.math.MathUtil; +import org.wpilib.math.util.MathUtil; import org.wpilib.math.filter.Debouncer; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.util.Units; diff --git a/src/main/java/frc/robot/subsystems/drive/OdometryThread.java b/src/main/java/frc/robot/subsystems/drive/OdometryThread.java index 0345d663d..505daa3bb 100644 --- a/src/main/java/frc/robot/subsystems/drive/OdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/OdometryThread.java @@ -6,7 +6,7 @@ import com.revrobotics.REVLibError; import com.revrobotics.spark.SparkBase; import org.wpilib.units.measure.Angle; -import org.wpilib.wpilibj.RobotController; +import org.wpilib.system.RobotController; import java.util.ArrayList; import java.util.List; import java.util.Queue; diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 1ed297f22..cd81577e4 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -2,9 +2,9 @@ import static frc.robot.subsystems.indexer.IndexerConstants.FEEDUP_VOLT; -import org.wpilib.wpilibj2.command.Command; -import org.wpilib.wpilibj2.command.Commands; -import org.wpilib.wpilibj2.command.SubsystemBase; +import org.wpilib.command2.Command; +import org.wpilib.command2.Commands; +import org.wpilib.command2.SubsystemBase; import frc.robot.RobotState; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java index 4536f6fdb..e9966b56e 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java @@ -4,8 +4,8 @@ import static frc.robot.subsystems.indexer.IndexerConstants.ENCODER_FEEDUP_POSITION_CONVERSION; import static frc.robot.subsystems.indexer.IndexerConstants.ENCODER_FEEDUP_VELOCITY_CONVERSION; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.EncoderConfig; diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 014ab894f..553b7e7e4 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -2,8 +2,8 @@ import static frc.robot.subsystems.intake.IntakeConstants.*; -import org.wpilib.wpilibj2.command.Command; -import org.wpilib.wpilibj2.command.Commands; +import org.wpilib.command2.Command; +import org.wpilib.command2.Commands; import frc.robot.subsystems.intake.extend.IntakeExtend; import frc.robot.subsystems.intake.rollers.IntakeRollers; diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java index 1a656fead..e04cf3c13 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java @@ -6,11 +6,11 @@ import static frc.robot.subsystems.intake.IntakeConstants.POSITION_TOLERANCE; import static frc.robot.subsystems.intake.IntakeConstants.STALL_VELOCITY; -import org.wpilib.wpilibj.Timer; -import org.wpilib.wpilibj2.command.Command; -import org.wpilib.wpilibj2.command.Commands; -import org.wpilib.wpilibj2.command.ConditionalCommand; -import org.wpilib.wpilibj2.command.SubsystemBase; +import org.wpilib.system.Timer; +import org.wpilib.command2.Command; +import org.wpilib.command2.Commands; +import org.wpilib.command2.ConditionalCommand; +import org.wpilib.command2.SubsystemBase; import frc.robot.RobotState; import frc.robot.RobotState.IntakePosition; import java.util.function.BooleanSupplier; diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java index 132cdf94b..75adae735 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java @@ -13,16 +13,16 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.ControlType; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.EncoderConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import org.wpilib.wpilibj.DigitalInput; +import org.wpilib.hardware.discrete.DigitalInput; public class IntakeExtendIOKraken implements IntakeExtendIO { private final SparkMax extendMotor; diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java index 381c13fa7..7d67a3cb1 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java @@ -11,16 +11,16 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.FeedbackSensor; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.ControlType; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.EncoderConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import org.wpilib.wpilibj.DigitalInput; +import org.wpilib.hardware.discrete.DigitalInput; public class IntakeExtendIOSparkMax implements IntakeExtendIO { diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java index c1d543ac7..7b717cad6 100644 --- a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java +++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java @@ -3,9 +3,9 @@ import static frc.robot.subsystems.intake.IntakeConstants.INTAKE_VOLTS; import static frc.robot.subsystems.intake.IntakeConstants.OUTTAKE_VOLTS; -import org.wpilib.wpilibj2.command.Command; -import org.wpilib.wpilibj2.command.Commands; -import org.wpilib.wpilibj2.command.SubsystemBase; +import org.wpilib.command2.Command; +import org.wpilib.command2.Commands; +import org.wpilib.command2.SubsystemBase; import frc.robot.RobotState; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/robot/subsystems/leds/LedPatterns.java b/src/main/java/frc/robot/subsystems/leds/LedPatterns.java index 18192f674..2b7d9d4a4 100644 --- a/src/main/java/frc/robot/subsystems/leds/LedPatterns.java +++ b/src/main/java/frc/robot/subsystems/leds/LedPatterns.java @@ -5,8 +5,8 @@ import static org.wpilib.units.Units.Seconds; import org.wpilib.units.measure.Distance; -import org.wpilib.wpilibj.LEDPattern; -import org.wpilib.wpilibj.util.Color; +import org.wpilib.hardware.led.LEDPattern; +import org.wpilib.util.Color; import java.util.Map; public enum LedPatterns { diff --git a/src/main/java/frc/robot/subsystems/leds/Leds.java b/src/main/java/frc/robot/subsystems/leds/Leds.java index d14faf7cf..a52a3b6b7 100644 --- a/src/main/java/frc/robot/subsystems/leds/Leds.java +++ b/src/main/java/frc/robot/subsystems/leds/Leds.java @@ -1,18 +1,22 @@ package frc.robot.subsystems.leds; import org.wpilib.networktables.GenericEntry; -import org.wpilib.wpilibj.AddressableLED; -import org.wpilib.wpilibj.AddressableLED.ColorOrder; -import org.wpilib.wpilibj.AddressableLEDBuffer; -import org.wpilib.wpilibj.AddressableLEDBufferView; -import org.wpilib.wpilibj.LEDPattern; +import org.wpilib.hardware.led.AddressableLED; +import org.wpilib.hardware.led.AddressableLED.ColorOrder; +import org.wpilib.hardware.led.AddressableLEDBuffer; +import org.wpilib.hardware.led.AddressableLEDBufferView; +import org.wpilib.hardware.led.LEDPattern; + +// (Begin): Removed or replaced import org.wpilib.wpilibj.shuffleboard.BuiltInLayouts; import org.wpilib.wpilibj.shuffleboard.BuiltInWidgets; import org.wpilib.wpilibj.shuffleboard.Shuffleboard; import org.wpilib.wpilibj.shuffleboard.ShuffleboardLayout; import org.wpilib.wpilibj.shuffleboard.ShuffleboardTab; -import org.wpilib.wpilibj.smartdashboard.SendableChooser; -import org.wpilib.wpilibj2.command.SubsystemBase; +// (End): Removed or replaced + +import org.wpilib.smartdashboard.SendableChooser; +import org.wpilib.command2.SubsystemBase; import frc.robot.Robot; import frc.robot.RobotState; import frc.robot.RobotState.Mode; diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java index acb745f97..14494a398 100644 --- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java +++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java @@ -1,8 +1,8 @@ package frc.robot.subsystems.magicCarpet; -import org.wpilib.wpilibj2.command.Command; -import org.wpilib.wpilibj2.command.Commands; -import org.wpilib.wpilibj2.command.SubsystemBase; +import org.wpilib.command2.Command; +import org.wpilib.command2.Commands; +import org.wpilib.command2.SubsystemBase; import frc.robot.RobotState; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetSparkMax.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetSparkMax.java index 01c075289..87ad37118 100644 --- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetSparkMax.java +++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetSparkMax.java @@ -4,13 +4,13 @@ package frc.robot.subsystems.magicCarpet; import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import org.wpilib.math.MathUtil; +import org.wpilib.math.util.MathUtil; import frc.robot.Schematic; public class MagicCarpetSparkMax implements MagicCarpetIO { diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index a3f9c1791..d65da63b1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -16,13 +16,13 @@ import org.wpilib.units.measure.AngularVelocity; import org.wpilib.units.measure.Distance; import org.wpilib.units.measure.Time; -import org.wpilib.wpilibj.smartdashboard.SmartDashboard; -import org.wpilib.wpilibj2.command.Command; -import org.wpilib.wpilibj2.command.Commands; -import org.wpilib.wpilibj2.command.SubsystemBase; -import org.wpilib.wpilibj2.command.sysid.SysIdRoutine; -import org.wpilib.wpilibj2.command.sysid.SysIdRoutine.Config; -import org.wpilib.wpilibj2.command.sysid.SysIdRoutine.Mechanism; +import org.wpilib.smartdashboard.SmartDashboard; +import org.wpilib.command2.Command; +import org.wpilib.command2.Commands; +import org.wpilib.command2.SubsystemBase; +import org.wpilib.command2.sysid.SysIdRoutine; +import org.wpilib.command2.sysid.SysIdRoutine.Config; +import org.wpilib.command2.sysid.SysIdRoutine.Mechanism; import frc.robot.RobotState; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 17cb653a8..742ba0ee6 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -7,8 +7,8 @@ import static frc.robot.subsystems.shooter.ShooterConstants.*; import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.EncoderConfig; diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 38c67bd6f..7cb98fc0e 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -2,16 +2,16 @@ import static frc.robot.subsystems.vision.VisionConstants.*; -import org.wpilib.math.Matrix; -import org.wpilib.math.VecBuilder; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Pose3d; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N3; -import org.wpilib.wpilibj.Alert; -import org.wpilib.wpilibj.Alert.AlertType; -import org.wpilib.wpilibj2.command.SubsystemBase; +import org.wpilib.driverstation.Alert; +import org.wpilib.wpilibj.Alert.AlertType; // Removed or renamed +import org.wpilib.command2.SubsystemBase; import frc.robot.RobotState; import java.util.LinkedList; import java.util.List; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 5a8d80b34..17953bf11 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -1,6 +1,6 @@ package frc.robot.subsystems.vision; -import org.wpilib.apriltag.AprilTagFieldLayout; +import org.wpilib.vision.apriltag.AprilTagFieldLayout; import org.wpilib.math.geometry.Rotation3d; import org.wpilib.math.geometry.Transform3d; import org.wpilib.math.geometry.Translation3d; diff --git a/src/main/java/frc/robot/util/CustomTriggers.java b/src/main/java/frc/robot/util/CustomTriggers.java index 57829c228..c229f235d 100644 --- a/src/main/java/frc/robot/util/CustomTriggers.java +++ b/src/main/java/frc/robot/util/CustomTriggers.java @@ -1,7 +1,7 @@ package frc.robot.util; -import org.wpilib.wpilibj2.command.CommandScheduler; -import org.wpilib.wpilibj2.command.button.Trigger; +import org.wpilib.command2.CommandScheduler; +import org.wpilib.command2.button.Trigger; import java.util.HashMap; import java.util.Map; import java.util.function.BooleanSupplier; From 13866c213a52aeb2d4e91b2b3def18e94391d705 Mon Sep 17 00:00:00 2001 From: EJainDev Date: Thu, 17 Sep 2026 15:53:27 -0400 Subject: [PATCH 04/15] fix: replace AlertType with Level --- src/main/java/frc/robot/Robot.java | 6 +++--- src/main/java/frc/robot/subsystems/drive/Drive.java | 8 ++++---- .../java/frc/robot/subsystems/drive/GyroIOPigeon2.java | 4 ++-- src/main/java/frc/robot/subsystems/drive/Module.java | 6 +++--- src/main/java/frc/robot/subsystems/vision/Vision.java | 4 ++-- 5 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9c34ae8ad..dd6afaecb 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,9 +4,9 @@ package frc.robot; +import org.wpilib.driverstation.Alert.Level; import org.wpilib.math.filter.LinearFilter; import org.wpilib.driverstation.Alert; -import org.wpilib.wpilibj.Alert.AlertType; // Removed or renamed import org.wpilib.driverstation.DriverStation; import org.wpilib.system.RobotController; import org.wpilib.system.Timer; @@ -53,11 +53,11 @@ public class Robot extends LoggedRobot { private static int lowBatteryCycleCount = 0; private final Alert canErrorAlert = - new Alert("CAN errors detected, robot may not be controllable.", AlertType.kError); + new Alert("CAN errors detected, robot may not be controllable.", Level.HIGH); private final Alert lowBatteryAlert = new Alert( "Battery voltage is very low, consider turning off the robot or replacing the battery.", - AlertType.kWarning); + Level.MEDIUM); /** * This function is run when the robot is first started up and should be used for any diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 5549e963c..ebced4901 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -5,6 +5,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathPlannerPath; +import org.wpilib.driverstation.Alert.Level; import org.wpilib.math.linalg.Matrix; import org.wpilib.math.controller.PIDController; import org.wpilib.math.estimator.SwerveDrivePoseEstimator; @@ -20,7 +21,6 @@ import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N3; import org.wpilib.driverstation.Alert; -import org.wpilib.wpilibj.Alert.AlertType; // Removed or renamed import org.wpilib.driverstation.DriverStation; import org.wpilib.command2.Command; import org.wpilib.command2.Commands; @@ -39,14 +39,14 @@ public class Drive extends SubsystemBase { private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Alert impactAlert = - new Alert("Impact Detected, lowering elevator to prevent flipping.", AlertType.kWarning); + new Alert("Impact Detected, lowering elevator to prevent flipping.", Level.MEDIUM); private final Alert tiltAlert = new Alert( - ("Tilt Threshold reached, lowering elevator to prevent flipping"), AlertType.kWarning); + ("Tilt Threshold reached, lowering elevator to prevent flipping"), Level.MEDIUM); private final Module[] modules = new Module[4]; // FL, FR, BL, BR private final SysIdRoutine sysId; private final Alert gyroDisconnectedAlert = - new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); + new Alert("Disconnected gyro, using kinematics as fallback.", Level.HIGH); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(moduleTranslations); private Rotation2d rawGyroRotation = Rotation2d.kZero; diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index dbbd6930d..1eae72c60 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -8,19 +8,19 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; +import org.wpilib.driverstation.Alert.Level; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.util.Units; import org.wpilib.units.measure.Angle; import org.wpilib.units.measure.AngularVelocity; import org.wpilib.units.measure.LinearAcceleration; import org.wpilib.driverstation.Alert; -import org.wpilib.wpilibj.Alert.AlertType; // Removed or renamed import java.util.Queue; /** IO implementation for Pigeon 2. */ public class GyroIOPigeon2 implements GyroIO { private final Alert impactAlert = - new Alert("Impact Detected, lowering elevator to prevent flipping.", AlertType.kWarning); + new Alert("Impact Detected, lowering elevator to prevent flipping.", Level.MEDIUM); private final Pigeon2 pigeon = new Pigeon2(pigeonCanId); private final StatusSignal yaw = pigeon.getYaw(); private final Queue yawPositionQueue; diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 5a71dbd96..a9414819f 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -2,12 +2,12 @@ import static frc.robot.subsystems.drive.DriveConstants.*; +import org.wpilib.driverstation.Alert.Level; import org.wpilib.math.util.MathUtil; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.kinematics.SwerveModulePosition; import org.wpilib.math.kinematics.SwerveModuleState; // Removed or renamed (seems like it was broken down into parts) import org.wpilib.driverstation.Alert; -import org.wpilib.wpilibj.Alert.AlertType; // Removed or renamed import org.littletonrobotics.junction.Logger; public class Module { @@ -25,10 +25,10 @@ public Module(ModuleIO io, int index) { driveDisconnectedAlert = new Alert( "Disconnected drive motor on module " + Integer.toString(index) + ".", - AlertType.kError); + Level.HIGH); turnDisconnectedAlert = new Alert( - "Disconnected turn motor on module " + Integer.toString(index) + ".", AlertType.kError); + "Disconnected turn motor on module " + Integer.toString(index) + ".", Level.HIGH); } public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 7cb98fc0e..3b04a4d85 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -2,6 +2,7 @@ import static frc.robot.subsystems.vision.VisionConstants.*; +import org.wpilib.driverstation.Alert.Level; import org.wpilib.math.linalg.Matrix; import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.geometry.Pose2d; @@ -10,7 +11,6 @@ import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N3; import org.wpilib.driverstation.Alert; -import org.wpilib.wpilibj.Alert.AlertType; // Removed or renamed import org.wpilib.command2.SubsystemBase; import frc.robot.RobotState; import java.util.LinkedList; @@ -38,7 +38,7 @@ public Vision(VisionConsumer consumer, VisionIO... io) { for (int i = 0; i < inputs.length; i++) { disconnectedAlerts[i] = new Alert( - "Vision camera " + Integer.toString(i) + " is disconnected.", AlertType.kWarning); + "Vision camera " + Integer.toString(i) + " is disconnected.", Level.MEDIUM); } } From be36b7fe5b2415b8c2eb29a9af53a128cf138cde Mon Sep 17 00:00:00 2001 From: EJainDev Date: Thu, 17 Sep 2026 15:55:44 -0400 Subject: [PATCH 05/15] fix: update easy imports for commands --- src/main/java/frc/robot/commands/auto/DriveToPose.java | 6 +++--- .../frc/robot/commands/auto/RotateToOrientation.java | 2 +- .../frc/robot/commands/auto/StraightDriveToPose.java | 6 +++--- .../java/frc/robot/commands/drive/DriveCommands.java | 10 +++++----- .../java/frc/robot/commands/drive/DriveWithDpad.java | 2 +- 5 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/DriveToPose.java b/src/main/java/frc/robot/commands/auto/DriveToPose.java index 3543b68d7..15c376e1c 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToPose.java +++ b/src/main/java/frc/robot/commands/auto/DriveToPose.java @@ -4,11 +4,11 @@ import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; -import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.ChassisSpeeds; // Removed or renamed import org.wpilib.math.trajectory.TrapezoidProfile; import org.wpilib.math.util.Units; -import org.wpilib.wpilibj.DriverStation; -import org.wpilib.wpilibj.DriverStation.Alliance; +import org.wpilib.driverstation.DriverStation; +import org.wpilib.driverstation.Alliance; import org.wpilib.command2.Command; import frc.lib.utils.GeomUtils; import frc.lib.utils.TunableNumber; diff --git a/src/main/java/frc/robot/commands/auto/RotateToOrientation.java b/src/main/java/frc/robot/commands/auto/RotateToOrientation.java index d7cf5306b..d5ac51a99 100644 --- a/src/main/java/frc/robot/commands/auto/RotateToOrientation.java +++ b/src/main/java/frc/robot/commands/auto/RotateToOrientation.java @@ -3,7 +3,7 @@ import org.wpilib.math.controller.ProfiledPIDController; import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Rotation2d; -import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.ChassisSpeeds; // Removed or renamed import org.wpilib.math.trajectory.TrapezoidProfile.Constraints; import org.wpilib.math.util.Units; import org.wpilib.command2.Command; diff --git a/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java b/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java index c9f4a54ff..c3bc0e419 100644 --- a/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java +++ b/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java @@ -4,11 +4,11 @@ import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; -import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.ChassisSpeeds; // Removed or renamed import org.wpilib.math.trajectory.TrapezoidProfile.Constraints; import org.wpilib.math.util.Units; -import org.wpilib.wpilibj.DriverStation; -import org.wpilib.wpilibj.DriverStation.Alliance; +import org.wpilib.driverstation.DriverStation; +import org.wpilib.driverstation.Alliance; import org.wpilib.command2.Command; import frc.lib.utils.GeomUtils; import frc.robot.subsystems.drive.Drive; diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index f8c864d6c..29addad99 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -1,18 +1,18 @@ package frc.robot.commands.drive; -import org.wpilib.math.MathUtil; +import org.wpilib.math.util.MathUtil; import org.wpilib.math.controller.ProfiledPIDController; import org.wpilib.math.filter.SlewRateLimiter; import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Transform2d; import org.wpilib.math.geometry.Translation2d; -import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.ChassisSpeeds; // Removed or renamed import org.wpilib.math.trajectory.TrapezoidProfile; import org.wpilib.math.util.Units; -import org.wpilib.wpilibj.DriverStation; -import org.wpilib.wpilibj.DriverStation.Alliance; -import org.wpilib.wpilibj.Timer; +import org.wpilib.driverstation.DriverStation; +import org.wpilib.driverstation.Alliance; +import org.wpilib.system.Timer; import org.wpilib.command2.Command; import org.wpilib.command2.Commands; import frc.robot.subsystems.drive.Drive; diff --git a/src/main/java/frc/robot/commands/drive/DriveWithDpad.java b/src/main/java/frc/robot/commands/drive/DriveWithDpad.java index 16d46ac04..faf5201a0 100644 --- a/src/main/java/frc/robot/commands/drive/DriveWithDpad.java +++ b/src/main/java/frc/robot/commands/drive/DriveWithDpad.java @@ -1,6 +1,6 @@ package frc.robot.commands.drive; -import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.ChassisSpeeds; // Removed or renamed import org.wpilib.math.util.Units; import org.wpilib.command2.Command; import frc.robot.subsystems.drive.Drive; From ed96ab04d848f100b8211e1bddf8e6756bb0428f Mon Sep 17 00:00:00 2001 From: EJainDev Date: Thu, 17 Sep 2026 17:48:21 -0400 Subject: [PATCH 06/15] deps: update to even more recent alpha --- build.gradle | 11 ++++------- vendordeps/AdvantageKit.json | 2 +- vendordeps/CommandsV2.json | 2 +- vendordeps/PathplannerLib.json | 2 +- vendordeps/Phoenix6.json | 2 +- vendordeps/REVLib.json | 20 ++++++++++---------- vendordeps/photonlib.json | 2 +- 7 files changed, 19 insertions(+), 22 deletions(-) diff --git a/build.gradle b/build.gradle index bf50cdb02..2cdb03006 100755 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "org.wpilib.GradleRIO" version "2027.0.0-alpha-5" + id "org.wpilib.GradleRIO" version "2027.0.0-alpha-7" id "com.peterabeles.gversion" version "1.10" id "com.diffplug.spotless" version "6.25.0" } @@ -8,7 +8,7 @@ plugins { wpi.maven.useLocal = false wpi.maven.useDevelopment = true -wpi.versions.wpilibVersion = providers.gradleProperty('wpilibVersion').orElse('2027.0.0-alpha-6').get() +wpi.versions.wpilibVersion = providers.gradleProperty('wpilibVersion').orElse('2027.0.0-alpha-7').get() java { sourceCompatibility = JavaVersion.VERSION_25 @@ -79,9 +79,6 @@ if (roborioTargetType != null) { } } -// Set to true to use debug for JNI. -wpi.java.debugJni = false - // Set this to true to enable desktop support. def includeDesktopSupport = true @@ -156,8 +153,8 @@ jar { if (deployArtifact != null) { deployArtifact.jarTask = jar } -wpi.java.configureExecutableTasks(jar) -wpi.java.configureTestTasks(test) +// IMPORTANT: wpi.java.configureExecutableTasks(jar) +// IMPORTANT: wpi.java.configureTestTasks(test) // Configure string concat to always inline compile tasks.withType(JavaCompile) { diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 1a8abd54c..c923bbef2 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -3,7 +3,7 @@ "name": "AdvantageKit", "version": "27.0.0-alpha-4", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", - "wpilibYear": "2027_alpha5", + "wpilibYear": "2027_alpha7", "mavenUrls": [ "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" ], diff --git a/vendordeps/CommandsV2.json b/vendordeps/CommandsV2.json index 8358b098e..26f7054db 100644 --- a/vendordeps/CommandsV2.json +++ b/vendordeps/CommandsV2.json @@ -3,7 +3,7 @@ "name": "Commands V2", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "wpilibYear": "2027_alpha5", + "wpilibYear": "2027_alpha7", "mavenUrls": [], "jsonUrl": "", "conflictsWith": [ diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 9b36d2bf1..0c94a9ce6 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -3,7 +3,7 @@ "name": "PathplannerLib", "version": "2027.0.0-alpha-3", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "wpilibYear": "2027_alpha5", + "wpilibYear": "2027_alpha7", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 40dd410c2..0f8d5a4cd 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -2,7 +2,7 @@ "fileName": "Phoenix6-26.50.0-alpha-1.json", "name": "CTRE-Phoenix (v6)", "version": "26.50.0-alpha-1", - "wpilibYear": "2027_alpha5", + "wpilibYear": "2027_alpha7", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 7efb9b64a..773db168c 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,8 +1,8 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2027.0.0-alpha-6", - "wpilibYear": "2027_alpha5", + "version": "2027.0.0-alpha-7", + "wpilibYear": "2027_alpha7", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "https://maven.revrobotics.com/" @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2027.0.0-alpha-6" + "version": "2027.0.0-alpha-7" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2027.0.0-alpha-6", + "version": "2027.0.0-alpha-7", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -33,7 +33,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2027.0.0-alpha-6", + "version": "2027.0.0-alpha-7", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -47,7 +47,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2027.0.0-alpha-6", + "version": "2027.0.0-alpha-7", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -63,7 +63,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2027.0.0-alpha-6", + "version": "2027.0.0-alpha-7", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -79,7 +79,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2027.0.0-alpha-6", + "version": "2027.0.0-alpha-7", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -95,7 +95,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2027.0.0-alpha-6", + "version": "2027.0.0-alpha-7", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -110,7 +110,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2027.0.0-alpha-6", + "version": "2027.0.0-alpha-7", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 3f6b31fca..1cb9ff7c2 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -3,7 +3,7 @@ "name": "photonlib", "version": "v2027.0.0-alpha-2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "wpilibYear": "2027_alpha5", + "wpilibYear": "2027_alpha7", "mavenUrls": [ "https://maven.photonvision.org/repository/internal", "https://maven.photonvision.org/repository/snapshots" From 35b40894c059521545a6be981e091b3cabf451e9 Mon Sep 17 00:00:00 2001 From: EJainDev Date: Thu, 17 Sep 2026 18:43:52 -0400 Subject: [PATCH 07/15] fix alert imports --- src/main/java/frc/robot/Robot.java | 4 ++-- src/main/java/frc/robot/subsystems/drive/Drive.java | 4 ++-- src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java | 4 ++-- src/main/java/frc/robot/subsystems/drive/Module.java | 4 ++-- src/main/java/frc/robot/subsystems/vision/Vision.java | 4 ++-- 5 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index dd6afaecb..b2f5fcc36 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,9 +4,9 @@ package frc.robot; -import org.wpilib.driverstation.Alert.Level; +import org.wpilib.util.Alert.Level; import org.wpilib.math.filter.LinearFilter; -import org.wpilib.driverstation.Alert; +import org.wpilib.util.Alert; import org.wpilib.driverstation.DriverStation; import org.wpilib.system.RobotController; import org.wpilib.system.Timer; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index ebced4901..34973e0da 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -5,7 +5,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathPlannerPath; -import org.wpilib.driverstation.Alert.Level; +import org.wpilib.util.Alert.Level; import org.wpilib.math.linalg.Matrix; import org.wpilib.math.controller.PIDController; import org.wpilib.math.estimator.SwerveDrivePoseEstimator; @@ -20,7 +20,7 @@ import org.wpilib.math.kinematics.SwerveModuleState; // Removed or renamed import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N3; -import org.wpilib.driverstation.Alert; +import org.wpilib.util.Alert; import org.wpilib.driverstation.DriverStation; import org.wpilib.command2.Command; import org.wpilib.command2.Commands; diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 1eae72c60..b9fb49cb9 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -8,13 +8,13 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; -import org.wpilib.driverstation.Alert.Level; +import org.wpilib.util.Alert.Level; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.util.Units; import org.wpilib.units.measure.Angle; import org.wpilib.units.measure.AngularVelocity; import org.wpilib.units.measure.LinearAcceleration; -import org.wpilib.driverstation.Alert; +import org.wpilib.util.Alert; import java.util.Queue; /** IO implementation for Pigeon 2. */ diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index a9414819f..a31b998a5 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -2,12 +2,12 @@ import static frc.robot.subsystems.drive.DriveConstants.*; -import org.wpilib.driverstation.Alert.Level; +import org.wpilib.util.Alert.Level; import org.wpilib.math.util.MathUtil; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.kinematics.SwerveModulePosition; import org.wpilib.math.kinematics.SwerveModuleState; // Removed or renamed (seems like it was broken down into parts) -import org.wpilib.driverstation.Alert; +import org.wpilib.util.Alert; import org.littletonrobotics.junction.Logger; public class Module { diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 3b04a4d85..085d2bc46 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -2,7 +2,7 @@ import static frc.robot.subsystems.vision.VisionConstants.*; -import org.wpilib.driverstation.Alert.Level; +import org.wpilib.util.Alert.Level; import org.wpilib.math.linalg.Matrix; import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.geometry.Pose2d; @@ -10,7 +10,7 @@ import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N3; -import org.wpilib.driverstation.Alert; +import org.wpilib.util.Alert; import org.wpilib.command2.SubsystemBase; import frc.robot.RobotState; import java.util.LinkedList; From 6a304b36590ebe570762233a40e5324df0c81b2c Mon Sep 17 00:00:00 2001 From: EJainDev Date: Fri, 18 Sep 2026 07:46:40 -0700 Subject: [PATCH 08/15] deps: add commands v3 --- vendordeps/CommandsV2.json | 46 -------------------------------------- vendordeps/CommandsV3.json | 25 +++++++++++++++++++++ 2 files changed, 25 insertions(+), 46 deletions(-) delete mode 100644 vendordeps/CommandsV2.json create mode 100644 vendordeps/CommandsV3.json diff --git a/vendordeps/CommandsV2.json b/vendordeps/CommandsV2.json deleted file mode 100644 index 26f7054db..000000000 --- a/vendordeps/CommandsV2.json +++ /dev/null @@ -1,46 +0,0 @@ -{ - "fileName": "CommandsV2.json", - "name": "Commands V2", - "version": "1.0.0", - "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "wpilibYear": "2027_alpha7", - "mavenUrls": [], - "jsonUrl": "", - "conflictsWith": [ - { - "uuid": "4decdc05-a056-46cf-9561-39449bbb01306", - "errorMessage": "Users can not have both Commands v2 and Commands v3 vendordeps in their robot program.", - "offlineFileName": "CommandsV3.json" - } - ], - "javaDependencies": [ - { - "groupId": "org.wpilib.commandsv2", - "artifactId": "commandsv2-java", - "version": "wpilib" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "org.wpilib.commandsv2", - "artifactId": "commandsv2-cpp", - "version": "wpilib", - "libName": "commandsv2", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxsystemcore", - "linuxathena", - "linuxarm32", - "linuxarm64", - "windowsx86-64", - "windowsx86", - "linuxx86-64", - "osxuniversal" - ] - } - ] -} diff --git a/vendordeps/CommandsV3.json b/vendordeps/CommandsV3.json new file mode 100644 index 000000000..7b118ed77 --- /dev/null +++ b/vendordeps/CommandsV3.json @@ -0,0 +1,25 @@ +{ + "fileName": "CommandsV3.json", + "name": "Commands v3", + "version": "1.0.0", + "uuid": "4decdc05-a056-46cf-9561-39449bbb0130", + "wpilibYear": "2027_alpha7", + "mavenUrls": [], + "jsonUrl": "", + "conflictsWith": [ + { + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "errorMessage": "Users can not have both Commands v2 and Commands v3 vendordeps in their robot program.", + "offlineFileName": "CommandsV2.json" + } + ], + "javaDependencies": [ + { + "groupId": "org.wpilib", + "artifactId": "commandsv3-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} \ No newline at end of file From 362e7d7f8f155c9806c89efa2abe04bab9a5441e Mon Sep 17 00:00:00 2001 From: EJainDev Date: Sat, 19 Sep 2026 00:01:57 -0400 Subject: [PATCH 09/15] fix: make DriveCommands 0 errors --- src/main/java/frc/robot/RobotContainer.java | 1 - .../robot/commands/drive/DriveCommands.java | 256 ++++++++---------- .../frc/robot/subsystems/drive/Drive.java | 10 +- 3 files changed, 119 insertions(+), 148 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0a92d8764..4cdeb2347 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,7 +4,6 @@ package frc.robot; -import static org.wpilib.units.Units.*; import static frc.robot.subsystems.drive.DriveConstants.ppConfig; import static frc.robot.subsystems.vision.VisionConstants.*; diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index 29addad99..8ff52e22a 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -7,14 +7,13 @@ import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Transform2d; import org.wpilib.math.geometry.Translation2d; -import org.wpilib.math.kinematics.ChassisSpeeds; // Removed or renamed +import org.wpilib.math.kinematics.ChassisVelocities; // Removed or renamed import org.wpilib.math.trajectory.TrapezoidProfile; import org.wpilib.math.util.Units; import org.wpilib.driverstation.DriverStation; import org.wpilib.driverstation.Alliance; import org.wpilib.system.Timer; -import org.wpilib.command2.Command; -import org.wpilib.command2.Commands; +import org.wpilib.command3.Command; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; import java.text.DecimalFormat; @@ -23,6 +22,9 @@ import java.util.List; import java.util.function.DoubleSupplier; import java.util.function.Supplier; +import org.wpilib.driverstation.MatchState; +import org.wpilib.units.TimeUnit; +import org.wpilib.units.measure.Time; public class DriveCommands { private static final double DEADBAND = 0.1; @@ -54,7 +56,7 @@ private static Translation2d getLinearVelocityFromJoysticks(double x, double y) // Return new linear velocity return new Pose2d(new Translation2d(), linearDirection) - .transformBy(new Transform2d(linearMagnitude, 0.0, Rotation2d.kZero)) + .transformBy(new Transform2d(linearMagnitude, 0.0, Rotation2d.ZERO)) .getTranslation(); } @@ -66,8 +68,7 @@ public static Command joystickDrive( DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier omegaSupplier) { - return Commands.run( - () -> { + return drive.runRepeatedly(() -> { // Get linear velocity Translation2d linearVelocity = getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()); @@ -79,22 +80,19 @@ public static Command joystickDrive( omega = Math.copySign(omega * omega, omega); // Convert to field relative speeds & send command - ChassisSpeeds speeds = - new ChassisSpeeds( + ChassisVelocities speeds = + new ChassisVelocities( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), omega * drive.getMaxAngularSpeedRadPerSec()); boolean isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; + MatchState.getAlliance().isPresent() + && MatchState.getAlliance().get() == Alliance.RED; drive.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( - speeds, - isFlipped + speeds.toFieldRelative(isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation())); - }, - drive); + }).named("Joystick Drive"); } /** @@ -118,11 +116,13 @@ public static Command joystickDriveAtAngle( angleController.enableContinuousInput(-Math.PI, Math.PI); // Construct command - return Commands.run( + return drive.runRepeatedly( () -> { + angleController.reset(drive.getRotation().getRadians()); + boolean isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; + MatchState.getAlliance().isPresent() + && MatchState.getAlliance().get() == Alliance.RED; // Get linear velocity Translation2d linearVelocity = getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()); @@ -133,22 +133,17 @@ public static Command joystickDriveAtAngle( drive.getRotation().getRadians(), rotationSupplier.get().getRadians()); // Convert to field relative speeds & send command - ChassisSpeeds speeds = - new ChassisSpeeds( + ChassisVelocities speeds = + new ChassisVelocities( -linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), -linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), omega); drive.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( - speeds, + speeds.toFieldRelative( isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation())); - }, - drive) - - // Reset PID controller when command starts - .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); + }).named("Joystick Drive at Angle"); } /** @@ -161,57 +156,44 @@ public static Command feedforwardCharacterization(Drive drive) { List voltageSamples = new LinkedList<>(); Timer timer = new Timer(); - return Commands.sequence( - // Reset data - Commands.runOnce( - () -> { - velocitySamples.clear(); - voltageSamples.clear(); - }), - - // Allow modules to orient - Commands.run( - () -> { - drive.runCharacterization(0.0); - }, - drive) - .withTimeout(FF_START_DELAY), - - // Start timer - Commands.runOnce(timer::restart), - - // Accelerate and gather data - Commands.run( - () -> { - double voltage = timer.get() * FF_RAMP_RATE; - drive.runCharacterization(voltage); - velocitySamples.add(drive.getFFCharacterizationVelocity()); - voltageSamples.add(voltage); - }, - drive) - - // When cancelled, calculate and print results - .finallyDo( - () -> { - int n = velocitySamples.size(); - double sumX = 0.0; - double sumY = 0.0; - double sumXY = 0.0; - double sumX2 = 0.0; - for (int i = 0; i < n; i++) { - sumX += velocitySamples.get(i); - sumY += voltageSamples.get(i); - sumXY += velocitySamples.get(i) * voltageSamples.get(i); - sumX2 += velocitySamples.get(i) * velocitySamples.get(i); - } - double kS = (sumY * sumX2 - sumX * sumXY) / (n * sumX2 - sumX * sumX); - double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); - - NumberFormat formatter = new DecimalFormat("#0.00000"); - System.out.println("********** Drive FF Characterization Results **********"); - System.out.println("\tkS: " + formatter.format(kS)); - System.out.println("\tkV: " + formatter.format(kV)); - })); + return drive.run( + coro -> { + velocitySamples.clear(); + voltageSamples.clear(); + + // Change: Originally had a timeout of FF_START_DELAY seconds + drive.runCharacterization(0.0); + + timer.restart(); + + while (true) { + double voltage = timer.get() * FF_RAMP_RATE; + drive.runCharacterization(voltage); + velocitySamples.add(drive.getFFCharacterizationVelocity()); + voltageSamples.add(voltage); + coro.yield(); + } + } + ).whenCanceled(() -> { + int n = velocitySamples.size(); + double sumX = 0.0; + double sumY = 0.0; + double sumXY = 0.0; + double sumX2 = 0.0; + for (int i = 0; i < n; i++) { + sumX += velocitySamples.get(i); + sumY += voltageSamples.get(i); + sumXY += velocitySamples.get(i) * voltageSamples.get(i); + sumX2 += velocitySamples.get(i) * velocitySamples.get(i); + } + double kS = (sumY * sumX2 - sumX * sumXY) / (n * sumX2 - sumX * sumX); + double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); + + NumberFormat formatter = new DecimalFormat("#0.00000"); + System.out.println("********** Drive FF Characterization Results **********"); + System.out.println("\tkS: " + formatter.format(kS)); + System.out.println("\tkV: " + formatter.format(kV)); + }).named("FF Characterization"); } /** Measures the robot's wheel radius by spinning in a circle. */ @@ -219,74 +201,64 @@ public static Command wheelRadiusCharacterization(Drive drive) { SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE); WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); - return Commands.parallel( - // Drive control sequence - Commands.sequence( - // Reset acceleration limiter - Commands.runOnce( - () -> { - limiter.reset(0.0); - }), - - // Turn in place, accelerating up to full speed - Commands.run( - () -> { - double speed = limiter.calculate(WHEEL_RADIUS_MAX_VELOCITY); - drive.runVelocity(new ChassisSpeeds(0.0, 0.0, speed)); - }, - drive)), - - // Measurement sequence - Commands.sequence( - // Wait for modules to fully orient before starting measurement - Commands.waitSeconds(1.0), - - // Record starting measurement - Commands.runOnce( - () -> { - state.positions = drive.getWheelRadiusCharacterizationPositions(); - state.lastAngle = drive.getRotation(); - state.gyroDelta = 0.0; - }), - - // Update gyro delta - Commands.run( - () -> { - var rotation = drive.getRotation(); - state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); - state.lastAngle = rotation; - }) - - // When cancelled, calculate and print results - .finallyDo( - () -> { - double[] positions = drive.getWheelRadiusCharacterizationPositions(); - double wheelDelta = 0.0; - for (int i = 0; i < 4; i++) { - wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; - } - double wheelRadius = - (state.gyroDelta * DriveConstants.driveBaseRadius) / wheelDelta; - - NumberFormat formatter = new DecimalFormat("#0.000"); - System.out.println( - "********** Wheel Radius Characterization Results **********"); - System.out.println( - "\tWheel Delta: " + formatter.format(wheelDelta) + " radians"); - System.out.println( - "\tGyro Delta: " + formatter.format(state.gyroDelta) + " radians"); - System.out.println( - "\tWheel Radius: " - + formatter.format(wheelRadius) - + " meters, " - + formatter.format(Units.metersToInches(wheelRadius)) - + " inches"); - }))); + return Command.parallel( + drive.run( + coro -> { + limiter.reset(0.0); + + while (true) { + double speed = limiter.calculate(WHEEL_RADIUS_MAX_VELOCITY); + drive.runVelocity(new ChassisVelocities(0.0, 0.0, speed)); + + coro.yield(); + } + } + ).named("Drive control sequence"), + Command.noRequirements( + coro -> { + coro.wait(org.wpilib.units.Units.Second.of(1)); + + state.positions = drive.getWheelRadiusCharacterizationPositions(); + state.lastAngle = drive.getRotation(); + state.gyroDelta = 0.0; + + while (true) { + var rotation = drive.getRotation(); + state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); + state.lastAngle = rotation; + + coro.yield(); + } + } + ).whenCanceled(() -> { + double[] positions = drive.getWheelRadiusCharacterizationPositions(); + double wheelDelta = 0.0; + for (int i = 0; i < 4; i++) { + wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; + } + double wheelRadius = + (state.gyroDelta * DriveConstants.driveBaseRadius) / wheelDelta; + + NumberFormat formatter = new DecimalFormat("#0.000"); + System.out.println( + "********** Wheel Radius Characterization Results **********"); + System.out.println( + "\tWheel Delta: " + formatter.format(wheelDelta) + " radians"); + System.out.println( + "\tGyro Delta: " + formatter.format(state.gyroDelta) + " radians"); + System.out.println( + "\tWheel Radius: " + + formatter.format(wheelRadius) + + " meters, " + + formatter.format(Units.metersToInches(wheelRadius)) + + " inches"); + }).named("Measurement sequence") + ).named("Wheel Radius Characterization"); } private static class WheelRadiusCharacterizationState { double[] positions = new double[4]; - Rotation2d lastAngle = Rotation2d.kZero; + Rotation2d lastAngle = Rotation2d.ZERO; double gyroDelta = 0.0; } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 34973e0da..e5ff3ccea 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -5,6 +5,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathPlannerPath; +import org.wpilib.math.kinematics.ChassisVelocities; import org.wpilib.util.Alert.Level; import org.wpilib.math.linalg.Matrix; import org.wpilib.math.controller.PIDController; @@ -22,9 +23,8 @@ import org.wpilib.math.numbers.N3; import org.wpilib.util.Alert; import org.wpilib.driverstation.DriverStation; -import org.wpilib.command2.Command; -import org.wpilib.command2.Commands; -import org.wpilib.command2.SubsystemBase; +import org.wpilib.command3.Command; +import org.wpilib.command3.Mechanism; import org.wpilib.command2.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.Mode; @@ -34,7 +34,7 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -public class Drive extends SubsystemBase { +public class Drive implements Mechanism { static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); @@ -160,7 +160,7 @@ public void periodic() { * * @param speeds Speeds in meters/sec */ - public void runVelocity(ChassisSpeeds speeds) { + public void runVelocity(ChassisVelocities speeds) { // Calculate module setpoints ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); From 207bd50f468a5a4af455543da1a7fac42c325858 Mon Sep 17 00:00:00 2001 From: EJainDev Date: Sat, 19 Sep 2026 10:15:04 -0400 Subject: [PATCH 10/15] fix: update DriveWithDpad command to v3 --- src/main/java/frc/robot/Constants.java | 3 + src/main/java/frc/robot/RobotContainer.java | 98 +++++++++++++------ .../frc/robot/commands/auto/DriveToPose.java | 70 +++++++------ .../commands/auto/RotateToOrientation.java | 4 +- .../commands/auto/StraightDriveToPose.java | 4 +- .../robot/commands/drive/DriveWithDpad.java | 32 ------ .../frc/robot/subsystems/drive/Drive.java | 14 +-- .../robot/util/ShooterLeadCompensator.java | 4 +- 8 files changed, 123 insertions(+), 106 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/drive/DriveWithDpad.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1f34a1380..77122b5e8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -2,12 +2,15 @@ import org.wpilib.driverstation.DriverStation; import org.wpilib.framework.RobotBase; +import org.wpilib.math.util.Units; import java.io.BufferedReader; import java.io.FileReader; import java.io.IOException; import java.util.Map; public class Constants { + public static final double SLOW_SPEED = Units.inchesToMeters(20); + // Change this temporarily to override the RobotType, e.g. RobotType.ROBOT_SIMBOT private static final RobotType ROBOT_TYPE_OVERRIDE = null; public static final boolean tuningMode = false; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4cdeb2347..aea04309f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,31 +4,30 @@ package frc.robot; +import static frc.robot.Constants.SLOW_SPEED; import static frc.robot.subsystems.drive.DriveConstants.ppConfig; -import static frc.robot.subsystems.vision.VisionConstants.*; +import static frc.robot.subsystems.vision.VisionConstants.camera0Name; +import static frc.robot.subsystems.vision.VisionConstants.camera1Name; +import static frc.robot.subsystems.vision.VisionConstants.camera2Name; +import static frc.robot.subsystems.vision.VisionConstants.robotToCamera0; +import static frc.robot.subsystems.vision.VisionConstants.robotToCamera1; +import static frc.robot.subsystems.vision.VisionConstants.robotToCamera2; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.PathPlannerLogging; -import org.wpilib.driverstation.GenericHID.RumbleType; -import org.wpilib.math.geometry.*; -import org.wpilib.driverstation.DriverStation; -import org.wpilib.driverstation.Alliance; -import org.wpilib.wpilibj.XboxController; // Removed -import org.wpilib.command2.Command; -import org.wpilib.command2.Commands; -import org.wpilib.command2.button.CommandXboxController; // Removed -import org.wpilib.command2.button.Trigger; -import org.wpilib.command2.sysid.SysIdRoutine; -import org.wpilib.command2.sysid.SysIdRoutine.Direction; import frc.lib.utils.LocalADStarAK; import frc.robot.RobotState.IntakePosition; import frc.robot.commands.auto.Autos; import frc.robot.commands.drive.DriveCommands; -import frc.robot.commands.drive.DriveWithDpad; -import frc.robot.subsystems.drive.*; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.GyroIO; +import frc.robot.subsystems.drive.GyroIOPigeon2; +import frc.robot.subsystems.drive.ModuleIO; +import frc.robot.subsystems.drive.ModuleIOSim; +import frc.robot.subsystems.drive.ModuleIOTalonSpark; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.indexer.IndexerIO; import frc.robot.subsystems.indexer.IndexerIOSparkMax; @@ -52,6 +51,19 @@ import frc.robot.util.CustomTriggers; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import org.wpilib.command2.Command; +import org.wpilib.command2.Commands; +import org.wpilib.command2.button.CommandXboxController; // Removed +import org.wpilib.command2.button.Trigger; +import org.wpilib.command2.sysid.SysIdRoutine; +import org.wpilib.command2.sysid.SysIdRoutine.Direction; +import org.wpilib.driverstation.Alliance; +import org.wpilib.driverstation.DriverStation; +import org.wpilib.driverstation.GenericHID.RumbleType; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.ChassisVelocities; +import org.wpilib.wpilibj.XboxController; // Removed /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -60,6 +72,7 @@ * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { + // Subsystems // private final Subsystem subsystem; private Drive drive; @@ -82,7 +95,9 @@ public class RobotContainer { // Dashboard inputs private final LoggedDashboardChooser autoChooser; - /** The container for the robot. Contains subsystems, OI devices, and commands. */ + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ public RobotContainer() { // Instantiate active subsystems if (Constants.getMode() != Constants.Mode.REPLAY) { @@ -130,7 +145,8 @@ public RobotContainer() { case ROBOT_SIMBOT -> { drive = new Drive( - new GyroIO() {}, + new GyroIO() { + }, new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim(), @@ -152,27 +168,37 @@ public RobotContainer() { if (drive == null) { drive = new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}); + new GyroIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }); } if (intakeRollers == null) { - intakeRollers = new IntakeRollers(new IntakeRollersIO() {}); + intakeRollers = new IntakeRollers(new IntakeRollersIO() { + }); } if (intakeExtend == null) { - intakeExtend = new IntakeExtend(new IntakeExtendIO() {}, () -> false); + intakeExtend = new IntakeExtend(new IntakeExtendIO() { + }, () -> false); } intake = new Intake(intakeRollers, intakeExtend); if (magicCarpet == null) { - magicCarpet = new MagicCarpet(new MagicCarpetIO() {}); + magicCarpet = new MagicCarpet(new MagicCarpetIO() { + }); } if (shooter == null) { - shooter = new Shooter(new ShooterIO() {}); + shooter = new Shooter(new ShooterIO() { + }); } if (indexer == null) { - indexer = new Indexer(new IndexerIO() {}); + indexer = new Indexer(new IndexerIO() { + }); } orchestrator = @@ -182,7 +208,7 @@ public RobotContainer() { AutoBuilder.configure( drive::getPose, drive::setPose, - drive::getChassisSpeeds, + drive::getChassisVelocities, drive::runVelocity, new PPHolonomicDriveController(new PIDConstants(14.0, 0, 0), new PIDConstants(7, 0, 0)), ppConfig, @@ -266,9 +292,9 @@ public RobotContainer() { /** * Use this method to define your button->command mappings. Buttons can be created by - * instantiating a {@link GenericHID} or one of its subclasses ({@link - * org.wpilib.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link - * org.wpilib.command2.button.JoystickButton}. + * instantiating a {@link GenericHID} or one of its subclasses + * ({@link org.wpilib.wpilibj.Joystick} or {@link XboxController}), and then passing it to a + * {@link org.wpilib.command2.button.JoystickButton}. */ private void configureButtonBindings() { indexer.setDefaultCommand(indexer.stop()); @@ -297,7 +323,17 @@ private void configureButtonBindings() { drive) .ignoringDisable(true)); new Trigger(() -> driverController.getHID().getPOV() != -1) - .whileTrue(new DriveWithDpad(drive, () -> driverController.getHID().getPOV())); + .whileTrue(drive.run(coro -> { + int pov = driverController.getHID().getPOV(); + + switch (pov) { + case 0 -> drive.runVelocity(new ChassisVelocities(SLOW_SPEED, 0, 0)); + case 90 -> drive.runVelocity(new ChassisVelocities(0, -SLOW_SPEED, 0)); + case 180 -> drive.runVelocity(new ChassisVelocities(-SLOW_SPEED, 0, 0)); + case 270 -> drive.runVelocity(new ChassisVelocities(0, SLOW_SPEED, 0)); + default -> drive.stop(); + } + }).named("DriveWithDpad")); driverController.x().toggleOnTrue(orchestrator.zoneBasedAim()); driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS)); driverController diff --git a/src/main/java/frc/robot/commands/auto/DriveToPose.java b/src/main/java/frc/robot/commands/auto/DriveToPose.java index 15c376e1c..86819433b 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToPose.java +++ b/src/main/java/frc/robot/commands/auto/DriveToPose.java @@ -1,28 +1,28 @@ package frc.robot.commands.auto; -import org.wpilib.math.controller.ProfiledPIDController; -import org.wpilib.math.geometry.Pose2d; -import org.wpilib.math.geometry.Rotation2d; -import org.wpilib.math.geometry.Translation2d; -import org.wpilib.math.kinematics.ChassisSpeeds; // Removed or renamed -import org.wpilib.math.trajectory.TrapezoidProfile; -import org.wpilib.math.util.Units; -import org.wpilib.driverstation.DriverStation; -import org.wpilib.driverstation.Alliance; -import org.wpilib.command2.Command; import frc.lib.utils.GeomUtils; import frc.lib.utils.TunableNumber; import frc.robot.Constants; import frc.robot.subsystems.drive.Drive; -import java.awt.*; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; +import org.wpilib.command3.Command; +import org.wpilib.driverstation.Alliance; +import org.wpilib.driverstation.DriverStation; +import org.wpilib.math.controller.ProfiledPIDController; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.math.kinematics.ChassisVelocities; // Removed or renamed +import org.wpilib.math.trajectory.TrapezoidProfile; +import org.wpilib.math.util.Units; /** * Command to drive the robot to a specified pose in a straight path. This is useful for very simple * autos or for alignment. */ public class DriveToPose extends Command { + private final Drive drive; private final Supplier poseSupplier; @@ -96,7 +96,7 @@ public class DriveToPose extends Command { * Drives to the specified pose under full software control. * * @param drive The drive subsystem - * @param pose The target pose + * @param pose The target pose */ public DriveToPose(Drive drive, Pose2d pose) { this(drive, () -> pose); @@ -110,26 +110,26 @@ public DriveToPose(Drive drive, double deltaXMeters, double deltaYMeters, double new Translation2d( drive.getPose().getTranslation().getX() + (DriverStation.getAlliance().isEmpty() - || DriverStation.getAlliance().get() == Alliance.Blue - ? deltaXMeters - : -deltaXMeters), + || DriverStation.getAlliance().get() == Alliance.Blue + ? deltaXMeters + : -deltaXMeters), drive.getPose().getTranslation().getY() + (DriverStation.getAlliance().isEmpty() - || DriverStation.getAlliance().get() == Alliance.Blue - ? deltaYMeters - : -deltaYMeters)), + || DriverStation.getAlliance().get() == Alliance.Blue + ? deltaYMeters + : -deltaYMeters)), new Rotation2d( drive.getPose().getRotation().getRadians() + (DriverStation.getAlliance().isEmpty() - || DriverStation.getAlliance().get() == Alliance.Blue - ? deltaThetaRad - : -deltaThetaRad)))); + || DriverStation.getAlliance().get() == Alliance.Blue + ? deltaThetaRad + : -deltaThetaRad)))); } /** * Constructs a new DriveToPose command that drives the robot to a specified pose. * - * @param drive The drive subsystem + * @param drive The drive subsystem * @param poseSupplier A supplier that provides the target pose */ public DriveToPose(Drive drive, Supplier poseSupplier) { @@ -194,7 +194,9 @@ public void execute() { double currentDistance = currentPose.getTranslation().getDistance(targetPose.getTranslation()); driveErrorAbs = currentDistance; double driveVelocityScalar = driveController.calculate(driveErrorAbs, 0.0); - if (driveController.atGoal()) driveVelocityScalar = 0.0; + if (driveController.atGoal()) { + driveVelocityScalar = 0.0; + } // Calculate theta speed double thetaVelocity = @@ -202,17 +204,19 @@ public void execute() { currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians()); thetaErrorAbs = Math.abs(currentPose.getRotation().minus(targetPose.getRotation()).getRadians()); - if (thetaController.atGoal()) thetaVelocity = 0.0; + if (thetaController.atGoal()) { + thetaVelocity = 0.0; + } // Command speeds var driveVelocity = new Pose2d( - new Translation2d(), - currentPose.getTranslation().minus(targetPose.getTranslation()).getAngle()) + new Translation2d(), + currentPose.getTranslation().minus(targetPose.getTranslation()).getAngle()) .transformBy(GeomUtils.transformFromTranslation(driveVelocityScalar, 0.0)) .getTranslation(); drive.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( + ChassisVelocities.fromFieldRelativeSpeeds( driveVelocity.getX(), driveVelocity.getY(), thetaVelocity, currentPose.getRotation())); } @@ -227,19 +231,25 @@ public boolean isFinished() { return atGoal(); } - /** Checks if the robot is stopped at the final pose. */ + /** + * Checks if the robot is stopped at the final pose. + */ public boolean atGoal() { return running && driveController.atGoal() && thetaController.atGoal(); } - /** Checks if the robot pose is within the allowed drive and theta tolerances. */ + /** + * Checks if the robot pose is within the allowed drive and theta tolerances. + */ public boolean withinTolerance(double driveTolerance, Rotation2d thetaTolerance) { return running && Math.abs(driveErrorAbs) < driveTolerance && Math.abs(thetaErrorAbs) < thetaTolerance.getRadians(); } - /** Returns whether the command is actively running. */ + /** + * Returns whether the command is actively running. + */ @AutoLogOutput public boolean isRunning() { return running; diff --git a/src/main/java/frc/robot/commands/auto/RotateToOrientation.java b/src/main/java/frc/robot/commands/auto/RotateToOrientation.java index d5ac51a99..c432e4c27 100644 --- a/src/main/java/frc/robot/commands/auto/RotateToOrientation.java +++ b/src/main/java/frc/robot/commands/auto/RotateToOrientation.java @@ -3,7 +3,7 @@ import org.wpilib.math.controller.ProfiledPIDController; import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Rotation2d; -import org.wpilib.math.kinematics.ChassisSpeeds; // Removed or renamed +import org.wpilib.math.kinematics.ChassisVelocities; // Removed or renamed import org.wpilib.math.trajectory.TrapezoidProfile.Constraints; import org.wpilib.math.util.Units; import org.wpilib.command2.Command; @@ -89,7 +89,7 @@ public void execute() { if (thetaController.atGoal()) thetaVelocity = 0.0; drive.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, thetaVelocity, currentPose.getRotation())); + ChassisVelocities.fromFieldRelativeSpeeds(0, 0, thetaVelocity, currentPose.getRotation())); } @Override diff --git a/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java b/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java index c3bc0e419..0d5bc9e14 100644 --- a/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java +++ b/src/main/java/frc/robot/commands/auto/StraightDriveToPose.java @@ -4,7 +4,7 @@ import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; -import org.wpilib.math.kinematics.ChassisSpeeds; // Removed or renamed +import org.wpilib.math.kinematics.ChassisVelocities; // Removed or renamed import org.wpilib.math.trajectory.TrapezoidProfile.Constraints; import org.wpilib.math.util.Units; import org.wpilib.driverstation.DriverStation; @@ -160,7 +160,7 @@ public void execute() { .transformBy(GeomUtils.transformFromTranslation(driveVelocityScalar, 0.0)) .getTranslation(); drive.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( + ChassisVelocities.fromFieldRelativeSpeeds( driveVelocity.getX(), driveVelocity.getY(), thetaVelocity, currentPose.getRotation())); } diff --git a/src/main/java/frc/robot/commands/drive/DriveWithDpad.java b/src/main/java/frc/robot/commands/drive/DriveWithDpad.java deleted file mode 100644 index faf5201a0..000000000 --- a/src/main/java/frc/robot/commands/drive/DriveWithDpad.java +++ /dev/null @@ -1,32 +0,0 @@ -package frc.robot.commands.drive; - -import org.wpilib.math.kinematics.ChassisSpeeds; // Removed or renamed -import org.wpilib.math.util.Units; -import org.wpilib.command2.Command; -import frc.robot.subsystems.drive.Drive; -import java.util.function.Supplier; - -public class DriveWithDpad extends Command { - private final Drive drive; - private final Supplier povSupplier; - public static final double SLOW_SPEED = Units.inchesToMeters(20); - - public DriveWithDpad(Drive drive, Supplier povSupplier) { - this.drive = drive; - this.povSupplier = povSupplier; - addRequirements(drive); - } - - @Override - public void execute() { - int pov = povSupplier.get(); - - switch (pov) { - case 0 -> drive.runVelocity(new ChassisSpeeds(SLOW_SPEED, 0, 0)); - case 90 -> drive.runVelocity(new ChassisSpeeds(0, -SLOW_SPEED, 0)); - case 180 -> drive.runVelocity(new ChassisSpeeds(-SLOW_SPEED, 0, 0)); - case 270 -> drive.runVelocity(new ChassisSpeeds(0, SLOW_SPEED, 0)); - default -> drive.stop(); - } - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index e5ff3ccea..c89cb341d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -15,7 +15,7 @@ import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Rotation3d; import org.wpilib.math.geometry.Twist2d; -import org.wpilib.math.kinematics.ChassisSpeeds; // Removed or renamed (maybe Velocities) +import org.wpilib.math.kinematics.ChassisVelocities; // Removed or renamed (maybe Velocities) import org.wpilib.math.kinematics.SwerveDriveKinematics; import org.wpilib.math.kinematics.SwerveModulePosition; import org.wpilib.math.kinematics.SwerveModuleState; // Removed or renamed @@ -162,13 +162,13 @@ public void periodic() { */ public void runVelocity(ChassisVelocities speeds) { // Calculate module setpoints - ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); + ChassisVelocities discreteSpeeds = ChassisVelocities.discretize(speeds, 0.02); SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, maxSpeedMetersPerSec); // Log unoptimized setpoints Logger.recordOutput("SwerveStates/Setpoints", setpointStates); - Logger.recordOutput("SwerveChassisSpeeds/Setpoints", discreteSpeeds); + Logger.recordOutput("SwerveChassisVelocities/Setpoints", discreteSpeeds); // Send setpoints to modules for (int i = 0; i < 4; i++) { @@ -201,7 +201,7 @@ public Command getAutonomousCommand(String Path) { /** Stops the drive. */ public void stop() { - runVelocity(new ChassisSpeeds()); + runVelocity(new ChassisVelocities()); } /** @@ -249,9 +249,9 @@ private SwerveModulePosition[] getModulePositions() { } /** Returns the measured chassis speeds of the robot. */ - @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") - public ChassisSpeeds getChassisSpeeds() { - return kinematics.toChassisSpeeds(getModuleStates()); + @AutoLogOutput(key = "SwerveChassisVelocities/Measured") + public ChassisVelocities getChassisVelocities() { + return kinematics.toChassisVelocities(getModuleStates()); } /** Returns the position of each module in radians. */ diff --git a/src/main/java/frc/robot/util/ShooterLeadCompensator.java b/src/main/java/frc/robot/util/ShooterLeadCompensator.java index cbdf2f7d2..66ce5d290 100644 --- a/src/main/java/frc/robot/util/ShooterLeadCompensator.java +++ b/src/main/java/frc/robot/util/ShooterLeadCompensator.java @@ -32,8 +32,8 @@ public ShooterLeadCompensator(Drive drive, Shooter shooter) { this.robotVelocity = () -> new Translation2d( - drive.getChassisSpeeds().vxMetersPerSecond, - drive.getChassisSpeeds().vyMetersPerSecond); + drive.getChassisVelocities().vxMetersPerSecond, + drive.getChassisVelocities().vyMetersPerSecond); this.shooter = shooter; } From 1828dcc368f698d29cbe29c68d277eb6026e9304 Mon Sep 17 00:00:00 2001 From: EJainDev Date: Mon, 21 Sep 2026 06:11:56 -0700 Subject: [PATCH 11/15] fix: replace SubsystemBase with Mechanism and more command3 --- .../java/frc/robot/subsystems/indexer/Indexer.java | 7 +++---- src/main/java/frc/robot/subsystems/intake/Intake.java | 3 +-- .../robot/subsystems/intake/extend/IntakeExtend.java | 6 ++---- .../robot/subsystems/intake/rollers/IntakeRollers.java | 7 +++---- src/main/java/frc/robot/subsystems/leds/Leds.java | 4 ++-- .../frc/robot/subsystems/magicCarpet/MagicCarpet.java | 7 +++---- .../java/frc/robot/subsystems/shooter/Shooter.java | 10 +++------- src/main/java/frc/robot/subsystems/vision/Vision.java | 4 ++-- 8 files changed, 19 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index cd81577e4..521797c4c 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -2,13 +2,12 @@ import static frc.robot.subsystems.indexer.IndexerConstants.FEEDUP_VOLT; -import org.wpilib.command2.Command; -import org.wpilib.command2.Commands; -import org.wpilib.command2.SubsystemBase; +import org.wpilib.command3.Command; +import org.wpilib.command3.Mechanism; import frc.robot.RobotState; import org.littletonrobotics.junction.Logger; -public class Indexer extends SubsystemBase { +public class Indexer implements Mechanism { private final IndexerIO io; private final IndexerIOInputsAutoLogged inputs = new IndexerIOInputsAutoLogged(); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 553b7e7e4..80968f642 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -2,8 +2,7 @@ import static frc.robot.subsystems.intake.IntakeConstants.*; -import org.wpilib.command2.Command; -import org.wpilib.command2.Commands; +import org.wpilib.command3.Command; import frc.robot.subsystems.intake.extend.IntakeExtend; import frc.robot.subsystems.intake.rollers.IntakeRollers; diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java index e04cf3c13..9516e27db 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java @@ -8,16 +8,14 @@ import org.wpilib.system.Timer; import org.wpilib.command2.Command; -import org.wpilib.command2.Commands; -import org.wpilib.command2.ConditionalCommand; -import org.wpilib.command2.SubsystemBase; +import org.wpilib.command3.Mechanism; import frc.robot.RobotState; import frc.robot.RobotState.IntakePosition; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; -public class IntakeExtend extends SubsystemBase { +public class IntakeExtend implements Mechanism { private final IntakeExtendIO io; private final IntakeExtendIOInputsAutoLogged inputs = new IntakeExtendIOInputsAutoLogged(); private final Timer stallExtendTimer = new Timer(); diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java index 7b717cad6..073ad9d3b 100644 --- a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java +++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java @@ -3,13 +3,12 @@ import static frc.robot.subsystems.intake.IntakeConstants.INTAKE_VOLTS; import static frc.robot.subsystems.intake.IntakeConstants.OUTTAKE_VOLTS; -import org.wpilib.command2.Command; -import org.wpilib.command2.Commands; -import org.wpilib.command2.SubsystemBase; +import org.wpilib.command3.Command; import frc.robot.RobotState; import org.littletonrobotics.junction.Logger; +import org.wpilib.command3.Mechanism; -public class IntakeRollers extends SubsystemBase { +public class IntakeRollers implements Mechanism { private final IntakeRollersIO io; private final IntakeRollersIOInputsAutoLogged inputs = new IntakeRollersIOInputsAutoLogged(); diff --git a/src/main/java/frc/robot/subsystems/leds/Leds.java b/src/main/java/frc/robot/subsystems/leds/Leds.java index a52a3b6b7..fe82de816 100644 --- a/src/main/java/frc/robot/subsystems/leds/Leds.java +++ b/src/main/java/frc/robot/subsystems/leds/Leds.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.leds; +import org.wpilib.command3.Mechanism; import org.wpilib.networktables.GenericEntry; import org.wpilib.hardware.led.AddressableLED; import org.wpilib.hardware.led.AddressableLED.ColorOrder; @@ -16,12 +17,11 @@ // (End): Removed or replaced import org.wpilib.smartdashboard.SendableChooser; -import org.wpilib.command2.SubsystemBase; import frc.robot.Robot; import frc.robot.RobotState; import frc.robot.RobotState.Mode; -public class Leds extends SubsystemBase { +public class Leds implements Mechanism { private static final boolean debug = false; private RobotState state = RobotState.getInstance(); private final AddressableLED leds; diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java index 14494a398..8802c0bec 100644 --- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java +++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java @@ -1,12 +1,11 @@ package frc.robot.subsystems.magicCarpet; -import org.wpilib.command2.Command; -import org.wpilib.command2.Commands; -import org.wpilib.command2.SubsystemBase; +import org.wpilib.command3.Command; import frc.robot.RobotState; import org.littletonrobotics.junction.Logger; +import org.wpilib.command3.Mechanism; -public class MagicCarpet extends SubsystemBase { +public class MagicCarpet implements Mechanism { private final MagicCarpetIO io; private final MagicCarpetIOInputsAutoLogged inputs = new MagicCarpetIOInputsAutoLogged(); public boolean manualRun; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index d65da63b1..864adae0f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -17,17 +17,13 @@ import org.wpilib.units.measure.Distance; import org.wpilib.units.measure.Time; import org.wpilib.smartdashboard.SmartDashboard; -import org.wpilib.command2.Command; -import org.wpilib.command2.Commands; -import org.wpilib.command2.SubsystemBase; -import org.wpilib.command2.sysid.SysIdRoutine; -import org.wpilib.command2.sysid.SysIdRoutine.Config; -import org.wpilib.command2.sysid.SysIdRoutine.Mechanism; +import org.wpilib.command3.Command; +import org.wpilib.command3.Mechanism; import frc.robot.RobotState; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; -public class Shooter extends SubsystemBase { +public class Shooter implements Mechanism { private final ShooterIO io; private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); private final SysIdRoutine sysId; diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 085d2bc46..6537e8f4e 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -2,6 +2,7 @@ import static frc.robot.subsystems.vision.VisionConstants.*; +import org.wpilib.command3.Mechanism; import org.wpilib.util.Alert.Level; import org.wpilib.math.linalg.Matrix; import org.wpilib.math.linalg.VecBuilder; @@ -11,13 +12,12 @@ import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N3; import org.wpilib.util.Alert; -import org.wpilib.command2.SubsystemBase; import frc.robot.RobotState; import java.util.LinkedList; import java.util.List; import org.littletonrobotics.junction.Logger; -public class Vision extends SubsystemBase { +public class Vision implements Mechanism { private final VisionConsumer consumer; private final VisionIO[] io; private final VisionIOInputsAutoLogged[] inputs; From f632a727f9d0042a576e2aef7f7451785dd916e4 Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Mon, 5 Oct 2026 20:33:17 -0400 Subject: [PATCH 12/15] drive rewrite to 2027 --- .../frc/robot/subsystems/drive/Drive.java | 147 +-- .../subsystems/drive/DriveConstants.java | 5 + .../frc/robot/subsystems/drive/GyroIO.java | 2 +- .../robot/subsystems/drive/GyroIOPigeon2.java | 5 +- .../frc/robot/subsystems/drive/Module.java | 26 +- .../frc/robot/subsystems/drive/ModuleIO.java | 2 +- .../robot/subsystems/drive/ModuleIOSim.java | 32 +- .../robot/subsystems/drive/ModuleIOSpark.java | 548 +++++----- .../subsystems/drive/ModuleIOTalonSpark.java | 988 ++++++++++++++---- .../subsystems/drive/OdometryThread.java | 2 +- 10 files changed, 1198 insertions(+), 559 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index c89cb341d..784599747 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -1,12 +1,17 @@ package frc.robot.subsystems.drive; +import static org.wpilib.framework.RobotBase.isDisabled; import static org.wpilib.units.Units.*; import static frc.robot.subsystems.drive.DriveConstants.*; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathPlannerPath; +import frc.robot.RobotState; +import org.wpilib.driverstation.MatchState; import org.wpilib.math.kinematics.ChassisVelocities; -import org.wpilib.util.Alert.Level; +import org.wpilib.math.kinematics.SwerveModuleVelocity; +import org.wpilib.wpiutil.Alert; // 2027 migration: Alert moved from HAL/wpilib into wpiutil +import org.wpilib.wpiutil.Alert.Level; import org.wpilib.math.linalg.Matrix; import org.wpilib.math.controller.PIDController; import org.wpilib.math.estimator.SwerveDrivePoseEstimator; @@ -15,16 +20,19 @@ import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Rotation3d; import org.wpilib.math.geometry.Twist2d; -import org.wpilib.math.kinematics.ChassisVelocities; // Removed or renamed (maybe Velocities) import org.wpilib.math.kinematics.SwerveDriveKinematics; import org.wpilib.math.kinematics.SwerveModulePosition; -import org.wpilib.math.kinematics.SwerveModuleState; // Removed or renamed +import org.wpilib.math.kinematics.SwerveModuleState; import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N3; -import org.wpilib.util.Alert; import org.wpilib.driverstation.DriverStation; import org.wpilib.command3.Command; import org.wpilib.command3.Mechanism; +// 2027 migration: SysIdRoutine below is still the Commands v2 class (no v3 equivalent +// shipped yet as of alpha-7). Its Mechanism.Mechanism(...) constructor's third argument +// expects a v2 Subsystem, but this class now implements the v3 Mechanism interface +// (see class declaration) instead. Passing `this` there is a real type mismatch that +// needs a team decision, not a mechanical fix — see summary below. import org.wpilib.command2.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.Mode; @@ -38,24 +46,17 @@ public class Drive implements Mechanism { static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); - private final Alert impactAlert = - new Alert("Impact Detected, lowering elevator to prevent flipping.", Level.MEDIUM); - private final Alert tiltAlert = - new Alert( - ("Tilt Threshold reached, lowering elevator to prevent flipping"), Level.MEDIUM); private final Module[] modules = new Module[4]; // FL, FR, BL, BR - private final SysIdRoutine sysId; - private final Alert gyroDisconnectedAlert = - new Alert("Disconnected gyro, using kinematics as fallback.", Level.HIGH); + // private final SysIdRoutine sysId; private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(moduleTranslations); - private Rotation2d rawGyroRotation = Rotation2d.kZero; + private Rotation2d rawGyroRotation = Rotation2d.ZERO; private SwerveModulePosition[] lastModulePositions = // For delta tracking new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() }; private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); @@ -83,20 +84,23 @@ public Drive( // Configure AutoBuilder for PathPlanner // Configure SysId - sysId = - new SysIdRoutine( - new SysIdRoutine.Config( - null, - null, - null, - (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), - new SysIdRoutine.Mechanism( - (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); +// sysId = +// new SysIdRoutine( +// new SysIdRoutine.Config( +// null, +// null, +// null, +// (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), +// // 2027 migration: `this` no longer satisfies this constructor's expected type +// // (v2 Subsystem) now that Drive implements the v3 Mechanism interface. Needs a +// // team decision — see summary. +// new SysIdRoutine.Mechanism( +// (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); headingController.enableContinuousInput(-Math.PI, Math.PI); } - @Override + public void periodic() { logCameraPositions(); // uncomment to show camera positions in advantage scope odometryLock.lock(); // Prevents odometry updates while reading data @@ -107,16 +111,19 @@ public void periodic() { } odometryLock.unlock(); // Stop moving when disabled - if (DriverStation.isDisabled()) { + // 2027 migration: DriverStation was split into MatchState/RobotState (alpha 5). + // isDisabled() reads robot state, so this likely needs to move to RobotState.isDisabled() + // — verify the exact package/class against your target alpha before changing this. + if (isDisabled()) { for (var module : modules) { module.stop(); } } // Log empty setpoint states when disabled - if (DriverStation.isDisabled()) { - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + if (isDisabled()) { + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleVelocity[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleVelocity[] {}); } // Update odometry @@ -131,8 +138,8 @@ public void periodic() { modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; moduleDeltas[moduleIndex] = new SwerveModulePosition( - modulePositions[moduleIndex].distanceMeters - - lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].distance + - lastModulePositions[moduleIndex].distance, modulePositions[moduleIndex].angle); lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; } @@ -152,7 +159,7 @@ public void periodic() { } // Update gyro alert - gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.getMode() != Mode.SIM); + //gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.getMode() != Mode.SIM); } /** @@ -162,9 +169,9 @@ public void periodic() { */ public void runVelocity(ChassisVelocities speeds) { // Calculate module setpoints - ChassisVelocities discreteSpeeds = ChassisVelocities.discretize(speeds, 0.02); - SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, maxSpeedMetersPerSec); + ChassisVelocities discreteSpeeds = speeds.discretize(0.02); + SwerveModuleVelocity[] setpointStates = kinematics.toSwerveModuleVelocities(discreteSpeeds); + var desaturatedStates = SwerveDriveKinematics.desaturateWheelVelocities(setpointStates, maxSpeedMetersPerSec); // Log unoptimized setpoints Logger.recordOutput("SwerveStates/Setpoints", setpointStates); @@ -172,7 +179,7 @@ public void runVelocity(ChassisVelocities speeds) { // Send setpoints to modules for (int i = 0; i < 4; i++) { - modules[i].runSetpoint(setpointStates[i]); + modules[i].runSetpoint(desaturatedStates[i]); } // Log optimized setpoints (runSetpoint mutates each state) @@ -186,18 +193,26 @@ public void runCharacterization(double output) { } } - public Command getAutonomousCommand(String Path) { - try { - // Load the path you want to follow using its name in the GUI - PathPlannerPath path = PathPlannerPath.fromPathFile(Path); - - // Create a path following command using AutoBuilder. This will also trigger event markers. - return AutoBuilder.followPath(path); - } catch (Exception e) { - DriverStation.reportError("Big oops: " + e.getMessage(), e.getStackTrace()); - return Commands.none(); - } - } +// public Command getAutonomousCommand(String Path) { +// try { +// // Load the path you want to follow using its name in the GUI +// PathPlannerPath path = PathPlannerPath.fromPathFile(Path); +// +// // Create a path following command using AutoBuilder. This will also trigger event markers. +// // 2027 migration: PathPlanner is a vendor library — it needs its own 2027-compatible +// // vendordep release. Confirm the version you're on returns a command type compatible +// // with org.wpilib.command3.Command (this method's return type); pre-2027 PathPlanner +// // versions return a Commands v2 command, which will not satisfy this signature. +// return AutoBuilder.followPath(path); +// } catch (Exception e) { +// DriverStation.reportError("Big oops: " + e.getMessage(), e.getStackTrace()); +// // 2027 migration: Commands.none() was already unresolved in the original file (no +// // Commands import) — pre-existing bug, not introduced by this migration. If you want a +// // no-op Command here, confirm what Commands v3's equivalent factory method is called +// // and import it explicitly; v3's API doesn't mirror v2's Commands class 1:1. +// return Commands.none(); +// } +// } /** Stops the drive. */ public void stop() { @@ -211,28 +226,28 @@ public void stop() { public void stopWithX() { Rotation2d[] headings = new Rotation2d[4]; for (int i = 0; i < 4; i++) { - headings[i] = moduleTranslations[i].getAngle(); + headings[i] = moduleTranslations[i].getAngle().get(); } kinematics.resetHeadings(headings); stop(); } - /** Returns a command to run a quasistatic test in the specified direction. */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return run(() -> runCharacterization(0.0)) - .withTimeout(1.0) - .andThen(sysId.quasistatic(direction)); - } - - /** Returns a command to run a dynamic test in the specified direction. */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction)); - } +// /** Returns a command to run a quasistatic test in the specified direction. */ +// public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { +// return run(() -> runCharacterization(0.0)) +// .withTimeout(1.0) +// .andThen(sysId.quasistatic(direction)); +// } +// +// /** Returns a command to run a dynamic test in the specified direction. */ +// public Command sysIdDynamic(SysIdRoutine.Direction direction) { +// return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction)); +// } /** Returns the module states (turn angles and drive velocities) for all of the modules. */ - @AutoLogOutput(key = "SwerveStates/Measured") - private SwerveModuleState[] getModuleStates() { - SwerveModuleState[] states = new SwerveModuleState[4]; + @AutoLogOutput(key = "SwerveVelocities/Measured") + private SwerveModuleVelocity[] getModuleStates() { + SwerveModuleVelocity[] states = new SwerveModuleVelocity[4]; for (int i = 0; i < 4; i++) { states[i] = modules[i].getState(); } @@ -321,4 +336,4 @@ public double getMaxLinearSpeedMetersPerSec() { public double getMaxAngularSpeedRadPerSec() { return maxSpeedMetersPerSec / driveBaseRadius; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index bdd04b1fd..aa69ba0a6 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -3,10 +3,12 @@ import static frc.robot.Schematic.*; import static java.lang.Math.PI; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.RobotConfig; +import org.wpilib.hardware.bus.CANPort; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.system.DCMotor; @@ -120,6 +122,9 @@ public class DriveConstants { driveMotorCurrentLimit, 1), moduleTranslations); + + //Pidgeon + public static final CANBus kCANBus = new CANBus("can_s17"); // Requirements for RobotTilt to trigger public static final double rollThreshhold = 10.0; public static final double pitchThreshold = 10.0; diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 64e8d6608..c73c9b0b8 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -7,7 +7,7 @@ public interface GyroIO { @AutoLog public static class GyroIOInputs { public boolean connected = false; - public Rotation2d yawPosition = Rotation2d.kZero; + public Rotation2d yawPosition = Rotation2d.ZERO; public double yawVelocityRadPerSec = 0.0; public double[] odometryYawTimestamps = new double[] {}; public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index b9fb49cb9..5cc0d1dc0 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -4,6 +4,7 @@ import static frc.robot.subsystems.drive.DriveConstants.*; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Pigeon2Configuration; @@ -20,8 +21,8 @@ /** IO implementation for Pigeon 2. */ public class GyroIOPigeon2 implements GyroIO { private final Alert impactAlert = - new Alert("Impact Detected, lowering elevator to prevent flipping.", Level.MEDIUM); - private final Pigeon2 pigeon = new Pigeon2(pigeonCanId); + new Alert("Gyro","Impact Detected, lowering elevator to prevent flipping.", Level.MEDIUM); + private final Pigeon2 pigeon = new Pigeon2(pigeonCanId,kCANBus); private final StatusSignal yaw = pigeon.getYaw(); private final Queue yawPositionQueue; private final Queue yawTimestampQueue; diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index a31b998a5..4bce53cef 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -2,6 +2,7 @@ import static frc.robot.subsystems.drive.DriveConstants.*; +import org.wpilib.math.kinematics.SwerveModuleVelocity; import org.wpilib.util.Alert.Level; import org.wpilib.math.util.MathUtil; import org.wpilib.math.geometry.Rotation2d; @@ -23,11 +24,11 @@ public Module(ModuleIO io, int index) { this.io = io; this.index = index; driveDisconnectedAlert = - new Alert( + new Alert("driveDisconnected", "Disconnected drive motor on module " + Integer.toString(index) + ".", Level.HIGH); turnDisconnectedAlert = - new Alert( + new Alert("driveDisconnected", "Disconnected turn motor on module " + Integer.toString(index) + ".", Level.HIGH); } @@ -50,21 +51,21 @@ public void periodic() { } /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */ - public void runSetpoint(SwerveModuleState state) { + public void runSetpoint(SwerveModuleVelocity state) { // Optimize velocity setpoint optimize(state, getAngle()); state.cosineScale(state.angle); // Apply setpoints - io.setDriveVelocity(state.speedMetersPerSecond / wheelRadiusMeters); + io.setDriveVelocity(state.velocity / wheelRadiusMeters); io.setTurnPosition(state.angle); } /** Runs the module with the specified output while controlling to zero degrees. */ public void runCharacterization(double output) { io.setDriveOpenLoop(output); - io.setTurnPosition(Rotation2d.kZero); + io.setTurnPosition(Rotation2d.ZERO); } /** Disables all outputs to motors. */ @@ -93,9 +94,11 @@ public SwerveModulePosition getPosition() { return new SwerveModulePosition(getPositionMeters(), getAngle()); } - /** Returns the module state (turn angle and drive velocity). */ - public SwerveModuleState getState() { - return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); + /** + * Returns the module state (turn angle and drive velocity). + */ + public SwerveModuleVelocity getState() { + return new SwerveModuleVelocity(getVelocityMetersPerSec(), getAngle()); } /** Returns the module positions received this cycle. */ @@ -118,11 +121,12 @@ public double getFFCharacterizationVelocity() { return inputs.driveVelocityRadPerSec; } - private static void optimize(SwerveModuleState state, Rotation2d currentAngle) { + private static void optimize( + SwerveModuleVelocity state, Rotation2d currentAngle) { var delta = state.angle.minus(currentAngle); if (Math.abs(MathUtil.inputModulus(delta.getDegrees(), -180, 180)) > 90) { - state.speedMetersPerSecond *= -1; - state.angle = state.angle.rotateBy(Rotation2d.kPi); + state.velocity *= -1; + state.angle = state.angle.rotateBy(Rotation2d.PI); } } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index ef0bd6ae1..a35da66da 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -13,7 +13,7 @@ public static class ModuleIOInputs { public double driveCurrentAmps = 0.0; public boolean turnConnected = false; - public Rotation2d turnPosition = Rotation2d.kZero; + public Rotation2d turnPosition = Rotation2d.ZERO; public double turnVelocityRadPerSec = 0.0; public double turnAppliedVolts = 0.0; public double turnCurrentAmps = 0.0; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index fefb66dcb..32e2785f4 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -2,10 +2,12 @@ import static frc.robot.subsystems.drive.DriveConstants.*; +import org.wpilib.math.system.LinearSystemLoop; +import org.wpilib.math.system.Models; import org.wpilib.math.util.MathUtil; import org.wpilib.math.controller.PIDController; -import org.wpilib.math.geometry.Rotation2d; -import org.wpilib.math.system.LinearSystemId; // Removed or renamed (likely LinearSystem now) +import org.wpilib.math.geometry.Rotation2d; // Removed or renamed (likely LinearSystem now) +import org.wpilib.simulation.LinearSystemSim; import org.wpilib.system.Timer; import org.wpilib.simulation.DCMotorSim; @@ -26,11 +28,11 @@ public ModuleIOSim() { // Create drive and turn sim models driveSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(driveGearbox, 0.025, driveMotorReduction), + Models.singleJointedArmFromPhysicalConstants(driveGearbox, 0.025, driveMotorReduction), driveGearbox); turnSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(turnGearbox, 0.004, turnMotorReduction), + Models.singleJointedArmFromPhysicalConstants(turnGearbox, 0.004, turnMotorReduction), turnGearbox); // Enable wrapping for turn PID @@ -42,38 +44,38 @@ public void updateInputs(ModuleIOInputs inputs) { // Run closed-loop control if (driveClosedLoop) { driveAppliedVolts = - driveFFVolts + driveController.calculate(driveSim.getAngularVelocityRadPerSec()); + driveFFVolts + driveController.calculate(driveSim.getAngularVelocity()); } else { driveController.reset(); } if (turnClosedLoop) { - turnAppliedVolts = turnController.calculate(turnSim.getAngularPositionRad()); + turnAppliedVolts = turnController.calculate(turnSim.getAngularPosition()); } else { turnController.reset(); } // Update simulation state - driveSim.setInputVoltage(MathUtil.clamp(driveAppliedVolts, -12.0, 12.0)); - turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0)); + driveSim.setInputVoltage(MathUtil.inputModulus(driveAppliedVolts, -12.0, 12.0)); + turnSim.setInputVoltage(MathUtil.inputModulus(turnAppliedVolts, -12.0, 12.0)); driveSim.update(0.02); turnSim.update(0.02); // Update drive inputs inputs.driveConnected = true; - inputs.drivePositionRad = driveSim.getAngularPositionRad(); - inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); + inputs.drivePositionRad = driveSim.getAngularPosition(); //.getAngularPositionRad(); + inputs.driveVelocityRadPerSec = driveSim.getAngularVelocity(); //getAngularVelocityRadPerSec(); inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); + inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDraw());//getCurrentDrawAmps()); // Update turn inputs inputs.turnConnected = true; - inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); - inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); + inputs.turnPosition = new Rotation2d(turnSim.getAngularPosition()); //getAngularPositionRad()); + inputs.turnVelocityRadPerSec = turnSim.getAngularVelocity(); inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); + inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDraw()); // Update odometry inputs (50Hz because high-frequency odometry in sim doesn't matter) - inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; + inputs.odometryTimestamps = new double[] {Timer.getTimestamp()}; inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 1fb261dd0..42b59130d 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -1,272 +1,276 @@ -package frc.robot.subsystems.drive; - -import static frc.lib.utils.SparkUtil.*; -import static frc.robot.Schematic.*; -import static frc.robot.subsystems.drive.DriveConstants.*; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.*; -import com.revrobotics.spark.FeedbackSensor; -import com.revrobotics.spark.SparkLowLevel.ControlType; -import com.revrobotics.PersistMode; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkMaxConfig; -import org.wpilib.math.util.MathUtil; -import org.wpilib.math.filter.Debouncer; -import org.wpilib.math.geometry.Rotation2d; -import org.wpilib.math.util.Units; -import org.wpilib.units.measure.Angle; -import java.util.Queue; -import java.util.function.DoubleSupplier; - -/** - * Module IO implementation for Spark Flex drive motor controller, Spark Max turn motor controller, - * and duty cycle absolute encoder. - */ -public class ModuleIOSpark implements ModuleIO { - private final Rotation2d zeroRotation; - - // Hardware objects - private final SparkBase driveSpark; - private final SparkBase turnSpark; - private final RelativeEncoder driveEncoder; - private final RelativeEncoder turnEncoder; - private final CANcoder turnEncoderAbsolute; - - // Input from turn encoder absolute - private final StatusSignal turnPositionAbsolute; - - // Closed loop controllers - private final SparkClosedLoopController driveController; - private final SparkClosedLoopController turnController; - - // Queue inputs from odometry thread - private final Queue timestampQueue; - private final Queue drivePositionQueue; - private final Queue turnPositionQueue; - - // Connection debouncers - private final Debouncer driveConnectedDebounce = new Debouncer(0.5); - private final Debouncer turnConnectedDebounce = new Debouncer(0.5); - - public ModuleIOSpark(int module) { - zeroRotation = - switch (module) { - case 0 -> frontLeftZeroRotation; - case 1 -> frontRightZeroRotation; - case 2 -> backLeftZeroRotation; - case 3 -> backRightZeroRotation; - default -> Rotation2d.kZero; - }; - driveSpark = - new SparkMax( - switch (module) { - case 0 -> frontLeftDriveCanId; - case 1 -> frontRightDriveCanId; - case 2 -> backLeftDriveCanId; - case 3 -> backRightDriveCanId; - default -> 0; - }, - MotorType.kBrushless); - turnSpark = - new SparkMax( - switch (module) { - case 0 -> frontLeftTurnCanId; - case 1 -> frontRightTurnCanId; - case 2 -> backLeftTurnCanId; - case 3 -> backRightTurnCanId; - default -> 0; - }, - MotorType.kBrushless); - driveEncoder = driveSpark.getEncoder(); - turnEncoder = turnSpark.getEncoder(); - turnEncoderAbsolute = - new CANcoder( - switch (module) { - case 0 -> frontLeftAbsoluteEncoderCanId; - case 1 -> frontRightAbsoluteEncoderCanId; - case 2 -> backLeftAbsoluteEncoderCanId; - case 3 -> backRightAbsoluteEncoderCanId; - default -> 0; - }); - driveController = driveSpark.getClosedLoopController(); - turnController = turnSpark.getClosedLoopController(); - - // Configure drive motor - var driveConfig = new SparkMaxConfig(); - driveConfig - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(driveMotorCurrentLimit) - .voltageCompensation(12.0); - driveConfig - .encoder - .positionConversionFactor(driveEncoderPositionFactor) - .velocityConversionFactor(driveEncoderVelocityFactor) - .uvwMeasurementPeriod(10) - .uvwAverageDepth(2); - driveConfig - .closedLoop - .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pidf( - driveKp, 0.0, - driveKd, 0.0); - driveConfig - .signals - .primaryEncoderPositionAlwaysOn(true) - .primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) - .primaryEncoderVelocityAlwaysOn(true) - .primaryEncoderVelocityPeriodMs(20) - .appliedOutputPeriodMs(20) - .busVoltagePeriodMs(20) - .outputCurrentPeriodMs(20); - tryUntilOk( - driveSpark, - 5, - () -> - driveSpark.configure( - driveConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - tryUntilOk(driveSpark, 5, () -> driveEncoder.setPosition(0.0)); - - // Configure turn motor - var turnConfig = new SparkMaxConfig(); - turnConfig - .inverted(turnInverted) - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(turnMotorCurrentLimit) - .voltageCompensation(12.0); - turnConfig - .encoder - .positionConversionFactor(turnEncoderPositionFactor) - .velocityConversionFactor(turnEncoderVelocityFactor) - .uvwAverageDepth(2); - turnConfig - .closedLoop - .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .positionWrappingEnabled(true) - .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) - .pidf(turnKp, 0.0, turnKd, 0.0); - turnConfig - .signals - .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) - .absoluteEncoderVelocityAlwaysOn(true) - .absoluteEncoderVelocityPeriodMs(20) - .appliedOutputPeriodMs(20) - .busVoltagePeriodMs(20) - .outputCurrentPeriodMs(20); - tryUntilOk( - turnSpark, - 5, - () -> - turnSpark.configure( - turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - - // Configure CANCoder - CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); - cancoderConfig.MagnetSensor.MagnetOffset = 0; // This is done later in the code - cancoderConfig.MagnetSensor.SensorDirection = - turnEncoderInverted - ? SensorDirectionValue.Clockwise_Positive - : SensorDirectionValue.CounterClockwise_Positive; - turnEncoderAbsolute.getConfigurator().apply(cancoderConfig); - turnPositionAbsolute = turnEncoderAbsolute.getPosition(); - - BaseStatusSignal.setUpdateFrequencyForAll(50.0, turnPositionAbsolute); - - // Create odometry queues - timestampQueue = OdometryThread.getInstance().makeTimestampQueue(); - drivePositionQueue = - OdometryThread.getInstance().registerSignal(driveSpark, driveEncoder::getPosition); - turnPositionQueue = - OdometryThread.getInstance().registerSignal(turnSpark, turnEncoder::getPosition); - - // Initialize turn relative encoder - tryUntilOk( - turnSpark, - 5, - () -> - turnEncoder.setPosition( - Units.rotationsToRadians( - turnEncoderAbsolute.getAbsolutePosition().getValueAsDouble()))); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - BaseStatusSignal.refreshAll(turnPositionAbsolute); - inputs.turnPositionAbsolute = Units.rotationsToRadians(turnPositionAbsolute.getValueAsDouble()); - // Update drive inputs - sparkStickyFault = false; - ifOk(driveSpark, driveEncoder::getPosition, (value) -> inputs.drivePositionRad = value); - ifOk(driveSpark, driveEncoder::getVelocity, (value) -> inputs.driveVelocityRadPerSec = value); - ifOk( - driveSpark, - new DoubleSupplier[] {driveSpark::getAppliedOutput, driveSpark::getBusVoltage}, - (values) -> inputs.driveAppliedVolts = values[0] * values[1]); - ifOk(driveSpark, driveSpark::getOutputCurrent, (value) -> inputs.driveCurrentAmps = value); - inputs.driveConnected = driveConnectedDebounce.calculate(!sparkStickyFault); - - // Update turn inputs - sparkStickyFault = false; - ifOk( - turnSpark, - turnEncoder::getPosition, - (value) -> inputs.turnPosition = new Rotation2d(value).minus(zeroRotation)); - ifOk(turnSpark, turnEncoder::getVelocity, (value) -> inputs.turnVelocityRadPerSec = value); - ifOk( - turnSpark, - new DoubleSupplier[] {turnSpark::getAppliedOutput, turnSpark::getBusVoltage}, - (values) -> inputs.turnAppliedVolts = values[0] * values[1]); - ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.turnCurrentAmps = value); - inputs.turnConnected = turnConnectedDebounce.calculate(!sparkStickyFault); - - // Update odometry inputs - inputs.odometryTimestamps = - timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((Double value) -> new Rotation2d(value).minus(zeroRotation)) - .toArray(Rotation2d[]::new); - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void setDriveOpenLoop(double output) { - driveSpark.setVoltage(output); - } - - @Override - public void setTurnOpenLoop(double output) { - turnSpark.setVoltage(output); - } - - @Override - public void setDriveVelocity(double velocityRadPerSec) { - double ffVolts = driveKs * Math.signum(velocityRadPerSec) + driveKv * velocityRadPerSec; - driveController.setReference( - velocityRadPerSec, - ControlType.kVelocity, - ClosedLoopSlot.kSlot0, - ffVolts, - ArbFFUnits.kVoltage); - } - - @Override - public void setTurnPosition(Rotation2d rotation) { - double setpoint = - MathUtil.inputModulus( - rotation.plus(zeroRotation).getRadians(), turnPIDMinInput, turnPIDMaxInput); - turnController.setReference(setpoint, ControlType.kPosition); - } -} +//package frc.robot.subsystems.drive; +// +//import static frc.lib.utils.SparkUtil.*; +//import static frc.robot.Schematic.*; +//import static frc.robot.subsystems.drive.DriveConstants.*; +// +//import com.ctre.phoenix6.BaseStatusSignal; +//import com.ctre.phoenix6.StatusSignal; +//import com.ctre.phoenix6.configs.CANcoderConfiguration; +//import com.ctre.phoenix6.hardware.CANcoder; +//import com.ctre.phoenix6.signals.SensorDirectionValue; +//import com.revrobotics.RelativeEncoder; +//import com.revrobotics.spark.*; +//import com.revrobotics.spark.FeedbackSensor; +//import com.revrobotics.spark.SparkLowLevel.ControlType; +//import com.revrobotics.PersistMode; +//import com.revrobotics.ResetMode; +//import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; +//import com.revrobotics.spark.SparkLowLevel.MotorType; +//import com.revrobotics.spark.config.EncoderConfig; +//import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +//import com.revrobotics.spark.config.SparkMaxConfig; +//import org.wpilib.math.util.MathUtil; +//import org.wpilib.math.filter.Debouncer; +//import org.wpilib.math.geometry.Rotation2d; +//import org.wpilib.math.util.Units; +//import org.wpilib.units.measure.Angle; +//import java.util.Queue; +//import java.util.function.DoubleSupplier; +// +///** +// * Module IO implementation for Spark Flex drive motor controller, Spark Max turn motor controller, +// * and duty cycle absolute encoder. +// */ +//public class ModuleIOSpark implements ModuleIO { +// private final Rotation2d zeroRotation; +// +// // Hardware objects +// private final SparkBase driveSpark; +// private final SparkBase turnSpark; +// private final RelativeEncoder driveEncoder; +// private final RelativeEncoder turnEncoder; +// private final CANcoder turnEncoderAbsolute; +// +// // Input from turn encoder absolute +// private final StatusSignal turnPositionAbsolute; +// +// // Closed loop controllers +// private final SparkClosedLoopController driveController; +// private final SparkClosedLoopController turnController; +// +// // Queue inputs from odometry thread +// private final Queue timestampQueue; +// private final Queue drivePositionQueue; +// private final Queue turnPositionQueue; +// +// // Connection debouncers +// private final Debouncer driveConnectedDebounce = new Debouncer(0.5); +// private final Debouncer turnConnectedDebounce = new Debouncer(0.5); +// +// public ModuleIOSpark(int module) { +// zeroRotation = +// switch (module) { +// case 0 -> frontLeftZeroRotation; +// case 1 -> frontRightZeroRotation; +// case 2 -> backLeftZeroRotation; +// case 3 -> backRightZeroRotation; +// default -> Rotation2d.ZERO; +// }; +// driveSpark = +// new SparkMax( +// switch (module) { +// case 0 -> frontLeftDriveCanId; +// case 1 -> frontRightDriveCanId; +// case 2 -> backLeftDriveCanId; +// case 3 -> backRightDriveCanId; +// default -> 0; +// }, +// MotorType.kBrushless); +// turnSpark = +// new SparkMax( +// switch (module) { +// case 0 -> frontLeftTurnCanId; +// case 1 -> frontRightTurnCanId; +// case 2 -> backLeftTurnCanId; +// case 3 -> backRightTurnCanId; +// default -> 0; +// }, +// MotorType.kBrushless); +// driveEncoder = driveSpark.getEncoder(); +// turnEncoder = turnSpark.getEncoder(); +// turnEncoderAbsolute = +// new CANcoder( +// switch (module) { +// case 0 -> frontLeftAbsoluteEncoderCanId; +// case 1 -> frontRightAbsoluteEncoderCanId; +// case 2 -> backLeftAbsoluteEncoderCanId; +// case 3 -> backRightAbsoluteEncoderCanId; +// default -> 0; +// },kCANBus); +// driveController = driveSpark.getClosedLoopController(); +// turnController = turnSpark.getClosedLoopController(); +// +// // Configure drive motor +// var driveConfig = new SparkMaxConfig(); +// +// +// driveConfig +// .idleMode(IdleMode.kBrake) +// .smartCurrentLimit(driveMotorCurrentLimit) +// .voltageCompensation(12.0); +// driveConfig +// .encoder +// .positionConversionFactor(driveEncoderPositionFactor) +// .velocityConversionFactor(driveEncoderVelocityFactor) +// .uvwMeasurementPeriod(10) +// .uvwAverageDepth(2); +// driveConfig +// .closedLoop +// .feedbackSensor(FeedbackSensor.kPrimaryEncoder) +// .pid( +// driveKp, 0.0, +// driveKd); +// driveConfig +// .signals +// .primaryEncoderPositionAlwaysOn(true) +// .primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) +// .primaryEncoderVelocityAlwaysOn(true) +// .primaryEncoderVelocityPeriodMs(20) +// .appliedOutputPeriodMs(20) +// .busVoltagePeriodMs(20) +// .outputCurrentPeriodMs(20); +// tryUntilOk( +// driveSpark, +// 5, +// () -> +// driveSpark.configure( +// driveConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); +// tryUntilOk(driveSpark, 5, () -> driveEncoder.setPosition(0.0)); +// EncoderConfig config = new EncoderConfig(); +// +// +// // Configure turn motor +// var turnConfig = new SparkMaxConfig(); +// turnConfig +// .inverted(turnInverted) +// .idleMode(IdleMode.kBrake) +// .smartCurrentLimit(turnMotorCurrentLimit) +// .voltageCompensation(12.0); +// turnConfig +// .encoder +// .positionConversionFactor(turnEncoderPositionFactor) +// .velocityConversionFactor(turnEncoderVelocityFactor) +// .uvwAverageDepth(2); +// turnConfig +// .closedLoop +// .feedbackSensor(FeedbackSensor.kPrimaryEncoder) +// .positionWrappingEnabled(true) +// .pid(turnKp, 0.0, turnKd); +// turnConfig +// .signals +// .absoluteEncoderPositionAlwaysOn(true) +// .absoluteEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) +// .absoluteEncoderVelocityAlwaysOn(true) +// .absoluteEncoderVelocityPeriodMs(20) +// .appliedOutputPeriodMs(20) +// .busVoltagePeriodMs(20) +// .outputCurrentPeriodMs(20); +// tryUntilOk( +// turnSpark, +// 5, +// () -> +// turnSpark.configure( +// turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); +// +// // Configure CANCoder +// CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); +// cancoderConfig.MagnetSensor.MagnetOffset = 0; // This is done later in the code +// cancoderConfig.MagnetSensor.SensorDirection = +// turnEncoderInverted +// ? SensorDirectionValue.Clockwise_Positive +// : SensorDirectionValue.CounterClockwise_Positive; +// turnEncoderAbsolute.getConfigurator().apply(cancoderConfig); +// turnPositionAbsolute = turnEncoderAbsolute.getPosition(); +// +// BaseStatusSignal.setUpdateFrequencyForAll(50.0, turnPositionAbsolute); +// +// // Create odometry queues +// timestampQueue = OdometryThread.getInstance().makeTimestampQueue(); +// drivePositionQueue = +// OdometryThread.getInstance().registerSignal(driveSpark, driveEncoder::getPosition); +// turnPositionQueue = +// OdometryThread.getInstance().registerSignal(turnSpark, turnEncoder::getPosition); +// +// // Initialize turn relative encoder +// tryUntilOk( +// turnSpark, +// 5, +// () -> +// turnEncoder.setPosition( +// Units.rotationsToRadians( +// turnEncoderAbsolute.getAbsolutePosition().getValueAsDouble()))); +// } +// +// @Override +// public void updateInputs(ModuleIOInputs inputs) { +// BaseStatusSignal.refreshAll(turnPositionAbsolute); +// inputs.turnPositionAbsolute = Units.rotationsToRadians(turnPositionAbsolute.getValueAsDouble()); +// // Update drive inputs +// sparkStickyFault = false; +// ifOk(driveSpark, driveEncoder::getPosition, (value) -> inputs.drivePositionRad = value); +// ifOk(driveSpark, driveEncoder::getVelocity, (value) -> inputs.driveVelocityRadPerSec = value); +// ifOk( +// driveSpark, +// new DoubleSupplier[] {driveSpark::getAppliedOutput, driveSpark::getBusVoltage}, +// (values) -> inputs.driveAppliedVolts = values[0] * values[1]); +// ifOk(driveSpark, driveSpark::getOutputCurrent, (value) -> inputs.driveCurrentAmps = value); +// inputs.driveConnected = driveConnectedDebounce.calculate(!sparkStickyFault); +// +// // Update turn inputs +// sparkStickyFault = false; +// ifOk( +// turnSpark, +// turnEncoder::getPosition, +// (value) -> inputs.turnPosition = new Rotation2d(value).minus(zeroRotation)); +// ifOk(turnSpark, turnEncoder::getVelocity, (value) -> inputs.turnVelocityRadPerSec = value); +// ifOk( +// turnSpark, +// new DoubleSupplier[] {turnSpark::getAppliedOutput, turnSpark::getBusVoltage}, +// (values) -> inputs.turnAppliedVolts = values[0] * values[1]); +// ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.turnCurrentAmps = value); +// inputs.turnConnected = turnConnectedDebounce.calculate(!sparkStickyFault); +// +// // Update odometry inputs +// inputs.odometryTimestamps = +// timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); +// inputs.odometryDrivePositionsRad = +// drivePositionQueue.stream().mapToDouble((Double value) -> value).toArray(); +// inputs.odometryTurnPositions = +// turnPositionQueue.stream() +// .map((Double value) -> new Rotation2d(value).minus(zeroRotation)) +// .toArray(Rotation2d[]::new); +// timestampQueue.clear(); +// drivePositionQueue.clear(); +// turnPositionQueue.clear(); +// } +// +// @Override +// public void setDriveOpenLoop(double output) { +// driveSpark.setVoltage(output); +// } +// +// @Override +// public void setTurnOpenLoop(double output) { +// turnSpark.setVoltage(output); +// } +// +// @Override +// public void setDriveVelocity(double velocityRadPerSec) { +// double ffVolts = driveKs * Math.signum(velocityRadPerSec) + driveKv * velocityRadPerSec; +// driveController.setReference( +// velocityRadPerSec, +// ControlType.kVelocity, +// ClosedLoopSlot.kSlot0, +// ffVolts, +// ArbFFUnits.kVoltage); +// } +// +// @Override +// public void setTurnPosition(Rotation2d rotation) { +// double setpoint = +// MathUtil.inputModulus( +// rotation.plus(zeroRotation).getRadians(), turnPIDMinInput, turnPIDMaxInput); +// turnController.setReference(setpoint, ControlType.kPosition); +// } +//} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonSpark.java index 0b36ee8ef..79e459805 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonSpark.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.drive; import static frc.lib.utils.PhoenixUtil.*; -import static frc.lib.utils.SparkUtil.*; import static frc.robot.Schematic.*; import static frc.robot.subsystems.drive.DriveConstants.*; @@ -10,26 +9,25 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.*; +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.ParentDevice; 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.signals.StaticFeedforwardSignValue; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.FeedbackSensor; -import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.SparkLowLevel.ControlType; -import com.revrobotics.PersistMode; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkMaxConfig; -import org.wpilib.math.util.MathUtil; + +import java.util.ArrayList; +import java.util.List; +import java.util.Queue; +import java.util.concurrent.ConcurrentLinkedQueue; + import org.wpilib.math.filter.Debouncer; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.util.Units; @@ -37,264 +35,874 @@ import org.wpilib.units.measure.AngularVelocity; import org.wpilib.units.measure.Current; import org.wpilib.units.measure.Voltage; -import frc.lib.utils.PhoenixUtil; -import java.util.Queue; -import java.util.function.DoubleSupplier; + /** - * Module IO implementation for Spark Flex drive motor controller, Spark Max turn motor controller, - * and duty cycle absolute encoder. + * Module IO implementation for Talon FX drive motor controller, + * Talon FX turn motor controller, and CANcoder absolute encoder. */ public class ModuleIOTalonSpark implements ModuleIO { + + // ========================================================================= + // Phoenix odometry thread + // ========================================================================= + // + // This is implemented here instead of using AdvantageKit's + // PhoenixOdometryThread because that class is not present in this project. + // + // Phoenix signals are updated at the configured odometry frequency and + // copied into queues. The normal ModuleIO updateInputs() method consumes + // those queues. + // + // ========================================================================= + + private static final class PhoenixOdometryThread extends Thread { + + private static final PhoenixOdometryThread INSTANCE = + new PhoenixOdometryThread(); + + private final List signals = new ArrayList<>(); + private final List> signalQueues = new ArrayList<>(); + private final List> timestampQueues = new ArrayList<>(); + + private PhoenixOdometryThread() { + super("PhoenixOdometryThread"); + + setDaemon(true); + start(); + } + + public static PhoenixOdometryThread getInstance() { + return INSTANCE; + } + + public synchronized Queue makeTimestampQueue() { + Queue queue = new ConcurrentLinkedQueue<>(); + + timestampQueues.add(queue); + + return queue; + } + + public synchronized Queue registerSignal( + StatusSignal signal) { + + Queue queue = new ConcurrentLinkedQueue<>(); + + signals.add(signal); + signalQueues.add(queue); + + return queue; + } + + @Override + public void run() { + + while (!Thread.currentThread().isInterrupted()) { + + BaseStatusSignal[] signalsSnapshot; + + synchronized (this) { + signalsSnapshot = + signals.toArray(new BaseStatusSignal[0]); + } + + if (signalsSnapshot.length == 0) { + try { + Thread.sleep(1); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + break; + } + + continue; + } + + /* + * Wait until a new sample is available from all registered + * Phoenix signals. + * + * This is preferable to using Timer.getFPGATimestamp() because + * Phoenix provides timestamps associated with the received CAN data. + */ + BaseStatusSignal.waitForAll( + 0.1, + signalsSnapshot); + + /* + * Use the timestamp from the first Phoenix signal. + * + * All odometry signals are configured at the same frequency and + * are sampled together by waitForAll(). + */ + double timestamp = + signalsSnapshot[0] + .getTimestamp() + .getTime(); + + synchronized (this) { + + /* + * Every ModuleIO instance gets its own timestamp queue. + */ + for (Queue queue : timestampQueues) { + queue.add(timestamp); + } + + /* + * Copy every registered signal into its corresponding queue. + */ + for (int i = 0; i < signals.size(); i++) { + + signalQueues + .get(i) + .add( + signals + .get(i) + .getValueAsDouble()); + } + } + } + } + } + + + // ========================================================================= + // Module state + // ========================================================================= + private final Rotation2d zeroRotation; + + // ========================================================================= // Hardware objects + // ========================================================================= + private final TalonFX driveTalon; - private final SparkBase turnSpark; - private final RelativeEncoder turnEncoder; + private final TalonFX turnTalon; private final CANcoder turnEncoderAbsolute; + + // ========================================================================= // Voltage control requests - private final VoltageOut voltageRequest = new VoltageOut(0); - private final PositionVoltage positionVoltageRequest = new PositionVoltage(0.0); - private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); + // ========================================================================= + + private final VoltageOut voltageRequest = + new VoltageOut(0); + + private final PositionVoltage positionVoltageRequest = + new PositionVoltage(0.0); + + private final VelocityVoltage velocityVoltageRequest = + new VelocityVoltage(0.0); + + // ========================================================================= // Torque-current control requests - private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); + // ========================================================================= + + private final TorqueCurrentFOC torqueCurrentRequest = + new TorqueCurrentFOC(0); + private final PositionTorqueCurrentFOC positionTorqueCurrentRequest = new PositionTorqueCurrentFOC(0.0); + private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = new VelocityTorqueCurrentFOC(0.0); - // Inputs from drive motor + + // ========================================================================= + // Timestamp inputs from Phoenix odometry thread + // ========================================================================= + + private final Queue timestampQueue; + + + // ========================================================================= + // Drive motor inputs + // ========================================================================= + private final StatusSignal drivePosition; + private final Queue drivePositionQueue; + private final StatusSignal driveVelocity; private final StatusSignal driveAppliedVolts; private final StatusSignal driveCurrent; - // Input from turn encoder absolute - private final StatusSignal turnPositionAbsolute; - // Closed loop controllers - private final SparkClosedLoopController turnController; + // ========================================================================= + // Turn motor inputs + // ========================================================================= - // Queue inputs from odometry thread - private final Queue timestampQueue; - private final Queue drivePositionQueue; + private final StatusSignal turnPosition; private final Queue turnPositionQueue; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + + // ========================================================================= + // Absolute CANcoder input + // ========================================================================= + + private final StatusSignal turnAbsolutePosition; + + + // ========================================================================= // Connection debouncers - private final Debouncer driveConnectedDebounce = new Debouncer(0.5); - private final Debouncer turnConnectedDebounce = new Debouncer(0.5); + // ========================================================================= + + private final Debouncer driveConnectedDebounce = + new Debouncer(0.5); + + private final Debouncer turnConnectedDebounce = + new Debouncer(0.5); + + private final Debouncer turnEncoderConnectedDebounce = + new Debouncer(0.5); + + + // ========================================================================= + // Constructor + // ========================================================================= public ModuleIOTalonSpark(int module) { + + // ----------------------------------------------------------------------- + // Module-specific zero rotation + // ----------------------------------------------------------------------- + zeroRotation = switch (module) { case 0 -> frontLeftZeroRotation; case 1 -> frontRightZeroRotation; case 2 -> backLeftZeroRotation; case 3 -> backRightZeroRotation; - default -> Rotation2d.kZero; + default -> Rotation2d.ZERO; + }; + + + // ----------------------------------------------------------------------- + // CAN IDs + // ----------------------------------------------------------------------- + + int driveCanId = + switch (module) { + case 0 -> frontLeftDriveCanId; + case 1 -> frontRightDriveCanId; + case 2 -> backLeftDriveCanId; + case 3 -> backRightDriveCanId; + default -> 0; + }; + + int turnCanId = + switch (module) { + case 0 -> frontLeftTurnCanId; + case 1 -> frontRightTurnCanId; + case 2 -> backLeftTurnCanId; + case 3 -> backRightTurnCanId; + default -> 0; + }; + + int encoderCanId = + switch (module) { + case 0 -> frontLeftAbsoluteEncoderCanId; + case 1 -> frontRightAbsoluteEncoderCanId; + case 2 -> backLeftAbsoluteEncoderCanId; + case 3 -> backRightAbsoluteEncoderCanId; + default -> 0; }; + + + // ----------------------------------------------------------------------- + // Hardware + // ----------------------------------------------------------------------- + driveTalon = new TalonFX( - switch (module) { - case 0 -> frontLeftDriveCanId; - case 1 -> frontRightDriveCanId; - case 2 -> backLeftDriveCanId; - case 3 -> backRightDriveCanId; - default -> 0; - }); - turnSpark = - new SparkMax( - switch (module) { - case 0 -> frontLeftTurnCanId; - case 1 -> frontRightTurnCanId; - case 2 -> backLeftTurnCanId; - case 3 -> backRightTurnCanId; - default -> 0; - }, - MotorType.kBrushless); - turnEncoder = turnSpark.getEncoder(); + driveCanId, + kCANBus); + + turnTalon = + new TalonFX( + turnCanId, + kCANBus); + turnEncoderAbsolute = new CANcoder( - switch (module) { - case 0 -> frontLeftAbsoluteEncoderCanId; - case 1 -> frontRightAbsoluteEncoderCanId; - case 2 -> backLeftAbsoluteEncoderCanId; - case 3 -> backRightAbsoluteEncoderCanId; - default -> 0; - }); - turnController = turnSpark.getClosedLoopController(); - - var driveConfig = new TalonFXConfiguration(); - driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - driveConfig.Feedback.SensorToMechanismRatio = driveMotorReduction; - driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = driveMotorCurrentLimit; - driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -driveMotorCurrentLimit; - driveConfig.CurrentLimits.StatorCurrentLimit = driveMotorCurrentLimit; - driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; - driveConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - var slot0config = new Slot0Configs(); - slot0config.kP = driveKp; - slot0config.kI = 0; - slot0config.kD = driveKd; - slot0config.kS = driveKs; - slot0config.kV = driveKv; - slot0config.kA = driveKa; - slot0config.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign; - driveConfig.withSlot0(slot0config); - PhoenixUtil.tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); - PhoenixUtil.tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); - - // Configure turn motor - var turnConfig = new SparkMaxConfig(); - turnConfig - .inverted(turnInverted) - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(turnMotorCurrentLimit) - .voltageCompensation(12.0); - turnConfig - .encoder - .positionConversionFactor(turnEncoderPositionFactor) - .velocityConversionFactor(turnEncoderVelocityFactor) - .uvwAverageDepth(2); - turnConfig - .closedLoop - .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .positionWrappingEnabled(true) - .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) - .pidf(turnKp, 0.0, turnKd, 0.0); - turnConfig - .signals - .primaryEncoderPositionAlwaysOn(true) - .primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) - .primaryEncoderVelocityAlwaysOn(true) - .primaryEncoderVelocityPeriodMs(20) - .appliedOutputPeriodMs(20) - .busVoltagePeriodMs(20) - .outputCurrentPeriodMs(20); + encoderCanId, + kCANBus); + + + // ========================================================================= + // DRIVE MOTOR + // ========================================================================= + + var driveConfig = + new TalonFXConfiguration(); + + driveConfig.MotorOutput.NeutralMode = + NeutralModeValue.Brake; + + driveConfig.Feedback.SensorToMechanismRatio = + driveMotorReduction; + + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = + driveMotorCurrentLimit; + + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = + -driveMotorCurrentLimit; + + driveConfig.CurrentLimits.StatorCurrentLimit = + driveMotorCurrentLimit; + + driveConfig.CurrentLimits.StatorCurrentLimitEnable = + true; + + driveConfig.MotorOutput.Inverted = + InvertedValue.CounterClockwise_Positive; + + + // ----------------------------------------------------------------------- + // Drive PID + feedforward + // ----------------------------------------------------------------------- + + var driveSlot0 = + new Slot0Configs(); + + driveSlot0.kP = + driveKp; + + driveSlot0.kI = + 0.0; + + driveSlot0.kD = + driveKd; + + driveSlot0.kS = + driveKs; + + driveSlot0.kV = + driveKv; + + driveSlot0.kA = + driveKa; + + driveConfig.Slot0 = + driveSlot0; + + + // ----------------------------------------------------------------------- + // Apply drive configuration + // ----------------------------------------------------------------------- + + tryUntilOk( + 5, + () -> + driveTalon + .getConfigurator() + .apply( + driveConfig, + 0.25)); + tryUntilOk( - turnSpark, 5, () -> - turnSpark.configure( - turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + driveTalon + .setPosition( + 0.0, + 0.25)); + + + // ========================================================================= + // TURN MOTOR + // ========================================================================= + + var turnConfig = + new TalonFXConfiguration(); + + + turnConfig.MotorOutput.NeutralMode = + NeutralModeValue.Brake; + + + // ----------------------------------------------------------------------- + // Steering feedback = CANcoder + // ----------------------------------------------------------------------- + + turnConfig.Feedback.FeedbackRemoteSensorID = + encoderCanId; + + turnConfig.Feedback.FeedbackSensorSource = + FeedbackSensorSourceValue.FusedCANcoder; + + turnConfig.Feedback.RotorToSensorRatio = + turnMotorReduction; + + + // ----------------------------------------------------------------------- + // Steering PID + // ----------------------------------------------------------------------- + + var turnSlot0 = + new Slot0Configs(); + + turnSlot0.kP = + turnKp; + + turnSlot0.kI = + 0.0; + + turnSlot0.kD = + turnKd; + + turnConfig.Slot0 = + turnSlot0; + + + // ----------------------------------------------------------------------- + // Steering motor direction + // ----------------------------------------------------------------------- + + turnConfig.MotorOutput.Inverted = + InvertedValue.CounterClockwise_Positive; + + + // ----------------------------------------------------------------------- + // Motion Magic + // ----------------------------------------------------------------------- + + turnConfig.MotionMagic.MotionMagicCruiseVelocity = + 100.0 / turnMotorReduction; + + turnConfig.MotionMagic.MotionMagicAcceleration = + turnConfig + .MotionMagic + .MotionMagicCruiseVelocity + / 0.100; + + + // ----------------------------------------------------------------------- + // Allow steering to cross the 0/360 boundary + // ----------------------------------------------------------------------- + + turnConfig.ClosedLoopGeneral.ContinuousWrap = + true; + + + // ----------------------------------------------------------------------- + // Apply turn configuration + // ----------------------------------------------------------------------- + + tryUntilOk( + 5, + () -> + turnTalon + .getConfigurator() + .apply( + turnConfig, + 0.25)); + + + // ========================================================================= + // CANCODER + // ========================================================================= + + var cancoderConfig = + new CANcoderConfiguration(); + + + /* + * Zero offset is handled using zeroRotation below. + */ + cancoderConfig.MagnetSensor.MagnetOffset = + 0.0; - // Configure CANCoder - CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); - cancoderConfig.MagnetSensor.MagnetOffset = 0; // This is done later in the code cancoderConfig.MagnetSensor.SensorDirection = turnEncoderInverted ? SensorDirectionValue.Clockwise_Positive : SensorDirectionValue.CounterClockwise_Positive; - turnEncoderAbsolute.getConfigurator().apply(cancoderConfig); - // Create timestamp queue - timestampQueue = OdometryThread.getInstance().makeTimestampQueue(); - // Create drive status signals - drivePosition = driveTalon.getPosition(); - drivePositionQueue = OdometryThread.getInstance().registerSignal(driveTalon.getPosition()); - driveVelocity = driveTalon.getVelocity(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getStatorCurrent(); + tryUntilOk( + 5, + () -> + turnEncoderAbsolute + .getConfigurator() + .apply( + cancoderConfig, + 0.25)); + + + // ========================================================================= + // PHOENIX ODOMETRY + // ========================================================================= + + PhoenixOdometryThread odometryThread = + PhoenixOdometryThread.getInstance(); + + timestampQueue = + odometryThread.makeTimestampQueue(); + + + // ----------------------------------------------------------------------- + // Drive signals + // ----------------------------------------------------------------------- + + drivePosition = + driveTalon.getPosition(); + + drivePositionQueue = + odometryThread.registerSignal( + drivePosition.clone()); + + driveVelocity = + driveTalon.getVelocity(); + + driveAppliedVolts = + driveTalon.getMotorVoltage(); + + driveCurrent = + driveTalon.getStatorCurrent(); + + + // ----------------------------------------------------------------------- + // Turn signals + // ----------------------------------------------------------------------- + + turnPosition = + turnTalon.getPosition(); - // Create turn status signals - turnPositionAbsolute = turnEncoderAbsolute.getPosition(); turnPositionQueue = - OdometryThread.getInstance().registerSignal(turnSpark, turnEncoder::getPosition); + odometryThread.registerSignal( + turnPosition.clone()); + + turnVelocity = + turnTalon.getVelocity(); + + turnAppliedVolts = + turnTalon.getMotorVoltage(); + + turnCurrent = + turnTalon.getStatorCurrent(); - // Configure periodic frames - BaseStatusSignal.setUpdateFrequencyForAll(odometryFrequency, drivePosition); + + // ----------------------------------------------------------------------- + // Absolute encoder + // ----------------------------------------------------------------------- + + turnAbsolutePosition = + turnEncoderAbsolute.getAbsolutePosition(); + + + // ========================================================================= + // SIGNAL UPDATE FREQUENCIES + // ========================================================================= + + /* + * These are the high-frequency signals used for odometry. + */ BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, driveVelocity, driveAppliedVolts, driveCurrent, turnPositionAbsolute); - ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnEncoderAbsolute); + odometryFrequency, + drivePosition, + turnPosition); - // Initialize turn relative encoder - tryUntilOk( - turnSpark, - 5, - () -> - turnEncoder.setPosition( - Units.rotationsToRadians( - turnEncoderAbsolute.getAbsolutePosition().getValueAsDouble()))); + + /* + * These signals are only needed for normal robot inputs. + */ + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + + + // ----------------------------------------------------------------------- + // Optimize CAN utilization + // ----------------------------------------------------------------------- + + ParentDevice.optimizeBusUtilizationForAll( + driveTalon, + turnTalon, + turnEncoderAbsolute); } + + // ========================================================================= + // INPUT UPDATE + // ========================================================================= + @Override - public void updateInputs(ModuleIOInputs inputs) { - // Refresh all signals + public void updateInputs( + ModuleIOInputs inputs) { + + // ----------------------------------------------------------------------- + // Refresh drive signals + // ----------------------------------------------------------------------- + var driveStatus = - BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); - var turnEncoderStatus = BaseStatusSignal.refreshAll(turnPositionAbsolute); - - // Update drive inputs - inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); - inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); - inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); - inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); - - // Update turn inputs - sparkStickyFault = false; - ifOk( - turnSpark, - turnEncoder::getPosition, - (value) -> inputs.turnPosition = new Rotation2d(value).minus(zeroRotation)); - ifOk(turnSpark, turnEncoder::getVelocity, (value) -> inputs.turnVelocityRadPerSec = value); - ifOk( - turnSpark, - new DoubleSupplier[] {turnSpark::getAppliedOutput, turnSpark::getBusVoltage}, - (values) -> inputs.turnAppliedVolts = values[0] * values[1]); - ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.turnCurrentAmps = value); - inputs.turnPositionAbsolute = Units.rotationsToRadians(turnPositionAbsolute.getValueAsDouble()); - inputs.turnConnected = turnConnectedDebounce.calculate(!sparkStickyFault); - - // Update odometry inputs + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveCurrent); + + + // ----------------------------------------------------------------------- + // Refresh steering signals + // ----------------------------------------------------------------------- + + var turnStatus = + BaseStatusSignal.refreshAll( + turnPosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + + + // ----------------------------------------------------------------------- + // Refresh CANcoder + // ----------------------------------------------------------------------- + + var turnEncoderStatus = + BaseStatusSignal.refreshAll( + turnAbsolutePosition); + + + // ========================================================================= + // DRIVE INPUTS + // ========================================================================= + + inputs.driveConnected = + driveConnectedDebounce.calculate( + driveStatus.isOK()); + + + inputs.drivePositionRad = + Units.rotationsToRadians( + drivePosition.getValueAsDouble()); + + + inputs.driveVelocityRadPerSec = + Units.rotationsToRadians( + driveVelocity.getValueAsDouble()); + + + inputs.driveAppliedVolts = + driveAppliedVolts.getValueAsDouble(); + + + inputs.driveCurrentAmps = + driveCurrent.getValueAsDouble(); + + + // ========================================================================= + // TURN INPUTS + // ========================================================================= + + inputs.turnConnected = + turnConnectedDebounce.calculate( + turnStatus.isOK()); + + + /* + * The TalonFX is configured to use the fused CANcoder + * as its steering feedback source. + * + * This is the primary steering position used by the module. + */ + inputs.turnPosition = + Rotation2d.fromRotations( + turnPosition.getValueAsDouble()) + .minus( + zeroRotation); + + + inputs.turnVelocityRadPerSec = + Units.rotationsToRadians( + turnVelocity.getValueAsDouble()); + + + inputs.turnAppliedVolts = + turnAppliedVolts.getValueAsDouble(); + + + inputs.turnCurrentAmps = + turnCurrent.getValueAsDouble(); + + + // ========================================================================= + // ABSOLUTE CANCODER + // ========================================================================= + + /* + * Keep the absolute CANcoder position separate from turnPosition. + * + * turnPosition is the fused TalonFX/CANcoder steering position. + * turnAbsolutePosition is the raw absolute CANcoder measurement. + */ + inputs.turnPosition = + Rotation2d.fromRotations( + turnAbsolutePosition.getValueAsDouble()) + .minus( + zeroRotation); + + + // ========================================================================= + // ODOMETRY + // ========================================================================= + + /* + * Copy the high-frequency Phoenix samples into the module inputs. + */ + inputs.odometryTimestamps = - timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + timestampQueue + .stream() + .mapToDouble( + (Double value) -> value) + .toArray(); + + inputs.odometryDrivePositionsRad = - drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value)) + drivePositionQueue + .stream() + .mapToDouble( + (Double value) -> + Units.rotationsToRadians(value)) .toArray(); + + inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((Double value) -> new Rotation2d(value).minus(zeroRotation)) - .toArray(Rotation2d[]::new); + turnPositionQueue + .stream() + .map( + (Double value) -> + Rotation2d + .fromRotations(value) + .minus(zeroRotation)) + .toArray( + Rotation2d[]::new); + + + // ----------------------------------------------------------------------- + // Clear queues after copying them + // ----------------------------------------------------------------------- + timestampQueue.clear(); drivePositionQueue.clear(); turnPositionQueue.clear(); } + + // ========================================================================= + // DRIVE OPEN LOOP + // ========================================================================= + @Override - public void setDriveOpenLoop(double output) { + public void setDriveOpenLoop( + double output) { + driveTalon.setControl( switch (driveClosedLoopOutput) { - case Voltage -> voltageRequest.withOutput(output); - case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); + + case Voltage -> + voltageRequest + .withOutput(output); + + case TorqueCurrentFOC -> + torqueCurrentRequest + .withOutput(output); }); } + + // ========================================================================= + // TURN OPEN LOOP + // ========================================================================= + @Override - public void setTurnOpenLoop(double output) { - turnSpark.setVoltage(output); + public void setTurnOpenLoop( + double output) { + + turnTalon.setControl( + switch (driveClosedLoopOutput) { + + case Voltage -> + voltageRequest + .withOutput(output); + + case TorqueCurrentFOC -> + torqueCurrentRequest + .withOutput(output); + }); } + + // ========================================================================= + // DRIVE VELOCITY + // ========================================================================= + @Override - public void setDriveVelocity(double velocityRadPerSec) { - double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); + public void setDriveVelocity( + double velocityRadPerSec) { + + double velocityRotPerSec = + Units.radiansToRotations( + velocityRadPerSec); + + driveTalon.setControl( switch (driveClosedLoopOutput) { - case Voltage -> velocityVoltageRequest.withVelocity(velocityRotPerSec); - case TorqueCurrentFOC -> velocityTorqueCurrentRequest.withVelocity(velocityRotPerSec); + + case Voltage -> + velocityVoltageRequest + .withVelocity( + velocityRotPerSec); + + case TorqueCurrentFOC -> + velocityTorqueCurrentRequest + .withVelocity( + velocityRotPerSec); }); } + + // ========================================================================= + // TURN POSITION + // ========================================================================= + @Override - public void setTurnPosition(Rotation2d rotation) { - double setpoint = - MathUtil.inputModulus( - rotation.plus(zeroRotation).getRadians(), turnPIDMinInput, turnPIDMaxInput); - turnController.setReference(setpoint, ControlType.kPosition); + public void setTurnPosition( + Rotation2d rotation) { + + /* + * Add the module's physical zero offset before + * sending the target to Phoenix. + */ + Rotation2d target = + rotation.plus( + zeroRotation); + + + turnTalon.setControl( + switch (driveClosedLoopOutput) { + + case Voltage -> + positionVoltageRequest + .withPosition( + target.getRotations()); + + case TorqueCurrentFOC -> + positionTorqueCurrentRequest + .withPosition( + target.getRotations()); + }); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/OdometryThread.java b/src/main/java/frc/robot/subsystems/drive/OdometryThread.java index 505daa3bb..7aeeff867 100644 --- a/src/main/java/frc/robot/subsystems/drive/OdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/OdometryThread.java @@ -137,7 +137,7 @@ public void run() { Drive.odometryLock.lock(); try { // Get sample timestamp - double timestamp = RobotController.getFPGATime() / 1e6; + double timestamp = RobotController.getTime() / 1e6; // Read Spark values, mark invalid in case of error double[] sparkValues = new double[sparkSignals.size()]; From 4c8ef1c85ab16f9f57e3aea0806180dabbf80ea6 Mon Sep 17 00:00:00 2001 From: EJainDev Date: Mon, 5 Oct 2026 20:34:47 -0400 Subject: [PATCH 13/15] feat: add back SubsystemBase --- src/main/java/frc/lib/SubsystemBase.java | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 src/main/java/frc/lib/SubsystemBase.java diff --git a/src/main/java/frc/lib/SubsystemBase.java b/src/main/java/frc/lib/SubsystemBase.java new file mode 100644 index 000000000..b2b98e374 --- /dev/null +++ b/src/main/java/frc/lib/SubsystemBase.java @@ -0,0 +1,17 @@ +package frc.lib; + +import org.wpilib.command3.Coroutine; +import org.wpilib.command3.Mechanism; +import org.wpilib.command3.NeedsNameBuilderStage; +import org.wpilib.command3.StagedCommandBuilder; +import java.util.function.Consumer; + +public abstract class SubsystemBase implements Mechanism { + public NeedsNameBuilderStage run(Consumer commandBody) { + return new StagedCommandBuilder().requiring(this).executing( + commandBody.andThen(_ -> periodic()) + ); + } + + public abstract void periodic(); +} From 34626c42f0286510f296bc226ed02a79310d85f4 Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Mon, 5 Oct 2026 20:36:18 -0400 Subject: [PATCH 14/15] Revert "drive rewrite to 2027" This reverts commit f632a727f9d0042a576e2aef7f7451785dd916e4. --- .../frc/robot/subsystems/drive/Drive.java | 147 ++- .../subsystems/drive/DriveConstants.java | 5 - .../frc/robot/subsystems/drive/GyroIO.java | 2 +- .../robot/subsystems/drive/GyroIOPigeon2.java | 5 +- .../frc/robot/subsystems/drive/Module.java | 26 +- .../frc/robot/subsystems/drive/ModuleIO.java | 2 +- .../robot/subsystems/drive/ModuleIOSim.java | 32 +- .../robot/subsystems/drive/ModuleIOSpark.java | 548 +++++----- .../subsystems/drive/ModuleIOTalonSpark.java | 988 ++++-------------- .../subsystems/drive/OdometryThread.java | 2 +- 10 files changed, 559 insertions(+), 1198 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 784599747..c89cb341d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -1,17 +1,12 @@ package frc.robot.subsystems.drive; -import static org.wpilib.framework.RobotBase.isDisabled; import static org.wpilib.units.Units.*; import static frc.robot.subsystems.drive.DriveConstants.*; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathPlannerPath; -import frc.robot.RobotState; -import org.wpilib.driverstation.MatchState; import org.wpilib.math.kinematics.ChassisVelocities; -import org.wpilib.math.kinematics.SwerveModuleVelocity; -import org.wpilib.wpiutil.Alert; // 2027 migration: Alert moved from HAL/wpilib into wpiutil -import org.wpilib.wpiutil.Alert.Level; +import org.wpilib.util.Alert.Level; import org.wpilib.math.linalg.Matrix; import org.wpilib.math.controller.PIDController; import org.wpilib.math.estimator.SwerveDrivePoseEstimator; @@ -20,19 +15,16 @@ import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Rotation3d; import org.wpilib.math.geometry.Twist2d; +import org.wpilib.math.kinematics.ChassisVelocities; // Removed or renamed (maybe Velocities) import org.wpilib.math.kinematics.SwerveDriveKinematics; import org.wpilib.math.kinematics.SwerveModulePosition; -import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.kinematics.SwerveModuleState; // Removed or renamed import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N3; +import org.wpilib.util.Alert; import org.wpilib.driverstation.DriverStation; import org.wpilib.command3.Command; import org.wpilib.command3.Mechanism; -// 2027 migration: SysIdRoutine below is still the Commands v2 class (no v3 equivalent -// shipped yet as of alpha-7). Its Mechanism.Mechanism(...) constructor's third argument -// expects a v2 Subsystem, but this class now implements the v3 Mechanism interface -// (see class declaration) instead. Passing `this` there is a real type mismatch that -// needs a team decision, not a mechanical fix — see summary below. import org.wpilib.command2.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.Mode; @@ -46,17 +38,24 @@ public class Drive implements Mechanism { static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + private final Alert impactAlert = + new Alert("Impact Detected, lowering elevator to prevent flipping.", Level.MEDIUM); + private final Alert tiltAlert = + new Alert( + ("Tilt Threshold reached, lowering elevator to prevent flipping"), Level.MEDIUM); private final Module[] modules = new Module[4]; // FL, FR, BL, BR - // private final SysIdRoutine sysId; + private final SysIdRoutine sysId; + private final Alert gyroDisconnectedAlert = + new Alert("Disconnected gyro, using kinematics as fallback.", Level.HIGH); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(moduleTranslations); - private Rotation2d rawGyroRotation = Rotation2d.ZERO; + private Rotation2d rawGyroRotation = Rotation2d.kZero; private SwerveModulePosition[] lastModulePositions = // For delta tracking new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() }; private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); @@ -84,23 +83,20 @@ public Drive( // Configure AutoBuilder for PathPlanner // Configure SysId -// sysId = -// new SysIdRoutine( -// new SysIdRoutine.Config( -// null, -// null, -// null, -// (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), -// // 2027 migration: `this` no longer satisfies this constructor's expected type -// // (v2 Subsystem) now that Drive implements the v3 Mechanism interface. Needs a -// // team decision — see summary. -// new SysIdRoutine.Mechanism( -// (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); + sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); headingController.enableContinuousInput(-Math.PI, Math.PI); } - + @Override public void periodic() { logCameraPositions(); // uncomment to show camera positions in advantage scope odometryLock.lock(); // Prevents odometry updates while reading data @@ -111,19 +107,16 @@ public void periodic() { } odometryLock.unlock(); // Stop moving when disabled - // 2027 migration: DriverStation was split into MatchState/RobotState (alpha 5). - // isDisabled() reads robot state, so this likely needs to move to RobotState.isDisabled() - // — verify the exact package/class against your target alpha before changing this. - if (isDisabled()) { + if (DriverStation.isDisabled()) { for (var module : modules) { module.stop(); } } // Log empty setpoint states when disabled - if (isDisabled()) { - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleVelocity[] {}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleVelocity[] {}); + if (DriverStation.isDisabled()) { + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); } // Update odometry @@ -138,8 +131,8 @@ public void periodic() { modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; moduleDeltas[moduleIndex] = new SwerveModulePosition( - modulePositions[moduleIndex].distance - - lastModulePositions[moduleIndex].distance, + modulePositions[moduleIndex].distanceMeters + - lastModulePositions[moduleIndex].distanceMeters, modulePositions[moduleIndex].angle); lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; } @@ -159,7 +152,7 @@ public void periodic() { } // Update gyro alert - //gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.getMode() != Mode.SIM); + gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.getMode() != Mode.SIM); } /** @@ -169,9 +162,9 @@ public void periodic() { */ public void runVelocity(ChassisVelocities speeds) { // Calculate module setpoints - ChassisVelocities discreteSpeeds = speeds.discretize(0.02); - SwerveModuleVelocity[] setpointStates = kinematics.toSwerveModuleVelocities(discreteSpeeds); - var desaturatedStates = SwerveDriveKinematics.desaturateWheelVelocities(setpointStates, maxSpeedMetersPerSec); + ChassisVelocities discreteSpeeds = ChassisVelocities.discretize(speeds, 0.02); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, maxSpeedMetersPerSec); // Log unoptimized setpoints Logger.recordOutput("SwerveStates/Setpoints", setpointStates); @@ -179,7 +172,7 @@ public void runVelocity(ChassisVelocities speeds) { // Send setpoints to modules for (int i = 0; i < 4; i++) { - modules[i].runSetpoint(desaturatedStates[i]); + modules[i].runSetpoint(setpointStates[i]); } // Log optimized setpoints (runSetpoint mutates each state) @@ -193,26 +186,18 @@ public void runCharacterization(double output) { } } -// public Command getAutonomousCommand(String Path) { -// try { -// // Load the path you want to follow using its name in the GUI -// PathPlannerPath path = PathPlannerPath.fromPathFile(Path); -// -// // Create a path following command using AutoBuilder. This will also trigger event markers. -// // 2027 migration: PathPlanner is a vendor library — it needs its own 2027-compatible -// // vendordep release. Confirm the version you're on returns a command type compatible -// // with org.wpilib.command3.Command (this method's return type); pre-2027 PathPlanner -// // versions return a Commands v2 command, which will not satisfy this signature. -// return AutoBuilder.followPath(path); -// } catch (Exception e) { -// DriverStation.reportError("Big oops: " + e.getMessage(), e.getStackTrace()); -// // 2027 migration: Commands.none() was already unresolved in the original file (no -// // Commands import) — pre-existing bug, not introduced by this migration. If you want a -// // no-op Command here, confirm what Commands v3's equivalent factory method is called -// // and import it explicitly; v3's API doesn't mirror v2's Commands class 1:1. -// return Commands.none(); -// } -// } + public Command getAutonomousCommand(String Path) { + try { + // Load the path you want to follow using its name in the GUI + PathPlannerPath path = PathPlannerPath.fromPathFile(Path); + + // Create a path following command using AutoBuilder. This will also trigger event markers. + return AutoBuilder.followPath(path); + } catch (Exception e) { + DriverStation.reportError("Big oops: " + e.getMessage(), e.getStackTrace()); + return Commands.none(); + } + } /** Stops the drive. */ public void stop() { @@ -226,28 +211,28 @@ public void stop() { public void stopWithX() { Rotation2d[] headings = new Rotation2d[4]; for (int i = 0; i < 4; i++) { - headings[i] = moduleTranslations[i].getAngle().get(); + headings[i] = moduleTranslations[i].getAngle(); } kinematics.resetHeadings(headings); stop(); } -// /** Returns a command to run a quasistatic test in the specified direction. */ -// public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { -// return run(() -> runCharacterization(0.0)) -// .withTimeout(1.0) -// .andThen(sysId.quasistatic(direction)); -// } -// -// /** Returns a command to run a dynamic test in the specified direction. */ -// public Command sysIdDynamic(SysIdRoutine.Direction direction) { -// return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction)); -// } + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)) + .withTimeout(1.0) + .andThen(sysId.quasistatic(direction)); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction)); + } /** Returns the module states (turn angles and drive velocities) for all of the modules. */ - @AutoLogOutput(key = "SwerveVelocities/Measured") - private SwerveModuleVelocity[] getModuleStates() { - SwerveModuleVelocity[] states = new SwerveModuleVelocity[4]; + @AutoLogOutput(key = "SwerveStates/Measured") + private SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; for (int i = 0; i < 4; i++) { states[i] = modules[i].getState(); } @@ -336,4 +321,4 @@ public double getMaxLinearSpeedMetersPerSec() { public double getMaxAngularSpeedRadPerSec() { return maxSpeedMetersPerSec / driveBaseRadius; } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index aa69ba0a6..bdd04b1fd 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -3,12 +3,10 @@ import static frc.robot.Schematic.*; import static java.lang.Math.PI; -import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.RobotConfig; -import org.wpilib.hardware.bus.CANPort; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.system.DCMotor; @@ -122,9 +120,6 @@ public class DriveConstants { driveMotorCurrentLimit, 1), moduleTranslations); - - //Pidgeon - public static final CANBus kCANBus = new CANBus("can_s17"); // Requirements for RobotTilt to trigger public static final double rollThreshhold = 10.0; public static final double pitchThreshold = 10.0; diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index c73c9b0b8..64e8d6608 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -7,7 +7,7 @@ public interface GyroIO { @AutoLog public static class GyroIOInputs { public boolean connected = false; - public Rotation2d yawPosition = Rotation2d.ZERO; + public Rotation2d yawPosition = Rotation2d.kZero; public double yawVelocityRadPerSec = 0.0; public double[] odometryYawTimestamps = new double[] {}; public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 5cc0d1dc0..b9fb49cb9 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -4,7 +4,6 @@ import static frc.robot.subsystems.drive.DriveConstants.*; import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Pigeon2Configuration; @@ -21,8 +20,8 @@ /** IO implementation for Pigeon 2. */ public class GyroIOPigeon2 implements GyroIO { private final Alert impactAlert = - new Alert("Gyro","Impact Detected, lowering elevator to prevent flipping.", Level.MEDIUM); - private final Pigeon2 pigeon = new Pigeon2(pigeonCanId,kCANBus); + new Alert("Impact Detected, lowering elevator to prevent flipping.", Level.MEDIUM); + private final Pigeon2 pigeon = new Pigeon2(pigeonCanId); private final StatusSignal yaw = pigeon.getYaw(); private final Queue yawPositionQueue; private final Queue yawTimestampQueue; diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 4bce53cef..a31b998a5 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -2,7 +2,6 @@ import static frc.robot.subsystems.drive.DriveConstants.*; -import org.wpilib.math.kinematics.SwerveModuleVelocity; import org.wpilib.util.Alert.Level; import org.wpilib.math.util.MathUtil; import org.wpilib.math.geometry.Rotation2d; @@ -24,11 +23,11 @@ public Module(ModuleIO io, int index) { this.io = io; this.index = index; driveDisconnectedAlert = - new Alert("driveDisconnected", + new Alert( "Disconnected drive motor on module " + Integer.toString(index) + ".", Level.HIGH); turnDisconnectedAlert = - new Alert("driveDisconnected", + new Alert( "Disconnected turn motor on module " + Integer.toString(index) + ".", Level.HIGH); } @@ -51,21 +50,21 @@ public void periodic() { } /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */ - public void runSetpoint(SwerveModuleVelocity state) { + public void runSetpoint(SwerveModuleState state) { // Optimize velocity setpoint optimize(state, getAngle()); state.cosineScale(state.angle); // Apply setpoints - io.setDriveVelocity(state.velocity / wheelRadiusMeters); + io.setDriveVelocity(state.speedMetersPerSecond / wheelRadiusMeters); io.setTurnPosition(state.angle); } /** Runs the module with the specified output while controlling to zero degrees. */ public void runCharacterization(double output) { io.setDriveOpenLoop(output); - io.setTurnPosition(Rotation2d.ZERO); + io.setTurnPosition(Rotation2d.kZero); } /** Disables all outputs to motors. */ @@ -94,11 +93,9 @@ public SwerveModulePosition getPosition() { return new SwerveModulePosition(getPositionMeters(), getAngle()); } - /** - * Returns the module state (turn angle and drive velocity). - */ - public SwerveModuleVelocity getState() { - return new SwerveModuleVelocity(getVelocityMetersPerSec(), getAngle()); + /** Returns the module state (turn angle and drive velocity). */ + public SwerveModuleState getState() { + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); } /** Returns the module positions received this cycle. */ @@ -121,12 +118,11 @@ public double getFFCharacterizationVelocity() { return inputs.driveVelocityRadPerSec; } - private static void optimize( - SwerveModuleVelocity state, Rotation2d currentAngle) { + private static void optimize(SwerveModuleState state, Rotation2d currentAngle) { var delta = state.angle.minus(currentAngle); if (Math.abs(MathUtil.inputModulus(delta.getDegrees(), -180, 180)) > 90) { - state.velocity *= -1; - state.angle = state.angle.rotateBy(Rotation2d.PI); + state.speedMetersPerSecond *= -1; + state.angle = state.angle.rotateBy(Rotation2d.kPi); } } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index a35da66da..ef0bd6ae1 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -13,7 +13,7 @@ public static class ModuleIOInputs { public double driveCurrentAmps = 0.0; public boolean turnConnected = false; - public Rotation2d turnPosition = Rotation2d.ZERO; + public Rotation2d turnPosition = Rotation2d.kZero; public double turnVelocityRadPerSec = 0.0; public double turnAppliedVolts = 0.0; public double turnCurrentAmps = 0.0; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 32e2785f4..fefb66dcb 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -2,12 +2,10 @@ import static frc.robot.subsystems.drive.DriveConstants.*; -import org.wpilib.math.system.LinearSystemLoop; -import org.wpilib.math.system.Models; import org.wpilib.math.util.MathUtil; import org.wpilib.math.controller.PIDController; -import org.wpilib.math.geometry.Rotation2d; // Removed or renamed (likely LinearSystem now) -import org.wpilib.simulation.LinearSystemSim; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.system.LinearSystemId; // Removed or renamed (likely LinearSystem now) import org.wpilib.system.Timer; import org.wpilib.simulation.DCMotorSim; @@ -28,11 +26,11 @@ public ModuleIOSim() { // Create drive and turn sim models driveSim = new DCMotorSim( - Models.singleJointedArmFromPhysicalConstants(driveGearbox, 0.025, driveMotorReduction), + LinearSystemId.createDCMotorSystem(driveGearbox, 0.025, driveMotorReduction), driveGearbox); turnSim = new DCMotorSim( - Models.singleJointedArmFromPhysicalConstants(turnGearbox, 0.004, turnMotorReduction), + LinearSystemId.createDCMotorSystem(turnGearbox, 0.004, turnMotorReduction), turnGearbox); // Enable wrapping for turn PID @@ -44,38 +42,38 @@ public void updateInputs(ModuleIOInputs inputs) { // Run closed-loop control if (driveClosedLoop) { driveAppliedVolts = - driveFFVolts + driveController.calculate(driveSim.getAngularVelocity()); + driveFFVolts + driveController.calculate(driveSim.getAngularVelocityRadPerSec()); } else { driveController.reset(); } if (turnClosedLoop) { - turnAppliedVolts = turnController.calculate(turnSim.getAngularPosition()); + turnAppliedVolts = turnController.calculate(turnSim.getAngularPositionRad()); } else { turnController.reset(); } // Update simulation state - driveSim.setInputVoltage(MathUtil.inputModulus(driveAppliedVolts, -12.0, 12.0)); - turnSim.setInputVoltage(MathUtil.inputModulus(turnAppliedVolts, -12.0, 12.0)); + driveSim.setInputVoltage(MathUtil.clamp(driveAppliedVolts, -12.0, 12.0)); + turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0)); driveSim.update(0.02); turnSim.update(0.02); // Update drive inputs inputs.driveConnected = true; - inputs.drivePositionRad = driveSim.getAngularPosition(); //.getAngularPositionRad(); - inputs.driveVelocityRadPerSec = driveSim.getAngularVelocity(); //getAngularVelocityRadPerSec(); + inputs.drivePositionRad = driveSim.getAngularPositionRad(); + inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDraw());//getCurrentDrawAmps()); + inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); // Update turn inputs inputs.turnConnected = true; - inputs.turnPosition = new Rotation2d(turnSim.getAngularPosition()); //getAngularPositionRad()); - inputs.turnVelocityRadPerSec = turnSim.getAngularVelocity(); + inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDraw()); + inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); // Update odometry inputs (50Hz because high-frequency odometry in sim doesn't matter) - inputs.odometryTimestamps = new double[] {Timer.getTimestamp()}; + inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 42b59130d..1fb261dd0 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -1,276 +1,272 @@ -//package frc.robot.subsystems.drive; -// -//import static frc.lib.utils.SparkUtil.*; -//import static frc.robot.Schematic.*; -//import static frc.robot.subsystems.drive.DriveConstants.*; -// -//import com.ctre.phoenix6.BaseStatusSignal; -//import com.ctre.phoenix6.StatusSignal; -//import com.ctre.phoenix6.configs.CANcoderConfiguration; -//import com.ctre.phoenix6.hardware.CANcoder; -//import com.ctre.phoenix6.signals.SensorDirectionValue; -//import com.revrobotics.RelativeEncoder; -//import com.revrobotics.spark.*; -//import com.revrobotics.spark.FeedbackSensor; -//import com.revrobotics.spark.SparkLowLevel.ControlType; -//import com.revrobotics.PersistMode; -//import com.revrobotics.ResetMode; -//import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; -//import com.revrobotics.spark.SparkLowLevel.MotorType; -//import com.revrobotics.spark.config.EncoderConfig; -//import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -//import com.revrobotics.spark.config.SparkMaxConfig; -//import org.wpilib.math.util.MathUtil; -//import org.wpilib.math.filter.Debouncer; -//import org.wpilib.math.geometry.Rotation2d; -//import org.wpilib.math.util.Units; -//import org.wpilib.units.measure.Angle; -//import java.util.Queue; -//import java.util.function.DoubleSupplier; -// -///** -// * Module IO implementation for Spark Flex drive motor controller, Spark Max turn motor controller, -// * and duty cycle absolute encoder. -// */ -//public class ModuleIOSpark implements ModuleIO { -// private final Rotation2d zeroRotation; -// -// // Hardware objects -// private final SparkBase driveSpark; -// private final SparkBase turnSpark; -// private final RelativeEncoder driveEncoder; -// private final RelativeEncoder turnEncoder; -// private final CANcoder turnEncoderAbsolute; -// -// // Input from turn encoder absolute -// private final StatusSignal turnPositionAbsolute; -// -// // Closed loop controllers -// private final SparkClosedLoopController driveController; -// private final SparkClosedLoopController turnController; -// -// // Queue inputs from odometry thread -// private final Queue timestampQueue; -// private final Queue drivePositionQueue; -// private final Queue turnPositionQueue; -// -// // Connection debouncers -// private final Debouncer driveConnectedDebounce = new Debouncer(0.5); -// private final Debouncer turnConnectedDebounce = new Debouncer(0.5); -// -// public ModuleIOSpark(int module) { -// zeroRotation = -// switch (module) { -// case 0 -> frontLeftZeroRotation; -// case 1 -> frontRightZeroRotation; -// case 2 -> backLeftZeroRotation; -// case 3 -> backRightZeroRotation; -// default -> Rotation2d.ZERO; -// }; -// driveSpark = -// new SparkMax( -// switch (module) { -// case 0 -> frontLeftDriveCanId; -// case 1 -> frontRightDriveCanId; -// case 2 -> backLeftDriveCanId; -// case 3 -> backRightDriveCanId; -// default -> 0; -// }, -// MotorType.kBrushless); -// turnSpark = -// new SparkMax( -// switch (module) { -// case 0 -> frontLeftTurnCanId; -// case 1 -> frontRightTurnCanId; -// case 2 -> backLeftTurnCanId; -// case 3 -> backRightTurnCanId; -// default -> 0; -// }, -// MotorType.kBrushless); -// driveEncoder = driveSpark.getEncoder(); -// turnEncoder = turnSpark.getEncoder(); -// turnEncoderAbsolute = -// new CANcoder( -// switch (module) { -// case 0 -> frontLeftAbsoluteEncoderCanId; -// case 1 -> frontRightAbsoluteEncoderCanId; -// case 2 -> backLeftAbsoluteEncoderCanId; -// case 3 -> backRightAbsoluteEncoderCanId; -// default -> 0; -// },kCANBus); -// driveController = driveSpark.getClosedLoopController(); -// turnController = turnSpark.getClosedLoopController(); -// -// // Configure drive motor -// var driveConfig = new SparkMaxConfig(); -// -// -// driveConfig -// .idleMode(IdleMode.kBrake) -// .smartCurrentLimit(driveMotorCurrentLimit) -// .voltageCompensation(12.0); -// driveConfig -// .encoder -// .positionConversionFactor(driveEncoderPositionFactor) -// .velocityConversionFactor(driveEncoderVelocityFactor) -// .uvwMeasurementPeriod(10) -// .uvwAverageDepth(2); -// driveConfig -// .closedLoop -// .feedbackSensor(FeedbackSensor.kPrimaryEncoder) -// .pid( -// driveKp, 0.0, -// driveKd); -// driveConfig -// .signals -// .primaryEncoderPositionAlwaysOn(true) -// .primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) -// .primaryEncoderVelocityAlwaysOn(true) -// .primaryEncoderVelocityPeriodMs(20) -// .appliedOutputPeriodMs(20) -// .busVoltagePeriodMs(20) -// .outputCurrentPeriodMs(20); -// tryUntilOk( -// driveSpark, -// 5, -// () -> -// driveSpark.configure( -// driveConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); -// tryUntilOk(driveSpark, 5, () -> driveEncoder.setPosition(0.0)); -// EncoderConfig config = new EncoderConfig(); -// -// -// // Configure turn motor -// var turnConfig = new SparkMaxConfig(); -// turnConfig -// .inverted(turnInverted) -// .idleMode(IdleMode.kBrake) -// .smartCurrentLimit(turnMotorCurrentLimit) -// .voltageCompensation(12.0); -// turnConfig -// .encoder -// .positionConversionFactor(turnEncoderPositionFactor) -// .velocityConversionFactor(turnEncoderVelocityFactor) -// .uvwAverageDepth(2); -// turnConfig -// .closedLoop -// .feedbackSensor(FeedbackSensor.kPrimaryEncoder) -// .positionWrappingEnabled(true) -// .pid(turnKp, 0.0, turnKd); -// turnConfig -// .signals -// .absoluteEncoderPositionAlwaysOn(true) -// .absoluteEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) -// .absoluteEncoderVelocityAlwaysOn(true) -// .absoluteEncoderVelocityPeriodMs(20) -// .appliedOutputPeriodMs(20) -// .busVoltagePeriodMs(20) -// .outputCurrentPeriodMs(20); -// tryUntilOk( -// turnSpark, -// 5, -// () -> -// turnSpark.configure( -// turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); -// -// // Configure CANCoder -// CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); -// cancoderConfig.MagnetSensor.MagnetOffset = 0; // This is done later in the code -// cancoderConfig.MagnetSensor.SensorDirection = -// turnEncoderInverted -// ? SensorDirectionValue.Clockwise_Positive -// : SensorDirectionValue.CounterClockwise_Positive; -// turnEncoderAbsolute.getConfigurator().apply(cancoderConfig); -// turnPositionAbsolute = turnEncoderAbsolute.getPosition(); -// -// BaseStatusSignal.setUpdateFrequencyForAll(50.0, turnPositionAbsolute); -// -// // Create odometry queues -// timestampQueue = OdometryThread.getInstance().makeTimestampQueue(); -// drivePositionQueue = -// OdometryThread.getInstance().registerSignal(driveSpark, driveEncoder::getPosition); -// turnPositionQueue = -// OdometryThread.getInstance().registerSignal(turnSpark, turnEncoder::getPosition); -// -// // Initialize turn relative encoder -// tryUntilOk( -// turnSpark, -// 5, -// () -> -// turnEncoder.setPosition( -// Units.rotationsToRadians( -// turnEncoderAbsolute.getAbsolutePosition().getValueAsDouble()))); -// } -// -// @Override -// public void updateInputs(ModuleIOInputs inputs) { -// BaseStatusSignal.refreshAll(turnPositionAbsolute); -// inputs.turnPositionAbsolute = Units.rotationsToRadians(turnPositionAbsolute.getValueAsDouble()); -// // Update drive inputs -// sparkStickyFault = false; -// ifOk(driveSpark, driveEncoder::getPosition, (value) -> inputs.drivePositionRad = value); -// ifOk(driveSpark, driveEncoder::getVelocity, (value) -> inputs.driveVelocityRadPerSec = value); -// ifOk( -// driveSpark, -// new DoubleSupplier[] {driveSpark::getAppliedOutput, driveSpark::getBusVoltage}, -// (values) -> inputs.driveAppliedVolts = values[0] * values[1]); -// ifOk(driveSpark, driveSpark::getOutputCurrent, (value) -> inputs.driveCurrentAmps = value); -// inputs.driveConnected = driveConnectedDebounce.calculate(!sparkStickyFault); -// -// // Update turn inputs -// sparkStickyFault = false; -// ifOk( -// turnSpark, -// turnEncoder::getPosition, -// (value) -> inputs.turnPosition = new Rotation2d(value).minus(zeroRotation)); -// ifOk(turnSpark, turnEncoder::getVelocity, (value) -> inputs.turnVelocityRadPerSec = value); -// ifOk( -// turnSpark, -// new DoubleSupplier[] {turnSpark::getAppliedOutput, turnSpark::getBusVoltage}, -// (values) -> inputs.turnAppliedVolts = values[0] * values[1]); -// ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.turnCurrentAmps = value); -// inputs.turnConnected = turnConnectedDebounce.calculate(!sparkStickyFault); -// -// // Update odometry inputs -// inputs.odometryTimestamps = -// timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); -// inputs.odometryDrivePositionsRad = -// drivePositionQueue.stream().mapToDouble((Double value) -> value).toArray(); -// inputs.odometryTurnPositions = -// turnPositionQueue.stream() -// .map((Double value) -> new Rotation2d(value).minus(zeroRotation)) -// .toArray(Rotation2d[]::new); -// timestampQueue.clear(); -// drivePositionQueue.clear(); -// turnPositionQueue.clear(); -// } -// -// @Override -// public void setDriveOpenLoop(double output) { -// driveSpark.setVoltage(output); -// } -// -// @Override -// public void setTurnOpenLoop(double output) { -// turnSpark.setVoltage(output); -// } -// -// @Override -// public void setDriveVelocity(double velocityRadPerSec) { -// double ffVolts = driveKs * Math.signum(velocityRadPerSec) + driveKv * velocityRadPerSec; -// driveController.setReference( -// velocityRadPerSec, -// ControlType.kVelocity, -// ClosedLoopSlot.kSlot0, -// ffVolts, -// ArbFFUnits.kVoltage); -// } -// -// @Override -// public void setTurnPosition(Rotation2d rotation) { -// double setpoint = -// MathUtil.inputModulus( -// rotation.plus(zeroRotation).getRadians(), turnPIDMinInput, turnPIDMaxInput); -// turnController.setReference(setpoint, ControlType.kPosition); -// } -//} +package frc.robot.subsystems.drive; + +import static frc.lib.utils.SparkUtil.*; +import static frc.robot.Schematic.*; +import static frc.robot.subsystems.drive.DriveConstants.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.*; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkLowLevel.ControlType; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import org.wpilib.math.util.MathUtil; +import org.wpilib.math.filter.Debouncer; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.util.Units; +import org.wpilib.units.measure.Angle; +import java.util.Queue; +import java.util.function.DoubleSupplier; + +/** + * Module IO implementation for Spark Flex drive motor controller, Spark Max turn motor controller, + * and duty cycle absolute encoder. + */ +public class ModuleIOSpark implements ModuleIO { + private final Rotation2d zeroRotation; + + // Hardware objects + private final SparkBase driveSpark; + private final SparkBase turnSpark; + private final RelativeEncoder driveEncoder; + private final RelativeEncoder turnEncoder; + private final CANcoder turnEncoderAbsolute; + + // Input from turn encoder absolute + private final StatusSignal turnPositionAbsolute; + + // Closed loop controllers + private final SparkClosedLoopController driveController; + private final SparkClosedLoopController turnController; + + // Queue inputs from odometry thread + private final Queue timestampQueue; + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + // Connection debouncers + private final Debouncer driveConnectedDebounce = new Debouncer(0.5); + private final Debouncer turnConnectedDebounce = new Debouncer(0.5); + + public ModuleIOSpark(int module) { + zeroRotation = + switch (module) { + case 0 -> frontLeftZeroRotation; + case 1 -> frontRightZeroRotation; + case 2 -> backLeftZeroRotation; + case 3 -> backRightZeroRotation; + default -> Rotation2d.kZero; + }; + driveSpark = + new SparkMax( + switch (module) { + case 0 -> frontLeftDriveCanId; + case 1 -> frontRightDriveCanId; + case 2 -> backLeftDriveCanId; + case 3 -> backRightDriveCanId; + default -> 0; + }, + MotorType.kBrushless); + turnSpark = + new SparkMax( + switch (module) { + case 0 -> frontLeftTurnCanId; + case 1 -> frontRightTurnCanId; + case 2 -> backLeftTurnCanId; + case 3 -> backRightTurnCanId; + default -> 0; + }, + MotorType.kBrushless); + driveEncoder = driveSpark.getEncoder(); + turnEncoder = turnSpark.getEncoder(); + turnEncoderAbsolute = + new CANcoder( + switch (module) { + case 0 -> frontLeftAbsoluteEncoderCanId; + case 1 -> frontRightAbsoluteEncoderCanId; + case 2 -> backLeftAbsoluteEncoderCanId; + case 3 -> backRightAbsoluteEncoderCanId; + default -> 0; + }); + driveController = driveSpark.getClosedLoopController(); + turnController = turnSpark.getClosedLoopController(); + + // Configure drive motor + var driveConfig = new SparkMaxConfig(); + driveConfig + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(driveMotorCurrentLimit) + .voltageCompensation(12.0); + driveConfig + .encoder + .positionConversionFactor(driveEncoderPositionFactor) + .velocityConversionFactor(driveEncoderVelocityFactor) + .uvwMeasurementPeriod(10) + .uvwAverageDepth(2); + driveConfig + .closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pidf( + driveKp, 0.0, + driveKd, 0.0); + driveConfig + .signals + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) + .primaryEncoderVelocityAlwaysOn(true) + .primaryEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20); + tryUntilOk( + driveSpark, + 5, + () -> + driveSpark.configure( + driveConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + tryUntilOk(driveSpark, 5, () -> driveEncoder.setPosition(0.0)); + + // Configure turn motor + var turnConfig = new SparkMaxConfig(); + turnConfig + .inverted(turnInverted) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(turnMotorCurrentLimit) + .voltageCompensation(12.0); + turnConfig + .encoder + .positionConversionFactor(turnEncoderPositionFactor) + .velocityConversionFactor(turnEncoderVelocityFactor) + .uvwAverageDepth(2); + turnConfig + .closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .positionWrappingEnabled(true) + .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) + .pidf(turnKp, 0.0, turnKd, 0.0); + turnConfig + .signals + .absoluteEncoderPositionAlwaysOn(true) + .absoluteEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) + .absoluteEncoderVelocityAlwaysOn(true) + .absoluteEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20); + tryUntilOk( + turnSpark, + 5, + () -> + turnSpark.configure( + turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + + // Configure CANCoder + CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); + cancoderConfig.MagnetSensor.MagnetOffset = 0; // This is done later in the code + cancoderConfig.MagnetSensor.SensorDirection = + turnEncoderInverted + ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive; + turnEncoderAbsolute.getConfigurator().apply(cancoderConfig); + turnPositionAbsolute = turnEncoderAbsolute.getPosition(); + + BaseStatusSignal.setUpdateFrequencyForAll(50.0, turnPositionAbsolute); + + // Create odometry queues + timestampQueue = OdometryThread.getInstance().makeTimestampQueue(); + drivePositionQueue = + OdometryThread.getInstance().registerSignal(driveSpark, driveEncoder::getPosition); + turnPositionQueue = + OdometryThread.getInstance().registerSignal(turnSpark, turnEncoder::getPosition); + + // Initialize turn relative encoder + tryUntilOk( + turnSpark, + 5, + () -> + turnEncoder.setPosition( + Units.rotationsToRadians( + turnEncoderAbsolute.getAbsolutePosition().getValueAsDouble()))); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + BaseStatusSignal.refreshAll(turnPositionAbsolute); + inputs.turnPositionAbsolute = Units.rotationsToRadians(turnPositionAbsolute.getValueAsDouble()); + // Update drive inputs + sparkStickyFault = false; + ifOk(driveSpark, driveEncoder::getPosition, (value) -> inputs.drivePositionRad = value); + ifOk(driveSpark, driveEncoder::getVelocity, (value) -> inputs.driveVelocityRadPerSec = value); + ifOk( + driveSpark, + new DoubleSupplier[] {driveSpark::getAppliedOutput, driveSpark::getBusVoltage}, + (values) -> inputs.driveAppliedVolts = values[0] * values[1]); + ifOk(driveSpark, driveSpark::getOutputCurrent, (value) -> inputs.driveCurrentAmps = value); + inputs.driveConnected = driveConnectedDebounce.calculate(!sparkStickyFault); + + // Update turn inputs + sparkStickyFault = false; + ifOk( + turnSpark, + turnEncoder::getPosition, + (value) -> inputs.turnPosition = new Rotation2d(value).minus(zeroRotation)); + ifOk(turnSpark, turnEncoder::getVelocity, (value) -> inputs.turnVelocityRadPerSec = value); + ifOk( + turnSpark, + new DoubleSupplier[] {turnSpark::getAppliedOutput, turnSpark::getBusVoltage}, + (values) -> inputs.turnAppliedVolts = values[0] * values[1]); + ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.turnCurrentAmps = value); + inputs.turnConnected = turnConnectedDebounce.calculate(!sparkStickyFault); + + // Update odometry inputs + inputs.odometryTimestamps = + timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryDrivePositionsRad = + drivePositionQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream() + .map((Double value) -> new Rotation2d(value).minus(zeroRotation)) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveOpenLoop(double output) { + driveSpark.setVoltage(output); + } + + @Override + public void setTurnOpenLoop(double output) { + turnSpark.setVoltage(output); + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + double ffVolts = driveKs * Math.signum(velocityRadPerSec) + driveKv * velocityRadPerSec; + driveController.setReference( + velocityRadPerSec, + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + ffVolts, + ArbFFUnits.kVoltage); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + double setpoint = + MathUtil.inputModulus( + rotation.plus(zeroRotation).getRadians(), turnPIDMinInput, turnPIDMaxInput); + turnController.setReference(setpoint, ControlType.kPosition); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonSpark.java index 79e459805..0b36ee8ef 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonSpark.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.drive; import static frc.lib.utils.PhoenixUtil.*; +import static frc.lib.utils.SparkUtil.*; import static frc.robot.Schematic.*; import static frc.robot.subsystems.drive.DriveConstants.*; @@ -9,25 +10,26 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.TorqueCurrentFOC; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.controls.*; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.ParentDevice; 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 java.util.ArrayList; -import java.util.List; -import java.util.Queue; -import java.util.concurrent.ConcurrentLinkedQueue; - +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkLowLevel.ControlType; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import org.wpilib.math.util.MathUtil; import org.wpilib.math.filter.Debouncer; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.util.Units; @@ -35,874 +37,264 @@ import org.wpilib.units.measure.AngularVelocity; import org.wpilib.units.measure.Current; import org.wpilib.units.measure.Voltage; - +import frc.lib.utils.PhoenixUtil; +import java.util.Queue; +import java.util.function.DoubleSupplier; /** - * Module IO implementation for Talon FX drive motor controller, - * Talon FX turn motor controller, and CANcoder absolute encoder. + * Module IO implementation for Spark Flex drive motor controller, Spark Max turn motor controller, + * and duty cycle absolute encoder. */ public class ModuleIOTalonSpark implements ModuleIO { - - // ========================================================================= - // Phoenix odometry thread - // ========================================================================= - // - // This is implemented here instead of using AdvantageKit's - // PhoenixOdometryThread because that class is not present in this project. - // - // Phoenix signals are updated at the configured odometry frequency and - // copied into queues. The normal ModuleIO updateInputs() method consumes - // those queues. - // - // ========================================================================= - - private static final class PhoenixOdometryThread extends Thread { - - private static final PhoenixOdometryThread INSTANCE = - new PhoenixOdometryThread(); - - private final List signals = new ArrayList<>(); - private final List> signalQueues = new ArrayList<>(); - private final List> timestampQueues = new ArrayList<>(); - - private PhoenixOdometryThread() { - super("PhoenixOdometryThread"); - - setDaemon(true); - start(); - } - - public static PhoenixOdometryThread getInstance() { - return INSTANCE; - } - - public synchronized Queue makeTimestampQueue() { - Queue queue = new ConcurrentLinkedQueue<>(); - - timestampQueues.add(queue); - - return queue; - } - - public synchronized Queue registerSignal( - StatusSignal signal) { - - Queue queue = new ConcurrentLinkedQueue<>(); - - signals.add(signal); - signalQueues.add(queue); - - return queue; - } - - @Override - public void run() { - - while (!Thread.currentThread().isInterrupted()) { - - BaseStatusSignal[] signalsSnapshot; - - synchronized (this) { - signalsSnapshot = - signals.toArray(new BaseStatusSignal[0]); - } - - if (signalsSnapshot.length == 0) { - try { - Thread.sleep(1); - } catch (InterruptedException e) { - Thread.currentThread().interrupt(); - break; - } - - continue; - } - - /* - * Wait until a new sample is available from all registered - * Phoenix signals. - * - * This is preferable to using Timer.getFPGATimestamp() because - * Phoenix provides timestamps associated with the received CAN data. - */ - BaseStatusSignal.waitForAll( - 0.1, - signalsSnapshot); - - /* - * Use the timestamp from the first Phoenix signal. - * - * All odometry signals are configured at the same frequency and - * are sampled together by waitForAll(). - */ - double timestamp = - signalsSnapshot[0] - .getTimestamp() - .getTime(); - - synchronized (this) { - - /* - * Every ModuleIO instance gets its own timestamp queue. - */ - for (Queue queue : timestampQueues) { - queue.add(timestamp); - } - - /* - * Copy every registered signal into its corresponding queue. - */ - for (int i = 0; i < signals.size(); i++) { - - signalQueues - .get(i) - .add( - signals - .get(i) - .getValueAsDouble()); - } - } - } - } - } - - - // ========================================================================= - // Module state - // ========================================================================= - private final Rotation2d zeroRotation; - - // ========================================================================= // Hardware objects - // ========================================================================= - private final TalonFX driveTalon; - private final TalonFX turnTalon; + private final SparkBase turnSpark; + private final RelativeEncoder turnEncoder; private final CANcoder turnEncoderAbsolute; - - // ========================================================================= // Voltage control requests - // ========================================================================= - - private final VoltageOut voltageRequest = - new VoltageOut(0); - - private final PositionVoltage positionVoltageRequest = - new PositionVoltage(0.0); - - private final VelocityVoltage velocityVoltageRequest = - new VelocityVoltage(0.0); + private final VoltageOut voltageRequest = new VoltageOut(0); + private final PositionVoltage positionVoltageRequest = new PositionVoltage(0.0); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); - - // ========================================================================= // Torque-current control requests - // ========================================================================= - - private final TorqueCurrentFOC torqueCurrentRequest = - new TorqueCurrentFOC(0); - + private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); private final PositionTorqueCurrentFOC positionTorqueCurrentRequest = new PositionTorqueCurrentFOC(0.0); - private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = new VelocityTorqueCurrentFOC(0.0); - - // ========================================================================= - // Timestamp inputs from Phoenix odometry thread - // ========================================================================= - - private final Queue timestampQueue; - - - // ========================================================================= - // Drive motor inputs - // ========================================================================= - + // Inputs from drive motor private final StatusSignal drivePosition; - private final Queue drivePositionQueue; - private final StatusSignal driveVelocity; private final StatusSignal driveAppliedVolts; private final StatusSignal driveCurrent; + // Input from turn encoder absolute + private final StatusSignal turnPositionAbsolute; - // ========================================================================= - // Turn motor inputs - // ========================================================================= + // Closed loop controllers + private final SparkClosedLoopController turnController; - private final StatusSignal turnPosition; + // Queue inputs from odometry thread + private final Queue timestampQueue; + private final Queue drivePositionQueue; private final Queue turnPositionQueue; - private final StatusSignal turnVelocity; - private final StatusSignal turnAppliedVolts; - private final StatusSignal turnCurrent; - - - // ========================================================================= - // Absolute CANcoder input - // ========================================================================= - - private final StatusSignal turnAbsolutePosition; - - - // ========================================================================= // Connection debouncers - // ========================================================================= - - private final Debouncer driveConnectedDebounce = - new Debouncer(0.5); - - private final Debouncer turnConnectedDebounce = - new Debouncer(0.5); - - private final Debouncer turnEncoderConnectedDebounce = - new Debouncer(0.5); - - - // ========================================================================= - // Constructor - // ========================================================================= + private final Debouncer driveConnectedDebounce = new Debouncer(0.5); + private final Debouncer turnConnectedDebounce = new Debouncer(0.5); public ModuleIOTalonSpark(int module) { - - // ----------------------------------------------------------------------- - // Module-specific zero rotation - // ----------------------------------------------------------------------- - zeroRotation = switch (module) { case 0 -> frontLeftZeroRotation; case 1 -> frontRightZeroRotation; case 2 -> backLeftZeroRotation; case 3 -> backRightZeroRotation; - default -> Rotation2d.ZERO; - }; - - - // ----------------------------------------------------------------------- - // CAN IDs - // ----------------------------------------------------------------------- - - int driveCanId = - switch (module) { - case 0 -> frontLeftDriveCanId; - case 1 -> frontRightDriveCanId; - case 2 -> backLeftDriveCanId; - case 3 -> backRightDriveCanId; - default -> 0; - }; - - int turnCanId = - switch (module) { - case 0 -> frontLeftTurnCanId; - case 1 -> frontRightTurnCanId; - case 2 -> backLeftTurnCanId; - case 3 -> backRightTurnCanId; - default -> 0; - }; - - int encoderCanId = - switch (module) { - case 0 -> frontLeftAbsoluteEncoderCanId; - case 1 -> frontRightAbsoluteEncoderCanId; - case 2 -> backLeftAbsoluteEncoderCanId; - case 3 -> backRightAbsoluteEncoderCanId; - default -> 0; + default -> Rotation2d.kZero; }; - - - // ----------------------------------------------------------------------- - // Hardware - // ----------------------------------------------------------------------- - driveTalon = new TalonFX( - driveCanId, - kCANBus); - - turnTalon = - new TalonFX( - turnCanId, - kCANBus); - + switch (module) { + case 0 -> frontLeftDriveCanId; + case 1 -> frontRightDriveCanId; + case 2 -> backLeftDriveCanId; + case 3 -> backRightDriveCanId; + default -> 0; + }); + turnSpark = + new SparkMax( + switch (module) { + case 0 -> frontLeftTurnCanId; + case 1 -> frontRightTurnCanId; + case 2 -> backLeftTurnCanId; + case 3 -> backRightTurnCanId; + default -> 0; + }, + MotorType.kBrushless); + turnEncoder = turnSpark.getEncoder(); turnEncoderAbsolute = new CANcoder( - encoderCanId, - kCANBus); - - - // ========================================================================= - // DRIVE MOTOR - // ========================================================================= - - var driveConfig = - new TalonFXConfiguration(); - - driveConfig.MotorOutput.NeutralMode = - NeutralModeValue.Brake; - - driveConfig.Feedback.SensorToMechanismRatio = - driveMotorReduction; - - driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = - driveMotorCurrentLimit; - - driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = - -driveMotorCurrentLimit; - - driveConfig.CurrentLimits.StatorCurrentLimit = - driveMotorCurrentLimit; - - driveConfig.CurrentLimits.StatorCurrentLimitEnable = - true; - - driveConfig.MotorOutput.Inverted = - InvertedValue.CounterClockwise_Positive; - - - // ----------------------------------------------------------------------- - // Drive PID + feedforward - // ----------------------------------------------------------------------- - - var driveSlot0 = - new Slot0Configs(); - - driveSlot0.kP = - driveKp; - - driveSlot0.kI = - 0.0; - - driveSlot0.kD = - driveKd; - - driveSlot0.kS = - driveKs; - - driveSlot0.kV = - driveKv; - - driveSlot0.kA = - driveKa; - - driveConfig.Slot0 = - driveSlot0; - - - // ----------------------------------------------------------------------- - // Apply drive configuration - // ----------------------------------------------------------------------- - - tryUntilOk( - 5, - () -> - driveTalon - .getConfigurator() - .apply( - driveConfig, - 0.25)); - + switch (module) { + case 0 -> frontLeftAbsoluteEncoderCanId; + case 1 -> frontRightAbsoluteEncoderCanId; + case 2 -> backLeftAbsoluteEncoderCanId; + case 3 -> backRightAbsoluteEncoderCanId; + default -> 0; + }); + turnController = turnSpark.getClosedLoopController(); + + var driveConfig = new TalonFXConfiguration(); + driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + driveConfig.Feedback.SensorToMechanismRatio = driveMotorReduction; + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = driveMotorCurrentLimit; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -driveMotorCurrentLimit; + driveConfig.CurrentLimits.StatorCurrentLimit = driveMotorCurrentLimit; + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + var slot0config = new Slot0Configs(); + slot0config.kP = driveKp; + slot0config.kI = 0; + slot0config.kD = driveKd; + slot0config.kS = driveKs; + slot0config.kV = driveKv; + slot0config.kA = driveKa; + slot0config.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign; + driveConfig.withSlot0(slot0config); + PhoenixUtil.tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); + PhoenixUtil.tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); + + // Configure turn motor + var turnConfig = new SparkMaxConfig(); + turnConfig + .inverted(turnInverted) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(turnMotorCurrentLimit) + .voltageCompensation(12.0); + turnConfig + .encoder + .positionConversionFactor(turnEncoderPositionFactor) + .velocityConversionFactor(turnEncoderVelocityFactor) + .uvwAverageDepth(2); + turnConfig + .closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .positionWrappingEnabled(true) + .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) + .pidf(turnKp, 0.0, turnKd, 0.0); + turnConfig + .signals + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) + .primaryEncoderVelocityAlwaysOn(true) + .primaryEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20); tryUntilOk( + turnSpark, 5, () -> - driveTalon - .setPosition( - 0.0, - 0.25)); - - - // ========================================================================= - // TURN MOTOR - // ========================================================================= - - var turnConfig = - new TalonFXConfiguration(); - - - turnConfig.MotorOutput.NeutralMode = - NeutralModeValue.Brake; - - - // ----------------------------------------------------------------------- - // Steering feedback = CANcoder - // ----------------------------------------------------------------------- - - turnConfig.Feedback.FeedbackRemoteSensorID = - encoderCanId; - - turnConfig.Feedback.FeedbackSensorSource = - FeedbackSensorSourceValue.FusedCANcoder; - - turnConfig.Feedback.RotorToSensorRatio = - turnMotorReduction; - - - // ----------------------------------------------------------------------- - // Steering PID - // ----------------------------------------------------------------------- - - var turnSlot0 = - new Slot0Configs(); - - turnSlot0.kP = - turnKp; - - turnSlot0.kI = - 0.0; - - turnSlot0.kD = - turnKd; - - turnConfig.Slot0 = - turnSlot0; - - - // ----------------------------------------------------------------------- - // Steering motor direction - // ----------------------------------------------------------------------- - - turnConfig.MotorOutput.Inverted = - InvertedValue.CounterClockwise_Positive; - - - // ----------------------------------------------------------------------- - // Motion Magic - // ----------------------------------------------------------------------- - - turnConfig.MotionMagic.MotionMagicCruiseVelocity = - 100.0 / turnMotorReduction; - - turnConfig.MotionMagic.MotionMagicAcceleration = - turnConfig - .MotionMagic - .MotionMagicCruiseVelocity - / 0.100; - - - // ----------------------------------------------------------------------- - // Allow steering to cross the 0/360 boundary - // ----------------------------------------------------------------------- - - turnConfig.ClosedLoopGeneral.ContinuousWrap = - true; - - - // ----------------------------------------------------------------------- - // Apply turn configuration - // ----------------------------------------------------------------------- - - tryUntilOk( - 5, - () -> - turnTalon - .getConfigurator() - .apply( - turnConfig, - 0.25)); - - - // ========================================================================= - // CANCODER - // ========================================================================= - - var cancoderConfig = - new CANcoderConfiguration(); - - - /* - * Zero offset is handled using zeroRotation below. - */ - cancoderConfig.MagnetSensor.MagnetOffset = - 0.0; + turnSpark.configure( + turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + // Configure CANCoder + CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); + cancoderConfig.MagnetSensor.MagnetOffset = 0; // This is done later in the code cancoderConfig.MagnetSensor.SensorDirection = turnEncoderInverted ? SensorDirectionValue.Clockwise_Positive : SensorDirectionValue.CounterClockwise_Positive; + turnEncoderAbsolute.getConfigurator().apply(cancoderConfig); + // Create timestamp queue + timestampQueue = OdometryThread.getInstance().makeTimestampQueue(); - tryUntilOk( - 5, - () -> - turnEncoderAbsolute - .getConfigurator() - .apply( - cancoderConfig, - 0.25)); - - - // ========================================================================= - // PHOENIX ODOMETRY - // ========================================================================= - - PhoenixOdometryThread odometryThread = - PhoenixOdometryThread.getInstance(); - - timestampQueue = - odometryThread.makeTimestampQueue(); - - - // ----------------------------------------------------------------------- - // Drive signals - // ----------------------------------------------------------------------- - - drivePosition = - driveTalon.getPosition(); - - drivePositionQueue = - odometryThread.registerSignal( - drivePosition.clone()); - - driveVelocity = - driveTalon.getVelocity(); - - driveAppliedVolts = - driveTalon.getMotorVoltage(); - - driveCurrent = - driveTalon.getStatorCurrent(); - - - // ----------------------------------------------------------------------- - // Turn signals - // ----------------------------------------------------------------------- - - turnPosition = - turnTalon.getPosition(); + // Create drive status signals + drivePosition = driveTalon.getPosition(); + drivePositionQueue = OdometryThread.getInstance().registerSignal(driveTalon.getPosition()); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getStatorCurrent(); + // Create turn status signals + turnPositionAbsolute = turnEncoderAbsolute.getPosition(); turnPositionQueue = - odometryThread.registerSignal( - turnPosition.clone()); - - turnVelocity = - turnTalon.getVelocity(); - - turnAppliedVolts = - turnTalon.getMotorVoltage(); - - turnCurrent = - turnTalon.getStatorCurrent(); - - - // ----------------------------------------------------------------------- - // Absolute encoder - // ----------------------------------------------------------------------- - - turnAbsolutePosition = - turnEncoderAbsolute.getAbsolutePosition(); - - - // ========================================================================= - // SIGNAL UPDATE FREQUENCIES - // ========================================================================= - - /* - * These are the high-frequency signals used for odometry. - */ - BaseStatusSignal.setUpdateFrequencyForAll( - odometryFrequency, - drivePosition, - turnPosition); - + OdometryThread.getInstance().registerSignal(turnSpark, turnEncoder::getPosition); - /* - * These signals are only needed for normal robot inputs. - */ + // Configure periodic frames + BaseStatusSignal.setUpdateFrequencyForAll(odometryFrequency, drivePosition); BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); + 50.0, driveVelocity, driveAppliedVolts, driveCurrent, turnPositionAbsolute); + ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnEncoderAbsolute); - - // ----------------------------------------------------------------------- - // Optimize CAN utilization - // ----------------------------------------------------------------------- - - ParentDevice.optimizeBusUtilizationForAll( - driveTalon, - turnTalon, - turnEncoderAbsolute); + // Initialize turn relative encoder + tryUntilOk( + turnSpark, + 5, + () -> + turnEncoder.setPosition( + Units.rotationsToRadians( + turnEncoderAbsolute.getAbsolutePosition().getValueAsDouble()))); } - - // ========================================================================= - // INPUT UPDATE - // ========================================================================= - @Override - public void updateInputs( - ModuleIOInputs inputs) { - - // ----------------------------------------------------------------------- - // Refresh drive signals - // ----------------------------------------------------------------------- - + public void updateInputs(ModuleIOInputs inputs) { + // Refresh all signals var driveStatus = - BaseStatusSignal.refreshAll( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveCurrent); - - - // ----------------------------------------------------------------------- - // Refresh steering signals - // ----------------------------------------------------------------------- - - var turnStatus = - BaseStatusSignal.refreshAll( - turnPosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); - - - // ----------------------------------------------------------------------- - // Refresh CANcoder - // ----------------------------------------------------------------------- - - var turnEncoderStatus = - BaseStatusSignal.refreshAll( - turnAbsolutePosition); - - - // ========================================================================= - // DRIVE INPUTS - // ========================================================================= - - inputs.driveConnected = - driveConnectedDebounce.calculate( - driveStatus.isOK()); - - - inputs.drivePositionRad = - Units.rotationsToRadians( - drivePosition.getValueAsDouble()); - - - inputs.driveVelocityRadPerSec = - Units.rotationsToRadians( - driveVelocity.getValueAsDouble()); - - - inputs.driveAppliedVolts = - driveAppliedVolts.getValueAsDouble(); - - - inputs.driveCurrentAmps = - driveCurrent.getValueAsDouble(); - - - // ========================================================================= - // TURN INPUTS - // ========================================================================= - - inputs.turnConnected = - turnConnectedDebounce.calculate( - turnStatus.isOK()); - - - /* - * The TalonFX is configured to use the fused CANcoder - * as its steering feedback source. - * - * This is the primary steering position used by the module. - */ - inputs.turnPosition = - Rotation2d.fromRotations( - turnPosition.getValueAsDouble()) - .minus( - zeroRotation); - - - inputs.turnVelocityRadPerSec = - Units.rotationsToRadians( - turnVelocity.getValueAsDouble()); - - - inputs.turnAppliedVolts = - turnAppliedVolts.getValueAsDouble(); - - - inputs.turnCurrentAmps = - turnCurrent.getValueAsDouble(); - - - // ========================================================================= - // ABSOLUTE CANCODER - // ========================================================================= - - /* - * Keep the absolute CANcoder position separate from turnPosition. - * - * turnPosition is the fused TalonFX/CANcoder steering position. - * turnAbsolutePosition is the raw absolute CANcoder measurement. - */ - inputs.turnPosition = - Rotation2d.fromRotations( - turnAbsolutePosition.getValueAsDouble()) - .minus( - zeroRotation); - - - // ========================================================================= - // ODOMETRY - // ========================================================================= - - /* - * Copy the high-frequency Phoenix samples into the module inputs. - */ - + BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); + var turnEncoderStatus = BaseStatusSignal.refreshAll(turnPositionAbsolute); + + // Update drive inputs + inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); + + // Update turn inputs + sparkStickyFault = false; + ifOk( + turnSpark, + turnEncoder::getPosition, + (value) -> inputs.turnPosition = new Rotation2d(value).minus(zeroRotation)); + ifOk(turnSpark, turnEncoder::getVelocity, (value) -> inputs.turnVelocityRadPerSec = value); + ifOk( + turnSpark, + new DoubleSupplier[] {turnSpark::getAppliedOutput, turnSpark::getBusVoltage}, + (values) -> inputs.turnAppliedVolts = values[0] * values[1]); + ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.turnCurrentAmps = value); + inputs.turnPositionAbsolute = Units.rotationsToRadians(turnPositionAbsolute.getValueAsDouble()); + inputs.turnConnected = turnConnectedDebounce.calculate(!sparkStickyFault); + + // Update odometry inputs inputs.odometryTimestamps = - timestampQueue - .stream() - .mapToDouble( - (Double value) -> value) - .toArray(); - - + timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); inputs.odometryDrivePositionsRad = - drivePositionQueue - .stream() - .mapToDouble( - (Double value) -> - Units.rotationsToRadians(value)) + drivePositionQueue.stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value)) .toArray(); - - inputs.odometryTurnPositions = - turnPositionQueue - .stream() - .map( - (Double value) -> - Rotation2d - .fromRotations(value) - .minus(zeroRotation)) - .toArray( - Rotation2d[]::new); - - - // ----------------------------------------------------------------------- - // Clear queues after copying them - // ----------------------------------------------------------------------- - + turnPositionQueue.stream() + .map((Double value) -> new Rotation2d(value).minus(zeroRotation)) + .toArray(Rotation2d[]::new); timestampQueue.clear(); drivePositionQueue.clear(); turnPositionQueue.clear(); } - - // ========================================================================= - // DRIVE OPEN LOOP - // ========================================================================= - @Override - public void setDriveOpenLoop( - double output) { - + public void setDriveOpenLoop(double output) { driveTalon.setControl( switch (driveClosedLoopOutput) { - - case Voltage -> - voltageRequest - .withOutput(output); - - case TorqueCurrentFOC -> - torqueCurrentRequest - .withOutput(output); + case Voltage -> voltageRequest.withOutput(output); + case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); }); } - - // ========================================================================= - // TURN OPEN LOOP - // ========================================================================= - @Override - public void setTurnOpenLoop( - double output) { - - turnTalon.setControl( - switch (driveClosedLoopOutput) { - - case Voltage -> - voltageRequest - .withOutput(output); - - case TorqueCurrentFOC -> - torqueCurrentRequest - .withOutput(output); - }); + public void setTurnOpenLoop(double output) { + turnSpark.setVoltage(output); } - - // ========================================================================= - // DRIVE VELOCITY - // ========================================================================= - @Override - public void setDriveVelocity( - double velocityRadPerSec) { - - double velocityRotPerSec = - Units.radiansToRotations( - velocityRadPerSec); - - + public void setDriveVelocity(double velocityRadPerSec) { + double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); driveTalon.setControl( switch (driveClosedLoopOutput) { - - case Voltage -> - velocityVoltageRequest - .withVelocity( - velocityRotPerSec); - - case TorqueCurrentFOC -> - velocityTorqueCurrentRequest - .withVelocity( - velocityRotPerSec); + case Voltage -> velocityVoltageRequest.withVelocity(velocityRotPerSec); + case TorqueCurrentFOC -> velocityTorqueCurrentRequest.withVelocity(velocityRotPerSec); }); } - - // ========================================================================= - // TURN POSITION - // ========================================================================= - @Override - public void setTurnPosition( - Rotation2d rotation) { - - /* - * Add the module's physical zero offset before - * sending the target to Phoenix. - */ - Rotation2d target = - rotation.plus( - zeroRotation); - - - turnTalon.setControl( - switch (driveClosedLoopOutput) { - - case Voltage -> - positionVoltageRequest - .withPosition( - target.getRotations()); - - case TorqueCurrentFOC -> - positionTorqueCurrentRequest - .withPosition( - target.getRotations()); - }); + public void setTurnPosition(Rotation2d rotation) { + double setpoint = + MathUtil.inputModulus( + rotation.plus(zeroRotation).getRadians(), turnPIDMinInput, turnPIDMaxInput); + turnController.setReference(setpoint, ControlType.kPosition); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/drive/OdometryThread.java b/src/main/java/frc/robot/subsystems/drive/OdometryThread.java index 7aeeff867..505daa3bb 100644 --- a/src/main/java/frc/robot/subsystems/drive/OdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/OdometryThread.java @@ -137,7 +137,7 @@ public void run() { Drive.odometryLock.lock(); try { // Get sample timestamp - double timestamp = RobotController.getTime() / 1e6; + double timestamp = RobotController.getFPGATime() / 1e6; // Read Spark values, mark invalid in case of error double[] sparkValues = new double[sparkSignals.size()]; From cf914c50b4684366118df52f4c0ea779b34039d6 Mon Sep 17 00:00:00 2001 From: EJainDev Date: Mon, 5 Oct 2026 20:43:39 -0400 Subject: [PATCH 15/15] feat: magic carpet 27 --- .../subsystems/magicCarpet/MagicCarpet.java | 18 +++++++++++------- .../magicCarpet/MagicCarpetSparkMax.java | 14 +++++++------- 2 files changed, 18 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java index 8802c0bec..13da0f95c 100644 --- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java +++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java @@ -3,9 +3,10 @@ import org.wpilib.command3.Command; import frc.robot.RobotState; import org.littletonrobotics.junction.Logger; -import org.wpilib.command3.Mechanism; -public class MagicCarpet implements Mechanism { +import frc.lib.SubsystemBase; + +public class MagicCarpet extends SubsystemBase { private final MagicCarpetIO io; private final MagicCarpetIOInputsAutoLogged inputs = new MagicCarpetIOInputsAutoLogged(); public boolean manualRun; @@ -26,19 +27,22 @@ public MagicCarpet(MagicCarpetIO io) { * @return A command to run the magic carpet at a constant speed */ public Command run() { - return setManualControl(true) - .andThen(Commands.run(() -> io.setSpeed(MagicCarpetConstants.BELT_SPEED), this)) - .finallyDo(() -> setManualControl(false).schedule()) - .withName("start"); + return this.run( + coro -> { + io.setSpeed(MagicCarpetConstants.BELT_SPEED); + coro.fork(setManualControl(false)); + } + ).named("MagicCarpet Start"); } + /** * Control whether to run magic carpet through commands or periodic * * @param manual Call run manually instead of through periodic */ public Command setManualControl(boolean manual) { - return Commands.runOnce(() -> this.manualRun = manual, this); + return this.run(_ -> this.manualRun = manual).named("MagicCarpet SetManualControl"); } @Override diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetSparkMax.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetSparkMax.java index 87ad37118..358295b05 100644 --- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetSparkMax.java +++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetSparkMax.java @@ -10,6 +10,7 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; +import org.wpilib.hardware.bus.CANPort; import org.wpilib.math.util.MathUtil; import frc.robot.Schematic; @@ -20,7 +21,7 @@ public class MagicCarpetSparkMax implements MagicCarpetIO { public MagicCarpetSparkMax() { - motor = new SparkMax(Schematic.magicCarpetCanId, MotorType.kBrushless); + motor = new SparkMax(CANPort.CAN_D0, Schematic.magicCarpetCanId, MotorType.kBrushless); SparkMaxConfig config = new SparkMaxConfig(); config @@ -37,15 +38,14 @@ public MagicCarpetSparkMax() { @Override public void updateInputs(MagicCarpetIOInputs inputs) { // Called every 20 ms by subsystem periodic - inputs.appliedVolts = motor.getAppliedOutput() * motor.getBusVoltage(); - inputs.currentAmps = motor.getOutputCurrent(); - inputs.motorVelocity = encoder.getVelocity(); + inputs.appliedVolts = motor.getAppliedOutput().get() * motor.getBusVoltage().get(); + inputs.currentAmps = motor.getOutputCurrent().get(); + inputs.motorVelocity = encoder.getVelocity().get(); } @Override public void setSpeed(double speed) { - - motor.set(MathUtil.clamp(speed, 0.0, 1.0)); // so the is between 0 and 1, 1==100% + motor.setVoltage(MagicCarpetConstants.CURRENT_LIMIT * Math.clamp(speed, 0.0, 1.0)); } /** @@ -53,6 +53,6 @@ public void setSpeed(double speed) { */ @Override public void stop() { - motor.set(0.0); + motor.setVoltage(0); } }