diff --git a/.github/workflows/build-robot-code.yml b/.github/workflows/build-robot-code.yml deleted file mode 100644 index 2b283a1..0000000 --- a/.github/workflows/build-robot-code.yml +++ /dev/null @@ -1,17 +0,0 @@ -name: Build Robot Code - -on: - push: - -jobs: - build: - runs-on: ubuntu-latest - container: wpilib/roborio-cross-ubuntu:2023-22.04 - steps: - - uses: actions/checkout@v3 - - name: Add repository to git safe directories - run: git config --global --add safe.directory $GITHUB_WORKSPACE - - name: Grant execute permission for gradlew - run: chmod +x gradlew - - name: Compile and run tests on robot code - run: ./gradlew build -x spotlessCheck \ No newline at end of file diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 0000000..ba0370f --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,18 @@ +name: Build + +on: + push: + pull_request: + +jobs: + build: + name: Build + runs-on: ubuntu-latest + container: wpilib/roborio-cross-ubuntu:2024-22.04 + steps: + - name: Checkout repository + uses: actions/checkout@v2 + - name: Grant execute permission + run: chmod +x gradlew + - name: Build robot code + run: ./gradlew build diff --git a/.github/workflows/formatting-check.yml b/.github/workflows/lint-format.yml similarity index 82% rename from .github/workflows/formatting-check.yml rename to .github/workflows/lint-format.yml index 24214cf..6a3a84d 100644 --- a/.github/workflows/formatting-check.yml +++ b/.github/workflows/lint-format.yml @@ -1,7 +1,8 @@ -name: Check Formatting +name: Lint and Format on: push: + pull_request: concurrency: group: ${{ github.workflow }}-${{ github.head_ref || github.ref }} @@ -9,7 +10,7 @@ concurrency: jobs: javaformat: - name: "Check Java Format" + name: "Java Formatting" runs-on: ubuntu-22.04 container: wpilib/ubuntu-base:22.04 steps: @@ -20,4 +21,4 @@ jobs: - name: Grant execute permission for gradlew run: chmod +x gradlew - name: Run Java format - run: ./gradlew spotlessCheck \ No newline at end of file + run: ./gradlew spotlessCheck diff --git a/.gitignore b/.gitignore index d45f8ce..0cce371 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,6 @@ +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + ### C++ ### # Prerequisites *.d @@ -106,7 +109,6 @@ Temporary Items !.vscode/tasks.json !.vscode/launch.json !.vscode/extensions.json -!.vscode/style.xml ### Windows ### # Windows thumbnail cache files @@ -156,12 +158,19 @@ gradle-app.setting .settings/ bin/ -.idea +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet -# Simulation GUI and other tools window save files -*-window.json +# Simulation GUI and other tools window save file simgui*.json networktables.json # Version file -src/main/java/frc/generated \ No newline at end of file +src/main/java/frc/generated diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..b8c1920 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,17 @@ +{ + "version": "0.2.0", + "configurations": [ + { + "type": "wpilib", + "name": "WPILib Desktop Debug", + "request": "launch", + "desktop": true + }, + { + "type": "wpilib", + "name": "WPILib roboRIO Debug", + "request": "launch", + "desktop": false + } + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..6543015 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,41 @@ +{ + "java.configuration.updateBuildConfiguration": "automatic", + "java.server.launchMode": "Standard", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true, + "**/*~": true + }, + "java.test.config": [ + { + "name": "WPIlibUnitTests", + "workingDirectory": "${workspaceFolder}/build/jni/release", + "vmargs": [ + "-Djava.library.path=${workspaceFolder}/build/jni/release" + ], + "env": { + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release", + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } + }, + null + ], + "java.test.defaultConfig": "WPIlibUnitTests", + "spotlessGradle.format.enable": true, + "spotlessGradle.diagnostics.enable": false, + "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle", + "[json]": { + "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" + }, + "[java]": { + "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" + } +} diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 5bd203a..2a7d784 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2023", + "projectYear": "2024beta", "teamNumber": 540 -} \ No newline at end of file +} diff --git a/build.gradle b/build.gradle index e5151f0..c9beb0e 100755 --- a/build.gradle +++ b/build.gradle @@ -1,31 +1,37 @@ -//file:noinspection DependencyNotationArgument -import edu.wpi.first.gradlerio.GradleRIOPlugin -import groovy.json.JsonSlurper - plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.4.3" - id 'com.diffplug.spotless' version "6.12.0" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" id "com.peterabeles.gversion" version "1.10" + id "com.diffplug.spotless" version "6.12.0" } -sourceCompatibility = JavaVersion.VERSION_17 -targetCompatibility = JavaVersion.VERSION_17 +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} def ROBOT_MAIN_CLASS = "frc.robot.Main" +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project DeployUtils. deploy { targets { roborio(getTargetTypeClass('RoboRIO')) { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. team = project.frc.getTeamOrDefault(540) 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')) { - gcType = edu.wpi.first.gradlerio.deploy.roborio.GarbageCollectorType.Other - jvmArgs << '-XX:+UseG1GC' } + // Static files artifact frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' @@ -37,12 +43,14 @@ deploy { def deployArtifact = deploy.targets.roborio.artifacts.frcJava +// Set to true to use debug for JNI. wpi.java.debugJni = false +// Set this to true to enable desktop support. def includeDesktopSupport = true +// Configuration for AdvantageKit repositories { - mavenLocal() maven { url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit") credentials { @@ -50,12 +58,15 @@ repositories { password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051" } } + mavenLocal() } -configurations.configureEach { +configurations.all { exclude group: "edu.wpi.first.wpilibj" } +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 4. dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() @@ -74,12 +85,14 @@ dependencies { nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() - testImplementation 'org.junit.jupiter:junit-jupiter-api:5.4.2' - testImplementation 'org.junit.jupiter:junit-jupiter-params:5.4.2' - testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.4.2' + testImplementation 'org.junit.jupiter:junit-jupiter-api:5.8.2' + testImplementation 'org.junit.jupiter:junit-jupiter-params:5.8.2' + testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.8.2' + + implementation "gov.nist.math:jama:1.0.3" - def AdvantageKitJSON = new JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) - annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$AdvantageKitJSON.version" + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" } test { @@ -87,27 +100,31 @@ test { systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' } -wpi.sim.addGui() +// Simulation configuration (e.g. environment variables). +wpi.sim.addGui().defaultEnabled = true wpi.sim.addDriverstation() +// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') +// in order to make them all available at runtime. Also adding the manifest so WPILib +// knows where to look for our Robot Class. jar { - from { - configurations.runtimeClasspath.collect { - it.isDirectory() ? it : zipTree(it) - } - } - manifest GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) + from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from sourceSets.main.allSource + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE } +// Configure jar and deploy tasks deployArtifact.jarTask = jar wpi.java.configureExecutableTasks(jar) wpi.java.configureTestTasks(test) -tasks.withType(JavaCompile).configureEach { +// Configure string concat to always inline compile +tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' } +// Create version file project.compileJava.dependsOn(createVersionFile) gversion { srcDir = "src/main/java/" @@ -118,11 +135,13 @@ gversion { indent = " " } +// Spotless formatting +project.compileJava.dependsOn(spotlessApply) spotless { java { - target fileTree('.') { - include '**/*.java' - exclude '**/build/**', '**/build-*/**', '.idea', '**/generated/**' + target fileTree(".") { + include "**/*.java" + exclude "**/build/**", "**/build-*/**" } toggleOffOn() googleJavaFormat() @@ -131,29 +150,26 @@ spotless { endWithNewline() } groovyGradle { - target fileTree('.') { - include '**/*.gradle' - exclude '**/build/**', '**/build-*/**', '.idea', '**/generated/**' + target fileTree(".") { + include "**/*.gradle" + exclude "**/build/**", "**/build-*/**" } greclipse() indentWithSpaces(4) trimTrailingWhitespace() endWithNewline() } - format 'xml', { - target fileTree('.') { - include '**/*.xml' - exclude '**/build/**', '**/build-*/**', '.idea', '**/generated/**' + json { + target fileTree(".") { + include "**/*.json" + exclude "**/build/**", "**/build-*/**" } - eclipseWtp('xml') - trimTrailingWhitespace() - indentWithSpaces(2) - endWithNewline() + gson().indentWithSpaces(2) } - format 'misc', { - target fileTree('.') { - include '**/*.md', '**/.gitignore' - exclude '**/build/**', '**/build-*/**', '.idea', '**/generated/**' + format "misc", { + target fileTree(".") { + include "**/*.md", "**/.gitignore" + exclude "**/build/**", "**/build-*/**" } trimTrailingWhitespace() indentWithSpaces(2) diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar index 249e583..7f93135 100644 Binary files a/gradle/wrapper/gradle-wrapper.jar and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index c23a1b3..1058752 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,7 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.5.1-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.4-bin.zip +networkTimeout=10000 +validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME zipStorePath=permwrapper/dists diff --git a/gradlew b/gradlew index a69d9cb..1aa94a4 100755 --- a/gradlew +++ b/gradlew @@ -55,7 +55,7 @@ # Darwin, MinGW, and NonStop. # # (3) This script is generated from the Groovy template -# https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt # within the Gradle project. # # You can find Gradle at https://github.com/gradle/gradle/. @@ -80,13 +80,11 @@ do esac done -APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit - -APP_NAME="Gradle" +# This is normally unused +# shellcheck disable=SC2034 APP_BASE_NAME=${0##*/} - -# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. -DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit # Use the maximum available, or set MAX_FD != -1 to use that value. MAX_FD=maximum @@ -133,22 +131,29 @@ location of your Java installation." fi else JAVACMD=java - which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + if ! command -v java >/dev/null 2>&1 + then + die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. Please set the JAVA_HOME variable in your environment to match the location of your Java installation." + fi fi # Increase the maximum file descriptors if we can. if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then case $MAX_FD in #( max*) + # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 MAX_FD=$( ulimit -H -n ) || warn "Could not query maximum file descriptor limit" esac case $MAX_FD in #( '' | soft) :;; #( *) + # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 ulimit -n "$MAX_FD" || warn "Could not set maximum file descriptor limit to $MAX_FD" esac @@ -193,11 +198,15 @@ if "$cygwin" || "$msys" ; then done fi -# Collect all arguments for the java command; -# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of -# shell script including quotes and variable substitutions, so put them in -# double quotes to make sure that they get re-expanded; and -# * put everything else in single quotes, so that it's not re-expanded. + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Collect all arguments for the java command: +# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# and any embedded shellness will be escaped. +# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be +# treated as '${Hostname}' itself on the command line. set -- \ "-Dorg.gradle.appname=$APP_BASE_NAME" \ diff --git a/gradlew.bat b/gradlew.bat index 53a6b23..6689b85 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -26,6 +26,7 @@ if "%OS%"=="Windows_NT" setlocal set DIRNAME=%~dp0 if "%DIRNAME%"=="" set DIRNAME=. +@rem This is normally unused set APP_BASE_NAME=%~n0 set APP_HOME=%DIRNAME% diff --git a/settings.gradle b/settings.gradle index 48c039e..d94f73c 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2023' + String frcYear = '2024' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') @@ -25,3 +25,6 @@ pluginManagement { } } } + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/src/main/java/frc/lib/LoggedIO.java b/src/main/java/frc/lib/LoggedIO.java deleted file mode 100644 index fee545f..0000000 --- a/src/main/java/frc/lib/LoggedIO.java +++ /dev/null @@ -1,17 +0,0 @@ -package frc.lib; - -/** - * Represents an IO interface that can control a {@link - * org.littletonrobotics.junction.inputs.LoggableInputs}. - * - * @param Loggable inputs that are updated. - */ -public interface LoggedIO { - - /** - * Update the inputs with the current state of the IO. - * - * @param inputs inputs to update. - */ - default void updateInputs(T inputs) {} -} diff --git a/src/main/java/frc/lib/LoggerUtil.java b/src/main/java/frc/lib/LoggerUtil.java deleted file mode 100644 index 246ac68..0000000 --- a/src/main/java/frc/lib/LoggerUtil.java +++ /dev/null @@ -1,100 +0,0 @@ -package frc.lib; - -import edu.wpi.first.networktables.ConnectionInfo; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.generated.BuildConstants; -import frc.robot.constants.Constants; -import java.io.IOException; -import java.nio.file.Path; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.Map; -import java.util.function.BiConsumer; -import org.littletonrobotics.junction.Logger; - -/** Utilities used by the AdvantageKit Logger. */ -@SuppressWarnings("DataFlowIssue") -public class LoggerUtil { - /** - * Initialize the Logger with the auto-generated data from the build. - * - * @param logger logger to update. - */ - public static void initializeLoggerMetadata(Logger logger) { - // Record metadata from generated state file. - logger.recordMetadata("ROBOT_NAME", Constants.getRobotType().toString()); - logger.recordMetadata("RUNTIME_ENVIRONMENT", RobotBase.getRuntimeType().toString()); - logger.recordMetadata("PROJECT_NAME", BuildConstants.MAVEN_NAME); - logger.recordMetadata("BUILD_DATE", BuildConstants.BUILD_DATE); - logger.recordMetadata("GIT_SHA", BuildConstants.GIT_SHA); - logger.recordMetadata("GIT_DATE", BuildConstants.GIT_DATE); - logger.recordMetadata("GIT_BRANCH", BuildConstants.GIT_BRANCH); - - // Set the current GIT state of the robot (helps manage the logs that are saved). - switch (BuildConstants.DIRTY) { - case 0 -> logger.recordMetadata("GIT_STATUS", "All changes committed"); - case 1 -> logger.recordMetadata("GIT_STATUS", "Uncommitted changes"); - default -> logger.recordMetadata("GIT_STATUS", "Unknown"); - } - } - - /** - * Get the path of the USB drive if it is plugged into the roboRIO. If one isn't found, it will - * return null. - * - * @return path of the USB drive. - */ - public static String getUSBPath() { - // Return the path of the USB drive it is plugged in, else, return null. - try { - Path drivePath = Path.of("/u").toRealPath(); - return drivePath.toString(); - } catch (IOException e) { - return null; - } - } - - /** - * Log all the currently connected NetworkTables clients. This method should be called - * periodically. - */ - public static void logNTClients() { - List clientNames = new ArrayList<>(); - List clientAddresses = new ArrayList<>(); - - for (ConnectionInfo client : NetworkTableInstance.getDefault().getConnections()) { - clientNames.add(client.remote_id); - clientAddresses.add(client.remote_ip); - } - - Logger.getInstance().recordOutput("NTClients/Names", clientNames.toArray(new String[0])); - Logger.getInstance() - .recordOutput("NTClients/Addresses", clientAddresses.toArray(new String[0])); - } - - /** Log Commands scheduled by the CommandScheduler to telemetry automatically. */ - public static void initCommandLogging() { - // Log active commands - Map commandCounts = new HashMap<>(); - BiConsumer logCommandFunction = - (Command command, Boolean active) -> { - String name = command.getName(); - int count = commandCounts.getOrDefault(name, 0) + (active ? 1 : -1); - commandCounts.put(name, count); - Logger.getInstance() - .recordOutput( - "CommandsUnique/" + name + "_" + Integer.toHexString(command.hashCode()), active); - Logger.getInstance().recordOutput("CommandsAll/" + name, count > 0); - }; - CommandScheduler.getInstance() - .onCommandInitialize((Command command) -> logCommandFunction.accept(command, true)); - CommandScheduler.getInstance() - .onCommandFinish((Command command) -> logCommandFunction.accept(command, false)); - CommandScheduler.getInstance() - .onCommandInterrupt((Command command) -> logCommandFunction.accept(command, false)); - } -} diff --git a/src/main/java/frc/lib/PoseEstimator.java b/src/main/java/frc/lib/PoseEstimator.java deleted file mode 100644 index 7aa2521..0000000 --- a/src/main/java/frc/lib/PoseEstimator.java +++ /dev/null @@ -1,34 +0,0 @@ -package frc.lib; - -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; - -public class PoseEstimator extends SwerveDrivePoseEstimator { - private static PoseEstimator instance; - - public static void createInstance( - SwerveDriveKinematics kinematics, - Rotation2d gyroHeading, - SwerveModulePosition... modulePositions) { - instance = new PoseEstimator(kinematics, gyroHeading, modulePositions); - } - - public static PoseEstimator getInstance() { - if (instance == null) { - throw new IllegalStateException( - "The PoseEstimator hasn't been created yet. Create it first using PoseEstimator::createInstance"); - } else { - return instance; - } - } - - private PoseEstimator( - SwerveDriveKinematics kinematics, - Rotation2d gyroHeading, - SwerveModulePosition... modulePositions) { - super(kinematics, gyroHeading, modulePositions, new Pose2d()); - } -} diff --git a/src/main/java/frc/lib/SparkMaxBurnManager.java b/src/main/java/frc/lib/SparkMaxBurnManager.java deleted file mode 100644 index 43801ae..0000000 --- a/src/main/java/frc/lib/SparkMaxBurnManager.java +++ /dev/null @@ -1,67 +0,0 @@ -package frc.lib; - -import edu.wpi.first.wpilibj.RobotBase; -import frc.generated.BuildConstants; -import java.io.File; -import java.io.FileWriter; -import java.io.IOException; -import java.nio.file.Files; -import java.nio.file.Paths; - -/** - * SparkMax's memory, like most NAND based memory, has a limited number of write commands it can - * sustain, in order to prevent this from hitting that maximum, we can check if the code was even - * changed, and if so, burn the new settings to the SparkMax. - */ -public class SparkMaxBurnManager { - public static final int kConfigurationStatusTimeoutMs = 500; - private static final String buildDateFile = "/home/lvuser/build-date.txt"; - private static boolean shouldBurn = false; - - private SparkMaxBurnManager() {} - - public static void checkBuildStatus() { - // TODO, make sure read and write functions work - if (RobotBase.isSimulation()) { - shouldBurn = false; - return; - } - - File file = new File(buildDateFile); - if (!file.exists()) { - // No build date file, burn flash - shouldBurn = true; - } else { - // Read previous build date - String previousBuildDate = ""; - try { - previousBuildDate = Files.readString(Paths.get(buildDateFile)); - } catch (IOException error) { - error.printStackTrace(); - } - - shouldBurn = !previousBuildDate.equals(BuildConstants.BUILD_DATE); - } - - // Write the current Build Time - try { - FileWriter fileWriter = new FileWriter(buildDateFile); - fileWriter.write(BuildConstants.BUILD_DATE); - fileWriter.close(); - } catch (IOException error) { - error.printStackTrace(); - } - - if (shouldBurn) { - System.out.println( - "[SparkMaxBurnManager] Build date changed, allowing SparkMaxes to burn flash"); - } else { - System.out.println( - "[SparkMaxBurnManager] Build date unchanged, will not burn SparkMax flash"); - } - } - - public static boolean shouldBurnFlash() { - return shouldBurn; - } -} diff --git a/src/main/java/frc/lib/SparkMaxPeriodicFrameConfig.java b/src/main/java/frc/lib/SparkMaxPeriodicFrameConfig.java deleted file mode 100644 index f0554de..0000000 --- a/src/main/java/frc/lib/SparkMaxPeriodicFrameConfig.java +++ /dev/null @@ -1,35 +0,0 @@ -package frc.lib; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; - -/** - * Configures the rates at which different status frames are sent from SparkMaxes. See here - * for different status frames. - */ -public class SparkMaxPeriodicFrameConfig { - public static void configureLeader(CANSparkMax motorController) { - motorController.setPeriodicFramePeriod(PeriodicFrame.kStatus0, 10); - motorController.setPeriodicFramePeriod(PeriodicFrame.kStatus3, 65535); - motorController.setPeriodicFramePeriod(PeriodicFrame.kStatus4, 65535); - motorController.setPeriodicFramePeriod(PeriodicFrame.kStatus5, 65535); - motorController.setPeriodicFramePeriod(PeriodicFrame.kStatus6, 65535); - } - - public static void configureFollower(CANSparkMax motorController) { - motorController.setPeriodicFramePeriod(PeriodicFrame.kStatus0, 10); - motorController.setPeriodicFramePeriod(PeriodicFrame.kStatus3, 65535); - motorController.setPeriodicFramePeriod(PeriodicFrame.kStatus4, 65535); - motorController.setPeriodicFramePeriod(PeriodicFrame.kStatus5, 65535); - motorController.setPeriodicFramePeriod(PeriodicFrame.kStatus6, 65535); - } - - public static void configureIsolated(CANSparkMax motorController) { - motorController.setPeriodicFramePeriod(PeriodicFrame.kStatus0, 100); - motorController.setPeriodicFramePeriod(PeriodicFrame.kStatus3, 65535); - motorController.setPeriodicFramePeriod(PeriodicFrame.kStatus4, 65535); - motorController.setPeriodicFramePeriod(PeriodicFrame.kStatus5, 65535); - motorController.setPeriodicFramePeriod(PeriodicFrame.kStatus6, 65535); - } -} diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 9b6d81d..8e389ea 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -5,7 +5,7 @@ public final class Main { private Main() {} - public static void main(String[] args) { + public static void main(String... args) { RobotBase.startRobot(Robot::new); } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7ec48b2..23e5b28 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,92 +1,67 @@ package frc.robot; -import edu.wpi.first.hal.AllianceStationID; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.simulation.DriverStationSim; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.lib.LoggerUtil; import frc.robot.constants.Constants; +import frc.robot.util.LoggerUtil; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.inputs.LoggedDriverStation; -import org.littletonrobotics.junction.inputs.LoggedPowerDistribution; -import org.littletonrobotics.junction.inputs.LoggedSystemStats; import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; public class Robot extends LoggedRobot { - private RobotContainer m_robotContainer; - private Command m_autonomousCommand; + private Command autonomousCommand; + private RobotContainer robotContainer; @Override public void robotInit() { - Logger logger = Logger.getInstance(); - - LoggerUtil.initializeLoggerMetadata(logger); + LoggerUtil.initializeLoggerMetadata(); // Set up data receivers & replay source switch (Constants.getRobotMode()) { - case REAL -> { - String path = LoggerUtil.getUSBPath(); - - if (path != null) { - logger.addDataReceiver(new WPILOGWriter(path)); - } else { - DriverStation.reportWarning( - "Unable to locate a USB drive plugged into the roboRIO. Hardware storage logging will be disabled.", - false); - } - - logger.addDataReceiver(new NT4Publisher()); - - LoggedPowerDistribution.getInstance(); - LoggedDriverStation.getInstance(); - LoggedSystemStats.getInstance(); - } - case SIM -> logger.addDataReceiver(new NT4Publisher()); - case REPLAY -> { - setUseTiming(false); - String path = LogFileUtil.findReplayLog(); - logger.setReplaySource(new WPILOGReader(path)); - logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(path, "_sim"))); - } + case REAL: + // Running on a real robot, log to a USB stick + Logger.addDataReceiver(new WPILOGWriter("/U")); + Logger.addDataReceiver(new NT4Publisher()); + break; + case SIM: + // Running a physics simulator, log to NT + Logger.addDataReceiver(new NT4Publisher()); + break; + case REPLAY: + // Replaying a log, set up replay source + setUseTiming(false); // Run as fast as possible + String logPath = LogFileUtil.findReplayLog(); + Logger.setReplaySource(new WPILOGReader(logPath)); + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); + break; } - // Start AdvantageKit logger - logger.start(); + Logger.start(); - LoggerUtil.initCommandLogging(); - - if (Constants.getRobotMode() == Constants.RobotMode.SIM) - DriverStationSim.setAllianceStationId(AllianceStationID.Blue1); - - m_robotContainer = new RobotContainer(); + robotContainer = new RobotContainer(); } @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - if (Constants.kAdvancedLoggingEnabled) { - LoggerUtil.logNTClients(); - } } @Override public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + autonomousCommand = robotContainer.getAutonomousCommand(); - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); + if (autonomousCommand != null) { + autonomousCommand.schedule(); } } @Override public void teleopInit() { - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); + if (autonomousCommand != null) { + autonomousCommand.cancel(); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1ed66a1..6418f67 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,98 +1,92 @@ package frc.robot; import edu.wpi.first.wpilibj2.command.Command; -import frc.lib.SparkMaxBurnManager; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; +import frc.robot.commands.drive.DriveCommandFactory; +import frc.robot.commands.drive.FeedForwardCharacterization; import frc.robot.constants.Constants; -import frc.robot.constants.HardwareDevices; -import frc.robot.constants.HardwareDevices.SwerveBase.Drivetrain.*; -import frc.robot.drive.*; -import frc.robot.drive.commands.DriveControl; -import frc.robot.drive.gyro.GyroIO; -import frc.robot.drive.gyro.GyroIOPigeon2; -import frc.robot.drive.gyro.GyroIOSim; -import frc.robot.oi.OIManager; +import frc.robot.constants.HardwareIds; +import frc.robot.subsystems.drive.*; +import frc.robot.subsystems.drive.DriveBase; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; public class RobotContainer { - private DriveBase m_driveBase; + // Subsystems + private final DriveBase m_drive; - private final OIManager m_OIManager = new OIManager(); + // Controller + private final CommandPS4Controller controller = new CommandPS4Controller(0); - public RobotContainer() { - SparkMaxBurnManager.checkBuildStatus(); + // Dashboard inputs + private final LoggedDashboardChooser autoChooser = + new LoggedDashboardChooser<>("Auto Choices"); - if (Constants.getRobotMode() != Constants.RobotMode.REPLAY) { - switch (Constants.getRobotType()) { - case ROBOT_SWERVE -> { - m_driveBase = - new DriveBase( - new GyroIOPigeon2(HardwareDevices.SwerveBase.kGyroId), - // FRONT LEFT - new SwerveModuleIOMK4DualSparkMax( - FrontLeft.kDriveMotorId, - FrontLeft.kSteerMotorId, - FrontLeft.kAbsoluteEncoderId, - Constants.Drivetrain.kSteerMotorInverted, - Constants.Drivetrain.kDriveMotorConversionFactor, - Constants.Drivetrain.kSteerMotorConversionFactor, - Constants.Drivetrain.FrontLeft.kMagneticOffsetDegrees), - // FRONT RIGHT - new SwerveModuleIOMK4DualSparkMax( - FrontRight.kDriveMotorId, - FrontRight.kSteerMotorId, - FrontRight.kAbsoluteEncoderId, - Constants.Drivetrain.kSteerMotorInverted, - Constants.Drivetrain.kDriveMotorConversionFactor, - Constants.Drivetrain.kSteerMotorConversionFactor, - Constants.Drivetrain.FrontRight.kMagneticOffsetDegrees), - // BACK LEFT - new SwerveModuleIOMK4DualSparkMax( - BackLeft.kDriveMotorId, - BackLeft.kSteerMotorId, - BackLeft.kAbsoluteEncoderId, - Constants.Drivetrain.kSteerMotorInverted, - Constants.Drivetrain.kDriveMotorConversionFactor, - Constants.Drivetrain.kSteerMotorConversionFactor, - Constants.Drivetrain.BackLeft.kMagneticOffsetDegrees), - // BACK RIGHT - new SwerveModuleIOMK4DualSparkMax( - BackRight.kDriveMotorId, - BackRight.kSteerMotorId, - BackRight.kAbsoluteEncoderId, - Constants.Drivetrain.kSteerMotorInverted, - Constants.Drivetrain.kDriveMotorConversionFactor, - Constants.Drivetrain.kSteerMotorConversionFactor, - Constants.Drivetrain.BackRight.kMagneticOffsetDegrees)); - } - case ROBOT_SIMBOT -> { - m_driveBase = - new DriveBase( - new GyroIOSim(), - new SwerveModuleIOSim(), - new SwerveModuleIOSim(), - new SwerveModuleIOSim(), - new SwerveModuleIOSim()); - } + public RobotContainer() { + switch (Constants.getRobotMode()) { + case REAL -> { + m_drive = + new DriveBase( + new GyroIOPigeon2(HardwareIds.kPigeonId), + new ModuleIOSparkMax( + HardwareIds.kFrontLeftDriveId, + HardwareIds.kFrontLeftTurnId, + HardwareIds.kFrontLeftEncoderId), + new ModuleIOSparkMax( + HardwareIds.kFrontRightDriveId, + HardwareIds.kFrontRightTurnId, + HardwareIds.kFrontRightEncoderId), + new ModuleIOSparkMax( + HardwareIds.kBackLeftDriveId, + HardwareIds.kBackLeftTurnId, + HardwareIds.kBackLeftEncoderId), + new ModuleIOSparkMax( + HardwareIds.kBackRightDriveId, + HardwareIds.kBackRightTurnId, + HardwareIds.kBackRightEncoderId)); + } + case SIM -> { + m_drive = + new DriveBase( + new GyroIO() {}, + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim()); + } + default -> { + m_drive = + new DriveBase( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}); } } - m_driveBase = - m_driveBase != null - ? m_driveBase - : new DriveBase( - new GyroIO() {}, - new SwerveModuleIO() {}, - new SwerveModuleIO() {}, - new SwerveModuleIO() {}, - new SwerveModuleIO() {}); + // // Set up FF characterization routines + autoChooser.addOption( + "DriveBase FF Characterization", + new FeedForwardCharacterization( + m_drive, m_drive::runCharacterizationVolts, m_drive::getCharacterizationVelocity)); - configureBindings(); + // Configure the button bindings + configureButtonBindings(); } - private void configureBindings() { - m_driveBase.setDefaultCommand(new DriveControl(m_driveBase, m_OIManager.getDriverInterface())); + private void configureButtonBindings() { + m_drive.setDefaultCommand( + DriveCommandFactory.joystickDrive( + m_drive, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> -controller.getRightX(), + 0.1)); + controller.cross().onTrue(Commands.runOnce(m_drive::stopWithX, m_drive)); } public Command getAutonomousCommand() { - return null; + return autoChooser.get(); } } diff --git a/src/main/java/frc/robot/commands/drive/DriveCommandFactory.java b/src/main/java/frc/robot/commands/drive/DriveCommandFactory.java new file mode 100644 index 0000000..9250678 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/DriveCommandFactory.java @@ -0,0 +1,42 @@ +package frc.robot.commands.drive; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.constants.Constants; +import frc.robot.subsystems.drive.DriveBase; +import java.util.function.DoubleSupplier; + +public class DriveCommandFactory { + public static Command joystickDrive( + DriveBase driveBase, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier, + double deadband) { + return Commands.run( + () -> { + double x_val = xSupplier.getAsDouble(); + double y_val = ySupplier.getAsDouble(); + double omega_val = omegaSupplier.getAsDouble(); + + double x = Math.copySign(Math.pow(MathUtil.applyDeadband(x_val, deadband), 2), x_val); + double y = Math.copySign(Math.pow(MathUtil.applyDeadband(y_val, deadband), 2), y_val); + double omega = + Math.copySign(Math.pow(MathUtil.applyDeadband(omega_val, deadband), 2), omega_val); + + driveBase.runVelocity( + // ChassisSpeeds.fromFieldRelativeSpeeds( + // x * Constants.Drivetrain.kMaxLinearVelocityMetersPerSecond, + // y * Constants.Drivetrain.kMaxLinearVelocityMetersPerSecond, + // omega * Constants.Drivetrain.kMaxLAngularVelocityRadiansPerSecond, + // PoseEstimator.getInstance().getPose().getRotation()) + new ChassisSpeeds( + x * Constants.Drivetrain.kMaxLinearVelocityMetersPerSecond, + y * Constants.Drivetrain.kMaxLinearVelocityMetersPerSecond, + omega * Constants.Drivetrain.kMaxLAngularVelocityRadiansPerSecond)); + }, + driveBase); + } +} diff --git a/src/main/java/frc/robot/commands/drive/FeedForwardCharacterization.java b/src/main/java/frc/robot/commands/drive/FeedForwardCharacterization.java new file mode 100644 index 0000000..8f5e314 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/FeedForwardCharacterization.java @@ -0,0 +1,100 @@ +// Copyright 2021-2023 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.commands.drive; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; +import frc.robot.util.PolynomialRegression; +import java.util.LinkedList; +import java.util.List; +import java.util.function.Consumer; +import java.util.function.Supplier; + +public class FeedForwardCharacterization extends Command { + private static final double START_DELAY_SECS = 2.0; + private static final double RAMP_VOLTS_PER_SEC = 0.1; + + private FeedForwardCharacterizationData data; + private final Consumer voltageConsumer; + private final Supplier velocitySupplier; + + private final Timer timer = new Timer(); + + /** Creates a new FeedForwardCharacterization command. */ + public FeedForwardCharacterization( + Subsystem subsystem, Consumer voltageConsumer, Supplier velocitySupplier) { + addRequirements(subsystem); + this.voltageConsumer = voltageConsumer; + this.velocitySupplier = velocitySupplier; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + data = new FeedForwardCharacterizationData(); + timer.reset(); + timer.start(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (timer.get() < START_DELAY_SECS) { + voltageConsumer.accept(0.0); + } else { + double voltage = (timer.get() - START_DELAY_SECS) * RAMP_VOLTS_PER_SEC; + voltageConsumer.accept(voltage); + data.add(velocitySupplier.get(), voltage); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + voltageConsumer.accept(0.0); + timer.stop(); + data.print(); + } + + public static class FeedForwardCharacterizationData { + private final List velocityData = new LinkedList<>(); + private final List voltageData = new LinkedList<>(); + + public void add(double velocity, double voltage) { + if (Math.abs(velocity) > 1E-4) { + velocityData.add(Math.abs(velocity)); + voltageData.add(Math.abs(voltage)); + } + } + + public void print() { + if (velocityData.size() == 0 || voltageData.size() == 0) { + return; + } + + PolynomialRegression regression = + new PolynomialRegression( + velocityData.stream().mapToDouble(Double::doubleValue).toArray(), + voltageData.stream().mapToDouble(Double::doubleValue).toArray(), + 1); + + System.out.println("FF Characterization Results:"); + System.out.println("\tCount=" + Integer.toString(velocityData.size()) + ""); + System.out.println(String.format("\tR2=%.5f", regression.R2())); + System.out.println(String.format("\tkS=%.5f", regression.beta(0))); + System.out.println(String.format("\tkV=%.5f", regression.beta(1))); + } + } +} diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 9781300..dd72731 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -1,6 +1,5 @@ package frc.robot.constants; -import com.revrobotics.CANSparkMax; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.util.Units; @@ -8,31 +7,27 @@ import edu.wpi.first.wpilibj.RobotBase; public final class Constants { - /** - * Whether advanced logging should be enabled. This can be disabled if there is too much going on. - */ - public static final boolean kAdvancedLoggingEnabled = true; - private static RobotType kRobotType = RobotType.ROBOT_SIMBOT; - public static final double loopPeriodSecs = 0.02; + + public static double kLoopPeriodSecs = 0.02; public enum RobotMode { REAL, - REPLAY, - SIM + SIM, + REPLAY } public enum RobotType { - ROBOT_SWERVE, + ROBOT_2023_OFFSEASON_SWERVE, ROBOT_SIMBOT } public static RobotType getRobotType() { if (RobotBase.isReal() && kRobotType == RobotType.ROBOT_SIMBOT) { DriverStation.reportError( - "Robot is set to SIM but it isn't a SIM, setting it to Swerve (Real) Robot as redundancy.", + "Robot is set to SIM but it isn't a SIM, setting it to Competition Robot as redundancy.", false); - kRobotType = RobotType.ROBOT_SWERVE; + kRobotType = RobotType.ROBOT_2023_OFFSEASON_SWERVE; } if (RobotBase.isSimulation() && kRobotType != RobotType.ROBOT_SIMBOT) { @@ -46,96 +41,63 @@ public static RobotType getRobotType() { public static RobotMode getRobotMode() { return switch (getRobotType()) { - case ROBOT_SWERVE -> RobotBase.isReal() ? RobotMode.REAL : RobotMode.REPLAY; + case ROBOT_2023_OFFSEASON_SWERVE -> RobotBase.isReal() ? RobotMode.REAL : RobotMode.REPLAY; case ROBOT_SIMBOT -> RobotMode.SIM; }; } public static class Drivetrain { - // MODULE LOAD ORDER IS FRONT LEFT -> FRONT RIGHT -> BACK LEFT -> BACK RIGHT - - public static final SwerveDriveKinematics kKinematics = - new SwerveDriveKinematics( - FrontLeft.kCenterOffset, // FRONT LEFT - FrontRight.kCenterOffset, // FRONT RIGHT - BackLeft.kCenterOffset, // BACK LEFT - BackRight.kCenterOffset // BACK RIGHT - ); - - public static final boolean kSteerMotorInverted = true; - - public static final double kMaxVelocityMetersPerSecond = Units.feetToMeters(12.0); // SDS MK4 L1 - // public static final double kMaxVelocityMetersPerSecond = Units.feetToMeters(14.5); // SDS MK4 - // L2 - public static final double kMaxAccelerationMetersPerSecondSquared = 3.5; - public static final double kMaxRotationVelocityRadPerSecond = 2 * Math.PI; - - public static final double kDriveGearRatio = 8.14; // SDS MK4 L1 - // public static final double kDriveGearRatio = 6.75; // SDS MK4 L2 - - public static final double kSteerGearRatio = 12.8; - - public static final double kWheelRadiusInches = 2; - public static final double KWheelRadiusMeters = Units.inchesToMeters(kWheelRadiusInches); - - public static final double kDriveMotorConversionFactor = - 2 * Math.PI * KWheelRadiusMeters / kDriveGearRatio; - public static final double kSteerMotorConversionFactor = 2 * Math.PI / kSteerGearRatio; - - public static class FrontLeft { - public static final Translation2d kCenterOffset = - new Translation2d(Units.inchesToMeters(8.825180), Units.inchesToMeters(8.825180)); - public static final double kMagneticOffsetDegrees = 0.0; // TODO - } - - public static class FrontRight { - public static final Translation2d kCenterOffset = - new Translation2d(Units.inchesToMeters(8.825180), -Units.inchesToMeters(8.825180)); - public static final double kMagneticOffsetDegrees = 0.0; // TODO - } - - public static class BackLeft { - public static final Translation2d kCenterOffset = - new Translation2d(-Units.inchesToMeters(8.825180), Units.inchesToMeters(8.825180)); - public static final double kMagneticOffsetDegrees = 0.0; // TODO + // L2 gearing + public static final double kDriveGearing = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + public static final double kTurnGearing = 12.8; + + public static final boolean kTurnMotorInverted = false; + + public static final double kWheelRadiusMeters = Units.inchesToMeters(2.0); + + public static final double kTrackWidthXMeters = Units.inchesToMeters(20.5); // TODO + public static final double kTrackWidthYMeters = Units.inchesToMeters(20.5); // TODO + public static final double kDriveBaseRadiusMeters = + Math.hypot(kTrackWidthXMeters / 2.0, kTrackWidthYMeters / 2.0); + + public static final double kMaxLinearVelocityMetersPerSecond = Units.feetToMeters(14.5); + public static final double kMaxLAngularVelocityRadiansPerSecond = + kMaxLinearVelocityMetersPerSecond / kDriveBaseRadiusMeters; + + public static final class DriveCoefficients { + public static final double kS = 0.0; // TODO + public static final double kV = 0.0; // TODO + public static final double kA = 0.0; // TODO + public static final double kP = 0.0; // TODO + public static final double kI = 0.0; // TODO + public static final double kD = 0.0; // TODO } - public static class BackRight { - public static final Translation2d kCenterOffset = - new Translation2d(-Units.inchesToMeters(8.825180), -Units.inchesToMeters(8.825180)); - public static final double kMagneticOffsetDegrees = 0.0; // TODO + public static final class TurnCoefficients { + public static final double kP = 0.0; // TODO + public static final double kI = 0.0; // TODO + public static final double kD = 0.0; // TODO } - public static class ControlValues { - public static class Drive { - public static final double kP = 0; // TODO - public static final double kI = 0; // TODO - public static final double kD = 0; // TODO - public static final double kFF = 0; // TODO - } - - public static class Steer { - public static final double kP = 0; // TODO - public static final double kI = 0; // TODO - public static final double kD = 0; // TODO - } - } - } - - public enum NeutralMode { - BRAKE, - COAST; - - /** - * Convert the Neutral mode to one used by the SparkMax API. - * - * @return SparkMax Idle Mode. - */ - public CANSparkMax.IdleMode toIdleMode() { - return switch (this) { - case BRAKE -> CANSparkMax.IdleMode.kBrake; - case COAST -> CANSparkMax.IdleMode.kCoast; + /** Returns an array of module translations. */ + public static Translation2d[] getModuleTranslations() { + return new Translation2d[] { + new Translation2d( + Constants.Drivetrain.kTrackWidthXMeters / 2.0, + Constants.Drivetrain.kTrackWidthYMeters / 2.0), + new Translation2d( + Constants.Drivetrain.kTrackWidthXMeters / 2.0, + -Constants.Drivetrain.kTrackWidthYMeters / 2.0), + new Translation2d( + -Constants.Drivetrain.kTrackWidthXMeters / 2.0, + Constants.Drivetrain.kTrackWidthYMeters / 2.0), + new Translation2d( + -Constants.Drivetrain.kTrackWidthXMeters / 2.0, + -Constants.Drivetrain.kTrackWidthYMeters / 2.0) }; } + + public static final SwerveDriveKinematics m_kinematics = + new SwerveDriveKinematics(getModuleTranslations()); } } diff --git a/src/main/java/frc/robot/constants/HardwareDevices.java b/src/main/java/frc/robot/constants/HardwareDevices.java deleted file mode 100644 index 1a4f24b..0000000 --- a/src/main/java/frc/robot/constants/HardwareDevices.java +++ /dev/null @@ -1,35 +0,0 @@ -package frc.robot.constants; - -public class HardwareDevices { - public static final int kDriverControllerPort = 0; - - public static class SwerveBase { - public static final int kGyroId = 0; // TODO - - public static class Drivetrain { - public static class FrontLeft { - public static final int kDriveMotorId = 0; // TODO - public static final int kSteerMotorId = 0; // TODO - public static final int kAbsoluteEncoderId = 0; // TODO - } - - public static class FrontRight { - public static final int kDriveMotorId = 0; // TODO - public static final int kSteerMotorId = 0; // TODO - public static final int kAbsoluteEncoderId = 0; // TODO - } - - public static class BackLeft { - public static final int kDriveMotorId = 0; // TODO - public static final int kSteerMotorId = 0; // TODO - public static final int kAbsoluteEncoderId = 0; // TODO - } - - public static class BackRight { - public static final int kDriveMotorId = 0; // TODO - public static final int kSteerMotorId = 0; // TODO - public static final int kAbsoluteEncoderId = 0; // TODO - } - } - } -} diff --git a/src/main/java/frc/robot/constants/HardwareIds.java b/src/main/java/frc/robot/constants/HardwareIds.java new file mode 100644 index 0000000..39129ac --- /dev/null +++ b/src/main/java/frc/robot/constants/HardwareIds.java @@ -0,0 +1,26 @@ +package frc.robot.constants; + +public class HardwareIds { + public static final int kPigeonId = 0; // TODO + + public static final int kFrontLeftTurnId = 0; // TODO + public static final int kFrontLeftDriveId = 0; // TODO + public static final int kFrontLeftEncoderId = 0; // TODO + + public static final int kBackLeftTurnId = 0; // TODO + public static final int kBackLeftDriveId = 0; // TODO + public static final int kBackLeftEncoderId = 0; // TODO + + public static final int kFrontRightTurnId = 0; // TODO + public static final int kFrontRightDriveId = 0; // TODO + public static final int kFrontRightEncoderId = 0; // TODO + + public static final int kBackRightTurnId = 0; // TODO + public static final int kBackRightDriveId = 0; // TODO + public static final int kBackRightEncoderId = 0; // TODO + + public static final String kCamOneName = ""; // TODO + public static final String kCamTwoName = ""; // TODO + public static final String kCamThreeName = ""; // TODO + public static final String kCamFourName = ""; // TODO +} diff --git a/src/main/java/frc/robot/drive/DriveBase.java b/src/main/java/frc/robot/drive/DriveBase.java deleted file mode 100644 index aaf1926..0000000 --- a/src/main/java/frc/robot/drive/DriveBase.java +++ /dev/null @@ -1,176 +0,0 @@ -package frc.robot.drive; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.lib.PoseEstimator; -import frc.robot.constants.Constants; -import frc.robot.drive.gyro.GyroIO; -import frc.robot.drive.gyro.GyroInputsAutoLogged; -import org.littletonrobotics.junction.Logger; - -public class DriveBase extends SubsystemBase { - private final SwerveModuleIO[] m_moduleIOs; - private final SwerveModuleInputsAutoLogged[] m_moduleInputs = - new SwerveModuleInputsAutoLogged[] { - new SwerveModuleInputsAutoLogged(), - new SwerveModuleInputsAutoLogged(), - new SwerveModuleInputsAutoLogged(), - new SwerveModuleInputsAutoLogged() - }; - - private final GyroIO m_gyroIO; - private final GyroInputsAutoLogged m_gyroInputs = new GyroInputsAutoLogged(); - - public DriveBase( - GyroIO gyro, - SwerveModuleIO frontLeft, - SwerveModuleIO frontRight, - SwerveModuleIO backLeft, - SwerveModuleIO backRight) { - m_moduleIOs = new SwerveModuleIO[] {frontLeft, frontRight, backLeft, backRight}; - m_gyroIO = gyro; - - m_gyroIO.resetHeading(); - - for (SwerveModuleIO module : m_moduleIOs) { - module.resyncEncoders(); - } - - PoseEstimator.createInstance( - Constants.Drivetrain.kKinematics, m_gyroIO.getHeading(), getModulePositions()); - } - - @Override - public void periodic() { - for (int i = 0; i < 4; i++) { - m_moduleIOs[i].updateInputs(m_moduleInputs[i]); - Logger.getInstance().processInputs("Drive/Module" + i, m_moduleInputs[i]); - } - - m_gyroIO.updateInputs(m_gyroInputs); - Logger.getInstance().processInputs("Drive/Gyro", m_gyroInputs); - - // Update PoseEstimator with drive data - PoseEstimator.getInstance().update(m_gyroIO.getHeading(), getModulePositions()); - Logger.getInstance() - .recordOutput("EstimatedPose", PoseEstimator.getInstance().getEstimatedPosition()); - } - - @Override - public void simulationPeriodic() { - m_gyroIO.incrementHeading(getChassisSpeeds().omegaRadiansPerSecond * Constants.loopPeriodSecs); - } - - public Rotation2d getHeading() { - return m_gyroIO.getHeading(); - } - - public ChassisSpeeds getChassisSpeeds() { - return Constants.Drivetrain.kKinematics.toChassisSpeeds(getModuleStates()); - } - - /** - * Set the drivetrain modules based on a series of speeds and spinning about a non-center of the - * robot given as a {@link Translation2d} object and following its conventions. - * - * @param xSpeed Horizontal speed to derive module states in meters per second - * @param ySpeed Vertical speed to derive module states in meters per second - * @param rotationalSpeed Rotational speed in radians per second the drivetrain spins in - * @param driveMode Control input method to derive module states from: {@code DriveMode.FIELD} or - * {@code DriveMode.DRIVER} (default FIELD) - */ - public final void setFromForces( - double xSpeed, - double ySpeed, - double rotationalSpeed, - Translation2d centerOfRobot, - DriveMode driveMode) { - - setModuleStates( - Constants.Drivetrain.kKinematics.toSwerveModuleStates( - switch (driveMode) { - case kRobot -> new ChassisSpeeds(xSpeed, ySpeed, rotationalSpeed); - case kField -> ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rotationalSpeed, getHeading()); - }, - centerOfRobot)); - } - - /** - * Control method to convert input stuff to drive stuff for all the modules - * - * @param xSpeed Horizontal speed to derive module states in meters per second - * @param ySpeed Vertical speed to derive module states in meters per second - * @param rotationalSpeed Rotational speed in radians per second the drivetrain spins in - * @param driveMode Control input method to derive module states from: {@code DriveMode.FIELD} or - * {@code DriveMode.DRIVER} - */ - public final void setFromForces( - double xSpeed, double ySpeed, double rotationalSpeed, DriveMode driveMode) { - setFromForces(xSpeed, ySpeed, rotationalSpeed, new Translation2d(), driveMode); - } - - /** Apply module states to modules based on a ChassisSpeed object */ - public final void setChassisSpeed(ChassisSpeeds chassisSpeed) { - setModuleStates(Constants.Drivetrain.kKinematics.toSwerveModuleStates(chassisSpeed)); - } - - /** - * Apply module states to modules based on an array of module states. - * - * @implNote MODULES MUST BE GIVEN IN ORDER: FRONT LEFT -> FRONT RIGHT -> BACK LEFT -> BACK RIGHT - */ - public final void setModuleStates(SwerveModuleState[] states) { - SwerveDriveKinematics.desaturateWheelSpeeds( - states, Constants.Drivetrain.kMaxVelocityMetersPerSecond); - - for (int i = 0; i < 4; i++) { - m_moduleIOs[i].setState(states[i]); - } - } - - public void stop() { - for (SwerveModuleIO module : m_moduleIOs) { - module.stop(); - } - } - - public void stopWithX() { - double[] stopAngles = - new double[] {-Math.PI / 4.0, Math.PI / 4.0, Math.PI / 4.0, -Math.PI / 4.0}; - - for (int i = 0; i < m_moduleIOs.length; i++) { - m_moduleIOs[i].setState(new SwerveModuleState(0, Rotation2d.fromRadians(stopAngles[i]))); - } - } - - /** Return module states in order of kinematic initialization from modules */ - private SwerveModuleState[] getModuleStates() { - return new SwerveModuleState[] { - m_moduleIOs[0].getState(), - m_moduleIOs[1].getState(), - m_moduleIOs[2].getState(), - m_moduleIOs[3].getState() - }; - } - - /** Return module positions in order of kinematic initialization from modules */ - private SwerveModulePosition[] getModulePositions() { - return new SwerveModulePosition[] { - m_moduleIOs[0].getPosition(), - m_moduleIOs[1].getPosition(), - m_moduleIOs[2].getPosition(), - m_moduleIOs[3].getPosition() - }; - } - - public enum DriveMode { - kRobot, - kField - } -} diff --git a/src/main/java/frc/robot/drive/SwerveModuleIO.java b/src/main/java/frc/robot/drive/SwerveModuleIO.java deleted file mode 100644 index 6eff6e2..0000000 --- a/src/main/java/frc/robot/drive/SwerveModuleIO.java +++ /dev/null @@ -1,42 +0,0 @@ -package frc.robot.drive; - -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import frc.lib.LoggedIO; -import frc.robot.constants.Constants; -import org.littletonrobotics.junction.AutoLog; - -public interface SwerveModuleIO extends LoggedIO { - @AutoLog - public class SwerveModuleInputs { - public double DriveCurrentAmps; - public double DriveTempCelsius; - public double DriveAppliedVoltage; - public double SteerCurrentAmps; - public double SteerTempCelsius; - public double SteerAppliedVoltage; - - public double VelocityRadPerSec; - public double VelocityMetersPerSec; - public double AbsolutePositionRad; - public double RelativePositionRad; - public double DistanceTraveledMeters; - } - - default SwerveModuleState getState() { - return new SwerveModuleState(); - } - - default SwerveModulePosition getPosition() { - return new SwerveModulePosition(); - } - - default void setState(SwerveModuleState state) {} - - default void resyncEncoders() {} - - default void stop() {} - - default void setNeutralMode( - Constants.NeutralMode driveNeutralMode, Constants.NeutralMode steerNeutralMode) {} -} diff --git a/src/main/java/frc/robot/drive/SwerveModuleIOMK4DualSparkMax.java b/src/main/java/frc/robot/drive/SwerveModuleIOMK4DualSparkMax.java deleted file mode 100644 index 1797fbe..0000000 --- a/src/main/java/frc/robot/drive/SwerveModuleIOMK4DualSparkMax.java +++ /dev/null @@ -1,166 +0,0 @@ -package frc.robot.drive; - -import com.ctre.phoenix.sensors.AbsoluteSensorRange; -import com.ctre.phoenix.sensors.CANCoderConfiguration; -import com.ctre.phoenix.sensors.WPI_CANCoder; -import com.revrobotics.*; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import frc.lib.SparkMaxBurnManager; -import frc.lib.SparkMaxPeriodicFrameConfig; -import frc.robot.constants.Constants; - -public class SwerveModuleIOMK4DualSparkMax implements SwerveModuleIO { - private final CANSparkMax m_driveMotor; - private final CANSparkMax m_steerMotor; - - private final RelativeEncoder m_driveEncoder; - private final RelativeEncoder m_steerEncoder; - - private final SparkMaxPIDController m_driveController; - private final SparkMaxPIDController m_steerController; - - private final WPI_CANCoder m_absoluteEncoder; - - public SwerveModuleIOMK4DualSparkMax( - int driveMotorId, - int steerMotorId, - int absoluteEncoderId, - boolean steerInverted, - double driveConversionFactor, - double steerConversionFactor, - double absoluteEncoderOffsetDegrees) { - m_driveMotor = new CANSparkMax(driveMotorId, CANSparkMaxLowLevel.MotorType.kBrushless); - m_steerMotor = new CANSparkMax(steerMotorId, CANSparkMaxLowLevel.MotorType.kBrushless); - - if (SparkMaxBurnManager.shouldBurnFlash()) { - m_driveMotor.restoreFactoryDefaults(); - m_steerMotor.restoreFactoryDefaults(); - } - - m_driveMotor.setCANTimeout(SparkMaxBurnManager.kConfigurationStatusTimeoutMs); - m_steerMotor.setCANTimeout(SparkMaxBurnManager.kConfigurationStatusTimeoutMs); - - m_driveEncoder = m_driveMotor.getEncoder(); - m_steerEncoder = m_steerMotor.getEncoder(); - - m_driveController = m_driveMotor.getPIDController(); - m_steerController = m_steerMotor.getPIDController(); - - SparkMaxPeriodicFrameConfig.configureIsolated(m_driveMotor); - SparkMaxPeriodicFrameConfig.configureIsolated(m_steerMotor); - - m_steerMotor.setInverted(steerInverted); - - m_driveMotor.setSmartCurrentLimit(40); - m_driveMotor.enableVoltageCompensation(12.0); - - m_steerMotor.setSmartCurrentLimit(20); - m_steerMotor.enableVoltageCompensation(12.0); - - m_driveEncoder.setPositionConversionFactor(driveConversionFactor); - m_driveEncoder.setVelocityConversionFactor(driveConversionFactor / 60.0); - m_driveEncoder.setPosition(0.0); - m_driveEncoder.setMeasurementPeriod(10); - m_driveEncoder.setAverageDepth(2); - - m_steerEncoder.setPositionConversionFactor(steerConversionFactor); - m_steerEncoder.setVelocityConversionFactor(steerConversionFactor / 60.0); - m_steerEncoder.setPosition(0.0); - m_steerEncoder.setMeasurementPeriod(10); - m_steerEncoder.setAverageDepth(2); - - m_driveController.setP(Constants.Drivetrain.ControlValues.Drive.kP); - m_driveController.setI(Constants.Drivetrain.ControlValues.Drive.kI); - m_driveController.setD(Constants.Drivetrain.ControlValues.Drive.kD); - m_driveController.setFF(Constants.Drivetrain.ControlValues.Drive.kFF); - - m_steerController.setP(Constants.Drivetrain.ControlValues.Steer.kP); - m_steerController.setI(Constants.Drivetrain.ControlValues.Steer.kI); - m_steerController.setD(Constants.Drivetrain.ControlValues.Steer.kD); - - m_steerController.setPositionPIDWrappingEnabled(true); - m_steerController.setPositionPIDWrappingMinInput(-Math.PI); - m_steerController.setPositionPIDWrappingMaxInput(Math.PI); - - m_driveMotor.setCANTimeout(0); - m_steerMotor.setCANTimeout(0); - - if (SparkMaxBurnManager.shouldBurnFlash()) { - m_driveMotor.burnFlash(); - m_steerMotor.burnFlash(); - } - - m_absoluteEncoder = new WPI_CANCoder(absoluteEncoderId); - - CANCoderConfiguration config = new CANCoderConfiguration(); - config.sensorCoefficient = 2 * Math.PI / 4096.0; - config.unitString = "rad"; - config.magnetOffsetDegrees = absoluteEncoderOffsetDegrees; - config.absoluteSensorRange = AbsoluteSensorRange.Signed_PlusMinus180; - - m_absoluteEncoder.configAllSettings(config); - - setNeutralMode(Constants.NeutralMode.COAST, Constants.NeutralMode.BRAKE); - } - - @Override - public void updateInputs(SwerveModuleInputs inputs) { - inputs.DriveCurrentAmps = m_driveMotor.getOutputCurrent(); - inputs.DriveTempCelsius = m_driveMotor.getMotorTemperature(); - inputs.DriveAppliedVoltage = m_driveMotor.getAppliedOutput() * m_driveMotor.getBusVoltage(); - inputs.SteerCurrentAmps = m_steerMotor.getOutputCurrent(); - inputs.SteerTempCelsius = m_steerMotor.getMotorTemperature(); - inputs.SteerAppliedVoltage = m_steerMotor.getAppliedOutput() * m_steerMotor.getBusVoltage(); - - inputs.VelocityRadPerSec = m_steerEncoder.getVelocity(); - inputs.VelocityMetersPerSec = m_driveEncoder.getVelocity(); - inputs.AbsolutePositionRad = m_absoluteEncoder.getAbsolutePosition(); - inputs.RelativePositionRad = m_steerEncoder.getPosition(); - inputs.DistanceTraveledMeters = m_driveEncoder.getPosition(); - } - - @Override - public SwerveModuleState getState() { - return new SwerveModuleState( - m_driveEncoder.getVelocity(), - Rotation2d.fromRadians(m_absoluteEncoder.getAbsolutePosition())); - } - - @Override - public SwerveModulePosition getPosition() { - return new SwerveModulePosition( - m_driveEncoder.getPosition(), - Rotation2d.fromRadians(m_absoluteEncoder.getAbsolutePosition())); - } - - @Override - public void setState(SwerveModuleState state) { - // Optimize the SwerveModuleState - state = - SwerveModuleState.optimize( - state, Rotation2d.fromRadians(m_absoluteEncoder.getAbsolutePosition())); - - m_driveController.setReference(state.speedMetersPerSecond, CANSparkMax.ControlType.kVelocity); - m_steerController.setReference(state.angle.getRadians(), CANSparkMax.ControlType.kPosition); - } - - @Override - public void resyncEncoders() { - m_driveEncoder.setPosition(0); - m_steerEncoder.setPosition(m_absoluteEncoder.getAbsolutePosition()); - } - - @Override - public void stop() { - m_driveMotor.stopMotor(); - } - - @Override - public void setNeutralMode( - Constants.NeutralMode driveNeutralMode, Constants.NeutralMode steerNeutralMode) { - m_driveMotor.setIdleMode(driveNeutralMode.toIdleMode()); - m_steerMotor.setIdleMode(steerNeutralMode.toIdleMode()); - } -} diff --git a/src/main/java/frc/robot/drive/SwerveModuleIOSim.java b/src/main/java/frc/robot/drive/SwerveModuleIOSim.java deleted file mode 100644 index a0016c5..0000000 --- a/src/main/java/frc/robot/drive/SwerveModuleIOSim.java +++ /dev/null @@ -1,46 +0,0 @@ -package frc.robot.drive; - -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import frc.robot.constants.Constants; - -public class SwerveModuleIOSim implements SwerveModuleIO { - private final SwerveModuleState m_currentState = new SwerveModuleState(); - private final SwerveModulePosition m_currentPosition = new SwerveModulePosition(); - - @Override - public SwerveModuleState getState() { - return m_currentState; - } - - @Override - public SwerveModulePosition getPosition() { - return m_currentPosition; - } - - @Override - public void setState(SwerveModuleState state) { - state = SwerveModuleState.optimize(state, m_currentState.angle); - - m_currentState.speedMetersPerSecond = state.speedMetersPerSecond; - m_currentState.angle = state.angle; - } - - @Override - public void stop() { - m_currentState.speedMetersPerSecond = 0; - } - - @Override - public void updateInputs(SwerveModuleInputs inputs) { - // Update Position - m_currentPosition.distanceMeters += - m_currentState.speedMetersPerSecond * Constants.loopPeriodSecs; - m_currentPosition.angle = m_currentState.angle; - - inputs.DistanceTraveledMeters = m_currentPosition.distanceMeters; - inputs.VelocityMetersPerSec = m_currentState.speedMetersPerSecond; - - inputs.AbsolutePositionRad = m_currentPosition.angle.getRadians(); - } -} diff --git a/src/main/java/frc/robot/drive/commands/DriveControl.java b/src/main/java/frc/robot/drive/commands/DriveControl.java deleted file mode 100644 index cbcfa48..0000000 --- a/src/main/java/frc/robot/drive/commands/DriveControl.java +++ /dev/null @@ -1,32 +0,0 @@ -package frc.robot.drive.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.constants.Constants; -import frc.robot.drive.DriveBase; -import frc.robot.oi.DriverInterface; - -public class DriveControl extends CommandBase { - private final DriveBase m_driveBase; - private final DriverInterface m_driverInterface; - - public DriveControl(DriveBase driveBase, DriverInterface driverInterface) { - m_driveBase = driveBase; - m_driverInterface = driverInterface; - - addRequirements(driveBase); - } - - @Override - public void execute() { - m_driveBase.setFromForces( - m_driverInterface.getTranslationalLateralPercent() - * Constants.Drivetrain.kMaxVelocityMetersPerSecond, - m_driverInterface.getTranslationalMedialPercent() - * Constants.Drivetrain.kMaxVelocityMetersPerSecond, - m_driverInterface.getRotationalPercent() - * Constants.Drivetrain.kMaxRotationVelocityRadPerSecond, - m_driverInterface.isRobotRelative().getAsBoolean() - ? DriveBase.DriveMode.kRobot - : DriveBase.DriveMode.kField); - } -} diff --git a/src/main/java/frc/robot/drive/gyro/GyroIO.java b/src/main/java/frc/robot/drive/gyro/GyroIO.java deleted file mode 100644 index 4691746..0000000 --- a/src/main/java/frc/robot/drive/gyro/GyroIO.java +++ /dev/null @@ -1,38 +0,0 @@ -package frc.robot.drive.gyro; - -import edu.wpi.first.math.geometry.Rotation2d; -import frc.lib.LoggedIO; -import org.littletonrobotics.junction.AutoLog; - -public interface GyroIO extends LoggedIO { - @AutoLog - public class GyroInputs { - public boolean Connected; - - public double RollPositionRad; - public double PitchPositionRad; - public double YawPositionRad; - - public double RollRateRadPerSecond; - public double PitchRateRadPerSecond; - public double YawRateRadPerSecond; - - public double AccelXGForce; - public double AccelYGForce; - public double AccelZGForce; - } - - /** - * Get the heading of the robot. - * - * @return robot heading. - */ - default Rotation2d getHeading() { - return new Rotation2d(); - } - - /** Reset the yaw of the gyro to 0. */ - default void resetHeading() {} - - default void incrementHeading(double incrementRad) {} -} diff --git a/src/main/java/frc/robot/drive/gyro/GyroIOPigeon2.java b/src/main/java/frc/robot/drive/gyro/GyroIOPigeon2.java deleted file mode 100644 index ba348e5..0000000 --- a/src/main/java/frc/robot/drive/gyro/GyroIOPigeon2.java +++ /dev/null @@ -1,46 +0,0 @@ -package frc.robot.drive.gyro; - -import com.ctre.phoenix.ErrorCode; -import com.ctre.phoenix.sensors.WPI_Pigeon2; -import edu.wpi.first.math.geometry.Rotation2d; - -public class GyroIOPigeon2 implements GyroIO { - private final WPI_Pigeon2 m_gyro; - - private final double[] xyzDegreesPerSecond = new double[3]; - private final short[] xyzAccelData = new short[3]; - - public GyroIOPigeon2(int gyroId) { - m_gyro = new WPI_Pigeon2(gyroId); - } - - @Override - public void updateInputs(GyroInputs inputs) { - m_gyro.getRawGyro(xyzDegreesPerSecond); - m_gyro.getBiasedAccelerometer(xyzAccelData); - - inputs.Connected = m_gyro.getLastError().equals(ErrorCode.OK); - - inputs.RollPositionRad = Math.toRadians(m_gyro.getRoll()); - inputs.PitchPositionRad = Math.toRadians(m_gyro.getPitch()); - inputs.YawPositionRad = Math.toRadians(m_gyro.getYaw()); - - inputs.RollRateRadPerSecond = Math.toRadians(xyzDegreesPerSecond[0]); - inputs.PitchRateRadPerSecond = Math.toRadians(xyzDegreesPerSecond[1]); - inputs.YawRateRadPerSecond = Math.toRadians(xyzDegreesPerSecond[2]); - - inputs.AccelXGForce = (double) xyzAccelData[0] / (1 << 14); - inputs.AccelYGForce = (double) xyzAccelData[1] / (1 << 14); - inputs.AccelZGForce = (double) xyzAccelData[2] / (1 << 14); - } - - @Override - public Rotation2d getHeading() { - return m_gyro.getRotation2d(); - } - - @Override - public void resetHeading() { - m_gyro.reset(); - } -} diff --git a/src/main/java/frc/robot/drive/gyro/GyroIOSim.java b/src/main/java/frc/robot/drive/gyro/GyroIOSim.java deleted file mode 100644 index 298184c..0000000 --- a/src/main/java/frc/robot/drive/gyro/GyroIOSim.java +++ /dev/null @@ -1,29 +0,0 @@ -package frc.robot.drive.gyro; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.simulation.AnalogGyroSim; - -public class GyroIOSim implements GyroIO { - private final AnalogGyroSim m_gyro = new AnalogGyroSim(1); - - @Override - public void updateInputs(GyroInputs inputs) { - inputs.YawPositionRad = m_gyro.getAngle(); - inputs.YawRateRadPerSecond = m_gyro.getRate(); - } - - @Override - public Rotation2d getHeading() { - return Rotation2d.fromRadians(m_gyro.getAngle()); - } - - @Override - public void resetHeading() { - m_gyro.setAngle(0); - } - - @Override - public void incrementHeading(double incrementRad) { - m_gyro.setAngle(m_gyro.getAngle() + incrementRad); - } -} diff --git a/src/main/java/frc/robot/oi/DriverInterface.java b/src/main/java/frc/robot/oi/DriverInterface.java deleted file mode 100644 index 6aff7f6..0000000 --- a/src/main/java/frc/robot/oi/DriverInterface.java +++ /dev/null @@ -1,13 +0,0 @@ -package frc.robot.oi; - -import edu.wpi.first.wpilibj2.command.button.Trigger; - -public interface DriverInterface { - public double getTranslationalMedialPercent(); - - public double getTranslationalLateralPercent(); - - public double getRotationalPercent(); - - public Trigger isRobotRelative(); -} diff --git a/src/main/java/frc/robot/oi/DualJoystickDriver.java b/src/main/java/frc/robot/oi/DualJoystickDriver.java deleted file mode 100644 index 468c5cc..0000000 --- a/src/main/java/frc/robot/oi/DualJoystickDriver.java +++ /dev/null @@ -1,35 +0,0 @@ -package frc.robot.oi; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj2.command.button.CommandJoystick; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -public class DualJoystickDriver implements DriverInterface { - private final CommandJoystick m_leftJoystick; - private final CommandJoystick m_rightJoystick; - - public DualJoystickDriver(int leftPort, int rightPort) { - m_leftJoystick = new CommandJoystick(leftPort); - m_rightJoystick = new CommandJoystick(rightPort); - } - - @Override - public double getTranslationalMedialPercent() { - return MathUtil.applyDeadband(-m_leftJoystick.getY(), 0.15); - } - - @Override - public double getTranslationalLateralPercent() { - return MathUtil.applyDeadband(m_leftJoystick.getX(), 0.15); - } - - @Override - public double getRotationalPercent() { - return -MathUtil.applyDeadband(m_rightJoystick.getX(), 0.15); - } - - @Override - public Trigger isRobotRelative() { - return m_leftJoystick.button(1); - } -} diff --git a/src/main/java/frc/robot/oi/OIManager.java b/src/main/java/frc/robot/oi/OIManager.java deleted file mode 100644 index 21cbe8b..0000000 --- a/src/main/java/frc/robot/oi/OIManager.java +++ /dev/null @@ -1,20 +0,0 @@ -package frc.robot.oi; - -import edu.wpi.first.wpilibj.RobotBase; -import frc.robot.constants.HardwareDevices; - -public class OIManager { - private final DriverInterface m_driverInterface; - - public OIManager() { - if (RobotBase.isSimulation()) { - m_driverInterface = new PS4Driver(HardwareDevices.kDriverControllerPort); - } else { - m_driverInterface = new XboxDriver(HardwareDevices.kDriverControllerPort); - } - } - - public DriverInterface getDriverInterface() { - return m_driverInterface; - } -} diff --git a/src/main/java/frc/robot/oi/PS4Driver.java b/src/main/java/frc/robot/oi/PS4Driver.java deleted file mode 100644 index 81127a3..0000000 --- a/src/main/java/frc/robot/oi/PS4Driver.java +++ /dev/null @@ -1,33 +0,0 @@ -package frc.robot.oi; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -public class PS4Driver implements DriverInterface { - private final CommandPS4Controller m_controller; - - public PS4Driver(int port) { - m_controller = new CommandPS4Controller(port); - } - - @Override - public double getTranslationalMedialPercent() { - return MathUtil.applyDeadband(-m_controller.getLeftY(), 0.15); - } - - @Override - public double getTranslationalLateralPercent() { - return MathUtil.applyDeadband(m_controller.getLeftX(), 0.15); - } - - @Override - public double getRotationalPercent() { - return -MathUtil.applyDeadband(m_controller.getRightX(), 0.15); - } - - @Override - public Trigger isRobotRelative() { - return m_controller.R1(); - } -} diff --git a/src/main/java/frc/robot/oi/XboxDriver.java b/src/main/java/frc/robot/oi/XboxDriver.java deleted file mode 100644 index 04e18e1..0000000 --- a/src/main/java/frc/robot/oi/XboxDriver.java +++ /dev/null @@ -1,33 +0,0 @@ -package frc.robot.oi; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -public class XboxDriver implements DriverInterface { - private final CommandXboxController m_controller; - - public XboxDriver(int port) { - m_controller = new CommandXboxController(port); - } - - @Override - public double getTranslationalMedialPercent() { - return MathUtil.applyDeadband(-m_controller.getLeftY(), 0.15); - } - - @Override - public double getTranslationalLateralPercent() { - return MathUtil.applyDeadband(m_controller.getLeftX(), 0.15); - } - - @Override - public double getRotationalPercent() { - return -MathUtil.applyDeadband(m_controller.getRightX(), 0.15); - } - - @Override - public Trigger isRobotRelative() { - return m_controller.rightBumper(); - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveBase.java b/src/main/java/frc/robot/subsystems/drive/DriveBase.java new file mode 100644 index 0000000..cbb8461 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveBase.java @@ -0,0 +1,162 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +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.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.Constants; +import frc.robot.util.PoseEstimator; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class DriveBase extends SubsystemBase { + public static double ODOMETRY_FREQUENCY = 250.0; + + private final GyroIO m_gyroIO; + private final GyroIOInputsAutoLogged m_gyroInputs = new GyroIOInputsAutoLogged(); + private final Module[] m_modules = new Module[4]; // FL, FR, BL, BR + + private Rotation2d m_lastGyroRotation = new Rotation2d(); + + public DriveBase( + GyroIO gyroIO, + ModuleIO frontLeftIO, + ModuleIO frontRightIO, + ModuleIO backLeftIO, + ModuleIO backRightIO) { + this.m_gyroIO = gyroIO; + + m_modules[0] = new Module(0, frontLeftIO); + m_modules[1] = new Module(1, frontRightIO); + m_modules[2] = new Module(2, backLeftIO); + m_modules[3] = new Module(3, backRightIO); + } + + public void periodic() { + PoseEstimator.odometryLock.lock(); + m_gyroIO.updateInputs(m_gyroInputs); + for (var module : m_modules) { + module.updateInputs(); + } + PoseEstimator.odometryLock.unlock(); + + Logger.processInputs("Drive/Gyro", m_gyroInputs); + for (var module : m_modules) { + module.periodic(); + } + + if (DriverStation.isDisabled()) { + // Stop moving when disabled + for (var module : m_modules) { + module.disable(); + } + + // Log empty setpoint states when disabled + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + } + + // Update odometry + int deltaCount = + m_gyroInputs.connected ? m_gyroInputs.odometryYawPositions.length : Integer.MAX_VALUE; + for (var module : m_modules) { + deltaCount = Math.min(deltaCount, module.getPositionDeltas().length); + } + for (int i = 0; i < deltaCount; i++) { + SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4]; + for (int j = 0; j < 4; j++) { + wheelDeltas[j] = m_modules[j].getPositionDeltas()[i]; + } + + // The twist represents the motion of the robot since the last + // sample in x, y, and theta based only on the modules, without + // the gyro. The gyro is always disconnected in simulation. + var twist = Constants.Drivetrain.m_kinematics.toTwist2d(wheelDeltas); + if (m_gyroInputs.connected) { + // If the gyro is connected, replace the theta component of the twist + // with the change in angle since the last sample. + Rotation2d gyroRotation = m_gyroInputs.odometryYawPositions[i]; + twist = + new Twist2d(twist.dx, twist.dy, gyroRotation.minus(m_lastGyroRotation).getRadians()); + m_lastGyroRotation = gyroRotation; + } + + // Apply the twist (change since last sample) to the current pose + PoseEstimator.getInstance().addDriveData(twist); + } + + Logger.recordOutput("Odometry/EstimatedPose", PoseEstimator.getInstance().getPose()); + } + + /** + * Runs the drive at the desired velocity. + * + * @param speeds Speeds in meters/sec + */ + public void runVelocity(ChassisSpeeds speeds) { + // Calculate module setpoints + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); + SwerveModuleState[] setpointStates = + Constants.Drivetrain.m_kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds( + setpointStates, Constants.Drivetrain.kMaxLinearVelocityMetersPerSecond); + + // Send setpoints to modules + SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + // The module returns the optimized state, useful for logging + optimizedSetpointStates[i] = m_modules[i].runSetpoint(setpointStates[i]); + } + + // Log setpoint states + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); + } + + /** Stops the drive. */ + public void stop() { + runVelocity(new ChassisSpeeds()); + } + + /** + * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will + * return to their normal orientations the next time a nonzero velocity is requested. + */ + public void stopWithX() { + var translations = Constants.Drivetrain.getModuleTranslations(); + Rotation2d[] headings = new Rotation2d[4]; + for (int i = 0; i < 4; i++) { + headings[i] = translations[i].getAngle(); + } + Constants.Drivetrain.m_kinematics.resetHeadings(headings); + stop(); + } + + /** Returns the module states (turn angles and drive velocities) for all the modules. */ + @AutoLogOutput(key = "SwerveStates/Measured") + private SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = m_modules[i].getState(); + } + return states; + } + + public void runCharacterizationVolts(double volts) { + for (var module : m_modules) { + module.runCharacterization(volts); + } + } + + public double getCharacterizationVelocity() { + double driveVelocityAverage = 0.0; + for (var module : m_modules) { + driveVelocityAverage += module.getCharacterizationVelocity(); + } + return driveVelocityAverage / 4.0; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java new file mode 100644 index 0000000..b47de3c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -0,0 +1,27 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface GyroIO { + @AutoLog + public static class GyroIOInputs { + public boolean connected = false; + + public Rotation2d rollPosition = new Rotation2d(); + public Rotation2d pitchPosition = new Rotation2d(); + public Rotation2d yawPosition = new Rotation2d(); + + public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; + + public double rollVelocityRadPerSec = 0.0; + public double pitchVelocityRadPerSec = 0.0; + public double yawVelocityRadPerSec = 0.0; + + public double accelX = 0.0; + public double accelY = 0.0; + public double accelZ = 0.0; + } + + public default void updateInputs(GyroIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java new file mode 100644 index 0000000..82e8205 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -0,0 +1,83 @@ +package frc.robot.subsystems.drive; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +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 java.util.Queue; + +/** IO implementation for Pigeon2 */ +public class GyroIOPigeon2 implements GyroIO { + private final Pigeon2 m_gyro; + + private final StatusSignal m_roll; + private final StatusSignal m_pitch; + private final StatusSignal m_yaw; + + private final StatusSignal m_rollVelocity; + private final StatusSignal m_pitchVelocity; + private final StatusSignal m_yawVelocity; + + private final StatusSignal m_accelX; + private final StatusSignal m_accelY; + private final StatusSignal m_accelZ; + + private final Queue yawPositionQueue; + + public GyroIOPigeon2(int id) { + this.m_gyro = new Pigeon2(id); + + this.m_gyro.getConfigurator().apply(new Pigeon2Configuration()); + this.m_gyro.getConfigurator().setYaw(0.0); + + this.m_roll = this.m_gyro.getRoll(); + this.m_pitch = this.m_gyro.getPitch(); + this.m_yaw = this.m_gyro.getYaw(); + + this.m_rollVelocity = this.m_gyro.getAngularVelocityX(); + this.m_pitchVelocity = this.m_gyro.getAngularVelocityY(); + this.m_yawVelocity = this.m_gyro.getAngularVelocityZ(); + + this.m_accelX = this.m_gyro.getAccelerationX(); + this.m_accelY = this.m_gyro.getAccelerationY(); + this.m_accelZ = this.m_gyro.getAccelerationZ(); + + // Faster rate for Yaw for Odometry + this.m_yaw.setUpdateFrequency(DriveBase.ODOMETRY_FREQUENCY); + this.m_yawVelocity.setUpdateFrequency(100); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, m_roll, m_pitch, m_rollVelocity, m_pitchVelocity, m_accelX, m_accelY, m_accelZ); + m_gyro.optimizeBusUtilization(); + + this.yawPositionQueue = + SparkMaxOdometryThread.getInstance() + .registerSignal(() -> m_gyro.getYaw().getValueAsDouble()); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + // Only check yaw and yaw velocity as they are needed for odometry + inputs.connected = BaseStatusSignal.refreshAll(m_yaw, m_yawVelocity).equals(StatusCode.OK); + + inputs.rollPosition = Rotation2d.fromDegrees(m_roll.getValueAsDouble()); + inputs.pitchPosition = Rotation2d.fromDegrees(m_pitch.getValueAsDouble()); + inputs.yawPosition = Rotation2d.fromDegrees(m_yaw.getValueAsDouble()); + + inputs.rollVelocityRadPerSec = Units.degreesToRadians(m_rollVelocity.getValueAsDouble()); + inputs.pitchVelocityRadPerSec = Units.degreesToRadians(m_pitchVelocity.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(m_yawVelocity.getValueAsDouble()); + + inputs.accelX = m_accelX.getValueAsDouble(); + inputs.accelY = m_accelY.getValueAsDouble(); + inputs.accelZ = m_accelZ.getValueAsDouble(); + + inputs.odometryYawPositions = + yawPositionQueue.stream() + .map((Double value) -> Rotation2d.fromDegrees(value)) + .toArray(Rotation2d[]::new); + this.yawPositionQueue.clear(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java new file mode 100644 index 0000000..61a3c0f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -0,0 +1,186 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import frc.robot.constants.Constants; +import frc.robot.constants.Constants.Drivetrain.DriveCoefficients; +import frc.robot.constants.Constants.Drivetrain.TurnCoefficients; +import org.littletonrobotics.junction.Logger; + +public class Module { + private final ModuleIO m_io; + private final ModuleIOInputsAutoLogged m_inputs = new ModuleIOInputsAutoLogged(); + private final int kModuleIndex; + + private final SimpleMotorFeedforward m_driveFeedforward; + private final PIDController m_driveController; + private final PIDController m_turnController; + + private Rotation2d m_turnAngleSetpoint = null; + private Double m_driveVelocitySetpoint = null; + + private Rotation2d m_turnRelativeOffset = null; + private double m_lastPositionMeters; + private SwerveModulePosition[] positionDeltas = new SwerveModulePosition[] {}; + + public Module(int index, ModuleIO io) { + this.kModuleIndex = index; + this.m_io = io; + + switch (Constants.getRobotMode()) { + case REPLAY, REAL: + m_driveFeedforward = + new SimpleMotorFeedforward( + DriveCoefficients.kS, DriveCoefficients.kV, DriveCoefficients.kA); + m_driveController = + new PIDController(DriveCoefficients.kP, DriveCoefficients.kI, DriveCoefficients.kD); + m_turnController = + new PIDController(TurnCoefficients.kP, TurnCoefficients.kI, TurnCoefficients.kD); + break; + case SIM: + m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); + m_driveController = new PIDController(0.1, 0, 0); + m_turnController = new PIDController(10.0, 0, 0); + break; + default: + m_driveFeedforward = new SimpleMotorFeedforward(0, 0); + m_driveController = new PIDController(0, 0, 0); + m_turnController = new PIDController(0, 0, 0); + break; + } + + m_turnController.enableContinuousInput(-Math.PI, Math.PI); + setBrakeMode(true); + } + + /** Updates the inputs of the module. Should be called before {@link #periodic} */ + public void updateInputs() { + m_io.updateInputs(m_inputs); + } + + public void periodic() { + Logger.processInputs("Drive/Module" + kModuleIndex, m_inputs); + + // On first cycle, reset relative turn encoder + // Wait until absolute angle is nonzero in case it wasn't initialized yet + if (m_turnRelativeOffset == null && m_inputs.turnAbsolutePosition.getRadians() != 0.0) { + m_turnRelativeOffset = m_inputs.turnAbsolutePosition.minus(m_inputs.turnPosition); + } + + // Run closed loop turn control + if (m_turnAngleSetpoint != null) { + m_io.setTurnVoltage( + m_turnController.calculate(getAngle().getRadians(), m_turnAngleSetpoint.getRadians())); + + // Run closed loop drive control. Only if turn closed loop is enabled. + if (m_driveVelocitySetpoint != null) { + // When the error is 90°, the velocity setpoint should be 0. As the wheel turns + // towards the setpoint, its velocity should increase. This is achieved by + // taking the component of the velocity in the direction of the setpoint. + double adjustSpeedSetpoint = + m_driveVelocitySetpoint * Math.cos(m_turnController.getPositionError()); + + // Run drive controller + double velocityRadPerSec = adjustSpeedSetpoint / Constants.Drivetrain.kWheelRadiusMeters; + m_io.setDriveVoltage( + m_driveFeedforward.calculate(velocityRadPerSec) + + m_driveController.calculate(m_inputs.driveVelocityRadPerSec, velocityRadPerSec)); + } + } + + // Calculate position deltas for odometry + int deltaCount = + Math.min(m_inputs.odometryDrivePositionsRad.length, m_inputs.odometryTurnPositions.length); + positionDeltas = new SwerveModulePosition[deltaCount]; + for (int i = 0; i < deltaCount; i++) { + double positionMeters = + m_inputs.odometryDrivePositionsRad[i] * Constants.Drivetrain.kWheelRadiusMeters; + Rotation2d angle = + m_inputs.odometryTurnPositions[i].plus( + m_turnRelativeOffset != null ? m_turnRelativeOffset : new Rotation2d()); + positionDeltas[i] = new SwerveModulePosition(positionMeters - m_lastPositionMeters, angle); + m_lastPositionMeters = positionMeters; + } + } + + /** + * Runs the module with the specified setpoint state. Must be called periodically. Returns the + * optimized state. + */ + public SwerveModuleState runSetpoint(SwerveModuleState state) { + // Optimize state based on current angle + // Controllers run in "periodic" when the setpoint is not null + var optimizedState = SwerveModuleState.optimize(state, getAngle()); + + // Update setpoints, controllers run in "periodic" + m_turnAngleSetpoint = optimizedState.angle; + m_driveVelocitySetpoint = optimizedState.speedMetersPerSecond; + + return optimizedState; + } + + public void runCharacterization(double volts) { + // Closed loop turn control + m_turnAngleSetpoint = new Rotation2d(); + + // Open loop drive control + m_io.setDriveVoltage(volts); + m_driveVelocitySetpoint = null; + } + + public double getCharacterizationVelocity() { + return m_inputs.driveVelocityRadPerSec; + } + + /** Disables all outputs to motors. */ + public void disable() { + m_io.setTurnVoltage(0.0); + m_io.setDriveVoltage(0.0); + + m_turnAngleSetpoint = null; + m_driveVelocitySetpoint = null; + } + + /** Sets whether brake mode is enabled. */ + public void setBrakeMode(boolean enabled) { + m_io.setDriveBrakeMode(enabled); + m_io.setTurnBrakeMode(enabled); + } + + /** Returns the current turn angle of the module. */ + public Rotation2d getAngle() { + if (m_turnRelativeOffset == null) { + return new Rotation2d(); + } else { + return m_inputs.turnPosition.plus(m_turnRelativeOffset); + } + } + + /** Returns the current drive position of the module in meters. */ + public double getPositionMeters() { + return m_inputs.drivePositionRad * Constants.Drivetrain.kWheelRadiusMeters; + } + + /** Returns the current drive velocity of the module in meters per second. */ + public double getVelocityMetersPerSec() { + return m_inputs.driveVelocityRadPerSec * Constants.Drivetrain.kWheelRadiusMeters; + } + + /** Returns the module position (turn angle and drive position). */ + 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 position deltas received this cycle. */ + public SwerveModulePosition[] getPositionDeltas() { + return positionDeltas; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java new file mode 100644 index 0000000..684f3cd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -0,0 +1,38 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface ModuleIO { + @AutoLog + public static class ModuleIOInputs { + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double[] driveCurrentAmps = new double[] {}; + + public Rotation2d turnAbsolutePosition = new Rotation2d(); + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double[] turnCurrentAmps = new double[] {}; + + public double[] odometryDrivePositionsRad = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} + + /** Run the drive motor at the specified voltage. */ + public default void setDriveVoltage(double volts) {} + + /** Run the turn motor at the specified voltage. */ + public default void setTurnVoltage(double volts) {} + + /** Enable or disable brake mode on the drive motor. */ + public default void setDriveBrakeMode(boolean enable) {} + + /** Enable or disable brake mode on the turn motor. */ + public default void setTurnBrakeMode(boolean enable) {} +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java new file mode 100644 index 0000000..8c17206 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -0,0 +1,61 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.constants.Constants; + +/** + * Physics sim implementation of module IO. + * + *

