diff --git a/.github/workflows/choreolib.yml b/.github/workflows/choreolib.yml index 12308bd749..8e7e33719e 100644 --- a/.github/workflows/choreolib.yml +++ b/.github/workflows/choreolib.yml @@ -8,19 +8,15 @@ jobs: fail-fast: false matrix: include: - - container: wpilib/roborio-cross-ubuntu:2025-24.04 - artifact-name: Athena - build-options: -Ponlylinuxathena + - container: wpilib/systemcore-cross-debian:trixie + artifact-name: Systemcore + build-options: -Ponlylinuxsystemcore - - container: wpilib/raspbian-cross-ubuntu:bookworm-24.04 - artifact-name: Linux-arm32 - build-options: -Ponlylinuxarm32 - - - container: wpilib/aarch64-cross-ubuntu:bookworm-24.04 + - container: wpilib/aarch64-cross-debian:trixie artifact-name: Linux-arm64 build-options: -Ponlylinuxarm64 - - container: wpilib/ubuntu-base:24.04 + - container: wpilib/systemcore-cross-debian:trixie artifact-name: Linux-x86_64 build-options: @@ -39,6 +35,11 @@ jobs: run: | git config --global --add safe.directory /__w/${{ github.event.repository.name }}/${{ github.event.repository.name }} + - uses: actions/setup-java@v5 + with: + distribution: "temurin" + java-version: 25 + - name: Set version (release) if: github.repository_owner == 'SleipnirGroup' && startsWith(github.ref, 'refs/tags/v') working-directory: choreolib @@ -96,7 +97,7 @@ jobs: - uses: actions/setup-java@v5 with: distribution: "temurin" - java-version: 17 + java-version: 25 # 6/27/25: an Actions issue makes GITHUB_REF unset on Windows - name: Set version (Windows release) @@ -115,7 +116,7 @@ jobs: - name: Build with Gradle working-directory: choreolib - run: ./gradlew build ${{ matrix.build-options }} + run: ./gradlew build -Pbuildalldesktop ${{ matrix.build-options }} - name: Build with Gradle (release) if: github.repository_owner == 'SleipnirGroup' && startsWith(github.ref, 'refs/tags/v') @@ -130,20 +131,6 @@ jobs: name: ${{ matrix.artifact-name }} path: choreolib/build/allOutputs - vendordep: - name: Upload vendordep JSON - runs-on: ubuntu-slim - steps: - - uses: actions/checkout@v6 - with: - persist-credentials: false - token: ${{secrets.GITHUB_TOKEN}} - - - uses: actions/upload-artifact@v7 - with: - name: ChoreoLib-json - path: choreolib/ChoreoLib2026Beta.json - build-python: name: Python runs-on: ubuntu-slim diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index 5ae5cf343f..d1ec73b00a 100644 --- a/.github/workflows/docs.yml +++ b/.github/workflows/docs.yml @@ -39,7 +39,7 @@ jobs: - uses: actions/setup-java@v5 with: distribution: "temurin" - java-version: 17 + java-version: 25 - name: Copy API docs run: ./make-docs.sh diff --git a/.github/workflows/lint-format.yml b/.github/workflows/lint-format.yml index f778715285..fb3bd4049b 100644 --- a/.github/workflows/lint-format.yml +++ b/.github/workflows/lint-format.yml @@ -25,7 +25,7 @@ jobs: - uses: actions/setup-java@v5 with: distribution: "temurin" - java-version: 21 + java-version: 25 - name: Make GCC 14 the default toolchain (Linux) run: | diff --git a/Cargo.lock b/Cargo.lock index 00794a83b2..a68b872ff5 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -535,7 +535,7 @@ checksum = "613afe47fcd5fac7ccf1db93babcb082c5994d996f20b8b159f2ad1658eb5724" [[package]] name = "choreo" -version = "2026.0.3" +version = "2027.0.0-alpha.2" dependencies = [ "base64 0.22.1", "built", @@ -561,7 +561,7 @@ dependencies = [ [[package]] name = "choreo-cli" -version = "2026.0.3" +version = "2027.0.0-alpha.2" dependencies = [ "choreo-core", "clap", @@ -571,7 +571,7 @@ dependencies = [ [[package]] name = "choreo-core" -version = "2026.0.3" +version = "2027.0.0-alpha.2" dependencies = [ "dashmap", "fastrand", diff --git a/choreolib/build.gradle b/choreolib/build.gradle index d72083bac1..528fdedeb4 100644 --- a/choreolib/build.gradle +++ b/choreolib/build.gradle @@ -1,62 +1,57 @@ -import edu.wpi.first.toolchain.* +import org.wpilib.toolchain.* plugins { id 'cpp' id 'java' - id 'java-library' id 'google-test' - id 'edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin' version '2025.0' - id 'edu.wpi.first.NativeUtils' version '2026.0.1' - id 'edu.wpi.first.GradleJni' version '1.1.0' - id 'edu.wpi.first.GradleVsCode' version '2.1.0' - id 'com.diffplug.spotless' version '8.0.0' - id 'jacoco' + id 'org.wpilib.WPILibRepositoriesPlugin' version '2027.0.0' + id 'org.wpilib.NativeUtils' version '2027.10.0' + id 'org.wpilib.GradleJni' version '2027.0.0' + id 'org.wpilib.GradleVsCode' version '2027.0.0' + id 'com.diffplug.spotless' version '8.4.0' } -ext.getCurrentArch = { - return NativePlatforms.desktop -} - -def systemArch = getCurrentArch() - -java { - sourceCompatibility = JavaVersion.VERSION_17 - targetCompatibility = JavaVersion.VERSION_17 -} +// WPILib Version +ext.wpilibVersion = "2027.+" repositories { mavenCentral() } +wpilibRepositories.use2027Repos() if (project.hasProperty('releaseMode')) { wpilibRepositories.addAllReleaseRepositories(project) } else { wpilibRepositories.addAllDevelopmentRepositories(project) } +java { + sourceCompatibility = JavaVersion.VERSION_25 + targetCompatibility = JavaVersion.VERSION_25 +} +var javaVersion = "25" + // Apply C++ configuration apply from: 'config.gradle' // Apply Java configuration dependencies { - implementation 'edu.wpi.first.wpilibj:wpilibj-java:2026.1.1' - implementation 'edu.wpi.first.wpiutil:wpiutil-java:2026.1.1' - implementation 'edu.wpi.first.wpimath:wpimath-java:2026.1.1' - implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2026.1.1' - implementation 'edu.wpi.first.wpiunits:wpiunits-java:2026.1.1' - implementation 'edu.wpi.first.ntcore:ntcore-java:2026.1.1' - implementation 'edu.wpi.first.ntcore:ntcore-cpp:2026.1.1' - implementation 'edu.wpi.first.hal:hal-java:2026.1.1' + implementation "org.wpilib.ntcore:ntcore-java:$wpilibVersion" + implementation "org.wpilib.wpilibj:wpilibj-java:$wpilibVersion" + implementation "org.wpilib.wpiutil:wpiutil-java:$wpilibVersion" + implementation "org.wpilib.wpimath:wpimath-java:$wpilibVersion" + implementation "org.wpilib.wpiunits:wpiunits-java:$wpilibVersion" + implementation "org.wpilib.hal:hal-java:$wpilibVersion" + implementation "org.wpilib:commands3-java:$wpilibVersion" - api "com.fasterxml.jackson.core:jackson-annotations:2.19.2" - api "com.fasterxml.jackson.core:jackson-core:2.19.2" - api "com.fasterxml.jackson.core:jackson-databind:2.19.2" + // This is needed to use wpilibj Filesystem class + testImplementation "org.wpilib.cscore:cscore-java:$wpilibVersion" + testImplementation "org.wpilib.cameraserver:cameraserver-java:$wpilibVersion" - implementation 'com.google.code.gson:gson:2.13.1' + implementation "com.fasterxml.jackson.core:jackson-annotations:2.19.2" + implementation "com.fasterxml.jackson.core:jackson-core:2.19.2" + implementation "com.fasterxml.jackson.core:jackson-databind:2.19.2" - // This is needed to use wpilibj Filesystem class - testImplementation 'edu.wpi.first.cscore:cscore-java:2026.1.1' - testImplementation 'edu.wpi.first.cameraserver:cameraserver-java:2026.1.1' - testImplementation 'edu.wpi.first.hal:hal-java:2026.1.1' + implementation "com.google.code.gson:gson:2.13.1" testImplementation 'us.hebi.quickbuf:quickbuf-runtime:1.4' @@ -66,7 +61,6 @@ dependencies { testRuntimeOnly 'org.junit.platform:junit-platform-launcher' } - // Set up java tests test { useJUnitPlatform() @@ -77,44 +71,53 @@ test { } } -if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxarm32') || project.hasProperty('onlylinuxarm64') || project.hasProperty('onlywindowsarm64')) { +if (project.hasProperty('onlylinuxarm64') || project.hasProperty('onlywindowsarm64') || project.hasProperty('onlylinuxsystemcore')) { test.enabled = false } +tasks.withType(JavaCompile) { + options.compilerArgs.add '-XDstringConcat=inline' + options.encoding = 'UTF-8' +} + +// Setup Javadocs to link back to WPILib docs +javadoc { + options { + links "https://docs.oracle.com/en/java/javase/$javaVersion/docs/api/", "https://github.wpilib.org/allwpilib/docs/release/java/" + addBooleanOption("Xdoclint:html,missing,reference,syntax", true) + addBooleanOption('html5', true) + addStringOption("charset", "utf-8") + addStringOption("docencoding", "utf-8") + addStringOption("encoding", "utf-8") + linkSource(true) + failOnError = true + + title = "ChoreoLib API" + + // Treat javadoc warnings as errors. + // + // The second argument '-quiet' is a hack. The one parameter + // addStringOption() doesn't work, so we add '-quiet', which is added + // anyway by gradle. See https://github.com/gradle/gradle/issues/2354. + options.addStringOption('Werror', '-quiet') + } +} + // Set up exports properly nativeUtils { exportsConfigs { + // Main library is just default empty. This will export everything ChoreoLib { - x86ExcludeSymbols = [ - '_CT??_R0?AV_System_error', - '_CT??_R0?AVexception', - '_CT??_R0?AVfailure', - '_CT??_R0?AVruntime_error', - '_CT??_R0?AVsystem_error', - '_CTA5?AVfailure', - '_TI5?AVfailure', - '_CT??_R0?AVout_of_range', - '_CTA3?AVout_of_range', - '_TI3?AVout_of_range', - '_CT??_R0?AVbad_cast' - ] - x64ExcludeSymbols = [ - '_CT??_R0?AV_System_error', - '_CT??_R0?AVexception', - '_CT??_R0?AVfailure', - '_CT??_R0?AVruntime_error', - '_CT??_R0?AVsystem_error', - '_CTA5?AVfailure', - '_TI5?AVfailure', - '_CT??_R0?AVout_of_range', - '_CTA3?AVout_of_range', - '_TI3?AVout_of_range', - '_CT??_R0?AVbad_cast' - ] } } } +ext.getCurrentArch = { + return NativePlatforms.desktop +} + +def systemArch = getCurrentArch() + model { components { ChoreoLib(NativeLibrarySpec) { @@ -141,8 +144,6 @@ model { } } - nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared", "googletest_static") - binaries.all { def arch = it.targetPlatform.name if (systemArch == arch && it.buildType.name == 'debug') { @@ -153,14 +154,12 @@ model { test.workingDir filePath } } + + nativeUtils.useRequiredLibrary(it, "wpilib_shared", "googletest_static") } } } -compileJava { - options.encoding = 'UTF-8' -} - spotless { java { target fileTree('.') { @@ -194,44 +193,23 @@ spotless { } } -javadoc { - options.links("https://docs.oracle.com/en/java/javase/17/docs/api/") - options.links("https://github.wpilib.org/allwpilib/docs/release/java/") - options.addBooleanOption("Xdoclint:html,missing,reference,syntax", true) - options.addBooleanOption('html5', true) - options.addStringOption("charset", "utf-8") - options.addStringOption("docencoding", "utf-8") - options.addStringOption("encoding", "utf-8") - options.linkSource(true) - failOnError = true - - title = "ChoreoLib API" - - // Treat javadoc warnings as errors. - // - // The second argument '-quiet' is a hack. The one parameter - // addStringOption() doesn't work, so we add '-quiet', which is added - // anyway by gradle. See https://github.com/gradle/gradle/issues/2354. - options.addStringOption('Werror', '-quiet') -} - -jacocoTestReport { - reports { - xml.required.set(true) - } -} - test { dependsOn cleanTest useJUnitPlatform() testLogging.showStandardStreams = true - finalizedBy jacocoTestReport + // For reflective access to continuation classes in unit tests + jvmArgs += [ + "--add-opens", + "java.base/jdk.internal.vm=ALL-UNNAMED", + "--add-opens", + "java.base/java.lang=ALL-UNNAMED", + ] } apply from: 'publish.gradle' wrapper { - gradleVersion = '8.14.3' + gradleVersion = '9.4.1' } diff --git a/choreolib/config.gradle b/choreolib/config.gradle index 6ef8ccdb8a..7a4148f6c8 100644 --- a/choreolib/config.gradle +++ b/choreolib/config.gradle @@ -1,15 +1,15 @@ import org.gradle.internal.os.OperatingSystem nativeUtils.addWpiNativeUtils() -nativeUtils.withCrossRoboRIO() -nativeUtils.withCrossLinuxArm32() +nativeUtils.withCrossSystemCore() nativeUtils.withCrossLinuxArm64() nativeUtils { wpi { configureDependencies { - wpiVersion = "2026.1.1" - niLibVersion = "2026.1.0" + wpiVersion = wpilibVersion + mrcLibVersion = '2027.1.0-alpha-1-65-g21f308e' + opencvVersion = "2027-4.13.0-3" } } } @@ -19,11 +19,6 @@ nativeUtils.wpi.addWarningsAsErrors() nativeUtils.setSinglePrintPerPlatform() -nativeUtils.wpi.addVendorDeps() -nativeUtils.wpi.getVendorDeps().loadAll() -nativeUtils.wpi.getVendorDeps().getNativeVendor().initializeNativeDependencies() -nativeUtils.wpi.getVendorDeps().addVendorReposToMaven(false) - model { components { all { @@ -33,7 +28,6 @@ model { binaries { withType(NativeBinarySpec).all { nativeUtils.usePlatformArguments(it) - nativeUtils.wpi.getVendorDeps().getNativeVendor().cpp(it) } } } diff --git a/choreolib/gradle/wrapper/gradle-wrapper.jar b/choreolib/gradle/wrapper/gradle-wrapper.jar index 1b33c55baa..b1b8ef56b4 100644 Binary files a/choreolib/gradle/wrapper/gradle-wrapper.jar and b/choreolib/gradle/wrapper/gradle-wrapper.jar differ diff --git a/choreolib/gradle/wrapper/gradle-wrapper.properties b/choreolib/gradle/wrapper/gradle-wrapper.properties index d4081da476..c61a118f7d 100644 --- a/choreolib/gradle/wrapper/gradle-wrapper.properties +++ b/choreolib/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.14.3-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-9.4.1-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/choreolib/gradlew b/choreolib/gradlew index 23d15a9367..b9bb139f79 100755 --- a/choreolib/gradlew +++ b/choreolib/gradlew @@ -1,7 +1,7 @@ #!/bin/sh # -# Copyright © 2015-2021 the original authors. +# Copyright © 2015 the original authors. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -57,7 +57,7 @@ # Darwin, MinGW, and NonStop. # # (3) This script is generated from the Groovy template -# https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# https://github.com/gradle/gradle/blob/3d91ce3b8caaf77ad09f381f43615b715b53f72c/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt # within the Gradle project. # # You can find Gradle at https://github.com/gradle/gradle/. @@ -114,7 +114,6 @@ case "$( uname )" in #( NONSTOP* ) nonstop=true ;; esac -CLASSPATH="\\\"\\\"" # Determine the Java command to use to start the JVM. @@ -172,7 +171,6 @@ fi # For Cygwin or MSYS, switch paths to Windows format before running java if "$cygwin" || "$msys" ; then APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) - CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) JAVACMD=$( cygpath --unix "$JAVACMD" ) @@ -212,7 +210,6 @@ DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' set -- \ "-Dorg.gradle.appname=$APP_BASE_NAME" \ - -classpath "$CLASSPATH" \ -jar "$APP_HOME/gradle/wrapper/gradle-wrapper.jar" \ "$@" diff --git a/choreolib/gradlew.bat b/choreolib/gradlew.bat index db3a6ac207..24c62d56f2 100644 --- a/choreolib/gradlew.bat +++ b/choreolib/gradlew.bat @@ -23,8 +23,8 @@ @rem @rem ########################################################################## -@rem Set local scope for the variables with windows NT shell -if "%OS%"=="Windows_NT" setlocal +@rem Set local scope for the variables, and ensure extensions are enabled +setlocal EnableExtensions set DIRNAME=%~dp0 if "%DIRNAME%"=="" set DIRNAME=. @@ -51,7 +51,7 @@ echo. 1>&2 echo Please set the JAVA_HOME variable in your environment to match the 1>&2 echo location of your Java installation. 1>&2 -goto fail +"%COMSPEC%" /c exit 1 :findJavaFromJavaHome set JAVA_HOME=%JAVA_HOME:"=% @@ -65,30 +65,18 @@ echo. 1>&2 echo Please set the JAVA_HOME variable in your environment to match the 1>&2 echo location of your Java installation. 1>&2 -goto fail +"%COMSPEC%" /c exit 1 :execute @rem Setup the command line -set CLASSPATH= @rem Execute Gradle -"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" -jar "%APP_HOME%\gradle\wrapper\gradle-wrapper.jar" %* +@rem endlocal doesn't take effect until after the line is parsed and variables are expanded +@rem which allows us to clear the local environment before executing the java command +endlocal & "%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -jar "%APP_HOME%\gradle\wrapper\gradle-wrapper.jar" %* & call :exitWithErrorLevel -:end -@rem End local scope for the variables with windows NT shell -if %ERRORLEVEL% equ 0 goto mainEnd - -:fail -rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of -rem the _cmd.exe /c_ return code! -set EXIT_CODE=%ERRORLEVEL% -if %EXIT_CODE% equ 0 set EXIT_CODE=1 -if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% -exit /b %EXIT_CODE% - -:mainEnd -if "%OS%"=="Windows_NT" endlocal - -:omega +:exitWithErrorLevel +@rem Use "%COMSPEC%" /c exit to allow operators to work properly in scripts +"%COMSPEC%" /c exit %ERRORLEVEL% diff --git a/choreolib/src/main/java/choreo/Choreo.java b/choreolib/src/main/java/choreo/Choreo.java index e381291c29..bb06f5f0f0 100644 --- a/choreolib/src/main/java/choreo/Choreo.java +++ b/choreolib/src/main/java/choreo/Choreo.java @@ -2,36 +2,25 @@ package choreo; -import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; -import static edu.wpi.first.wpilibj.Alert.AlertType.kError; +import static org.wpilib.util.ErrorMessages.requireNonNullParam; -import choreo.trajectory.DifferentialSample; -import choreo.trajectory.EventMarker; -import choreo.trajectory.SwerveSample; -import choreo.trajectory.Trajectory; -import choreo.trajectory.TrajectorySample; +import choreo.trajectory.*; import choreo.util.ChoreoAlert; -import choreo.util.ChoreoAlert.*; +import choreo.util.ChoreoAlert.MultiAlert; import choreo.util.TrajSchemaVersion; import com.google.gson.Gson; import com.google.gson.GsonBuilder; import com.google.gson.JsonObject; import com.google.gson.JsonSyntaxException; -import edu.wpi.first.hal.FRCNetComm.tResourceType; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Filesystem; import java.io.BufferedReader; import java.io.File; import java.io.FileNotFoundException; import java.io.FileReader; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.HashMap; -import java.util.List; -import java.util.Map; -import java.util.Optional; +import java.util.*; import java.util.function.BiConsumer; +import org.wpilib.driverstation.Alert.Level; +import org.wpilib.driverstation.DriverStationErrors; +import org.wpilib.system.Filesystem; /** Utilities to load and follow Choreo Trajectories */ public final class Choreo { @@ -42,9 +31,9 @@ public final class Choreo { private static final String TRAJECTORY_FILE_EXTENSION = ".traj"; private static final int TRAJ_SCHEMA_VERSION = TrajSchemaVersion.TRAJ_SCHEMA_VERSION; private static final MultiAlert cantFindTrajectory = - ChoreoAlert.multiAlert(causes -> "Could not find trajectory files: " + causes, kError); + ChoreoAlert.multiAlert(causes -> "Could not find trajectory files: " + causes, Level.HIGH); private static final MultiAlert cantParseTrajectory = - ChoreoAlert.multiAlert(causes -> "Could not parse trajectory files: " + causes, kError); + ChoreoAlert.multiAlert(causes -> "Could not parse trajectory files: " + causes, Level.HIGH); private static File CHOREO_DIR = new File(Filesystem.getDeployDirectory(), "choreo"); @@ -58,8 +47,7 @@ static void setChoreoDir(File choreoDir) { * Trajectory}, {@link Boolean})->void, where the function consumes a trajectory and a boolean * indicating whether the trajectory is starting or finishing. * - * @param {@link choreo.trajectory.DifferentialSample} or {@link - * choreo.trajectory.SwerveSample} + * @param {@link DifferentialSample} or {@link SwerveSample} */ public interface TrajectoryLogger> extends BiConsumer, Boolean> {} @@ -101,9 +89,9 @@ Optional> loadTrajectory(String trajectoryName) { } catch (Exception ex) { ChoreoAlert.alert( "Unknown error when parsing " + trajectoryFile + "; check console for more details", - kError) + Level.HIGH) .set(true); - DriverStation.reportError(ex.getMessage(), ex.getStackTrace()); + DriverStationErrors.reportError(ex.getMessage(), ex.getStackTrace()); } return Optional.empty(); } @@ -171,17 +159,16 @@ static Trajectory> loadTrajectoryString( } String sampleType = trajectoryObj.get("sampleType").getAsString(); if (sampleType.equals("Swerve")) { - HAL.report(tResourceType.kResourceType_ChoreoTrajectory, 1); + // HAL.report(tResourceType.kResourceType_ChoreoTrajectory, 1); SwerveSample[] samples = GSON.fromJson(trajectoryObj.get("samples"), SwerveSample[].class); return new Trajectory(name, List.of(samples), List.of(splits), List.of(events)); } else if (sampleType.equals("Differential")) { - HAL.report(tResourceType.kResourceType_ChoreoTrajectory, 2); + // HAL.report(tResourceType.kResourceType_ChoreoTrajectory, 2); DifferentialSample[] sampleArray = GSON.fromJson(trajectoryObj.get("samples"), DifferentialSample[].class); - return new Trajectory( - name, List.of(sampleArray), List.of(splits), List.of(events)); + return new Trajectory<>(name, List.of(sampleArray), List.of(splits), List.of(events)); } else { throw new RuntimeException("Unknown drive type: " + sampleType); } diff --git a/choreolib/src/main/java/choreo/auto/AutoChooser.java b/choreolib/src/main/java/choreo/auto/AutoChooser.java deleted file mode 100644 index 970431fe41..0000000000 --- a/choreolib/src/main/java/choreo/auto/AutoChooser.java +++ /dev/null @@ -1,231 +0,0 @@ -// Copyright (c) Choreo contributors - -package choreo.auto; - -import static edu.wpi.first.wpilibj.Alert.AlertType.kError; - -import choreo.util.ChoreoAlert; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import java.util.HashMap; -import java.util.Optional; -import java.util.function.Supplier; - -/** - * An Choreo specific {@code SendableChooser} that allows for the selection of {@link AutoRoutine}s - * at runtime via a Dashboard. - * - *

This chooser takes a lazy loading - * approach to {@link AutoRoutine}s, only generating the {@link AutoRoutine} when it is selected. - * This approach has the benefit of not loading all autos on startup, but also not loading the auto - * during auto start causing a delay. - * - *