Uses two DCMotor sims for the drive and turn motors, with the absolute position initialized to + * a random value. The DCMotor sims are not physically accurate, but provide a decent approximation + * for the behavior of the module. + */ +public class ModuleIOSim implements ModuleIO { + private final DCMotorSim m_driveSim; + private final DCMotorSim m_turnSim; + + private final Rotation2d turnAbsoluteInitPosition = new Rotation2d(Math.random() * 2.0 * Math.PI); + private double driveAppliedVolts = 0.0; + private double turnAppliedVolts = 0.0; + + public ModuleIOSim() { + this.m_driveSim = new DCMotorSim(DCMotor.getNEO(1), Constants.Drivetrain.kDriveGearing, 0.025); + this.m_turnSim = new DCMotorSim(DCMotor.getNEO(1), Constants.Drivetrain.kTurnGearing, 0.004); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + m_driveSim.update(Constants.kLoopPeriodSecs); + m_turnSim.update(Constants.kLoopPeriodSecs); + + inputs.drivePositionRad = m_driveSim.getAngularPositionRad(); + inputs.driveVelocityRadPerSec = m_driveSim.getAngularVelocityRadPerSec(); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveCurrentAmps = new double[] {Math.abs(m_driveSim.getCurrentDrawAmps())}; + + inputs.turnAbsolutePosition = + new Rotation2d(m_turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); + inputs.turnPosition = new Rotation2d(m_turnSim.getAngularPositionRad()); + inputs.turnVelocityRadPerSec = m_turnSim.getAngularVelocityRadPerSec(); + inputs.turnAppliedVolts = turnAppliedVolts; + inputs.turnCurrentAmps = new double[] {Math.abs(m_turnSim.getCurrentDrawAmps())}; + + inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; + inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; + } + + @Override + public void setDriveVoltage(double volts) { + driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + m_driveSim.setInputVoltage(driveAppliedVolts); + } + + @Override + public void setTurnVoltage(double volts) { + turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + m_turnSim.setInputVoltage(turnAppliedVolts); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java new file mode 100644 index 0000000..ec755fe --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -0,0 +1,135 @@ +package frc.robot.subsystems.drive; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import frc.robot.constants.Constants; +import java.util.Queue; + +public class ModuleIOSparkMax implements ModuleIO { + private final CANSparkMax m_driveMotor; + private final CANSparkMax m_turnMotor; + private final CANcoder m_absoluteEncoder; + + private final RelativeEncoder m_driveEncoder; + private final RelativeEncoder m_turnRelativeEncoder; + private final StatusSignal m_turnAbsoluteEncoder; + + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + public ModuleIOSparkMax(int driveID, int turnID, int encoderID) { + this.m_driveMotor = new CANSparkMax(driveID, MotorType.kBrushless); + this.m_turnMotor = new CANSparkMax(turnID, MotorType.kBrushless); + this.m_absoluteEncoder = new CANcoder(encoderID); + + this.m_driveMotor.restoreFactoryDefaults(); + this.m_turnMotor.restoreFactoryDefaults(); + this.m_absoluteEncoder.getConfigurator().apply(new CANcoderConfiguration()); + + this.m_driveMotor.setCANTimeout(250); + this.m_turnMotor.setCANTimeout(250); + + this.m_driveEncoder = this.m_driveMotor.getEncoder(); + this.m_turnRelativeEncoder = this.m_turnMotor.getEncoder(); + this.m_turnAbsoluteEncoder = this.m_absoluteEncoder.getAbsolutePosition(); + + this.m_turnMotor.setInverted(Constants.Drivetrain.kTurnMotorInverted); + this.m_driveMotor.setSmartCurrentLimit(40); + this.m_turnMotor.setSmartCurrentLimit(30); + this.m_driveMotor.enableVoltageCompensation(12.0); + this.m_turnMotor.enableVoltageCompensation(12.0); + + this.m_driveEncoder.setPosition(0.0); + this.m_driveEncoder.setMeasurementPeriod(10); + this.m_driveEncoder.setAverageDepth(2); + + this.m_turnRelativeEncoder.setPosition(0.0); + this.m_turnRelativeEncoder.setMeasurementPeriod(10); + this.m_turnRelativeEncoder.setAverageDepth(2); + + this.m_turnAbsoluteEncoder.setUpdateFrequency(50); + + this.m_driveMotor.setCANTimeout(0); + this.m_turnMotor.setCANTimeout(0); + + this.m_driveMotor.setPeriodicFramePeriod( + PeriodicFrame.kStatus2, (int) (1000.0 / DriveBase.ODOMETRY_FREQUENCY)); + this.m_turnMotor.setPeriodicFramePeriod( + PeriodicFrame.kStatus2, (int) (1000.0 / DriveBase.ODOMETRY_FREQUENCY)); + this.drivePositionQueue = + SparkMaxOdometryThread.getInstance().registerSignal(m_driveEncoder::getPosition); + this.turnPositionQueue = + SparkMaxOdometryThread.getInstance().registerSignal(m_turnRelativeEncoder::getPosition); + + this.m_driveMotor.burnFlash(); + this.m_turnMotor.burnFlash(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + inputs.drivePositionRad = + Units.rotationsToRadians(m_driveEncoder.getPosition()) / Constants.Drivetrain.kDriveGearing; + inputs.driveVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(m_driveEncoder.getVelocity()) + / Constants.Drivetrain.kDriveGearing; + inputs.driveAppliedVolts = m_driveMotor.getAppliedOutput() * m_driveMotor.getBusVoltage(); + inputs.driveCurrentAmps = new double[] {m_driveMotor.getOutputCurrent()}; + + // Refresh the Encoder data becasue it is cached. This is non-blocking. + m_turnAbsoluteEncoder.refresh(); + inputs.turnAbsolutePosition = + Rotation2d.fromRotations(m_turnAbsoluteEncoder.getValueAsDouble()); + + inputs.turnPosition = + Rotation2d.fromRotations( + m_turnRelativeEncoder.getPosition() / Constants.Drivetrain.kTurnGearing); + inputs.turnVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(m_turnRelativeEncoder.getVelocity()) + / Constants.Drivetrain.kTurnGearing; + inputs.turnAppliedVolts = m_turnMotor.getAppliedOutput() * m_turnMotor.getBusVoltage(); + inputs.turnCurrentAmps = new double[] {m_turnMotor.getOutputCurrent()}; + + inputs.odometryDrivePositionsRad = + this.drivePositionQueue.stream() + .mapToDouble( + (Double value) -> + Units.rotationsToRadians(value) / Constants.Drivetrain.kDriveGearing) + .toArray(); + inputs.odometryTurnPositions = + this.turnPositionQueue.stream() + .map( + (Double value) -> + Rotation2d.fromRotations(value / Constants.Drivetrain.kTurnGearing)) + .toArray(Rotation2d[]::new); + this.drivePositionQueue.clear(); + this.turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + m_driveMotor.setVoltage(volts); + } + + @Override + public void setTurnVoltage(double volts) { + m_turnMotor.setVoltage(volts); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + m_driveMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + m_turnMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java new file mode 100644 index 0000000..38fbb2e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java @@ -0,0 +1,59 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.wpilibj.Notifier; +import frc.robot.util.PoseEstimator; +import java.util.ArrayList; +import java.util.List; +import java.util.Queue; +import java.util.concurrent.ArrayBlockingQueue; +import java.util.function.DoubleSupplier; + +/** + * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. + * + *

This version is intended for devices like the SparkMax that require polling rather than a + * blocking thread. A Notifier thread is used to gather samples with consistent timing. + */ +public class SparkMaxOdometryThread { + private List signals = new ArrayList<>(); + private List> queues = new ArrayList<>(); + + private final Notifier notifier; + private static SparkMaxOdometryThread instance = null; + + public static SparkMaxOdometryThread getInstance() { + if (instance == null) { + instance = new SparkMaxOdometryThread(); + } + return instance; + } + + private SparkMaxOdometryThread() { + notifier = new Notifier(this::periodic); + notifier.setName("SparkMaxOdometryThread"); + notifier.startPeriodic(1.0 / DriveBase.ODOMETRY_FREQUENCY); + } + + public Queue registerSignal(DoubleSupplier signal) { + Queue queue = new ArrayBlockingQueue<>(100); + PoseEstimator.odometryLock.lock(); + try { + signals.add(signal); + queues.add(queue); + } finally { + PoseEstimator.odometryLock.unlock(); + } + return queue; + } + + private void periodic() { + PoseEstimator.odometryLock.lock(); + try { + for (int i = 0; i < signals.size(); i++) { + queues.get(i).offer(signals.get(i).getAsDouble()); + } + } finally { + PoseEstimator.odometryLock.unlock(); + } + } +} diff --git a/src/main/java/frc/robot/util/LoggerUtil.java b/src/main/java/frc/robot/util/LoggerUtil.java new file mode 100644 index 0000000..fbdae2d --- /dev/null +++ b/src/main/java/frc/robot/util/LoggerUtil.java @@ -0,0 +1,27 @@ +package frc.robot.util; + +import edu.wpi.first.wpilibj.RobotBase; +import frc.generated.BuildConstants; +import frc.robot.constants.Constants; +import org.littletonrobotics.junction.Logger; + +public class LoggerUtil { + /** Initialize the Logger with the auto-generated data from the build. */ + public static void initializeLoggerMetadata() { + // Record metadata from generated state file. + Logger.recordMetadata("ROBOT_NAME", Constants.getRobotType().toString()); + Logger.recordMetadata("RUNTIME_ENVIRONMENT", RobotBase.getRuntimeType().toString()); + Logger.recordMetadata("PROJECT_NAME", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BUILD_DATE", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GIT_SHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GIT_DATE", BuildConstants.GIT_DATE); + Logger.recordMetadata("GIT_BRANCH", BuildConstants.GIT_BRANCH); + + // Set the current GIT state of the robot (helps manage the logs that are saved). + switch (BuildConstants.DIRTY) { + case 0 -> Logger.recordMetadata("GIT_STATUS", "All changes committed"); + case 1 -> Logger.recordMetadata("GIT_STATUS", "Uncommitted changes"); + default -> Logger.recordMetadata("GIT_STATUS", "Unknown"); + } + } +} diff --git a/src/main/java/frc/robot/util/PolynomialRegression.java b/src/main/java/frc/robot/util/PolynomialRegression.java new file mode 100644 index 0000000..28c5796 --- /dev/null +++ b/src/main/java/frc/robot/util/PolynomialRegression.java @@ -0,0 +1,205 @@ +package frc.robot.util; + +import Jama.Matrix; +import Jama.QRDecomposition; + +// NOTE: This file is available at +// http://algs4.cs.princeton.edu/14analysis/PolynomialRegression.java.html + +/** + * The {@code PolynomialRegression} class performs a polynomial regression on an set of N + * data points (yi, xi). That is, it fits a polynomial + * y = β0 + β1 x + β2 + * x2 + ... + βd xd (where + * y is the response variable, x is the predictor variable, and the + * βi are the regression coefficients) that minimizes the sum of squared + * residuals of the multiple regression model. It also computes associated the coefficient of + * determination R2. + * + *

This implementation performs a QR-decomposition of the underlying Vandermonde matrix, so it is + * neither the fastest nor the most numerically stable way to perform the polynomial regression. + * + * @author Robert Sedgewick + * @author Kevin Wayne + */ +public class PolynomialRegression implements Comparable { + private final String variableName; // name of the predictor variable + private int degree; // degree of the polynomial regression + private Matrix beta; // the polynomial regression coefficients + private double sse; // sum of squares due to error + private double sst; // total sum of squares + + /** + * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. Uses n as the name + * of the predictor variable. + * + * @param x the values of the predictor variable + * @param y the corresponding values of the response variable + * @param degree the degree of the polynomial to fit + * @throws IllegalArgumentException if the lengths of the two arrays are not equal + */ + public PolynomialRegression(double[] x, double[] y, int degree) { + this(x, y, degree, "n"); + } + + /** + * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. + * + * @param x the values of the predictor variable + * @param y the corresponding values of the response variable + * @param degree the degree of the polynomial to fit + * @param variableName the name of the predictor variable + * @throws IllegalArgumentException if the lengths of the two arrays are not equal + */ + public PolynomialRegression(double[] x, double[] y, int degree, String variableName) { + this.degree = degree; + this.variableName = variableName; + + int n = x.length; + QRDecomposition qr = null; + Matrix matrixX = null; + + // in case Vandermonde matrix does not have full rank, reduce degree until it + // does + while (true) { + + // build Vandermonde matrix + double[][] vandermonde = new double[n][this.degree + 1]; + for (int i = 0; i < n; i++) { + for (int j = 0; j <= this.degree; j++) { + vandermonde[i][j] = Math.pow(x[i], j); + } + } + matrixX = new Matrix(vandermonde); + + // find least squares solution + qr = new QRDecomposition(matrixX); + if (qr.isFullRank()) break; + + // decrease degree and try again + this.degree--; + } + + // create matrix from vector + Matrix matrixY = new Matrix(y, n); + + // linear regression coefficients + beta = qr.solve(matrixY); + + // mean of y[] values + double sum = 0.0; + for (int i = 0; i < n; i++) sum += y[i]; + double mean = sum / n; + + // total variation to be accounted for + for (int i = 0; i < n; i++) { + double dev = y[i] - mean; + sst += dev * dev; + } + + // variation not accounted for + Matrix residuals = matrixX.times(beta).minus(matrixY); + sse = residuals.norm2() * residuals.norm2(); + } + + /** + * Returns the {@code j}th regression coefficient. + * + * @param j the index + * @return the {@code j}th regression coefficient + */ + public double beta(int j) { + // to make -0.0 print as 0.0 + if (Math.abs(beta.get(j, 0)) < 1E-4) return 0.0; + return beta.get(j, 0); + } + + /** + * Returns the degree of the polynomial to fit. + * + * @return the degree of the polynomial to fit + */ + public int degree() { + return degree; + } + + /** + * Returns the coefficient of determination R2. + * + * @return the coefficient of determination R2, which is a real number between + * 0 and 1 + */ + public double R2() { + if (sst == 0.0) return 1.0; // constant function + return 1.0 - sse / sst; + } + + /** + * Returns the expected response {@code y} given the value of the predictor variable {@code x}. + * + * @param x the value of the predictor variable + * @return the expected response {@code y} given the value of the predictor variable {@code x} + */ + public double predict(double x) { + // horner's method + double y = 0.0; + for (int j = degree; j >= 0; j--) y = beta(j) + (x * y); + return y; + } + + /** + * Returns a string representation of the polynomial regression model. + * + * @return a string representation of the polynomial regression model, including the best-fit + * polynomial and the coefficient of determination R2 + */ + public String toString() { + StringBuilder s = new StringBuilder(); + int j = degree; + + // ignoring leading zero coefficients + while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--; + + // create remaining terms + while (j >= 0) { + if (j == 0) s.append(String.format("%.10f ", beta(j))); + else if (j == 1) s.append(String.format("%.10f %s + ", beta(j), variableName)); + else s.append(String.format("%.10f %s^%d + ", beta(j), variableName, j)); + j--; + } + s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")"); + + // replace "+ -2n" with "- 2n" + return s.toString().replace("+ -", "- "); + } + + /** Compare lexicographically. */ + public int compareTo(PolynomialRegression that) { + double EPSILON = 1E-5; + int maxDegree = Math.max(this.degree(), that.degree()); + for (int j = maxDegree; j >= 0; j--) { + double term1 = 0.0; + double term2 = 0.0; + if (this.degree() >= j) term1 = this.beta(j); + if (that.degree() >= j) term2 = that.beta(j); + if (Math.abs(term1) < EPSILON) term1 = 0.0; + if (Math.abs(term2) < EPSILON) term2 = 0.0; + if (term1 < term2) return -1; + else if (term1 > term2) return +1; + } + return 0; + } + + /** + * Unit tests the {@code PolynomialRegression} data type. + * + * @param args the command-line arguments + */ + public static void main(String[] args) { + double[] x = {10, 20, 40, 80, 160, 200}; + double[] y = {100, 350, 1500, 6700, 20160, 40000}; + PolynomialRegression regression = new PolynomialRegression(x, y, 3); + + System.out.println(regression); + } +} diff --git a/src/main/java/frc/robot/util/PoseEstimator.java b/src/main/java/frc/robot/util/PoseEstimator.java new file mode 100644 index 0000000..08438c8 --- /dev/null +++ b/src/main/java/frc/robot/util/PoseEstimator.java @@ -0,0 +1,33 @@ +package frc.robot.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Twist2d; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; + +public class PoseEstimator { + private static PoseEstimator instance; + + public static PoseEstimator getInstance() { + if (instance == null) { + instance = new PoseEstimator(); + } + + return instance; + } + + public static final Lock odometryLock = new ReentrantLock(); + private Pose2d pose = new Pose2d(); + + public Pose2d getPose() { + return pose; + } + + public void resetPose(Pose2d pose) { + this.pose = pose; + } + + public void addDriveData(Twist2d twist) { + this.pose = pose.exp(twist); + } +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index fa08705..261dc8f 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,41 +1,42 @@ { - "fileName": "AdvantageKit.json", - "name": "AdvantageKit", - "version": "2.2.4", - "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", - "mavenUrls": [], - "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", - "javaDependencies": [ - { - "groupId": "org.littletonrobotics.akit.junction", - "artifactId": "wpilib-shim", - "version": "2.2.4" - }, - { - "groupId": "org.littletonrobotics.akit.junction", - "artifactId": "junction-core", - "version": "2.2.4" - }, - { - "groupId": "org.littletonrobotics.akit.conduit", - "artifactId": "conduit-api", - "version": "2.2.4" - } - ], - "jniDependencies": [ - { - "groupId": "org.littletonrobotics.akit.conduit", - "artifactId": "conduit-wpilibio", - "version": "2.2.4", - "skipInvalidPlatforms": false, - "isJar": false, - "validPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "cppDependencies": [] -} \ No newline at end of file + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "3.0.0-beta-5", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2024", + "mavenUrls": [], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "wpilib-shim", + "version": "3.0.0-beta-5" + }, + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "junction-core", + "version": "3.0.0-beta-5" + }, + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-api", + "version": "3.0.0-beta-5" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-wpilibio", + "version": "3.0.0-beta-5", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [] +} diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json deleted file mode 100644 index 04bb659..0000000 --- a/vendordeps/Phoenix.json +++ /dev/null @@ -1,423 +0,0 @@ -{ - "fileName": "Phoenix.json", - "name": "CTRE-Phoenix (v5)", - "version": "5.30.4+23.0.11", - "frcYear": 2023, - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json", - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.30.4" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.30.4" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.30.4", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.30.4", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro", - "artifactId": "tools", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "tools-sim", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simTalonSRX", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simTalonFX", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simVictorSPX", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simPigeonIMU", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simCANCoder", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProTalonFX", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProCANcoder", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProPigeon2", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-cpp", - "version": "5.30.4", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.30.4", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.30.4", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenixpro", - "artifactId": "tools", - "version": "23.0.11", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "5.30.4", - "libName": "CTRE_Phoenix_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "api-cpp-sim", - "version": "5.30.4", - "libName": "CTRE_PhoenixSim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.30.4", - "libName": "CTRE_PhoenixCCISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "tools-sim", - "version": "23.0.11", - "libName": "CTRE_PhoenixTools_Sim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simTalonSRX", - "version": "23.0.11", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simTalonFX", - "version": "23.0.11", - "libName": "CTRE_SimTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simVictorSPX", - "version": "23.0.11", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simPigeonIMU", - "version": "23.0.11", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simCANCoder", - "version": "23.0.11", - "libName": "CTRE_SimCANCoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProTalonFX", - "version": "23.0.11", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProCANcoder", - "version": "23.0.11", - "libName": "CTRE_SimProCANcoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProPigeon2", - "version": "23.0.11", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} \ No newline at end of file diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json new file mode 100644 index 0000000..3f79af2 --- /dev/null +++ b/vendordeps/Phoenix6.json @@ -0,0 +1,339 @@ +{ + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "24.0.0-beta-4", + "frcYear": 2024, + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-beta-latest.json", + "conflictsWith": [ + { + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", + "offlineFileName": "Phoenix6And5.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "24.0.0-beta-4" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "24.0.0-beta-4", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.0.0-beta-4", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "24.0.0-beta-4", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.0.0-beta-4", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.0.0-beta-4", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.0.0-beta-4", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.0.0-beta-4", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.0.0-beta-4", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.0.0-beta-4", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.0.0-beta-4", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.0.0-beta-4", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.0.0-beta-4", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index f2d0b7d..cfafaba 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,73 +1,74 @@ { - "fileName": "REVLib.json", - "name": "REVLib", - "version": "2023.1.3", - "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", - "mavenUrls": [ - "https://maven.revrobotics.com/" - ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2023.json", - "javaDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-java", - "version": "2023.1.3" - } - ], - "jniDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2023.1.3", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-cpp", - "version": "2023.1.3", - "libName": "REVLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2023.1.3", - "libName": "REVLibDriver", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ] -} \ No newline at end of file + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2024.0.0", + "frcYear": "2024", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2024.0.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.0.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2024.0.0", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.0.0", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index 0f59e5b..67bf389 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -1,37 +1,38 @@ -{ - "fileName": "WPILibNewCommands.json", - "name": "WPILib-New-Commands", - "version": "1.0.0", - "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "mavenUrls": [], - "jsonUrl": "", - "javaDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-java", - "version": "wpilib" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-cpp", - "version": "wpilib", - "libName": "wpilibNewCommands", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxarm32", - "linuxarm64", - "windowsx86-64", - "windowsx86", - "linuxx86-64", - "osxuniversal" - ] - } - ] -} \ No newline at end of file +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "1.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "frcYear": "2024", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } + ] +}