Once the {@link AutoChooser} is made you can add {@link AutoRoutine}s to it using {@link - * #addRoutine} or add {@link Command}s to it using {@link #addCmd}. Similar to {@code - * SendableChooser} this chooser can be added to the {@link - * edu.wpi.first.wpilibj.smartdashboard.SmartDashboard} using {@code - * SmartDashboard.putData(Sendable)}. - * - *

You can set the Robot's autonomous command to the chooser's chosen auto routine via - * RobotModeTriggers.autonomous.whileTrue(chooser.autoSchedulingCmd()); - */ -public class AutoChooser implements Sendable { - private final String DO_NOTHING_NAME; - private static final Alert selectedNonexistentAuto = - ChoreoAlert.alert("Selected an auto that isn't an option", kError); - - private final HashMap> autoRoutines = new HashMap<>(); - - private String selected; - private String[] options = new String[] {}; - - private Optional allianceAtGeneration = Optional.empty(); - private String nameAtGeneration; - private Command generatedCommand = Commands.none(); - - /** Constructs a new {@link AutoChooser}. */ - public AutoChooser() { - this("Nothing"); - } - - /** - * Constructs a new {@link AutoChooser} with the given name for the do-nothing default option. - * - * @param doNothingName The option name for the default choice. - */ - public AutoChooser(String doNothingName) { - DO_NOTHING_NAME = doNothingName; - nameAtGeneration = DO_NOTHING_NAME; - generatedCommand = Commands.none(); - addCmd(DO_NOTHING_NAME, Commands::none); - select(DO_NOTHING_NAME); - } - - /** - * Returns the name of the default do-nothing option. - * - * @return the name of the default do-nothing option. - */ - public String getDefaultName() { - return DO_NOTHING_NAME; - } - - /** - * Select a new option in the chooser. - * - *

This method is called automatically when published as a sendable. - * - * @param selectStr The name of the option to select. - * @return The name of the selected option. - */ - public String select(String selectStr) { - return select(selectStr, false); - } - - private String select(String selectStr, boolean force) { - selected = selectStr; - if (selected.equals(nameAtGeneration) - && allianceAtGeneration.equals(DriverStation.getAlliance())) { - // early return if the selected auto matches the active auto - return nameAtGeneration; - } - boolean dsValid = DriverStation.isDisabled() && DriverStation.getAlliance().isPresent(); - if (dsValid || force) { - if (!autoRoutines.containsKey(selected) && !selected.equals(DO_NOTHING_NAME)) { - selected = DO_NOTHING_NAME; - selectedNonexistentAuto.set(true); - } else { - selectedNonexistentAuto.set(false); - } - allianceAtGeneration = DriverStation.getAlliance(); - nameAtGeneration = selected; - generatedCommand = autoRoutines.get(nameAtGeneration).get().withName(nameAtGeneration); - } else { - allianceAtGeneration = Optional.empty(); - nameAtGeneration = DO_NOTHING_NAME; - generatedCommand = Commands.none(); - } - return nameAtGeneration; - } - - /** - * Add an AutoRoutine to the chooser. - * - *

This is done to load AutoRoutines when and only when they are selected, in order to save - * memory and file loading time for unused AutoRoutines. - * - *

The generators are only run when the DriverStation is disabled and the alliance is known. - * - *

One way to keep this clean is to make an `Autos` class that all of your subsystems/resources - * are dependency injected into. - * Then create methods inside that class that take an {@link AutoFactory} and return an {@link - * AutoRoutine}. - * - *

Example:

- * - * 

-   * AutoChooser chooser;
-   * Autos autos = new Autos(swerve, shooter, intake, feeder);
-   * public Robot() {
-   *   chooser = new AutoChooser("/Choosers");
-   *   SmartDashboard.putData(chooser);
-   *   // fourPieceRight is a method that accepts an AutoFactory and returns an AutoRoutine.
-   *   chooser.addRoutine("4 Piece right", autos::fourPieceRight);
-   *   chooser.addRoutine("4 Piece Left", autos::fourPieceLeft);
-   *   chooser.addRoutine("3 Piece Close", autos::threePieceClose);
-   * }
-   *
- * - * @param name The name of the auto routine. - * @param generator The function that generates the auto routine. - * @return This {@link AutoChooser} instance, to allow for method chaining. - */ - public AutoChooser addRoutine(String name, Supplier generator) { - autoRoutines.put(name, () -> generator.get().cmd()); - options = autoRoutines.keySet().toArray(new String[0]); - return this; - } - - /** - * Adds a Command to the auto chooser. - * - *

This is done to load autonomous commands when and only when they are selected, in order to - * save memory and file loading time for unused autonomous commands. - * - *

The generators are only run when the DriverStation is disabled and the alliance is known. - * - *

Example:

- * - * 

-   * AutoChooser chooser;
-   * Autos autos = new Autos(swerve, shooter, intake, feeder);
-   * public Robot() {
-   *   chooser = new AutoChooser("/Choosers");
-   *   SmartDashboard.putData(chooser);
-   *   // fourPieceLeft is a method that accepts an AutoFactory and returns a command.
-   *   chooser.addCmd("4 Piece left", autos::fourPieceLeft);
-   *   chooser.addCmd("Just Shoot", shooter::shoot);
-   * }
-   *
- * - * @param name The name of the autonomous command. - * @param generator The function that generates an autonomous command. - * @return This {@link AutoChooser} instance, to allow for method chaining. - * @see AutoChooser#addRoutine - */ - public AutoChooser addCmd(String name, Supplier generator) { - autoRoutines.put(name, generator); - options = autoRoutines.keySet().toArray(new String[0]); - return this; - } - - /** - * Gets a Command that schedules the selected auto routine. This Command shares the lifetime of - * the scheduled Command. This Command can directly be bound to a trigger, like so: - * - * 

-   *     AutoChooser chooser = ...;
-   *
-   *     public Robot() {
-   *         RobotModeTriggers.autonomous().whileTrue(chooser.selectedCommandScheduler());
-   *     }
-   *
- * - * @return A command that runs the selected {@link AutoRoutine} - */ - public Command selectedCommandScheduler() { - return Commands.deferredProxy(() -> selectedCommand()); - } - - /** - * Returns the currently selected command. - * - *

If you plan on using this {@link Command} in a {@code Trigger} it is recommended to use - * {@link #selectedCommandScheduler()} instead. - * - * @return The currently selected command. - */ - public Command selectedCommand() { - if (RobotBase.isSimulation() && nameAtGeneration == DO_NOTHING_NAME) { - select(selected, true); - } - return generatedCommand; - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("String Chooser"); - builder.publishConstBoolean(".controllable", true); - builder.publishConstString("default", DO_NOTHING_NAME); - builder.addStringArrayProperty("options", () -> options, null); - builder.addStringProperty("selected", null, this::select); - builder.addStringProperty("active", () -> select(selected), null); - } -} diff --git a/choreolib/src/main/java/choreo/auto/AutoFactory.java b/choreolib/src/main/java/choreo/auto/AutoFactory.java index a7458f0de3..19b2eb1705 100644 --- a/choreolib/src/main/java/choreo/auto/AutoFactory.java +++ b/choreolib/src/main/java/choreo/auto/AutoFactory.java @@ -2,7 +2,7 @@ package choreo.auto; -import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; +import static org.wpilib.util.ErrorMessages.requireNonNullParam; import choreo.Choreo.TrajectoryCache; import choreo.Choreo.TrajectoryLogger; @@ -10,32 +10,24 @@ import choreo.trajectory.Trajectory; import choreo.trajectory.TrajectorySample; import choreo.util.ChoreoAllianceFlipUtil; -import edu.wpi.first.hal.FRCNetComm.tResourceType; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.button.Trigger; import java.util.HashMap; import java.util.List; import java.util.Optional; -import java.util.function.BooleanSupplier; import java.util.function.Consumer; -import java.util.function.Function; import java.util.function.Supplier; +import org.wpilib.command3.Command; +import org.wpilib.command3.Mechanism; +import org.wpilib.driverstation.Alliance; +import org.wpilib.driverstation.internal.DriverStationBackend; +import org.wpilib.math.geometry.Pose2d; /** - * A factory used to create {@link AutoRoutine}s and {@link AutoTrajectory}s. + * A factory used to create {@link AutoTrajectory}s. * * @see Auto Routine Docs */ public class AutoFactory { - static record AllianceContext( - boolean useAllianceFlipping, Supplier> allianceGetter) { + record AllianceContext(boolean useAllianceFlipping, Supplier> allianceGetter) { boolean allianceKnownOrIgnored() { return allianceGetter.get().isPresent() || !useAllianceFlipping; } @@ -46,7 +38,7 @@ boolean doFlip() { .get() .orElseThrow( () -> new RuntimeException("Flip check was called with an unknown alliance")) - .equals(Alliance.Red); + .equals(Alliance.RED); } Optional alliance() { @@ -61,7 +53,7 @@ Supplier> getFlippedPose(Optional bluePose) { /** A class used to bind commands to events in all trajectories created by this factory. */ static class AutoBindings { - private HashMap bindings = new HashMap<>(); + private final HashMap bindings = new HashMap<>(); /** Default constructor. */ public AutoBindings() {} @@ -93,13 +85,12 @@ HashMap getBindings() { private final Consumer resetOdometry; private final Consumer> controller; private final AllianceContext allianceCtx; - private final Subsystem driveSubsystem; + private final Mechanism driveMechanism; private final AutoBindings bindings = new AutoBindings(); private final TrajectoryLogger> trajectoryLogger; - private final AutoRoutine voidRoutine; /** - * Create a factory that can be used to create {@link AutoRoutine} and {@link AutoTrajectory}. + * Create a factory that can be used to create {@link AutoTrajectory}(s). * * @param The type of samples in the trajectory. * @param poseSupplier A function that returns the current field-relative {@link Pose2d} of the @@ -110,63 +101,36 @@ HashMap getBindings() { * robot. * @param useAllianceFlipping If this is true, when on the red alliance, the path will be mirrored * to the opposite side, while keeping the same coordinate system origin. - * @param driveSubsystem The drive {@link Subsystem} to require for {@link AutoTrajectory} {@link + * @param driveMechanism The drive {@link Mechanism} to require for {@link AutoTrajectory} {@link * Command}s. * @param trajectoryLogger A {@link TrajectoryLogger} to log {@link Trajectory} as they start and * finish. - * @see AutoChooser using this factory with AutoChooser to generate auto routines. */ public > AutoFactory( Supplier poseSupplier, Consumer resetOdometry, Consumer controller, boolean useAllianceFlipping, - Subsystem driveSubsystem, + Mechanism driveMechanism, TrajectoryLogger trajectoryLogger) { requireNonNullParam(poseSupplier, "poseSupplier", "AutoFactory"); requireNonNullParam(resetOdometry, "resetOdometry", "AutoFactory"); requireNonNullParam(controller, "controller", "AutoFactory"); - requireNonNullParam(driveSubsystem, "driveSubsystem", "AutoFactory"); + requireNonNullParam(driveMechanism, "driveMechanism", "AutoFactory"); requireNonNullParam(useAllianceFlipping, "useAllianceFlipping", "AutoFactory"); this.poseSupplier = poseSupplier; this.resetOdometry = resetOdometry; this.controller = controller; - this.driveSubsystem = driveSubsystem; - this.allianceCtx = new AllianceContext(useAllianceFlipping, DriverStation::getAlliance); + this.driveMechanism = driveMechanism; + this.allianceCtx = new AllianceContext(useAllianceFlipping, DriverStationBackend::getAlliance); this.trajectoryLogger = trajectoryLogger; - HAL.report(tResourceType.kResourceType_ChoreoTrigger, 1); - - voidRoutine = - new AutoRoutine(this, "VOID-ROUTINE", allianceCtx) { - @Override - public Command cmd() { - return Commands.none().withName("VoidAutoRoutine"); - } - - @Override - public Command cmd(BooleanSupplier _finishCondition) { - return cmd(); - } - - @Override - public void poll() {} - - @Override - public void reset() {} - - @Override - public Trigger active() { - return new Trigger(this.loop(), () -> true); - } - }; } /** - * Create a factory that can be used to create {@link AutoRoutine} and {@link AutoTrajectory}. + * Create a factory that can be used to create an {@link AutoTrajectory}. * - * @param {@link choreo.trajectory.DifferentialSample} or {@link - * choreo.trajectory.SwerveSample} + * @param {@link choreo.trajectory.DifferentialSample} or {@link SwerveSample} * @param poseSupplier A function that returns the current field-relative {@link Pose2d} of the * robot. * @param resetOdometry A function that receives a field-relative {@link Pose2d} to reset the @@ -174,47 +138,26 @@ public Trigger active() { * @param controller A function that receives the current {@link ST} and controls the robot. * @param useAllianceFlipping If this returns true, when on the red alliance, the path will be * mirrored to the opposite side, while keeping the same coordinate system origin. - * @param driveSubsystem The drive {@link Subsystem} to require for {@link AutoTrajectory} {@link + * @param driveMechanism The drive {@link Mechanism} to require for {@link AutoTrajectory} {@link * Command}s. - * @see AutoChooser using this factory with AutoChooser to generate auto routines. */ public > AutoFactory( Supplier poseSupplier, Consumer resetOdometry, Consumer controller, boolean useAllianceFlipping, - Subsystem driveSubsystem) { + Mechanism driveMechanism) { this( - poseSupplier, - resetOdometry, - controller, - useAllianceFlipping, - driveSubsystem, - (sample, isStart) -> {}); - } - - /** - * Creates a new {@link AutoRoutine}. - * - * @param name The name of the {@link AutoRoutine}. - * @return A new {@link AutoRoutine}. - */ - public AutoRoutine newRoutine(String name) { - // Clear cache in simulation to allow a form of "hot-reloading" trajectories - if (RobotBase.isSimulation()) { - trajectoryCache.clear(); - } - - return new AutoRoutine(this, name, allianceCtx); + poseSupplier, resetOdometry, controller, useAllianceFlipping, driveMechanism, (_, _) -> {}); } /** - * A package protected method to create a new {@link AutoTrajectory} to be used in an {@link - * AutoRoutine}. + * Creates a new {@link AutoTrajectory} to be used in an auto routine. * - * @see AutoRoutine#trajectory(String) + * @param trajectoryName The name of the trajectory to use. + * @return A new {@link AutoTrajectory}. */ - AutoTrajectory trajectory(String trajectoryName, AutoRoutine routine, boolean useBindings) { + public AutoTrajectory trajectory(String trajectoryName) { Optional> optTrajectory = trajectoryCache.loadTrajectory(trajectoryName); Trajectory trajectory; @@ -223,17 +166,17 @@ AutoTrajectory trajectory(String trajectoryName, AutoRoutine routine, boolean us } else { trajectory = new Trajectory(trajectoryName, List.of(), List.of(), List.of()); } - return trajectory(trajectory, routine, useBindings); + return trajectory(trajectory); } /** - * A package protected method to create a new {@link AutoTrajectory} to be used in an {@link - * AutoRoutine}. + * Creates a new {@link AutoTrajectory} to be used in an auto routine. * - * @see AutoRoutine#trajectory(String, int) + * @param trajectoryName The name of the trajectory to use. + * @param splitIndex The index of the split trajectory to use. + * @return A new {@link AutoTrajectory}. */ - AutoTrajectory trajectory( - String trajectoryName, final int splitIndex, AutoRoutine routine, boolean useBindings) { + public AutoTrajectory trajectory(String trajectoryName, final int splitIndex) { Optional> optTrajectory = trajectoryCache.loadTrajectory(trajectoryName, splitIndex); Trajectory trajectory; @@ -242,228 +185,28 @@ AutoTrajectory trajectory( } else { trajectory = new Trajectory(trajectoryName, List.of(), List.of(), List.of()); } - return trajectory(trajectory, routine, useBindings); - } - - /** - * A package protected method to create a new {@link AutoTrajectory} to be used in an {@link - * AutoRoutine}. - * - * @see AutoRoutine#trajectory(Trajectory) - */ - > AutoTrajectory trajectory( - Trajectory trajectory, AutoRoutine routine, boolean useBindings) { - return trajectory(trajectory, routine, useBindings, Function.identity()); + return trajectory(trajectory); } /** - * A package protected method to create a new {@link AutoTrajectory} to be used in an {@link - * AutoRoutine}. + * Creates a new {@link AutoTrajectory} to be used in an auto routine. * - * @see AutoRoutine#trajectory(Trajectory) + * @param The type of the trajectory samples. + * @param trajectory The trajectory to use. + * @return A new {@link AutoTrajectory}. */ @SuppressWarnings("unchecked") - > AutoTrajectory trajectory( - Trajectory trajectory, - AutoRoutine routine, - boolean useBindings, - Function, Trajectory> trajectoryTransform) { - // type solidify everything - final Trajectory solidTrajectory = trajectoryTransform.apply(trajectory); - final Consumer solidController = (Consumer) this.controller; + public > AutoTrajectory trajectory(Trajectory trajectory) { return new AutoTrajectory( trajectory.name(), - solidTrajectory, + trajectory, poseSupplier, resetOdometry, - solidController, + (Consumer) this.controller, allianceCtx, (TrajectoryLogger) trajectoryLogger, - driveSubsystem, - routine, - useBindings ? bindings : new AutoBindings()); - } - - /** - * Warms up Choreo to ensure that there is no delay at the start of auto. It is recommended to - * schedule this command in your Robot constructor, like so: - * - * 


-   *     CommandScheduler.getInstance().schedule(autoFactory.warmupCmd());
-   *
- * - * @return A command that warms up Choreo's autonomous functionality. - */ - public Command warmupCmd() { - var autoTraj = trajectory("", voidRoutine, false); - autoTraj.suppressWarnings(); - return autoTraj.cmd().ignoringDisable(true).withTimeout(0.5).withName("Choreo Warmup Command"); - } - - /** - * Creates a new {@link AutoTrajectory} command to be used in an auto routine. - * - *

Important - * - *

{@link #trajectoryCmd} and {@link #trajectory} methods should not be mixed in the same auto - * routine. {@link #trajectoryCmd} is used as an escape hatch for teams that don't need the - * benefits of the {@link #trajectory} method and its {@link Trigger} API. {@link #trajectoryCmd} - * does not invoke bindings added via calling {@link #bind} or {@link AutoBindings} passed into - * the factory constructor. - * - * @param trajectoryName The name of the trajectory to use. - * @return A new {@link AutoTrajectory}. - */ - public Command trajectoryCmd(String trajectoryName) { - return trajectory(trajectoryName, voidRoutine, false).cmd(); - } - - /** - * Creates a new {@link AutoTrajectory} command to be used in an auto routine. - * - *

Important - * - *

{@link #trajectoryCmd} and {@link #trajectory} methods should not be mixed in the same auto - * routine. {@link #trajectoryCmd} is used as an escape hatch for teams that don't need the - * benefits of the {@link #trajectory} method and its {@link Trigger} API. {@link #trajectoryCmd} - * does not invoke bindings added via calling {@link #bind} or {@link AutoBindings} passed into - * the factory constructor. - * - * @param trajectoryName The name of the trajectory to use. - * @param splitIndex The index of the split trajectory to use. - * @param transform A function that takes in the loaded trajectory and applies a transformation to - * it, such as left-to-right mirroring. - * @return A new {@link AutoTrajectory}. - */ - public Command trajectoryCmd( - String trajectoryName, - final int splitIndex, - Function transform) { - return transform.apply(trajectory(trajectoryName, splitIndex, voidRoutine, false)).cmd(); - } - - /** - * Creates a new {@link AutoTrajectory} command to be used in an auto routine. - * - *

Important - * - *

{@link #trajectoryCmd} and {@link #trajectory} methods should not be mixed in the same auto - * routine. {@link #trajectoryCmd} is used as an escape hatch for teams that don't need the - * benefits of the {@link #trajectory} method and its {@link Trigger} API. {@link #trajectoryCmd} - * does not invoke bindings added via calling {@link #bind} or {@link AutoBindings} passed into - * the factory constructor. - * - * @param trajectoryName The name of the trajectory to use. - * @param transform A function that takes in the loaded trajectory and applies a transformation to - * it, such as left-to-right mirroring. - * @return A new {@link AutoTrajectory}. - */ - public Command trajectoryCmd( - String trajectoryName, Function transform) { - return transform.apply(trajectory(trajectoryName, voidRoutine, false)).cmd(); - } - - /** - * Creates a new {@link AutoTrajectory} command to be used in an auto routine. - * - *

Important - * - *

{@link #trajectoryCmd} and {@link #trajectory} methods should not be mixed in the same auto - * routine. {@link #trajectoryCmd} is used as an escape hatch for teams that don't need the - * benefits of the {@link #trajectory} method and its {@link Trigger} API. {@link #trajectoryCmd} - * does not invoke bindings added via calling {@link #bind} or {@link AutoBindings} passed into - * the factory constructor. - * - * @param trajectoryName The name of the trajectory to use. - * @param splitIndex The index of the split trajectory to use. - * @return A new {@link AutoTrajectory}. - */ - public Command trajectoryCmd(String trajectoryName, final int splitIndex) { - return trajectory(trajectoryName, splitIndex, voidRoutine, false).cmd(); - } - - /** - * Creates a new {@link AutoTrajectory} command to be used in an auto routine. - * - *

Important - * - *

{@link #trajectoryCmd} and {@link #trajectory} methods should not be mixed in the same auto - * routine. {@link #trajectoryCmd} is used as an escape hatch for teams that don't need the - * benefits of the {@link #trajectory} method and its {@link Trigger} API. {@link #trajectoryCmd} - * does not invoke bindings added via calling {@link #bind} or {@link AutoBindings} passed into - * the factory constructor. - * - * @param {@link choreo.trajectory.DifferentialSample} or {@link - * choreo.trajectory.SwerveSample} - * @param trajectory The trajectory to use. - * @return A new {@link AutoTrajectory}. - */ - public > Command trajectoryCmd(Trajectory trajectory) { - return trajectory(trajectory, voidRoutine, false).cmd(); - } - - /** - * Creates a command that resets the robot's odometry to the start of a trajectory. - * - * @param trajectoryName The name of the trajectory to use. - * @return A command that resets the robot's odometry. - */ - public Command resetOdometry(String trajectoryName) { - return trajectory(trajectoryName, voidRoutine, false).resetOdometry(); - } - - /** - * Creates a command that resets the robot's odometry to the start of a trajectory. - * - * @param trajectoryName The name of the trajectory to use. - * @param splitIndex The index of the split trajectory to use. - * @return A command that resets the robot's odometry. - */ - public Command resetOdometry(String trajectoryName, final int splitIndex) { - return trajectory(trajectoryName, splitIndex, voidRoutine, false).resetOdometry(); - } - - /** - * Creates a command that resets the robot's odometry to the start of a trajectory. - * - * @param {@link choreo.trajectory.DifferentialSample} or {@link - * choreo.trajectory.SwerveSample} - * @param trajectory The trajectory to use. - * @return A command that resets the robot's odometry. - */ - public > Command resetOdometry(Trajectory trajectory) { - return trajectory(trajectory, voidRoutine, false).resetOdometry(); - } - - /** - * Creates a command that resets the robot's odometry to the supplied pose - * - * @param pose A function that is called when the command is run. It returns an - * Optional<Pose2d> of the robot's desired odometry position. - * @return A command that resets the robot's odometry to the supplied pose, or does nothing if the - * supplied Optional is empty. - */ - public Command resetOdometry(Supplier> pose) { - return driveSubsystem.runOnce(() -> pose.get().ifPresent(resetOdometry)); - } - - /** - * Creates a command that resets the robot's odometry to the given pose - * - * @param pose An Optional<Pose2d> of the robot's desired odometry position. - * @param doFlipForAlliance True if the given pose still needs to be flipped according to the - * alliance (usually true). False if it is an absolute field position. - * @return A command that resets the robot's odometry to the given pose (flipped as directed), or - * does nothing if the supplied Optional is empty. - */ - public Command resetOdometry(Optional pose, boolean doFlipForAlliance) { - if (pose.isEmpty()) { - return driveSubsystem.runOnce( - () -> {}); // equivalent to Commands.none() requiring driveSubsystem. - } - Supplier> supplier = - doFlipForAlliance ? allianceCtx.getFlippedPose(pose) : (() -> pose); - return resetOdometry(supplier); + driveMechanism, + bindings); } /** diff --git a/choreolib/src/main/java/choreo/auto/AutoRoutine.java b/choreolib/src/main/java/choreo/auto/AutoRoutine.java deleted file mode 100644 index fe0cab2e85..0000000000 --- a/choreolib/src/main/java/choreo/auto/AutoRoutine.java +++ /dev/null @@ -1,305 +0,0 @@ -// Copyright (c) Choreo contributors - -package choreo.auto; - -import static edu.wpi.first.wpilibj.Alert.AlertType.kWarning; - -import choreo.auto.AutoFactory.AllianceContext; -import choreo.trajectory.Trajectory; -import choreo.trajectory.TrajectorySample; -import choreo.util.ChoreoAlert; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.event.EventLoop; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import java.util.function.BooleanSupplier; - -/** - * An object that represents an autonomous routine. - * - *

This object is used to handle autonomous trigger logic and schedule commands for a single - * autonomous routine. This object should **not** be shared across multiple autonomous routines. - * - * @see AutoFactory#newRoutine Creating a routine from a AutoFactory - */ -public class AutoRoutine { - /** - * The factory that created this loop. This is used to create commands that are associated with - * this loop. - */ - private final AutoFactory factory; - - /** The underlying {@link EventLoop} that triggers are bound to and polled */ - private final EventLoop loop = new EventLoop(); - - /** The name of the auto routine this loop is associated with */ - private final String name; - - /** The alliance helper that is used to determine flipping logic */ - final AllianceContext allianceCtx; - - /** A boolean utilized in {@link #active()} to resolve trueness */ - private boolean isActive = false; - - private final Trigger isActiveTrigger = - new Trigger(loop, () -> isActive && DriverStation.isEnabled()); - - /** A boolean indicating if a trajectory is running on the routine right now */ - private boolean isIdle = true; - - private final Trigger isIdleTrigger = new Trigger(loop, () -> isIdle); - - /** A boolean that is true when the loop is killed */ - boolean isKilled = false; - - /** The amount of times the routine has been polled */ - private int pollCount = 0; - - /** The timestamp of the current cycle */ - private double cycleTimestamp = 0; - - /** - * Creates a new loop with a specific name and a custom alliance supplier. - * - * @param factory The factory that created this loop - * @param name The name of the loop - * @param allianceHelper The alliance helper that is used to determine flipping logic - * @see AutoFactory#newRoutine Creating a loop from a AutoFactory - */ - AutoRoutine(AutoFactory factory, String name, AllianceContext allianceHelper) { - this.factory = factory; - this.name = name; - this.allianceCtx = allianceHelper; - } - - /** - * Returns a {@link Trigger} that is true while this autonomous routine is being polled. - * - *

Using a {@link Trigger#onFalse(Command)} will do nothing as when this is false the routine - * is not being polled anymore. - * - * @return A {@link Trigger} that is true while this autonomous routine is being polled. - */ - public Trigger active() { - return isActiveTrigger; - } - - /** Polls the routine. Should be called in the autonomous periodic method. */ - public void poll() { - if (DriverStation.isDisabled() || !allianceCtx.allianceKnownOrIgnored() || isKilled) { - isActive = false; - return; - } - pollCount++; - cycleTimestamp = Timer.getTimestamp(); - loop.poll(); - isActive = true; - } - - /** - * Gets the event loop that this routine is using. - * - * @return The event loop that this routine is using. - */ - public EventLoop loop() { - return loop; - } - - /** - * Creates a {@link Trigger} that is bound to the routine's {@link EventLoop}. - * - * @param condition The condition represented by the trigger. - * @return A {@link Trigger} that mirrors the state of the provided {@code condition} - */ - public Trigger observe(BooleanSupplier condition) { - return new Trigger(loop, condition); - } - - int pollCount() { - return pollCount; - } - - double cycleTimestamp() { - return cycleTimestamp; - } - - /** - * Updates the idle state of the routine. - * - * @param isIdle The new idle state of the routine. - */ - void updateIdle(boolean isIdle) { - this.isIdle = isIdle; - } - - /** - * Resets the routine. This can either be called on auto init or auto end to reset the routine - * incase you run it again. If this is called on a routine that doesn't need to be reset it will - * do nothing. - */ - public void reset() { - pollCount = 0; - cycleTimestamp = 0; - isActive = false; - } - - /** Kills the loop and prevents it from running again. */ - public void kill() { - CommandScheduler.getInstance().cancelAll(); - if (isKilled) { - return; - } - reset(); - ChoreoAlert.alert("Killed an auto loop", kWarning).set(true); - isKilled = true; - } - - /** - * Creates a trigger that is true when the routine is idle. - * - *

Idle is defined as no trajectories made by the routine are running. - * - * @return A trigger that is true when the routine is idle. - */ - public Trigger idle() { - return isIdleTrigger; - } - - /** - * Creates a new {@link AutoTrajectory} to be used in an auto routine. - * - * @param trajectoryName The name of the trajectory to use. - * @return A new {@link AutoTrajectory}. - */ - public AutoTrajectory trajectory(String trajectoryName) { - return factory.trajectory(trajectoryName, this, true); - } - - /** - * Creates a new {@link AutoTrajectory} to be used in an auto routine. - * - * @param trajectoryName The name of the trajectory to use. - * @param splitIndex The index of the split trajectory to use. - * @return A new {@link AutoTrajectory}. - */ - public AutoTrajectory trajectory(String trajectoryName, final int splitIndex) { - return factory.trajectory(trajectoryName, splitIndex, this, true); - } - - /** - * Creates a new {@link AutoTrajectory} to be used in an auto routine. - * - * @param The type of the trajectory samples. - * @param trajectory The trajectory to use. - * @return A new {@link AutoTrajectory}. - */ - public > AutoTrajectory trajectory( - Trajectory trajectory) { - return factory.trajectory(trajectory, this, true); - } - - /** - * Creates a trigger that produces a rising edge when any of the trajectories are finished. - * - * @param trajectory The first trajectory to watch. - * @param trajectories The other trajectories to watch - * @return a trigger that determines if any of the trajectories are finished - * @see #anyDoneDelayed(int, AutoTrajectory, AutoTrajectory...) A version of this method that - * takes a delay in cycles before the trigger is true. - */ - public Trigger anyDone(AutoTrajectory trajectory, AutoTrajectory... trajectories) { - return anyDoneDelayed(0, trajectory, trajectories); - } - - /** - * Creates a trigger that produces a rising edge when any of the trajectories are finished. - * - * @param cyclesToDelay The number of cycles to delay. - * @param trajectory The first trajectory to watch. - * @param trajectories The other trajectories to watch - * @return a trigger that goes true for one cycle whenever any of the trajectories finishes, - * delayed by the given number of cycles. - * @see AutoTrajectory#doneDelayed(int) - */ - public Trigger anyDoneDelayed( - int cyclesToDelay, AutoTrajectory trajectory, AutoTrajectory... trajectories) { - var trigger = trajectory.doneDelayed(cyclesToDelay); - for (int i = 0; i < trajectories.length; i++) { - trigger = trigger.or(trajectories[i].doneDelayed(cyclesToDelay)); - } - return trigger.and(this.active()); - } - - /** - * Creates a trigger that returns true when any of the trajectories given are active. - * - * @param trajectory The first trajectory to watch. - * @param trajectories The other trajectories to watch - * @return a trigger that determines if any of the trajectories are active - */ - public Trigger anyActive(AutoTrajectory trajectory, AutoTrajectory... trajectories) { - var trigger = trajectory.active(); - for (int i = 0; i < trajectories.length; i++) { - trigger = trigger.or(trajectories[i].active()); - } - return trigger.and(this.active()); - } - - /** - * Creates a trigger that returns true when any of the trajectories given are inactive. - * - *

This trigger will only return true if the routine is active. - * - * @param trajectory The first trajectory to watch. - * @param trajectories The other trajectories to watch - * @return a trigger that determines if any of the trajectories are inactive - */ - public Trigger allInactive(AutoTrajectory trajectory, AutoTrajectory... trajectories) { - var trigger = trajectory.inactive(); - for (int i = 0; i < trajectories.length; i++) { - trigger = trigger.and(trajectories[i].inactive()); - } - return trigger.and(this.active()); - } - - /** - * Creates a command that will poll this event loop and reset it when it is cancelled. - * - *

The command will end instantly and kill the routine if the alliance supplier returns an - * empty optional when the command is scheduled. - * - * @return A command that will poll this event loop and reset it when it is cancelled. - * @see #cmd(BooleanSupplier) A version of this method that takes a condition to finish the loop. - */ - public Command cmd() { - return cmd(() -> false); - } - - /** - * Creates a command that will poll this event loop and reset it when it is finished or canceled. - * - *

The command will end instantly and kill the routine if the alliance supplier returns an - * empty optional when the command is scheduled. - * - * @param finishCondition A condition that will finish the loop when it is true. - * @return A command that will poll this event loop and reset it when it is finished or canceled. - * @see #cmd() A version of this method that doesn't take a condition and never finishes except if - * the alliance supplier returns an empty optional when scheduled. - */ - public Command cmd(BooleanSupplier finishCondition) { - return Commands.either( - Commands.run(this::poll) - .finallyDo(this::reset) - .until(() -> DriverStation.isDisabled() || finishCondition.getAsBoolean()) - .withName(name), - Commands.runOnce( - () -> { - ChoreoAlert.alert("Alliance not known when starting routine", kWarning).set(true); - kill(); - }), - allianceCtx::allianceKnownOrIgnored); - } -} diff --git a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java index 0ee472d208..82f00ef72f 100644 --- a/choreolib/src/main/java/choreo/auto/AutoTrajectory.java +++ b/choreolib/src/main/java/choreo/auto/AutoTrajectory.java @@ -3,7 +3,6 @@ package choreo.auto; import static choreo.util.ChoreoAlert.allianceNotReady; -import static edu.wpi.first.wpilibj.Alert.AlertType.kError; import choreo.Choreo.TrajectoryLogger; import choreo.auto.AutoFactory.AllianceContext; @@ -15,21 +14,18 @@ import choreo.util.ChoreoAlert; import choreo.util.ChoreoAlert.MultiAlert; import choreo.util.ChoreoAllianceFlipUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.FunctionalCommand; -import edu.wpi.first.wpilibj2.command.ScheduleCommand; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.button.Trigger; import java.util.Optional; -import java.util.OptionalInt; -import java.util.function.BooleanSupplier; import java.util.function.Consumer; import java.util.function.Supplier; +import org.wpilib.command3.Command; +import org.wpilib.command3.Mechanism; +import org.wpilib.command3.Trigger; +import org.wpilib.driverstation.Alert.Level; +import org.wpilib.driverstation.internal.DriverStationBackend; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.geometry.Translation2d; +import org.wpilib.system.Timer; /** * A class that represents a trajectory that can be used in an autonomous routine and have triggers @@ -44,44 +40,35 @@ public class AutoTrajectory { // and far between. This helps with more novice users private static final MultiAlert triggerTimeNegative = - ChoreoAlert.multiAlert(causes -> "Trigger time cannot be negative for " + causes, kError); + ChoreoAlert.multiAlert(causes -> "Trigger time cannot be negative for " + causes, Level.HIGH); private static final MultiAlert triggerTimeAboveMax = ChoreoAlert.multiAlert( causes -> "Trigger time cannot be greater than total trajectory time for " + causes + ".", - kError); + Level.HIGH); private static final MultiAlert eventNotFound = - ChoreoAlert.multiAlert(causes -> "Event Markers " + causes + " not found.", kError); + ChoreoAlert.multiAlert(causes -> "Event Markers " + causes + " not found.", Level.HIGH); private static final MultiAlert noSamples = - ChoreoAlert.multiAlert(causes -> "Trajectories " + causes + " have no samples.", kError); + ChoreoAlert.multiAlert(causes -> "Trajectories " + causes + " have no samples.", Level.HIGH); private static final MultiAlert noInitialPose = ChoreoAlert.multiAlert( - causes -> "Unable to get initial pose for trajectories " + causes + ".", kError); - - private final String name; - private final Trajectory> trajectory; - private final TrajectoryLogger> trajectoryLogger; - private final Supplier poseSupplier; - private final Consumer resetOdometry; - private final Consumer> controller; - private final AllianceContext allianceCtx; + causes -> "Unable to get initial pose for trajectories " + causes + ".", Level.HIGH); + + final String name; + final Trajectory> trajectory; + final TrajectoryLogger> trajectoryLogger; + final Supplier poseSupplier; + final Consumer resetOdometry; + final Consumer> controller; + final AllianceContext allianceCtx; + final Mechanism driveMechanism; + final AutoBindings bindings; + private final Timer activeTimer = new Timer(); private final Timer inactiveTimer = new Timer(); - private final Subsystem driveSubsystem; - private final AutoRoutine routine; - private final AutoBindings bindings; - - /** - * A way to create slightly less triggers for many actions. Not static as to not leak triggers - * made here into another static EventLoop. - */ - private final Trigger offTrigger; /** If this trajectory us currently running */ private boolean isActive = false; - /** If the trajectory ran to completion */ - private boolean isCompleted = false; - /** Whether to suppress warnings for this trajectory. */ private boolean warnUser = true; @@ -94,8 +81,7 @@ public class AutoTrajectory { * @param controller The controller function. * @param allianceCtx The alliance context. * @param trajectoryLogger Optional trajectory logger. - * @param driveSubsystem Drive subsystem. - * @param routine Event loop. + * @param driveMechanism Drive Mechanism. * @param bindings {@link AutoFactory} */ > AutoTrajectory( @@ -106,8 +92,7 @@ > AutoTrajectory( Consumer controller, AllianceContext allianceCtx, TrajectoryLogger trajectoryLogger, - Subsystem driveSubsystem, - AutoRoutine routine, + Mechanism driveMechanism, AutoBindings bindings) { this.name = name; this.trajectory = trajectory; @@ -115,9 +100,7 @@ > AutoTrajectory( this.resetOdometry = resetOdometry; this.controller = controller; this.allianceCtx = allianceCtx; - this.driveSubsystem = driveSubsystem; - this.routine = routine; - this.offTrigger = new Trigger(routine.loop(), () -> false); + this.driveMechanism = driveMechanism; this.trajectoryLogger = trajectoryLogger; this.bindings = bindings; @@ -151,9 +134,7 @@ private void cmdInitialize() { inactiveTimer.stop(); inactiveTimer.reset(); isActive = true; - isCompleted = false; logTrajectory(true); - routine.updateIdle(false); } @SuppressWarnings("unchecked") @@ -182,7 +163,6 @@ private void cmdEnd(boolean interrupted) { activeTimer.reset(); inactiveTimer.start(); isActive = false; - isCompleted = !interrupted; if (!interrupted && allianceCtx.allianceKnownOrIgnored()) { var sampleOpt = trajectory.getFinalSample(allianceCtx.doFlip()); @@ -199,12 +179,11 @@ private void cmdEnd(boolean interrupted) { } logTrajectory(false); - routine.updateIdle(true); } private boolean cmdIsFinished() { return activeTimer.get() > trajectory.getTotalTime() - || !routine.active().getAsBoolean() + || DriverStationBackend.isDisabled() || !allianceCtx.allianceKnownOrIgnored(); } @@ -214,35 +193,29 @@ void suppressWarnings() { } /** - * Creates a command that allocates the drive subsystem and follows the trajectory using the + * Creates a command that allocates the drive Mechanism and follows the trajectory using the * factories control function * * @return The command that will follow the trajectory */ public Command cmd() { - // if the trajectory is empty, return a command that will print an error - if (trajectory.samples().isEmpty() && warnUser) { - return driveSubsystem.runOnce(() -> noSamples.addCause(name)).withName("Trajectory_" + name); - } - return new FunctionalCommand( - this::cmdInitialize, - this::cmdExecute, - this::cmdEnd, - this::cmdIsFinished, - driveSubsystem) - .withName("Trajectory_" + name); - } - - /** - * Creates a command that will schedule another command that will follow the trajectory. - * - *

This can be useful when putting {@link AutoTrajectory} commands in sequences that require - * subsystems also required by in AutoTrajectory-bound subsystems. - * - * @return The command that will schedule the trajectory following command. - */ - public Command spawnCmd() { - return new ScheduleCommand(cmd()).withName("Trajectory_" + name + "_Spawner"); + return driveMechanism + .run( + coro -> { + // if the trajectory is empty, return a command that will print an error + if (trajectory.samples().isEmpty() && warnUser) { + noSamples.addCause(name); + return; + } + cmdInitialize(); + while (!cmdIsFinished()) { + cmdExecute(); + coro.yield(); + } + cmdEnd(false); + }) + .whenCanceled(() -> cmdEnd(true)) + .named("Trajectory_" + name); } /** @@ -251,18 +224,20 @@ public Command spawnCmd() { * @return A command that resets the robot's odometry. */ public Command resetOdometry() { - return Commands.either( - Commands.runOnce(() -> resetOdometry.accept(getInitialPose().get()), driveSubsystem), - Commands.runOnce( - () -> { - if (warnUser) { - noInitialPose.addCause(name); - } - routine.kill(); - }) - .andThen(driveSubsystem.run(() -> {})), - () -> getInitialPose().isPresent()) - .withName("Trajectory_ResetOdometry_" + name); + return driveMechanism + .run( + coro -> { + var initialPose = getInitialPose(); + if (initialPose.isPresent()) { + resetOdometry.accept(initialPose.get()); + } else { + if (warnUser) { + noInitialPose.addCause(name); + } + coro.park(); + } + }) + .named("Trajectory_ResetOdometry_" + name); } /** @@ -298,8 +273,7 @@ public > AutoTrajectory mirrorX( (Consumer) controller, allianceCtx, (TrajectoryLogger) trajectoryLogger, - driveSubsystem, - routine, + driveMechanism, bindings); } @@ -321,8 +295,7 @@ public > AutoTrajectory mirrorY( (Consumer) controller, allianceCtx, (TrajectoryLogger) trajectoryLogger, - driveSubsystem, - routine, + driveMechanism, bindings); } @@ -344,8 +317,7 @@ public > AutoTrajectory rotateAr (Consumer) controller, allianceCtx, (TrajectoryLogger) trajectoryLogger, - driveSubsystem, - routine, + driveMechanism, bindings); } @@ -395,7 +367,7 @@ public Optional getFinalPose() { * @return A trigger that is true while the trajectory is scheduled. */ public Trigger active() { - return new Trigger(routine.loop(), () -> this.isActive && routine.active().getAsBoolean()); + return new Trigger(() -> this.isActive); } /** @@ -411,164 +383,7 @@ public Trigger inactive() { private Trigger timeTrigger(double targetTime, Timer timer) { // Make the trigger only be high for 1 cycle when the time has elapsed - return new Trigger( - routine.loop(), - new BooleanSupplier() { - double lastTimestamp = -1.0; - OptionalInt pollTarget = OptionalInt.empty(); - - public boolean getAsBoolean() { - if (!timer.isRunning()) { - lastTimestamp = -1.0; - pollTarget = OptionalInt.empty(); - return false; - } - double nowTimestamp = timer.get(); - try { - boolean timeAligns = lastTimestamp < targetTime && nowTimestamp >= targetTime; - if (pollTarget.isEmpty() && timeAligns) { - // if the time aligns for this cycle and it hasn't aligned previously this cycle - pollTarget = OptionalInt.of(routine.pollCount()); - return true; - } else if (pollTarget.isPresent() && routine.pollCount() == pollTarget.getAsInt()) { - // if the time aligned previously this cycle - return true; - } else if (pollTarget.isPresent()) { - // if the time aligned last cycle - pollTarget = OptionalInt.empty(); - return false; - } - return false; - } finally { - lastTimestamp = nowTimestamp; - } - } - }); - } - - private Trigger enterExitTrigger(Trigger enter, Trigger exit) { - return new Trigger( - routine.loop(), - new BooleanSupplier() { - boolean output = false; - - @Override - public boolean getAsBoolean() { - if (enter.getAsBoolean()) { - output = true; - } - if (exit.getAsBoolean()) { - output = false; - } - return output; - } - }); - } - - /** - * Returns a trigger that rises to true a number of cycles after the trajectory ends and falls - * after one pulse. - * - *

This is different from inactive() in a few ways. - * - *

    - *
  • This will never be true if the trajectory is interrupted - *
  • This will never be true before the trajectory is run - *
  • This will fall the next cycle after the trajectory ends - *
- * - *

Why does the trigger need to fall? - * - * 


-   * //Lets say we had this code segment
-   * Trigger hasGamepiece = ...;
-   * Trigger noGamepiece = hasGamepiece.negate();
-   *
-   * AutoTrajectory rushMidTraj = ...;
-   * AutoTrajectory goShootGamepiece = ...;
-   * AutoTrajectory pickupAnotherGamepiece = ...;
-   *
-   * routine.enabled().onTrue(rushMidTraj.cmd());
-   *
-   * rushMidTraj.doneDelayed(10).and(noGamepiece).onTrue(pickupAnotherGamepiece.cmd());
-   * rushMidTraj.doneDelayed(10).and(hasGamepiece).onTrue(goShootGamepiece.cmd());
-   *
-   * // If done never falls when a new trajectory is scheduled
-   * // then these triggers leak into the next trajectory, causing the next note pickup
-   * // to trigger goShootGamepiece.cmd() even if we no longer care about these checks
-   *
- * - * @param seconds The seconds to delay the trigger from rising to true. - * @return A trigger that is true when the trajectory is finished. - */ - public Trigger doneDelayed(double seconds) { - return timeTrigger(seconds, inactiveTimer).and(new Trigger(routine.loop(), () -> isCompleted)); - } - - /** - * Returns a trigger that rises to true when the trajectory ends and falls after one pulse. - * - *

This is different from inactive() in a few ways. - * - *

    - *
  • This will never be true if the trajectory is interrupted - *
  • This will never be true before the trajectory is run - *
  • This will fall the next cycle after the trajectory ends - *
- * - *

Why does the trigger need to fall? - * - * 


-   * //Lets say we had this code segment
-   * Trigger hasGamepiece = ...;
-   * Trigger noGamepiece = hasGamepiece.negate();
-   *
-   * AutoTrajectory rushMidTraj = ...;
-   * AutoTrajectory goShootGamepiece = ...;
-   * AutoTrajectory pickupAnotherGamepiece = ...;
-   *
-   * routine.enabled().onTrue(rushMidTraj.cmd());
-   *
-   * rushMidTraj.done().and(noGamepiece).onTrue(pickupAnotherGamepiece.cmd());
-   * rushMidTraj.done().and(hasGamepiece).onTrue(goShootGamepiece.cmd());
-   *
-   * // If done never falls when a new trajectory is scheduled
-   * // then these triggers leak into the next trajectory, causing the next note pickup
-   * // to trigger goShootGamepiece.cmd() even if we no longer care about these checks
-   *
- * - * @return A trigger that is true when the trajectory is finished. - */ - public Trigger done() { - return doneDelayed(0); - } - - /** - * Returns a trigger that stays true for a number of cycles after the trajectory ends. - * - * @param seconds Seconds to stay true after the trajectory ends. - * @return A trigger that stays true for a number of cycles after the trajectory ends. - */ - public Trigger doneFor(double seconds) { - return enterExitTrigger(doneDelayed(0), doneDelayed(seconds)); - } - - /** - * Returns a trigger that is true when the trajectory was the last one active and is done. - * - * @return A trigger that is true when the trajectory was the last one active and is done. - */ - public Trigger recentlyDone() { - return enterExitTrigger(doneDelayed(0), routine.idle().negate()); - } - - /** - * A shorthand for `.done().onTrue(otherTrajectory.cmd())` - * - * @param otherTrajectory The other trajectory to run when this one is done. - */ - public void chain(AutoTrajectory otherTrajectory) { - done().onTrue(otherTrajectory.cmd()); + return new Trigger(() -> timer.get() > targetTime).risingEdge(); } /** @@ -583,7 +398,7 @@ public Trigger atTime(double timeSinceStart) { if (warnUser) { triggerTimeNegative.addCause(name); } - return offTrigger; + return new Trigger(() -> false); } // The timer should never exceed the total trajectory time so report this as a warning @@ -591,7 +406,7 @@ public Trigger atTime(double timeSinceStart) { if (warnUser) { triggerTimeAboveMax.addCause(name); } - return offTrigger; + return new Trigger(() -> false); } return timeTrigger(timeSinceStart, activeTimer); @@ -623,7 +438,7 @@ public Trigger atTimeBeforeEnd(double timeBeforeEnd) { */ public Trigger atTime(String eventName) { boolean foundEvent = false; - Trigger trig = offTrigger; + Trigger trig = new Trigger(() -> false); for (var event : trajectory.getEvents(eventName)) { // This could create a lot of objects, could be done a more efficient way @@ -669,7 +484,6 @@ private boolean withinTolerance(Rotation2d lhs, Rotation2d rhs, double tolerance public Trigger atPose(Pose2d pose, double toleranceMeters, double toleranceRadians) { Pose2d flippedPose = ChoreoAllianceFlipUtil.flip(pose); return new Trigger( - routine.loop(), () -> { if (allianceCtx.allianceKnownOrIgnored()) { final Pose2d currentPose = poseSupplier.get(); @@ -715,7 +529,7 @@ public Trigger atPose(Pose2d pose, double toleranceMeters, double toleranceRadia */ public Trigger atPose(String eventName, double toleranceMeters, double toleranceRadians) { boolean foundEvent = false; - Trigger trig = offTrigger; + Trigger trig = new Trigger(() -> false); for (var event : trajectory.getEvents(eventName)) { // This could create a lot of objects, could be done a more efficient way @@ -761,7 +575,6 @@ public Trigger atPose(String eventName, double toleranceMeters, double tolerance public Trigger atTranslation(Translation2d translation, double toleranceMeters) { Translation2d flippedTranslation = ChoreoAllianceFlipUtil.flip(translation); return new Trigger( - routine.loop(), () -> { if (allianceCtx.allianceKnownOrIgnored()) { final Translation2d currentTrans = poseSupplier.get().getTranslation(); @@ -794,7 +607,7 @@ public Trigger atTranslation(Translation2d translation, double toleranceMeters) */ public Trigger atTranslation(String eventName, double toleranceMeters) { boolean foundEvent = false; - Trigger trig = offTrigger; + Trigger trig = new Trigger(() -> false); for (var event : trajectory.getEvents(eventName)) { // This could create a lot of objects, could be done a more efficient way diff --git a/choreolib/src/main/java/choreo/trajectory/DifferentialSample.java b/choreolib/src/main/java/choreo/trajectory/DifferentialSample.java index be60553ec7..f44b7ae11f 100644 --- a/choreolib/src/main/java/choreo/trajectory/DifferentialSample.java +++ b/choreolib/src/main/java/choreo/trajectory/DifferentialSample.java @@ -3,19 +3,19 @@ package choreo.trajectory; import choreo.util.ChoreoAllianceFlipUtil; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.numbers.N6; -import edu.wpi.first.math.system.NumericalIntegration; -import edu.wpi.first.util.struct.Struct; import java.nio.ByteBuffer; import java.util.function.BiFunction; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.ChassisVelocities; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.math.numbers.N1; +import org.wpilib.math.numbers.N3; +import org.wpilib.math.numbers.N6; +import org.wpilib.math.system.NumericalIntegration; +import org.wpilib.math.util.MathUtil; +import org.wpilib.util.struct.Struct; /** A single differential drive robot sample in a Trajectory. */ public class DifferentialSample implements TrajectorySample { @@ -112,11 +112,11 @@ public Pose2d getPose() { * Returns the field-relative chassis speeds of this sample. * * @return the field-relative chassis speeds of this sample. - * @see edu.wpi.first.math.kinematics.DifferentialDriveKinematics#toChassisSpeeds + * @see org.wpilib.math.kinematics.DifferentialDriveKinematics#toChassisVelocities */ @Override - public ChassisSpeeds getChassisSpeeds() { - return new ChassisSpeeds((vl + vr) / 2, 0, omega); + public ChassisVelocities getChassisVelocities() { + return new ChassisVelocities((vl + vr) / 2, 0, omega); } @Override @@ -156,7 +156,7 @@ public DifferentialSample interpolate(DifferentialSample endValue, double timest var sample = NumericalIntegration.rkdp(f, initialState, VecBuilder.fill(al, ar, alpha), τ); return new DifferentialSample( - MathUtil.interpolate(this.t, endValue.t, scale), + this.t + (endValue.t - this.t) * scale, sample.get(0, 0), sample.get(1, 0), sample.get(2, 0), @@ -166,8 +166,8 @@ public DifferentialSample interpolate(DifferentialSample endValue, double timest this.al, this.ar, this.alpha, - MathUtil.interpolate(this.fl, endValue.fl, scale), - MathUtil.interpolate(this.fr, endValue.fr, scale)); + this.fl + (endValue.fl - this.fl) * scale, + this.fr + (endValue.fr - this.fr) * scale); } public DifferentialSample flipped() { @@ -210,7 +210,7 @@ public String getTypeName() { @Override public int getSize() { - return Struct.kSizeDouble * 10; + return Struct.DOUBLE_SIZE * 10; } @Override diff --git a/choreolib/src/main/java/choreo/trajectory/SwerveSample.java b/choreolib/src/main/java/choreo/trajectory/SwerveSample.java index b9597e632a..f9294d4a7c 100644 --- a/choreolib/src/main/java/choreo/trajectory/SwerveSample.java +++ b/choreolib/src/main/java/choreo/trajectory/SwerveSample.java @@ -4,12 +4,12 @@ import choreo.util.ChoreoAllianceFlipUtil; import choreo.util.ChoreoArrayUtil; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.util.struct.Struct; import java.nio.ByteBuffer; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.ChassisVelocities; +import org.wpilib.math.util.MathUtil; +import org.wpilib.util.struct.Struct; /** A single swerve robot sample in a Trajectory. */ public class SwerveSample implements TrajectorySample { @@ -137,8 +137,8 @@ public Pose2d getPose() { } @Override - public ChassisSpeeds getChassisSpeeds() { - return new ChassisSpeeds(vx, vy, omega); + public ChassisVelocities getChassisVelocities() { + return new ChassisVelocities(vx, vy, omega); } @Override @@ -149,9 +149,9 @@ public SwerveSample interpolate(SwerveSample endValue, double timestamp) { double[] interp_fy = new double[4]; for (int i = 0; i < 4; ++i) { interp_fx[i] = - MathUtil.interpolate(this.moduleForcesX()[i], endValue.moduleForcesX()[i], scale); + this.moduleForcesX()[i] + (endValue.moduleForcesX()[i] - this.moduleForcesX()[i]) * scale; interp_fy[i] = - MathUtil.interpolate(this.moduleForcesY()[i], endValue.moduleForcesY()[i], scale); + this.moduleForcesY()[i] + (endValue.moduleForcesY()[i] - this.moduleForcesY()[i]) * scale; } // Integrate the acceleration to get the rest of the state, since linearly @@ -232,7 +232,7 @@ public String getTypeName() { @Override public int getSize() { - return Struct.kSizeDouble * 18; + return Struct.DOUBLE_SIZE * 18; } @Override diff --git a/choreolib/src/main/java/choreo/trajectory/Trajectory.java b/choreolib/src/main/java/choreo/trajectory/Trajectory.java index 584ce29baf..edb3df8a09 100644 --- a/choreolib/src/main/java/choreo/trajectory/Trajectory.java +++ b/choreolib/src/main/java/choreo/trajectory/Trajectory.java @@ -2,10 +2,10 @@ package choreo.trajectory; -import edu.wpi.first.math.geometry.Pose2d; import java.util.ArrayList; import java.util.List; import java.util.Optional; +import org.wpilib.math.geometry.Pose2d; /** * A trajectory loaded from Choreo. @@ -255,7 +255,7 @@ public Trajectory mirrorY() { public Trajectory rotateAround() { var flippedStates = new ArrayList(); for (var state : samples) { - flippedStates.add(state.rotateAround()); + flippedStates.add(state.mirrorY()); } return new Trajectory(this.name, flippedStates, this.splits, this.events); } diff --git a/choreolib/src/main/java/choreo/trajectory/TrajectorySample.java b/choreolib/src/main/java/choreo/trajectory/TrajectorySample.java index 4f991d7ab7..a8f7af8835 100644 --- a/choreolib/src/main/java/choreo/trajectory/TrajectorySample.java +++ b/choreolib/src/main/java/choreo/trajectory/TrajectorySample.java @@ -2,10 +2,10 @@ package choreo.trajectory; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.interpolation.Interpolatable; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.util.struct.StructSerializable; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.interpolation.Interpolatable; +import org.wpilib.math.kinematics.ChassisVelocities; +import org.wpilib.util.struct.StructSerializable; /** * The generic interface for a sample in a trajectory. @@ -33,7 +33,7 @@ public interface TrajectorySample> * * @return the field-relative chassis speeds of this sample. */ - ChassisSpeeds getChassisSpeeds(); + ChassisVelocities getChassisVelocities(); /** * Returns this sample, flipped to the other alliance according to the symmetry of the field. diff --git a/choreolib/src/main/java/choreo/util/ChoreoAlert.java b/choreolib/src/main/java/choreo/util/ChoreoAlert.java index 4280a840dd..d44b0e2830 100644 --- a/choreolib/src/main/java/choreo/util/ChoreoAlert.java +++ b/choreolib/src/main/java/choreo/util/ChoreoAlert.java @@ -2,11 +2,11 @@ package choreo.util; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; import java.util.ArrayList; import java.util.List; import java.util.function.Function; +import org.wpilib.driverstation.Alert; +import org.wpilib.driverstation.Alert.Level; /** A utility class for creating alerts under the "Choreo Alerts" group. */ public class ChoreoAlert { @@ -14,23 +14,22 @@ public class ChoreoAlert { * Creates an alert under the "Choreo" group. * * @param name The name of the alert - * @param type The type of alert + * @param level The level of alert * @return an Alert published under the "Choreo" group */ - public static Alert alert(String name, AlertType type) { - return new Alert("Choreo Alerts", name, type); + public static Alert alert(String name, Level level) { + return new Alert("Choreo Alerts", name, level); } /** * Creates a {@link MultiAlert} under the "Choreo" group. * * @param textGenerator A function that accepts a list of causes and returns an alert message - * @param type The type of alert + * @param level The level of alert * @return a MultiAlert published under the "Choreo" group */ - public static MultiAlert multiAlert( - Function, String> textGenerator, AlertType type) { - return new MultiAlert(textGenerator, type); + public static MultiAlert multiAlert(Function, String> textGenerator, Level level) { + return new MultiAlert(textGenerator, level); } /** @@ -41,8 +40,8 @@ public static class MultiAlert extends Alert { private final Function, String> textGenerator; private final List causes = new ArrayList<>(); - MultiAlert(Function, String> textGenerator, AlertType type) { - super("Choreo Alerts", textGenerator.apply(List.of()), type); + MultiAlert(Function, String> textGenerator, Level level) { + super("Choreo Alerts", textGenerator.apply(List.of()), level); this.textGenerator = textGenerator; } @@ -67,5 +66,5 @@ private ChoreoAlert() {} * determined. */ public static final Alert allianceNotReady = - ChoreoAlert.alert("Alliance used but not ready", AlertType.kError); + ChoreoAlert.alert("Alliance used but not ready", Level.HIGH); } diff --git a/choreolib/src/main/java/choreo/util/ChoreoAllianceFlipUtil.java b/choreolib/src/main/java/choreo/util/ChoreoAllianceFlipUtil.java index 82cb06f2f6..1e6d509166 100644 --- a/choreolib/src/main/java/choreo/util/ChoreoAllianceFlipUtil.java +++ b/choreolib/src/main/java/choreo/util/ChoreoAllianceFlipUtil.java @@ -2,21 +2,13 @@ package choreo.util; -import static choreo.util.FieldDimensions.FIELD_LENGTH; -import static choreo.util.FieldDimensions.FIELD_WIDTH; - import choreo.trajectory.DifferentialSample; import choreo.trajectory.SwerveSample; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import java.util.Optional; import java.util.function.Supplier; +import org.wpilib.driverstation.Alliance; +import org.wpilib.driverstation.MatchState; +import org.wpilib.math.geometry.*; /** * A utility to standardize flipping of coordinate data based on the current alliance across @@ -34,17 +26,10 @@ public abstract static class Flipper { /** Constructs a flipper. */ protected Flipper() {} - /** - * X becomes fieldLength - x, leaves the y coordinate unchanged, and heading becomes PI - - * heading. - */ + /** X becomes -x, leaves the y coordinate unchanged, and heading becomes PI - heading. */ static class MirroredX extends Flipper { - public MirroredX(double fieldLength, double fieldWidth) { - super(fieldLength, fieldWidth); - } - public double flipX(double x) { - return getFieldLength() - x; + return -x; } public double flipY(double y) { @@ -112,12 +97,10 @@ public DifferentialSample flip(DifferentialSample sample) { * Creates a new flipper that mirrors across x=fieldLength/2. This is intended for * alliance-based flipping in rotationally asymmetric games. * - * @param fieldLength The length of the field. - * @param fieldWidth The width of the field. * @return a new flipper. */ - public static MirroredX mirroredX(double fieldLength, double fieldWidth) { - return new MirroredX(fieldLength, fieldWidth); + public static MirroredX mirroredX() { + return new MirroredX(); } /** @@ -125,16 +108,12 @@ public static MirroredX mirroredX(double fieldLength, double fieldWidth) { * fieldWidth-y, and heading becomes -heading. */ static class MirroredY extends Flipper { - public MirroredY(double fieldLength, double fieldWidth) { - super(fieldLength, fieldWidth); - } - public double flipX(double x) { return x; } public double flipY(double y) { - return getFieldWidth() - y; + return -y; } public double flipHeading(double heading) { @@ -201,24 +180,16 @@ public DifferentialSample flip(DifferentialSample sample) { * same alliance half, but can be used to mirror left and right sides of the field, from driver * perspective. * - * @param fieldLength The length of the field. - * @param fieldWidth The width of the field. * @return a new flipper. */ - public static MirroredY mirroredY(double fieldLength, double fieldWidth) { - return new MirroredY(fieldLength, fieldWidth); + public static MirroredY mirroredY() { + return new MirroredY(); } /** X becomes fieldLength - x, Y becomes fieldWidth - y, and heading becomes PI + heading. */ static class RotatedAround extends Flipper { - public RotatedAround(double fieldLength, double fieldWidth) { - super(fieldLength, fieldWidth); - this.mirrorX = mirroredX(fieldLength, fieldWidth); - this.mirrorY = mirroredY(fieldLength, fieldWidth); - } - - MirroredX mirrorX = mirroredX(getFieldLength(), getFieldWidth()); - MirroredY mirrorY = mirroredY(getFieldLength(), getFieldWidth()); + MirroredX mirrorX = mirroredX(); + MirroredY mirrorY = mirroredY(); public double flipX(double x) { return mirrorX.flipX(mirrorY.flipX(x)); @@ -286,12 +257,10 @@ public DifferentialSample flip(DifferentialSample sample) { * Creates a new rotated flipper around the center of the field. This is intended for * alliance-based flipping in rotationally symmetric games. * - * @param fieldLength The length of the field. - * @param fieldWidth The width of the field. * @return A new rotated flipper around the center of the field. */ - public static RotatedAround rotatedAround(double fieldLength, double fieldWidth) { - return new RotatedAround(fieldLength, fieldWidth); + public static RotatedAround rotatedAround() { + return new RotatedAround(); } // ***** Class Definition *****/ @@ -310,24 +279,6 @@ public Flipper(double fieldLength, double fieldWidth) { this.fieldWidth = fieldWidth; } - /** - * Gets the length (X axis) of the field. - * - * @return the length (X axis) of the field. - */ - public double getFieldLength() { - return fieldLength; - } - - /** - * Gets the width (Y axis) of the field. - * - * @return the width (Y axis) of the field. - */ - public double getFieldWidth() { - return fieldWidth; - } - /** * Flips the X coordinate. * @@ -429,7 +380,7 @@ public Pose3d flip(Pose3d pose) { } /** The default flipper for the current FRC year. */ - public static Flipper FRC_CURRENT = rotatedAround(FIELD_LENGTH, FIELD_WIDTH); + public static Flipper FRC_CURRENT = rotatedAround(); } private static Flipper activeAllianceFlip; @@ -490,7 +441,7 @@ public static Flipper getRotateAround() { * @return If you are on red alliance. */ public static boolean shouldFlip() { - return DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red; + return MatchState.getAlliance().orElse(Alliance.BLUE) == Alliance.RED; } /** @@ -504,9 +455,9 @@ public static boolean shouldFlip() { */ public static void setFlipper(Flipper flipper) { activeAllianceFlip = flipper; - activeMirrorX = Flipper.mirroredX(flipper.getFieldLength(), flipper.getFieldWidth()); - activeMirrorY = Flipper.mirroredY(flipper.getFieldLength(), flipper.getFieldWidth()); - activeRotateAround = Flipper.rotatedAround(flipper.getFieldLength(), flipper.getFieldWidth()); + activeMirrorX = Flipper.mirroredX(); + activeMirrorY = Flipper.mirroredY(); + activeRotateAround = Flipper.rotatedAround(); } /** @@ -638,7 +589,7 @@ public static Supplier> optionalFlippedPose2d( return () -> doFlip - ? allianceOpt.get().flatMap(ally -> ally == Alliance.Red ? flippedPose : poseOpt) + ? allianceOpt.get().flatMap(ally -> ally == Alliance.RED ? flippedPose : poseOpt) : poseOpt; } @@ -666,7 +617,7 @@ public static Supplier> optionalFlippedTranslation2d( doFlip ? allianceOpt .get() - .flatMap(ally -> ally == Alliance.Red ? flippedTranslation : translationOpt) + .flatMap(ally -> ally == Alliance.RED ? flippedTranslation : translationOpt) : translationOpt; } } diff --git a/choreolib/src/main/java/choreo/util/ChoreoArrayUtil.java b/choreolib/src/main/java/choreo/util/ChoreoArrayUtil.java index 7a8dc42869..36b1eaf8b5 100644 --- a/choreolib/src/main/java/choreo/util/ChoreoArrayUtil.java +++ b/choreolib/src/main/java/choreo/util/ChoreoArrayUtil.java @@ -26,10 +26,7 @@ public static boolean zipEquals( if (arr1 == null && arr2 == null) { return true; } - if (arr1 != null && arr2 == null) { - return false; - } - if (arr1 == null && arr2 != null) { + if (arr1 == null || arr2 == null) { return false; } // arr1 and arr2 both not null diff --git a/choreolib/src/main/native/cpp/choreo/trajectory/DifferentialSample.cpp b/choreolib/src/main/native/cpp/choreo/trajectory/DifferentialSample.cpp index a11e3b34aa..435a68469f 100644 --- a/choreolib/src/main/native/cpp/choreo/trajectory/DifferentialSample.cpp +++ b/choreolib/src/main/native/cpp/choreo/trajectory/DifferentialSample.cpp @@ -2,37 +2,36 @@ #include "choreo/trajectory/DifferentialSample.hpp" -#include +#include -void choreo::to_json(wpi::json& json, +void choreo::to_json(wpi::util::json& json, const DifferentialSample& trajectorySample) { - json = wpi::json{{"t", trajectorySample.timestamp.value()}, - {"x", trajectorySample.x.value()}, - {"y", trajectorySample.y.value()}, - {"heading", trajectorySample.heading.value()}, - {"vl", trajectorySample.vl.value()}, - {"vr", trajectorySample.vr.value()}, - {"omega", trajectorySample.omega.value()}, - {"al", trajectorySample.al.value()}, - {"ar", trajectorySample.ar.value()}, - {"fl", trajectorySample.fl.value()}, - {"fr", trajectorySample.fr.value()}}; + json = wpi::util::json::object( + "t", trajectorySample.timestamp.value(), "x", trajectorySample.x.value(), + "y", trajectorySample.y.value(), "heading", + trajectorySample.heading.value(), "vl", trajectorySample.vl.value(), "vr", + trajectorySample.vr.value(), "omega", trajectorySample.omega.value(), + "al", trajectorySample.al.value(), "ar", trajectorySample.ar.value(), + "fl", trajectorySample.fl.value(), "fr", trajectorySample.fr.value()); } -void choreo::from_json(const wpi::json& json, +void choreo::from_json(const wpi::util::json& json, DifferentialSample& trajectorySample) { - trajectorySample.timestamp = units::second_t{json.at("t").get()}; - trajectorySample.x = units::meter_t{json.at("x").get()}; - trajectorySample.y = units::meter_t{json.at("y").get()}; - trajectorySample.heading = units::radian_t{json.at("heading").get()}; - trajectorySample.vl = units::meters_per_second_t{json.at("vl").get()}; - trajectorySample.vr = units::meters_per_second_t{json.at("vr").get()}; + trajectorySample.timestamp = wpi::units::second_t{json.at("t").get_number()}; + trajectorySample.x = wpi::units::meter_t{json.at("x").get_number()}; + trajectorySample.y = wpi::units::meter_t{json.at("y").get_number()}; + trajectorySample.heading = + wpi::units::radian_t{json.at("heading").get_number()}; + trajectorySample.vl = + wpi::units::meters_per_second_t{json.at("vl").get_number()}; + trajectorySample.vr = + wpi::units::meters_per_second_t{json.at("vr").get_number()}; trajectorySample.omega = - units::radians_per_second_t{json.at("omega").get()}; + wpi::units::radians_per_second_t{json.at("omega").get_number()}; trajectorySample.al = - units::meters_per_second_squared_t{json.at("al").get()}; + wpi::units::meters_per_second_squared_t{json.at("al").get_number()}; trajectorySample.ar = - units::meters_per_second_squared_t{json.at("ar").get()}; - trajectorySample.fl = units::newton_t{json.at("fl").get()}; - trajectorySample.fr = units::newton_t{json.at("fr").get()}; + wpi::units::meters_per_second_squared_t{json.at("ar").get_number()}; + trajectorySample.fl = wpi::units::newton_t{json.at("fl").get_number()}; + trajectorySample.fr = wpi::units::newton_t{json.at("fr").get_number()}; } diff --git a/choreolib/src/main/native/cpp/choreo/trajectory/EventMarker.cpp b/choreolib/src/main/native/cpp/choreo/trajectory/EventMarker.cpp index 4205a89311..aed7a623dc 100644 --- a/choreolib/src/main/native/cpp/choreo/trajectory/EventMarker.cpp +++ b/choreolib/src/main/native/cpp/choreo/trajectory/EventMarker.cpp @@ -4,22 +4,23 @@ #include -#include +#include -void choreo::to_json(wpi::json& json, const EventMarker& event) { - json = wpi::json{{"data", wpi::json{{"t", event.timestamp.value()}}}, - {"event", wpi::json{{"name", event.event}}}}; +void choreo::to_json(wpi::util::json& json, const EventMarker& event) { + json = wpi::util::json::object( + "data", wpi::util::json::object("t", event.timestamp.value()), "event", + wpi::util::json::object("name", event.event)); } -void choreo::from_json(const wpi::json& json, EventMarker& event) { +void choreo::from_json(const wpi::util::json& json, EventMarker& event) { auto targetTimestamp = json.at("from").at("targetTimestamp"); if (!targetTimestamp.is_number()) { - event.timestamp = units::second_t{-1}; + event.timestamp = wpi::units::second_t{-1}; event.event = ""; } else { - event.timestamp = - units::second_t{json.at("from").at("offset").at("val").get() + - targetTimestamp.get()}; - event.event = json.at("name").get(); + event.timestamp = wpi::units::second_t{ + json.at("from").at("offset").at("val").get_number() + + targetTimestamp.get_number()}; + event.event = json.at("name").get_string(); } } diff --git a/choreolib/src/main/native/cpp/choreo/trajectory/SwerveSample.cpp b/choreolib/src/main/native/cpp/choreo/trajectory/SwerveSample.cpp index 2bbc2f267d..467c7d2cdf 100644 --- a/choreolib/src/main/native/cpp/choreo/trajectory/SwerveSample.cpp +++ b/choreolib/src/main/native/cpp/choreo/trajectory/SwerveSample.cpp @@ -4,52 +4,54 @@ #include -#include +#include -void choreo::to_json(wpi::json& json, const SwerveSample& trajectorySample) { +void choreo::to_json(wpi::util::json& json, + const SwerveSample& trajectorySample) { std::array fx; std::transform(trajectorySample.moduleForcesX.begin(), trajectorySample.moduleForcesX.end(), fx.begin(), - [](units::newton_t x) { return x.value(); }); + [](wpi::units::newton_t x) { return x.value(); }); std::array fy; std::transform(trajectorySample.moduleForcesY.begin(), trajectorySample.moduleForcesY.end(), fy.begin(), - [](units::newton_t x) { return x.value(); }); + [](wpi::units::newton_t x) { return x.value(); }); - json = wpi::json{{"t", trajectorySample.timestamp.value()}, - {"x", trajectorySample.x.value()}, - {"y", trajectorySample.y.value()}, - {"heading", trajectorySample.heading.value()}, - {"vx", trajectorySample.vx.value()}, - {"vy", trajectorySample.vy.value()}, - {"omega", trajectorySample.omega.value()}, - {"ax", trajectorySample.ax.value()}, - {"ay", trajectorySample.ay.value()}, - {"alpha", trajectorySample.alpha.value()}, - {"fx", fx}, - {"fy", fy}}; + json = wpi::util::json::object( + "t", trajectorySample.timestamp.value(), "x", trajectorySample.x.value(), + "y", trajectorySample.y.value(), "heading", + trajectorySample.heading.value(), "vx", trajectorySample.vx.value(), "vy", + trajectorySample.vy.value(), "omega", trajectorySample.omega.value(), + "ax", trajectorySample.ax.value(), "ay", trajectorySample.ay.value(), + "alpha", trajectorySample.alpha.value(), "fx", fx, "fy", fy); } -void choreo::from_json(const wpi::json& json, SwerveSample& trajectorySample) { - trajectorySample.timestamp = units::second_t{json.at("t").get()}; - trajectorySample.x = units::meter_t{json.at("x").get()}; - trajectorySample.y = units::meter_t{json.at("y").get()}; - trajectorySample.heading = units::radian_t{json.at("heading").get()}; - trajectorySample.vx = units::meters_per_second_t{json.at("vx").get()}; - trajectorySample.vy = units::meters_per_second_t{json.at("vy").get()}; +void choreo::from_json(const wpi::util::json& json, + SwerveSample& trajectorySample) { + trajectorySample.timestamp = wpi::units::second_t{json.at("t").get_number()}; + trajectorySample.x = wpi::units::meter_t{json.at("x").get_number()}; + trajectorySample.y = wpi::units::meter_t{json.at("y").get_number()}; + trajectorySample.heading = + wpi::units::radian_t{json.at("heading").get_number()}; + trajectorySample.vx = + wpi::units::meters_per_second_t{json.at("vx").get_number()}; + trajectorySample.vy = + wpi::units::meters_per_second_t{json.at("vy").get_number()}; trajectorySample.omega = - units::radians_per_second_t{json.at("omega").get()}; + wpi::units::radians_per_second_t{json.at("omega").get_number()}; trajectorySample.ax = - units::meters_per_second_squared_t{json.at("ax").get()}; + wpi::units::meters_per_second_squared_t{json.at("ax").get_number()}; trajectorySample.ay = - units::meters_per_second_squared_t{json.at("ay").get()}; + wpi::units::meters_per_second_squared_t{json.at("ay").get_number()}; trajectorySample.alpha = - units::radians_per_second_squared_t{json.at("alpha").get()}; + wpi::units::radians_per_second_squared_t{json.at("alpha").get_number()}; const auto& fx = json.at("fx"); const auto& fy = json.at("fy"); for (int i = 0; i < 4; ++i) { - trajectorySample.moduleForcesX[i] = units::newton_t{fx.at(i).get()}; - trajectorySample.moduleForcesY[i] = units::newton_t{fy.at(i).get()}; + trajectorySample.moduleForcesX[i] = + wpi::units::newton_t{fx.at(i).get_number()}; + trajectorySample.moduleForcesY[i] = + wpi::units::newton_t{fy.at(i).get_number()}; } } diff --git a/choreolib/src/main/native/cpp/choreo/trajectory/Trajectory.cpp b/choreolib/src/main/native/cpp/choreo/trajectory/Trajectory.cpp index 43893ea65b..278784b40f 100644 --- a/choreolib/src/main/native/cpp/choreo/trajectory/Trajectory.cpp +++ b/choreolib/src/main/native/cpp/choreo/trajectory/Trajectory.cpp @@ -4,59 +4,58 @@ #include -#include +#include -void choreo::to_json(wpi::json& json, +#include "choreo/util/ArrayUtil.hpp" + +void choreo::to_json(wpi::util::json& json, const Trajectory& trajectory) { - json = wpi::json{{"name", trajectory.name}, - {"samples", trajectory.samples}, - {"splits", trajectory.splits}, - {"events", trajectory.events}}; + json = wpi::util::json::object( + "name", trajectory.name, "samples", trajectory.samples, "splits", + trajectory.splits, "events", trajectory.events); } -void choreo::from_json(const wpi::json& json, +void choreo::from_json(const wpi::util::json& json, Trajectory& trajectory) { - trajectory.name = json.at("name").get(); - trajectory.samples = - json.at("trajectory").at("samples").get>(); + trajectory.name = json.at("name").get_string(); + trajectory.samples = choreo::util::ParseObjectArray( + json.at("trajectory").at("samples")); trajectory.splits = - json.at("trajectory").at("splits").get>(); + choreo::util::ParseIntArray(json.at("trajectory").at("splits")); // Add 0 as the first split index. if (trajectory.splits.size() == 0 || trajectory.splits.at(0) != 0) { trajectory.splits.insert(trajectory.splits.begin(), 0); } - auto events = json.at("events").get>(); + auto events = choreo::util::ParseObjectArray(json.at("events")); trajectory.events.clear(); for (EventMarker event : events) { - if (event.timestamp >= units::second_t{0} || event.event.size() == 0) { + if (event.timestamp >= wpi::units::second_t{0} || event.event.size() == 0) { trajectory.events.push_back(event); } } } -void choreo::to_json(wpi::json& json, +void choreo::to_json(wpi::util::json& json, const Trajectory& trajectory) { - json = wpi::json{{"name", trajectory.name}, - {"samples", trajectory.samples}, - {"splits", trajectory.splits}, - {"events", trajectory.events}}; + json = wpi::util::json::object( + "name", trajectory.name, "samples", trajectory.samples, "splits", + trajectory.splits, "events", trajectory.events); } -void choreo::from_json(const wpi::json& json, +void choreo::from_json(const wpi::util::json& json, Trajectory& trajectory) { - trajectory.samples = json.at("trajectory") - .at("samples") - .get>(); + trajectory.samples = choreo::util::ParseObjectArray( + json.at("trajectory").at("samples")); trajectory.splits = - json.at("trajectory").at("splits").get>(); + choreo::util::ParseIntArray(json.at("trajectory").at("splits")); // Add 0 as the first split index. if (trajectory.splits.size() == 0 || trajectory.splits.at(0) != 0) { trajectory.splits.insert(trajectory.splits.begin(), 0); } - auto events = json.at("events").get>(); + auto events = choreo::util::ParseObjectArray(json.at("events")); trajectory.events.clear(); for (EventMarker event : events) { - if (event.timestamp >= units::second_t{0} || event.event.size() == 0) { + if (event.timestamp >= wpi::units::second_t{0} || event.event.size() == 0) { trajectory.events.push_back(event); } } diff --git a/choreolib/src/main/native/cpp/choreo/trajectory/struct/DifferentialSampleStruct.cpp b/choreolib/src/main/native/cpp/choreo/trajectory/struct/DifferentialSampleStruct.cpp index c233868360..6a9013ce3d 100644 --- a/choreolib/src/main/native/cpp/choreo/trajectory/struct/DifferentialSampleStruct.cpp +++ b/choreolib/src/main/native/cpp/choreo/trajectory/struct/DifferentialSampleStruct.cpp @@ -2,7 +2,7 @@ #include "choreo/trajectory/struct/DifferentialSampleStruct.hpp" -#include +#include namespace { constexpr size_t kTimestampOff = 0; @@ -19,39 +19,43 @@ constexpr size_t kFlOff = kAlphaOff + 8; constexpr size_t kFrOff = kFlOff + 8; } // namespace -using StructType = wpi::Struct; +using StructType = wpi::util::Struct; choreo::DifferentialSample StructType::Unpack(std::span data) { return choreo::DifferentialSample{ - units::second_t{wpi::UnpackStruct(data)}, - units::meter_t{wpi::UnpackStruct(data)}, - units::meter_t{wpi::UnpackStruct(data)}, - units::radian_t{wpi::UnpackStruct(data)}, - units::meters_per_second_t{wpi::UnpackStruct(data)}, - units::meters_per_second_t{wpi::UnpackStruct(data)}, - units::radians_per_second_t{wpi::UnpackStruct(data)}, - units::meters_per_second_squared_t{ - wpi::UnpackStruct(data)}, - units::meters_per_second_squared_t{ - wpi::UnpackStruct(data)}, - units::radians_per_second_squared_t{ - wpi::UnpackStruct(data)}, - units::newton_t{wpi::UnpackStruct(data)}, - units::newton_t{wpi::UnpackStruct(data)}}; + wpi::units::second_t{ + wpi::util::UnpackStruct(data)}, + wpi::units::meter_t{wpi::util::UnpackStruct(data)}, + wpi::units::meter_t{wpi::util::UnpackStruct(data)}, + wpi::units::radian_t{wpi::util::UnpackStruct(data)}, + wpi::units::meters_per_second_t{ + wpi::util::UnpackStruct(data)}, + wpi::units::meters_per_second_t{ + wpi::util::UnpackStruct(data)}, + wpi::units::radians_per_second_t{ + wpi::util::UnpackStruct(data)}, + wpi::units::meters_per_second_squared_t{ + wpi::util::UnpackStruct(data)}, + wpi::units::meters_per_second_squared_t{ + wpi::util::UnpackStruct(data)}, + wpi::units::radians_per_second_squared_t{ + wpi::util::UnpackStruct(data)}, + wpi::units::newton_t{wpi::util::UnpackStruct(data)}, + wpi::units::newton_t{wpi::util::UnpackStruct(data)}}; } void StructType::Pack(std::span data, const choreo::DifferentialSample& value) { - wpi::PackStruct(data, value.timestamp.value()); - wpi::PackStruct(data, value.x.value()); - wpi::PackStruct(data, value.y.value()); - wpi::PackStruct(data, value.heading.value()); - wpi::PackStruct(data, value.vl.value()); - wpi::PackStruct(data, value.vr.value()); - wpi::PackStruct(data, value.omega.value()); - wpi::PackStruct(data, value.al.value()); - wpi::PackStruct(data, value.ar.value()); - wpi::PackStruct(data, value.alpha.value()); - wpi::PackStruct(data, value.fl.value()); - wpi::PackStruct(data, value.fr.value()); + wpi::util::PackStruct(data, value.timestamp.value()); + wpi::util::PackStruct(data, value.x.value()); + wpi::util::PackStruct(data, value.y.value()); + wpi::util::PackStruct(data, value.heading.value()); + wpi::util::PackStruct(data, value.vl.value()); + wpi::util::PackStruct(data, value.vr.value()); + wpi::util::PackStruct(data, value.omega.value()); + wpi::util::PackStruct(data, value.al.value()); + wpi::util::PackStruct(data, value.ar.value()); + wpi::util::PackStruct(data, value.alpha.value()); + wpi::util::PackStruct(data, value.fl.value()); + wpi::util::PackStruct(data, value.fr.value()); } diff --git a/choreolib/src/main/native/cpp/choreo/trajectory/struct/SwerveSampleStruct.cpp b/choreolib/src/main/native/cpp/choreo/trajectory/struct/SwerveSampleStruct.cpp index 0508753b3b..14ae3e9bfc 100644 --- a/choreolib/src/main/native/cpp/choreo/trajectory/struct/SwerveSampleStruct.cpp +++ b/choreolib/src/main/native/cpp/choreo/trajectory/struct/SwerveSampleStruct.cpp @@ -23,52 +23,56 @@ constexpr size_t kMfY2Off = kMfY1Off + 8; constexpr size_t kMfY3Off = kMfY2Off + 8; } // namespace -using StructType = wpi::Struct; +using StructType = wpi::util::Struct; choreo::SwerveSample StructType::Unpack(std::span data) { return choreo::SwerveSample{ - units::second_t{wpi::UnpackStruct(data)}, - units::meter_t{wpi::UnpackStruct(data)}, - units::meter_t{wpi::UnpackStruct(data)}, - units::radian_t{wpi::UnpackStruct(data)}, - units::meters_per_second_t{wpi::UnpackStruct(data)}, - units::meters_per_second_t{wpi::UnpackStruct(data)}, - units::radians_per_second_t{wpi::UnpackStruct(data)}, - units::meters_per_second_squared_t{ - wpi::UnpackStruct(data)}, - units::meters_per_second_squared_t{ - wpi::UnpackStruct(data)}, - units::radians_per_second_squared_t{ - wpi::UnpackStruct(data)}, - {units::newton_t{wpi::UnpackStruct(data)}, - units::newton_t{wpi::UnpackStruct(data)}, - units::newton_t{wpi::UnpackStruct(data)}, - units::newton_t{wpi::UnpackStruct(data)}}, - {units::newton_t{wpi::UnpackStruct(data)}, - units::newton_t{wpi::UnpackStruct(data)}, - units::newton_t{wpi::UnpackStruct(data)}, - units::newton_t{wpi::UnpackStruct(data)}}, + wpi::units::second_t{ + wpi::util::UnpackStruct(data)}, + wpi::units::meter_t{wpi::util::UnpackStruct(data)}, + wpi::units::meter_t{wpi::util::UnpackStruct(data)}, + wpi::units::radian_t{wpi::util::UnpackStruct(data)}, + wpi::units::meters_per_second_t{ + wpi::util::UnpackStruct(data)}, + wpi::units::meters_per_second_t{ + wpi::util::UnpackStruct(data)}, + wpi::units::radians_per_second_t{ + wpi::util::UnpackStruct(data)}, + wpi::units::meters_per_second_squared_t{ + wpi::util::UnpackStruct(data)}, + wpi::units::meters_per_second_squared_t{ + wpi::util::UnpackStruct(data)}, + wpi::units::radians_per_second_squared_t{ + wpi::util::UnpackStruct(data)}, + {wpi::units::newton_t{wpi::util::UnpackStruct(data)}, + wpi::units::newton_t{wpi::util::UnpackStruct(data)}, + wpi::units::newton_t{wpi::util::UnpackStruct(data)}, + wpi::units::newton_t{wpi::util::UnpackStruct(data)}}, + {wpi::units::newton_t{wpi::util::UnpackStruct(data)}, + wpi::units::newton_t{wpi::util::UnpackStruct(data)}, + wpi::units::newton_t{wpi::util::UnpackStruct(data)}, + wpi::units::newton_t{wpi::util::UnpackStruct(data)}}, }; } void StructType::Pack(std::span data, const choreo::SwerveSample& value) { - wpi::PackStruct(data, value.timestamp.value()); - wpi::PackStruct(data, value.x.value()); - wpi::PackStruct(data, value.y.value()); - wpi::PackStruct(data, value.heading.value()); - wpi::PackStruct(data, value.vx.value()); - wpi::PackStruct(data, value.vy.value()); - wpi::PackStruct(data, value.omega.value()); - wpi::PackStruct(data, value.ax.value()); - wpi::PackStruct(data, value.ay.value()); - wpi::PackStruct(data, value.alpha.value()); - wpi::PackStruct(data, value.moduleForcesX[0].value()); - wpi::PackStruct(data, value.moduleForcesX[1].value()); - wpi::PackStruct(data, value.moduleForcesX[2].value()); - wpi::PackStruct(data, value.moduleForcesX[3].value()); - wpi::PackStruct(data, value.moduleForcesY[0].value()); - wpi::PackStruct(data, value.moduleForcesY[1].value()); - wpi::PackStruct(data, value.moduleForcesY[2].value()); - wpi::PackStruct(data, value.moduleForcesY[3].value()); + wpi::util::PackStruct(data, value.timestamp.value()); + wpi::util::PackStruct(data, value.x.value()); + wpi::util::PackStruct(data, value.y.value()); + wpi::util::PackStruct(data, value.heading.value()); + wpi::util::PackStruct(data, value.vx.value()); + wpi::util::PackStruct(data, value.vy.value()); + wpi::util::PackStruct(data, value.omega.value()); + wpi::util::PackStruct(data, value.ax.value()); + wpi::util::PackStruct(data, value.ay.value()); + wpi::util::PackStruct(data, value.alpha.value()); + wpi::util::PackStruct(data, value.moduleForcesX[0].value()); + wpi::util::PackStruct(data, value.moduleForcesX[1].value()); + wpi::util::PackStruct(data, value.moduleForcesX[2].value()); + wpi::util::PackStruct(data, value.moduleForcesX[3].value()); + wpi::util::PackStruct(data, value.moduleForcesY[0].value()); + wpi::util::PackStruct(data, value.moduleForcesY[1].value()); + wpi::util::PackStruct(data, value.moduleForcesY[2].value()); + wpi::util::PackStruct(data, value.moduleForcesY[3].value()); } diff --git a/choreolib/src/main/native/include/choreo/Choreo.hpp b/choreolib/src/main/native/include/choreo/Choreo.hpp index 77e4da96ab..b7f15add86 100644 --- a/choreolib/src/main/native/include/choreo/Choreo.hpp +++ b/choreolib/src/main/native/include/choreo/Choreo.hpp @@ -9,12 +9,11 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include "choreo/trajectory/DifferentialSample.hpp" #include "choreo/trajectory/SwerveSample.hpp" @@ -46,25 +45,17 @@ class Choreo { std::string trajectoryFileName = fmt::format( "{}/{}{}", CHOREO_DIR, trajectoryName, TRAJECTORY_FILE_EXTENSION); - auto fileBuffer = wpi::MemoryBuffer::GetFile(trajectoryFileName); + auto fileBuffer = wpi::util::MemoryBuffer::GetFile(trajectoryFileName); if (!fileBuffer) { - FRC_ReportError(frc::warn::Warning, "Could not find trajectory file: {}", - trajectoryName); + WPILIB_ReportWarning("Could not find trajectory file: {}", + trajectoryName); return {}; } - try { - return LoadTrajectoryString( - std::string{fileBuffer.value()->GetCharBuffer().data(), - fileBuffer.value()->size()}, - trajectoryName); - } catch (wpi::json::parse_error& ex) { - FRC_ReportError(frc::warn::Warning, "Could not parse trajectory file: {}", - trajectoryName); - FRC_ReportError(frc::warn::Warning, "{}", ex.what()); - return {}; - } - return {}; + return LoadTrajectoryString( + std::string{fileBuffer.value()->GetCharBuffer().data(), + fileBuffer.value()->size()}, + trajectoryName); } /// Load a trajectory from a string. @@ -79,19 +70,24 @@ class Choreo { static std::optional> LoadTrajectoryString( std::string_view trajectoryJsonString, std::string_view trajectoryName) { if constexpr (std::same_as) { - HAL_Report(HALUsageReporting::kResourceType_ChoreoTrajectory, 1); + HAL_ReportUsage("ChoreoLib/SwerveTrajectory", 1, ""); } else if constexpr (std::same_as) { - HAL_Report(HALUsageReporting::kResourceType_ChoreoTrajectory, 2); + HAL_ReportUsage("ChoreoLib/DifferentialTrajectory", 2, ""); } - wpi::json json = wpi::json::parse(trajectoryJsonString); - uint32_t version = json["version"]; + auto json = wpi::util::json::parse(trajectoryJsonString); + if (!json) { + WPILIB_ReportWarning("Could not parse trajectory file: {}", + trajectoryName); + return {}; + } + uint32_t version = json->at("version").get_int(); if (version != kTrajSchemaVersion) { throw fmt::format("{}.traj: Wrong version {}. Expected {}", trajectoryName, version, kTrajSchemaVersion); } Trajectory trajectory; - from_json(json, trajectory); + from_json(*json, trajectory); return trajectory; } @@ -170,7 +166,7 @@ class Choreo { static constexpr std::string_view TRAJECTORY_FILE_EXTENSION = ".traj"; static inline const std::string CHOREO_DIR = - frc::filesystem::GetDeployDirectory() + "/choreo"; + wpi::filesystem::GetDeployDirectory() + "/choreo"; Choreo(); }; diff --git a/choreolib/src/main/native/include/choreo/trajectory/DifferentialSample.hpp b/choreolib/src/main/native/include/choreo/trajectory/DifferentialSample.hpp index 26901ee50d..614fb3504a 100644 --- a/choreolib/src/main/native/include/choreo/trajectory/DifferentialSample.hpp +++ b/choreolib/src/main/native/include/choreo/trajectory/DifferentialSample.hpp @@ -6,19 +6,19 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include "choreo/util/AllianceFlipperUtil.hpp" @@ -46,15 +46,16 @@ class DifferentialSample { /// @param alpha The chassis angular acceleration /// @param fl The force of the left wheels /// @param fr The force of the right wheels - constexpr DifferentialSample(units::second_t timestamp, units::meter_t x, - units::meter_t y, units::radian_t heading, - units::meters_per_second_t vl, - units::meters_per_second_t vr, - units::radians_per_second_t omega, - units::meters_per_second_squared_t al, - units::meters_per_second_squared_t ar, - units::radians_per_second_squared_t alpha, - units::newton_t fl, units::newton_t fr) + constexpr DifferentialSample(wpi::units::second_t timestamp, + wpi::units::meter_t x, wpi::units::meter_t y, + wpi::units::radian_t heading, + wpi::units::meters_per_second_t vl, + wpi::units::meters_per_second_t vr, + wpi::units::radians_per_second_t omega, + wpi::units::meters_per_second_squared_t al, + wpi::units::meters_per_second_squared_t ar, + wpi::units::radians_per_second_squared_t alpha, + wpi::units::newton_t fl, wpi::units::newton_t fr) : timestamp{timestamp}, x{x}, y{y}, @@ -71,27 +72,28 @@ class DifferentialSample { /// Gets the timestamp of the DifferentialSample. /// /// @return The timestamp. - units::second_t GetTimestamp() const { return timestamp; } + wpi::units::second_t GetTimestamp() const { return timestamp; } /// Gets the Pose2d of the DifferentialSample. /// /// @return The pose. - constexpr frc::Pose2d GetPose() const { - return frc::Pose2d{x, y, frc::Rotation2d{heading}}; + constexpr wpi::math::Pose2d GetPose() const { + return wpi::math::Pose2d{x, y, wpi::math::Rotation2d{heading}}; } /// Gets the field-relative chassis speeds of the DifferentialSample. /// /// @return The field-relative chassis speeds. - constexpr frc::ChassisSpeeds GetChassisSpeeds() const { - return frc::ChassisSpeeds{(vl + vr) / 2.0, 0_mps, omega}; + constexpr wpi::math::ChassisVelocities GetChassisVelocities() const { + return wpi::math::ChassisVelocities{(vl + vr) / 2.0, 0_mps, omega}; } /// Returns the current sample offset by a the time offset passed in. /// /// @param timeStampOffset time to move sample by /// @return DifferentialSample that is moved forward by the offset - constexpr DifferentialSample OffsetBy(units::second_t timeStampOffset) const { + constexpr DifferentialSample OffsetBy( + wpi::units::second_t timeStampOffset) const { return DifferentialSample{timestamp + timeStampOffset, x, y, @@ -112,8 +114,9 @@ class DifferentialSample { /// @param t time to move sample by /// @return the interpolated sample DifferentialSample Interpolate(const DifferentialSample& endValue, - units::second_t t) const { - units::scalar_t scale = (t - timestamp) / (endValue.timestamp - timestamp); + wpi::units::second_t t) const { + wpi::units::scalar_t scale = + (t - timestamp) / (endValue.timestamp - timestamp); // Integrate the acceleration to get the rest of the state, since linearly // interpolating the state gives an inaccurate result if the accelerations @@ -149,24 +152,24 @@ class DifferentialSample { v * std::cos(θ), v * std::sin(θ), ω, al, ar, α}; }; - units::second_t τ = t - timestamp; - auto sample = frc::RKDP( + wpi::units::second_t τ = t - timestamp; + auto sample = wpi::math::RKDP( f, initialState, Eigen::Vector(al.value(), ar.value(), alpha.value()), τ); return DifferentialSample{ - wpi::Lerp(timestamp, endValue.timestamp, scale), - units::meter_t{sample(0, 0)}, - units::meter_t{sample(1, 0)}, - units::radian_t{sample(2, 0)}, - units::meters_per_second_t{sample(3, 0)}, - units::meters_per_second_t{sample(4, 0)}, - units::radians_per_second_t{sample(5, 0)}, + wpi::util::Lerp(timestamp, endValue.timestamp, scale), + wpi::units::meter_t{sample(0, 0)}, + wpi::units::meter_t{sample(1, 0)}, + wpi::units::radian_t{sample(2, 0)}, + wpi::units::meters_per_second_t{sample(3, 0)}, + wpi::units::meters_per_second_t{sample(4, 0)}, + wpi::units::radians_per_second_t{sample(5, 0)}, al, ar, alpha, - wpi::Lerp(fl, endValue.fl, scale), - wpi::Lerp(fr, endValue.fr, scale), + wpi::util::Lerp(fl, endValue.fl, scale), + wpi::util::Lerp(fr, endValue.fr, scale), }; } @@ -231,7 +234,7 @@ class DifferentialSample { auto compare_units = [epsilon](const auto& a, const auto& b) { using UnitType = std::remove_const_t>; - return units::math::abs(a - b) < UnitType(epsilon); + return wpi::units::math::abs(a - b) < UnitType(epsilon); }; return compare_units(timestamp, other.timestamp) && @@ -244,44 +247,45 @@ class DifferentialSample { } /// The timestamp of this sample relative to the beginning of the trajectory. - units::second_t timestamp = 0_s; + wpi::units::second_t timestamp = 0_s; /// The X position of the sample relative to the blue alliance wall origin. - units::meter_t x = 0_m; + wpi::units::meter_t x = 0_m; /// The Y position of the sample relative to the blue alliance wall origin. - units::meter_t y = 0_m; + wpi::units::meter_t y = 0_m; /// The heading of the sample, with 0 being in the +X direction. - units::radian_t heading = 0_rad; + wpi::units::radian_t heading = 0_rad; /// The velocity of the left wheels. - units::meters_per_second_t vl = 0_mps; + wpi::units::meters_per_second_t vl = 0_mps; /// The velocity of the right wheels. - units::meters_per_second_t vr = 0_mps; + wpi::units::meters_per_second_t vr = 0_mps; /// The chassis angular velocity. - units::radians_per_second_t omega = 0_rad_per_s; + wpi::units::radians_per_second_t omega = 0_rad_per_s; /// The acceleration of the left wheels. - units::meters_per_second_squared_t al = 0_mps_sq; + wpi::units::meters_per_second_squared_t al = 0_mps_sq; /// The acceleration of the right wheels. - units::meters_per_second_squared_t ar = 0_mps_sq; + wpi::units::meters_per_second_squared_t ar = 0_mps_sq; /// The chassis angular acceleration. - units::radians_per_second_squared_t alpha = 0_rad_per_s_sq; + wpi::units::radians_per_second_squared_t alpha = 0_rad_per_s_sq; /// The force of the left wheels. - units::newton_t fl = 0_N; + wpi::units::newton_t fl = 0_N; /// The force of the right wheels. - units::newton_t fr = 0_N; + wpi::units::newton_t fr = 0_N; }; -void to_json(wpi::json& json, const DifferentialSample& trajectorySample); -void from_json(const wpi::json& json, DifferentialSample& trajectorySample); +void to_json(wpi::util::json& json, const DifferentialSample& trajectorySample); +void from_json(const wpi::util::json& json, + DifferentialSample& trajectorySample); } // namespace choreo diff --git a/choreolib/src/main/native/include/choreo/trajectory/EventMarker.hpp b/choreolib/src/main/native/include/choreo/trajectory/EventMarker.hpp index 7157eaa5e4..7d53c2a7e1 100644 --- a/choreolib/src/main/native/include/choreo/trajectory/EventMarker.hpp +++ b/choreolib/src/main/native/include/choreo/trajectory/EventMarker.hpp @@ -4,15 +4,15 @@ #include -#include -#include +#include +#include namespace choreo { /// A marker for an event in a trajectory. struct EventMarker { /// The timestamp of the event. - units::second_t timestamp; + wpi::units::second_t timestamp; /// The event. std::string event; @@ -23,7 +23,7 @@ struct EventMarker { /// @param timestampOffset The amount to offset the timestamp by. /// @return A new EventMarker with the timestamp offset by the specified /// amount. - EventMarker OffsetBy(units::second_t timestampOffset) const { + EventMarker OffsetBy(wpi::units::second_t timestampOffset) const { return EventMarker{timestamp + timestampOffset, event}; } @@ -33,7 +33,7 @@ struct EventMarker { bool operator==(const EventMarker&) const = default; }; -void to_json(wpi::json& json, const EventMarker& event); -void from_json(const wpi::json& json, EventMarker& event); +void to_json(wpi::util::json& json, const EventMarker& event); +void from_json(const wpi::util::json& json, EventMarker& event); } // namespace choreo diff --git a/choreolib/src/main/native/include/choreo/trajectory/SwerveSample.hpp b/choreolib/src/main/native/include/choreo/trajectory/SwerveSample.hpp index e20fb1a347..c6c3b36b47 100644 --- a/choreolib/src/main/native/include/choreo/trajectory/SwerveSample.hpp +++ b/choreolib/src/main/native/include/choreo/trajectory/SwerveSample.hpp @@ -6,17 +6,17 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include "choreo/util/AllianceFlipperUtil.hpp" @@ -46,16 +46,16 @@ class SwerveSample { /// Module forces appear in the following order: [FL, FR, BL, BR]. /// @param moduleForcesY The force on each swerve module in the Y direction. /// Module forces appear in the following order: [FL, FR, BL, BR]. - constexpr SwerveSample(units::second_t timestamp, units::meter_t x, - units::meter_t y, units::radian_t heading, - units::meters_per_second_t vx, - units::meters_per_second_t vy, - units::radians_per_second_t omega, - units::meters_per_second_squared_t ax, - units::meters_per_second_squared_t ay, - units::radians_per_second_squared_t alpha, - std::array moduleForcesX, - std::array moduleForcesY) + constexpr SwerveSample(wpi::units::second_t timestamp, wpi::units::meter_t x, + wpi::units::meter_t y, wpi::units::radian_t heading, + wpi::units::meters_per_second_t vx, + wpi::units::meters_per_second_t vy, + wpi::units::radians_per_second_t omega, + wpi::units::meters_per_second_squared_t ax, + wpi::units::meters_per_second_squared_t ay, + wpi::units::radians_per_second_squared_t alpha, + std::array moduleForcesX, + std::array moduleForcesY) : timestamp{timestamp}, x{x}, y{y}, @@ -72,20 +72,20 @@ class SwerveSample { /// Gets the timestamp of the SwerveSample. /// /// @return The timestamp. - constexpr units::second_t GetTimestamp() const { return timestamp; } + constexpr wpi::units::second_t GetTimestamp() const { return timestamp; } /// Gets the Pose2d of the SwerveSample. /// /// @return The pose. - constexpr frc::Pose2d GetPose() const { - return frc::Pose2d{x, y, frc::Rotation2d{heading}}; + constexpr wpi::math::Pose2d GetPose() const { + return wpi::math::Pose2d{x, y, wpi::math::Rotation2d{heading}}; } /// Gets the field-relative chassis speeds of the SwerveSample. /// /// @return The field-relative chassis speeds. - constexpr frc::ChassisSpeeds GetChassisSpeeds() const { - return frc::ChassisSpeeds{vx, vy, omega}; + constexpr wpi::math::ChassisVelocities GetChassisVelocities() const { + return wpi::math::ChassisVelocities{vx, vy, omega}; } /// Returns the current sample flipped based on the field year. @@ -178,7 +178,7 @@ class SwerveSample { /// /// @param timeStampOffset time to move sample by /// @return SwerveSample that is moved forward by the offset - constexpr SwerveSample OffsetBy(units::second_t timeStampOffset) const { + constexpr SwerveSample OffsetBy(wpi::units::second_t timeStampOffset) const { return SwerveSample{timestamp + timeStampOffset, x, y, @@ -199,16 +199,17 @@ class SwerveSample { /// @param t time to move sample by /// @return the interpolated sample constexpr SwerveSample Interpolate(const SwerveSample& endValue, - units::second_t t) const { - units::scalar_t scale = (t - timestamp) / (endValue.timestamp - timestamp); + wpi::units::second_t t) const { + wpi::units::scalar_t scale = + (t - timestamp) / (endValue.timestamp - timestamp); - std::array interpolatedForcesX; - std::array interpolatedForcesY; + std::array interpolatedForcesX; + std::array interpolatedForcesY; for (int i = 0; i < 4; i++) { - interpolatedForcesX[i] = - wpi::Lerp(moduleForcesX[i], endValue.moduleForcesX[i], scale.value()); - interpolatedForcesY[i] = - wpi::Lerp(moduleForcesY[i], endValue.moduleForcesY[i], scale.value()); + interpolatedForcesX[i] = wpi::util::Lerp( + moduleForcesX[i], endValue.moduleForcesX[i], scale.value()); + interpolatedForcesY[i] = wpi::util::Lerp( + moduleForcesY[i], endValue.moduleForcesY[i], scale.value()); } // Integrate the acceleration to get the rest of the state, since linearly @@ -221,7 +222,7 @@ class SwerveSample { // v(τ) = vₖ + aₖτ auto τ = t - timestamp; auto τ2 = τ * τ; - return SwerveSample{wpi::Lerp(timestamp, endValue.timestamp, scale), + return SwerveSample{wpi::util::Lerp(timestamp, endValue.timestamp, scale), x + vx * τ + 0.5 * ax * τ2, y + vy * τ + 0.5 * ay * τ2, heading + omega * τ + 0.5 * alpha * τ2, @@ -245,7 +246,7 @@ class SwerveSample { auto compare_units = [epsilon](const auto& a, const auto& b) { using UnitType = std::remove_const_t>; - return units::math::abs(a - b) < UnitType(epsilon); + return wpi::units::math::abs(a - b) < UnitType(epsilon); }; auto compare_arrays = [&compare_units](const auto& arr1, const auto& arr2) { @@ -263,46 +264,46 @@ class SwerveSample { } /// The timestamp of this sample relative to the beginning of the trajectory. - units::second_t timestamp = 0_s; + wpi::units::second_t timestamp = 0_s; /// The X position of the sample relative to the blue alliance wall origin. - units::meter_t x = 0_m; + wpi::units::meter_t x = 0_m; /// The Y position of the sample relative to the blue alliance wall origin. - units::meter_t y = 0_m; + wpi::units::meter_t y = 0_m; /// The heading of the sample, with 0 being in the +X direction. - units::radian_t heading = 0_rad; + wpi::units::radian_t heading = 0_rad; /// The velocity of the sample in the X direction. - units::meters_per_second_t vx = 0_mps; + wpi::units::meters_per_second_t vx = 0_mps; /// The velocity of the sample in the Y direction. - units::meters_per_second_t vy = 0_mps; + wpi::units::meters_per_second_t vy = 0_mps; /// The angular velocity of the sample. - units::radians_per_second_t omega = 0_rad_per_s; + wpi::units::radians_per_second_t omega = 0_rad_per_s; /// The acceleration of the in the X direction. - units::meters_per_second_squared_t ax = 0_mps_sq; + wpi::units::meters_per_second_squared_t ax = 0_mps_sq; /// The acceleration of the in the Y direction. - units::meters_per_second_squared_t ay = 0_mps_sq; + wpi::units::meters_per_second_squared_t ay = 0_mps_sq; /// The angular acceleration of the sample. - units::radians_per_second_squared_t alpha = 0_rad_per_s_sq; + wpi::units::radians_per_second_squared_t alpha = 0_rad_per_s_sq; /// The force on each swerve module in the X direction. Module forces appear /// in the following order: [FL, FR, BL, BR]. - std::array moduleForcesX{0_N, 0_N, 0_N, 0_N}; + std::array moduleForcesX{0_N, 0_N, 0_N, 0_N}; /// The force on each swerve module in the Y direction. Module forces appear /// in the following order: [FL, FR, BL, BR]. - std::array moduleForcesY{0_N, 0_N, 0_N, 0_N}; + std::array moduleForcesY{0_N, 0_N, 0_N, 0_N}; }; -void to_json(wpi::json& json, const SwerveSample& trajectorySample); -void from_json(const wpi::json& json, SwerveSample& trajectorySample); +void to_json(wpi::util::json& json, const SwerveSample& trajectorySample); +void from_json(const wpi::util::json& json, SwerveSample& trajectorySample); } // namespace choreo diff --git a/choreolib/src/main/native/include/choreo/trajectory/Trajectory.hpp b/choreolib/src/main/native/include/choreo/trajectory/Trajectory.hpp index 9db9d98c34..67ca4dfb1b 100644 --- a/choreolib/src/main/native/include/choreo/trajectory/Trajectory.hpp +++ b/choreolib/src/main/native/include/choreo/trajectory/Trajectory.hpp @@ -10,8 +10,8 @@ #include #include -#include -#include +#include +#include #include "choreo/trajectory/DifferentialSample.hpp" #include "choreo/trajectory/EventMarker.hpp" @@ -122,7 +122,7 @@ class Trajectory { /// @param mirrorForRedAlliance whether or not to return the sample mirrored. /// @return The SampleType at the given time. template - std::optional SampleAt(units::second_t timestamp, + std::optional SampleAt(wpi::units::second_t timestamp, bool mirrorForRedAlliance = false) const { if (auto state = SampleInternal(timestamp)) { return mirrorForRedAlliance ? state.value().template Flipped() @@ -140,7 +140,7 @@ class Trajectory { /// @param mirrorForRedAlliance whether or not to return the Pose mirrored. /// @return The first Pose in the trajectory. template - std::optional GetInitialPose( + std::optional GetInitialPose( bool mirrorForRedAlliance = false) const { if (samples.size() == 0) { return {}; @@ -160,7 +160,7 @@ class Trajectory { /// @param mirrorForRedAlliance whether or not to return the Pose mirrored. /// @return The last Pose in the trajectory. template - std::optional GetFinalPose( + std::optional GetFinalPose( bool mirrorForRedAlliance = false) const { if (samples.size() == 0) { return {}; @@ -176,7 +176,7 @@ class Trajectory { /// /// @return The total time the trajectory will take to follow, if empty will /// return 0 seconds. - units::second_t GetTotalTime() const { + wpi::units::second_t GetTotalTime() const { if (samples.size() == 0) { return 0_s; } @@ -186,8 +186,8 @@ class Trajectory { /// Returns the vector of poses corresponding to the trajectory. /// /// @return the vector of poses corresponding to the trajectory. - std::vector GetPoses() const { - std::vector poses; + std::vector GetPoses() const { + std::vector poses; for (const auto& sample : samples) { poses.push_back(sample.GetPose()); } @@ -248,8 +248,8 @@ class Trajectory { name + "[" + std::to_string(splitIndex) + "]", {}, {}, {}}; } // Now we know sublist.size() >= 1 - units::second_t startTime = sublist.front().GetTimestamp(); - units::second_t endTime = sublist.back().GetTimestamp(); + wpi::units::second_t startTime = sublist.front().GetTimestamp(); + wpi::units::second_t endTime = sublist.back().GetTimestamp(); auto offsetSamples = sublist | std::views::transform([startTime](const SampleType& s) { @@ -313,7 +313,8 @@ class Trajectory { std::vector events; private: - std::optional SampleInternal(units::second_t timestamp) const { + std::optional SampleInternal( + wpi::units::second_t timestamp) const { if (samples.size() == 0) { return {}; } @@ -354,11 +355,13 @@ class Trajectory { } }; -void to_json(wpi::json& json, const Trajectory& trajectory); -void from_json(const wpi::json& json, Trajectory& trajectory); +void to_json(wpi::util::json& json, const Trajectory& trajectory); +void from_json(const wpi::util::json& json, + Trajectory& trajectory); -void to_json(wpi::json& json, const Trajectory& trajectory); -void from_json(const wpi::json& json, +void to_json(wpi::util::json& json, + const Trajectory& trajectory); +void from_json(const wpi::util::json& json, Trajectory& trajectory); } // namespace choreo diff --git a/choreolib/src/main/native/include/choreo/trajectory/TrajectorySample.hpp b/choreolib/src/main/native/include/choreo/trajectory/TrajectorySample.hpp index 6bb81c8028..a7eb68b9bc 100644 --- a/choreolib/src/main/native/include/choreo/trajectory/TrajectorySample.hpp +++ b/choreolib/src/main/native/include/choreo/trajectory/TrajectorySample.hpp @@ -4,9 +4,9 @@ #include -#include -#include -#include +#include +#include +#include namespace choreo { @@ -21,10 +21,12 @@ concept EqualityComparable = requires(const T& a, const T& b) { template concept TrajectorySample = EqualityComparable && - requires(T t, units::second_t time, T tother, int year) { - { t.GetTimestamp() } -> std::same_as; - { t.GetPose() } -> std::same_as; - { t.GetChassisSpeeds() } -> std::same_as; + requires(T t, wpi::units::second_t time, T tother, int year) { + { t.GetTimestamp() } -> std::same_as; + { t.GetPose() } -> std::same_as; + { + t.GetChassisVelocities() + } -> std::same_as; { t.OffsetBy(time) } -> std::same_as; { t.Interpolate(tother, time) } -> std::same_as; // FIXME: This works around a roboRIO GCC internal compiler error; it diff --git a/choreolib/src/main/native/include/choreo/trajectory/struct/DifferentialSampleStruct.hpp b/choreolib/src/main/native/include/choreo/trajectory/struct/DifferentialSampleStruct.hpp index e7cc966355..910a6174a6 100644 --- a/choreolib/src/main/native/include/choreo/trajectory/struct/DifferentialSampleStruct.hpp +++ b/choreolib/src/main/native/include/choreo/trajectory/struct/DifferentialSampleStruct.hpp @@ -2,13 +2,13 @@ #pragma once -#include -#include +#include +#include #include "choreo/trajectory/DifferentialSample.hpp" template <> -struct wpi::Struct { +struct wpi::util::Struct { static constexpr std::string_view GetTypeName() { return "DifferentialSample"; } @@ -24,4 +24,4 @@ struct wpi::Struct { const choreo::DifferentialSample& value); }; -static_assert(wpi::StructSerializable); +static_assert(wpi::util::StructSerializable); diff --git a/choreolib/src/main/native/include/choreo/trajectory/struct/SwerveSampleStruct.hpp b/choreolib/src/main/native/include/choreo/trajectory/struct/SwerveSampleStruct.hpp index 1c87c8a169..7d7491bc6e 100644 --- a/choreolib/src/main/native/include/choreo/trajectory/struct/SwerveSampleStruct.hpp +++ b/choreolib/src/main/native/include/choreo/trajectory/struct/SwerveSampleStruct.hpp @@ -2,13 +2,13 @@ #pragma once -#include -#include +#include +#include #include "choreo/trajectory/SwerveSample.hpp" template <> -struct wpi::Struct { +struct wpi::util::Struct { static constexpr std::string_view GetTypeName() { return "SwerveSample"; } static constexpr size_t GetSize() { return 144; } static constexpr std::string_view GetSchema() { @@ -21,4 +21,4 @@ struct wpi::Struct { static void Pack(std::span data, const choreo::SwerveSample& value); }; -static_assert(wpi::StructSerializable); +static_assert(wpi::util::StructSerializable); diff --git a/choreolib/src/main/native/include/choreo/util/AllianceFlipperUtil.hpp b/choreolib/src/main/native/include/choreo/util/AllianceFlipperUtil.hpp index 6a3cb89290..08af0b36e0 100644 --- a/choreolib/src/main/native/include/choreo/util/AllianceFlipperUtil.hpp +++ b/choreolib/src/main/native/include/choreo/util/AllianceFlipperUtil.hpp @@ -6,8 +6,8 @@ #include #include -#include -#include +#include +#include #include "choreo/util/FieldDimensions.hpp" #include "choreo/util/Map.hpp" @@ -16,7 +16,7 @@ namespace choreo::util { enum class FlipperType { Mirrored, RotateAround }; -/// X becomes fieldLength - x, leaves the y coordinate unchanged, and heading +/// X becomes -x, leaves the y coordinate unchanged, and heading /// becomes pi - heading. struct MirroredFlipper { /// Whether pose should be mirrored. @@ -26,26 +26,29 @@ struct MirroredFlipper { /// /// @param x The X coordinate to flip. /// @return The flipped X coordinate. - static constexpr units::meter_t FlipX(units::meter_t x) { - return fieldLength - x; + static constexpr wpi::units::meter_t FlipX(wpi::units::meter_t x) { + return -x; } /// Flips the Y coordinate. /// /// @param y The Y coordinate to flip. /// @return The flipped Y coordinate. - static constexpr units::meter_t FlipY(units::meter_t y) { return y; } + static constexpr wpi::units::meter_t FlipY(wpi::units::meter_t y) { + return y; + } /// Flips the heading. /// /// @param heading The heading to flip. /// @return The flipped heading. - static constexpr units::radian_t FlipHeading(units::radian_t heading) { - return units::radian_t{std::numbers::pi} - heading; + static constexpr wpi::units::radian_t FlipHeading( + wpi::units::radian_t heading) { + return wpi::units::radian_t{std::numbers::pi} - heading; } }; -/// X is unchanged, y becomes fieldWidth - y, and heading +/// X is unchanged, y becomes -y, and heading /// becomes -heading. struct MirroredYFlipper { /// Whether pose should be mirrored. @@ -55,26 +58,29 @@ struct MirroredYFlipper { /// /// @param x The X coordinate to flip. /// @return The flipped X coordinate. - static constexpr units::meter_t FlipX(units::meter_t x) { return x; } + static constexpr wpi::units::meter_t FlipX(wpi::units::meter_t x) { + return x; + } /// Flips the Y coordinate. /// /// @param y The Y coordinate to flip. /// @return The flipped Y coordinate. - static constexpr units::meter_t FlipY(units::meter_t y) { - return fieldWidth - y; + static constexpr wpi::units::meter_t FlipY(wpi::units::meter_t y) { + return -y; } /// Flips the heading. /// /// @param heading The heading to flip. /// @return The flipped heading. - static constexpr units::radian_t FlipHeading(units::radian_t heading) { + static constexpr wpi::units::radian_t FlipHeading( + wpi::units::radian_t heading) { return -heading; } }; -/// X becomes fieldLength - x, Y becomes fieldWidth - y, and heading becomes +/// X becomes -x, Y becomes -y, and heading becomes /// pi - heading. struct RotateAroundFlipper { /// Whether pose should be mirrored. @@ -84,24 +90,25 @@ struct RotateAroundFlipper { /// /// @param x The X coordinate to flip. /// @return The flipped X coordinate. - static constexpr units::meter_t FlipX(units::meter_t x) { - return fieldLength - x; + static constexpr wpi::units::meter_t FlipX(wpi::units::meter_t x) { + return -x; } /// Flips the Y coordinate. /// /// @param y The Y coordinate to flip. /// @return The flipped Y coordinate. - static constexpr units::meter_t FlipY(units::meter_t y) { - return fieldWidth - y; + static constexpr wpi::units::meter_t FlipY(wpi::units::meter_t y) { + return -y; } /// Flips the heading. /// /// @param heading The heading to flip. /// @return The flipped heading. - static constexpr units::radian_t FlipHeading(units::radian_t heading) { - return units::radian_t{std::numbers::pi} + heading; + static constexpr wpi::units::radian_t FlipHeading( + wpi::units::radian_t heading) { + return wpi::units::radian_t{std::numbers::pi} + heading; } }; diff --git a/choreolib/src/main/native/include/choreo/util/ArrayUtil.hpp b/choreolib/src/main/native/include/choreo/util/ArrayUtil.hpp new file mode 100644 index 0000000000..4a1b95ab03 --- /dev/null +++ b/choreolib/src/main/native/include/choreo/util/ArrayUtil.hpp @@ -0,0 +1,36 @@ +// Copyright (c) Choreo contributors + +#pragma once + +#include +#include + +#include + +namespace choreo::util { +template +std::vector ParseObjectArray(const wpi::util::json& json) { + std::vector result; + for (auto&& element : json.get_array()) { + result.push_back(element.get()); + } + return result; +} + +template +std::vector ParseObjectArray(const wpi::util::json& json) { + std::vector result; + for (auto&& element : json.get_array()) { + result.push_back(element.get()); + } + return result; +} + +std::vector ParseIntArray(const wpi::util::json& json) { + std::vector result; + for (auto&& element : json.get_array()) { + result.push_back(element.get_int()); + } + return result; +} +} // namespace choreo::util diff --git a/choreolib/src/main/native/include/choreo/util/FieldDimensions.hpp b/choreolib/src/main/native/include/choreo/util/FieldDimensions.hpp index abe2c81bb3..e7e792ae1e 100644 --- a/choreolib/src/main/native/include/choreo/util/FieldDimensions.hpp +++ b/choreolib/src/main/native/include/choreo/util/FieldDimensions.hpp @@ -4,9 +4,9 @@ #pragma once -#include +#include namespace choreo::util { -static constexpr units::meter_t fieldLength = 16.541_m; -static constexpr units::meter_t fieldWidth = 8.0692_m; +static constexpr wpi::units::meter_t fieldLength = 16.541_m; +static constexpr wpi::units::meter_t fieldWidth = 8.0692_m; } // namespace choreo::util diff --git a/choreolib/src/test/java/choreo/auto/AutoChooserTest.java b/choreolib/src/test/java/choreo/auto/AutoChooserTest.java deleted file mode 100644 index 2c87a4bac2..0000000000 --- a/choreolib/src/test/java/choreo/auto/AutoChooserTest.java +++ /dev/null @@ -1,159 +0,0 @@ -// Copyright (c) Choreo contributors - -package choreo.auto; - -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertTrue; - -import edu.wpi.first.hal.AllianceStationID; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.simulation.DriverStationSim; -import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl; -import edu.wpi.first.wpilibj2.command.Commands; -import java.util.Set; -import org.junit.jupiter.api.AfterEach; -import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Test; - -public class AutoChooserTest { - private static final String NOT_FOUND = "__NOT_FOUND__"; - - private SendableBuilderImpl builder; - private NetworkTableInstance ntInstance; - - @BeforeEach - public void setup() { - assert HAL.initialize(500, 0); - ntInstance = NetworkTableInstance.create(); - builder = new SendableBuilderImpl(); - builder.startListeners(); - - ntInstance.stopClient(); - ntInstance.stopServer(); - ntInstance.startLocal(); - } - - @AfterEach - public void tearDown() { - ntInstance.close(); - builder.close(); - builder.clearProperties(); - builder.stopListeners(); - } - - private String chooserPath(String testFuncName) { - return "/Test/AutoChooser/" + testFuncName; - } - - private NetworkTable table(String testFuncName) { - // i'm unsure if this is needed but it won't hurt - ntInstance.flushLocal(); - return ntInstance.getTable(chooserPath(testFuncName) + "/AutoChooser"); - } - - private void assertNTType(String testFuncName) { - String type = table(testFuncName).getEntry(".type").getString(NOT_FOUND); - assertEquals("String Chooser", type); - } - - private void assertNTSelected(String testFuncName, String expected) { - String type = table(testFuncName).getEntry("selected").getString(NOT_FOUND); - assertEquals(expected, type); - } - - private void assertNTActive(String testFuncName, String expected) { - String type = table(testFuncName).getEntry("active").getString(NOT_FOUND); - assertEquals(expected, type); - } - - private void assertNTDefault(String testFuncName, String expectedDefault) { - String type = table(testFuncName).getEntry("default").getString(NOT_FOUND); - assertEquals(expectedDefault, type); - } - - private void assertNTOptions(String testFuncName, String... expected) { - Set options = - Set.of(table(testFuncName).getEntry("options").getStringArray(new String[0])); - - assertEquals(expected.length, options.size()); - for (int i = 0; i < expected.length; i++) { - assertTrue(options.contains(expected[i]), "Missing option: " + expected[i]); - } - } - - private void selectNT(String testFuncName, String value) { - table(testFuncName).getEntry("selected").setString(value); - } - - @Test - public void initializeTest() { - final String fnName = "initializeTest"; - builder.setTable(table(fnName)); - final var chooser = new AutoChooser(); - chooser.initSendable(builder); - builder.update(); - assertNTType(fnName); - assertNTSelected(fnName, NOT_FOUND); - assertNTActive(fnName, chooser.getDefaultName()); - assertNTDefault(fnName, chooser.getDefaultName()); - assertNTOptions(fnName, chooser.getDefaultName()); - } - - @Test - public void addAutoTest() { - final String fnName = "addAutoTest"; - builder.setTable(table(fnName)); - AutoFactory factory = AutoTestHelper.factory(); - AutoChooser chooser = new AutoChooser(); - chooser.initSendable(builder); - chooser.addCmd("AddAutoTestCommand", () -> Commands.none().withName("AddAutoTestCommand")); - chooser.addRoutine("AddAutoTestRoutine", () -> factory.newRoutine("AddAutoTestRoutine")); - - builder.update(); - - assertNTOptions(fnName, chooser.getDefaultName(), "AddAutoTestCommand", "AddAutoTestRoutine"); - } - - @Test - public void selectTest() { - final String fnName = "selectTest"; - builder.setTable(table(fnName)); - AutoFactory factory = AutoTestHelper.factory(); - AutoChooser chooser = new AutoChooser(); - chooser.initSendable(builder); - chooser.addCmd("SelectTestCommand", () -> Commands.none().withName("SelectTestCommand")); - chooser.addRoutine("SelectTestRoutine", () -> factory.newRoutine("SelectTestRoutine")); - - builder.update(); - - selectNT(fnName, "SelectTestRoutine"); - assertNTSelected(fnName, "SelectTestRoutine"); - assertNTActive(fnName, chooser.getDefaultName()); - - builder.update(); - - // DriverStation should report disconnected causing the active to not update - assertNTActive(fnName, chooser.getDefaultName()); - - DriverStationSim.setAllianceStationId(AllianceStationID.Blue1); - DriverStationSim.setEnabled(false); - DriverStationSim.setDsAttached(true); - DriverStationSim.notifyNewData(); - DriverStation.refreshData(); - - builder.update(); - builder.update(); - - assertNTActive(fnName, "SelectTestRoutine"); - - assertEquals(chooser.selectedCommand().getName(), "SelectTestRoutine"); - - DriverStationSim.setAllianceStationId(AllianceStationID.Unknown); - DriverStationSim.setDsAttached(false); - DriverStationSim.notifyNewData(); - DriverStation.refreshData(); - } -} diff --git a/choreolib/src/test/java/choreo/auto/AutoTestHelper.java b/choreolib/src/test/java/choreo/auto/AutoTestHelper.java index 1fddcd36d2..98f22b122b 100644 --- a/choreolib/src/test/java/choreo/auto/AutoTestHelper.java +++ b/choreolib/src/test/java/choreo/auto/AutoTestHelper.java @@ -2,35 +2,31 @@ package choreo.auto; -import edu.wpi.first.hal.AllianceStationID; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.simulation.DriverStationSim; -import edu.wpi.first.wpilibj2.command.Subsystem; import java.util.Optional; import java.util.concurrent.atomic.AtomicReference; +import org.wpilib.command3.Mechanism; +import org.wpilib.command3.Scheduler; +import org.wpilib.driverstation.Alliance; +import org.wpilib.driverstation.internal.DriverStationBackend; +import org.wpilib.hardware.hal.AllianceStationID; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.simulation.DriverStationSim; public class AutoTestHelper { public static AutoFactory factory( - boolean useAllianceFlipping, AtomicReference robotPose) { - // AtomicReference pose = new AtomicReference<>(new Pose2d()); + Scheduler scheduler, boolean useAllianceFlipping, AtomicReference robotPose) { return new AutoFactory( - () -> robotPose.get(), - newPose -> robotPose.set(newPose), + robotPose::get, + robotPose::set, sample -> robotPose.set(sample.getPose()), useAllianceFlipping, - new Subsystem() {}, + new Mechanism() {}, (sample, isStart) -> {}); } - public static AutoFactory factory(boolean useAllianceFlipping) { + public static AutoFactory factory(Scheduler scheduler, boolean useAllianceFlipping) { AtomicReference pose = new AtomicReference<>(new Pose2d()); - return factory(useAllianceFlipping, pose); - } - - public static AutoFactory factory() { - return factory(false); + return factory(scheduler, useAllianceFlipping, pose); } public static void setAlliance(Optional alliance) { @@ -38,15 +34,15 @@ public static void setAlliance(Optional alliance) { alliance .map( all -> { - if (all.equals(Alliance.Blue)) { - return AllianceStationID.Blue1; + if (all.equals(Alliance.BLUE)) { + return AllianceStationID.BLUE_1; } else { - return AllianceStationID.Red1; + return AllianceStationID.RED_1; } }) - .orElse(AllianceStationID.Unknown); + .orElse(AllianceStationID.UNKNOWN); DriverStationSim.setAllianceStationId(id); DriverStationSim.notifyNewData(); - DriverStation.refreshData(); + DriverStationBackend.refreshData(); } } diff --git a/choreolib/src/test/java/choreo/auto/DoneTest.java b/choreolib/src/test/java/choreo/auto/DoneTest.java deleted file mode 100644 index 9867a857eb..0000000000 --- a/choreolib/src/test/java/choreo/auto/DoneTest.java +++ /dev/null @@ -1,151 +0,0 @@ -// Copyright (c) Choreo contributors - -package choreo.auto; - -import static org.junit.jupiter.api.Assertions.assertFalse; -import static org.junit.jupiter.api.Assertions.assertTrue; - -import choreo.trajectory.SwerveSample; -import choreo.trajectory.Trajectory; -import choreo.trajectory.TrajectoryTestHelper; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.simulation.DriverStationSim; -import edu.wpi.first.wpilibj.simulation.SimHooks; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SchedulerMaker; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import org.junit.jupiter.api.Test; - -public class DoneTest { - private static final Pose2d start = new Pose2d(); - private static final Pose2d end = new Pose2d(2.0, 2.0, new Rotation2d(Math.PI)); - - @Test - public void testExecution() { - assert HAL.initialize(500, 0); - CommandScheduler scheduler = SchedulerMaker.make(); - - AutoFactory factory = AutoTestHelper.factory(); - Trajectory trajectory = - TrajectoryTestHelper.linearTrajectory("test", start, end, 3.0, SwerveSample.class); - AutoRoutine routine = factory.newRoutine("test"); - AutoTrajectory traj = factory.trajectory(trajectory, routine, true); - - Trigger oneSecondIn = traj.atTime(1.0); - Trigger twoSecondIn = traj.atTime(2.0); - - Trigger done = traj.done(); - Trigger doneDelayed = traj.doneDelayed(2.0); - Trigger doneFor = traj.doneFor(2.5); - Trigger recentlyDone = traj.recentlyDone(); - - // makes the scheduler poll the triggers every cycle - oneSecondIn.onTrue(Commands.none()); - twoSecondIn.onTrue(Commands.none()); - done.onTrue(Commands.none()); - doneDelayed.onTrue(Commands.none()); - doneFor.onTrue(Commands.none()); - recentlyDone.onTrue(Commands.none()); - - SimHooks.pauseTiming(); - - DriverStationSim.setDsAttached(true); - DriverStationSim.setEnabled(true); - DriverStationSim.setAutonomous(true); - DriverStationSim.notifyNewData(); - DriverStation.refreshData(); - assertTrue(DriverStation.isAutonomousEnabled()); - - assertFalse(oneSecondIn); - assertFalse(twoSecondIn); - assertFalse(done); - assertFalse(doneDelayed); - assertFalse(doneFor); - assertFalse(recentlyDone); - - scheduler.schedule(routine.cmd()); - scheduler.schedule(traj.cmd()); - scheduler.run(); - - assertTrue(routine.active()); - assertTrue(traj.active()); - - assertFalse(oneSecondIn); - assertFalse(twoSecondIn); - assertFalse(done); - assertFalse(doneDelayed); - assertFalse(doneFor); - assertFalse(recentlyDone); - - // FIXME: Timing is flaky in CI - SimHooks.stepTiming(1.0); - scheduler.run(); - // assertTrue(oneSecondIn); - // assertTrue(oneSecondIn); - // assertFalse(twoSecondIn); - SimHooks.stepTiming(1.1); - scheduler.run(); - // assertFalse(oneSecondIn); - // assertTrue(twoSecondIn); - SimHooks.stepTiming(1.05); - scheduler.run(); - // assertTrue(traj.inactive()); - - assertTrue(done); - assertFalse(doneDelayed); - assertTrue(doneFor); - assertTrue(recentlyDone); - SimHooks.stepTiming(0.2); - assertTrue(done); - - SimHooks.stepTiming(1.0); - scheduler.run(); - assertFalse(done); - assertFalse(doneDelayed); - assertTrue(doneFor); - assertTrue(recentlyDone); - - SimHooks.stepTiming(1.0); - scheduler.run(); - assertFalse(done); - assertTrue(doneDelayed); - assertTrue(doneDelayed); - assertTrue(doneFor); - assertTrue(recentlyDone); - - routine.updateIdle(false); // simulating to starting a new trajectory - SimHooks.stepTiming(1.0); - scheduler.run(); - assertFalse(doneFor); - assertFalse(recentlyDone); - - // test re-running a trajectory - scheduler.schedule(traj.cmd()); - scheduler.run(); - - assertTrue(routine.active()); - assertTrue(traj.active()); - - SimHooks.stepTiming(2.0); - scheduler.run(); - scheduler.run(); - SimHooks.stepTiming(1.05); - scheduler.run(); - - assertTrue(traj.inactive()); - assertTrue(done); - - SimHooks.resumeTiming(); - - DriverStationSim.setDsAttached(true); - DriverStationSim.setEnabled(true); - DriverStationSim.setAutonomous(true); - DriverStationSim.notifyNewData(); - DriverStation.refreshData(); - assertTrue(DriverStation.isAutonomousEnabled()); - } -} diff --git a/choreolib/src/test/java/choreo/auto/PoseFlippingTest.java b/choreolib/src/test/java/choreo/auto/PoseFlippingTest.java index 6a979685f4..ad5bcb4616 100644 --- a/choreolib/src/test/java/choreo/auto/PoseFlippingTest.java +++ b/choreolib/src/test/java/choreo/auto/PoseFlippingTest.java @@ -9,17 +9,17 @@ import choreo.trajectory.SwerveSample; import choreo.trajectory.Trajectory; import choreo.util.ChoreoAllianceFlipUtil; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import java.util.List; import java.util.Optional; import java.util.function.Supplier; import org.junit.jupiter.api.Test; +import org.wpilib.command3.Scheduler; +import org.wpilib.driverstation.Alliance; +import org.wpilib.hardware.hal.HAL; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; public class PoseFlippingTest { - Optional alliance; AutoFactory factoryFlip; AutoFactory factoryNoFlip; @@ -29,26 +29,26 @@ void testPoseProperlyFlipped( Pose2d unflipped, Pose2d flipped, Supplier> poseToTest) { setAlliance(Optional.empty()); assertTrue(poseToTest.get().isEmpty()); - setAlliance(Optional.of(Alliance.Blue)); + setAlliance(Optional.of(Alliance.BLUE)); assertEquals(poseToTest.get(), Optional.of(unflipped)); - setAlliance(Optional.of(Alliance.Red)); + setAlliance(Optional.of(Alliance.RED)); assertEquals(poseToTest.get(), Optional.of(flipped)); } - void testPoseProperlyNoFlipped( - Pose2d unflipped, Pose2d flipped, Supplier> poseToTest) { + void testPoseProperlyNoFlipped(Pose2d unflipped, Supplier> poseToTest) { assertEquals(poseToTest.get(), Optional.of(unflipped)); - setAlliance(Optional.of(Alliance.Blue)); + setAlliance(Optional.of(Alliance.BLUE)); assertEquals(poseToTest.get(), Optional.of(unflipped)); - setAlliance(Optional.of(Alliance.Red)); + setAlliance(Optional.of(Alliance.RED)); assertEquals(poseToTest.get(), Optional.of(unflipped)); } @Test void testGetEndPose() { assert HAL.initialize(500, 0); - factoryFlip = AutoTestHelper.factory(true); - factoryNoFlip = AutoTestHelper.factory(false); + Scheduler scheduler = Scheduler.createIndependentScheduler(); + factoryFlip = AutoTestHelper.factory(scheduler, true); + factoryNoFlip = AutoTestHelper.factory(scheduler, false); Pose2d start = Pose2d.kZero; Pose2d end = new Pose2d(1, 1, Rotation2d.fromRadians(1)); Pose2d startFlipped = ChoreoAllianceFlipUtil.flip(start); @@ -86,12 +86,12 @@ void testGetEndPose() { List.of(), List.of()); - AutoTrajectory autoTrajFlipped = factoryFlip.newRoutine("testroutine").trajectory(trajectory); + AutoTrajectory autoTrajFlipped = factoryFlip.trajectory(trajectory); testPoseProperlyFlipped(start, startFlipped, autoTrajFlipped::getInitialPose); testPoseProperlyFlipped(end, endFlipped, autoTrajFlipped::getFinalPose); - AutoTrajectory autoTrajNoFlip = factoryNoFlip.newRoutine("testroutine").trajectory(trajectory); - testPoseProperlyNoFlipped(start, startFlipped, autoTrajNoFlip::getInitialPose); - testPoseProperlyNoFlipped(end, endFlipped, autoTrajNoFlip::getFinalPose); + AutoTrajectory autoTrajNoFlip = factoryNoFlip.trajectory(trajectory); + testPoseProperlyNoFlipped(start, autoTrajNoFlip::getInitialPose); + testPoseProperlyNoFlipped(end, autoTrajNoFlip::getFinalPose); } } diff --git a/choreolib/src/test/java/choreo/auto/RoutineKillNoAllianceTest.java b/choreolib/src/test/java/choreo/auto/RoutineKillNoAllianceTest.java deleted file mode 100644 index 76653ca1de..0000000000 --- a/choreolib/src/test/java/choreo/auto/RoutineKillNoAllianceTest.java +++ /dev/null @@ -1,72 +0,0 @@ -// Copyright (c) Choreo contributors - -package choreo.auto; - -import static choreo.auto.AutoTestHelper.setAlliance; -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertFalse; - -import edu.wpi.first.hal.HAL; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import java.util.Optional; -import java.util.function.Supplier; -import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Test; - -public class RoutineKillNoAllianceTest { - AutoFactory factoryFlip; - AutoFactory factoryNoFlip; - Supplier routineFlip = () -> factoryFlip.newRoutine("testRoutineKill"); - Supplier routineNoFlip = () -> factoryNoFlip.newRoutine("testRoutineKill"); - - @BeforeEach - void setup() { - assert HAL.initialize(500, 0); - factoryFlip = AutoTestHelper.factory(true); - factoryNoFlip = AutoTestHelper.factory(false); - } - - void testRoutineKill(Supplier routineSupplier, boolean expectKill) { - AutoRoutine routine = routineSupplier.get(); - assertFalse(routine.isKilled); - routine.cmd().initialize(); - // don't need to run, this should kill on schedule/initialize - assertEquals(expectKill, routine.isKilled); - } - - @Test - void testUnFlippedEmpty() { - setAlliance(Optional.empty()); - testRoutineKill(routineNoFlip, false); - } - - @Test - void testUnFlippedBlue() { - setAlliance((Optional.of(Alliance.Blue))); - testRoutineKill(routineNoFlip, false); - } - - @Test - void testUnFlippedRed() { - setAlliance(Optional.of(Alliance.Red)); - testRoutineKill(routineNoFlip, false); - } - - @Test - void testFlippedEmpty() { - setAlliance(Optional.empty()); - testRoutineKill(routineFlip, true); - } - - @Test - void testFlippedBlue() { - setAlliance(Optional.of(Alliance.Blue)); - testRoutineKill(routineFlip, false); - } - - @Test - void testFlippedRed() { - setAlliance(Optional.of(Alliance.Red)); - testRoutineKill(routineFlip, false); - } -} diff --git a/choreolib/src/test/java/choreo/auto/TrajectoryCmdTest.java b/choreolib/src/test/java/choreo/auto/TrajectoryCmdTest.java index 7eb2171d0b..06528fd5cf 100644 --- a/choreolib/src/test/java/choreo/auto/TrajectoryCmdTest.java +++ b/choreolib/src/test/java/choreo/auto/TrajectoryCmdTest.java @@ -8,32 +8,32 @@ import choreo.trajectory.SwerveSample; import choreo.trajectory.Trajectory; import choreo.trajectory.TrajectoryTestHelper; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.simulation.DriverStationSim; -import edu.wpi.first.wpilibj.simulation.SimHooks; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.SchedulerMaker; import java.util.concurrent.atomic.AtomicReference; import org.junit.jupiter.api.Test; +import org.wpilib.command3.Command; +import org.wpilib.command3.Scheduler; +import org.wpilib.driverstation.internal.DriverStationBackend; +import org.wpilib.hardware.hal.HAL; +import org.wpilib.hardware.hal.RobotMode; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.simulation.DriverStationSim; +import org.wpilib.simulation.SimHooks; public class TrajectoryCmdTest { private static final Pose2d start = new Pose2d(); - private static final Pose2d end = new Pose2d(2.0, 2.0, new Rotation2d(Math.PI)); + private static final Pose2d end = new Pose2d(2.0, 2.0, Rotation2d.k180deg); @Test public void testExecution() { assert HAL.initialize(500, 0); - CommandScheduler scheduler = SchedulerMaker.make(); + Scheduler scheduler = Scheduler.createIndependentScheduler(); AtomicReference pose = new AtomicReference<>(new Pose2d()); - AutoFactory factory = AutoTestHelper.factory(false, pose); + AutoFactory factory = AutoTestHelper.factory(scheduler, false, pose); Trajectory trajectory = TrajectoryTestHelper.linearTrajectory("test", start, end, 3.0, SwerveSample.class); - Command trajectoryCmd = factory.trajectoryCmd(trajectory); + Command trajectoryCmd = factory.trajectory(trajectory).cmd(); scheduler.schedule(trajectoryCmd); @@ -42,33 +42,32 @@ public void testExecution() { SimHooks.pauseTiming(); - DriverStationSim.setDsAttached(true); - DriverStationSim.setEnabled(true); - DriverStationSim.setAutonomous(true); - DriverStationSim.notifyNewData(); - DriverStation.refreshData(); - assertTrue(DriverStation.isAutonomousEnabled()); + updateAndAssertIsAutonomous(); for (int i = 0; i < 149; i++) { scheduler.run(); - assertTrue(scheduler.isScheduled(trajectoryCmd)); + assertTrue(scheduler.isScheduledOrRunning(trajectoryCmd)); SimHooks.stepTiming(0.02); } SimHooks.stepTiming(0.1); scheduler.run(); - assertFalse(scheduler.isScheduled(trajectoryCmd)); + assertFalse(scheduler.isScheduledOrRunning(trajectoryCmd)); assertTrue(pose.get().getTranslation().getDistance(end.getTranslation()) < 0.5); + updateAndAssertIsAutonomous(); + + SimHooks.resumeTiming(); + } + + private void updateAndAssertIsAutonomous() { DriverStationSim.setDsAttached(true); DriverStationSim.setEnabled(true); - DriverStationSim.setAutonomous(true); + DriverStationSim.setRobotMode(RobotMode.AUTONOMOUS); DriverStationSim.notifyNewData(); - DriverStation.refreshData(); - assertTrue(DriverStation.isAutonomousEnabled()); - - SimHooks.resumeTiming(); + DriverStationBackend.refreshData(); + assertTrue(DriverStationBackend.isAutonomousEnabled()); } } diff --git a/choreolib/src/test/java/choreo/trajectory/SampleFlippingTest.java b/choreolib/src/test/java/choreo/trajectory/SampleFlippingTest.java index d6d92f379e..a40fb4a9d5 100644 --- a/choreolib/src/test/java/choreo/trajectory/SampleFlippingTest.java +++ b/choreolib/src/test/java/choreo/trajectory/SampleFlippingTest.java @@ -9,9 +9,6 @@ import org.junit.jupiter.api.Test; public class SampleFlippingTest { - private static final double FIELD_LENGTH_2026 = 16.541; - private static final double FIELD_WIDTH_2026 = 8.0692; - @Test void testZeroSwerveSample() { SwerveSample sample = @@ -20,7 +17,7 @@ void testZeroSwerveSample() { SwerveSample mirrored2026 = new SwerveSample( 0, - FIELD_LENGTH_2026 - 0, + 0, 0, Math.PI - 0, 0, @@ -34,8 +31,8 @@ void testZeroSwerveSample() { SwerveSample rotated2026 = new SwerveSample( 0, - FIELD_LENGTH_2026, - FIELD_WIDTH_2026, + 0, + 0, Math.PI, 0, 0, @@ -45,9 +42,9 @@ void testZeroSwerveSample() { 0, new double[] {-0.0, -0.0, -0.0, -0.0}, new double[] {-0.0, -0.0, -0.0, -0.0}); - ChoreoAllianceFlipUtil.setFlipper(Flipper.mirroredX(FIELD_LENGTH_2026, FIELD_WIDTH_2026)); + ChoreoAllianceFlipUtil.setFlipper(Flipper.mirroredX()); assertEquals(sample.mirrorX(), mirrored2026); - ChoreoAllianceFlipUtil.setFlipper(Flipper.rotatedAround(FIELD_LENGTH_2026, FIELD_WIDTH_2026)); + ChoreoAllianceFlipUtil.setFlipper(Flipper.rotatedAround()); assertEquals(sample.flipped(), rotated2026); } @@ -70,7 +67,7 @@ void testSwerveSample() { SwerveSample mirrored2026 = new SwerveSample( 0.0, - FIELD_LENGTH_2026 - 1.0, + -1.0, 2.0, Math.PI - 3.0, -4.0, @@ -84,8 +81,8 @@ void testSwerveSample() { SwerveSample rotated2026 = new SwerveSample( 0.0, - FIELD_LENGTH_2026 - 1.0, - FIELD_WIDTH_2026 - 2.0, + -1.0, + -2.0, Math.PI + 3.0, -4.0, -5.0, @@ -95,10 +92,10 @@ void testSwerveSample() { 9.0, new double[] {-10.0, -11.0, -12.0, -13.0}, new double[] {-14.0, -15.0, -16.0, -17.0}); - ChoreoAllianceFlipUtil.setFlipper(Flipper.mirroredX(FIELD_LENGTH_2026, FIELD_WIDTH_2026)); + ChoreoAllianceFlipUtil.setFlipper(Flipper.mirroredX()); assertEquals(sample.flipped(), mirrored2026); assertEquals(sample.mirrorX(), mirrored2026); - ChoreoAllianceFlipUtil.setFlipper(Flipper.rotatedAround(FIELD_LENGTH_2026, FIELD_WIDTH_2026)); + ChoreoAllianceFlipUtil.setFlipper(Flipper.rotatedAround()); assertEquals(sample.flipped(), rotated2026); assertEquals(sample.mirrorX(), mirrored2026); } @@ -107,26 +104,13 @@ void testSwerveSample() { void testZeroDifferentialSample() { DifferentialSample sample = new DifferentialSample(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); DifferentialSample mirrored2026 = - new DifferentialSample( - 0.0, FIELD_LENGTH_2026 - 0, 0.0, Math.PI - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + new DifferentialSample(0.0, 0, 0.0, Math.PI - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); DifferentialSample rotated2026 = - new DifferentialSample( - 0.0, - FIELD_LENGTH_2026, - FIELD_WIDTH_2026, - Math.PI, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0); - ChoreoAllianceFlipUtil.setFlipper(Flipper.mirroredX(FIELD_LENGTH_2026, FIELD_WIDTH_2026)); + new DifferentialSample(0.0, 0, 0, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + ChoreoAllianceFlipUtil.setFlipper(Flipper.mirroredX()); assertEquals(sample.flipped(), mirrored2026); assertEquals(sample.mirrorX(), mirrored2026); - ChoreoAllianceFlipUtil.setFlipper(Flipper.rotatedAround(FIELD_LENGTH_2026, FIELD_WIDTH_2026)); + ChoreoAllianceFlipUtil.setFlipper(Flipper.rotatedAround()); assertEquals(sample.flipped(), rotated2026); assertEquals(sample.mirrorX(), mirrored2026); } @@ -137,36 +121,14 @@ void testDifferentialSample() { new DifferentialSample(0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0); DifferentialSample mirrored2026 = new DifferentialSample( - 0.0, - FIELD_LENGTH_2026 - 1.0, - 2.0, - Math.PI - 3.0, - 5.0, - 4.0, - -6.0, - 8.0, - 7.0, - -9.0, - 11.0, - 10.0); + 0.0, -1.0, 2.0, Math.PI - 3.0, 5.0, 4.0, -6.0, 8.0, 7.0, -9.0, 11.0, 10.0); DifferentialSample rotated2026 = new DifferentialSample( - 0.0, - FIELD_LENGTH_2026 - 1.0, - FIELD_WIDTH_2026 - 2.0, - Math.PI + 3.0, - 4.0, - 5.0, - 6.0, - 7.0, - 8.0, - 9.0, - 10.0, - 11.0); - ChoreoAllianceFlipUtil.setFlipper(Flipper.mirroredX(FIELD_LENGTH_2026, FIELD_WIDTH_2026)); + 0.0, -1.0, -2.0, Math.PI + 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0); + ChoreoAllianceFlipUtil.setFlipper(Flipper.mirroredX()); assertEquals(sample.flipped(), mirrored2026); assertEquals(sample.mirrorX(), mirrored2026); - ChoreoAllianceFlipUtil.setFlipper(Flipper.rotatedAround(FIELD_LENGTH_2026, FIELD_WIDTH_2026)); + ChoreoAllianceFlipUtil.setFlipper(Flipper.rotatedAround()); assertEquals(sample.flipped(), rotated2026); assertEquals(sample.mirrorX(), mirrored2026); } @@ -190,8 +152,8 @@ void testBothMirrorEqualsRotation() { ChoreoAllianceFlipUtil.setFlipper(Flipper.FRC_CURRENT); SwerveSample rotated = sample.mirrorX().mirrorY(); assertEquals(rotated.t, 0.0); - assertEquals(rotated.x, FIELD_LENGTH_2026 - 1.0); - assertEquals(rotated.y, FIELD_WIDTH_2026 - 2.0); + assertEquals(rotated.x, -1.0); + assertEquals(rotated.y, -2.0); assertEquals(rotated.heading, -(-3.0 + Math.PI)); assertEquals(rotated.vx, -4.0); assertEquals(rotated.vy, -5.0); @@ -219,19 +181,8 @@ void testMirrorIsInverse() { new double[] {14.0, 15.0, 16.0, 17.0}); DifferentialSample differentialSample = new DifferentialSample( - 0.0, - FIELD_LENGTH_2026 - 1.0, - 2.0, - Math.PI - 3.0, - 5.0, - 4.0, - -6.0, - 8.0, - 7.0, - -9.0, - 11.0, - 10.0); - ChoreoAllianceFlipUtil.setFlipper(Flipper.mirroredX(FIELD_LENGTH_2026, FIELD_WIDTH_2026)); + 0.0, -1.0, 2.0, Math.PI - 3.0, 5.0, 4.0, -6.0, 8.0, 7.0, -9.0, 11.0, 10.0); + ChoreoAllianceFlipUtil.setFlipper(Flipper.mirroredX()); assertEquals(sample.mirrorX().mirrorX(), sample); assertEquals(sample.mirrorY().mirrorY(), sample); diff --git a/choreolib/src/test/java/choreo/trajectory/TrajectoryTestHelper.java b/choreolib/src/test/java/choreo/trajectory/TrajectoryTestHelper.java index 650b65e5d6..3e0aa0c3d6 100644 --- a/choreolib/src/test/java/choreo/trajectory/TrajectoryTestHelper.java +++ b/choreolib/src/test/java/choreo/trajectory/TrajectoryTestHelper.java @@ -2,8 +2,8 @@ package choreo.trajectory; -import edu.wpi.first.math.geometry.Pose2d; import java.util.List; +import org.wpilib.math.geometry.Pose2d; public class TrajectoryTestHelper { private static final double DT = 0.005; diff --git a/choreolib/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerMaker.java b/choreolib/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerMaker.java deleted file mode 100644 index 44219fc666..0000000000 --- a/choreolib/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerMaker.java +++ /dev/null @@ -1,9 +0,0 @@ -// Copyright (c) Choreo contributors - -package edu.wpi.first.wpilibj2.command; - -public class SchedulerMaker { - public static CommandScheduler make() { - return new CommandScheduler(); - } -} diff --git a/choreolib/src/test/native/cpp/SampleFlippingTests.cpp b/choreolib/src/test/native/cpp/SampleFlippingTests.cpp index 4fe6f45738..2c460c5e84 100644 --- a/choreolib/src/test/native/cpp/SampleFlippingTests.cpp +++ b/choreolib/src/test/native/cpp/SampleFlippingTests.cpp @@ -3,8 +3,8 @@ #include #include -#include -#include +#include +#include #include "choreo/trajectory/SwerveSample.hpp" #include "choreo/trajectory/Trajectory.hpp" @@ -27,9 +27,9 @@ TEST(SampleFlippingTest, ZeroSwerveSample) { {0_N, 0_N, 0_N, 0_N}, {0_N, 0_N, 0_N, 0_N}}; SwerveSample mirrored2024{0_s, - util::fieldLength - 0_m, 0_m, - units::radian_t{std::numbers::pi} - 0_rad, + 0_m, + wpi::units::radian_t{std::numbers::pi} - 0_rad, 0_mps, 0_mps, 0_rad_per_s, @@ -39,9 +39,9 @@ TEST(SampleFlippingTest, ZeroSwerveSample) { {0_N, 0_N, 0_N, 0_N}, {0_N, 0_N, 0_N, 0_N}}; SwerveSample rotated2022{0_s, - util::fieldLength - 0_m, - util::fieldWidth - 0_m, - units::radian_t{std::numbers::pi} + 0_rad, + 0_m, + 0_m, + wpi::units::radian_t{std::numbers::pi} + 0_rad, 0_mps, 0_mps, 0_rad_per_s, @@ -74,9 +74,9 @@ TEST(SampleFlippingTest, SwerveSample) { {10_N, 11_N, 12_N, 13_N}, {14_N, 15_N, 16_N, 17_N}}; SwerveSample mirrored2024{0_s, - util::fieldLength - 1_m, + -1_m, 2_m, - units::radian_t{std::numbers::pi} - 3_rad, + wpi::units::radian_t{std::numbers::pi} - 3_rad, -4_mps, 5_mps, -6_rad_per_s, @@ -86,9 +86,9 @@ TEST(SampleFlippingTest, SwerveSample) { {-11_N, -10_N, -13_N, -12_N}, {15_N, 14_N, 17_N, 16_N}}; SwerveSample rotated2022{0_s, - util::fieldLength - 1_m, - util::fieldWidth - 2_m, - units::radian_t{std::numbers::pi} + 3_rad, + -1_m, + -2_m, + wpi::units::radian_t{std::numbers::pi} + 3_rad, -4_mps, -5_mps, 6_rad_per_s, @@ -112,24 +112,19 @@ TEST(SampleFlippingTest, ZeroDifferentialSample) { 0_s, 0_m, 0_m, 0_rad, 0_mps, 0_mps, 0_rad_per_s, 0_mps_sq, 0_mps_sq, 0_rad_per_s_sq, 0_N, 0_N}; DifferentialSample mirrored2024{ - 0_s, util::fieldLength - 0_m, - 0_m, units::radian_t{std::numbers::pi} - 0_rad, + 0_s, 0_m, + 0_m, wpi::units::radian_t{std::numbers::pi} - 0_rad, + 0_mps, 0_mps, + 0_rad_per_s, 0_mps_sq, + 0_mps_sq, 0_rad_per_s_sq, + 0_N, 0_N}; + DifferentialSample rotated2022{ + 0_s, 0_m, + 0_m, wpi::units::radian_t{std::numbers::pi} + 0_rad, 0_mps, 0_mps, 0_rad_per_s, 0_mps_sq, 0_mps_sq, 0_rad_per_s_sq, 0_N, 0_N}; - DifferentialSample rotated2022{0_s, - util::fieldLength - 0_m, - util::fieldWidth - 0_m, - units::radian_t{std::numbers::pi} + 0_rad, - 0_mps, - 0_mps, - 0_rad_per_s, - 0_mps_sq, - 0_mps_sq, - 0_rad_per_s_sq, - 0_N, - 0_N}; ASSERT_TRUE(sample.Flipped<2024>() == mirrored2024); ASSERT_TRUE(sample.Flipped<2022>() == rotated2022); @@ -145,24 +140,19 @@ TEST(SampleFlippingTest, DifferentialSample) { 0_s, 1_m, 2_m, 3_rad, 4_mps, 5_mps, 6_rad_per_s, 7_mps_sq, 8_mps_sq, 9_rad_per_s_sq, 10_N, 11_N}; DifferentialSample mirrored2024{ - 0_s, util::fieldLength - 1_m, - 2_m, units::radian_t{std::numbers::pi} - 3_rad, + 0_s, -1_m, + 2_m, wpi::units::radian_t{std::numbers::pi} - 3_rad, 5_mps, 4_mps, -6_rad_per_s, 8_mps_sq, 7_mps_sq, -9_rad_per_s_sq, 11_N, 10_N}; - DifferentialSample rotated2022{0_s, - util::fieldLength - 1_m, - util::fieldWidth - 2_m, - units::radian_t{std::numbers::pi} + 3_rad, - 4_mps, - 5_mps, - 6_rad_per_s, - 7_mps_sq, - 8_mps_sq, - 9_rad_per_s_sq, - 10_N, - 11_N}; + DifferentialSample rotated2022{ + 0_s, -1_m, + -2_m, wpi::units::radian_t{std::numbers::pi} + 3_rad, + 4_mps, 5_mps, + 6_rad_per_s, 7_mps_sq, + 8_mps_sq, 9_rad_per_s_sq, + 10_N, 11_N}; ASSERT_TRUE(sample.Flipped<2024>() == mirrored2024); ASSERT_TRUE(sample.Flipped<2022>() == rotated2022); diff --git a/choreolib/src/test/native/cpp/TrajectoryFileTests.cpp b/choreolib/src/test/native/cpp/TrajectoryFileTests.cpp index e4edcfd6b0..24d2dac2df 100644 --- a/choreolib/src/test/native/cpp/TrajectoryFileTests.cpp +++ b/choreolib/src/test/native/cpp/TrajectoryFileTests.cpp @@ -3,8 +3,7 @@ #include #include -#include -#include +#include #include "choreo/trajectory/SwerveSample.hpp" #include "choreo/trajectory/Trajectory.hpp" @@ -75,7 +74,8 @@ constexpr std::string_view swerveTrajectoryString = ] })"; -const wpi::json swerveTrajectoryJson = wpi::json::parse(swerveTrajectoryString); +const wpi::util::json swerveTrajectoryJson = + wpi::util::json::parse_or_throw(swerveTrajectoryString); const Trajectory correctSwerveTrajectory{ "New Path", @@ -111,9 +111,6 @@ TEST(TrajectoryFileTest, DeserializeSwerveTrajectory) { Trajectory deserializedSwerveTrajectory = swerveTrajectoryJson.get>(); ASSERT_EQ(correctSwerveTrajectory, deserializedSwerveTrajectory); - } catch (wpi::json::parse_error& e) { - std::cerr << "JSON parse error: " << e.what() << std::endl; - FAIL(); } catch (std::exception& e) { std::cerr << "Error: " << e.what() << std::endl; FAIL(); diff --git a/choreolib/vendor_jsons/ChoreoLib2027Alpha.json b/choreolib/vendor_jsons/ChoreoLib2027Alpha.json index eb0adc5d09..178e59fea7 100644 --- a/choreolib/vendor_jsons/ChoreoLib2027Alpha.json +++ b/choreolib/vendor_jsons/ChoreoLib2027Alpha.json @@ -1,9 +1,9 @@ { "fileName": "ChoreoLib2027Alpha.json", "name": "ChoreoLib", - "version": "2027.0.0-alpha-1", + "version": "2027.0.0-alpha-2", "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", - "frcYear": "2027_alpha1", + "wpilibYear": "2027_alpha5", "mavenUrls": [ "https://frcmaven.wpi.edu/artifactory/sleipnirgroup-mvn-release/" ], @@ -12,7 +12,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-java", - "version": "2027.0.0-alpha-1" + "version": "2027.0.0-alpha-2" }, { "groupId": "com.google.code.gson", @@ -25,7 +25,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-cpp", - "version": "2027.0.0-alpha-1", + "version": "2027.0.0-alpha-2", "libName": "ChoreoLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/choreolib/vendordeps/CommandsV3.json b/choreolib/vendordeps/CommandsV3.json new file mode 100644 index 0000000000..cb1219b518 --- /dev/null +++ b/choreolib/vendordeps/CommandsV3.json @@ -0,0 +1,25 @@ +{ + "fileName": "CommandsV3.json", + "name": "Commands v3", + "version": "1.0.0", + "uuid": "4decdc05-a056-46cf-9561-39449bbb0130", + "wpilibYear": "2027_alpha5", + "mavenUrls": [], + "jsonUrl": "", + "conflictsWith": [ + { + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "errorMessage": "Users can not have both Commands v2 and Commands v3 vendordeps in their robot program.", + "offlineFileName": "CommandsV2.json" + } + ], + "javaDependencies": [ + { + "groupId": "org.wpilib", + "artifactId": "commands3-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} diff --git a/choreolib/vendordeps/WPILibNewCommands.json b/choreolib/vendordeps/WPILibNewCommands.json deleted file mode 100644 index 900f4707aa..0000000000 --- a/choreolib/vendordeps/WPILibNewCommands.json +++ /dev/null @@ -1,37 +0,0 @@ -{ - "fileName": "WPILibNewCommands.json", - "name": "WPILib-New-Commands", - "version": "2026.1.1", - "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": "2026.1.1", - "libName": "wpilibNewCommands", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxarm32", - "linuxarm64", - "windowsx86-64", - "windowsx86", - "linuxx86-64", - "osxuniversal" - ] - } - ] -} diff --git a/docs/choreolib/getting-started.md b/docs/choreolib/getting-started.md index 98622e2be4..a77d18e320 100644 --- a/docs/choreolib/getting-started.md +++ b/docs/choreolib/getting-started.md @@ -77,9 +77,9 @@ In general, trajectory followers accept trajectory "samples" that represent the frc::Pose2d pose = GetPose(); // Calculate feedback velocities - units::meters_per_second_t xFeedback{xController.Calculate(pose.X().value(), sample.x.value())}; - units::meters_per_second_t yFeedback{yController.Calculate(pose.Y().value(), sample.y.value())}; - units::radians_per_second_t headingFeedback{ + wpi::units::meters_per_second_t xFeedback{xController.Calculate(pose.X().value(), sample.x.value())}; + wpi::units::meters_per_second_t yFeedback{yController.Calculate(pose.Y().value(), sample.y.value())}; + wpi::units::radians_per_second_t headingFeedback{ headingController.Calculate(pose.Rotation().Radians().value(), sample.heading.value()) }; diff --git a/docs/contributing/release-process.md b/docs/contributing/release-process.md index 5143945c3d..da4aa4b33d 100644 --- a/docs/contributing/release-process.md +++ b/docs/contributing/release-process.md @@ -19,7 +19,7 @@ git clone git@github.com:SleipnirGroup/ChoreoLib Update the vendordep JSON in ChoreoLib/dep. 1. Update the Choreo version keys to VERSION -2. Update the frcYear key if needed +2. Update the wpilibYear key if needed ## Choreo Release diff --git a/package.json b/package.json index b72c987b93..cf720f7a3f 100644 --- a/package.json +++ b/package.json @@ -1,6 +1,6 @@ { "name": "choreo", - "version": "2026.0.3", + "version": "2027.0.0-alpha-2", "description": "A graphical tool for planning time-optimized trajectories for autonomous mobile robots in the FIRST Robotics Competition.", "homepage": "https://github.com/SleipnirGroup/Choreo", "bugs": { diff --git a/src-cli/Cargo.toml b/src-cli/Cargo.toml index e83279c91c..d448008238 100644 --- a/src-cli/Cargo.toml +++ b/src-cli/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "choreo-cli" -version = "2026.0.3" +version = "2027.0.0-alpha.2" edition = "2024" homepage = "https://github.com/SleipnirGroup/Choreo" repository = "https://github.com/SleipnirGroup/Choreo.git" diff --git a/src-core/Cargo.toml b/src-core/Cargo.toml index 16b5d544aa..5a60929d3b 100644 --- a/src-core/Cargo.toml +++ b/src-core/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "choreo-core" -version = "2026.0.3" +version = "2027.0.0-alpha.2" edition = "2024" homepage = "https://github.com/SleipnirGroup/Choreo" repository = "https://github.com/SleipnirGroup/Choreo.git" diff --git a/src-tauri/Cargo.toml b/src-tauri/Cargo.toml index 85478a335c..5bdf67a6ae 100644 --- a/src-tauri/Cargo.toml +++ b/src-tauri/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "choreo" -version = "2026.0.3" +version = "2027.0.0-alpha.2" edition = "2024" description = """\ A graphical tool for planning time-optimized trajectories for autonomous \ diff --git a/src-tauri/tauri.conf.json b/src-tauri/tauri.conf.json index 2b13b1e5c7..7eb58e1631 100644 --- a/src-tauri/tauri.conf.json +++ b/src-tauri/tauri.conf.json @@ -24,7 +24,7 @@ "externalBin": ["../target/choreo-cli"] }, "productName": "Choreo", - "version": "2026.0.3", + "version": "2027.0.0-alpha-2", "identifier": "org.sleipnirgroup", "plugins": {}, "app": { @@ -33,7 +33,7 @@ { "fullscreen": false, "resizable": true, - "title": "Choreo v2026.0.3", + "title": "Choreo v2027.0.0-alpha-2", "width": 1200, "height": 800 } diff --git a/update_field_dimensions.py b/update_field_dimensions.py index c696507ad9..742e1cc255 100755 --- a/update_field_dimensions.py +++ b/update_field_dimensions.py @@ -64,11 +64,11 @@ class FieldDimensions {{ #pragma once -#include +#include namespace choreo::util {{ -static constexpr units::meter_t fieldLength = {length}_m; -static constexpr units::meter_t fieldWidth = {width}_m; +static constexpr wpi::units::meter_t fieldLength = {length}_m; +static constexpr wpi::units::meter_t fieldWidth = {width}_m; }} // namespace choreo::util""" ), ),