.
diff --git a/lib/examples/swerve/README.md b/lib/examples/swerve/README.md
new file mode 100644
index 00000000..60dad4ae
--- /dev/null
+++ b/lib/examples/swerve/README.md
@@ -0,0 +1 @@
+# 2k26-Robot-Code
diff --git a/lib/examples/swerve/WPILib-License.md b/lib/examples/swerve/WPILib-License.md
new file mode 100644
index 00000000..051080d8
--- /dev/null
+++ b/lib/examples/swerve/WPILib-License.md
@@ -0,0 +1,25 @@
+Copyright (c) 2009-2025 FIRST and other WPILib contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+- Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+- Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+- Neither the name of FIRST, WPILib, nor the names of other WPILib
+ contributors may be used to endorse or promote products derived from
+ this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/config.json b/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/config.json
new file mode 100644
index 00000000..3cc3b6e3
--- /dev/null
+++ b/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/config.json
@@ -0,0 +1,142 @@
+{
+ "name": "Doomspiral",
+ "sourceUrl": "https://frc190.onshape.com/documents/b452dd5a31bf95c6384826fb/w/4d65f099a7d3627693a2bc3a/e/934f62b4a3bd58acfff00615",
+ "disableSimplification": true,
+ "rotations": [
+ {
+ "axis": "x",
+ "degrees": 90
+ },
+ {
+ "axis": "z",
+ "degrees": 90
+ }
+ ],
+ "position": [
+ 0,
+ 0,
+ 0
+ ],
+ "cameras": [],
+ "components": [
+ {
+ "zeroedRotations": [
+ {
+ "axis": "x",
+ "degrees": 90
+ },
+ {
+ "axis": "z",
+ "degrees": 90
+ }
+ ],
+ "zeroedPosition": [
+ 0.009525,
+ 0,
+ -0.067589
+ ]
+ },
+ {
+ "zeroedRotations": [
+ {
+ "axis": "x",
+ "degrees": 90
+ },
+ {
+ "axis": "z",
+ "degrees": 90
+ }
+ ],
+ "zeroedPosition": [
+ 0.202788,
+ -0.090048,
+ -0.477077
+ ]
+ },
+ {
+ "zeroedRotations": [
+ {
+ "axis": "x",
+ "degrees": 90
+ },
+ {
+ "axis": "z",
+ "degrees": 90
+ }
+ ],
+ "zeroedPosition": [
+ 0,
+ 0,
+ 0
+ ]
+ },
+ {
+ "zeroedRotations": [
+ {
+ "axis": "x",
+ "degrees": 90
+ },
+ {
+ "axis": "z",
+ "degrees": 90
+ }
+ ],
+ "zeroedPosition": [
+ -0.142476,
+ 0,
+ -0.278075
+ ]
+ },
+ {
+ "zeroedRotations": [
+ {
+ "axis": "x",
+ "degrees": 90
+ },
+ {
+ "axis": "z",
+ "degrees": 90
+ }
+ ],
+ "zeroedPosition": [
+ 0.030565,
+ 0,
+ -0.312095
+ ]
+ },
+ {
+ "zeroedRotations": [
+ {
+ "axis": "x",
+ "degrees": 90
+ },
+ {
+ "axis": "z",
+ "degrees": 90
+ }
+ ],
+ "zeroedPosition": [
+ -0.278238,
+ 0,
+ -0.196629
+ ]
+ },
+ {
+ "zeroedRotations": [
+ {
+ "axis": "x",
+ "degrees": 90
+ },
+ {
+ "axis": "z",
+ "degrees": 90
+ }
+ ],
+ "zeroedPosition": [
+ 0.009856,
+ 0,
+ -0.304569
+ ]
+ }
+ ]
+}
diff --git a/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model.glb b/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model.glb
new file mode 100644
index 00000000..780d3e14
Binary files /dev/null and b/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model.glb differ
diff --git a/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_0.glb b/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_0.glb
new file mode 100644
index 00000000..69d24ee9
Binary files /dev/null and b/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_0.glb differ
diff --git a/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_1.glb b/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_1.glb
new file mode 100644
index 00000000..f29c3ac2
Binary files /dev/null and b/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_1.glb differ
diff --git a/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_2.glb b/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_2.glb
new file mode 100644
index 00000000..9da3d878
Binary files /dev/null and b/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_2.glb differ
diff --git a/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_3.glb b/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_3.glb
new file mode 100644
index 00000000..07c74d72
Binary files /dev/null and b/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_3.glb differ
diff --git a/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_4.glb b/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_4.glb
new file mode 100644
index 00000000..29fc4cc9
Binary files /dev/null and b/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_4.glb differ
diff --git a/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_5.glb b/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_5.glb
new file mode 100644
index 00000000..2e97af12
Binary files /dev/null and b/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_5.glb differ
diff --git a/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_6.glb b/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_6.glb
new file mode 100644
index 00000000..22d3a452
Binary files /dev/null and b/lib/examples/swerve/advantagescope_assets/Robot_Doomspiral/model_6.glb differ
diff --git a/lib/examples/swerve/build.gradle b/lib/examples/swerve/build.gradle
new file mode 100755
index 00000000..c59ac10a
--- /dev/null
+++ b/lib/examples/swerve/build.gradle
@@ -0,0 +1,456 @@
+plugins {
+ id "java"
+ id "edu.wpi.first.GradleRIO" version "2026.2.1"
+ id "com.peterabeles.gversion" version "1.10"
+ id "io.freefair.lombok" version "8.4"
+ id "com.diffplug.spotless" version "6.25.0"
+ id "io.freefair.aspectj.post-compile-weaving" version "8.6"
+}
+
+repositories {
+ maven { url = uri("https://3015rangerrobotics.github.io/pathplannerlib/repo") }
+}
+
+ext {
+ deploySrcDir = "src/main/deploy"
+ deployRobotDir = "/home/lvuser/deploy"
+ useLocalGompeiLib = project.hasProperty("useLocalGompeiLib")
+}
+
+java {
+ sourceCompatibility = JavaVersion.VERSION_17
+ targetCompatibility = JavaVersion.VERSION_17
+}
+
+def ROBOT_MAIN_CLASS = "frc.robot.Main"
+
+gradle.projectsEvaluated {
+ def buildDir = new File("${project.buildDir}/classes/java/main")
+ if (!buildDir.exists()) {
+ println "⚠️ Warning: compiled classes not found. Make sure compileJava runs first."
+ project.ext.deploySrcDir = "src/main/deploy"
+ return
+ }
+
+ try {
+ def loader = new URLClassLoader([buildDir.toURI().toURL()] as URL[], this.class.classLoader)
+ def constantsClass = loader.loadClass("frc.robot.RobotConfig")
+ def robotField = constantsClass.getDeclaredField("ROBOT")
+ robotField.setAccessible(true)
+ def robotEnumValue = robotField.get(null) // static field
+
+ println "Detected RobotType: ${robotEnumValue}"
+
+ def baseName = robotEnumValue.toString().toLowerCase().replaceAll(/_sim$/, "")
+ def candidateDir = "src/main/deploy/${baseName}"
+
+ if (file(candidateDir).exists()) {
+ project.ext.deploySrcDir = candidateDir
+ project.ext.deployRobotDir = "/home/lvuser/deploy/${baseName}"
+ } else {
+ println "Folder '${candidateDir}' does not exist. Using default deploy folder."
+ }
+
+ println "deploySrcDir set to: ${project.ext.deploySrcDir}"
+ } catch (Throwable e) {
+ println "Could not read RobotType: ${e.message}"
+ project.ext.deploySrcDir = "src/main/deploy"
+ }
+}
+
+// Define my targets (RoboRIO) and artifacts (deployable files)
+// This is added by GradleRIO's backing project DeployUtils.
+deploy {
+ targets {
+ roborio(getTargetTypeClass('RoboRIO')) {
+ // Team number is loaded either from the .wpilib/wpilib_preferences.json
+ // or from command line. If not found an exception will be thrown.
+ // You can use getTeamOrDefault(team) instead of getTeamNumber if you
+ // want to store a team number in this file.
+ team = 190
+ debug = project.frc.getDebugOrDefault(false)
+
+ artifacts {
+ // First part is artifact name, 2nd is artifact type
+ // getTargetTypeClass is a shortcut to get the class type using a string
+
+ frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
+ jvmArgs.add("-XX:+UnlockExperimentalVMOptions")
+ jvmArgs.add("-XX:GCTimeRatio=5")
+ jvmArgs.add("-XX:+UseSerialGC")
+ jvmArgs.add("-XX:MaxGCPauseMillis=50")
+
+ // The options below may improve performance, but should only be enabled on the RIO 2
+
+ final MAX_JAVA_HEAP_SIZE_MB = 100
+ jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M")
+ jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M")
+ jvmArgs.add("-XX:+AlwaysPreTouch")
+
+ // Enable VisualVM connection
+ // jvmArgs.add("-Dcom.sun.management.jmxremote=true")
+ // jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198")
+ // jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false")
+ // jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false")
+ // jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false")
+ // jvmArgs.add("-Djava.rmi.server.hostname=10.1.90.2") // Replace TE.AM with team number
+ }
+
+ // Static files artifact
+ frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+ files = project.fileTree(project.ext.deploySrcDir)
+ directory = deployRobotDir
+ // Change to true to delete files on roboRIO that no
+ // longer exist in deploy directory on roboRIO
+ deleteOldFiles = false
+ }
+ }
+ }
+ }
+}
+
+// Grab the roboRIO comment via SSH
+
+import com.jcraft.jsch.JSch
+import edu.wpi.first.gradlerio.GradleRIOPlugin
+import groovy.json.JsonSlurper
+
+ext {
+ // RoboRIO SSH details
+ roboRIOHost = 'roboRIO-190-FRC.local' // Replace with your team number
+ roboRIOUser = 'admin' // Default user
+ roboRIOPassword = '' // Default password
+ machineInfoFile = '/etc/machine-info' // Path to the file to read
+}
+
+task checkRoboRIOtoRobotType {
+ dependsOn compileJava
+ doLast {
+ def ROBORIO_COMMENT = fetchNameUsingJSch(
+ roboRIOHost, roboRIOUser, roboRIOPassword, machineInfoFile
+ )
+ println "RoboRIO Name: ${ROBORIO_COMMENT}"
+
+ // Ensure the build output directory is in the classpath
+ def buildOutputDir = file('build/classes/java/main')
+ URLClassLoader loader = new URLClassLoader([
+ buildOutputDir.toURI().toURL()
+ ] as URL[], this.class.classLoader)
+
+ // Set file path names and constants
+ def robotTypeClassName = 'frc.robot.RobotConfig$RobotType'
+ def constantsClassName = 'frc.robot.RobotConfig'
+ def robotType = null
+ def constantsClass = null
+ def robotFieldName = 'ROBOT'
+
+ try {
+ // Load the class from the extended classloader
+ robotType = loader.loadClass(robotTypeClassName)
+ println "Loaded class: ${robotType.name}"
+
+ def constants = robotType.enumConstants
+ println "Enum constants: ${constants.collect { it.name() }}"
+ } catch (ClassNotFoundException e) {
+ println "Error: Class ${robotTypeClassName} not found. ${e.message}"
+ }
+
+ // Check for valid name of roboRIO comment
+ boolean foundMatch = false
+ for (enumConstant in robotType.enumConstants) {
+ if (enumConstant.toString().equalsIgnoreCase(ROBORIO_COMMENT)) {
+ foundMatch = true
+ break // Exit the loop once a match is found
+ }
+ }
+
+ if (foundMatch) {
+ println "Match found for possible RobotType"
+ } else {
+ throw new GradleException("Error: '${ROBORIO_COMMENT}' is not a valid RobotType\nTo Fix: Enter valid RoboRIO comment of valid RobotType in file '${machineInfoFile}' or via Webserver!")
+ }
+
+ try {
+ // Load the class from the extended classloader
+ constantsClass = loader.loadClass(constantsClassName)
+ println "Loaded class: ${constantsClass.name}"
+ } catch (ClassNotFoundException e) {
+ println "Error: Class ${constantsClassName} not found. ${e.message}"
+ }
+
+ // Get the ROBOT field
+ def robotField = constantsClass.getDeclaredField(robotFieldName)
+ robotField.setAccessible(true)
+
+ // Get the value of the ROBOT field
+ def robotEnumValue = robotField.get(null) // For static fields, pass `null`
+ println "Declared ROBOT in Constants.java: ${robotEnumValue}"
+
+ // Compare roboRIO name with ROBOT value
+ if (ROBORIO_COMMENT.equalsIgnoreCase(robotEnumValue.name())) {
+ println "The roboRIO name matches the ROBOT value in Constants.java!"
+ } else {
+ throw new GradleException("Mismatch! The RoboRIO comment '${ROBORIO_COMMENT}' does not match the ROBOT value '${robotEnumValue.name()}'\nTo Fix: Match RoboRIO comment to ROBOT in file '${constantsClassName}'")
+ }
+ }
+}
+
+// Function to fetch the file content using JSch
+def fetchNameUsingJSch(host, user, password, remoteFile) {
+ def session = null
+ def channel = null
+ try {
+ JSch jsch = new JSch()
+ session = jsch.getSession(user, host, 22)
+ session.setPassword(password)
+
+ // Skip strict host key checking for simplicity
+ session.setConfig("StrictHostKeyChecking", "no")
+
+ // Connect to the session
+ session.connect()
+
+ // Open an exec channel to run the command
+ channel = session.openChannel("exec")
+ def command = "cat ${remoteFile}"
+ channel.setCommand(command)
+
+ // Capture the output
+ def inputStream = channel.inputStream
+ channel.connect()
+
+ def output = inputStream.text.trim()
+ if (output.isEmpty()) {
+ throw new GradleException("Failed to fetch data from ${remoteFile}")
+ }
+
+ // Extract the name within quotes if the output contains PRETTY_HOSTNAME
+ def matcher = output =~ /PRETTY_HOSTNAME="(.+?)"/
+ if (matcher.find()) {
+ return matcher.group(1) // The name within quotes
+ } else {
+ throw new GradleException("RoboRIO Comment is Empty in file '${remoteFile}'\nTo Fix: Add RoboRIO Comment in file '${remoteFile}' or via Webserver!")
+ }
+ } catch (Exception e) {
+ throw new GradleException("Error during SSH connection: ${e.message}", e)
+ } finally {
+ if (channel != null) {
+ channel.disconnect()
+ }
+ if (session != null) {
+ session.disconnect()
+ }
+ }
+}
+
+deployroborio.dependsOn(checkRoboRIOtoRobotType)
+
+
+def deployArtifact = deploy.targets.roborio.artifacts.frcJava
+
+// Set to true to use debug for JNI.
+wpi.java.debugJni = false
+
+// Set this to true to enable desktop support.
+def includeDesktopSupport = true
+
+// Configuration for AdvantageKit
+task(replayWatch, type: JavaExec) {
+ mainClass = "org.littletonrobotics.junction.ReplayWatch"
+ classpath = sourceSets.main.runtimeClasspath
+}
+
+// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+// Also defines JUnit 4.
+dependencies {
+ annotationProcessor wpi.java.deps.wpilibAnnotations()
+ implementation wpi.java.deps.wpilib()
+ implementation wpi.java.vendor.java()
+ implementation 'org.jgrapht:jgrapht-core:1.5.1'
+ implementation 'org.jgrapht:jgrapht-io:1.5.1'
+
+ implementation 'com.pathplanner.lib:PathplannerLib-java:2026.1.2'
+
+
+ implementation project(":") // <- root project (GompeiLib library)
+ // This is required for the @Tracer annotations to work at runtime
+ implementation 'org.aspectj:aspectjrt:1.9.21'
+ implementation 'org.aspectj:aspectjtools:1.9.21'
+
+ roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
+ roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)
+
+ nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
+ nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
+ simulationDebug wpi.sim.enableDebug()
+
+ nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop)
+ nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
+ simulationRelease wpi.sim.enableRelease()
+
+ testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
+ testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
+ def akitJson = new JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
+ annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
+}
+
+test {
+ useJUnitPlatform()
+ systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
+}
+
+// Simulation configuration (e.g. environment variables).
+//
+// The sim GUI is *disabled* by default to support running
+// AdvantageKit log replay from the command line. Set the
+// value to "true" to enable the sim GUI by default (this
+// is the standard WPILib behavior).
+wpi.sim.addGui().defaultEnabled = false
+wpi.sim.addDriverstation()
+
+// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
+// in order to make them all available at runtime. Also adding the manifest so WPILib
+// knows where to look for our Robot Class.
+jar {
+ from {
+ configurations.runtimeClasspath.collect {
+ it.isDirectory() ? it : zipTree(it)
+ }
+ }
+ from sourceSets.main.allSource
+ manifest GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
+ duplicatesStrategy = DuplicatesStrategy.INCLUDE
+}
+
+// Configure jar and deploy tasks
+deployArtifact.jarTask = jar
+wpi.java.configureExecutableTasks(jar)
+wpi.java.configureTestTasks(test)
+
+// Configure string concat to always inline compile
+tasks.withType(JavaCompile).configureEach {
+ options.compilerArgs.add '-XDstringConcat=inline'
+}
+
+// Create version file
+project.compileJava.dependsOn(createVersionFile)
+gversion {
+ srcDir = "src/main/java/"
+ classPackage = "frc.robot"
+ className = "BuildConstants"
+ dateFormat = "yyyy-MM-dd HH:mm:ss z"
+ timeZone = "America/New_York"
+ indent = " "
+}
+
+// Create commit with working changes on event branches
+tasks.register('eventDeploy') {
+ doLast {
+ if (project.gradle.startParameter.taskNames.any({ it.toLowerCase().contains("deploy") })) {
+ def branchPrefix = "event"
+ def branch = 'git branch --show-current'.execute().text.trim()
+ def commitMessage = "Update at '${new Date().toString()}'"
+
+ if (branch.startsWith(branchPrefix)) {
+ exec {
+ workingDir(projectDir)
+ executable 'git'
+ args 'add', '-A'
+ }
+ exec {
+ workingDir(projectDir)
+ executable 'git'
+ args 'commit', '-m', commitMessage
+ ignoreExitValue = true
+ }
+
+ println "Committed to branch: '$branch'"
+ println "Commit message: '$commitMessage'"
+ } else {
+ println "Not on an event branch, skipping commit"
+ }
+ } else {
+ println "Not running deploy task, skipping commit"
+ }
+ }
+}
+createVersionFile.dependsOn(eventDeploy)
+
+// Spotless formatting
+project.compileJava.dependsOn(spotlessApply)
+spotless {
+ java {
+ target fileTree(".") {
+ include "**/*.java"
+ exclude "**/build/**", "**/build-*/**"
+ }
+ toggleOffOn()
+ googleJavaFormat()
+ removeUnusedImports()
+ trimTrailingWhitespace()
+ endWithNewline()
+ }
+ groovyGradle {
+ target fileTree(".") {
+ include "**/*.gradle"
+ exclude "**/build/**", "**/build-*/**"
+ }
+ greclipse()
+ indentWithSpaces(4)
+ trimTrailingWhitespace()
+ endWithNewline()
+ }
+ json {
+ target fileTree(".") {
+ include "**/*.json"
+ exclude "**/build/**", "**/build-*/**"
+ }
+ gson().indentWithSpaces(2)
+ }
+ format "misc", {
+ target fileTree(".") {
+ include "**/*.md", "**/.gitignore"
+ exclude "**/build/**", "**/build-*/**"
+ }
+ trimTrailingWhitespace()
+ indentWithSpaces(2)
+ endWithNewline()
+ }
+}
+
+//tasks.register('appendBuildVersion') {
+// doLast {
+// def src = useLocalGompeiLib
+// ? "GompeiLib:local"
+// : ((project.hasProperty("gpr.user") && project.hasProperty("gpr.key"))
+// ? "GompeiLib:github-packages"
+// : "GompeiLib:jitpack")
+//
+// def dep = configurations.detachedConfiguration(
+// dependencies.create(GLibRepoPath)
+// ).resolvedConfiguration.firstLevelModuleDependencies.find {
+// it.moduleName == "gompeilib"
+// }
+//
+// def ver = dep?.moduleVersion ?: "unknown"
+// "${src}:${ver}"
+// def outFile = file("src/main/java/frc/robot/BuildConstants.java")
+// if (!outFile.exists()) throw new GradleException("BuildConstants.java not found. Run gversion first.")
+//
+// def contents = outFile.text
+//
+// // Replace existing VERSION field if present, otherwise append before last brace
+// if (contents =~ /public static final String VERSION\s*=\s*".*";/) {
+// contents = contents.replaceFirst(/public static final String VERSION\s*=\s*".*";/,
+// "public static final String VERSION = \"${src}:${ver}\";")
+// } else {
+// contents = contents.replaceFirst(/\}\s*$/, " public static final String VERSION = \"${src}:${ver}\";\n}")
+// }
+//
+// outFile.text = contents
+// println "VERSION field set to ${src}:${ver}"
+// }
+//}
+
+//createVersionFile.finalizedBy appendBuildVersion
diff --git a/lib/examples/swerve/gradle/wrapper/gradle-wrapper.jar b/lib/examples/swerve/gradle/wrapper/gradle-wrapper.jar
new file mode 100644
index 00000000..a4b76b95
Binary files /dev/null and b/lib/examples/swerve/gradle/wrapper/gradle-wrapper.jar differ
diff --git a/lib/examples/swerve/gradle/wrapper/gradle-wrapper.properties b/lib/examples/swerve/gradle/wrapper/gradle-wrapper.properties
new file mode 100644
index 00000000..34bd9ce9
--- /dev/null
+++ b/lib/examples/swerve/gradle/wrapper/gradle-wrapper.properties
@@ -0,0 +1,7 @@
+distributionBase=GRADLE_USER_HOME
+distributionPath=permwrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip
+networkTimeout=10000
+validateDistributionUrl=true
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=permwrapper/dists
diff --git a/lib/examples/swerve/gradlew b/lib/examples/swerve/gradlew
new file mode 100755
index 00000000..1aa94a42
--- /dev/null
+++ b/lib/examples/swerve/gradlew
@@ -0,0 +1,249 @@
+#!/bin/sh
+
+#
+# Copyright © 2015-2021 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.
+# You may obtain a copy of the License at
+#
+# https://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
+##############################################################################
+#
+# Gradle start up script for POSIX generated by Gradle.
+#
+# Important for running:
+#
+# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is
+# noncompliant, but you have some other compliant shell such as ksh or
+# bash, then to run this script, type that shell name before the whole
+# command line, like:
+#
+# ksh Gradle
+#
+# Busybox and similar reduced shells will NOT work, because this script
+# requires all of these POSIX shell features:
+# * functions;
+# * expansions «$var», «${var}», «${var:-default}», «${var+SET}»,
+# «${var#prefix}», «${var%suffix}», and «$( cmd )»;
+# * compound commands having a testable exit status, especially «case»;
+# * various built-in commands including «command», «set», and «ulimit».
+#
+# Important for patching:
+#
+# (2) This script targets any POSIX shell, so it avoids extensions provided
+# by Bash, Ksh, etc; in particular arrays are avoided.
+#
+# The "traditional" practice of packing multiple parameters into a
+# space-separated string is a well documented source of bugs and security
+# problems, so this is (mostly) avoided, by progressively accumulating
+# options in "$@", and eventually passing that to Java.
+#
+# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS,
+# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly;
+# see the in-line comments for details.
+#
+# There are tweaks for specific operating systems such as AIX, CygWin,
+# Darwin, MinGW, and NonStop.
+#
+# (3) This script is generated from the Groovy template
+# https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+# within the Gradle project.
+#
+# You can find Gradle at https://github.com/gradle/gradle/.
+#
+##############################################################################
+
+# Attempt to set APP_HOME
+
+# Resolve links: $0 may be a link
+app_path=$0
+
+# Need this for daisy-chained symlinks.
+while
+ APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path
+ [ -h "$app_path" ]
+do
+ ls=$( ls -ld "$app_path" )
+ link=${ls#*' -> '}
+ case $link in #(
+ /*) app_path=$link ;; #(
+ *) app_path=$APP_HOME$link ;;
+ esac
+done
+
+# This is normally unused
+# shellcheck disable=SC2034
+APP_BASE_NAME=${0##*/}
+# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036)
+APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD=maximum
+
+warn () {
+ echo "$*"
+} >&2
+
+die () {
+ echo
+ echo "$*"
+ echo
+ exit 1
+} >&2
+
+# OS specific support (must be 'true' or 'false').
+cygwin=false
+msys=false
+darwin=false
+nonstop=false
+case "$( uname )" in #(
+ CYGWIN* ) cygwin=true ;; #(
+ Darwin* ) darwin=true ;; #(
+ MSYS* | MINGW* ) msys=true ;; #(
+ NONSTOP* ) nonstop=true ;;
+esac
+
+CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
+
+
+# Determine the Java command to use to start the JVM.
+if [ -n "$JAVA_HOME" ] ; then
+ if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
+ # IBM's JDK on AIX uses strange locations for the executables
+ JAVACMD=$JAVA_HOME/jre/sh/java
+ else
+ JAVACMD=$JAVA_HOME/bin/java
+ fi
+ if [ ! -x "$JAVACMD" ] ; then
+ die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+ fi
+else
+ JAVACMD=java
+ if ! command -v java >/dev/null 2>&1
+ then
+ die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+ fi
+fi
+
+# Increase the maximum file descriptors if we can.
+if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
+ case $MAX_FD in #(
+ max*)
+ # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked.
+ # shellcheck disable=SC2039,SC3045
+ MAX_FD=$( ulimit -H -n ) ||
+ warn "Could not query maximum file descriptor limit"
+ esac
+ case $MAX_FD in #(
+ '' | soft) :;; #(
+ *)
+ # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked.
+ # shellcheck disable=SC2039,SC3045
+ ulimit -n "$MAX_FD" ||
+ warn "Could not set maximum file descriptor limit to $MAX_FD"
+ esac
+fi
+
+# Collect all arguments for the java command, stacking in reverse order:
+# * args from the command line
+# * the main class name
+# * -classpath
+# * -D...appname settings
+# * --module-path (only if needed)
+# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables.
+
+# 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" )
+
+ # Now convert the arguments - kludge to limit ourselves to /bin/sh
+ for arg do
+ if
+ case $arg in #(
+ -*) false ;; # don't mess with options #(
+ /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath
+ [ -e "$t" ] ;; #(
+ *) false ;;
+ esac
+ then
+ arg=$( cygpath --path --ignore --mixed "$arg" )
+ fi
+ # Roll the args list around exactly as many times as the number of
+ # args, so each arg winds up back in the position where it started, but
+ # possibly modified.
+ #
+ # NB: a `for` loop captures its iteration list before it begins, so
+ # changing the positional parameters here affects neither the number of
+ # iterations, nor the values presented in `arg`.
+ shift # remove old arg
+ set -- "$@" "$arg" # push replacement arg
+ done
+fi
+
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+
+# Collect all arguments for the java command:
+# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments,
+# and any embedded shellness will be escaped.
+# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be
+# treated as '${Hostname}' itself on the command line.
+
+set -- \
+ "-Dorg.gradle.appname=$APP_BASE_NAME" \
+ -classpath "$CLASSPATH" \
+ org.gradle.wrapper.GradleWrapperMain \
+ "$@"
+
+# Stop when "xargs" is not available.
+if ! command -v xargs >/dev/null 2>&1
+then
+ die "xargs is not available"
+fi
+
+# Use "xargs" to parse quoted args.
+#
+# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
+#
+# In Bash we could simply go:
+#
+# readarray ARGS < <( xargs -n1 <<<"$var" ) &&
+# set -- "${ARGS[@]}" "$@"
+#
+# but POSIX shell has neither arrays nor command substitution, so instead we
+# post-process each arg (as a line of input to sed) to backslash-escape any
+# character that might be a shell metacharacter, then use eval to reverse
+# that process (while maintaining the separation between arguments), and wrap
+# the whole thing up as a single "set" statement.
+#
+# This will of course break if any of these variables contains a newline or
+# an unmatched quote.
+#
+
+eval "set -- $(
+ printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
+ xargs -n1 |
+ sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
+ tr '\n' ' '
+ )" '"$@"'
+
+exec "$JAVACMD" "$@"
diff --git a/lib/examples/swerve/gradlew.bat b/lib/examples/swerve/gradlew.bat
new file mode 100644
index 00000000..93e3f59f
--- /dev/null
+++ b/lib/examples/swerve/gradlew.bat
@@ -0,0 +1,92 @@
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+
+@if "%DEBUG%"=="" @echo off
+@rem ##########################################################################
+@rem
+@rem Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%"=="" set DIRNAME=.
+@rem This is normally unused
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Resolve any "." and ".." in APP_HOME to make it shorter.
+for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if %ERRORLEVEL% equ 0 goto execute
+
+echo.
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto execute
+
+echo.
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+
+: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
diff --git a/lib/examples/swerve/lvrt.sh b/lib/examples/swerve/lvrt.sh
new file mode 100644
index 00000000..ac3af941
--- /dev/null
+++ b/lib/examples/swerve/lvrt.sh
@@ -0,0 +1,17 @@
+#!/bin/sh
+exec 2>&1 >/dev/null
+while true; do
+ APP_PATH=`nirtcfg --file /etc/natinst/share/lvrt.conf --get section=LVRT,token=RTTarget.ApplicationPath`
+ if [ "$APP_PATH" = /home/lvuser/natinst/bin/TBLStartupApp.rtexe ]
+ then
+ APP_BOOT=`nirtcfg --file /etc/natinst/share/lvrt.conf --get section=LVRT,token=RTTarget.LaunchAppAtBoot,value=true | tr "[:upper:]" "[:lower:]"`
+ APP_DISABLED=`nirtcfg --get section=SYSTEMSETTINGS,token=NoApp.enabled,value=false | tr "[:upper:]" "[:lower:]"`
+ if [ "$APP_BOOT" = true ] && [ "$APP_DISABLED" = false ]
+ then
+ /usr/local/frc/bin/frcRunRobot.sh
+ fi
+ else
+ exec -a lvrt ./ni-lvrt
+ fi
+ sleep 1
+done
\ No newline at end of file
diff --git a/lib/examples/swerve/settings.gradle b/lib/examples/swerve/settings.gradle
new file mode 100644
index 00000000..8ea4b5d1
--- /dev/null
+++ b/lib/examples/swerve/settings.gradle
@@ -0,0 +1,30 @@
+import org.gradle.internal.os.OperatingSystem
+
+pluginManagement {
+ repositories {
+ mavenLocal()
+ gradlePluginPortal()
+ String frcYear = '2026'
+ File frcHome
+ if (OperatingSystem.current().isWindows()) {
+ String publicFolder = System.getenv('PUBLIC')
+ if (publicFolder == null) {
+ publicFolder = "C:\\Users\\Public"
+ }
+ def homeRoot = new File(publicFolder, "wpilib")
+ frcHome = new File(homeRoot, frcYear)
+ } else {
+ def userFolder = System.getProperty("user.home")
+ def homeRoot = new File(userFolder, "wpilib")
+ frcHome = new File(homeRoot, frcYear)
+ }
+ def frcHomeMaven = new File(frcHome, 'maven')
+ maven {
+ name 'frcHome'
+ url frcHomeMaven
+ }
+ }
+}
+
+Properties props = System.getProperties()
+props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true")
diff --git a/lib/examples/swerve/src/main/deploy/choreo/DEPOT.traj b/lib/examples/swerve/src/main/deploy/choreo/DEPOT.traj
new file mode 100644
index 00000000..16e01b16
--- /dev/null
+++ b/lib/examples/swerve/src/main/deploy/choreo/DEPOT.traj
@@ -0,0 +1,153 @@
+{
+ "name":"DEPOT",
+ "version":3,
+ "snapshot":{
+ "waypoints":[
+ {"x":3.7033300399780273, "y":5.980339527130127, "heading":3.141592653589793, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":1.141133427619934, "y":5.967901706695557, "heading":3.127100394245751, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":0.4082718193531037, "y":5.990222454071045, "heading":-3.1415914918446664, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":2.01178240776062, "y":5.955463409423828, "heading":0.0, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":1.9869070053100584, "y":3.828591346740722, "heading":1.5874614566235736, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true},
+ {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true},
+ {"from":1, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}],
+ "targetDt":0.05
+ },
+ "params":{
+ "waypoints":[
+ {"x":{"exp":"3.7033300399780273 m", "val":3.7033300399780273}, "y":{"exp":"5.980339527130127 m", "val":5.980339527130127}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"1.141133427619934 m", "val":1.141133427619934}, "y":{"exp":"5.967901706695557 m", "val":5.967901706695557}, "heading":{"exp":"3.127100394245751 rad", "val":3.127100394245751}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"0.40827181935310364 m", "val":0.4082718193531037}, "y":{"exp":"5.990222454071045 m", "val":5.990222454071045}, "heading":{"exp":"-3.1415914918446664 rad", "val":-3.1415914918446664}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":{"exp":"2.01178240776062 m", "val":2.01178240776062}, "y":{"exp":"5.955463409423828 m", "val":5.955463409423828}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":{"exp":"1.9869070053100586 m", "val":1.9869070053100584}, "y":{"exp":"3.8285913467407227 m", "val":3.828591346740722}, "heading":{"exp":"1.5874614566235736 rad", "val":1.5874614566235736}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true},
+ {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true},
+ {"from":1, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}],
+ "targetDt":{
+ "exp":"0.05 s",
+ "val":0.05
+ }
+ },
+ "trajectory":{
+ "config":{
+ "frontLeft":{
+ "x":0.225425,
+ "y":0.333375
+ },
+ "backLeft":{
+ "x":-0.225425,
+ "y":0.333375
+ },
+ "mass":68.0388555,
+ "inertia":7.5,
+ "gearing":7.03,
+ "radius":0.0508,
+ "vmax":607.3745796940267,
+ "tmax":1.2,
+ "cof":1.5,
+ "bumper":{
+ "front":0.34925,
+ "side":0.4445,
+ "back":0.34925
+ },
+ "differentialTrackWidth":0.5588
+ },
+ "sampleType":"Swerve",
+ "waypoints":[0.0,0.94415,1.72511,2.4094,3.26434],
+ "samples":[
+ {"t":0.0, "x":3.70333, "y":5.98034, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-9.75303, "ay":-0.03176, "alpha":-0.09915, "fx":[-165.90006,-165.89478,-165.89222,-165.89765], "fy":[0.28066,-1.35304,-1.37302,0.2848]},
+ {"t":0.04292, "x":3.69435, "y":5.98031, "heading":3.14159, "vx":-0.41856, "vy":-0.00136, "omega":-0.00426, "ax":-9.75196, "ay":-0.03139, "alpha":-0.09884, "fx":[-165.88194,-165.87675,-165.87392,-165.87928], "fy":[0.28405,-1.34414,-1.36391,0.28823]},
+ {"t":0.08583, "x":3.66741, "y":5.98022, "heading":3.14141, "vx":-0.83707, "vy":-0.00271, "omega":-0.0085, "ax":-9.75062, "ay":-0.03102, "alpha":-0.09845, "fx":[-165.8593,-165.85418,-165.85104,-165.85631], "fy":[0.28702,-1.33429,-1.35428,0.29078]},
+ {"t":0.12875, "x":3.6225, "y":5.98008, "heading":3.14105, "vx":-1.25553, "vy":-0.00404, "omega":-0.01272, "ax":-9.7489, "ay":-0.03065, "alpha":-0.09795, "fx":[-165.83017,-165.82515,-165.8216,-165.82678], "fy":[0.28926,-1.32319,-1.34379,0.29215]},
+ {"t":0.17166, "x":3.55964, "y":5.97988, "heading":3.1405, "vx":-1.67391, "vy":-0.00536, "omega":-0.01693, "ax":-9.7466, "ay":-0.03028, "alpha":-0.09727, "fx":[-165.79133,-165.78642,-165.78232,-165.7874], "fy":[0.29032,-1.31034,-1.33191,0.29189]},
+ {"t":0.21458, "x":3.47883, "y":5.97962, "heading":3.13977, "vx":-2.09219, "vy":-0.00666, "omega":-0.0211, "ax":-9.74338, "ay":-0.02989, "alpha":-0.09634, "fx":[-165.73694,-165.73215,-165.7273,-165.73227], "fy":[0.28934,-1.29483,-1.31768,0.28916]},
+ {"t":0.25749, "x":3.38007, "y":5.97931, "heading":3.13887, "vx":-2.51034, "vy":-0.00794, "omega":-0.02523, "ax":-9.73856, "ay":-0.0295, "alpha":-0.09493, "fx":[-165.65533,-165.65069,-165.64475,-165.64959], "fy":[0.28469,-1.27489,-1.29922,0.28233]},
+ {"t":0.30041, "x":3.26337, "y":5.97894, "heading":3.13778, "vx":-2.92827, "vy":-0.00921, "omega":-0.02931, "ax":-9.73052, "ay":-0.02908, "alpha":-0.09261, "fx":[-165.51927,-165.51484,-165.50718,-165.51182], "fy":[0.27258,-1.24644,-1.2722,0.26765]},
+ {"t":0.34333, "x":3.12874, "y":5.97852, "heading":3.13653, "vx":-3.34587, "vy":-0.01045, "omega":-0.03328, "ax":-9.71444, "ay":-0.02859, "alpha":-0.08808, "fx":[-165.24724,-165.24314,-165.23232,-165.23663], "fy":[0.24218,-1.19771,-1.22419,0.2344]},
+ {"t":0.38624, "x":2.9762, "y":5.97804, "heading":3.1351, "vx":-3.76277, "vy":-0.01168, "omega":-0.03706, "ax":-9.66645, "ay":-0.02785, "alpha":-0.07537, "fx":[-164.43421,-164.43087,-164.41273,-164.41623], "fy":[0.14487,-1.07543,-1.0991,0.13461]},
+ {"t":0.42916, "x":2.80582, "y":5.97751, "heading":3.13351, "vx":-4.17761, "vy":-0.01288, "omega":-0.0403, "ax":-4.82767, "ay":0.00382, "alpha":0.93655, "fx":[-79.12989,-79.09411,-85.10537,-85.13952], "fy":[-3.39746,3.45052,3.3697,-3.16305]},
+ {"t":0.47207, "x":2.62209, "y":5.97696, "heading":3.13178, "vx":-4.3848, "vy":-0.01271, "omega":-0.0001, "ax":-0.00128, "ay":0.00422, "alpha":0.00029, "fx":[-0.02063,-0.02064,-0.02284,-0.02283], "fy":[0.07101,0.0725,0.07248,0.07099]},
+ {"t":0.51499, "x":2.43391, "y":5.97642, "heading":3.13177, "vx":-4.38485, "vy":-0.01253, "omega":-0.00009, "ax":0.00029, "ay":-0.03094, "alpha":-0.00007, "fx":[0.0047,0.00472,0.00523,0.00521], "fy":[-0.52606,-0.52641,-0.52638,-0.52604]},
+ {"t":0.55791, "x":2.24573, "y":5.97586, "heading":3.13177, "vx":-4.38484, "vy":-0.01386, "omega":-0.0001, "ax":1.05143, "ay":-0.24558, "alpha":-0.41961, "fx":[16.29224,16.26004,19.47581,19.50981], "fy":[-3.05602,-5.28375,-5.29398,-3.07554]},
+ {"t":0.60082, "x":2.05852, "y":5.97504, "heading":3.13177, "vx":-4.33972, "vy":-0.0244, "omega":-0.0181, "ax":9.65858, "ay":0.01752, "alpha":-0.04815, "fx":[164.28282,164.28417,164.29659,164.2953], "fy":[0.69727,-0.08788,-0.09861,0.6811]},
+ {"t":0.64374, "x":1.88117, "y":5.97401, "heading":3.13099, "vx":-3.92521, "vy":-0.02365, "omega":-0.02017, "ax":9.71258, "ay":0.02128, "alpha":0.02972, "fx":[165.21059,165.20957,165.2058,165.20686], "fy":[0.11346,0.60114,0.61144,0.12163]},
+ {"t":0.68665, "x":1.72166, "y":5.97301, "heading":3.13012, "vx":-3.50839, "vy":-0.02273, "omega":-0.01889, "ax":9.72974, "ay":0.02228, "alpha":0.05453, "fx":[165.50336,165.5014,165.49678,165.49885], "fy":[-0.07843,0.81805,0.83996,-0.06373]},
+ {"t":0.72957, "x":1.58006, "y":5.97205, "heading":3.12931, "vx":-3.09083, "vy":-0.02178, "omega":-0.01655, "ax":9.73815, "ay":0.0226, "alpha":0.06668, "fx":[165.64653,165.6441,165.63982,165.64243], "fy":[-0.17608,0.92118,0.9504,-0.15769]},
+ {"t":0.77248, "x":1.45638, "y":5.97114, "heading":3.1286, "vx":-2.67291, "vy":-0.02081, "omega":-0.01369, "ax":9.74315, "ay":0.02265, "alpha":0.07388, "fx":[165.73137,165.72868,165.72484,165.72777], "fy":[-0.23671,0.9797,1.01406,-0.21567]},
+ {"t":0.8154, "x":1.35064, "y":5.97027, "heading":3.12801, "vx":-2.25477, "vy":-0.01983, "omega":-0.01052, "ax":9.74645, "ay":0.02257, "alpha":0.07864, "fx":[165.78749,165.78464,165.78119,165.78431], "fy":[-0.27898,1.01625,1.05442,-0.25592]},
+ {"t":0.85832, "x":1.26285, "y":5.96944, "heading":3.12756, "vx":-1.8365, "vy":-0.01887, "omega":-0.00715, "ax":9.7488, "ay":0.02241, "alpha":0.08201, "fx":[165.82734,165.8244,165.82127,165.82452], "fy":[-0.31078,1.0404,1.08145,-0.28619]},
+ {"t":0.90123, "x":1.19301, "y":5.96865, "heading":3.12726, "vx":-1.41812, "vy":-0.0179, "omega":-0.00363, "ax":9.75056, "ay":0.0222, "alpha":0.08453, "fx":[165.85711,165.85411,165.85125,165.85458], "fy":[-0.33601,1.05694,1.10012,-0.31033]},
+ {"t":0.94415, "x":1.14113, "y":5.9679, "heading":3.1271, "vx":-0.99966, "vy":-0.01695, "omega":0.0, "ax":0.04698, "ay":-1.09887, "alpha":0.0, "fx":[0.79911,0.79911,0.79911,0.79911], "fy":[-18.69149,-18.69149,-18.69149,-18.69149]},
+ {"t":0.99009, "x":1.09526, "y":5.96596, "heading":3.1271, "vx":-0.99751, "vy":-0.06743, "omega":0.0, "ax":0.00061, "ay":-0.00899, "alpha":0.0, "fx":[0.01037,0.01037,0.01037,0.01037], "fy":[-0.15299,-0.15299,-0.15299,-0.15299]},
+ {"t":1.03603, "x":1.04944, "y":5.96286, "heading":3.1271, "vx":-0.99748, "vy":-0.06785, "omega":0.0, "ax":-0.00002, "ay":0.00034, "alpha":0.0, "fx":[-0.0004,-0.0004,-0.0004,-0.0004], "fy":[0.00584,0.00584,0.00584,0.00584]},
+ {"t":1.08196, "x":1.00361, "y":5.95974, "heading":3.1271, "vx":-0.99748, "vy":-0.06783, "omega":0.0, "ax":-0.00003, "ay":0.00043, "alpha":0.0, "fx":[-0.0005,-0.0005,-0.0005,-0.0005], "fy":[0.00724,0.00724,0.00724,0.00724]},
+ {"t":1.1279, "x":0.95779, "y":5.95662, "heading":3.1271, "vx":-0.99748, "vy":-0.06781, "omega":0.0, "ax":-0.00003, "ay":0.00043, "alpha":0.0, "fx":[-0.0005,-0.0005,-0.0005,-0.0005], "fy":[0.00733,0.00733,0.00733,0.00733]},
+ {"t":1.17384, "x":0.91197, "y":5.95351, "heading":3.1271, "vx":-0.99748, "vy":-0.06779, "omega":0.0, "ax":-0.00003, "ay":0.00044, "alpha":0.0, "fx":[-0.00051,-0.00051,-0.00051,-0.00051], "fy":[0.00741,0.00741,0.00741,0.00741]},
+ {"t":1.21978, "x":0.86614, "y":5.9504, "heading":3.1271, "vx":-0.99748, "vy":-0.06777, "omega":0.0, "ax":-0.00003, "ay":0.00044, "alpha":0.0, "fx":[-0.00052,-0.00052,-0.00052,-0.00052], "fy":[0.00751,0.00751,0.00751,0.00751]},
+ {"t":1.26572, "x":0.82032, "y":5.94728, "heading":3.1271, "vx":-0.99749, "vy":-0.06775, "omega":0.0, "ax":-0.00003, "ay":0.00045, "alpha":0.0, "fx":[-0.00052,-0.00052,-0.00052,-0.00052], "fy":[0.00763,0.00763,0.00763,0.00763]},
+ {"t":1.31166, "x":0.7745, "y":5.94417, "heading":3.1271, "vx":-0.99749, "vy":-0.06773, "omega":0.0, "ax":-0.00003, "ay":0.00046, "alpha":0.0, "fx":[-0.00053,-0.00053,-0.00053,-0.00053], "fy":[0.00778,0.00778,0.00778,0.00778]},
+ {"t":1.3576, "x":0.72867, "y":5.94106, "heading":3.1271, "vx":-0.99749, "vy":-0.06771, "omega":0.0, "ax":-0.00003, "ay":0.00047, "alpha":0.0, "fx":[-0.00055,-0.00055,-0.00055,-0.00055], "fy":[0.00797,0.00797,0.00797,0.00797]},
+ {"t":1.40354, "x":0.68285, "y":5.93795, "heading":3.1271, "vx":-0.99749, "vy":-0.06769, "omega":0.0, "ax":-0.00003, "ay":0.00048, "alpha":0.0, "fx":[-0.00057,-0.00057,-0.00057,-0.00057], "fy":[0.00822,0.00822,0.00822,0.00822]},
+ {"t":1.44947, "x":0.63703, "y":5.93484, "heading":3.1271, "vx":-0.99749, "vy":-0.06766, "omega":0.0, "ax":-0.00004, "ay":0.00058, "alpha":0.0, "fx":[-0.00067,-0.00067,-0.00067,-0.00067], "fy":[0.0098,0.0098,0.0098,0.0098]},
+ {"t":1.49541, "x":0.5912, "y":5.93173, "heading":3.1271, "vx":-0.99749, "vy":-0.06764, "omega":0.0, "ax":-0.00063, "ay":0.00937, "alpha":0.0, "fx":[-0.01078,-0.01078,-0.01078,-0.01078], "fy":[0.1594,0.1594,0.1594,0.1594]},
+ {"t":1.54135, "x":0.54538, "y":5.92864, "heading":3.1271, "vx":-0.99752, "vy":-0.06721, "omega":0.0, "ax":-0.04497, "ay":1.03556, "alpha":0.0, "fx":[-0.765,-0.765,-0.765,-0.765], "fy":[17.61461,17.61461,17.61461,17.61461]},
+ {"t":1.58729, "x":0.49951, "y":5.92664, "heading":3.1271, "vx":-0.99959, "vy":-0.01963, "omega":0.0, "ax":1.85009, "ay":9.20613, "alpha":0.0, "fx":[31.46956,31.46956,31.46956,31.46956], "fy":[156.59357,156.59357,156.59357,156.59357]},
+ {"t":1.63323, "x":0.45554, "y":5.93545, "heading":3.1271, "vx":-0.9146, "vy":0.40328, "omega":0.0, "ax":8.4588, "ay":4.8153, "alpha":0.0, "fx":[143.88173,143.88173,143.88173,143.88173], "fy":[81.90682,81.90682,81.90682,81.90682]},
+ {"t":1.67917, "x":0.42245, "y":5.95906, "heading":3.1271, "vx":-0.52601, "vy":0.62449, "omega":0.0, "ax":9.4653, "ay":2.34333, "alpha":0.0, "fx":[161.00202,161.00202,161.00202,161.00202], "fy":[39.85944,39.85944,39.85944,39.85944]},
+ {"t":1.72511, "x":0.40827, "y":5.99022, "heading":3.1271, "vx":-0.09119, "vy":0.73214, "omega":0.0, "ax":9.58027, "ay":1.82225, "alpha":0.0, "fx":[162.9577,162.9577,162.9577,162.9577], "fy":[30.99602,30.99602,30.99602,30.99602]},
+ {"t":1.76536, "x":0.41236, "y":6.02117, "heading":3.1271, "vx":0.29445, "vy":0.80549, "omega":0.0, "ax":9.63169, "ay":1.51949, "alpha":0.0, "fx":[163.83224,163.83224,163.83224,163.83224], "fy":[25.84611,25.84611,25.84611,25.84611]},
+ {"t":1.80561, "x":0.43202, "y":6.05482, "heading":3.1271, "vx":0.68215, "vy":0.86666, "omega":0.0, "ax":9.68275, "ay":1.13706, "alpha":0.0, "fx":[164.70079,164.70079,164.70079,164.70079], "fy":[19.34105,19.34105,19.34105,19.34105]},
+ {"t":1.84587, "x":0.46732, "y":6.09063, "heading":3.1271, "vx":1.07191, "vy":0.91243, "omega":0.0, "ax":9.72619, "ay":0.64204, "alpha":0.0, "fx":[165.43967,165.43967,165.43967,165.43967], "fy":[10.92085,10.92085,10.92085,10.92085]},
+ {"t":1.88612, "x":0.51835, "y":6.12788, "heading":3.1271, "vx":1.46341, "vy":0.93827, "omega":0.0, "ax":9.74485, "ay":-0.01709, "alpha":0.0, "fx":[165.75707,165.75707,165.75707,165.75707], "fy":[-0.29068,-0.29068,-0.29068,-0.29068]},
+ {"t":1.92637, "x":0.58515, "y":6.16563, "heading":3.1271, "vx":1.85567, "vy":0.93758, "omega":0.0, "ax":9.69785, "ay":-0.92196, "alpha":0.0, "fx":[164.95766,164.95766,164.95766,164.95766], "fy":[-15.68234,-15.68234,-15.68234,-15.68234]},
+ {"t":1.96662, "x":0.6677, "y":6.20263, "heading":3.1271, "vx":2.24603, "vy":0.90047, "omega":0.0, "ax":9.48599, "ay":-2.19757, "alpha":0.0, "fx":[161.35399,161.35399,161.35399,161.35399], "fy":[-37.38012,-37.38012,-37.38012,-37.38012]},
+ {"t":2.00688, "x":0.7658, "y":6.23709, "heading":3.1271, "vx":2.62787, "vy":0.81201, "omega":0.0, "ax":8.87245, "ay":-3.99806, "alpha":0.0, "fx":[150.91776,150.91776,150.91776,150.91776], "fy":[-68.00594,-68.00594,-68.00594,-68.00594]},
+ {"t":2.04713, "x":0.87876, "y":6.26654, "heading":3.1271, "vx":2.98501, "vy":0.65108, "omega":0.0, "ax":7.38032, "ay":-6.33403, "alpha":0.0, "fx":[125.53714,125.53714,125.53714,125.53714], "fy":[-107.74008,-107.74008,-107.74008,-107.74008]},
+ {"t":2.08738, "x":1.0049, "y":6.28761, "heading":3.1271, "vx":3.28209, "vy":0.39612, "omega":0.0, "ax":4.56235, "ay":-8.58558, "alpha":0.0, "fx":[77.60432,77.60432,77.60432,77.60432], "fy":[-146.03829,-146.03829,-146.03829,-146.03829]},
+ {"t":2.12764, "x":1.14071, "y":6.2966, "heading":3.1271, "vx":3.46574, "vy":0.05052, "omega":0.0, "ax":1.04918, "ay":-9.66822, "alpha":0.0, "fx":[17.84626,17.84626,17.84626,17.84626], "fy":[-164.4537,-164.4537,-164.4537,-164.4537]},
+ {"t":2.16789, "x":1.28106, "y":6.29081, "heading":3.1271, "vx":3.50797, "vy":-0.33865, "omega":0.0, "ax":-1.84957, "ay":-9.55339, "alpha":0.0, "fx":[-31.4607,-31.4607,-31.4607,-31.4607], "fy":[-162.50035,-162.50035,-162.50035,-162.50035]},
+ {"t":2.20814, "x":1.42077, "y":6.26943, "heading":3.1271, "vx":3.43352, "vy":-0.7232, "omega":0.0, "ax":-3.7718, "ay":-8.97623, "alpha":0.0, "fx":[-64.1572,-64.1572,-64.1572,-64.1572], "fy":[-152.68317,-152.68317,-152.68317,-152.68317]},
+ {"t":2.24839, "x":1.55592, "y":6.23305, "heading":3.1271, "vx":3.28169, "vy":-1.08452, "omega":0.0, "ax":-4.98969, "ay":-8.36603, "alpha":0.0, "fx":[-84.87317,-84.87317,-84.87317,-84.87317], "fy":[-142.30373,-142.30373,-142.30373,-142.30373]},
+ {"t":2.28865, "x":1.68398, "y":6.18262, "heading":3.1271, "vx":3.08085, "vy":-1.42127, "omega":0.0, "ax":-5.78309, "ay":-7.84282, "alpha":0.0, "fx":[-98.36874,-98.36874,-98.36874,-98.36874], "fy":[-133.40412,-133.40412,-133.40412,-133.40412]},
+ {"t":2.3289, "x":1.8033, "y":6.11906, "heading":3.1271, "vx":2.84806, "vy":-1.73697, "omega":0.0, "ax":-6.32408, "ay":-7.4169, "alpha":0.0, "fx":[-107.5708,-107.5708,-107.5708,-107.5708], "fy":[-126.15934,-126.15934,-126.15934,-126.15934]},
+ {"t":2.36915, "x":1.91282, "y":6.04313, "heading":3.1271, "vx":2.5935, "vy":-2.03552, "omega":0.0, "ax":-6.70971, "ay":-7.0727, "alpha":0.0, "fx":[-114.1303,-114.1303,-114.1303,-114.1303], "fy":[-120.30455,-120.30455,-120.30455,-120.30455]},
+ {"t":2.4094, "x":2.01178, "y":5.95546, "heading":3.1271, "vx":2.32341, "vy":-2.32021, "omega":0.0, "ax":-5.9679, "ay":-6.77454, "alpha":-9.62165, "fx":[-158.2226,-126.2258,-37.23442,-84.36622], "fy":[-49.58274,-107.5984,-161.53004,-142.22086]},
+ {"t":2.43889, "x":2.07768, "y":5.88412, "heading":3.1271, "vx":2.14748, "vy":-2.51993, "omega":-0.28365, "ax":-6.22896, "ay":-6.60978, "alpha":-9.13493, "fx":[-158.59772,-127.86037,-43.0544,-94.29898], "fy":[-48.28734,-105.61735,-160.04118,-135.77615]},
+ {"t":2.46837, "x":2.13829, "y":5.80696, "heading":3.11874, "vx":1.96384, "vy":-2.71479, "omega":-0.55296, "ax":-6.5172, "ay":-6.39924, "alpha":-8.67285, "fx":[-159.18867,-130.14943,-50.27634,-103.80838], "fy":[-46.1962,-102.74128,-157.87795,-128.58164]},
+ {"t":2.49785, "x":2.19335, "y":5.72414, "heading":3.10244, "vx":1.77171, "vy":-2.90345, "omega":-0.80864, "ax":-6.84461, "ay":-6.12749, "alpha":-8.19268, "fx":[-159.9767,-133.13028,-59.25873,-113.33388], "fy":[-43.24795,-98.79352,-154.67647,-120.18921]},
+ {"t":2.52733, "x":2.24261, "y":5.63588, "heading":3.0786, "vx":1.56993, "vy":-3.08409, "omega":-1.05017, "ax":-7.22531, "ay":-5.76879, "alpha":-7.64447, "fx":[-160.93522,-136.86214,-70.53855,-123.2658], "fy":[-39.33667,-93.48113,-149.79879,-109.88545]},
+ {"t":2.55681, "x":2.28575, "y":5.54246, "heading":3.04764, "vx":1.35692, "vy":-3.25416, "omega":-1.27553, "ax":-7.67403, "ay":-5.28138, "alpha":-6.96355, "fx":[-162.02471,-141.42947,-84.87796,-133.80038], "fy":[-34.28798,-86.30957,-142.07025,-96.67093]},
+ {"t":2.58629, "x":2.32242, "y":5.44423, "heading":3.01003, "vx":1.13068, "vy":-3.40986, "omega":-1.48082, "ax":-8.20109, "ay":-4.59726, "alpha":-6.05807, "fx":[-163.1807,-146.9306,-103.19642,-144.68479], "fy":[-27.81337,-76.40591,-129.24053,-79.33242]},
+ {"t":2.61577, "x":2.35219, "y":5.3417, "heading":2.96638, "vx":0.88891, "vy":-3.54539, "omega":-1.65942, "ax":-8.79525, "ay":-3.60722, "alpha":-4.79665, "fx":[-164.2832,-153.38937,-125.87916,-154.86677], "fy":[-19.42086,-62.14523,-107.05653,-56.80843]},
+ {"t":2.64525, "x":2.37457, "y":5.23561, "heading":2.91746, "vx":0.62962, "vy":-3.65173, "omega":-1.80082, "ax":-9.37004, "ay":-2.16452, "alpha":-3.06979, "fx":[-165.07286,-160.29083,-149.83644,-162.32639], "fy":[-8.24256,-40.53617,-69.3038,-29.18892]},
+ {"t":2.67473, "x":2.38906, "y":5.12702, "heading":2.86437, "vx":0.35338, "vy":-3.71554, "omega":-1.89132, "ax":-9.66296, "ay":-0.47806, "alpha":-1.55074, "fx":[-164.93844,-164.6331,-163.02442,-164.8609], "fy":[6.98388,-12.68236,-25.01402,-1.81437]},
+ {"t":2.70421, "x":2.39528, "y":5.01727, "heading":2.80861, "vx":0.06851, "vy":-3.72964, "omega":-1.93704, "ax":-9.63608, "ay":0.54738, "alpha":-1.80678, "fx":[-162.80398,-164.91789,-164.3415,-163.56433], "fy":[26.24697,3.04869,-10.40675,18.35398]},
+ {"t":2.73369, "x":2.39311, "y":4.90756, "heading":2.75151, "vx":-0.21556, "vy":-3.7135, "omega":-1.99031, "ax":-9.45559, "ay":1.59404, "alpha":-2.69027, "fx":[-157.02956,-163.86996,-164.24786,-158.20011], "fy":[49.73084,17.1615,-2.11677,43.68077]},
+ {"t":2.76317, "x":2.38265, "y":4.79878, "heading":2.69283, "vx":-0.49432, "vy":-3.66651, "omega":-2.06962, "ax":-9.00544, "ay":2.84238, "alpha":-4.27347, "fx":[-145.51414,-161.35878,-163.55181,-142.29539], "fy":[76.77915,32.37511,3.98101,80.25676]},
+ {"t":2.79265, "x":2.36416, "y":4.69192, "heading":2.63182, "vx":-0.75981, "vy":-3.58271, "omega":-2.1956, "ax":-7.98278, "ay":4.38014, "alpha":-7.00269, "fx":[-126.30498,-156.30916,-161.93137,-98.59381], "fy":[105.18263,50.8421,12.93244,129.06252]},
+ {"t":2.82213, "x":2.33829, "y":4.5882, "heading":2.56709, "vx":-0.99515, "vy":-3.45358, "omega":-2.40205, "ax":-6.17213, "ay":5.9355, "alpha":-10.36658, "fx":[-98.87944,-146.45829,-155.69908,-18.90777], "fy":[131.17126,74.15216,37.57254,160.94867]},
+ {"t":2.85162, "x":2.30627, "y":4.48897, "heading":2.49627, "vx":-1.17711, "vy":-3.2786, "omega":-2.70766, "ax":-4.01683, "ay":6.97646, "alpha":-12.69006, "fx":[-67.02797,-132.82502,-132.55122,59.10397], "fy":[149.96806,96.21382,77.11218,151.3766]},
+ {"t":2.8811, "x":2.26983, "y":4.39534, "heading":2.41645, "vx":-1.29552, "vy":-3.07293, "omega":-3.08177, "ax":-0.34937, "ay":8.55357, "alpha":-11.07355, "fx":[-30.65918,-101.71791,20.39353,88.2126], "fy":[161.42691,128.50169,154.68873,137.35753]},
+ {"t":2.91058, "x":2.23148, "y":4.30847, "heading":2.3256, "vx":-1.30582, "vy":-2.82076, "omega":-3.40823, "ax":1.59761, "ay":9.52496, "alpha":0.74197, "fx":[29.17513,33.2747,25.2491,21.00086], "fy":[161.67052,160.93927,162.45445,163.003]},
+ {"t":2.94006, "x":2.19368, "y":4.22945, "heading":2.22512, "vx":-1.25873, "vy":-2.53996, "omega":-3.38635, "ax":2.83092, "ay":8.95992, "alpha":6.54608, "fx":[71.4913,95.06673,32.42521,-6.37103], "fy":[148.01543,134.79297,161.96722,164.8473]},
+ {"t":2.96954, "x":2.1578, "y":4.15846, "heading":2.12529, "vx":-1.17527, "vy":-2.27581, "omega":-3.19337, "ax":3.51483, "ay":8.41599, "alpha":9.09785, "fx":[94.91377,120.36955,39.98687,-16.12508], "fy":[134.32075,113.21959,160.56392,164.50992]},
+ {"t":2.99902, "x":2.12468, "y":4.09503, "heading":2.03115, "vx":-1.07165, "vy":-2.0277, "omega":-2.92516, "ax":3.94388, "ay":8.02729, "alpha":10.23151, "fx":[105.95459,133.88009,47.26326,-18.76076], "fy":[125.8602,97.15522,158.72962,164.42292]},
+ {"t":3.0285, "x":2.0948, "y":4.03874, "heading":1.94491, "vx":-0.95538, "vy":-1.79105, "omega":-2.62353, "ax":4.21824, "ay":7.77393, "alpha":10.64812, "fx":[108.90806,142.41963,53.97943,-18.30317], "fy":[123.31836,84.34513,156.66864,164.59706]},
+ {"t":3.05798, "x":2.06847, "y":3.98931, "heading":1.86757, "vx":-0.83102, "vy":-1.56187, "omega":-2.30961, "ax":4.37092, "ay":7.63037, "alpha":10.69562, "fx":[105.51601,148.34576,60.02446,-16.49387], "fy":[126.1894,73.58237,154.51852,164.87137]},
+ {"t":3.08746, "x":2.04587, "y":3.94659, "heading":1.79948, "vx":-0.70217, "vy":-1.33693, "omega":-1.9943, "ax":4.40758, "ay":7.57339, "alpha":10.60339, "fx":[96.02209,152.66818,65.36339,-14.16728], "fy":[133.48691,64.26718,152.38402,165.14683]},
+ {"t":3.11694, "x":2.02709, "y":3.91046, "heading":1.74068, "vx":-0.57223, "vy":-1.11366, "omega":-1.6817, "ax":4.3262, "ay":7.57397, "alpha":10.56761, "fx":[80.21112,155.89122,69.99579,-11.74871], "fy":[143.48066,56.11471,150.34825,165.38036]},
+ {"t":3.14642, "x":2.0121, "y":3.88092, "heading":1.69111, "vx":-0.44469, "vy":-0.89037, "omega":-1.37016, "ax":4.13776, "ay":7.59191, "alpha":10.75621, "fx":[58.76247,158.29962,73.93249,-9.46628], "fy":[153.48733,49.01746,148.48003,165.56011]},
+ {"t":3.1759, "x":2.00078, "y":3.85797, "heading":1.65071, "vx":-0.3227, "vy":-0.66656, "omega":-1.05306, "ax":3.88424, "ay":7.58699, "alpha":11.23683, "fx":[34.48087,160.07594,77.18057,-7.45845], "fy":[160.71569,42.96547,146.84041,165.68873]},
+ {"t":3.20538, "x":1.99296, "y":3.84162, "heading":1.61967, "vx":-0.20819, "vy":-0.44289, "omega":-0.72179, "ax":3.63056, "ay":7.5438, "alpha":11.91068, "fx":[11.76645,161.35282,79.73373,-5.8337], "fy":[164.01727,37.99115,145.4883,165.77451]},
+ {"t":3.23486, "x":1.9884, "y":3.83184, "heading":1.59839, "vx":-0.10116, "vy":-0.22049, "omega":-0.37066, "ax":3.43148, "ay":7.4791, "alpha":12.57291, "fx":[-5.63216,162.23384,81.5694,-4.69735], "fy":[164.42428,34.13534,144.48244,165.82715]},
+ {"t":3.26434, "x":1.98691, "y":3.82859, "heading":1.58746, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+ "splits":[0]
+ },
+ "events":[]
+}
diff --git a/lib/examples/swerve/src/main/deploy/choreo/LEFT_TRENCH.traj b/lib/examples/swerve/src/main/deploy/choreo/LEFT_TRENCH.traj
new file mode 100644
index 00000000..863d839f
--- /dev/null
+++ b/lib/examples/swerve/src/main/deploy/choreo/LEFT_TRENCH.traj
@@ -0,0 +1,387 @@
+{
+ "name":"LEFT_TRENCH",
+ "version":3,
+ "snapshot":{
+ "waypoints":[
+ {"x":4.441253185272217, "y":7.693458557128906, "heading":-1.5707972442597578, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":7.795207023620605, "y":7.227133750915527, "heading":-1.5707963267948966, "intervals":56, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":7.795207023620605, "y":4.531103515625, "heading":-1.5707963267948966, "intervals":129, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":5.723647117614746, "y":7.42442512512207, "heading":0.0, "intervals":51, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":3.2933757305145264, "y":7.325779438018799, "heading":-1.5707963267948966, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":2.809114933013916, "y":5.299058437347412, "heading":0.8379811662545806, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true},
+ {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.00001}}, "enabled":true},
+ {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true},
+ {"from":3, "to":4, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true},
+ {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true},
+ {"from":2, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":0.5}}, "enabled":true},
+ {"from":3, "to":4, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}],
+ "targetDt":0.05
+ },
+ "params":{
+ "waypoints":[
+ {"x":{"exp":"4.441253185272217 m", "val":4.441253185272217}, "y":{"exp":"7.693458557128906 m", "val":7.693458557128906}, "heading":{"exp":"-1.5707972442597578 rad", "val":-1.5707972442597578}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"7.7952070236206055 m", "val":7.795207023620605}, "y":{"exp":"7.227133750915527 m", "val":7.227133750915527}, "heading":{"exp":"-1.5707963267948966 rad", "val":-1.5707963267948966}, "intervals":56, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"7.7952070236206055 m", "val":7.795207023620605}, "y":{"exp":"4.531103515625 m", "val":4.531103515625}, "heading":{"exp":"-1.5707963267948966 rad", "val":-1.5707963267948966}, "intervals":129, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":{"exp":"5.723647117614746 m", "val":5.723647117614746}, "y":{"exp":"7.42442512512207 m", "val":7.42442512512207}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":51, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"3.2933757305145264 m", "val":3.2933757305145264}, "y":{"exp":"7.325779438018799 m", "val":7.325779438018799}, "heading":{"exp":"-90 deg", "val":-1.5707963267948966}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":{"exp":"2.809114933013916 m", "val":2.809114933013916}, "y":{"exp":"5.299058437347412 m", "val":5.299058437347412}, "heading":{"exp":"0.8379811662545806 rad", "val":0.8379811662545806}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true},
+ {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"1e-5 m", "val":0.00001}}}, "enabled":true},
+ {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true},
+ {"from":3, "to":4, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true},
+ {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true},
+ {"from":2, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0.5 rad / s", "val":0.5}}}, "enabled":true},
+ {"from":3, "to":4, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}],
+ "targetDt":{
+ "exp":"0.05 s",
+ "val":0.05
+ }
+ },
+ "trajectory":{
+ "config":{
+ "frontLeft":{
+ "x":0.225425,
+ "y":0.333375
+ },
+ "backLeft":{
+ "x":-0.225425,
+ "y":0.333375
+ },
+ "mass":68.0388555,
+ "inertia":7.5,
+ "gearing":7.03,
+ "radius":0.0508,
+ "vmax":607.3745796940267,
+ "tmax":1.2,
+ "cof":1.5,
+ "bumper":{
+ "front":0.34925,
+ "side":0.4445,
+ "back":0.34925
+ },
+ "differentialTrackWidth":0.5588
+ },
+ "sampleType":"Swerve",
+ "waypoints":[0.0,1.88626,4.58447,7.75189,10.19451,11.3735],
+ "samples":[
+ {"t":0.0, "x":4.44125, "y":7.69346, "heading":-1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.71375, "ay":-0.89248, "alpha":0.0, "fx":[165.22814,165.22814,165.22814,165.22814], "fy":[-15.18078,-15.18078,-15.18079,-15.18079]},
+ {"t":0.04964, "x":4.45322, "y":7.69236, "heading":-1.5708, "vx":0.48217, "vy":-0.0443, "omega":0.0, "ax":9.71164, "ay":-0.88564, "alpha":0.0, "fx":[165.1922,165.1922,165.1922,165.1922], "fy":[-15.06444,-15.06444,-15.06445,-15.06445]},
+ {"t":0.09928, "x":4.48912, "y":7.68907, "heading":-1.5708, "vx":0.96424, "vy":-0.08826, "omega":0.0, "ax":9.70675, "ay":-0.87909, "alpha":0.0, "fx":[165.10907,165.10907,165.10907,165.10907], "fy":[-14.95309,-14.9531,-14.9531,-14.9531]},
+ {"t":0.14891, "x":4.54894, "y":7.6836, "heading":-1.5708, "vx":1.44607, "vy":-0.1319, "omega":0.0, "ax":9.69092, "ay":-0.87202, "alpha":0.0, "fx":[164.83972,164.83972,164.83972,164.83972], "fy":[-14.83278,-14.83278,-14.83279,-14.83279]},
+ {"t":0.19855, "x":4.63266, "y":7.67598, "heading":-1.5708, "vx":1.92711, "vy":-0.17518, "omega":0.0, "ax":1.30117, "ay":-0.04707, "alpha":0.00006, "fx":[22.13265,22.13235,22.13235,22.13265], "fy":[-0.80035,-0.80035,-0.80081,-0.80081]},
+ {"t":0.24819, "x":4.72992, "y":7.66723, "heading":-1.5708, "vx":1.9917, "vy":-0.17752, "omega":0.0, "ax":0.00291, "ay":0.03269, "alpha":0.00002, "fx":[0.04948,0.0494,0.0494,0.04948], "fy":[0.55606,0.55606,0.55593,0.55593]},
+ {"t":0.29783, "x":4.82879, "y":7.65846, "heading":-1.5708, "vx":1.99184, "vy":-0.1759, "omega":0.0, "ax":0.00247, "ay":0.02806, "alpha":-0.00001, "fx":[0.04196,0.04199,0.04199,0.04196], "fy":[0.47736,0.47736,0.4774,0.4774]},
+ {"t":0.34747, "x":4.92767, "y":7.64976, "heading":-1.5708, "vx":1.99197, "vy":-0.17451, "omega":0.0, "ax":0.00219, "ay":0.02507, "alpha":-0.00001, "fx":[0.03718,0.03725,0.03725,0.03718], "fy":[0.42642,0.42642,0.42653,0.42653]},
+ {"t":0.39711, "x":5.02655, "y":7.64113, "heading":-1.5708, "vx":1.99207, "vy":-0.17326, "omega":0.0, "ax":0.00197, "ay":0.02276, "alpha":-0.00002, "fx":[0.03352,0.03359,0.03359,0.03352], "fy":[0.38711,0.38711,0.38723,0.38723]},
+ {"t":0.44674, "x":5.12543, "y":7.63256, "heading":-1.5708, "vx":1.99217, "vy":-0.17213, "omega":0.0, "ax":0.0018, "ay":0.02089, "alpha":-0.00001, "fx":[0.03056,0.03064,0.03064,0.03056], "fy":[0.35526,0.35526,0.35537,0.35537]},
+ {"t":0.49638, "x":5.22432, "y":7.62404, "heading":-1.5708, "vx":1.99226, "vy":-0.17109, "omega":0.0, "ax":0.00166, "ay":0.01934, "alpha":-0.00001, "fx":[0.02813,0.02819,0.02819,0.02813], "fy":[0.32886,0.32886,0.32895,0.32895]},
+ {"t":0.54602, "x":5.32322, "y":7.61557, "heading":-1.5708, "vx":1.99234, "vy":-0.17013, "omega":0.0, "ax":0.00154, "ay":0.01803, "alpha":-0.00001, "fx":[0.02609,0.02613,0.02613,0.02609], "fy":[0.30663,0.30663,0.30669,0.30669]},
+ {"t":0.59566, "x":5.42211, "y":7.60715, "heading":-1.5708, "vx":1.99242, "vy":-0.16924, "omega":0.0, "ax":0.00143, "ay":0.01691, "alpha":-0.00001, "fx":[0.02435,0.02438,0.02438,0.02435], "fy":[0.28761,0.28761,0.28765,0.28765]},
+ {"t":0.6453, "x":5.52102, "y":7.59877, "heading":-1.5708, "vx":1.99249, "vy":-0.1684, "omega":0.0, "ax":0.00134, "ay":0.01594, "alpha":0.0, "fx":[0.02285,0.02287,0.02287,0.02285], "fy":[0.27113,0.27113,0.27116,0.27116]},
+ {"t":0.69494, "x":5.61992, "y":7.59043, "heading":-1.5708, "vx":1.99256, "vy":-0.16761, "omega":0.0, "ax":0.00127, "ay":0.01509, "alpha":0.0, "fx":[0.02153,0.02155,0.02155,0.02153], "fy":[0.25671,0.25671,0.25673,0.25673]},
+ {"t":0.74457, "x":5.71883, "y":7.58213, "heading":-1.5708, "vx":1.99262, "vy":-0.16686, "omega":0.0, "ax":0.0012, "ay":0.01434, "alpha":0.0, "fx":[0.02038,0.02038,0.02038,0.02038], "fy":[0.24396,0.24396,0.24397,0.24397]},
+ {"t":0.79421, "x":5.81774, "y":7.57386, "heading":-1.5708, "vx":1.99268, "vy":-0.16615, "omega":0.0, "ax":0.00114, "ay":0.01367, "alpha":0.0, "fx":[0.01935,0.01935,0.01935,0.01935], "fy":[0.2326,0.2326,0.23261,0.23261]},
+ {"t":0.84385, "x":5.91666, "y":7.56563, "heading":-1.5708, "vx":1.99274, "vy":-0.16547, "omega":0.0, "ax":0.00108, "ay":0.01308, "alpha":0.0, "fx":[0.01843,0.01843,0.01843,0.01843], "fy":[0.22241,0.22241,0.22241,0.22241]},
+ {"t":0.89349, "x":6.01557, "y":7.55743, "heading":-1.5708, "vx":1.99279, "vy":-0.16482, "omega":0.0, "ax":0.00103, "ay":0.01253, "alpha":0.0, "fx":[0.0176,0.01759,0.01759,0.0176], "fy":[0.2132,0.2132,0.2132,0.2132]},
+ {"t":0.94313, "x":6.11449, "y":7.54927, "heading":-1.5708, "vx":1.99284, "vy":-0.1642, "omega":0.0, "ax":0.00099, "ay":0.01204, "alpha":0.0, "fx":[0.01684,0.01684,0.01684,0.01684], "fy":[0.20484,0.20484,0.20484,0.20484]},
+ {"t":0.99277, "x":6.21342, "y":7.54113, "heading":-1.5708, "vx":1.99289, "vy":-0.1636, "omega":0.0, "ax":0.00095, "ay":0.01159, "alpha":0.0, "fx":[0.01616,0.01615,0.01615,0.01616], "fy":[0.1972,0.1972,0.1972,0.1972]},
+ {"t":1.0424, "x":6.31234, "y":7.53303, "heading":-1.5708, "vx":1.99294, "vy":-0.16302, "omega":0.0, "ax":0.00091, "ay":0.01118, "alpha":0.0, "fx":[0.01553,0.01553,0.01553,0.01553], "fy":[0.1902,0.1902,0.1902,0.1902]},
+ {"t":1.09204, "x":6.41127, "y":7.52495, "heading":-1.5708, "vx":1.99298, "vy":-0.16247, "omega":0.0, "ax":0.00088, "ay":0.0108, "alpha":0.0, "fx":[0.01495,0.01495,0.01495,0.01495], "fy":[0.18376,0.18376,0.18375,0.18375]},
+ {"t":1.14168, "x":6.5102, "y":7.5169, "heading":-1.5708, "vx":1.99303, "vy":-0.16193, "omega":0.0, "ax":0.00085, "ay":0.01045, "alpha":0.0, "fx":[0.01442,0.01442,0.01442,0.01442], "fy":[0.1778,0.1778,0.17779,0.17779]},
+ {"t":1.19132, "x":6.60913, "y":7.50887, "heading":-1.5708, "vx":1.99307, "vy":-0.16141, "omega":0.0, "ax":0.00082, "ay":0.01013, "alpha":0.0, "fx":[0.01393,0.01393,0.01393,0.01393], "fy":[0.17227,0.17227,0.17227,0.17227]},
+ {"t":1.24096, "x":6.70806, "y":7.50087, "heading":-1.5708, "vx":1.99311, "vy":-0.16091, "omega":0.0, "ax":0.00079, "ay":0.00983, "alpha":0.0, "fx":[0.01347,0.01347,0.01347,0.01347], "fy":[0.16713,0.16713,0.16713,0.16713]},
+ {"t":1.2906, "x":6.807, "y":7.49289, "heading":-1.5708, "vx":1.99315, "vy":-0.16042, "omega":0.0, "ax":0.00077, "ay":0.00954, "alpha":0.0, "fx":[0.01304,0.01304,0.01304,0.01304], "fy":[0.16229,0.16229,0.16229,0.16229]},
+ {"t":1.34023, "x":6.90594, "y":7.48494, "heading":-1.5708, "vx":1.99319, "vy":-0.15995, "omega":0.0, "ax":0.00074, "ay":0.00927, "alpha":0.0, "fx":[0.01263,0.01263,0.01263,0.01263], "fy":[0.15771,0.15771,0.15771,0.15771]},
+ {"t":1.38987, "x":7.00488, "y":7.47702, "heading":-1.5708, "vx":1.99322, "vy":-0.15949, "omega":0.0, "ax":0.00072, "ay":0.00904, "alpha":0.0, "fx":[0.01229,0.01229,0.01229,0.01229], "fy":[0.15382,0.15382,0.15382,0.15382]},
+ {"t":1.43951, "x":7.10382, "y":7.46911, "heading":-1.5708, "vx":1.99326, "vy":-0.15904, "omega":0.0, "ax":0.0007, "ay":0.00881, "alpha":0.0, "fx":[0.01194,0.01194,0.01194,0.01194], "fy":[0.14992,0.14992,0.14992,0.14992]},
+ {"t":1.48915, "x":7.20276, "y":7.46123, "heading":-1.5708, "vx":1.99329, "vy":-0.1586, "omega":0.0, "ax":0.00059, "ay":0.00742, "alpha":0.0, "fx":[0.01002,0.01002,0.01002,0.01002], "fy":[0.12614,0.12614,0.12614,0.12614]},
+ {"t":1.53879, "x":7.3017, "y":7.45336, "heading":-1.5708, "vx":1.99332, "vy":-0.15824, "omega":0.0, "ax":-0.00226, "ay":-0.02832, "alpha":0.0, "fx":[-0.03841,-0.03841,-0.03841,-0.03841], "fy":[-0.48167,-0.48167,-0.48167,-0.48167]},
+ {"t":1.58843, "x":7.40065, "y":7.44547, "heading":-1.5708, "vx":1.99321, "vy":-0.15964, "omega":0.0, "ax":-0.13389, "ay":-1.37465, "alpha":0.0, "fx":[-2.27744,-2.27744,-2.27744,-2.27744], "fy":[-23.38237,-23.38237,-23.38237,-23.38237]},
+ {"t":1.63806, "x":7.49942, "y":7.43586, "heading":-1.5708, "vx":1.98657, "vy":-0.22788, "omega":0.0, "ax":-1.99103, "ay":-8.68601, "alpha":0.0, "fx":[-33.86677,-33.86677,-33.86677,-33.86677], "fy":[-147.7466,-147.7466,-147.7466,-147.7466]},
+ {"t":1.6877, "x":7.59558, "y":7.41384, "heading":-1.5708, "vx":1.88773, "vy":-0.65904, "omega":0.0, "ax":-7.82486, "ay":-5.68329, "alpha":0.0, "fx":[-133.09866,-133.09866,-133.09866,-133.09866], "fy":[-96.67106,-96.67106,-96.67106,-96.67106]},
+ {"t":1.73734, "x":7.67964, "y":7.37413, "heading":-1.5708, "vx":1.49932, "vy":-0.94114, "omega":0.0, "ax":-9.68718, "ay":-0.98874, "alpha":0.0, "fx":[-164.77616,-164.77616,-164.77616,-164.77616], "fy":[-16.81816,-16.81816,-16.81816,-16.81816]},
+ {"t":1.78698, "x":7.74213, "y":7.32619, "heading":-1.5708, "vx":1.01847, "vy":-0.99022, "omega":0.0, "ax":-9.74584, "ay":-0.22781, "alpha":0.0, "fx":[-165.77401,-165.77401,-165.77401,-165.77401], "fy":[-3.87492,-3.87492,-3.87492,-3.87492]},
+ {"t":1.83662, "x":7.78068, "y":7.27676, "heading":-1.5708, "vx":0.5347, "vy":-1.00153, "omega":0.0, "ax":-9.75259, "ay":0.07234, "alpha":0.0, "fx":[-165.88882,-165.88882,-165.88882,-165.88882], "fy":[1.23044,1.23044,1.23044,1.23044]},
+ {"t":1.88626, "x":7.79521, "y":7.22713, "heading":-1.5708, "vx":0.0506, "vy":-0.99794, "omega":0.0, "ax":-2.09267, "ay":-0.01224, "alpha":0.0, "fx":[-35.59575,-35.59575,-35.59575,-35.59575], "fy":[-0.20819,-0.20819,-0.20819,-0.20819]},
+ {"t":1.93444, "x":7.79522, "y":7.17904, "heading":-1.5708, "vx":-0.05023, "vy":-0.99853, "omega":0.0, "ax":2.07007, "ay":-0.00075, "alpha":0.0, "fx":[35.21127,35.21127,35.21127,35.21127], "fy":[-0.01279,-0.01279,-0.01279,-0.01279]},
+ {"t":1.98262, "x":7.7952, "y":7.13092, "heading":-1.5708, "vx":0.04951, "vy":-0.99857, "omega":0.0, "ax":-2.04001, "ay":-0.00074, "alpha":0.0, "fx":[-34.69992,-34.69992,-34.69992,-34.69992], "fy":[-0.01255,-0.01255,-0.01255,-0.01255]},
+ {"t":2.0308, "x":7.79522, "y":7.08281, "heading":-1.5708, "vx":-0.04878, "vy":-0.9986, "omega":0.0, "ax":2.01006, "ay":-0.00072, "alpha":0.0, "fx":[34.19054,34.19054,34.19054,34.19054], "fy":[-0.01232,-0.01232,-0.01232,-0.01232]},
+ {"t":2.07899, "x":7.7952, "y":7.03469, "heading":-1.5708, "vx":0.04806, "vy":-0.99864, "omega":0.0, "ax":-1.98023, "ay":-0.00071, "alpha":0.0, "fx":[-33.68323,-33.68323,-33.68323,-33.68323], "fy":[-0.01209,-0.01209,-0.01209,-0.01209]},
+ {"t":2.12717, "x":7.79522, "y":6.98658, "heading":-1.5708, "vx":-0.04735, "vy":-0.99867, "omega":0.0, "ax":1.95054, "ay":-0.0007, "alpha":0.0, "fx":[33.17806,33.17806,33.17806,33.17806], "fy":[-0.01185,-0.01185,-0.01185,-0.01185]},
+ {"t":2.17535, "x":7.7952, "y":6.93846, "heading":-1.5708, "vx":0.04663, "vy":-0.9987, "omega":0.0, "ax":-1.92097, "ay":-0.00068, "alpha":0.0, "fx":[-32.6751,-32.6751,-32.6751,-32.6751], "fy":[-0.01162,-0.01162,-0.01162,-0.01162]},
+ {"t":2.22353, "x":7.79522, "y":6.89034, "heading":-1.5708, "vx":-0.04592, "vy":-0.99874, "omega":0.0, "ax":1.89153, "ay":-0.00067, "alpha":0.0, "fx":[32.17446,32.17446,32.17446,32.17446], "fy":[-0.01139,-0.01139,-0.01139,-0.01139]},
+ {"t":2.27172, "x":7.7952, "y":6.84221, "heading":-1.5708, "vx":0.04522, "vy":-0.99877, "omega":0.0, "ax":-1.86224, "ay":-0.00066, "alpha":0.0, "fx":[-31.67621,-31.67621,-31.67621,-31.67621], "fy":[-0.01116,-0.01116,-0.01116,-0.01116]},
+ {"t":2.3199, "x":7.79522, "y":6.79409, "heading":-1.5708, "vx":-0.04451, "vy":-0.9988, "omega":0.0, "ax":1.8331, "ay":-0.00064, "alpha":0.0, "fx":[31.18047,31.18047,31.18047,31.18047], "fy":[-0.01093,-0.01093,-0.01093,-0.01093]},
+ {"t":2.36808, "x":7.7952, "y":6.74596, "heading":-1.5708, "vx":0.04381, "vy":-0.99883, "omega":0.0, "ax":-1.80411, "ay":-0.00063, "alpha":0.0, "fx":[-30.68732,-30.68732,-30.68732,-30.68732], "fy":[-0.0107,-0.0107,-0.0107,-0.0107]},
+ {"t":2.41626, "x":7.79522, "y":6.69784, "heading":-1.5708, "vx":-0.04311, "vy":-0.99886, "omega":0.0, "ax":1.77527, "ay":-0.00062, "alpha":0.0, "fx":[30.19689,30.19689,30.19689,30.19689], "fy":[-0.01047,-0.01047,-0.01047,-0.01047]},
+ {"t":2.46445, "x":7.7952, "y":6.64971, "heading":-1.5708, "vx":0.04242, "vy":-0.99889, "omega":0.0, "ax":-1.74661, "ay":-0.0006, "alpha":0.0, "fx":[-29.70927,-29.70927,-29.70927,-29.70927], "fy":[-0.01024,-0.01024,-0.01024,-0.01024]},
+ {"t":2.51263, "x":7.79522, "y":6.60158, "heading":-1.5708, "vx":-0.04173, "vy":-0.99892, "omega":0.0, "ax":1.71811, "ay":-0.00059, "alpha":0.0, "fx":[29.2246,29.2246,29.2246,29.2246], "fy":[-0.01001,-0.01001,-0.01001,-0.01001]},
+ {"t":2.56081, "x":7.7952, "y":6.55345, "heading":-1.5708, "vx":0.04105, "vy":-0.99895, "omega":0.0, "ax":-1.6898, "ay":-0.00057, "alpha":0.0, "fx":[-28.74299,-28.74299,-28.74299,-28.74299], "fy":[-0.00978,-0.00978,-0.00978,-0.00978]},
+ {"t":2.60899, "x":7.79522, "y":6.50532, "heading":-1.5708, "vx":-0.04037, "vy":-0.99898, "omega":0.0, "ax":1.66167, "ay":-0.00056, "alpha":0.0, "fx":[28.26459,28.26459,28.26459,28.26459], "fy":[-0.00955,-0.00955,-0.00955,-0.00955]},
+ {"t":2.65718, "x":7.7952, "y":6.45718, "heading":-1.5708, "vx":0.03969, "vy":-0.999, "omega":0.0, "ax":-1.63374, "ay":-0.00055, "alpha":0.0, "fx":[-27.78953,-27.78953,-27.78953,-27.78953], "fy":[-0.00932,-0.00932,-0.00932,-0.00932]},
+ {"t":2.70536, "x":7.79522, "y":6.40905, "heading":-1.5708, "vx":-0.03902, "vy":-0.99903, "omega":0.0, "ax":1.60602, "ay":-0.00053, "alpha":0.0, "fx":[27.31796,27.31796,27.31796,27.31796], "fy":[-0.00909,-0.00909,-0.00909,-0.00909]},
+ {"t":2.75354, "x":7.7952, "y":6.36091, "heading":-1.5708, "vx":0.03836, "vy":-0.99906, "omega":0.0, "ax":-1.57851, "ay":-0.00052, "alpha":0.0, "fx":[-26.85004,-26.85004,-26.85004,-26.85004], "fy":[-0.00887,-0.00887,-0.00887,-0.00887]},
+ {"t":2.80172, "x":7.79521, "y":6.31277, "heading":-1.5708, "vx":-0.0377, "vy":-0.99908, "omega":0.0, "ax":1.55123, "ay":-0.00051, "alpha":0.0, "fx":[26.38594,26.38594,26.38594,26.38594], "fy":[-0.00864,-0.00864,-0.00864,-0.00864]},
+ {"t":2.84991, "x":7.7952, "y":6.26463, "heading":-1.5708, "vx":0.03704, "vy":-0.99911, "omega":0.0, "ax":-1.52418, "ay":-0.00049, "alpha":0.0, "fx":[-25.92584,-25.92584,-25.92584,-25.92584], "fy":[-0.00841,-0.00841,-0.00841,-0.00841]},
+ {"t":2.89809, "x":7.79521, "y":6.21649, "heading":-1.5708, "vx":-0.03639, "vy":-0.99913, "omega":0.0, "ax":1.49737, "ay":-0.00048, "alpha":0.0, "fx":[25.46992,25.46992,25.46992,25.46992], "fy":[-0.00819,-0.00819,-0.00819,-0.00819]},
+ {"t":2.94627, "x":7.7952, "y":6.16835, "heading":-1.5708, "vx":0.03575, "vy":-0.99915, "omega":0.0, "ax":-1.47083, "ay":-0.00047, "alpha":0.0, "fx":[-25.01838,-25.01838,-25.01838,-25.01838], "fy":[-0.00796,-0.00796,-0.00796,-0.00796]},
+ {"t":2.99445, "x":7.79521, "y":6.12021, "heading":-1.5708, "vx":-0.03512, "vy":-0.99918, "omega":0.0, "ax":1.44455, "ay":-0.00045, "alpha":0.0, "fx":[24.57145,24.57145,24.57145,24.57145], "fy":[-0.00774,-0.00774,-0.00774,-0.00774]},
+ {"t":3.04264, "x":7.7952, "y":6.07207, "heading":-1.5708, "vx":0.03449, "vy":-0.9992, "omega":0.0, "ax":-1.41856, "ay":-0.00044, "alpha":0.0, "fx":[-24.12934,-24.12934,-24.12934,-24.12934], "fy":[-0.00752,-0.00752,-0.00752,-0.00752]},
+ {"t":3.09082, "x":7.79521, "y":6.02392, "heading":-1.5708, "vx":-0.03386, "vy":-0.99922, "omega":0.0, "ax":1.39287, "ay":-0.00043, "alpha":0.0, "fx":[23.6923,23.6923,23.6923,23.6923], "fy":[-0.00729,-0.00729,-0.00729,-0.00729]},
+ {"t":3.139, "x":7.7952, "y":5.97578, "heading":-1.5708, "vx":0.03325, "vy":-0.99924, "omega":0.0, "ax":-1.36749, "ay":-0.00042, "alpha":0.0, "fx":[-23.26058,-23.26058,-23.26058,-23.26058], "fy":[-0.00707,-0.00707,-0.00707,-0.00707]},
+ {"t":3.18718, "x":7.79521, "y":5.92763, "heading":-1.5708, "vx":-0.03264, "vy":-0.99926, "omega":0.0, "ax":1.34244, "ay":-0.0004, "alpha":0.0, "fx":[22.83447,22.83447,22.83447,22.83447], "fy":[-0.00685,-0.00685,-0.00685,-0.00685]},
+ {"t":3.23537, "x":7.7952, "y":5.87948, "heading":-1.5708, "vx":0.03204, "vy":-0.99928, "omega":0.0, "ax":-1.31773, "ay":-0.00039, "alpha":0.0, "fx":[-22.41425,-22.41425,-22.41425,-22.41425], "fy":[-0.00662,-0.00662,-0.00662,-0.00662]},
+ {"t":3.28355, "x":7.79521, "y":5.83134, "heading":-1.5708, "vx":-0.03145, "vy":-0.9993, "omega":0.0, "ax":1.29339, "ay":-0.00038, "alpha":0.0, "fx":[22.00025,22.00025,22.00025,22.00025], "fy":[-0.0064,-0.0064,-0.0064,-0.0064]},
+ {"t":3.33173, "x":7.7952, "y":5.78319, "heading":-1.5708, "vx":0.03087, "vy":-0.99932, "omega":0.0, "ax":-1.26944, "ay":-0.00036, "alpha":0.0, "fx":[-21.59279,-21.59279,-21.59279,-21.59279], "fy":[-0.00618,-0.00618,-0.00618,-0.00618]},
+ {"t":3.37991, "x":7.79521, "y":5.73504, "heading":-1.5708, "vx":-0.0303, "vy":-0.99933, "omega":0.0, "ax":1.24589, "ay":-0.00035, "alpha":0.0, "fx":[21.19224,21.19224,21.19224,21.19224], "fy":[-0.00596,-0.00596,-0.00596,-0.00596]},
+ {"t":3.42809, "x":7.7952, "y":5.68689, "heading":-1.5708, "vx":0.02973, "vy":-0.99935, "omega":0.0, "ax":-1.22277, "ay":-0.00034, "alpha":0.0, "fx":[-20.799,-20.799,-20.799,-20.799], "fy":[-0.00574,-0.00574,-0.00574,-0.00574]},
+ {"t":3.47628, "x":7.79521, "y":5.63873, "heading":-1.5708, "vx":-0.02918, "vy":-0.99937, "omega":0.0, "ax":1.20011, "ay":-0.00032, "alpha":0.0, "fx":[20.41346,20.41346,20.41346,20.41346], "fy":[-0.00552,-0.00552,-0.00552,-0.00552]},
+ {"t":3.52446, "x":7.7952, "y":5.59058, "heading":-1.5708, "vx":0.02864, "vy":-0.99938, "omega":0.0, "ax":-1.17792, "ay":-0.00031, "alpha":0.0, "fx":[-20.03609,-20.03609,-20.03609,-20.03609], "fy":[-0.0053,-0.0053,-0.0053,-0.0053]},
+ {"t":3.57264, "x":7.79521, "y":5.54243, "heading":-1.5708, "vx":-0.02811, "vy":-0.9994, "omega":0.0, "ax":1.15624, "ay":-0.0003, "alpha":0.0, "fx":[19.66737,19.66737,19.66737,19.66737], "fy":[-0.00507,-0.00507,-0.00507,-0.00507]},
+ {"t":3.62082, "x":7.7952, "y":5.49427, "heading":-1.5708, "vx":0.0276, "vy":-0.99941, "omega":0.0, "ax":-1.1351, "ay":-0.00029, "alpha":0.0, "fx":[-19.30781,-19.30781,-19.30781,-19.30781], "fy":[-0.00485,-0.00485,-0.00485,-0.00485]},
+ {"t":3.66901, "x":7.79521, "y":5.44612, "heading":-1.5708, "vx":-0.02709, "vy":-0.99943, "omega":0.0, "ax":1.11454, "ay":-0.00027, "alpha":0.0, "fx":[18.95797,18.95797,18.95797,18.95797], "fy":[-0.00463,-0.00463,-0.00463,-0.00463]},
+ {"t":3.71719, "x":7.7952, "y":5.39796, "heading":-1.5708, "vx":0.02661, "vy":-0.99944, "omega":0.0, "ax":-1.09458, "ay":-0.00026, "alpha":0.0, "fx":[-18.61845,-18.61845,-18.61845,-18.61845], "fy":[-0.00441,-0.00441,-0.00441,-0.00441]},
+ {"t":3.76537, "x":7.79521, "y":5.34981, "heading":-1.5708, "vx":-0.02613, "vy":-0.99945, "omega":0.0, "ax":1.07526, "ay":-0.00025, "alpha":0.0, "fx":[18.28991,18.28991,18.28991,18.28991], "fy":[-0.00418,-0.00418,-0.00418,-0.00418]},
+ {"t":3.81355, "x":7.7952, "y":5.30165, "heading":-1.5708, "vx":0.02568, "vy":-0.99946, "omega":0.0, "ax":-1.05663, "ay":-0.00023, "alpha":0.0, "fx":[-17.97304,-17.97304,-17.97304,-17.97304], "fy":[-0.00396,-0.00396,-0.00396,-0.00396]},
+ {"t":3.86174, "x":7.79521, "y":5.2535, "heading":-1.5708, "vx":-0.02524, "vy":-0.99947, "omega":0.0, "ax":1.03874, "ay":-0.00022, "alpha":0.0, "fx":[17.66859,17.66859,17.66859,17.66859], "fy":[-0.00373,-0.00373,-0.00373,-0.00373]},
+ {"t":3.90992, "x":7.7952, "y":5.20534, "heading":-1.5708, "vx":0.02481, "vy":-0.99948, "omega":0.0, "ax":-1.02161, "ay":-0.00021, "alpha":0.0, "fx":[-17.37737,-17.37737,-17.37737,-17.37737], "fy":[-0.0035,-0.0035,-0.0035,-0.0035]},
+ {"t":3.9581, "x":7.79521, "y":5.15718, "heading":-1.5708, "vx":-0.02441, "vy":-0.99949, "omega":0.0, "ax":1.00532, "ay":-0.00019, "alpha":0.0, "fx":[17.10025,17.10025,17.10025,17.10025], "fy":[-0.00327,-0.00327,-0.00327,-0.00327]},
+ {"t":4.00628, "x":7.7952, "y":5.10902, "heading":-1.5708, "vx":0.02403, "vy":-0.9995, "omega":0.0, "ax":-0.98991, "ay":-0.00018, "alpha":0.0, "fx":[-16.83814,-16.83814,-16.83814,-16.83814], "fy":[-0.00303,-0.00303,-0.00303,-0.00303]},
+ {"t":4.05447, "x":7.79521, "y":5.06086, "heading":-1.5708, "vx":-0.02367, "vy":-0.99951, "omega":0.0, "ax":0.97544, "ay":-0.00016, "alpha":0.0, "fx":[16.59203,16.59203,16.59203,16.59203], "fy":[-0.0028,-0.0028,-0.0028,-0.0028]},
+ {"t":4.10265, "x":7.7952, "y":5.0127, "heading":-1.5708, "vx":0.02333, "vy":-0.99952, "omega":0.0, "ax":-0.96198, "ay":-0.00015, "alpha":0.0, "fx":[-16.36294,-16.36294,-16.36294,-16.36294], "fy":[-0.00255,-0.00255,-0.00255,-0.00255]},
+ {"t":4.15083, "x":7.79521, "y":4.96454, "heading":-1.5708, "vx":-0.02302, "vy":-0.99953, "omega":0.0, "ax":0.94957, "ay":-0.00014, "alpha":0.0, "fx":[16.15197,16.15197,16.15197,16.15197], "fy":[-0.00231,-0.00231,-0.00231,-0.00231]},
+ {"t":4.19901, "x":7.7952, "y":4.91639, "heading":-1.5708, "vx":0.02273, "vy":-0.99953, "omega":0.0, "ax":-0.9383, "ay":-0.00012, "alpha":0.0, "fx":[-15.96021,-15.96021,-15.96021,-15.96021], "fy":[-0.00206,-0.00206,-0.00206,-0.00206]},
+ {"t":4.2472, "x":7.79521, "y":4.86822, "heading":-1.5708, "vx":-0.02248, "vy":-0.99954, "omega":0.0, "ax":0.92822, "ay":-0.00011, "alpha":0.0, "fx":[15.78881,15.78881,15.78881,15.78881], "fy":[-0.0018,-0.0018,-0.0018,-0.0018]},
+ {"t":4.29538, "x":7.7952, "y":4.82006, "heading":-1.5708, "vx":0.02225, "vy":-0.99955, "omega":0.0, "ax":-0.91941, "ay":-0.00009, "alpha":0.0, "fx":[-15.63889,-15.63889,-15.63889,-15.63889], "fy":[-0.00154,-0.00154,-0.00154,-0.00154]},
+ {"t":4.34356, "x":7.79521, "y":4.7719, "heading":-1.5708, "vx":-0.02205, "vy":-0.99955, "omega":0.0, "ax":0.91192, "ay":-0.00007, "alpha":0.0, "fx":[15.51153,15.51153,15.51153,15.51153], "fy":[-0.00127,-0.00127,-0.00127,-0.00127]},
+ {"t":4.39174, "x":7.79521, "y":4.72374, "heading":-1.5708, "vx":0.02189, "vy":-0.99955, "omega":0.0, "ax":-0.90582, "ay":-0.00006, "alpha":0.0, "fx":[-15.40774,-15.40774,-15.40774,-15.40774], "fy":[-0.001,-0.001,-0.001,-0.001]},
+ {"t":4.43993, "x":7.79521, "y":4.67558, "heading":-1.5708, "vx":-0.02176, "vy":-0.99956, "omega":0.0, "ax":0.90116, "ay":-0.00004, "alpha":0.0, "fx":[15.3284,15.3284,15.3284,15.3284], "fy":[-0.00073,-0.00073,-0.00073,-0.00073]},
+ {"t":4.48811, "x":7.79521, "y":4.62742, "heading":-1.5708, "vx":0.02166, "vy":-0.99956, "omega":0.0, "ax":-0.89797, "ay":-0.00003, "alpha":0.0, "fx":[-15.27424,-15.27424,-15.27424,-15.27424], "fy":[-0.00045,-0.00045,-0.00045,-0.00045]},
+ {"t":4.53629, "x":7.79521, "y":4.57926, "heading":-1.5708, "vx":-0.0216, "vy":-0.99956, "omega":0.0, "ax":0.89633, "ay":0.00439, "alpha":0.0, "fx":[15.2464,15.2464,15.2464,15.2464], "fy":[0.07462,0.07462,0.07462,0.07462]},
+ {"t":4.58447, "x":7.79521, "y":4.5311, "heading":-1.5708, "vx":0.02158, "vy":-0.99935, "omega":0.0, "ax":-1.30151, "ay":2.51868, "alpha":20.35533, "fx":[25.14587,-64.03427,-99.35853,49.69392], "fy":[122.27364,111.83408,-26.19061,-36.54879]},
+ {"t":4.60903, "x":7.79534, "y":4.50733, "heading":-1.5708, "vx":-0.01037, "vy":-0.93751, "omega":0.4998, "ax":-2.03873, "ay":4.70262, "alpha":0.00002, "fx":[-34.67812,-34.67823,-34.67823,-34.67813], "fy":[79.99028,79.99027,79.99014,79.99015]},
+ {"t":4.63358, "x":7.79448, "y":4.48572, "heading":-1.55852, "vx":-0.06043, "vy":-0.82204, "omega":0.4998, "ax":-1.95127, "ay":4.49795, "alpha":0.0, "fx":[-33.19054,-33.19054,-33.19054,-33.19054], "fy":[76.50886,76.50886,76.50886,76.50886]},
+ {"t":4.65814, "x":7.7924, "y":4.4669, "heading":-1.54625, "vx":-0.10834, "vy":-0.7116, "omega":0.4998, "ax":-1.86168, "ay":4.2944, "alpha":0.0, "fx":[-31.66671,-31.66671,-31.66671,-31.66671], "fy":[73.0466,73.0466,73.0466,73.0466]},
+ {"t":4.68269, "x":7.78918, "y":4.45072, "heading":-1.53398, "vx":-0.15405, "vy":-0.60615, "omega":0.4998, "ax":-1.77052, "ay":4.09198, "alpha":0.0, "fx":[-30.11611,-30.11611,-30.11611,-30.11611], "fy":[69.60336,69.60336,69.60336,69.60336]},
+ {"t":4.70724, "x":7.78487, "y":4.43707, "heading":-1.52171, "vx":-0.19753, "vy":-0.50568, "omega":0.4998, "ax":-1.67836, "ay":3.89092, "alpha":0.0, "fx":[-28.54844,-28.54844,-28.54844,-28.54844], "fy":[66.18352,66.18352,66.18352,66.18352]},
+ {"t":4.7318, "x":7.77951, "y":4.42582, "heading":-1.50944, "vx":-0.23874, "vy":-0.41015, "omega":0.4998, "ax":-1.58577, "ay":3.69171, "alpha":0.0, "fx":[-26.97346,-26.97346,-26.97346,-26.97346], "fy":[62.79485,62.79485,62.79485,62.79485]},
+ {"t":4.75635, "x":7.77317, "y":4.41687, "heading":-1.49717, "vx":-0.27767, "vy":-0.3195, "omega":0.4998, "ax":-1.49333, "ay":3.49491, "alpha":0.0, "fx":[-25.40112,-25.40112,-25.40112,-25.40112], "fy":[59.44742,59.44742,59.44742,59.44742]},
+ {"t":4.7809, "x":7.7659, "y":4.41008, "heading":-1.48489, "vx":-0.31434, "vy":-0.23369, "omega":0.4998, "ax":-1.40162, "ay":3.30123, "alpha":0.0, "fx":[-23.84123,-23.84123,-23.84123,-23.84123], "fy":[56.15296,56.15296,56.15296,56.15296]},
+ {"t":4.80546, "x":7.75776, "y":4.40533, "heading":-1.47262, "vx":-0.34875, "vy":-0.15263, "omega":0.4998, "ax":-1.31122, "ay":3.1114, "alpha":0.0, "fx":[-22.30342,-22.30342,-22.30342,-22.30342], "fy":[52.92404,52.92404,52.92404,52.92404]},
+ {"t":4.83001, "x":7.7488, "y":4.40252, "heading":-1.46035, "vx":-0.38095, "vy":-0.07623, "omega":0.4998, "ax":-1.22264, "ay":2.92618, "alpha":0.0, "fx":[-20.79681,-20.79681,-20.79681,-20.79682], "fy":[49.7735,49.7735,49.7735,49.7735]},
+ {"t":4.85456, "x":7.73908, "y":4.40153, "heading":-1.44808, "vx":-0.41097, "vy":-0.00439, "omega":0.4998, "ax":-1.1364, "ay":2.74631, "alpha":0.0, "fx":[-19.32987,-19.32987,-19.32988,-19.32988], "fy":[46.71391,46.71391,46.71391,46.71391]},
+ {"t":4.87912, "x":7.72865, "y":4.40225, "heading":-1.43581, "vx":-0.43887, "vy":0.06305, "omega":0.4998, "ax":-1.05294, "ay":2.57247, "alpha":0.0, "fx":[-17.91021,-17.91021,-17.91021,-17.91021], "fy":[43.75701,43.75702,43.75701,43.75701]},
+ {"t":4.90367, "x":7.71755, "y":4.40458, "heading":-1.42353, "vx":-0.46473, "vy":0.12621, "omega":0.4998, "ax":-0.97265, "ay":2.40529, "alpha":0.0, "fx":[-16.54443,-16.54443,-16.54443,-16.54443], "fy":[40.91324,40.91324,40.91324,40.91324]},
+ {"t":4.92823, "x":7.70585, "y":4.4084, "heading":-1.41126, "vx":-0.48861, "vy":0.18527, "omega":0.4998, "ax":-0.89584, "ay":2.24528, "alpha":0.0, "fx":[-15.23801,-15.23801,-15.23802,-15.23802], "fy":[38.19158,38.19158,38.19158,38.19158]},
+ {"t":4.95278, "x":7.69358, "y":4.41363, "heading":-1.39899, "vx":-0.5106, "vy":0.2404, "omega":0.4998, "ax":-0.82279, "ay":2.09288, "alpha":0.0, "fx":[-13.99537,-13.99537,-13.99538,-13.99538], "fy":[35.59924,35.59924,35.59924,35.59924]},
+ {"t":4.97733, "x":7.6808, "y":4.42016, "heading":-1.38672, "vx":-0.53081, "vy":0.29179, "omega":0.4998, "ax":-0.75366, "ay":1.9484, "alpha":0.0, "fx":[-12.81955,-12.81955,-12.81955,-12.81955], "fy":[33.14171,33.14171,33.14171,33.14171]},
+ {"t":5.00189, "x":7.66754, "y":4.42791, "heading":-1.37445, "vx":-0.54931, "vy":0.33963, "omega":0.4998, "ax":-0.68858, "ay":1.81204, "alpha":0.0, "fx":[-11.71254,-11.71254,-11.71254,-11.71254], "fy":[30.82231,30.82231,30.82231,30.82231]},
+ {"t":5.02644, "x":7.65384, "y":4.4368, "heading":-1.36217, "vx":-0.56622, "vy":0.38412, "omega":0.4998, "ax":-0.6276, "ay":1.68391, "alpha":0.0, "fx":[-10.67525,-10.67525,-10.67526,-10.67526], "fy":[28.64287,28.64287,28.64287,28.64287]},
+ {"t":5.05099, "x":7.63975, "y":4.44674, "heading":-1.3499, "vx":-0.58163, "vy":0.42546, "omega":0.4998, "ax":-0.57071, "ay":1.56403, "alpha":0.0, "fx":[-9.70761,-9.70761,-9.70761,-9.70761], "fy":[26.60362,26.60362,26.60362,26.60362]},
+ {"t":5.07555, "x":7.6253, "y":4.45765, "heading":-1.33763, "vx":-0.59564, "vy":0.46387, "omega":0.4998, "ax":-0.51786, "ay":1.45227, "alpha":0.0, "fx":[-8.80871,-8.80871,-8.80871,-8.80871], "fy":[24.70272,24.70272,24.70272,24.70272]},
+ {"t":5.1001, "x":7.61052, "y":4.46948, "heading":-1.32536, "vx":-0.60836, "vy":0.49953, "omega":0.4998, "ax":-0.46896, "ay":1.3485, "alpha":0.0, "fx":[-7.97691,-7.97691,-7.97691,-7.97691], "fy":[22.93759,22.93759,22.93759,22.93759]},
+ {"t":5.12465, "x":7.59544, "y":4.48215, "heading":-1.31309, "vx":-0.61987, "vy":0.53264, "omega":0.4998, "ax":-0.42387, "ay":1.25248, "alpha":0.0, "fx":[-7.20997,-7.20997,-7.20997,-7.20997], "fy":[21.30439,21.30439,21.30439,21.30439]},
+ {"t":5.14921, "x":7.58009, "y":4.49561, "heading":-1.30082, "vx":-0.63028, "vy":0.56339, "omega":0.4998, "ax":-0.38243, "ay":1.16395, "alpha":0.0, "fx":[-6.50511,-6.50511,-6.50511,-6.50511], "fy":[19.79838,19.79838,19.79838,19.79838]},
+ {"t":5.17376, "x":7.5645, "y":4.50979, "heading":-1.28854, "vx":-0.63967, "vy":0.59197, "omega":0.4998, "ax":-0.34447, "ay":1.08257, "alpha":0.0, "fx":[-5.85935,-5.85935,-5.85936,-5.85936], "fy":[18.41422,18.41422,18.41422,18.41422]},
+ {"t":5.19831, "x":7.54869, "y":4.52465, "heading":-1.27627, "vx":-0.64813, "vy":0.61855, "omega":0.4998, "ax":-0.30979, "ay":1.00801, "alpha":0.0, "fx":[-5.26943,-5.26943,-5.26944,-5.26944], "fy":[17.14597,17.14597,17.14597,17.14597]},
+ {"t":5.22287, "x":7.53268, "y":4.54015, "heading":-1.264, "vx":-0.65573, "vy":0.6433, "omega":0.4998, "ax":-0.27819, "ay":0.93991, "alpha":0.0, "fx":[-4.73185,-4.73185,-4.73185,-4.73185], "fy":[15.98754,15.98754,15.98754,15.98754]},
+ {"t":5.24742, "x":7.5165, "y":4.55622, "heading":-1.25173, "vx":-0.66256, "vy":0.66638, "omega":0.4998, "ax":-0.24945, "ay":0.87788, "alpha":0.0, "fx":[-4.24307,-4.24308,-4.24308,-4.24308], "fy":[14.93244,14.93244,14.93244,14.93244]},
+ {"t":5.27198, "x":7.50015, "y":4.57285, "heading":-1.23946, "vx":-0.66869, "vy":0.68793, "omega":0.4998, "ax":-0.22338, "ay":0.82154, "alpha":0.0, "fx":[-3.79957,-3.79957,-3.79957,-3.79957], "fy":[13.97419,13.97419,13.97419,13.97419]},
+ {"t":5.29653, "x":7.48367, "y":4.58999, "heading":-1.22718, "vx":-0.67417, "vy":0.7081, "omega":0.4998, "ax":-0.19976, "ay":0.77052, "alpha":0.0, "fx":[-3.39782,-3.39782,-3.39782,-3.39782], "fy":[13.10641,13.10641,13.10641,13.10641]},
+ {"t":5.32108, "x":7.46705, "y":4.60761, "heading":-1.21491, "vx":-0.67908, "vy":0.72702, "omega":0.4998, "ax":-0.17839, "ay":0.72444, "alpha":0.0, "fx":[-3.03442,-3.03442,-3.03442,-3.03442], "fy":[12.32255,12.32255,12.32255,12.32255]},
+ {"t":5.34564, "x":7.45033, "y":4.62568, "heading":-1.20264, "vx":-0.68346, "vy":0.74481, "omega":0.4998, "ax":-0.1591, "ay":0.68295, "alpha":0.0, "fx":[-2.70619,-2.70619,-2.70619,-2.70619], "fy":[11.61671,11.61671,11.61671,11.61671]},
+ {"t":5.37019, "x":7.4335, "y":4.64417, "heading":-1.19037, "vx":-0.68736, "vy":0.76158, "omega":0.4998, "ax":-0.14168, "ay":0.64569, "alpha":0.0, "fx":[-2.41,-2.41001,-2.41001,-2.41001], "fy":[10.98302,10.98302,10.98302,10.98302]},
+ {"t":5.39474, "x":7.41658, "y":4.66307, "heading":-1.1781, "vx":-0.69084, "vy":0.77743, "omega":0.4998, "ax":-0.12598, "ay":0.61234, "alpha":0.0, "fx":[-2.14293,-2.14293,-2.14293,-2.14293], "fy":[10.41581,10.41581,10.41581,10.4158]},
+ {"t":5.4193, "x":7.39958, "y":4.68234, "heading":-1.16583, "vx":-0.69394, "vy":0.79247, "omega":0.4998, "ax":-0.11183, "ay":0.58259, "alpha":0.0, "fx":[-1.90218,-1.90218,-1.90218,-1.90218], "fy":[9.90973,9.90973,9.90973,9.90973]},
+ {"t":5.44385, "x":7.3825, "y":4.70197, "heading":-1.15355, "vx":-0.69668, "vy":0.80677, "omega":0.4998, "ax":-0.09907, "ay":0.55614, "alpha":0.0, "fx":[-1.68515,-1.68515,-1.68516,-1.68515], "fy":[9.45975,9.45976,9.45975,9.45975]},
+ {"t":5.4684, "x":7.36537, "y":4.72195, "heading":-1.14128, "vx":-0.69912, "vy":0.82043, "omega":0.4998, "ax":-0.08757, "ay":0.53269, "alpha":0.0, "fx":[-1.48954,-1.48954,-1.48954,-1.48954], "fy":[9.06084,9.06084,9.06084,9.06084]},
+ {"t":5.49296, "x":7.34818, "y":4.74225, "heading":-1.12901, "vx":-0.70127, "vy":0.83351, "omega":0.4998, "ax":-0.07719, "ay":0.51199, "alpha":0.0, "fx":[-1.31301,-1.31302,-1.31302,-1.31302], "fy":[8.70883,8.70883,8.70883,8.70883]},
+ {"t":5.51751, "x":7.33093, "y":4.76287, "heading":-1.11674, "vx":-0.70316, "vy":0.84608, "omega":0.4998, "ax":-0.06783, "ay":0.49381, "alpha":0.0, "fx":[-1.15376,-1.15376,-1.15376,-1.15376], "fy":[8.39956,8.39956,8.39956,8.39956]},
+ {"t":5.54207, "x":7.31365, "y":4.7838, "heading":-1.10447, "vx":-0.70483, "vy":0.8582, "omega":0.4998, "ax":-0.05936, "ay":0.47791, "alpha":0.0, "fx":[-1.00969,-1.00969,-1.00969,-1.00969], "fy":[8.12917,8.12918,8.12918,8.12917]},
+ {"t":5.56662, "x":7.29632, "y":4.80501, "heading":-1.09219, "vx":-0.70628, "vy":0.86994, "omega":0.4998, "ax":-0.05168, "ay":0.46409, "alpha":0.0, "fx":[-0.87912,-0.87912,-0.87912,-0.87912], "fy":[7.89407,7.89407,7.89407,7.89407]},
+ {"t":5.59117, "x":7.27897, "y":4.82651, "heading":-1.07992, "vx":-0.70755, "vy":0.88133, "omega":0.4998, "ax":-0.04471, "ay":0.45213, "alpha":0.0, "fx":[-0.76048,-0.76048,-0.76048,-0.76048], "fy":[7.69054,7.69054,7.69054,7.69054]},
+ {"t":5.61573, "x":7.26158, "y":4.84829, "heading":-1.06765, "vx":-0.70865, "vy":0.89244, "omega":0.4998, "ax":-0.03835, "ay":0.44186, "alpha":0.0, "fx":[-0.65238,-0.65238,-0.65238,-0.65238], "fy":[7.51595,7.51595,7.51595,7.51595]},
+ {"t":5.64028, "x":7.24417, "y":4.87034, "heading":-1.05538, "vx":-0.70959, "vy":0.90328, "omega":0.4998, "ax":-0.03254, "ay":0.43313, "alpha":0.0, "fx":[-0.55352,-0.55352,-0.55352,-0.55352], "fy":[7.36746,7.36746,7.36746,7.36746]},
+ {"t":5.66483, "x":7.22674, "y":4.89265, "heading":-1.04311, "vx":-0.71039, "vy":0.91392, "omega":0.4998, "ax":-0.0272, "ay":0.42579, "alpha":0.0, "fx":[-0.46272,-0.46272,-0.46273,-0.46272], "fy":[7.24249,7.24249,7.24249,7.24249]},
+ {"t":5.68939, "x":7.20928, "y":4.91521, "heading":-1.03084, "vx":-0.71106, "vy":0.92437, "omega":0.4998, "ax":-0.02228, "ay":0.41968, "alpha":0.0, "fx":[-0.37893,-0.37893,-0.37894,-0.37894], "fy":[7.13866,7.13866,7.13866,7.13866]},
+ {"t":5.71394, "x":7.19182, "y":4.93804, "heading":-1.01856, "vx":-0.71161, "vy":0.93468, "omega":0.4998, "ax":-0.01771, "ay":0.41469, "alpha":0.0, "fx":[-0.3012,-0.3012,-0.3012,-0.3012], "fy":[7.05378,7.05378,7.05378,7.05378]},
+ {"t":5.73849, "x":7.17434, "y":4.96111, "heading":-1.00629, "vx":-0.71204, "vy":0.94486, "omega":0.4998, "ax":-0.01344, "ay":0.4107, "alpha":0.0, "fx":[-0.22869,-0.22869,-0.22869,-0.22869], "fy":[6.98584,6.98584,6.98584,6.98584]},
+ {"t":5.76305, "x":7.15685, "y":4.98444, "heading":-0.99402, "vx":-0.71237, "vy":0.95495, "omega":0.4998, "ax":-0.00944, "ay":0.40758, "alpha":0.0, "fx":[-0.16062,-0.16062,-0.16062,-0.16062], "fy":[6.9328,6.9328,6.9328,6.9328]},
+ {"t":5.7876, "x":7.13936, "y":5.00801, "heading":-0.98175, "vx":-0.7126, "vy":0.96495, "omega":0.4998, "ax":-0.00566, "ay":0.40525, "alpha":0.0, "fx":[-0.09631,-0.09631,-0.09631,-0.09631], "fy":[6.8932,6.8932,6.8932,6.8932]},
+ {"t":5.81216, "x":7.12186, "y":5.03182, "heading":-0.96948, "vx":-0.71274, "vy":0.9749, "omega":0.4998, "ax":-0.00207, "ay":0.40362, "alpha":0.0, "fx":[-0.03517,-0.03517,-0.03517,-0.03517], "fy":[6.8655,6.8655,6.8655,6.8655]},
+ {"t":5.83671, "x":7.10436, "y":5.05588, "heading":-0.9572, "vx":-0.71279, "vy":0.98481, "omega":0.4998, "ax":0.00137, "ay":0.40261, "alpha":0.0, "fx":[0.02333,0.02333,0.02333,0.02333], "fy":[6.8483,6.8483,6.8483,6.8483]},
+ {"t":5.86126, "x":7.08686, "y":5.08018, "heading":-0.94493, "vx":-0.71276, "vy":0.9947, "omega":0.4998, "ax":0.00468, "ay":0.40214, "alpha":0.0, "fx":[0.07968,0.07968,0.07968,0.07968], "fy":[6.84032,6.84032,6.84032,6.84032]},
+ {"t":5.88582, "x":7.06936, "y":5.10473, "heading":-0.93266, "vx":-0.71264, "vy":1.00457, "omega":0.4998, "ax":0.0079, "ay":0.40215, "alpha":0.0, "fx":[0.13432,0.13431,0.13431,0.13431], "fy":[6.84038,6.84038,6.84038,6.84038]},
+ {"t":5.91037, "x":7.05186, "y":5.12951, "heading":-0.92039, "vx":-0.71245, "vy":1.01445, "omega":0.4998, "ax":0.01101, "ay":0.40257, "alpha":0.0, "fx":[0.18736,0.18736,0.18736,0.18736], "fy":[6.84758,6.84758,6.84758,6.84758]},
+ {"t":5.93492, "x":7.03437, "y":5.15454, "heading":-0.90812, "vx":-0.71218, "vy":1.02433, "omega":0.4998, "ax":0.01407, "ay":0.40333, "alpha":0.0, "fx":[0.23936,0.23936,0.23936,0.23936], "fy":[6.86061,6.86061,6.86061,6.86061]},
+ {"t":5.95948, "x":7.01689, "y":5.17982, "heading":-0.89584, "vx":-0.71183, "vy":1.03424, "omega":0.4998, "ax":0.01708, "ay":0.4044, "alpha":0.0, "fx":[0.29059,0.29059,0.29059,0.29059], "fy":[6.87873,6.87873,6.87873,6.87873]},
+ {"t":5.98403, "x":6.99942, "y":5.20533, "heading":-0.88357, "vx":-0.71141, "vy":1.04416, "omega":0.4998, "ax":0.02006, "ay":0.40573, "alpha":0.0, "fx":[0.34128,0.34128,0.34128,0.34128], "fy":[6.90138,6.90138,6.90138,6.90138]},
+ {"t":6.00858, "x":6.98196, "y":5.23109, "heading":-0.8713, "vx":-0.71092, "vy":1.05413, "omega":0.4998, "ax":0.02302, "ay":0.40729, "alpha":0.0, "fx":[0.39164,0.39164,0.39164,0.39164], "fy":[6.92781,6.92781,6.92781,6.92781]},
+ {"t":6.03314, "x":6.96451, "y":5.2571, "heading":-0.85903, "vx":-0.71036, "vy":1.06413, "omega":0.4998, "ax":0.02598, "ay":0.409, "alpha":0.0, "fx":[0.44185,0.44185,0.44185,0.44185], "fy":[6.95703,6.95703,6.95703,6.95703]},
+ {"t":6.05769, "x":6.94707, "y":5.28335, "heading":-0.84676, "vx":-0.70972, "vy":1.07417, "omega":0.4998, "ax":0.02893, "ay":0.41085, "alpha":0.0, "fx":[0.49205,0.49205,0.49205,0.49205], "fy":[6.98849,6.98849,6.98849,6.98849]},
+ {"t":6.08225, "x":6.92966, "y":5.30985, "heading":-0.83449, "vx":-0.70901, "vy":1.08426, "omega":0.4998, "ax":0.03188, "ay":0.41281, "alpha":0.0, "fx":[0.54234,0.54234,0.54234,0.54234], "fy":[7.02183,7.02183,7.02183,7.02183]},
+ {"t":6.1068, "x":6.91226, "y":5.3366, "heading":-0.82221, "vx":-0.70823, "vy":1.09439, "omega":0.4998, "ax":0.03485, "ay":0.41485, "alpha":0.0, "fx":[0.59282,0.59282,0.59282,0.59282], "fy":[7.0565,7.0565,7.0565,7.0565]},
+ {"t":6.13135, "x":6.89488, "y":5.36359, "heading":-0.80994, "vx":-0.70737, "vy":1.10458, "omega":0.4998, "ax":0.03783, "ay":0.41695, "alpha":0.0, "fx":[0.64355,0.64355,0.64355,0.64355], "fy":[7.09224,7.09224,7.09224,7.09224]},
+ {"t":6.15591, "x":6.87752, "y":5.39084, "heading":-0.79767, "vx":-0.70644, "vy":1.11482, "omega":0.4998, "ax":0.04083, "ay":0.41906, "alpha":0.0, "fx":[0.69455,0.69455,0.69455,0.69455], "fy":[7.12814,7.12814,7.12814,7.12814]},
+ {"t":6.18046, "x":6.86019, "y":5.41834, "heading":-0.7854, "vx":-0.70544, "vy":1.12511, "omega":0.4998, "ax":0.04385, "ay":0.42119, "alpha":0.0, "fx":[0.74583,0.74583,0.74583,0.74583], "fy":[7.16424,7.16424,7.16424,7.16424]},
+ {"t":6.20501, "x":6.84288, "y":5.44609, "heading":-0.77313, "vx":-0.70436, "vy":1.13545, "omega":0.4998, "ax":0.04688, "ay":0.42328, "alpha":0.0, "fx":[0.7974,0.7974,0.7974,0.7974], "fy":[7.19995,7.19995,7.19995,7.19995]},
+ {"t":6.22957, "x":6.8256, "y":5.4741, "heading":-0.76085, "vx":-0.70321, "vy":1.14584, "omega":0.4998, "ax":0.04993, "ay":0.42535, "alpha":0.0, "fx":[0.84923,0.84923,0.84923,0.84923], "fy":[7.2351,7.2351,7.2351,7.2351]},
+ {"t":6.25412, "x":6.80835, "y":5.50236, "heading":-0.74858, "vx":-0.70199, "vy":1.15629, "omega":0.4998, "ax":0.05297, "ay":0.42735, "alpha":0.0, "fx":[0.90101,0.90101,0.90101,0.90101], "fy":[7.26913,7.26913,7.26913,7.26913]},
+ {"t":6.27867, "x":6.79113, "y":5.53088, "heading":-0.73631, "vx":-0.70068, "vy":1.16678, "omega":0.4998, "ax":0.05602, "ay":0.42928, "alpha":0.0, "fx":[0.95289,0.95289,0.95289,0.95289], "fy":[7.30197,7.30197,7.30197,7.30197]},
+ {"t":6.30323, "x":6.77394, "y":5.55966, "heading":-0.72404, "vx":-0.69931, "vy":1.17732, "omega":0.4998, "ax":0.05907, "ay":0.43114, "alpha":0.0, "fx":[1.00479,1.00479,1.00479,1.00479], "fy":[7.33355,7.33355,7.33355,7.33355]},
+ {"t":6.32778, "x":6.75679, "y":5.5887, "heading":-0.71177, "vx":-0.69786, "vy":1.18791, "omega":0.4998, "ax":0.06211, "ay":0.43288, "alpha":0.0, "fx":[1.05655,1.05655,1.05655,1.05655], "fy":[7.36309,7.36309,7.36309,7.36309]},
+ {"t":6.35234, "x":6.73967, "y":5.61799, "heading":-0.6995, "vx":-0.69633, "vy":1.19853, "omega":0.4998, "ax":0.06514, "ay":0.43451, "alpha":0.0, "fx":[1.10803,1.10803,1.10803,1.10803], "fy":[7.39081,7.39081,7.39081,7.39081]},
+ {"t":6.37689, "x":6.72259, "y":5.64755, "heading":-0.68722, "vx":-0.69473, "vy":1.2092, "omega":0.4998, "ax":0.06814, "ay":0.43601, "alpha":0.0, "fx":[1.15904,1.15904,1.15904,1.15904], "fy":[7.41643,7.41643,7.41643,7.41643]},
+ {"t":6.40144, "x":6.70556, "y":5.67737, "heading":-0.67495, "vx":-0.69306, "vy":1.21991, "omega":0.4998, "ax":0.0711, "ay":0.43737, "alpha":0.0, "fx":[1.20936,1.20936,1.20936,1.20936], "fy":[7.43946,7.43946,7.43946,7.43946]},
+ {"t":6.426, "x":6.68856, "y":5.70746, "heading":-0.66268, "vx":-0.69132, "vy":1.23065, "omega":0.4998, "ax":0.074, "ay":0.43858, "alpha":0.0, "fx":[1.25872,1.25872,1.25872,1.25872], "fy":[7.46009,7.46009,7.46009,7.46009]},
+ {"t":6.45055, "x":6.67161, "y":5.73781, "heading":-0.65041, "vx":-0.6895, "vy":1.24142, "omega":0.4998, "ax":0.07683, "ay":0.43962, "alpha":0.0, "fx":[1.30689,1.30689,1.30689,1.30689], "fy":[7.47781,7.47781,7.47781,7.47781]},
+ {"t":6.4751, "x":6.6547, "y":5.76842, "heading":-0.63814, "vx":-0.68761, "vy":1.25221, "omega":0.4998, "ax":0.07956, "ay":0.44048, "alpha":0.0, "fx":[1.35337,1.35337,1.35337,1.35337], "fy":[7.49246,7.49246,7.49246,7.49246]},
+ {"t":6.49966, "x":6.63784, "y":5.7993, "heading":-0.62586, "vx":-0.68566, "vy":1.26303, "omega":0.4998, "ax":0.08218, "ay":0.44117, "alpha":0.0, "fx":[1.39789,1.39789,1.39789,1.39789], "fy":[7.50414,7.50414,7.50414,7.50414]},
+ {"t":6.52421, "x":6.62103, "y":5.83045, "heading":-0.61359, "vx":-0.68364, "vy":1.27386, "omega":0.4998, "ax":0.08466, "ay":0.44164, "alpha":0.0, "fx":[1.44005,1.44005,1.44005,1.44005], "fy":[7.51217,7.51217,7.51217,7.51217]},
+ {"t":6.54876, "x":6.60427, "y":5.86186, "heading":-0.60132, "vx":-0.68156, "vy":1.2847, "omega":0.4998, "ax":0.08697, "ay":0.44191, "alpha":0.0, "fx":[1.4794,1.4794,1.4794,1.4794], "fy":[7.51668,7.51668,7.51668,7.51668]},
+ {"t":6.57332, "x":6.58756, "y":5.89353, "heading":-0.58905, "vx":-0.67943, "vy":1.29555, "omega":0.4998, "ax":0.08909, "ay":0.44194, "alpha":0.0, "fx":[1.51541,1.51541,1.51541,1.51541], "fy":[7.51736,7.51736,7.51736,7.51736]},
+ {"t":6.59787, "x":6.57091, "y":5.92548, "heading":-0.57678, "vx":-0.67724, "vy":1.3064, "omega":0.4998, "ax":0.09097, "ay":0.44174, "alpha":0.0, "fx":[1.54744,1.54744,1.54744,1.54744], "fy":[7.51387,7.51387,7.51387,7.51387]},
+ {"t":6.62243, "x":6.55431, "y":5.95769, "heading":-0.5645, "vx":-0.675, "vy":1.31725, "omega":0.4998, "ax":0.09258, "ay":0.44127, "alpha":0.0, "fx":[1.57476,1.57476,1.57476,1.57476], "fy":[7.50591,7.50591,7.50591,7.50591]},
+ {"t":6.64698, "x":6.53776, "y":5.99016, "heading":-0.55223, "vx":-0.67273, "vy":1.32808, "omega":0.4998, "ax":0.09387, "ay":0.44053, "alpha":0.0, "fx":[1.59675,1.59675,1.59675,1.59675], "fy":[7.49321,7.49321,7.49321,7.49321]},
+ {"t":6.67153, "x":6.52127, "y":6.02291, "heading":-0.53996, "vx":-0.67043, "vy":1.3389, "omega":0.4998, "ax":0.09479, "ay":0.43949, "alpha":0.0, "fx":[1.61236,1.61236,1.61236,1.61236], "fy":[7.47561,7.47561,7.47561,7.47561]},
+ {"t":6.69609, "x":6.50484, "y":6.05591, "heading":-0.52769, "vx":-0.6681, "vy":1.34969, "omega":0.4998, "ax":0.09528, "ay":0.43812, "alpha":0.0, "fx":[1.62064,1.62064,1.62064,1.62064], "fy":[7.45238,7.45238,7.45238,7.45238]},
+ {"t":6.72064, "x":6.48846, "y":6.08919, "heading":-0.51542, "vx":-0.66576, "vy":1.36045, "omega":0.4998, "ax":0.09527, "ay":0.43641, "alpha":0.0, "fx":[1.62047,1.62047,1.62047,1.62047], "fy":[7.42313,7.42313,7.42313,7.42313]},
+ {"t":6.74519, "x":6.47214, "y":6.12272, "heading":-0.50315, "vx":-0.66342, "vy":1.37117, "omega":0.4998, "ax":0.09468, "ay":0.43431, "alpha":0.0, "fx":[1.61056,1.61056,1.61056,1.61056], "fy":[7.38748,7.38748,7.38748,7.38748]},
+ {"t":6.76975, "x":6.45588, "y":6.15652, "heading":-0.49087, "vx":-0.6611, "vy":1.38183, "omega":0.4998, "ax":0.09345, "ay":0.43179, "alpha":0.0, "fx":[1.58951,1.58951,1.58951,1.58951], "fy":[7.34465,7.34465,7.34465,7.34465]},
+ {"t":6.7943, "x":6.43968, "y":6.19058, "heading":-0.4786, "vx":-0.6588, "vy":1.39243, "omega":0.4998, "ax":0.09147, "ay":0.42881, "alpha":0.0, "fx":[1.55581,1.55581,1.55581,1.55581], "fy":[7.29389,7.29389,7.29389,7.29389]},
+ {"t":6.81885, "x":6.42353, "y":6.2249, "heading":-0.46633, "vx":-0.65656, "vy":1.40296, "omega":0.4998, "ax":0.08862, "ay":0.4253, "alpha":0.0, "fx":[1.50743,1.50743,1.50743,1.50743], "fy":[7.23424,7.23424,7.23424,7.23424]},
+ {"t":6.84341, "x":6.40744, "y":6.25947, "heading":-0.45406, "vx":-0.65438, "vy":1.4134, "omega":0.4998, "ax":0.08479, "ay":0.42121, "alpha":0.0, "fx":[1.44231,1.44231,1.44231,1.44231], "fy":[7.16464,7.16464,7.16464,7.16464]},
+ {"t":6.86796, "x":6.39139, "y":6.2943, "heading":-0.44179, "vx":-0.6523, "vy":1.42374, "omega":0.4998, "ax":0.07984, "ay":0.41645, "alpha":0.0, "fx":[1.35808,1.35807,1.35807,1.35808], "fy":[7.08363,7.08364,7.08363,7.08363]},
+ {"t":6.89252, "x":6.3754, "y":6.32939, "heading":-0.42951, "vx":-0.65034, "vy":1.43397, "omega":0.4998, "ax":0.07361, "ay":0.41093, "alpha":0.0, "fx":[1.25209,1.25209,1.25209,1.25209], "fy":[6.98979,6.98979,6.98979,6.98979]},
+ {"t":6.91707, "x":6.35946, "y":6.36472, "heading":-0.41724, "vx":-0.64853, "vy":1.44406, "omega":0.4998, "ax":0.06594, "ay":0.40453, "alpha":0.0, "fx":[1.1216,1.1216,1.1216,1.1216], "fy":[6.88101,6.88101,6.88101,6.88101]},
+ {"t":6.94162, "x":6.34355, "y":6.4003, "heading":-0.40497, "vx":-0.64691, "vy":1.45399, "omega":0.4998, "ax":0.05661, "ay":0.39712, "alpha":0.0, "fx":[0.96287,0.96287,0.96287,0.96287], "fy":[6.75488,6.75488,6.75488,6.75488]},
+ {"t":6.96618, "x":6.32769, "y":6.43612, "heading":-0.3927, "vx":-0.64552, "vy":1.46374, "omega":0.4998, "ax":0.04538, "ay":0.3885, "alpha":0.0, "fx":[0.77195,0.77195,0.77195,0.77195], "fy":[6.60835,6.60835,6.60835,6.60835]},
+ {"t":6.99073, "x":6.31185, "y":6.47218, "heading":-0.38043, "vx":-0.64441, "vy":1.47328, "omega":0.4998, "ax":0.03203, "ay":0.37846, "alpha":0.0, "fx":[0.54479,0.54479,0.54478,0.54479], "fy":[6.43752,6.43752,6.43752,6.43752]},
+ {"t":7.01528, "x":6.29604, "y":6.50847, "heading":-0.36816, "vx":-0.64362, "vy":1.48257, "omega":0.4998, "ax":0.01625, "ay":0.36671, "alpha":0.0, "fx":[0.27639,0.27639,0.27639,0.27639], "fy":[6.23769,6.23769,6.23769,6.23769]},
+ {"t":7.03984, "x":6.28024, "y":6.54498, "heading":-0.35588, "vx":-0.64322, "vy":1.49158, "omega":0.4998, "ax":-0.00228, "ay":0.3529, "alpha":0.0, "fx":[-0.03882,-0.03882,-0.03883,-0.03882], "fy":[6.00281,6.00281,6.00281,6.00281]},
+ {"t":7.06439, "x":6.26444, "y":6.58171, "heading":-0.34361, "vx":-0.64328, "vy":1.50024, "omega":0.4998, "ax":-0.02392, "ay":0.33659, "alpha":0.0, "fx":[-0.40685,-0.40685,-0.40685,-0.40685], "fy":[5.72524,5.72524,5.72524,5.72524]},
+ {"t":7.08894, "x":6.24864, "y":6.61865, "heading":-0.33134, "vx":-0.64386, "vy":1.50851, "omega":0.4998, "ax":-0.04902, "ay":0.3172, "alpha":0.0, "fx":[-0.83388,-0.83388,-0.83388,-0.83388], "fy":[5.39549,5.39549,5.39549,5.39549]},
+ {"t":7.1135, "x":6.23282, "y":6.65578, "heading":-0.31907, "vx":-0.64507, "vy":1.5163, "omega":0.4998, "ax":-0.07806, "ay":0.294, "alpha":0.0, "fx":[-1.32778,-1.32778,-1.32778,-1.32778], "fy":[5.00079,5.00079,5.00079,5.00079]},
+ {"t":7.13805, "x":6.21696, "y":6.6931, "heading":-0.3068, "vx":-0.64698, "vy":1.52352, "omega":0.4998, "ax":-0.11144, "ay":0.26603, "alpha":0.0, "fx":[-1.89561,-1.89561,-1.89561,-1.89561], "fy":[4.52516,4.52516,4.52516,4.52516]},
+ {"t":7.16261, "x":6.20104, "y":6.73059, "heading":-0.29452, "vx":-0.64972, "vy":1.53005, "omega":0.4998, "ax":-0.14969, "ay":0.23208, "alpha":0.0, "fx":[-2.54625,-2.54626,-2.54626,-2.54626], "fy":[3.94769,3.94769,3.94769,3.94769]},
+ {"t":7.18716, "x":6.18504, "y":6.76823, "heading":-0.28225, "vx":-0.6534, "vy":1.53575, "omega":0.4998, "ax":-0.19328, "ay":0.19055, "alpha":0.0, "fx":[-3.28759,-3.28759,-3.28759,-3.28759], "fy":[3.24113,3.24113,3.24113,3.24113]},
+ {"t":7.21171, "x":6.16894, "y":6.80599, "heading":-0.26998, "vx":-0.65814, "vy":1.54042, "omega":0.4998, "ax":-0.24263, "ay":0.13934, "alpha":0.0, "fx":[-4.12706,-4.12706,-4.12707,-4.12706], "fy":[2.37011,2.37011,2.37011,2.37011]},
+ {"t":7.23627, "x":6.1527, "y":6.84386, "heading":-0.25771, "vx":-0.6641, "vy":1.54385, "omega":0.4998, "ax":-0.29818, "ay":0.07571, "alpha":0.0, "fx":[-5.07195,-5.07195,-5.07196,-5.07195], "fy":[1.28778,1.28778,1.28778,1.28778]},
+ {"t":7.26082, "x":6.13631, "y":6.88179, "heading":-0.24544, "vx":-0.67142, "vy":1.54571, "omega":0.4998, "ax":-0.36012, "ay":-0.00388, "alpha":0.0, "fx":[-6.12551,-6.12551,-6.12551,-6.12551], "fy":[-0.06598,-0.06598,-0.06598,-0.06598]},
+ {"t":7.28537, "x":6.11971, "y":6.91974, "heading":-0.23317, "vx":-0.68026, "vy":1.54561, "omega":0.4998, "ax":-0.42847, "ay":-0.10408, "alpha":0.0, "fx":[-7.28809,-7.2881,-7.2881,-7.28809], "fy":[-1.77031,-1.77031,-1.77031,-1.77032]},
+ {"t":7.30993, "x":6.10288, "y":6.95766, "heading":-0.22089, "vx":-0.69078, "vy":1.54305, "omega":0.4998, "ax":-0.50283, "ay":-0.2308, "alpha":0.0, "fx":[-8.55299,-8.553,-8.553,-8.55299], "fy":[-3.92579,-3.92579,-3.92579,-3.92579]},
+ {"t":7.33448, "x":6.08577, "y":6.99548, "heading":-0.20862, "vx":-0.70313, "vy":1.53739, "omega":0.4998, "ax":-0.58227, "ay":-0.39155, "alpha":0.0, "fx":[-9.90424,-9.90425,-9.90425,-9.90425], "fy":[-6.66009,-6.66009,-6.6601,-6.6601]},
+ {"t":7.35903, "x":6.06833, "y":7.03311, "heading":-0.19635, "vx":-0.71743, "vy":1.52777, "omega":0.4998, "ax":-0.66496, "ay":-0.59536, "alpha":0.0, "fx":[-11.31079,-11.31079,-11.31079,-11.31079], "fy":[-10.12683,-10.12683,-10.12683,-10.12683]},
+ {"t":7.38359, "x":6.05051, "y":7.07044, "heading":-0.18408, "vx":-0.73375, "vy":1.51316, "omega":0.4998, "ax":-0.74798, "ay":-0.8526, "alpha":0.0, "fx":[-12.72284,-12.72285,-12.72285,-12.72285], "fy":[-14.50256,-14.50256,-14.50257,-14.50257]},
+ {"t":7.40814, "x":6.03227, "y":7.10734, "heading":-0.17181, "vx":-0.75212, "vy":1.49222, "omega":0.4998, "ax":-0.82705, "ay":-1.17401, "alpha":0.0, "fx":[-14.06782,-14.06782,-14.06782,-14.06782], "fy":[-19.96964,-19.96964,-19.96965,-19.96965]},
+ {"t":7.4327, "x":6.01355, "y":7.14362, "heading":-0.15953, "vx":-0.77243, "vy":1.46339, "omega":0.4998, "ax":-0.89643, "ay":-1.56863, "alpha":0.0, "fx":[-15.24798,-15.24799,-15.24799,-15.24798], "fy":[-26.682,-26.682,-26.682,-26.68201]},
+ {"t":7.45725, "x":5.99432, "y":7.17908, "heading":-0.14726, "vx":-0.79444, "vy":1.42488, "omega":0.4998, "ax":-0.94929, "ay":-2.04036, "alpha":0.0, "fx":[-16.14707,-16.14708,-16.14708,-16.14707], "fy":[-34.70589,-34.70589,-34.70589,-34.70589]},
+ {"t":7.4818, "x":5.97453, "y":7.21345, "heading":-0.13499, "vx":-0.81775, "vy":1.37478, "omega":0.4998, "ax":-0.9786, "ay":-2.58411, "alpha":0.0, "fx":[-16.64569,-16.64569,-16.64569,-16.64569], "fy":[-43.9549,-43.9549,-43.9549,-43.9549]},
+ {"t":7.50636, "x":5.95415, "y":7.24643, "heading":-0.12272, "vx":-0.84177, "vy":1.31133, "omega":0.4998, "ax":-0.97888, "ay":-3.18268, "alpha":0.0, "fx":[-16.65055,-16.65055,-16.65055,-16.65055], "fy":[-54.13653,-54.13653,-54.13653,-54.13653]},
+ {"t":7.53091, "x":5.93319, "y":7.27767, "heading":-0.11045, "vx":-0.86581, "vy":1.23319, "omega":0.4998, "ax":-0.94824, "ay":-3.80816, "alpha":0.0, "fx":[-16.12931,-16.12931,-16.12931,-16.12931], "fy":[-64.77571,-64.77571,-64.77571,-64.77571]},
+ {"t":7.55546, "x":5.91164, "y":7.3068, "heading":-0.09817, "vx":-0.88909, "vy":1.13968, "omega":0.4998, "ax":-0.88944, "ay":-4.42806, "alpha":0.0, "fx":[-15.12907,-15.12907,-15.12907,-15.12907], "fy":[-75.32008,-75.32008,-75.32008,-75.32008]},
+ {"t":7.58002, "x":5.88955, "y":7.33345, "heading":-0.0859, "vx":-0.91093, "vy":1.03096, "omega":0.4998, "ax":-0.80919, "ay":-5.01404, "alpha":0.0, "fx":[-13.76416,-13.76416,-13.76416,-13.76416], "fy":[-85.28744,-85.28744,-85.28744,-85.28744]},
+ {"t":7.60457, "x":5.86693, "y":7.35725, "heading":-0.07363, "vx":-0.9308, "vy":0.90784, "omega":0.4998, "ax":-0.71604, "ay":-5.54782, "alpha":0.0, "fx":[-12.1797,-12.1797,-12.1797,-12.1797], "fy":[-94.36686,-94.36686,-94.36686,-94.36686]},
+ {"t":7.62912, "x":5.84386, "y":7.37787, "heading":-0.06136, "vx":-0.94838, "vy":0.77162, "omega":0.4998, "ax":-0.61806, "ay":-6.022, "alpha":0.0, "fx":[-10.51308,-10.51308,-10.51308,-10.51308], "fy":[-102.43252,-102.43252,-102.43252,-102.43252]},
+ {"t":7.65368, "x":5.82039, "y":7.395, "heading":-0.04909, "vx":-0.96356, "vy":0.62376, "omega":0.4998, "ax":-0.52157, "ay":-6.43713, "alpha":0.0, "fx":[-8.87169,-8.87169,-8.87169,-8.87169], "fy":[-109.49375,-109.49375,-109.49375,-109.49375]},
+ {"t":7.67823, "x":5.79658, "y":7.40837, "heading":-0.03682, "vx":-0.97636, "vy":0.46571, "omega":0.4998, "ax":-0.4308, "ay":-6.79807, "alpha":0.0, "fx":[-7.32773,-7.32773,-7.32773,-7.32773], "fy":[-115.63324,-115.63324,-115.63324,-115.63324]},
+ {"t":7.70279, "x":5.77247, "y":7.41776, "heading":-0.02454, "vx":-0.98694, "vy":0.29879, "omega":0.4998, "ax":-0.34828, "ay":-7.11122, "alpha":-0.00003, "fx":[-5.924,-5.92401,-5.92429,-5.92428], "fy":[-120.9599,-120.95984,-120.95983,-120.95989]},
+ {"t":7.72734, "x":5.74814, "y":7.42295, "heading":-0.01227, "vx":-0.99549, "vy":0.12418, "omega":0.4998, "ax":-0.14922, "ay":-5.22788, "alpha":-20.35532, "fx":[65.16188,110.36447,-114.32447,-71.35441], "fy":[-126.19003,-55.46564,-50.91362,-123.12938]},
+ {"t":7.75189, "x":5.72365, "y":7.42443, "heading":0.0, "vx":-0.99916, "vy":-0.00418, "omega":0.0, "ax":-0.00406, "ay":-0.53282, "alpha":0.0, "fx":[-0.06907,-0.06907,-0.06907,-0.06907], "fy":[-9.06304,-9.06304,-9.06304,-9.06304]},
+ {"t":7.79979, "x":5.67579, "y":7.42361, "heading":0.0, "vx":-0.99935, "vy":-0.0297, "omega":0.0, "ax":0.00004, "ay":-0.00138, "alpha":0.0, "fx":[0.0007,0.0007,0.0007,0.0007], "fy":[-0.02352,-0.02352,-0.02352,-0.02352]},
+ {"t":7.84768, "x":5.62793, "y":7.42219, "heading":0.0, "vx":-0.99935, "vy":-0.02976, "omega":0.0, "ax":-0.00008, "ay":0.00258, "alpha":0.0, "fx":[-0.00131,-0.00131,-0.00131,-0.00131], "fy":[0.04396,0.04396,0.04396,0.04396]},
+ {"t":7.89558, "x":5.58006, "y":7.42077, "heading":0.0, "vx":-0.99935, "vy":-0.02964, "omega":0.0, "ax":-0.00008, "ay":0.0026, "alpha":0.0, "fx":[-0.00131,-0.00131,-0.00131,-0.00131], "fy":[0.04424,0.04424,0.04424,0.04424]},
+ {"t":7.94347, "x":5.5322, "y":7.41935, "heading":0.0, "vx":-0.99936, "vy":-0.02952, "omega":0.0, "ax":-0.00008, "ay":0.00259, "alpha":0.0, "fx":[-0.0013,-0.0013,-0.0013,-0.0013], "fy":[0.04398,0.04398,0.04398,0.04398]},
+ {"t":7.99136, "x":5.48433, "y":7.41794, "heading":0.0, "vx":-0.99936, "vy":-0.02939, "omega":0.0, "ax":-0.00008, "ay":0.00257, "alpha":0.0, "fx":[-0.00128,-0.00128,-0.00128,-0.00128], "fy":[0.04373,0.04373,0.04373,0.04373]},
+ {"t":8.03926, "x":5.43647, "y":7.41654, "heading":0.0, "vx":-0.99936, "vy":-0.02927, "omega":0.0, "ax":-0.00007, "ay":0.00256, "alpha":0.0, "fx":[-0.00127,-0.00127,-0.00127,-0.00127], "fy":[0.04348,0.04348,0.04348,0.04348]},
+ {"t":8.08715, "x":5.38861, "y":7.41514, "heading":0.0, "vx":-0.99937, "vy":-0.02915, "omega":0.0, "ax":-0.00007, "ay":0.00254, "alpha":0.0, "fx":[-0.00126,-0.00126,-0.00126,-0.00126], "fy":[0.04323,0.04323,0.04323,0.04323]},
+ {"t":8.13505, "x":5.34074, "y":7.41374, "heading":0.0, "vx":-0.99937, "vy":-0.02903, "omega":0.0, "ax":-0.00007, "ay":0.00253, "alpha":0.0, "fx":[-0.00125,-0.00125,-0.00125,-0.00125], "fy":[0.04299,0.04299,0.04299,0.04299]},
+ {"t":8.18294, "x":5.29288, "y":7.41236, "heading":0.0, "vx":-0.99937, "vy":-0.0289, "omega":0.0, "ax":-0.00007, "ay":0.00251, "alpha":0.0, "fx":[-0.00123,-0.00123,-0.00123,-0.00123], "fy":[0.04276,0.04276,0.04276,0.04276]},
+ {"t":8.23084, "x":5.24501, "y":7.41097, "heading":0.0, "vx":-0.99938, "vy":-0.02878, "omega":0.0, "ax":-0.00007, "ay":0.0025, "alpha":0.0, "fx":[-0.00122,-0.00122,-0.00122,-0.00122], "fy":[0.04252,0.04252,0.04252,0.04252]},
+ {"t":8.27873, "x":5.19715, "y":7.4096, "heading":0.0, "vx":-0.99938, "vy":-0.02866, "omega":0.0, "ax":-0.00007, "ay":0.00249, "alpha":0.0, "fx":[-0.00121,-0.00121,-0.00121,-0.00121], "fy":[0.04229,0.04229,0.04229,0.04229]},
+ {"t":8.32663, "x":5.14928, "y":7.40823, "heading":0.0, "vx":-0.99938, "vy":-0.02855, "omega":0.0, "ax":-0.00007, "ay":0.00247, "alpha":0.0, "fx":[-0.0012,-0.0012,-0.0012,-0.0012], "fy":[0.04207,0.04207,0.04207,0.04207]},
+ {"t":8.37452, "x":5.10142, "y":7.40686, "heading":0.0, "vx":-0.99939, "vy":-0.02843, "omega":0.0, "ax":-0.00007, "ay":0.00246, "alpha":0.0, "fx":[-0.00119,-0.00119,-0.00119,-0.00119], "fy":[0.04185,0.04185,0.04185,0.04185]},
+ {"t":8.42241, "x":5.05355, "y":7.40551, "heading":0.0, "vx":-0.99939, "vy":-0.02831, "omega":0.0, "ax":-0.00007, "ay":0.00245, "alpha":0.0, "fx":[-0.00118,-0.00118,-0.00118,-0.00118], "fy":[0.04163,0.04163,0.04163,0.04163]},
+ {"t":8.47031, "x":5.00569, "y":7.40415, "heading":0.0, "vx":-0.99939, "vy":-0.02819, "omega":0.0, "ax":-0.00007, "ay":0.00243, "alpha":0.0, "fx":[-0.00117,-0.00117,-0.00117,-0.00117], "fy":[0.04141,0.04141,0.04141,0.04141]},
+ {"t":8.5182, "x":4.95782, "y":7.40281, "heading":0.0, "vx":-0.9994, "vy":-0.02808, "omega":0.0, "ax":-0.00007, "ay":0.00242, "alpha":0.0, "fx":[-0.00116,-0.00116,-0.00116,-0.00116], "fy":[0.0412,0.0412,0.0412,0.0412]},
+ {"t":8.5661, "x":4.90996, "y":7.40146, "heading":0.0, "vx":-0.9994, "vy":-0.02796, "omega":0.0, "ax":-0.00007, "ay":0.00241, "alpha":0.0, "fx":[-0.00114,-0.00114,-0.00114,-0.00114], "fy":[0.04099,0.04099,0.04099,0.04099]},
+ {"t":8.61399, "x":4.86209, "y":7.40013, "heading":0.0, "vx":-0.9994, "vy":-0.02784, "omega":0.0, "ax":-0.00007, "ay":0.0024, "alpha":0.0, "fx":[-0.00113,-0.00113,-0.00113,-0.00113], "fy":[0.04079,0.04079,0.04079,0.04079]},
+ {"t":8.66189, "x":4.81423, "y":7.3988, "heading":0.0, "vx":-0.99941, "vy":-0.02773, "omega":0.0, "ax":-0.00007, "ay":0.00239, "alpha":0.0, "fx":[-0.00112,-0.00112,-0.00112,-0.00112], "fy":[0.04059,0.04059,0.04059,0.04059]},
+ {"t":8.70978, "x":4.76636, "y":7.39747, "heading":0.0, "vx":-0.99941, "vy":-0.02761, "omega":0.0, "ax":-0.00007, "ay":0.00237, "alpha":0.0, "fx":[-0.00111,-0.00111,-0.00111,-0.00111], "fy":[0.04039,0.04039,0.04039,0.04039]},
+ {"t":8.75767, "x":4.71849, "y":7.39615, "heading":0.0, "vx":-0.99941, "vy":-0.0275, "omega":0.0, "ax":-0.00006, "ay":0.00236, "alpha":0.0, "fx":[-0.0011,-0.0011,-0.0011,-0.0011], "fy":[0.04019,0.04019,0.04019,0.04019]},
+ {"t":8.80557, "x":4.67063, "y":7.39484, "heading":0.0, "vx":-0.99942, "vy":-0.02739, "omega":0.0, "ax":-0.00006, "ay":0.00235, "alpha":0.0, "fx":[-0.00109,-0.00109,-0.00109,-0.00109], "fy":[0.04,0.04,0.04,0.04]},
+ {"t":8.85346, "x":4.62276, "y":7.39353, "heading":0.0, "vx":-0.99942, "vy":-0.02728, "omega":0.0, "ax":-0.00006, "ay":0.00234, "alpha":0.0, "fx":[-0.00108,-0.00108,-0.00108,-0.00108], "fy":[0.03981,0.03981,0.03981,0.03981]},
+ {"t":8.90136, "x":4.57489, "y":7.39222, "heading":0.0, "vx":-0.99942, "vy":-0.02716, "omega":0.0, "ax":-0.00006, "ay":0.00233, "alpha":0.0, "fx":[-0.00107,-0.00107,-0.00107,-0.00107], "fy":[0.03962,0.03962,0.03962,0.03962]},
+ {"t":8.94925, "x":4.52703, "y":7.39093, "heading":0.0, "vx":-0.99943, "vy":-0.02705, "omega":0.0, "ax":-0.00006, "ay":0.00232, "alpha":0.0, "fx":[-0.00107,-0.00107,-0.00107,-0.00107], "fy":[0.03944,0.03944,0.03944,0.03944]},
+ {"t":8.99715, "x":4.47916, "y":7.38963, "heading":0.0, "vx":-0.99943, "vy":-0.02694, "omega":0.0, "ax":-0.00006, "ay":0.00231, "alpha":0.0, "fx":[-0.00106,-0.00106,-0.00106,-0.00106], "fy":[0.03926,0.03926,0.03926,0.03926]},
+ {"t":9.04504, "x":4.43129, "y":7.38834, "heading":0.0, "vx":-0.99943, "vy":-0.02683, "omega":0.0, "ax":-0.00006, "ay":0.0023, "alpha":0.0, "fx":[-0.00105,-0.00105,-0.00105,-0.00105], "fy":[0.03908,0.03908,0.03908,0.03908]},
+ {"t":9.09294, "x":4.38343, "y":7.38706, "heading":0.0, "vx":-0.99943, "vy":-0.02672, "omega":0.0, "ax":-0.00006, "ay":0.00229, "alpha":0.0, "fx":[-0.00104,-0.00104,-0.00104,-0.00104], "fy":[0.0389,0.0389,0.0389,0.0389]},
+ {"t":9.14083, "x":4.33556, "y":7.38579, "heading":0.0, "vx":-0.99944, "vy":-0.02661, "omega":0.0, "ax":-0.00006, "ay":0.00228, "alpha":0.0, "fx":[-0.00103,-0.00103,-0.00103,-0.00103], "fy":[0.03872,0.03872,0.03872,0.03872]},
+ {"t":9.18872, "x":4.28769, "y":7.38451, "heading":0.0, "vx":-0.99944, "vy":-0.0265, "omega":0.0, "ax":-0.00006, "ay":0.00227, "alpha":0.0, "fx":[-0.00102,-0.00102,-0.00102,-0.00102], "fy":[0.03855,0.03855,0.03855,0.03855]},
+ {"t":9.23662, "x":4.23982, "y":7.38325, "heading":0.0, "vx":-0.99944, "vy":-0.02639, "omega":0.0, "ax":-0.00006, "ay":0.00226, "alpha":0.0, "fx":[-0.00101,-0.00101,-0.00101,-0.00101], "fy":[0.03838,0.03838,0.03838,0.03838]},
+ {"t":9.28451, "x":4.19196, "y":7.38199, "heading":0.0, "vx":-0.99945, "vy":-0.02628, "omega":0.0, "ax":-0.00006, "ay":0.00225, "alpha":0.0, "fx":[-0.001,-0.001,-0.001,-0.001], "fy":[0.03822,0.03822,0.03822,0.03822]},
+ {"t":9.33241, "x":4.14409, "y":7.38073, "heading":0.0, "vx":-0.99945, "vy":-0.02618, "omega":0.0, "ax":-0.00006, "ay":0.00224, "alpha":0.0, "fx":[-0.00099,-0.00099,-0.00099,-0.00099], "fy":[0.03805,0.03805,0.03805,0.03805]},
+ {"t":9.3803, "x":4.09622, "y":7.37948, "heading":0.0, "vx":-0.99945, "vy":-0.02607, "omega":0.0, "ax":-0.00006, "ay":0.00223, "alpha":0.0, "fx":[-0.00099,-0.00099,-0.00099,-0.00099], "fy":[0.03789,0.03789,0.03789,0.03789]},
+ {"t":9.4282, "x":4.04835, "y":7.37823, "heading":0.0, "vx":-0.99945, "vy":-0.02596, "omega":0.0, "ax":-0.00006, "ay":0.00222, "alpha":0.0, "fx":[-0.00098,-0.00098,-0.00098,-0.00098], "fy":[0.03773,0.03773,0.03773,0.03773]},
+ {"t":9.47609, "x":4.00048, "y":7.37699, "heading":0.0, "vx":-0.99946, "vy":-0.02586, "omega":0.0, "ax":-0.00006, "ay":0.00221, "alpha":0.0, "fx":[-0.00097,-0.00097,-0.00097,-0.00097], "fy":[0.03757,0.03757,0.03757,0.03757]},
+ {"t":9.52399, "x":3.95262, "y":7.37576, "heading":0.0, "vx":-0.99946, "vy":-0.02575, "omega":0.0, "ax":-0.00006, "ay":0.0022, "alpha":0.0, "fx":[-0.00096,-0.00096,-0.00096,-0.00096], "fy":[0.03741,0.03741,0.03741,0.03741]},
+ {"t":9.57188, "x":3.90475, "y":7.37452, "heading":0.0, "vx":-0.99946, "vy":-0.02565, "omega":0.0, "ax":-0.00006, "ay":0.00219, "alpha":0.0, "fx":[-0.00095,-0.00095,-0.00095,-0.00095], "fy":[0.03726,0.03726,0.03726,0.03726]},
+ {"t":9.61977, "x":3.85688, "y":7.3733, "heading":0.0, "vx":-0.99947, "vy":-0.02554, "omega":0.0, "ax":-0.00006, "ay":0.00218, "alpha":0.0, "fx":[-0.00095,-0.00095,-0.00095,-0.00095], "fy":[0.03711,0.03711,0.03711,0.03711]},
+ {"t":9.66767, "x":3.80901, "y":7.37208, "heading":0.0, "vx":-0.99947, "vy":-0.02544, "omega":0.0, "ax":-0.00006, "ay":0.00217, "alpha":0.0, "fx":[-0.00094,-0.00094,-0.00094,-0.00094], "fy":[0.03696,0.03696,0.03696,0.03696]},
+ {"t":9.71556, "x":3.76114, "y":7.37086, "heading":0.0, "vx":-0.99947, "vy":-0.02533, "omega":0.0, "ax":-0.00005, "ay":0.00216, "alpha":0.0, "fx":[-0.00093,-0.00093,-0.00093,-0.00093], "fy":[0.03681,0.03681,0.03681,0.03681]},
+ {"t":9.76346, "x":3.71327, "y":7.36965, "heading":0.0, "vx":-0.99947, "vy":-0.02523, "omega":0.0, "ax":-0.00005, "ay":0.00216, "alpha":0.0, "fx":[-0.00092,-0.00092,-0.00092,-0.00092], "fy":[0.03666,0.03666,0.03666,0.03666]},
+ {"t":9.81135, "x":3.6654, "y":7.36845, "heading":0.0, "vx":-0.99948, "vy":-0.02513, "omega":0.0, "ax":-0.00005, "ay":0.00215, "alpha":0.0, "fx":[-0.00092,-0.00092,-0.00092,-0.00092], "fy":[0.03652,0.03652,0.03652,0.03652]},
+ {"t":9.85925, "x":3.61753, "y":7.36724, "heading":0.0, "vx":-0.99948, "vy":-0.02502, "omega":0.0, "ax":-0.00005, "ay":0.00214, "alpha":0.0, "fx":[-0.00091,-0.00091,-0.00091,-0.00091], "fy":[0.03638,0.03638,0.03638,0.03638]},
+ {"t":9.90714, "x":3.56966, "y":7.36605, "heading":0.0, "vx":-0.99948, "vy":-0.02492, "omega":0.0, "ax":-0.00005, "ay":0.00213, "alpha":0.0, "fx":[-0.0009,-0.0009,-0.0009,-0.0009], "fy":[0.03624,0.03624,0.03624,0.03624]},
+ {"t":9.95504, "x":3.52179, "y":7.36486, "heading":0.0, "vx":-0.99948, "vy":-0.02482, "omega":0.0, "ax":-0.00005, "ay":0.00212, "alpha":0.0, "fx":[-0.00089,-0.00089,-0.00089,-0.00089], "fy":[0.036,0.036,0.036,0.036]},
+ {"t":10.00293, "x":3.47392, "y":7.36367, "heading":0.0, "vx":-0.99949, "vy":-0.02472, "omega":0.0, "ax":-0.00003, "ay":0.00134, "alpha":0.0, "fx":[-0.00056,-0.00056,-0.00056,-0.00056], "fy":[0.02275,0.02275,0.02275,0.02275]},
+ {"t":10.05082, "x":3.42605, "y":7.36249, "heading":0.0, "vx":-0.99949, "vy":-0.02465, "omega":0.0, "ax":0.00275, "ay":-0.10126, "alpha":0.0, "fx":[0.04673,0.04673,0.04673,0.04673], "fy":[-1.72236,-1.72236,-1.72236,-1.72236]},
+ {"t":10.09872, "x":3.37819, "y":7.36119, "heading":0.0, "vx":-0.99936, "vy":-0.0295, "omega":0.0, "ax":1.35107, "ay":-6.79397, "alpha":0.0, "fx":[22.98136,22.98136,22.98136,22.98136], "fy":[-115.56353,-115.56353,-115.56353,-115.56353]},
+ {"t":10.14661, "x":3.33187, "y":7.35199, "heading":0.0, "vx":-0.93465, "vy":-0.3549, "omega":0.0, "ax":5.4641, "ay":-8.02996, "alpha":0.0, "fx":[92.9427,92.9427,92.9427,92.9427], "fy":[-136.58729,-136.58729,-136.58729,-136.58729]},
+ {"t":10.19451, "x":3.29338, "y":7.32578, "heading":0.0, "vx":-0.67295, "vy":-0.73949, "omega":0.0, "ax":-1.68946, "ay":-9.59453, "alpha":0.04225, "fx":[-28.9994,-28.94334,-28.47564,-28.53083], "fy":[-163.1537,-163.16432,-163.24642,-163.23611]},
+ {"t":10.23023, "x":3.26826, "y":7.29324, "heading":0.0, "vx":-0.73331, "vy":-1.08227, "omega":0.00151, "ax":-1.55749, "ay":-9.60644, "alpha":0.05731, "fx":[-26.8456,-26.77529,-26.13998,-26.2087], "fy":[-163.34486,-163.35776,-163.4603,-163.44794]},
+ {"t":10.26596, "x":3.24106, "y":7.24844, "heading":0.00005, "vx":-0.78895, "vy":-1.42548, "omega":0.00356, "ax":-1.16738, "ay":-9.63105, "alpha":0.10199, "fx":[-20.47267,-20.37749,-19.24363,-19.33366], "fy":[-163.74471,-163.76131,-163.89758,-163.88224]},
+ {"t":10.30169, "x":3.21213, "y":7.19136, "heading":0.00018, "vx":-0.83066, "vy":-1.76957, "omega":0.0072, "ax":7.94846, "ay":-4.30137, "alpha":0.66806, "fx":[135.13088,131.94123,135.43156,138.30013], "fy":[-72.28454,-78.56537,-73.953,-67.85718]},
+ {"t":10.33742, "x":3.18753, "y":7.1254, "heading":0.00044, "vx":-0.54668, "vy":-1.92324, "omega":0.03107, "ax":3.73105, "ay":-0.92368, "alpha":5.81351, "fx":[44.00803,42.04529,82.79482,85.00801], "fy":[2.55237,-37.21006,-30.4723,2.28413]},
+ {"t":10.37314, "x":3.17038, "y":7.0561, "heading":0.00155, "vx":-0.41338, "vy":-1.95624, "omega":0.23877, "ax":0.31077, "ay":-0.06475, "alpha":7.00137, "fx":[-21.72223,-21.51997,32.14892,32.23766], "fy":[17.52294,-20.08319,-19.00365,17.15824]},
+ {"t":10.40887, "x":3.15581, "y":6.98616, "heading":0.01008, "vx":-0.40228, "vy":-1.95856, "omega":0.48891, "ax":0.01431, "ay":-0.00294, "alpha":5.99484, "fx":[-23.11175,-22.65922,23.57196,23.17247], "fy":[15.33719,-16.0819,-15.32552,15.87051]},
+ {"t":10.4446, "x":3.14144, "y":6.91619, "heading":0.02755, "vx":-0.40177, "vy":-1.95866, "omega":0.70308, "ax":-0.00004, "ay":0.00001, "alpha":5.19946, "fx":[-20.49584,-19.62093,20.49016,19.62418], "fy":[12.92758,-14.21393,-12.91072,14.19765]},
+ {"t":10.48032, "x":3.12709, "y":6.84621, "heading":0.05266, "vx":-0.40177, "vy":-1.95866, "omega":0.88884, "ax":-0.00151, "ay":0.00031, "alpha":4.25552, "fx":[-17.06436,-15.78192,17.01286,15.7307], "fy":[10.14516,-12.03056,-10.13392,12.04047]},
+ {"t":10.51605, "x":3.11273, "y":6.77624, "heading":0.08442, "vx":-0.40183, "vy":-1.95865, "omega":1.04088, "ax":-0.00209, "ay":0.00043, "alpha":3.54022, "fx":[-14.47259,-12.81627,14.40217,12.74461], "fy":[7.9836,-10.41935,-7.97042,10.4354]},
+ {"t":10.55178, "x":3.09838, "y":6.70626, "heading":0.12161, "vx":-0.4019, "vy":-1.95863, "omega":1.16736, "ax":-0.0021, "ay":0.00043, "alpha":2.84998, "fx":[-11.88884,-10.00495,11.81813,9.93309], "fy":[5.99142,-8.76367,-5.97787,8.77945]},
+ {"t":10.5875, "x":3.08402, "y":6.63628, "heading":0.16331, "vx":-0.40198, "vy":-1.95862, "omega":1.26918, "ax":-0.00199, "ay":0.00041, "alpha":2.12121, "fx":[-9.03418,-7.17508,8.96688,7.10701], "fy":[4.08907,-6.82511,-4.07577,6.83965]},
+ {"t":10.62323, "x":3.06965, "y":6.56631, "heading":0.20866, "vx":-0.40205, "vy":-1.9586, "omega":1.34497, "ax":-0.00193, "ay":0.0004, "alpha":1.65148, "fx":[-7.1758,-5.34792,7.11052,5.2822], "fy":[2.86689,-5.55695,-2.85371,5.57073]},
+ {"t":10.65896, "x":3.05529, "y":6.49633, "heading":0.25671, "vx":-0.40212, "vy":-1.95859, "omega":1.40397, "ax":-0.0019, "ay":0.00039, "alpha":0.97022, "fx":[-4.30432,-2.99495,4.23981,2.93024], "fy":[1.48433,-3.40758,-1.47114,3.42097]},
+ {"t":10.69469, "x":3.04092, "y":6.42636, "heading":0.30687, "vx":-0.40218, "vy":-1.95858, "omega":1.43863, "ax":-0.00188, "ay":0.00039, "alpha":0.5636, "fx":[-2.55328,-1.65228,2.48923,1.58817], "fy":[0.74054,-2.05987,-0.72738,2.07307]},
+ {"t":10.73041, "x":3.02655, "y":6.35639, "heading":0.35827, "vx":-0.40225, "vy":-1.95856, "omega":1.45877, "ax":-0.00187, "ay":0.00039, "alpha":-0.0061, "fx":[-0.00399,-0.01631,-0.05974,-0.04742], "fy":[0.00098,0.03035,0.01213,-0.01723]},
+ {"t":10.76614, "x":3.01218, "y":6.28641, "heading":0.41039, "vx":-0.40232, "vy":-1.95855, "omega":1.45855, "ax":-0.00186, "ay":0.00038, "alpha":-0.44486, "fx":[2.00723,1.07353,-2.07056,-1.13692], "fy":[-0.36611,1.75993,0.37913,-1.74688]},
+ {"t":10.80187, "x":2.9978, "y":6.21644, "heading":0.4625, "vx":-0.40238, "vy":-1.95853, "omega":1.44266, "ax":-0.00186, "ay":0.00038, "alpha":-0.96356, "fx":[4.42063,2.16209,-4.4836,-2.22534], "fy":[-0.57051,3.9234,0.58344,-3.91035]},
+ {"t":10.83759, "x":2.98343, "y":6.14647, "heading":0.51404, "vx":-0.40245, "vy":-1.95852, "omega":1.40823, "ax":-0.00185, "ay":0.00038, "alpha":-1.44886, "fx":[6.69858,2.96356,-6.76113,-3.02674], "fy":[-0.51998,6.05608,0.53281,-6.04304]},
+ {"t":10.87332, "x":2.96905, "y":6.0765, "heading":0.56435, "vx":-0.40252, "vy":-1.95851, "omega":1.35647, "ax":-0.00184, "ay":0.00038, "alpha":-2.00384, "fx":[9.30135,3.68913,-9.36344,-3.7524], "fy":[-0.25739,8.56917,0.27015,-8.55614]},
+ {"t":10.90905, "x":2.95466, "y":6.00652, "heading":0.61281, "vx":-0.40258, "vy":-1.95849, "omega":1.28488, "ax":-0.00184, "ay":0.00038, "alpha":-2.55618, "fx":[11.87653,4.18633,-11.93802,-4.24976], "fy":[0.24016,11.14409,-0.22744,-11.1311]},
+ {"t":10.94477, "x":2.94028, "y":5.93655, "heading":0.65872, "vx":-0.40265, "vy":-1.95848, "omega":1.19355, "ax":-0.00183, "ay":0.00038, "alpha":-3.1995, "fx":[14.84499,4.60983,-14.90573,-4.67356], "fy":[0.97489,14.17221,-0.96216,-14.15933]},
+ {"t":10.9805, "x":2.92589, "y":5.86658, "heading":0.70136, "vx":-0.40271, "vy":-1.95847, "omega":1.07925, "ax":-0.00182, "ay":0.00037, "alpha":-3.86742, "fx":[17.88551,4.85224,-17.94532,-4.91637], "fy":[1.93361,17.3505,-1.92079,-17.33781]},
+ {"t":11.01623, "x":2.9115, "y":5.79661, "heading":0.73992, "vx":-0.40278, "vy":-1.95845, "omega":0.94107, "ax":-0.00181, "ay":0.00037, "alpha":-4.63604, "fx":[21.34298,5.02604,-21.40165,-5.09072], "fy":[3.13353,21.0054,-3.12052,-20.99303]},
+ {"t":11.05196, "x":2.89711, "y":5.72664, "heading":0.77354, "vx":-0.40284, "vy":-1.95844, "omega":0.77544, "ax":-0.00179, "ay":0.00037, "alpha":-5.4723, "fx":[25.06234,5.11103,-25.11936,-5.17612], "fy":[4.53425,24.97781,-4.52101,-24.96592]},
+ {"t":11.08768, "x":2.88272, "y":5.65667, "heading":0.80124, "vx":-0.40291, "vy":-1.95843, "omega":0.57993, "ax":-0.00164, "ay":0.00034, "alpha":-6.40481, "fx":[29.1849,5.18601,-29.23545,-5.24701], "fy":[6.10837,29.38667,-6.09617,-29.37591]},
+ {"t":11.12341, "x":2.86832, "y":5.58671, "heading":0.82196, "vx":-0.40297, "vy":-1.95841, "omega":0.35111, "ax":0.00017, "ay":0.00032, "alpha":-7.44726, "fx":[33.81244,5.35994,-33.79406,-5.36676], "fy":[7.7885,34.30644,-7.8038,-34.26904]},
+ {"t":11.15914, "x":2.85393, "y":5.51674, "heading":0.83451, "vx":-0.40296, "vy":-1.9584, "omega":0.08504, "ax":1.46379, "ay":7.14205, "alpha":-2.23813, "fx":[40.84857,25.24997,8.12129,25.37473], "fy":[120.3549,126.8043,122.53659,116.24117]},
+ {"t":11.19486, "x":2.84047, "y":5.45133, "heading":0.83754, "vx":-0.35066, "vy":-1.70324, "omega":0.00508, "ax":1.95623, "ay":9.50251, "alpha":-0.05325, "fx":[33.80583,33.23846,32.74298,33.31262], "fy":[161.52691,161.64682,161.74232,161.62371]},
+ {"t":11.23059, "x":2.82919, "y":5.39654, "heading":0.83773, "vx":-0.28077, "vy":-1.36374, "omega":0.00318, "ax":1.96232, "ay":9.53206, "alpha":-0.0305, "fx":[33.68397,33.35701,33.07255,33.40026], "fy":[162.07504,162.14329,162.19993,162.13212]},
+ {"t":11.26632, "x":2.82041, "y":5.3539, "heading":0.83784, "vx":-0.21066, "vy":-1.02319, "omega":0.00209, "ax":1.96448, "ay":9.54205, "alpha":-0.02282, "fx":[33.64415,33.39899,33.18598,33.43157], "fy":[162.26049,162.31145,162.35432,162.30361]},
+ {"t":11.30204, "x":2.81413, "y":5.32344, "heading":0.83791, "vx":-0.14048, "vy":-0.68228, "omega":0.00127, "ax":1.96564, "ay":9.54705, "alpha":-0.01896, "fx":[33.62533,33.42141,33.24437,33.44858], "fy":[162.35353,162.39583,162.43166,162.38953]},
+ {"t":11.33777, "x":2.81037, "y":5.30515, "heading":0.83796, "vx":-0.07025, "vy":-0.34119, "omega":0.00059, "ax":1.9664, "ay":9.55005, "alpha":-0.01664, "fx":[33.61499,33.43594,33.28057,33.45985], "fy":[162.40933,162.44642,162.47797,162.441]},
+ {"t":11.3735, "x":2.80911, "y":5.29906, "heading":0.83798, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+ "splits":[0]
+ },
+ "events":[]
+}
diff --git a/lib/examples/swerve/src/main/deploy/choreo/OUTPOST_1.traj b/lib/examples/swerve/src/main/deploy/choreo/OUTPOST_1.traj
new file mode 100644
index 00000000..be0713c4
--- /dev/null
+++ b/lib/examples/swerve/src/main/deploy/choreo/OUTPOST_1.traj
@@ -0,0 +1,82 @@
+{
+ "name":"OUTPOST_1",
+ "version":3,
+ "snapshot":{
+ "waypoints":[
+ {"x":3.690892219543457, "y":0.6445010900497437, "heading":3.1415922179358766, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":0.4072994291782379, "y":0.6569390296936035, "heading":3.141592653589793, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true}],
+ "targetDt":0.05
+ },
+ "params":{
+ "waypoints":[
+ {"x":{"exp":"3.690892219543457 m", "val":3.690892219543457}, "y":{"exp":"0.6445010900497437 m", "val":0.6445010900497437}, "heading":{"exp":"3.1415922179358766 rad", "val":3.1415922179358766}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"0.4072994291782379 m", "val":0.4072994291782379}, "y":{"exp":"0.6569390296936035 m", "val":0.6569390296936035}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true}],
+ "targetDt":{
+ "exp":"0.05 s",
+ "val":0.05
+ }
+ },
+ "trajectory":{
+ "config":{
+ "frontLeft":{
+ "x":0.225425,
+ "y":0.333375
+ },
+ "backLeft":{
+ "x":-0.225425,
+ "y":0.333375
+ },
+ "mass":68.0388555,
+ "inertia":7.5,
+ "gearing":7.03,
+ "radius":0.0508,
+ "vmax":607.3745796940267,
+ "tmax":1.2,
+ "cof":1.5,
+ "bumper":{
+ "front":0.34925,
+ "side":0.4445,
+ "back":0.34925
+ },
+ "differentialTrackWidth":0.5588
+ },
+ "sampleType":"Swerve",
+ "waypoints":[0.0,1.19873],
+ "samples":[
+ {"t":0.0, "x":3.69089, "y":0.6445, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-9.75433, "ay":0.07475, "alpha":0.0, "fx":[-165.91833,-165.91833,-165.91833,-165.91833], "fy":[1.27151,1.27156,1.27156,1.27151]},
+ {"t":0.04995, "x":3.67873, "y":0.64459, "heading":3.14159, "vx":-0.4872, "vy":0.00373, "omega":0.0, "ax":-9.75328, "ay":0.07192, "alpha":0.0, "fx":[-165.90055,-165.90055,-165.90055,-165.90055], "fy":[1.22324,1.22329,1.22329,1.22324]},
+ {"t":0.09989, "x":3.64223, "y":0.64487, "heading":3.14159, "vx":-0.97435, "vy":0.00733, "omega":0.0, "ax":-9.75192, "ay":0.06906, "alpha":0.0, "fx":[-165.87735,-165.87735,-165.87735,-165.87735], "fy":[1.17462,1.17467,1.17467,1.17462]},
+ {"t":0.14984, "x":3.5814, "y":0.64532, "heading":3.14159, "vx":-1.46143, "vy":0.01077, "omega":0.0, "ax":-9.75007, "ay":0.06617, "alpha":0.0, "fx":[-165.84591,-165.84591,-165.84591,-165.84591], "fy":[1.12545,1.12549,1.12549,1.12545]},
+ {"t":0.19979, "x":3.49624, "y":0.64594, "heading":3.14159, "vx":-1.94842, "vy":0.01408, "omega":0.0, "ax":-9.74743, "ay":0.06322, "alpha":0.0, "fx":[-165.80096,-165.80096,-165.80096,-165.80096], "fy":[1.07532,1.07535,1.07535,1.07532]},
+ {"t":0.24974, "x":3.38676, "y":0.64673, "heading":3.14159, "vx":-2.43527, "vy":0.01724, "omega":0.0, "ax":-9.74335, "ay":0.06017, "alpha":0.0, "fx":[-165.73164,-165.73164,-165.73164,-165.73164], "fy":[1.0234,1.02342,1.02342,1.0234]},
+ {"t":0.29968, "x":3.25297, "y":0.64766, "heading":3.14159, "vx":-2.92192, "vy":0.02024, "omega":0.0, "ax":-9.73626, "ay":0.05689, "alpha":0.0, "fx":[-165.61106,-165.61106,-165.61106,-165.61106], "fy":[0.96763,0.96763,0.96763,0.96763]},
+ {"t":0.34963, "x":3.09489, "y":0.64874, "heading":3.14159, "vx":-3.40822, "vy":0.02308, "omega":0.0, "ax":-9.72091, "ay":0.05298, "alpha":0.0, "fx":[-165.34993,-165.34993,-165.34993,-165.34993], "fy":[0.90115,0.90112,0.90112,0.90115]},
+ {"t":0.39958, "x":2.91253, "y":0.64996, "heading":3.14159, "vx":-3.89375, "vy":0.02573, "omega":0.0, "ax":-9.66322, "ay":0.04586, "alpha":-0.00001, "fx":[-164.36854,-164.36854,-164.36854,-164.36854], "fy":[0.78019,0.78001,0.78002,0.78019]},
+ {"t":0.44952, "x":2.706, "y":0.6513, "heading":3.14159, "vx":-4.3764, "vy":0.02802, "omega":0.0, "ax":-0.17953, "ay":-0.12526, "alpha":0.0, "fx":[-3.05381,-3.05381,-3.0538,-3.0538], "fy":[-2.13065,-2.13066,-2.13066,-2.13065]},
+ {"t":0.49947, "x":2.48719, "y":0.65255, "heading":3.14159, "vx":-4.38537, "vy":0.02176, "omega":0.0, "ax":-0.00029, "ay":-0.05629, "alpha":0.0, "fx":[-0.00487,-0.00487,-0.00487,-0.00487], "fy":[-0.95753,-0.95753,-0.95753,-0.95753]},
+ {"t":0.54942, "x":2.26815, "y":0.65356, "heading":3.14159, "vx":-4.38538, "vy":0.01895, "omega":0.0, "ax":-0.0002, "ay":-0.04883, "alpha":0.0, "fx":[-0.00347,-0.00347,-0.00347,-0.00347], "fy":[-0.83063,-0.83063,-0.83063,-0.83063]},
+ {"t":0.59936, "x":2.04911, "y":0.65445, "heading":3.14159, "vx":-4.38539, "vy":0.01651, "omega":0.0, "ax":-0.00018, "ay":-0.04864, "alpha":0.0, "fx":[-0.00301,-0.00301,-0.00301,-0.00301], "fy":[-0.82741,-0.82741,-0.82741,-0.82741]},
+ {"t":0.64931, "x":1.83007, "y":0.65521, "heading":3.14159, "vx":-4.3854, "vy":0.01408, "omega":0.0, "ax":-0.00015, "ay":-0.05553, "alpha":0.0, "fx":[-0.00255,-0.00255,-0.00255,-0.00255], "fy":[-0.94462,-0.94462,-0.94462,-0.94462]},
+ {"t":0.69926, "x":1.61104, "y":0.65585, "heading":3.14159, "vx":-4.38541, "vy":0.01131, "omega":0.0, "ax":0.17841, "ay":-0.1233, "alpha":0.0, "fx":[3.03464,3.03464,3.03462,3.03462], "fy":[-2.09734,-2.09733,-2.09733,-2.09734]},
+ {"t":0.74921, "x":1.39222, "y":0.65626, "heading":3.14159, "vx":-4.3765, "vy":0.00515, "omega":0.0, "ax":9.66334, "ay":-0.02688, "alpha":0.00001, "fx":[164.37059,164.37059,164.37059,164.37059], "fy":[-0.4573,-0.45713,-0.45713,-0.4573]},
+ {"t":0.79915, "x":1.18568, "y":0.65648, "heading":3.14159, "vx":-3.89384, "vy":0.00381, "omega":0.0, "ax":9.72106, "ay":-0.02038, "alpha":0.0, "fx":[165.35252,165.35252,165.35252,165.35252], "fy":[-0.34672,-0.34669,-0.34669,-0.34672]},
+ {"t":0.8491, "x":1.00332, "y":0.65665, "heading":3.14159, "vx":-3.4083, "vy":0.00279, "omega":0.0, "ax":9.73644, "ay":-0.0167, "alpha":0.0, "fx":[165.61401,165.61401,165.61401,165.61401], "fy":[-0.28404,-0.28405,-0.28405,-0.28404]},
+ {"t":0.89905, "x":0.84523, "y":0.65677, "heading":3.14159, "vx":-2.922, "vy":0.00196, "omega":0.0, "ax":9.74355, "ay":-0.01358, "alpha":0.0, "fx":[165.73496,165.73496,165.73496,165.73496], "fy":[-0.2309,-0.23093,-0.23093,-0.2309]},
+ {"t":0.94899, "x":0.71144, "y":0.65685, "heading":3.14159, "vx":-2.43534, "vy":0.00128, "omega":0.0, "ax":9.74764, "ay":-0.01066, "alpha":0.0, "fx":[165.80465,165.80465,165.80465,165.80465], "fy":[-0.1813,-0.18133,-0.18133,-0.1813]},
+ {"t":0.99894, "x":0.60196, "y":0.6569, "heading":3.14159, "vx":-1.94847, "vy":0.00075, "omega":0.0, "ax":9.75031, "ay":-0.00784, "alpha":0.0, "fx":[165.84996,165.84996,165.84996,165.84996], "fy":[-0.13339,-0.13343,-0.13343,-0.13339]},
+ {"t":1.04889, "x":0.5168, "y":0.65693, "heading":3.14159, "vx":-1.46147, "vy":0.00035, "omega":0.0, "ax":9.75218, "ay":-0.00508, "alpha":0.0, "fx":[165.88178,165.88178,165.88178,165.88178], "fy":[-0.08644,-0.08648,-0.08648,-0.08644]},
+ {"t":1.09884, "x":0.45597, "y":0.65694, "heading":3.14159, "vx":-0.97438, "vy":0.0001, "omega":0.0, "ax":9.75357, "ay":-0.00236, "alpha":0.0, "fx":[165.90535,165.90535,165.90535,165.90535], "fy":[-0.04009,-0.04013,-0.04013,-0.04009]},
+ {"t":1.14878, "x":0.41947, "y":0.65694, "heading":3.14159, "vx":-0.48722, "vy":-0.00002, "omega":0.0, "ax":9.75463, "ay":0.00034, "alpha":0.0, "fx":[165.92351,165.92351,165.92351,165.92351], "fy":[0.00586,0.00581,0.00581,0.00586]},
+ {"t":1.19873, "x":0.4073, "y":0.65694, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+ "splits":[0]
+ },
+ "events":[]
+}
diff --git a/lib/examples/swerve/src/main/deploy/choreo/OUTPOST_2.traj b/lib/examples/swerve/src/main/deploy/choreo/OUTPOST_2.traj
new file mode 100644
index 00000000..d82e5f21
--- /dev/null
+++ b/lib/examples/swerve/src/main/deploy/choreo/OUTPOST_2.traj
@@ -0,0 +1,111 @@
+{
+ "name":"OUTPOST_2",
+ "version":3,
+ "snapshot":{
+ "waypoints":[
+ {"x":0.4072994291782379, "y":0.6569390296936035, "heading":3.141592653589793, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":1.6635223627090454, "y":1.6768428087234497, "heading":0.0, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":1.93715500831604, "y":3.816153049468994, "heading":1.5707963267948966, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true}],
+ "targetDt":0.05
+ },
+ "params":{
+ "waypoints":[
+ {"x":{"exp":"0.4072994291782379 m", "val":0.4072994291782379}, "y":{"exp":"0.6569390296936035 m", "val":0.6569390296936035}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"1.6635223627090454 m", "val":1.6635223627090454}, "y":{"exp":"1.6768428087234497 m", "val":1.6768428087234497}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":{"exp":"1.93715500831604 m", "val":1.93715500831604}, "y":{"exp":"3.816153049468994 m", "val":3.816153049468994}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true}],
+ "targetDt":{
+ "exp":"0.05 s",
+ "val":0.05
+ }
+ },
+ "trajectory":{
+ "config":{
+ "frontLeft":{
+ "x":0.225425,
+ "y":0.333375
+ },
+ "backLeft":{
+ "x":-0.225425,
+ "y":0.333375
+ },
+ "mass":68.0388555,
+ "inertia":7.5,
+ "gearing":7.03,
+ "radius":0.0508,
+ "vmax":607.3745796940267,
+ "tmax":1.2,
+ "cof":1.5,
+ "bumper":{
+ "front":0.34925,
+ "side":0.4445,
+ "back":0.34925
+ },
+ "differentialTrackWidth":0.5588
+ },
+ "sampleType":"Swerve",
+ "waypoints":[0.0,0.62176,1.36669],
+ "samples":[
+ {"t":0.0, "x":0.4073, "y":0.65694, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":7.77717, "ay":4.27371, "alpha":-9.49132, "fx":[60.69069,159.57563,165.55075,143.33265], "fy":[154.2335,42.83598,10.18965,83.51943]},
+ {"t":0.02591, "x":0.40991, "y":0.65837, "heading":3.14159, "vx":0.20148, "vy":0.11072, "omega":-0.24589, "ax":7.7829, "ay":4.33302, "alpha":-9.30566, "fx":[62.25925,158.6326,165.45658,143.19121], "fy":[153.5832,46.09709,11.39773,83.73551]},
+ {"t":0.05181, "x":0.41774, "y":0.6627, "heading":3.13522, "vx":0.40311, "vy":0.22297, "omega":-0.48697, "ax":7.78577, "ay":4.40975, "alpha":-9.0904, "fx":[64.11681,157.0168,165.36648,143.23464], "fy":[152.79141,51.21555,12.39646,83.6309]},
+ {"t":0.07772, "x":0.4308, "y":0.66995, "heading":3.12261, "vx":0.60482, "vy":0.33722, "omega":-0.72247, "ax":7.784, "ay":4.50084, "alpha":-8.86245, "fx":[66.25622,154.63762,165.27768,143.44267], "fy":[151.84787,57.88755,13.25842,83.2379]},
+ {"t":0.10363, "x":0.44908, "y":0.6802, "heading":3.10389, "vx":0.80648, "vy":0.45382, "omega":-0.95207, "ax":7.7759, "ay":4.60277, "alpha":-8.64099, "fx":[68.68086,151.41134,165.18306,143.788], "fy":[150.73533,65.75389,14.08072,82.59718]},
+ {"t":0.12953, "x":0.47258, "y":0.6935, "heading":3.07922, "vx":1.00793, "vy":0.57306, "omega":-1.17593, "ax":7.76066, "ay":4.71148, "alpha":-8.44347, "fx":[71.40842,147.31314,165.06964,144.23518], "fy":[149.42643,74.37862,14.99682,81.76155]},
+ {"t":0.15544, "x":0.5013, "y":0.70993, "heading":3.04876, "vx":1.20898, "vy":0.69512, "omega":-1.39468, "ax":7.73909, "ay":4.82271, "alpha":-8.27952, "fx":[74.47638,142.42952,164.91506,144.73789], "fy":[147.8785,83.25301,16.1974,80.80299]},
+ {"t":0.18135, "x":0.53522, "y":0.72955, "heading":3.01263, "vx":1.40948, "vy":0.82006, "omega":-1.60917, "ax":7.71412, "ay":4.93316, "alpha":-8.14257, "fx":[77.95028,136.99801,164.67879,145.2328], "fy":[146.02475,91.82775,17.96812,79.82621]},
+ {"t":0.20725, "x":0.57432, "y":0.75246, "heading":2.97094, "vx":1.60932, "vy":0.94787, "omega":-1.82012, "ax":7.69066, "ay":5.04236, "alpha":-7.99924, "fx":[81.93759,131.41981,164.28049,145.6261], "fy":[143.75837,99.56213,20.76018,78.99546]},
+ {"t":0.23316, "x":0.61859, "y":0.7787, "heading":2.92379, "vx":1.80857, "vy":1.0785, "omega":-2.02736, "ax":7.67443, "ay":5.15567, "alpha":-7.7745, "fx":[86.6128,126.241,163.5431,145.76245], "fy":[140.90154,105.96525,25.32803,78.59109]},
+ {"t":0.25907, "x":0.66802, "y":0.80837, "heading":2.87126, "vx":2.00739, "vy":1.21207, "omega":-2.22877, "ax":7.66855, "ay":5.28941, "alpha":-7.32432, "fx":[92.26812,122.11431,162.03054,145.34656], "fy":[137.13784,110.59971,33.00935,79.13833]},
+ {"t":0.28498, "x":0.7226, "y":0.84155, "heading":2.81352, "vx":2.20605, "vy":1.3491, "omega":-2.41852, "ax":7.66388, "ay":5.4807, "alpha":-6.37164, "fx":[99.42833,119.76473,158.53737,143.71106], "fy":[131.84361,113.02239,46.29696,81.73754]},
+ {"t":0.31088, "x":0.78232, "y":0.87834, "heading":2.75087, "vx":2.4046, "vy":1.49108, "omega":-2.58359, "ax":7.60468, "ay":5.80652, "alpha":-4.33868, "fx":[109.14167,119.97839,149.35139,138.94221], "fy":[123.58506,112.63713,69.7856,89.06148]},
+ {"t":0.33679, "x":0.84717, "y":0.91892, "heading":2.68393, "vx":2.60162, "vy":1.64151, "omega":-2.69599, "ax":7.25268, "ay":6.3889, "alpha":0.08665, "fx":[123.77637,123.60167,122.95623,123.12999], "fy":[108.20511,108.4146,109.14004,108.93398]},
+ {"t":0.3627, "x":0.91701, "y":0.96359, "heading":2.61409, "vx":2.78951, "vy":1.80703, "omega":-2.69375, "ax":5.63806, "ay":7.01338, "alpha":10.52124, "fx":[147.9122,131.29068,59.79707,44.60698], "fy":[69.23918,98.58209,152.7597,156.60157]},
+ {"t":0.3886, "x":0.99117, "y":1.01276, "heading":2.5443, "vx":2.93557, "vy":1.98873, "omega":-2.42118, "ax":4.11035, "ay":6.95462, "alpha":16.56456, "fx":[152.76319,130.7319,26.94712,-30.77847], "fy":[53.4785,98.64773,161.63738,159.42072]},
+ {"t":0.41451, "x":1.0686, "y":1.06661, "heading":2.48158, "vx":3.04206, "vy":2.1689, "omega":-1.99204, "ax":3.0681, "ay":7.29683, "alpha":16.76076, "fx":[141.24708,121.62593,7.83578,-61.95879], "fy":[74.0591,108.78012,163.46856,150.1601]},
+ {"t":0.44042, "x":1.14844, "y":1.12525, "heading":2.42997, "vx":3.12155, "vy":2.35794, "omega":-1.55782, "ax":1.23006, "ay":7.72831, "alpha":15.30137, "fx":[99.6387,106.36278,-22.4584,-99.85152], "fy":[112.57909,122.60625,162.0271,128.61297]},
+ {"t":0.46632, "x":1.22972, "y":1.18893, "heading":2.38961, "vx":3.15341, "vy":2.55815, "omega":-1.16141, "ax":-2.29365, "ay":8.29783, "alpha":9.738, "fx":[-67.54971,64.59442,-43.54905,-109.5533], "fy":[138.05052,148.04881,157.62962,120.84622]},
+ {"t":0.49223, "x":1.31064, "y":1.25799, "heading":2.35952, "vx":3.09399, "vy":2.77312, "omega":-0.90913, "ax":-4.27522, "ay":7.87841, "alpha":7.60081, "fx":[-115.57049,5.34869,-63.69191,-116.96742], "fy":[109.70683,161.37124,150.76111,114.19865]},
+ {"t":0.51814, "x":1.38936, "y":1.33248, "heading":2.33597, "vx":2.98323, "vy":2.97723, "omega":-0.71222, "ax":-5.88777, "ay":7.0464, "alpha":6.193, "fx":[-139.59493,-48.75622,-84.18922,-128.05675], "fy":[81.70741,154.53197,140.76161,102.42833]},
+ {"t":0.54404, "x":1.46468, "y":1.41197, "heading":2.31752, "vx":2.8307, "vy":3.15978, "omega":-0.55177, "ax":-6.70056, "ay":6.63914, "alpha":4.15368, "fx":[-140.98492,-86.32496,-97.73019,-130.85845], "fy":[82.01704,138.2949,132.05453,99.35312]},
+ {"t":0.56995, "x":1.53576, "y":1.49606, "heading":2.30322, "vx":2.65711, "vy":3.33178, "omega":-0.44417, "ax":-7.2751, "ay":6.18766, "alpha":2.99487, "fx":[-142.85827,-108.59245,-109.41701,-134.12203], "fy":[80.2549,122.54395,122.86015,95.34256]},
+ {"t":0.59586, "x":1.60216, "y":1.58445, "heading":2.29172, "vx":2.46863, "vy":3.49208, "omega":-0.36658, "ax":-7.71643, "ay":5.72882, "alpha":2.33817, "fx":[-145.44838,-122.57096,-119.18988,-137.80816], "fy":[76.47321,109.28884,113.70177,90.31867]},
+ {"t":0.62176, "x":1.66352, "y":1.67684, "heading":2.28222, "vx":2.26872, "vy":3.6405, "omega":-0.306, "ax":-7.99224, "ay":5.34776, "alpha":2.34764, "fx":[-149.33006,-129.18991,-124.00736,-141.25586], "fy":[68.85107,101.52966,108.51767,84.9574]},
+ {"t":0.64935, "x":1.72307, "y":1.77932, "heading":2.27378, "vx":2.04822, "vy":3.78804, "omega":-0.24123, "ax":-8.21166, "ay":4.95431, "alpha":2.50031, "fx":[-152.73824,-134.22213,-127.53763,-144.21369], "fy":[59.92593,94.02474,103.86978,79.26533]},
+ {"t":0.67694, "x":1.77646, "y":1.88572, "heading":2.26712, "vx":1.82166, "vy":3.92473, "omega":-0.17225, "ax":-8.47224, "ay":4.45653, "alpha":2.34695, "fx":[-154.90895,-140.78016,-133.45917,-147.29332], "fy":[52.59904,82.83765,95.35474,72.4259]},
+ {"t":0.70453, "x":1.82349, "y":1.9957, "heading":2.26237, "vx":1.58791, "vy":4.04769, "omega":-0.1075, "ax":-8.77187, "ay":3.79511, "alpha":1.7918, "fx":[-156.01909,-147.94488,-142.10126,-150.76255], "fy":[46.97871,67.67217,80.4535,63.11084]},
+ {"t":0.73212, "x":1.86397, "y":2.10882, "heading":2.2594, "vx":1.34589, "vy":4.15239, "omega":-0.05806, "ax":-9.04267, "ay":2.97324, "alpha":0.92574, "fx":[-156.47882,-153.56139,-150.99668,-154.2159], "fy":[41.7564,50.93668,59.09401,50.5089]},
+ {"t":0.75971, "x":1.89766, "y":2.22451, "heading":2.2578, "vx":1.09641, "vy":4.23443, "omega":-0.03252, "ax":-9.16617, "ay":2.19665, "alpha":0.31402, "fx":[-156.55778,-155.85699,-155.24971,-155.99093], "fy":[34.47372,37.18522,40.23507,37.56347]},
+ {"t":0.7873, "x":1.92442, "y":2.34218, "heading":2.2569, "vx":0.84351, "vy":4.29503, "omega":-0.02385, "ax":-9.12235, "ay":1.55405, "alpha":0.11036, "fx":[-155.31958,-155.13701,-155.01515,-155.20253], "fy":[25.45586,26.30322,27.41073,26.56584]},
+ {"t":0.81489, "x":1.94422, "y":2.46127, "heading":2.25625, "vx":0.59183, "vy":4.33791, "omega":-0.02081, "ax":-8.88343, "ay":0.93286, "alpha":-0.05759, "fx":[-151.06209,-151.13811,-151.14645,-151.07153], "fy":[16.35079,15.96315,15.38437,15.77215]},
+ {"t":0.84248, "x":1.95717, "y":2.58131, "heading":2.25567, "vx":0.34673, "vy":4.36364, "omega":-0.0224, "ax":-8.05529, "ay":-1.50505, "alpha":-4.86941, "fx":[-141.84265,-145.49045,-131.19864,-129.54129], "fy":[9.37723,-12.62806,-56.89348,-42.25761]},
+ {"t":0.87007, "x":1.96367, "y":2.70113, "heading":2.25505, "vx":0.12449, "vy":4.32212, "omega":-0.15675, "ax":-3.09027, "ay":-6.58686, "alpha":-16.57932, "fx":[-124.68132,-128.67674,-17.85233,60.95195], "fy":[-55.92677,-89.42926,-158.28971,-144.51694]},
+ {"t":0.89766, "x":1.96593, "y":2.81787, "heading":2.25073, "vx":0.03923, "vy":4.14039, "omega":-0.61417, "ax":-2.24404, "ay":-6.8673, "alpha":-17.05268, "fx":[-112.98053,-126.80477,0.59621,86.50708], "fy":[-69.31018,-98.63212,-162.65069,-136.65058]},
+ {"t":0.92525, "x":1.96615, "y":2.92949, "heading":2.23378, "vx":-0.02269, "vy":3.95092, "omega":-1.08465, "ax":-1.94074, "ay":-6.92253, "alpha":-17.07797, "fx":[-101.17933,-128.1769,4.5931,92.7175], "fy":[-73.37593,-99.5066,-163.73689,-134.38149]},
+ {"t":0.95284, "x":1.96479, "y":3.03586, "heading":2.20386, "vx":-0.07623, "vy":3.75993, "omega":-1.55583, "ax":-1.4001, "ay":-6.93119, "alpha":-16.93117, "fx":[-62.89761,-130.74453,4.28005,94.10089], "fy":[-75.41779,-97.52727,-164.31766,-134.32749]},
+ {"t":0.98043, "x":1.96215, "y":3.13695, "heading":2.16093, "vx":-0.11486, "vy":3.5687, "omega":-2.02296, "ax":-0.03003, "ay":-9.16966, "alpha":-7.12956, "fx":[9.19421,-75.65058,2.74228,61.67093], "fy":[-161.67353,-145.22742,-164.60588,-152.38657]},
+ {"t":1.00802, "x":1.95897, "y":3.23193, "heading":2.10512, "vx":-0.11569, "vy":3.3157, "omega":-2.21967, "ax":0.3067, "ay":-9.66915, "alpha":-0.86889, "fx":[6.2934,-3.81708,4.44671,13.94479], "fy":[-164.46639,-164.59411,-164.69429,-164.12331]},
+ {"t":1.03561, "x":1.9559, "y":3.31973, "heading":2.04388, "vx":-0.10723, "vy":3.04893, "omega":-2.24364, "ax":0.30834, "ay":-9.6568, "alpha":2.01223, "fx":[2.4602,24.96071,10.15981,-16.60184], "fy":[-165.14749,-163.2164,-164.54891,-164.12495]},
+ {"t":1.0632, "x":1.95306, "y":3.40017, "heading":1.98198, "vx":-0.09872, "vy":2.7825, "omega":-2.18812, "ax":0.27341, "ay":-9.57609, "alpha":3.60313, "fx":[-1.66261,37.20008,19.0941,-36.02931], "fy":[-165.40956,-161.12975,-163.85257,-161.15446]},
+ {"t":1.09079, "x":1.95044, "y":3.47329, "heading":1.92161, "vx":-0.09118, "vy":2.5183, "omega":-2.08871, "ax":0.26339, "ay":-9.48721, "alpha":4.68297, "fx":[-5.81563,42.68919,30.03949,-48.99242], "fy":[-165.45644,-159.92117,-162.29994,-157.82148]},
+ {"t":1.11838, "x":1.94802, "y":3.53916, "heading":1.86398, "vx":-0.08391, "vy":2.25655, "omega":-1.95951, "ax":0.27654, "ay":-9.397, "alpha":5.54878, "fx":[-9.86087,44.97026,41.73205,-58.02604], "fy":[-165.35446,-159.39897,-159.7893,-154.81854]},
+ {"t":1.14597, "x":1.94581, "y":3.59784, "heading":1.80992, "vx":-0.07628, "vy":1.99728, "omega":-1.80642, "ax":0.30049, "ay":-9.30672, "alpha":6.30936, "fx":[-13.71151,45.57312,53.0917,-64.50843], "fy":[-165.14197,-159.30078,-156.47611,-152.29961]},
+ {"t":1.17356, "x":1.94382, "y":3.64941, "heading":1.76008, "vx":-0.06799, "vy":1.74051, "omega":-1.63234, "ax":0.32482, "ay":-9.21884, "alpha":6.99365, "fx":[-17.30502,45.26708,63.38087,-69.24229], "fy":[-164.84984,-159.44175,-152.68834,-150.25952]},
+ {"t":1.20115, "x":1.94207, "y":3.69392, "heading":1.71504, "vx":-0.05903, "vy":1.48616, "omega":-1.43939, "ax":0.3438, "ay":-9.13647, "alpha":7.60217, "fx":[-20.59228,44.48911,72.23601,-72.74082], "fy":[-164.50656,-159.70149,-148.78706,-148.64005]},
+ {"t":1.22874, "x":1.94057, "y":3.73145, "heading":1.67533, "vx":-0.04954, "vy":1.23409, "omega":-1.22965, "ax":0.35571, "ay":-9.06194, "alpha":8.13179, "fx":[-23.53336,43.51245,79.59079,-75.36778], "fy":[-164.13938,-160.00239,-145.06118,-147.36107]},
+ {"t":1.25633, "x":1.93934, "y":3.76205, "heading":1.6414, "vx":-0.03973, "vy":0.98407, "omega":-1.00529, "ax":0.36124, "ay":-8.99617, "alpha":8.58475, "fx":[-26.09556,42.52071,85.55948,-77.40646], "fy":[-163.77417,-160.29447,-141.68914,-146.33146]},
+ {"t":1.28392, "x":1.93838, "y":3.78577, "heading":1.61367, "vx":-0.02976, "vy":0.73586, "omega":-0.76844, "ax":0.36207, "ay":-8.9388, "alpha":8.96948, "fx":[-28.25229,41.64372,90.33551,-79.09244], "fy":[-163.43475,-160.54552,-138.7504,-145.4552]},
+ {"t":1.31151, "x":1.9377, "y":3.80267, "heading":1.59247, "vx":-0.01977, "vy":0.48924, "omega":-0.52097, "ax":0.36, "ay":-8.88861, "alpha":9.29824, "fx":[-29.98185,40.97657,94.12526,-80.62628], "fy":[-163.14231,-160.7345,-136.25697,-144.63713]},
+ {"t":1.3391, "x":1.93729, "y":3.81279, "heading":1.57809, "vx":-0.00984, "vy":0.24401, "omega":-0.26443, "ax":0.3566, "ay":-8.84401, "alpha":9.58431, "fx":[-31.26606,40.59054,97.11279,-82.17482], "fy":[-162.91488,-160.84708,-134.18526,-143.7889]},
+ {"t":1.36669, "x":1.93716, "y":3.81615, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+ "splits":[0]
+ },
+ "events":[]
+}
diff --git a/lib/examples/swerve/src/main/deploy/choreo/OUTPOST_DEPOT.traj b/lib/examples/swerve/src/main/deploy/choreo/OUTPOST_DEPOT.traj
new file mode 100644
index 00000000..7651266b
--- /dev/null
+++ b/lib/examples/swerve/src/main/deploy/choreo/OUTPOST_DEPOT.traj
@@ -0,0 +1,193 @@
+{
+ "name":"OUTPOST_DEPOT",
+ "version":3,
+ "snapshot":{
+ "waypoints":[
+ {"x":0.4072994291782379, "y":0.6569390296936035, "heading":3.141592653589793, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":1.6635223627090454, "y":1.6768428087234497, "heading":0.0, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":1.8984053134918213, "y":4.315596580505371, "heading":0.0, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":1.2030097246170044, "y":5.9760308265686035, "heading":3.141592653589793, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":0.42246365547180176, "y":5.961839199066162, "heading":-3.118608391365919, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":1.8416383266448977, "y":5.9760308265686035, "heading":0.0, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":1.9835559129714968, "y":3.8046934604644775, "heading":1.591626654119535, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true},
+ {"from":3, "to":4, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true},
+ {"from":3, "to":5, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}],
+ "targetDt":0.05
+ },
+ "params":{
+ "waypoints":[
+ {"x":{"exp":"0.4072994291782379 m", "val":0.4072994291782379}, "y":{"exp":"0.6569390296936035 m", "val":0.6569390296936035}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"1.6635223627090454 m", "val":1.6635223627090454}, "y":{"exp":"1.6768428087234497 m", "val":1.6768428087234497}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":{"exp":"1.8984053134918213 m", "val":1.8984053134918213}, "y":{"exp":"4.315596580505371 m", "val":4.315596580505371}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":{"exp":"1.2030097246170044 m", "val":1.2030097246170044}, "y":{"exp":"5.9760308265686035 m", "val":5.9760308265686035}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"0.42246365547180176 m", "val":0.42246365547180176}, "y":{"exp":"5.961839199066162 m", "val":5.961839199066162}, "heading":{"exp":"-3.118608391365919 rad", "val":-3.118608391365919}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":{"exp":"1.8416383266448975 m", "val":1.8416383266448977}, "y":{"exp":"5.9760308265686035 m", "val":5.9760308265686035}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":{"exp":"1.9835559129714966 m", "val":1.9835559129714968}, "y":{"exp":"3.8046934604644775 m", "val":3.8046934604644775}, "heading":{"exp":"1.591626654119535 rad", "val":1.591626654119535}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true},
+ {"from":3, "to":4, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true},
+ {"from":3, "to":5, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}],
+ "targetDt":{
+ "exp":"0.05 s",
+ "val":0.05
+ }
+ },
+ "trajectory":{
+ "config":{
+ "frontLeft":{
+ "x":0.225425,
+ "y":0.333375
+ },
+ "backLeft":{
+ "x":-0.225425,
+ "y":0.333375
+ },
+ "mass":68.0388555,
+ "inertia":7.5,
+ "gearing":7.03,
+ "radius":0.0508,
+ "vmax":607.3745796940267,
+ "tmax":1.2,
+ "cof":1.5,
+ "bumper":{
+ "front":0.34925,
+ "side":0.4445,
+ "back":0.34925
+ },
+ "differentialTrackWidth":0.5588
+ },
+ "sampleType":"Swerve",
+ "waypoints":[0.0,0.59907,1.21595,1.76993,2.60807,3.24518,4.1111],
+ "samples":[
+ {"t":0.0, "x":0.4073, "y":0.65694, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":8.45162, "ay":4.86406, "alpha":0.0, "fx":[143.75966,143.75966,143.75966,143.75966], "fy":[82.73623,82.73623,82.73623,82.73623]},
+ {"t":0.03524, "x":0.41255, "y":0.65996, "heading":3.14159, "vx":0.29783, "vy":0.17141, "omega":0.0, "ax":8.41694, "ay":4.92189, "alpha":0.0, "fx":[143.16972,143.16972,143.16972,143.16972], "fy":[83.72001,83.72001,83.72001,83.72001]},
+ {"t":0.07048, "x":0.42827, "y":0.66906, "heading":3.14159, "vx":0.59444, "vy":0.34485, "omega":0.0, "ax":8.37543, "ay":4.98995, "alpha":0.0, "fx":[142.46362,142.46362,142.46362,142.46362], "fy":[84.87754,84.87754,84.87754,84.87754]},
+ {"t":0.10572, "x":0.45442, "y":0.68431, "heading":3.14159, "vx":0.88959, "vy":0.5207, "omega":0.0, "ax":8.32478, "ay":5.07131, "alpha":0.0, "fx":[141.60221,141.60221,141.60221,141.60221], "fy":[86.26152,86.26152,86.26152,86.26152]},
+ {"t":0.14096, "x":0.49093, "y":0.7058, "heading":3.14159, "vx":1.18295, "vy":0.69941, "omega":0.0, "ax":8.2616, "ay":5.1704, "alpha":0.0, "fx":[140.52744,140.52744,140.52744,140.52744], "fy":[87.94699,87.94699,87.94699,87.94699]},
+ {"t":0.1762, "x":0.53775, "y":0.73366, "heading":3.14159, "vx":1.47408, "vy":0.88161, "omega":0.0, "ax":8.1806, "ay":5.2937, "alpha":0.0, "fx":[139.14967,139.14967,139.14967,139.14967], "fy":[90.04435,90.04435,90.04435,90.04435]},
+ {"t":0.21144, "x":0.59478, "y":0.76802, "heading":3.14159, "vx":1.76236, "vy":1.06816, "omega":0.0, "ax":8.07321, "ay":5.45118, "alpha":0.0, "fx":[137.32291,137.32291,137.32291,137.32291], "fy":[92.72304,92.72304,92.72304,92.72304]},
+ {"t":0.24668, "x":0.66189, "y":0.80904, "heading":3.14159, "vx":2.04686, "vy":1.26025, "omega":0.0, "ax":7.92448, "ay":5.65892, "alpha":0.0, "fx":[134.79316,134.79316,134.79316,134.79316], "fy":[96.25653,96.25653,96.25653,96.25653]},
+ {"t":0.28192, "x":0.73894, "y":0.85697, "heading":3.14159, "vx":2.32611, "vy":1.45967, "omega":0.0, "ax":7.70615, "ay":5.94444, "alpha":0.0, "fx":[131.07942,131.07942,131.07942,131.07942], "fy":[101.11326,101.11326,101.11326,101.11326]},
+ {"t":0.31716, "x":0.8257, "y":0.9121, "heading":3.14159, "vx":2.59768, "vy":1.66915, "omega":0.0, "ax":7.35835, "ay":6.3583, "alpha":0.0, "fx":[125.16336,125.16336,125.16336,125.16336], "fy":[108.15285,108.15285,108.15285,108.15285]},
+ {"t":0.3524, "x":0.92181, "y":0.97486, "heading":-3.14159, "vx":2.85698, "vy":1.89321, "omega":0.0, "ax":6.73347, "ay":6.9996, "alpha":0.0, "fx":[114.53434,114.53434,114.53434,114.53434], "fy":[119.06121,119.06121,119.06121,119.06121]},
+ {"t":0.38763, "x":1.02667, "y":1.04593, "heading":-3.14159, "vx":3.09426, "vy":2.13988, "omega":0.0, "ax":5.38512, "ay":8.05623, "alpha":0.0, "fx":[91.59934,91.59934,91.59934,91.59934], "fy":[137.03415,137.03415,137.03415,137.03415]},
+ {"t":0.42287, "x":1.13905, "y":1.12634, "heading":-3.14159, "vx":3.28403, "vy":2.42377, "omega":0.0, "ax":1.72721, "ay":9.49302, "alpha":0.0, "fx":[29.37929,29.37929,29.37929,29.37929], "fy":[161.47351,161.47351,161.47351,161.47351]},
+ {"t":0.45811, "x":1.25585, "y":1.21764, "heading":-3.14159, "vx":3.3449, "vy":2.7583, "omega":0.0, "ax":-5.36969, "ay":7.99104, "alpha":0.0, "fx":[-91.33685,-91.33685,-91.33685,-91.33685], "fy":[135.92529,135.92529,135.92529,135.92529]},
+ {"t":0.49335, "x":1.37039, "y":1.31981, "heading":-3.14159, "vx":3.15567, "vy":3.0399, "omega":0.0, "ax":-6.95017, "ay":6.70502, "alpha":0.0, "fx":[-118.22046,-118.22046,-118.22046,-118.22046], "fy":[114.05052,114.05052,114.05052,114.05052]},
+ {"t":0.52859, "x":1.47728, "y":1.43109, "heading":-3.14159, "vx":2.91075, "vy":3.27619, "omega":0.0, "ax":-7.49246, "ay":6.12989, "alpha":0.0, "fx":[-127.4446,-127.4446,-127.4446,-127.4446], "fy":[104.2676,104.2676,104.2676,104.2676]},
+ {"t":0.56383, "x":1.5752, "y":1.55035, "heading":-3.14159, "vx":2.64672, "vy":3.4922, "omega":0.0, "ax":-7.97277, "ay":5.52057, "alpha":0.0, "fx":[-135.6146,-135.6146,-135.6146,-135.6146], "fy":[93.90327,93.90327,93.90327,93.90327]},
+ {"t":0.59907, "x":1.66352, "y":1.67684, "heading":-3.14159, "vx":2.36577, "vy":3.68674, "omega":0.0, "ax":-8.31322, "ay":4.96267, "alpha":0.0, "fx":[-141.40541,-141.40541,-141.40541,-141.40541], "fy":[84.41356,84.41356,84.41356,84.41356]},
+ {"t":0.62711, "x":1.72659, "y":1.78217, "heading":-3.14159, "vx":2.13267, "vy":3.82589, "omega":0.0, "ax":-8.57173, "ay":4.46051, "alpha":0.0, "fx":[-145.80271,-145.80271,-145.80271,-145.80271], "fy":[75.87199,75.87199,75.87199,75.87199]},
+ {"t":0.65515, "x":1.78302, "y":1.8912, "heading":-3.14159, "vx":1.89232, "vy":3.95097, "omega":0.0, "ax":-8.80711, "ay":3.91052, "alpha":0.0, "fx":[-149.80637,-149.80637,-149.80637,-149.80637], "fy":[66.51683,66.51683,66.51683,66.51683]},
+ {"t":0.68319, "x":1.83262, "y":2.00352, "heading":-3.14159, "vx":1.64537, "vy":4.06062, "omega":0.0, "ax":-8.99611, "ay":3.34374, "alpha":0.0, "fx":[-153.02129,-153.02129,-153.02129,-153.02129], "fy":[56.87604,56.87604,56.87604,56.87604]},
+ {"t":0.71123, "x":1.87522, "y":2.11869, "heading":-3.14159, "vx":1.39312, "vy":4.15437, "omega":0.0, "ax":-9.12793, "ay":2.76628, "alpha":0.0, "fx":[-155.2634,-155.2634,-155.2634,-155.2634], "fy":[47.05356,47.05356,47.05356,47.05356]},
+ {"t":0.73927, "x":1.91069, "y":2.23627, "heading":-3.14159, "vx":1.13717, "vy":4.23194, "omega":0.0, "ax":-9.18499, "ay":2.18198, "alpha":0.0, "fx":[-156.23411,-156.23411,-156.23411,-156.23411], "fy":[37.11488,37.11488,37.11488,37.11488]},
+ {"t":0.76731, "x":1.93897, "y":2.35579, "heading":-3.14159, "vx":0.87963, "vy":4.29312, "omega":0.0, "ax":-9.12966, "ay":1.59664, "alpha":0.0, "fx":[-155.29292,-155.29292,-155.29292,-155.29292], "fy":[27.15832,27.15832,27.15832,27.15832]},
+ {"t":0.79535, "x":1.96004, "y":2.4768, "heading":-3.14159, "vx":0.62363, "vy":4.33789, "omega":0.0, "ax":-8.87067, "ay":1.02203, "alpha":0.0, "fx":[-150.88763,-150.88763,-150.88763,-150.88763], "fy":[17.38443,17.38443,17.38443,17.38443]},
+ {"t":0.82339, "x":1.97404, "y":2.59883, "heading":-3.14159, "vx":0.3749, "vy":4.36655, "omega":0.0, "ax":-8.16454, "ay":0.48884, "alpha":0.0, "fx":[-138.87649,-138.87649,-138.87649,-138.87649], "fy":[8.31511,8.31511,8.31511,8.31511]},
+ {"t":0.85143, "x":1.98134, "y":2.72146, "heading":-3.14159, "vx":0.14597, "vy":4.38026, "omega":0.0, "ax":-6.42487, "ay":0.08318, "alpha":0.0, "fx":[-109.28525,-109.28525,-109.28525,-109.28525], "fy":[1.41483,1.41483,1.41483,1.41483]},
+ {"t":0.87947, "x":1.98291, "y":2.84432, "heading":-3.14159, "vx":-0.03418, "vy":4.38259, "omega":0.0, "ax":-3.47135, "ay":-0.06529, "alpha":0.0, "fx":[-59.04674,-59.04674,-59.04674,-59.04674], "fy":[-1.11055,-1.11055,-1.11055,-1.11055]},
+ {"t":0.90751, "x":1.98059, "y":2.96718, "heading":3.14159, "vx":-0.13152, "vy":4.38076, "omega":0.0, "ax":-1.30676, "ay":-0.04465, "alpha":0.0, "fx":[-22.22754,-22.22754,-22.22754,-22.22754], "fy":[-0.75944,-0.75944,-0.75944,-0.75944]},
+ {"t":0.93555, "x":1.97639, "y":3.08999, "heading":3.14159, "vx":-0.16816, "vy":4.37951, "omega":0.0, "ax":-0.45001, "ay":-0.01793, "alpha":0.0, "fx":[-7.65448,-7.65448,-7.65448,-7.65448], "fy":[-0.30491,-0.30491,-0.30491,-0.30491]},
+ {"t":0.96359, "x":1.97149, "y":3.21279, "heading":3.14159, "vx":-0.18078, "vy":4.379, "omega":0.0, "ax":-0.16431, "ay":-0.00687, "alpha":0.0, "fx":[-2.79485,-2.79485,-2.79485,-2.79485], "fy":[-0.11691,-0.11691,-0.11691,-0.11691]},
+ {"t":0.99163, "x":1.96636, "y":3.33557, "heading":3.14159, "vx":-0.18539, "vy":4.37881, "omega":0.0, "ax":-0.08064, "ay":-0.00344, "alpha":0.0, "fx":[-1.37159,-1.37159,-1.37159,-1.37159], "fy":[-0.05848,-0.05848,-0.05848,-0.05848]},
+ {"t":1.01967, "x":1.96113, "y":3.45835, "heading":3.14159, "vx":-0.18765, "vy":4.37871, "omega":0.0, "ax":-0.08557, "ay":-0.00369, "alpha":0.0, "fx":[-1.45553,-1.45553,-1.45553,-1.45553], "fy":[-0.06282,-0.06282,-0.06282,-0.06282]},
+ {"t":1.04771, "x":1.95584, "y":3.58113, "heading":3.14159, "vx":-0.19005, "vy":4.37861, "omega":0.0, "ax":-0.18578, "ay":-0.00818, "alpha":0.0, "fx":[-3.16008,-3.16008,-3.16008,-3.16008], "fy":[-0.13909,-0.13909,-0.13909,-0.13909]},
+ {"t":1.07575, "x":1.95043, "y":3.7039, "heading":3.14159, "vx":-0.19525, "vy":4.37838, "omega":0.0, "ax":-0.51666, "ay":-0.02391, "alpha":0.0, "fx":[-8.78817,-8.78817,-8.78817,-8.78817], "fy":[-0.40666,-0.40666,-0.40666,-0.40666]},
+ {"t":1.10379, "x":1.94476, "y":3.82666, "heading":3.14159, "vx":-0.20974, "vy":4.37771, "omega":0.0, "ax":-1.49723, "ay":-0.07901, "alpha":0.0, "fx":[-25.46745,-25.46745,-25.46745,-25.46745], "fy":[-1.34397,-1.34397,-1.34397,-1.34397]},
+ {"t":1.13183, "x":1.93829, "y":3.94938, "heading":3.14159, "vx":-0.25172, "vy":4.3755, "omega":0.0, "ax":-3.85159, "ay":-0.26978, "alpha":0.0, "fx":[-65.51451,-65.51451,-65.51451,-65.51451], "fy":[-4.5889,-4.5889,-4.5889,-4.5889]},
+ {"t":1.15987, "x":1.92971, "y":4.07196, "heading":3.14159, "vx":-0.35972, "vy":4.36793, "omega":0.0, "ax":-6.7014, "ay":-0.69895, "alpha":0.0, "fx":[-113.98888,-113.98888,-113.98888,-113.98888], "fy":[-11.88889,-11.88889,-11.88889,-11.88889]},
+ {"t":1.18791, "x":1.91699, "y":4.19416, "heading":3.14159, "vx":-0.54763, "vy":4.34833, "omega":0.0, "ax":-8.22221, "ay":-1.24883, "alpha":0.0, "fx":[-139.85741,-139.85741,-139.85741,-139.85741], "fy":[-21.24224,-21.24224,-21.24224,-21.24224]},
+ {"t":1.21595, "x":1.89841, "y":4.3156, "heading":3.14159, "vx":-0.77818, "vy":4.31332, "omega":0.0, "ax":-8.19102, "ay":-1.71636, "alpha":0.0, "fx":[-139.32693,-139.32693,-139.32693,-139.32693], "fy":[-29.19479,-29.19479,-29.19479,-29.19479]},
+ {"t":1.24672, "x":1.87058, "y":4.44753, "heading":3.14159, "vx":-1.03027, "vy":4.26049, "omega":0.0, "ax":-6.29351, "ay":-1.67392, "alpha":0.0, "fx":[-107.05084,-107.05084,-107.05084,-107.05084], "fy":[-28.47291,-28.47291,-28.47291,-28.47291]},
+ {"t":1.2775, "x":1.83589, "y":4.57787, "heading":3.14159, "vx":-1.22397, "vy":4.20897, "omega":0.0, "ax":-3.15206, "ay":-0.956, "alpha":0.0, "fx":[-53.61567,-53.61567,-53.61567,-53.61567], "fy":[-16.26121,-16.26121,-16.26121,-16.26121]},
+ {"t":1.30828, "x":1.79672, "y":4.70695, "heading":3.14159, "vx":-1.32098, "vy":4.17955, "omega":0.0, "ax":-1.54381, "ay":-0.49759, "alpha":0.0, "fx":[-26.25981,-26.25981,-26.25981,-26.25981], "fy":[-8.46383,-8.46383,-8.46383,-8.46383]},
+ {"t":1.33906, "x":1.75534, "y":4.83535, "heading":3.14159, "vx":-1.36849, "vy":4.16424, "omega":0.0, "ax":-2.26552, "ay":-0.7658, "alpha":0.0, "fx":[-38.53589,-38.53589,-38.53589,-38.53589], "fy":[-13.0261,-13.0261,-13.0261,-13.0261]},
+ {"t":1.36983, "x":1.71215, "y":4.96315, "heading":3.14159, "vx":-1.43822, "vy":4.14067, "omega":0.0, "ax":-5.04703, "ay":-1.88432, "alpha":0.0, "fx":[-85.84847,-85.84847,-85.84847,-85.84847], "fy":[-32.05173,-32.05173,-32.05173,-32.05173]},
+ {"t":1.40061, "x":1.66549, "y":5.0897, "heading":3.14159, "vx":-1.59355, "vy":4.08267, "omega":0.0, "ax":-2.66062, "ay":-8.09233, "alpha":0.0, "fx":[-45.25635,-45.25635,-45.25635,-45.25635], "fy":[-137.64814,-137.64814,-137.64814,-137.64814]},
+ {"t":1.43139, "x":1.61519, "y":5.21151, "heading":3.14159, "vx":-1.67543, "vy":3.83362, "omega":0.0, "ax":2.25404, "ay":-9.32669, "alpha":0.0, "fx":[38.34059,38.34059,38.34059,38.34059], "fy":[-158.64434,-158.64434,-158.64434,-158.64434]},
+ {"t":1.46216, "x":1.56469, "y":5.32508, "heading":3.14159, "vx":-1.60606, "vy":3.54657, "omega":0.0, "ax":2.61258, "ay":-9.31519, "alpha":0.0, "fx":[44.43926,44.43926,44.43926,44.43926], "fy":[-158.44874,-158.44874,-158.44874,-158.44874]},
+ {"t":1.49294, "x":1.5165, "y":5.42982, "heading":3.14159, "vx":-1.52565, "vy":3.25988, "omega":0.0, "ax":2.74232, "ay":-9.3075, "alpha":0.0, "fx":[46.64616,46.64616,46.64616,46.64616], "fy":[-158.31789,-158.31789,-158.31789,-158.31789]},
+ {"t":1.52372, "x":1.47084, "y":5.52575, "heading":3.14159, "vx":-1.44125, "vy":2.97342, "omega":0.0, "ax":2.80951, "ay":-9.30269, "alpha":0.0, "fx":[47.789,47.789,47.789,47.789], "fy":[-158.23613,-158.23613,-158.23613,-158.23613]},
+ {"t":1.55449, "x":1.42782, "y":5.61285, "heading":3.14159, "vx":-1.35479, "vy":2.68711, "omega":0.0, "ax":2.85075, "ay":-9.29942, "alpha":0.0, "fx":[48.49041,48.49041,48.49041,48.49041], "fy":[-158.18051,-158.18051,-158.18051,-158.18051]},
+ {"t":1.58527, "x":1.38747, "y":5.69115, "heading":3.14159, "vx":-1.26705, "vy":2.40091, "omega":0.0, "ax":2.87874, "ay":-9.29704, "alpha":0.0, "fx":[48.96654,48.96654,48.96654,48.96654], "fy":[-158.13996,-158.13996,-158.13996,-158.13996]},
+ {"t":1.61605, "x":1.34984, "y":5.76064, "heading":3.14159, "vx":-1.17845, "vy":2.11477, "omega":0.0, "ax":2.89906, "ay":-9.29521, "alpha":0.0, "fx":[49.31222,49.31222,49.31222,49.31222], "fy":[-158.10881,-158.10881,-158.10881,-158.10881]},
+ {"t":1.64682, "x":1.31494, "y":5.82132, "heading":3.14159, "vx":-1.08923, "vy":1.82869, "omega":0.0, "ax":2.91455, "ay":-9.29374, "alpha":0.0, "fx":[49.57562,49.57562,49.57562,49.57562], "fy":[-158.08388,-158.08388,-158.08388,-158.08388]},
+ {"t":1.6776, "x":1.2828, "y":5.8732, "heading":3.14159, "vx":-0.99952, "vy":1.54266, "omega":0.0, "ax":2.92679, "ay":-9.29253, "alpha":0.0, "fx":[49.78381,49.78381,49.78381,49.78381], "fy":[-158.06327,-158.06327,-158.06327,-158.06327]},
+ {"t":1.70838, "x":1.25342, "y":5.91628, "heading":3.14159, "vx":-0.90945, "vy":1.25667, "omega":0.0, "ax":2.93674, "ay":-9.2915, "alpha":0.0, "fx":[49.95317,49.95317,49.95317,49.95317], "fy":[-158.04578,-158.04578,-158.04578,-158.04578]},
+ {"t":1.73915, "x":1.22682, "y":5.95056, "heading":3.14159, "vx":-0.81906, "vy":0.9707, "omega":0.0, "ax":2.94503, "ay":-9.29061, "alpha":0.0, "fx":[50.09418,50.09418,50.09418,50.09418], "fy":[-158.03062,-158.03062,-158.03062,-158.03062]},
+ {"t":1.76993, "x":1.20301, "y":5.97603, "heading":3.14159, "vx":-0.72842, "vy":0.68477, "omega":0.0, "ax":-4.88244, "ay":-8.40909, "alpha":0.0, "fx":[-83.04891,-83.04891,-83.04891,-83.04891], "fy":[-143.03621,-143.03621,-143.03621,-143.03621]},
+ {"t":1.8165, "x":1.1638, "y":5.9988, "heading":3.14159, "vx":-0.95577, "vy":0.29321, "omega":0.0, "ax":-0.77597, "ay":-8.98901, "alpha":0.0, "fx":[-13.19897,-13.19897,-13.19897,-13.19897], "fy":[-152.90048,-152.90048,-152.90048,-152.90048]},
+ {"t":1.86306, "x":1.11845, "y":6.00271, "heading":3.14159, "vx":-0.9919, "vy":-0.12534, "omega":0.0, "ax":0.06692, "ay":-0.48521, "alpha":0.0, "fx":[1.13832,1.13832,1.13832,1.13832], "fy":[-8.25327,-8.25327,-8.25327,-8.25327]},
+ {"t":1.90962, "x":1.07234, "y":5.99635, "heading":3.14159, "vx":-0.98878, "vy":-0.14794, "omega":0.0, "ax":0.00052, "ay":-0.00344, "alpha":0.0, "fx":[0.00876,0.00876,0.00876,0.00876], "fy":[-0.05856,-0.05856,-0.05856,-0.05856]},
+ {"t":1.95618, "x":1.0263, "y":5.98945, "heading":3.14159, "vx":-0.98876, "vy":-0.1481, "omega":0.0, "ax":-0.00007, "ay":0.00047, "alpha":0.0, "fx":[-0.00121,-0.00121,-0.00121,-0.00121], "fy":[0.00808,0.00808,0.00808,0.00808]},
+ {"t":2.00275, "x":0.98026, "y":5.98256, "heading":3.14159, "vx":-0.98876, "vy":-0.14808, "omega":0.0, "ax":-0.00008, "ay":0.00051, "alpha":0.0, "fx":[-0.00132,-0.00132,-0.00132,-0.00132], "fy":[0.00876,0.00876,0.00876,0.00876]},
+ {"t":2.04931, "x":0.93422, "y":5.97566, "heading":3.14159, "vx":-0.98876, "vy":-0.14805, "omega":0.0, "ax":-0.00008, "ay":0.00052, "alpha":0.0, "fx":[-0.00134,-0.00134,-0.00134,-0.00134], "fy":[0.00893,0.00893,0.00893,0.00893]},
+ {"t":2.09587, "x":0.88818, "y":5.96877, "heading":3.14159, "vx":-0.98877, "vy":-0.14803, "omega":0.0, "ax":-0.00008, "ay":0.00054, "alpha":0.0, "fx":[-0.00137,-0.00137,-0.00137,-0.00137], "fy":[0.00913,0.00913,0.00913,0.00913]},
+ {"t":2.14244, "x":0.84214, "y":5.96188, "heading":3.14159, "vx":-0.98877, "vy":-0.148, "omega":0.0, "ax":-0.00008, "ay":0.00055, "alpha":0.0, "fx":[-0.00141,-0.00141,-0.00141,-0.00141], "fy":[0.00936,0.00936,0.00936,0.00936]},
+ {"t":2.189, "x":0.7961, "y":5.95499, "heading":3.14159, "vx":-0.98878, "vy":-0.14798, "omega":0.0, "ax":-0.00009, "ay":0.00057, "alpha":0.0, "fx":[-0.00145,-0.00145,-0.00145,-0.00145], "fy":[0.00965,0.00965,0.00965,0.00965]},
+ {"t":2.23556, "x":0.75006, "y":5.9481, "heading":3.14159, "vx":-0.98878, "vy":-0.14795, "omega":0.0, "ax":-0.00009, "ay":0.00059, "alpha":0.0, "fx":[-0.0015,-0.0015,-0.0015,-0.0015], "fy":[0.01001,0.01001,0.01001,0.01001]},
+ {"t":2.28213, "x":0.70402, "y":5.94121, "heading":3.14159, "vx":-0.98878, "vy":-0.14792, "omega":0.0, "ax":-0.00009, "ay":0.00062, "alpha":0.0, "fx":[-0.00157,-0.00157,-0.00157,-0.00157], "fy":[0.01047,0.01047,0.01047,0.01047]},
+ {"t":2.32869, "x":0.65798, "y":5.93432, "heading":3.14159, "vx":-0.98879, "vy":-0.14789, "omega":0.0, "ax":-0.0001, "ay":0.00067, "alpha":0.0, "fx":[-0.00172,-0.00172,-0.00172,-0.00172], "fy":[0.01145,0.01145,0.01145,0.01145]},
+ {"t":2.37525, "x":0.61194, "y":5.92744, "heading":3.14159, "vx":-0.98879, "vy":-0.14786, "omega":0.0, "ax":-0.00053, "ay":0.00356, "alpha":0.0, "fx":[-0.00906,-0.00906,-0.00906,-0.00906], "fy":[0.06055,0.06055,0.06055,0.06055]},
+ {"t":2.42182, "x":0.56589, "y":5.92056, "heading":3.14159, "vx":-0.98882, "vy":-0.1477, "omega":0.0, "ax":-0.04983, "ay":0.35385, "alpha":0.0, "fx":[-0.84761,-0.84761,-0.84761,-0.84761], "fy":[6.01893,6.01893,6.01893,6.01893]},
+ {"t":2.46838, "x":0.5198, "y":5.91406, "heading":3.14159, "vx":-0.99114, "vy":-0.13122, "omega":0.0, "ax":0.65084, "ay":8.73589, "alpha":0.0, "fx":[11.07052,11.07052,11.07052,11.07052], "fy":[148.595,148.595,148.595,148.595]},
+ {"t":2.51494, "x":0.47435, "y":5.91742, "heading":3.14159, "vx":-0.96083, "vy":0.27555, "omega":0.0, "ax":8.44163, "ay":4.84358, "alpha":0.0, "fx":[143.58977,143.58977,143.58977,143.58977], "fy":[82.38793,82.38793,82.38793,82.38793]},
+ {"t":2.56151, "x":0.43877, "y":5.9355, "heading":3.14159, "vx":-0.56776, "vy":0.50108, "omega":0.0, "ax":9.3492, "ay":2.7711, "alpha":0.0, "fx":[159.02729,159.02729,159.02729,159.02729], "fy":[47.13566,47.13566,47.13566,47.13566]},
+ {"t":2.60807, "x":0.42246, "y":5.96184, "heading":3.14159, "vx":-0.13244, "vy":0.63011, "omega":0.0, "ax":9.47314, "ay":2.31528, "alpha":0.0, "fx":[161.13544,161.13544,161.13544,161.13544], "fy":[39.38233,39.38233,39.38233,39.38233]},
+ {"t":2.64789, "x":0.4247, "y":5.98877, "heading":3.14159, "vx":0.24478, "vy":0.72231, "omega":0.0, "ax":9.5479, "ay":1.97827, "alpha":0.0, "fx":[162.40699,162.40699,162.40699,162.40699], "fy":[33.64986,33.64986,33.64986,33.64986]},
+ {"t":2.68771, "x":0.44202, "y":6.0191, "heading":3.14159, "vx":0.62498, "vy":0.80108, "omega":0.0, "ax":9.62544, "ay":1.54776, "alpha":0.0, "fx":[163.72597,163.72597,163.72597,163.72597], "fy":[26.32689,26.32689,26.32689,26.32689]},
+ {"t":2.72753, "x":0.47453, "y":6.05222, "heading":3.14159, "vx":1.00826, "vy":0.86271, "omega":0.0, "ax":9.69732, "ay":0.98323, "alpha":0.0, "fx":[164.94872,164.94872,164.94872,164.94872], "fy":[16.72448,16.72448,16.72448,16.72448]},
+ {"t":2.76735, "x":0.52237, "y":6.08735, "heading":3.14159, "vx":1.3944, "vy":0.90186, "omega":0.0, "ax":9.74189, "ay":0.22086, "alpha":0.0, "fx":[165.70678,165.70678,165.70678,165.70678], "fy":[3.75675,3.75675,3.75675,3.75675]},
+ {"t":2.80717, "x":0.58562, "y":6.12344, "heading":3.14159, "vx":1.78232, "vy":0.91066, "omega":0.0, "ax":9.70463, "ay":-0.83986, "alpha":0.0, "fx":[165.07301,165.07301,165.07301,165.07301], "fy":[-14.28583,-14.28583,-14.28583,-14.28583]},
+ {"t":2.84699, "x":0.66428, "y":6.15904, "heading":3.14159, "vx":2.16875, "vy":0.87722, "omega":0.0, "ax":9.4498, "ay":-2.34477, "alpha":0.0, "fx":[160.73839,160.73839,160.73839,160.73839], "fy":[-39.8839,-39.8839,-39.8839,-39.8839]},
+ {"t":2.88681, "x":0.75813, "y":6.19211, "heading":3.14159, "vx":2.54504, "vy":0.78385, "omega":0.0, "ax":8.66341, "ay":-4.43124, "alpha":0.0, "fx":[147.36215,147.36215,147.36215,147.36215], "fy":[-75.37411,-75.37411,-75.37411,-75.37411]},
+ {"t":2.92663, "x":0.86635, "y":6.21981, "heading":3.14159, "vx":2.89001, "vy":0.6074, "omega":0.0, "ax":6.82056, "ay":-6.93383, "alpha":0.0, "fx":[116.01573,116.01573,116.01573,116.01573], "fy":[-117.94252,-117.94252,-117.94252,-117.94252]},
+ {"t":2.96645, "x":0.98683, "y":6.2385, "heading":3.14159, "vx":3.16161, "vy":0.3313, "omega":0.0, "ax":3.77071, "ay":-8.96463, "alpha":0.0, "fx":[64.13872,64.13872,64.13872,64.13872], "fy":[-152.48583,-152.48583,-152.48583,-152.48583]},
+ {"t":3.00626, "x":1.11571, "y":6.24458, "heading":3.14159, "vx":3.31175, "vy":-0.02567, "omega":0.0, "ax":0.51801, "ay":-9.7155, "alpha":0.0, "fx":[8.81126,8.81126,8.81126,8.81126], "fy":[-165.25782,-165.25782,-165.25782,-165.25782]},
+ {"t":3.04608, "x":1.248, "y":6.23586, "heading":3.14159, "vx":3.33238, "vy":-0.41254, "omega":0.0, "ax":-1.9561, "ay":-9.53625, "alpha":0.0, "fx":[-33.27275,-33.27275,-33.27275,-33.27275], "fy":[-162.20882,-162.20882,-162.20882,-162.20882]},
+ {"t":3.0859, "x":1.37914, "y":6.21187, "heading":3.14159, "vx":3.25449, "vy":-0.79227, "omega":0.0, "ax":-3.59234, "ay":-9.05296, "alpha":0.0, "fx":[-61.10467,-61.10467,-61.10467,-61.10467], "fy":[-153.9883,-153.9883,-153.9883,-153.9883]},
+ {"t":3.12572, "x":1.50589, "y":6.17314, "heading":3.14159, "vx":3.11144, "vy":-1.15275, "omega":0.0, "ax":-4.66024, "ay":-8.55669, "alpha":0.0, "fx":[-79.26927,-79.26927,-79.26927,-79.26927], "fy":[-145.54679,-145.54679,-145.54679,-145.54679]},
+ {"t":3.16554, "x":1.62609, "y":6.12046, "heading":3.14159, "vx":2.92587, "vy":-1.49348, "omega":0.0, "ax":-5.38038, "ay":-8.12663, "alpha":0.0, "fx":[-91.51872,-91.51872,-91.51872,-91.51872], "fy":[-138.23165,-138.23165,-138.23165,-138.23165]},
+ {"t":3.20536, "x":1.73833, "y":6.05455, "heading":3.14159, "vx":2.71163, "vy":-1.81708, "omega":0.0, "ax":-5.88681, "ay":-7.77039, "alpha":0.0, "fx":[-100.13296,-100.13296,-100.13296,-100.13296], "fy":[-132.17207,-132.17207,-132.17207,-132.17207]},
+ {"t":3.24518, "x":1.84164, "y":5.97603, "heading":3.14159, "vx":2.47722, "vy":-2.12649, "omega":0.0, "ax":-5.23919, "ay":-7.35511, "alpha":-9.96289, "fx":[-152.26373,-117.57601,-27.23444,-59.39415], "fy":[-65.5778,-116.9683,-163.5104,-154.37663]},
+ {"t":3.27405, "x":1.91096, "y":5.91159, "heading":3.14159, "vx":2.326, "vy":-2.33879, "omega":-0.28757, "ax":-5.51629, "ay":-7.23196, "alpha":-9.37966, "fx":[-152.59374,-119.25674,-33.36758,-70.1037], "fy":[-64.73809,-115.22055,-162.33379,-149.76191]},
+ {"t":3.30291, "x":1.9758, "y":5.84107, "heading":3.13329, "vx":2.16678, "vy":-2.54753, "omega":-0.5583, "ax":-5.83074, "ay":-7.06397, "alpha":-8.80217, "fx":[-153.17534,-121.63811,-41.04599,-80.85716], "fy":[-63.26337,-112.66119,-160.52053,-144.17938]},
+ {"t":3.33177, "x":2.03591, "y":5.76459, "heading":3.11718, "vx":1.99848, "vy":-2.75142, "omega":-0.81237, "ax":-6.19521, "ay":-6.8368, "alpha":-8.18471, "fx":[-153.99457,-124.77007,-50.67825,-92.07242], "fy":[-61.13034,-109.12709,-157.68979,-137.22056]},
+ {"t":3.36064, "x":2.09101, "y":5.68233, "heading":3.09373, "vx":1.81966, "vy":-2.94876, "omega":-1.04861, "ax":-6.62572, "ay":-6.5258, "alpha":-7.47021, "fx":[-155.0362,-128.7378,-62.8783,-104.15417], "fy":[-58.28907,-104.34177,-153.16296,-128.21394]},
+ {"t":3.3895, "x":2.14078, "y":5.5945, "heading":3.06346, "vx":1.62842, "vy":-3.13712, "omega":-1.26423, "ax":-7.14032, "ay":-6.0895, "alpha":-6.58306, "fx":[-156.28504,-133.67842,-78.51857,-117.33709], "fy":[-54.64677,-97.82473,-145.66826,-116.18281]},
+ {"t":3.41837, "x":2.1848, "y":5.50141, "heading":3.02697, "vx":1.42232, "vy":-3.31289, "omega":-1.45424, "ax":-7.75384, "ay":-5.45812, "alpha":-5.41824, "fx":[-157.72746,-139.79819,-98.63199,-131.40453], "fy":[-50.0337,-88.69383,-132.73771,-99.89914]},
+ {"t":3.44723, "x":2.22263, "y":5.40352, "heading":2.985, "vx":1.19851, "vy":-3.47043, "omega":-1.61063, "ax":-8.45834, "ay":-4.51715, "alpha":-3.83927, "fx":[-159.3526,-147.33975,-123.51732,-145.28577], "fy":[-44.12574,-75.21704,-109.74684,-78.25236]},
+ {"t":3.47609, "x":2.2537, "y":5.30146, "heading":2.93851, "vx":0.95437, "vy":-3.60081, "omega":-1.72145, "ax":-9.1601, "ay":-3.12124, "alpha":-1.79436, "fx":[-161.15152,-156.13877,-149.1739,-156.77828], "fy":[-36.23186,-54.0358,-70.69145,-51.40624]},
+ {"t":3.50496, "x":2.27743, "y":5.19623, "heading":2.88882, "vx":0.68998, "vy":-3.6909, "omega":-1.77324, "ax":-9.55142, "ay":-1.66222, "alpha":-0.4037, "fx":[-163.17302,-162.38935,-161.69574,-162.60938], "fy":[-24.17844,-29.06049,-32.44891,-27.4074]},
+ {"t":3.53382, "x":2.29337, "y":5.08901, "heading":2.83764, "vx":0.41429, "vy":-3.73888, "omega":-1.78489, "ax":-9.64906, "ay":-0.74458, "alpha":-0.67148, "fx":[-164.63187,-164.13768,-163.46711,-164.27466], "fy":[-5.84365,-14.32241,-19.76324,-10.73118]},
+ {"t":3.56269, "x":2.3013, "y":4.98078, "heading":2.78612, "vx":0.13578, "vy":-3.76037, "omega":-1.80427, "ax":-9.63684, "ay":0.25439, "alpha":-1.4703, "fx":[-163.43255,-164.54481,-163.85049,-163.8517], "fy":[18.63293,0.08014,-11.5835,10.17881]},
+ {"t":3.59155, "x":2.30121, "y":4.87234, "heading":2.73404, "vx":-0.14238, "vy":-3.75303, "omega":-1.84671, "ax":-9.43478, "ay":1.45377, "alpha":-2.87821, "fx":[-156.716,-163.60312,-163.48471,-158.12812], "fy":[49.15977,15.38084,-6.75859,41.13058]},
+ {"t":3.62041, "x":2.29317, "y":4.76462, "heading":2.68074, "vx":-0.4147, "vy":-3.71107, "omega":-1.92979, "ax":-8.79621, "ay":2.98612, "alpha":-5.38101, "fx":[-141.439,-160.81009,-162.46953,-133.76559], "fy":[83.10557,32.74388,-4.24365,91.56607]},
+ {"t":3.64928, "x":2.27753, "y":4.65875, "heading":2.62504, "vx":-0.6686, "vy":-3.62488, "omega":-2.08511, "ax":-7.22374, "ay":4.87173, "alpha":-9.35282, "fx":[-116.23308,-153.95943,-160.07168,-61.23094], "fy":[115.608,56.18282,10.64212,149.03417]},
+ {"t":3.67814, "x":2.25523, "y":4.55615, "heading":2.56485, "vx":-0.8771, "vy":-3.48426, "omega":-2.35506, "ax":-5.05046, "ay":6.30992, "alpha":-12.54923, "fx":[-84.39127,-141.82363,-147.4446,30.03231], "fy":[140.57365,81.81128,48.09584,158.83926]},
+ {"t":3.707, "x":2.22781, "y":4.45821, "heading":2.49688, "vx":-1.02288, "vy":-3.30213, "omega":-2.71728, "ax":-1.78963, "ay":7.20981, "alpha":-14.91305, "fx":[-53.31823,-129.05342,-32.19106,92.79818], "fy":[155.19988,100.71114,101.13473,133.50172]},
+ {"t":3.73587, "x":2.19754, "y":4.3659, "heading":2.41844, "vx":-1.07453, "vy":-3.09403, "omega":-3.14773, "ax":0.6823, "ay":8.80232, "alpha":-10.18299, "fx":[-14.56522,-76.17763,50.96836,86.19755], "fy":[163.56412,144.87491,151.62703,138.83373]},
+ {"t":3.76473, "x":2.16681, "y":4.28026, "heading":2.32759, "vx":-1.05484, "vy":-2.83996, "omega":-3.44165, "ax":1.84452, "ay":9.46254, "alpha":1.9236, "fx":[37.0798,46.65387,26.24109,15.52403], "fy":[160.01463,157.66248,162.44866,163.69479]},
+ {"t":3.7936, "x":2.13713, "y":4.20223, "heading":2.22825, "vx":-1.0016, "vy":-2.56683, "omega":-3.38613, "ax":2.67008, "ay":8.96882, "alpha":6.93388, "fx":[69.98179,95.71282,29.04617,-13.0719], "fy":[148.71386,134.37242,162.64731,164.49485]},
+ {"t":3.82246, "x":2.10933, "y":4.13188, "heading":2.13051, "vx":-0.92453, "vy":-2.30796, "omega":-3.18599, "ax":3.17126, "ay":8.54228, "alpha":9.04046, "fx":[86.79727,117.57777,34.82344,-23.42957], "fy":[139.65475,116.11937,161.77763,163.65518]},
+ {"t":3.85132, "x":2.08397, "y":4.06882, "heading":2.03855, "vx":-0.833, "vy":-2.0614, "omega":-2.92505, "ax":3.48878, "ay":8.25752, "alpha":9.89636, "fx":[92.61038,130.11597,41.12849,-26.48255], "fy":[135.8988,102.12528,160.43485,163.37315]},
+ {"t":3.88019, "x":2.06138, "y":4.01276, "heading":1.95412, "vx":-0.7323, "vy":-1.82305, "omega":-2.6394, "ax":3.66426, "ay":8.09388, "alpha":10.14612, "fx":[89.92737,138.44843,47.26372,-26.32746], "fy":[137.66875,90.68774,158.82568,163.51587]},
+ {"t":3.90905, "x":2.04176, "y":3.96351, "heading":1.87794, "vx":-0.62653, "vy":-1.58943, "omega":-2.34654, "ax":3.70256, "ay":8.02354, "alpha":10.15008, "fx":[79.2075,144.44861,52.9657,-24.70404], "fy":[144.05885,80.9266,157.07777,163.84911]},
+ {"t":3.93792, "x":2.02522, "y":3.92098, "heading":1.81021, "vx":-0.51966, "vy":-1.35784, "omega":-2.05357, "ax":3.59945, "ay":8.00816, "alpha":10.19996, "fx":[60.26358,148.94802,58.12626,-22.43535], "fy":[152.92232,72.42429,155.28695,164.23239]},
+ {"t":3.96678, "x":2.01172, "y":3.88512, "heading":1.75094, "vx":-0.41577, "vy":-1.12669, "omega":-1.75916, "ax":3.37279, "ay":7.99385, "alpha":10.54411, "fx":[34.35649,152.37186,62.69827,-19.94617], "fy":[160.75597,65.00856,153.532,164.59569]},
+ {"t":3.99564, "x":2.00113, "y":3.85593, "heading":1.70016, "vx":-0.31842, "vy":-0.89596, "omega":-1.45482, "ax":3.08557, "ay":7.93362, "alpha":11.26732, "fx":[5.80367,154.96961,66.65259,-17.4869], "fy":[164.37081,58.63258,151.88379,164.90746]},
+ {"t":4.02451, "x":1.99322, "y":3.83337, "heading":1.65817, "vx":-0.22935, "vy":-0.66697, "omega":-1.1296, "ax":2.81984, "ay":7.82331, "alpha":12.21022, "fx":[-19.76488,156.90998,69.95532,-15.24178], "fy":[163.41901,53.30183,150.41263,165.15532]},
+ {"t":4.05337, "x":1.98778, "y":3.81738, "heading":1.62556, "vx":-0.14796, "vy":-0.44115, "omega":-0.77717, "ax":2.6239, "ay":7.69625, "alpha":13.11584, "fx":[-38.97793,158.31825,72.56007,-13.37335], "fy":[160.07848,49.03714,149.19113,165.33711]},
+ {"t":4.08223, "x":1.9846, "y":3.80785, "heading":1.60313, "vx":-0.07223, "vy":-0.21901, "omega":-0.39859, "ax":2.50235, "ay":7.58773, "alpha":13.80941, "fx":[-51.42039,159.28993,74.41472,-12.02725], "fy":[156.65654,45.85797,148.28945,165.45655]},
+ {"t":4.1111, "x":1.98356, "y":3.80469, "heading":1.59163, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+ "splits":[0]
+ },
+ "events":[]
+}
diff --git a/lib/examples/swerve/src/main/deploy/choreo/RIGHT_TRENCH.traj b/lib/examples/swerve/src/main/deploy/choreo/RIGHT_TRENCH.traj
new file mode 100644
index 00000000..bee355fd
--- /dev/null
+++ b/lib/examples/swerve/src/main/deploy/choreo/RIGHT_TRENCH.traj
@@ -0,0 +1,213 @@
+{
+ "name":"RIGHT_TRENCH",
+ "version":3,
+ "snapshot":{
+ "waypoints":[
+ {"x":4.441253185272217, "y":0.6037423729896545, "heading":1.5707963267948966, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":7.795207023620605, "y":1.3253390531539917, "heading":1.5707963267948966, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":7.795207023620605, "y":3.931103515625, "heading":-1.5707963267948966, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":5.714679718017578, "y":0.62682590484619, "heading":1.5707963267948966, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":3.2933757305145264, "y":0.62682590484619, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":1.9037671089172363, "y":4.050841808319092, "heading":1.570796602033749, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true},
+ {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":3.0}}, "enabled":true},
+ {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}],
+ "targetDt":0.05
+ },
+ "params":{
+ "waypoints":[
+ {"x":{"exp":"4.441253185272217 m", "val":4.441253185272217}, "y":{"exp":"0.6037423729896545 m", "val":0.6037423729896545}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"7.7952070236206055 m", "val":7.795207023620605}, "y":{"exp":"1.3253390531539917 m", "val":1.3253390531539917}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"7.7952070236206055 m", "val":7.795207023620605}, "y":{"exp":"3.931103515625 m", "val":3.931103515625}, "heading":{"exp":"-1.5707963267948966 rad", "val":-1.5707963267948966}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":{"exp":"5.714679718017578 m", "val":5.714679718017578}, "y":{"exp":"0.62682590484619 m", "val":0.62682590484619}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"3.2933757305145264 m", "val":3.2933757305145264}, "y":{"exp":"0.62682590484619 m", "val":0.62682590484619}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":{"exp":"1.9037671089172363 m", "val":1.9037671089172363}, "y":{"exp":"4.050841808319092 m", "val":4.050841808319092}, "heading":{"exp":"1.570796602033749 rad", "val":1.570796602033749}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true},
+ {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3 m / s", "val":3.0}}}, "enabled":true},
+ {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}],
+ "targetDt":{
+ "exp":"0.05 s",
+ "val":0.05
+ }
+ },
+ "trajectory":{
+ "config":{
+ "frontLeft":{
+ "x":0.225425,
+ "y":0.333375
+ },
+ "backLeft":{
+ "x":-0.225425,
+ "y":0.333375
+ },
+ "mass":68.0388555,
+ "inertia":7.5,
+ "gearing":7.03,
+ "radius":0.0508,
+ "vmax":607.3745796940267,
+ "tmax":1.2,
+ "cof":1.5,
+ "bumper":{
+ "front":0.34925,
+ "side":0.4445,
+ "back":0.34925
+ },
+ "differentialTrackWidth":0.5588
+ },
+ "sampleType":"Swerve",
+ "waypoints":[0.0,1.40394,2.41372,3.87973,4.70708,6.10492],
+ "samples":[
+ {"t":0.0, "x":4.44125, "y":0.60374, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.75357, "ay":-0.05824, "alpha":0.0, "fx":[165.90536,165.90536,165.90536,165.90536], "fy":[-0.99062,-0.99062,-0.99062,-0.99062]},
+ {"t":0.0468, "x":4.45193, "y":0.60368, "heading":1.5708, "vx":0.45645, "vy":-0.00273, "omega":0.0, "ax":9.75174, "ay":-0.05985, "alpha":0.0, "fx":[165.8743,165.8743,165.8743,165.8743], "fy":[-1.01805,-1.01805,-1.01805,-1.01805]},
+ {"t":0.0936, "x":4.48397, "y":0.60349, "heading":1.5708, "vx":0.91281, "vy":-0.00553, "omega":0.0, "ax":9.74901, "ay":-0.06147, "alpha":0.0, "fx":[165.82782,165.82782,165.82782,165.82782], "fy":[-1.04558,-1.04558,-1.04558,-1.04558]},
+ {"t":0.14039, "x":4.53737, "y":0.60316, "heading":1.5708, "vx":1.36904, "vy":-0.0084, "omega":0.0, "ax":9.74446, "ay":-0.0631, "alpha":0.0, "fx":[165.75056,165.75056,165.75056,165.75056], "fy":[-1.07332,-1.07332,-1.07332,-1.07332]},
+ {"t":0.18719, "x":4.6121, "y":0.6027, "heading":1.5708, "vx":1.82506, "vy":-0.01136, "omega":0.0, "ax":9.73541, "ay":-0.06476, "alpha":0.0, "fx":[165.59661,165.59661,165.59661,165.59661], "fy":[-1.10152,-1.10152,-1.10152,-1.10152]},
+ {"t":0.23399, "x":4.70817, "y":0.60209, "heading":1.5708, "vx":2.28066, "vy":-0.01439, "omega":0.0, "ax":9.70849, "ay":-0.06652, "alpha":0.0, "fx":[165.13856,165.13856,165.13856,165.13856], "fy":[-1.13142,-1.13142,-1.13142,-1.13142]},
+ {"t":0.28079, "x":4.82553, "y":0.60135, "heading":1.5708, "vx":2.73499, "vy":-0.0175, "omega":0.0, "ax":5.64742, "ay":-0.07918, "alpha":0.0, "fx":[96.06098,96.06098,96.06098,96.06098], "fy":[-1.34684,-1.34684,-1.34684,-1.34684]},
+ {"t":0.32758, "x":4.95971, "y":0.60044, "heading":1.5708, "vx":2.99928, "vy":-0.0212, "omega":0.0, "ax":-0.00004, "ay":-0.02368, "alpha":0.0, "fx":[-0.00064,-0.00064,-0.00064,-0.00064], "fy":[-0.40272,-0.40272,-0.40272,-0.40272]},
+ {"t":0.37438, "x":5.10007, "y":0.59942, "heading":1.5708, "vx":2.99928, "vy":-0.02231, "omega":0.0, "ax":-0.00016, "ay":-0.02147, "alpha":0.0, "fx":[-0.00278,-0.00278,-0.00278,-0.00278], "fy":[-0.36526,-0.36526,-0.36526,-0.36526]},
+ {"t":0.42118, "x":5.24043, "y":0.59836, "heading":1.5708, "vx":2.99927, "vy":-0.02332, "omega":0.0, "ax":-0.00017, "ay":-0.02141, "alpha":0.0, "fx":[-0.00289,-0.00289,-0.00289,-0.00289], "fy":[-0.3641,-0.3641,-0.3641,-0.3641]},
+ {"t":0.46798, "x":5.38079, "y":0.59724, "heading":1.5708, "vx":2.99926, "vy":-0.02432, "omega":0.0, "ax":-0.00018, "ay":-0.02148, "alpha":0.0, "fx":[-0.00303,-0.00303,-0.00303,-0.00303], "fy":[-0.36533,-0.36533,-0.36533,-0.36533]},
+ {"t":0.51478, "x":5.52115, "y":0.59608, "heading":1.5708, "vx":2.99925, "vy":-0.02532, "omega":0.0, "ax":-0.00019, "ay":-0.02156, "alpha":0.0, "fx":[-0.00316,-0.00316,-0.00316,-0.00316], "fy":[-0.36679,-0.36679,-0.36679,-0.36679]},
+ {"t":0.56157, "x":5.66151, "y":0.59487, "heading":1.5708, "vx":2.99924, "vy":-0.02633, "omega":0.0, "ax":-0.00019, "ay":-0.02165, "alpha":0.0, "fx":[-0.0033,-0.0033,-0.0033,-0.0033], "fy":[-0.36833,-0.36833,-0.36833,-0.36833]},
+ {"t":0.60837, "x":5.80186, "y":0.59362, "heading":1.5708, "vx":2.99924, "vy":-0.02735, "omega":0.0, "ax":-0.0002, "ay":-0.02175, "alpha":0.0, "fx":[-0.00344,-0.00344,-0.00344,-0.00344], "fy":[-0.36994,-0.36994,-0.36994,-0.36994]},
+ {"t":0.65517, "x":5.94222, "y":0.59231, "heading":1.5708, "vx":2.99923, "vy":-0.02836, "omega":0.0, "ax":-0.00021, "ay":-0.02185, "alpha":0.0, "fx":[-0.00358,-0.00358,-0.00358,-0.00358], "fy":[-0.37162,-0.37162,-0.37162,-0.37162]},
+ {"t":0.70197, "x":6.08258, "y":0.59096, "heading":1.5708, "vx":2.99922, "vy":-0.02939, "omega":0.0, "ax":-0.00022, "ay":-0.02195, "alpha":0.0, "fx":[-0.00372,-0.00372,-0.00372,-0.00372], "fy":[-0.37332,-0.37332,-0.37332,-0.37332]},
+ {"t":0.74877, "x":6.22293, "y":0.58956, "heading":1.5708, "vx":2.99921, "vy":-0.03041, "omega":0.0, "ax":-0.00023, "ay":-0.022, "alpha":0.0, "fx":[-0.00386,-0.00386,-0.00386,-0.00386], "fy":[-0.37419,-0.37419,-0.37419,-0.37419]},
+ {"t":0.79556, "x":6.36329, "y":0.58811, "heading":1.5708, "vx":2.9992, "vy":-0.03144, "omega":0.0, "ax":-0.00023, "ay":-0.02122, "alpha":0.0, "fx":[-0.00384,-0.00384,-0.00384,-0.00384], "fy":[-0.36088,-0.36088,-0.36088,-0.36088]},
+ {"t":0.84236, "x":6.50365, "y":0.58662, "heading":1.5708, "vx":2.99918, "vy":-0.03244, "omega":0.0, "ax":-0.00008, "ay":-0.00741, "alpha":0.0, "fx":[-0.00137,-0.00137,-0.00137,-0.00137], "fy":[-0.12601,-0.12601,-0.12601,-0.12601]},
+ {"t":0.88916, "x":6.644, "y":0.58509, "heading":1.5708, "vx":2.99918, "vy":-0.03278, "omega":0.0, "ax":0.00194, "ay":0.20892, "alpha":0.0, "fx":[0.03303,0.03303,0.03303,0.03303], "fy":[3.55373,3.55373,3.55373,3.55373]},
+ {"t":0.93596, "x":6.78436, "y":0.58379, "heading":1.5708, "vx":2.99927, "vy":-0.02301, "omega":0.0, "ax":-0.05514, "ay":3.19297, "alpha":0.0, "fx":[-0.93799,-0.93799,-0.93799,-0.93799], "fy":[54.31147,54.31147,54.31147,54.31147]},
+ {"t":0.98275, "x":6.92466, "y":0.58621, "heading":1.5708, "vx":2.99669, "vy":0.12642, "omega":0.0, "ax":-0.98673, "ay":8.81506, "alpha":0.0, "fx":[-16.78395,-16.78395,-16.78395,-16.78395], "fy":[149.94157,149.94157,149.94157,149.94157]},
+ {"t":1.02955, "x":7.06382, "y":0.60178, "heading":1.5708, "vx":2.95051, "vy":0.53894, "omega":0.0, "ax":-2.41154, "ay":9.23765, "alpha":0.0, "fx":[-41.01952,-41.01952,-41.01952,-41.01952], "fy":[157.12981,157.12981,157.12981,157.12981]},
+ {"t":1.07635, "x":7.19925, "y":0.63711, "heading":1.5708, "vx":2.83766, "vy":0.97125, "omega":0.0, "ax":-3.81516, "ay":8.88585, "alpha":0.0, "fx":[-64.89478,-64.89478,-64.89478,-64.89478], "fy":[151.14583,151.14583,151.14583,151.14583]},
+ {"t":1.12315, "x":7.32787, "y":0.6923, "heading":1.5708, "vx":2.65912, "vy":1.38708, "omega":0.0, "ax":-5.13673, "ay":8.24187, "alpha":0.0, "fx":[-87.37424,-87.37424,-87.37424,-87.37424], "fy":[140.19186,140.19186,140.19186,140.19186]},
+ {"t":1.16995, "x":7.44669, "y":0.76623, "heading":1.5708, "vx":2.41873, "vy":1.77279, "omega":0.0, "ax":-6.51104, "ay":7.23067, "alpha":0.0, "fx":[-110.751,-110.751,-110.751,-110.751], "fy":[122.99159,122.99159,122.99159,122.99159]},
+ {"t":1.21674, "x":7.55275, "y":0.85711, "heading":1.5708, "vx":2.11403, "vy":2.11117, "omega":0.0, "ax":-8.27465, "ay":5.13891, "alpha":0.0, "fx":[-140.7494,-140.7494,-140.7494,-140.7494], "fy":[87.41144,87.41144,87.41144,87.41144]},
+ {"t":1.26354, "x":7.64262, "y":0.96154, "heading":1.5708, "vx":1.72679, "vy":2.35166, "omega":0.0, "ax":-8.96407, "ay":3.8262, "alpha":0.0, "fx":[-152.47628,-152.47628,-152.47628,-152.47628], "fy":[65.08257,65.08257,65.08257,65.08257]},
+ {"t":1.31034, "x":7.71362, "y":1.07578, "heading":1.5708, "vx":1.30729, "vy":2.53071, "omega":0.0, "ax":-9.26781, "ay":3.02852, "alpha":0.0, "fx":[-157.64283,-157.64283,-157.64283,-157.64283], "fy":[51.5142,51.5142,51.5142,51.5142]},
+ {"t":1.35714, "x":7.76465, "y":1.19753, "heading":1.5708, "vx":0.87358, "vy":2.67244, "omega":0.0, "ax":-9.42477, "ay":2.5067, "alpha":0.0, "fx":[-160.31267,-160.31267,-160.31267,-160.31267], "fy":[42.63818,42.63818,42.63818,42.63818]},
+ {"t":1.40394, "x":7.79521, "y":1.32534, "heading":1.5708, "vx":0.43252, "vy":2.78975, "omega":0.0, "ax":-9.26749, "ay":3.004, "alpha":0.0, "fx":[-157.63742,-157.63742,-157.63742,-157.63742], "fy":[51.09726,51.09726,51.09726,51.09726]},
+ {"t":1.44601, "x":7.8052, "y":1.44538, "heading":1.5708, "vx":0.04259, "vy":2.91614, "omega":0.0, "ax":-3.01713, "ay":1.94817, "alpha":0.0, "fx":[-51.32047,-51.32047,-51.32047,-51.32047], "fy":[33.13778,33.13778,33.13778,33.13778]},
+ {"t":1.48808, "x":7.80432, "y":1.5698, "heading":1.5708, "vx":-0.08435, "vy":2.99811, "omega":0.0, "ax":0.87649, "ay":0.01931, "alpha":0.0, "fx":[14.90881,14.90881,14.90881,14.90881], "fy":[0.3285,0.3285,0.3285,0.3285]},
+ {"t":1.53016, "x":7.80155, "y":1.69596, "heading":1.5708, "vx":-0.04747, "vy":2.99892, "omega":0.0, "ax":0.38228, "ay":0.00502, "alpha":0.0, "fx":[6.5025,6.5025,6.5025,6.5025], "fy":[0.08546,0.08546,0.08546,0.08546]},
+ {"t":1.57223, "x":7.79989, "y":1.82214, "heading":1.5708, "vx":-0.03139, "vy":2.99913, "omega":0.0, "ax":0.2206, "ay":0.00197, "alpha":0.0, "fx":[3.75241,3.75241,3.75241,3.75241], "fy":[0.03344,0.03344,0.03344,0.03344]},
+ {"t":1.61431, "x":7.79877, "y":1.94833, "heading":1.5708, "vx":-0.02211, "vy":2.99922, "omega":0.0, "ax":0.15127, "ay":0.00095, "alpha":0.0, "fx":[2.57305,2.57305,2.57305,2.57305], "fy":[0.01622,0.01622,0.01622,0.01622]},
+ {"t":1.65638, "x":7.79797, "y":2.07452, "heading":1.5708, "vx":-0.01574, "vy":2.99926, "omega":0.0, "ax":0.11267, "ay":0.0005, "alpha":0.0, "fx":[1.91641,1.91641,1.91641,1.91641], "fy":[0.00853,0.00853,0.00853,0.00853]},
+ {"t":1.69846, "x":7.79741, "y":2.20071, "heading":1.5708, "vx":-0.011, "vy":2.99928, "omega":0.0, "ax":0.08874, "ay":0.00027, "alpha":0.0, "fx":[1.50943,1.50943,1.50943,1.50943], "fy":[0.00458,0.00458,0.00458,0.00458]},
+ {"t":1.74053, "x":7.79702, "y":2.32691, "heading":1.5708, "vx":-0.00727, "vy":2.99929, "omega":0.0, "ax":0.07388, "ay":0.00014, "alpha":0.0, "fx":[1.25663,1.25663,1.25663,1.25663], "fy":[0.00238,0.00238,0.00238,0.00238]},
+ {"t":1.78261, "x":7.79678, "y":2.4531, "heading":1.5708, "vx":-0.00416, "vy":2.9993, "omega":0.0, "ax":0.06563, "ay":0.00006, "alpha":0.0, "fx":[1.11643,1.11643,1.11643,1.11643], "fy":[0.00102,0.00102,0.00102,0.00102]},
+ {"t":1.82468, "x":7.79667, "y":2.57929, "heading":1.5708, "vx":-0.0014, "vy":2.9993, "omega":0.0, "ax":0.0629, "ay":0.0, "alpha":0.0, "fx":[1.06999,1.06999,1.06999,1.06999], "fy":[0.00002,0.00002,0.00002,0.00002]},
+ {"t":1.86676, "x":7.79666, "y":2.70549, "heading":1.5708, "vx":0.00125, "vy":2.9993, "omega":0.0, "ax":0.06535, "ay":-0.00006, "alpha":0.0, "fx":[1.11163,1.11163,1.11163,1.11163], "fy":[-0.00098,-0.00098,-0.00098,-0.00098]},
+ {"t":1.90883, "x":7.79677, "y":2.83168, "heading":1.5708, "vx":0.004, "vy":2.9993, "omega":0.0, "ax":0.07328, "ay":-0.00014, "alpha":0.0, "fx":[1.24653,1.24653,1.24653,1.24653], "fy":[-0.00231,-0.00231,-0.00231,-0.00231]},
+ {"t":1.9509, "x":7.79701, "y":2.95788, "heading":1.5708, "vx":0.00708, "vy":2.99929, "omega":0.0, "ax":0.08781, "ay":-0.00026, "alpha":0.0, "fx":[1.49358,1.49358,1.49358,1.49358], "fy":[-0.00445,-0.00445,-0.00445,-0.00445]},
+ {"t":1.99298, "x":7.79738, "y":3.08407, "heading":1.5708, "vx":0.01078, "vy":2.99928, "omega":0.0, "ax":0.11187, "ay":-0.00049, "alpha":0.0, "fx":[1.90292,1.90292,1.90292,1.90292], "fy":[-0.00833,-0.00833,-0.00833,-0.00833]},
+ {"t":2.03505, "x":7.79793, "y":3.21026, "heading":1.5708, "vx":0.01548, "vy":2.99926, "omega":0.0, "ax":0.15781, "ay":-0.00099, "alpha":0.0, "fx":[2.68438,2.68438,2.68438,2.68438], "fy":[-0.01683,-0.01683,-0.01683,-0.01683]},
+ {"t":2.07713, "x":7.79873, "y":3.33645, "heading":1.5708, "vx":0.02212, "vy":2.99922, "omega":0.0, "ax":0.32125, "ay":-0.00545, "alpha":0.0, "fx":[5.46437,5.46437,5.46437,5.46437], "fy":[-0.09263,-0.09263,-0.09263,-0.09263]},
+ {"t":2.1192, "x":7.79994, "y":3.46264, "heading":1.5708, "vx":0.03564, "vy":2.99899, "omega":0.0, "ax":0.05531, "ay":-9.23843, "alpha":0.0, "fx":[0.94075,0.94075,0.94075,0.94075], "fy":[-157.14308,-157.14308,-157.14308,-157.14308]},
+ {"t":2.16128, "x":7.80149, "y":3.58064, "heading":1.5708, "vx":0.03797, "vy":2.61029, "omega":0.0, "ax":-0.12294, "ay":-9.70283, "alpha":0.0, "fx":[-2.09121,-2.09121,-2.09121,-2.09121], "fy":[-165.04235,-165.04235,-165.04235,-165.04235]},
+ {"t":2.20335, "x":7.80298, "y":3.68188, "heading":1.5708, "vx":0.03279, "vy":2.20204, "omega":0.0, "ax":-0.17444, "ay":-9.72991, "alpha":0.0, "fx":[-2.96726,-2.96726,-2.96726,-2.96726], "fy":[-165.50292,-165.50292,-165.50292,-165.50292]},
+ {"t":2.24543, "x":7.8042, "y":3.76592, "heading":1.5708, "vx":0.02545, "vy":1.79266, "omega":0.0, "ax":-0.25446, "ay":-9.73819, "alpha":0.0, "fx":[-4.3283,-4.3283,-4.3283,-4.3283], "fy":[-165.64377,-165.64377,-165.64377,-165.64377]},
+ {"t":2.2875, "x":7.80505, "y":3.83273, "heading":1.5708, "vx":0.01475, "vy":1.38293, "omega":0.0, "ax":-0.5469, "ay":-9.73135, "alpha":0.0, "fx":[-9.30265,-9.30265,-9.30265,-9.30265], "fy":[-165.5274,-165.5274,-165.5274,-165.5274]},
+ {"t":2.32958, "x":7.80519, "y":3.8823, "heading":1.5708, "vx":-0.00826, "vy":0.97349, "omega":0.0, "ax":-2.15269, "ay":-9.50954, "alpha":0.0, "fx":[-36.61662,-36.61662,-36.61662,-36.61662], "fy":[-161.75456,-161.75456,-161.75456,-161.75456]},
+ {"t":2.37165, "x":7.80293, "y":3.91484, "heading":1.5708, "vx":-0.09884, "vy":0.57338, "omega":0.0, "ax":-4.02989, "ay":-8.88141, "alpha":0.0, "fx":[-68.54735,-68.54735,-68.54735,-68.54735], "fy":[-151.0703,-151.0703,-151.0703,-151.0703]},
+ {"t":2.41372, "x":7.79521, "y":3.9311, "heading":1.5708, "vx":-0.26839, "vy":0.1997, "omega":0.0, "ax":-4.64597, "ay":-8.57627, "alpha":0.0, "fx":[-79.02665,-79.02665,-79.02665,-79.02665], "fy":[-145.87993,-145.87993,-145.87993,-145.87993]},
+ {"t":2.45815, "x":7.7787, "y":3.93151, "heading":1.5708, "vx":-0.47479, "vy":-0.1813, "omega":0.0, "ax":-4.63899, "ay":-8.5783, "alpha":0.0, "fx":[-78.90788,-78.90788,-78.90788,-78.90788], "fy":[-145.91439,-145.91439,-145.91439,-145.91439]},
+ {"t":2.50257, "x":7.75303, "y":3.91499, "heading":1.5708, "vx":-0.68087, "vy":-0.56238, "omega":0.0, "ax":-4.62912, "ay":-8.58115, "alpha":0.0, "fx":[-78.74003,-78.74003,-78.74003,-78.74003], "fy":[-145.96289,-145.96289,-145.96289,-145.96289]},
+ {"t":2.547, "x":7.71821, "y":3.88154, "heading":1.5708, "vx":-0.88652, "vy":-0.9436, "omega":0.0, "ax":-4.61412, "ay":-8.58546, "alpha":0.0, "fx":[-78.48489,-78.48489,-78.48489,-78.48489], "fy":[-146.03619,-146.03619,-146.03619,-146.03619]},
+ {"t":2.59142, "x":7.67428, "y":3.83115, "heading":1.5708, "vx":-1.0915, "vy":-1.325, "omega":0.0, "ax":-4.58858, "ay":-8.59272, "alpha":0.0, "fx":[-78.05037,-78.05037,-78.05037,-78.05037], "fy":[-146.15978,-146.15978,-146.15978,-146.15978]},
+ {"t":2.63585, "x":7.62126, "y":3.76381, "heading":1.5708, "vx":-1.29534, "vy":-1.70673, "omega":0.0, "ax":-4.5353, "ay":-8.6076, "alpha":0.0, "fx":[-77.14417,-77.14417,-77.14417,-77.14417], "fy":[-146.41281,-146.41281,-146.41281,-146.41281]},
+ {"t":2.68027, "x":7.55924, "y":3.6795, "heading":1.5708, "vx":-1.49682, "vy":-2.08912, "omega":0.0, "ax":-4.35514, "ay":-8.65532, "alpha":0.0, "fx":[-74.07975,-74.07975,-74.07975,-74.07975], "fy":[-147.22451,-147.22451,-147.22451,-147.22451]},
+ {"t":2.7247, "x":7.48845, "y":3.57815, "heading":1.5708, "vx":-1.6903, "vy":-2.47362, "omega":0.0, "ax":4.97585, "ay":-3.17804, "alpha":0.0, "fx":[84.63787,84.63787,84.63787,84.63787], "fy":[-54.05759,-54.05759,-54.05759,-54.05759]},
+ {"t":2.76912, "x":7.41827, "y":3.46512, "heading":1.5708, "vx":-1.46925, "vy":-2.61481, "omega":0.0, "ax":0.60481, "ay":-0.33579, "alpha":0.0, "fx":[10.28766,10.28766,10.28766,10.28766], "fy":[-5.71166,-5.71166,-5.71166,-5.71166]},
+ {"t":2.81354, "x":7.35359, "y":3.34863, "heading":1.5708, "vx":-1.44238, "vy":-2.62972, "omega":0.0, "ax":0.04559, "ay":-0.02498, "alpha":0.0, "fx":[0.77543,0.77543,0.77543,0.77543], "fy":[-0.42493,-0.42493,-0.42493,-0.42493]},
+ {"t":2.85797, "x":7.28956, "y":3.23178, "heading":1.5708, "vx":-1.44035, "vy":-2.63083, "omega":0.0, "ax":0.00363, "ay":-0.00199, "alpha":0.0, "fx":[0.06175,0.06175,0.06175,0.06175], "fy":[-0.03381,-0.03381,-0.03381,-0.03381]},
+ {"t":2.90239, "x":7.22558, "y":3.1149, "heading":1.5708, "vx":-1.44019, "vy":-2.63092, "omega":0.0, "ax":0.00056, "ay":-0.00031, "alpha":0.0, "fx":[0.00952,0.00952,0.00952,0.00952], "fy":[-0.00522,-0.00522,-0.00522,-0.00522]},
+ {"t":2.94682, "x":7.1616, "y":2.99803, "heading":1.5708, "vx":-1.44017, "vy":-2.63094, "omega":0.0, "ax":0.00039, "ay":-0.00021, "alpha":0.0, "fx":[0.00661,0.00661,0.00661,0.00661], "fy":[-0.00363,-0.00363,-0.00363,-0.00363]},
+ {"t":2.99124, "x":7.09762, "y":2.88115, "heading":1.5708, "vx":-1.44015, "vy":-2.63095, "omega":0.0, "ax":0.00044, "ay":-0.00024, "alpha":0.0, "fx":[0.00744,0.00744,0.00744,0.00744], "fy":[-0.00408,-0.00408,-0.00408,-0.00408]},
+ {"t":3.03567, "x":7.03364, "y":2.76427, "heading":1.5708, "vx":-1.44013, "vy":-2.63096, "omega":0.0, "ax":0.00051, "ay":-0.00028, "alpha":0.0, "fx":[0.00863,0.00863,0.00863,0.00863], "fy":[-0.00473,-0.00473,-0.00473,-0.00473]},
+ {"t":3.08009, "x":6.96967, "y":2.64739, "heading":1.5708, "vx":-1.44011, "vy":-2.63097, "omega":0.0, "ax":0.00058, "ay":-0.00032, "alpha":0.0, "fx":[0.00992,0.00992,0.00992,0.00992], "fy":[-0.00544,-0.00544,-0.00544,-0.00544]},
+ {"t":3.12452, "x":6.90569, "y":2.53051, "heading":1.5708, "vx":-1.44008, "vy":-2.63098, "omega":0.0, "ax":0.00067, "ay":-0.00037, "alpha":0.0, "fx":[0.01133,0.01133,0.01133,0.01133], "fy":[-0.00621,-0.00621,-0.00621,-0.00621]},
+ {"t":3.16894, "x":6.84172, "y":2.41363, "heading":1.5708, "vx":-1.44005, "vy":-2.631, "omega":0.0, "ax":0.00076, "ay":-0.00042, "alpha":0.0, "fx":[0.01288,0.01288,0.01288,0.01288], "fy":[-0.00706,-0.00706,-0.00706,-0.00706]},
+ {"t":3.21337, "x":6.77774, "y":2.29675, "heading":1.5708, "vx":-1.44002, "vy":-2.63102, "omega":0.0, "ax":0.00086, "ay":-0.00047, "alpha":0.0, "fx":[0.01458,0.01458,0.01458,0.01458], "fy":[-0.00799,-0.00799,-0.00799,-0.00799]},
+ {"t":3.25779, "x":6.71377, "y":2.17987, "heading":1.5708, "vx":-1.43998, "vy":-2.63104, "omega":0.0, "ax":0.00097, "ay":-0.00053, "alpha":0.0, "fx":[0.01647,0.01647,0.01647,0.01647], "fy":[-0.00903,-0.00903,-0.00903,-0.00903]},
+ {"t":3.30221, "x":6.6498, "y":2.06298, "heading":1.5708, "vx":-1.43994, "vy":-2.63106, "omega":0.0, "ax":0.00109, "ay":-0.0006, "alpha":0.0, "fx":[0.0186,0.0186,0.0186,0.0186], "fy":[-0.01019,-0.01019,-0.01019,-0.01019]},
+ {"t":3.34664, "x":6.58584, "y":1.9461, "heading":1.5708, "vx":-1.43989, "vy":-2.63109, "omega":0.0, "ax":0.00123, "ay":-0.00068, "alpha":0.0, "fx":[0.021,0.021,0.021,0.021], "fy":[-0.01151,-0.01151,-0.01151,-0.01151]},
+ {"t":3.39106, "x":6.52187, "y":1.82922, "heading":1.5708, "vx":-1.43983, "vy":-2.63112, "omega":0.0, "ax":0.0014, "ay":-0.00076, "alpha":0.0, "fx":[0.02374,0.02374,0.02374,0.02374], "fy":[-0.01301,-0.01301,-0.01301,-0.01301]},
+ {"t":3.43549, "x":6.45791, "y":1.71233, "heading":1.5708, "vx":-1.43977, "vy":-2.63115, "omega":0.0, "ax":0.00158, "ay":-0.00087, "alpha":0.0, "fx":[0.02689,0.02689,0.02689,0.02689], "fy":[-0.01473,-0.01473,-0.01473,-0.01473]},
+ {"t":3.47991, "x":6.39395, "y":1.59544, "heading":1.5708, "vx":-1.4397, "vy":-2.63119, "omega":0.0, "ax":0.00178, "ay":-0.00098, "alpha":0.0, "fx":[0.03029,0.03029,0.03029,0.03029], "fy":[-0.01659,-0.01659,-0.01659,-0.01659]},
+ {"t":3.52434, "x":6.32999, "y":1.47855, "heading":1.5708, "vx":-1.43962, "vy":-2.63123, "omega":0.0, "ax":0.00178, "ay":-0.00097, "alpha":0.0, "fx":[0.03026,0.03026,0.03026,0.03026], "fy":[-0.01658,-0.01658,-0.01658,-0.01658]},
+ {"t":3.56876, "x":6.26604, "y":1.36166, "heading":1.5708, "vx":-1.43954, "vy":-2.63128, "omega":0.0, "ax":-0.00143, "ay":0.00078, "alpha":0.0, "fx":[-0.02433,-0.02433,-0.02433,-0.02433], "fy":[0.01328,0.01328,0.01328,0.01328]},
+ {"t":3.61319, "x":6.20209, "y":1.24476, "heading":1.5708, "vx":-1.43961, "vy":-2.63124, "omega":0.0, "ax":-0.04841, "ay":0.02651, "alpha":0.0, "fx":[-0.82352,-0.82352,-0.82352,-0.82352], "fy":[0.45097,0.45097,0.45097,0.45097]},
+ {"t":3.65761, "x":6.13809, "y":1.1279, "heading":1.5708, "vx":-1.44176, "vy":-2.63007, "omega":0.0, "ax":-0.67803, "ay":0.37678, "alpha":0.0, "fx":[-11.53308,-11.53308,-11.53308,-11.53308], "fy":[6.40885,6.40885,6.40885,6.40885]},
+ {"t":3.70203, "x":6.07337, "y":1.01143, "heading":1.5708, "vx":-1.47188, "vy":-2.61333, "omega":0.0, "ax":-5.27758, "ay":3.3021, "alpha":0.0, "fx":[-89.77013,-89.77013,-89.77013,-89.77013], "fy":[56.16769,56.16769,56.16769,56.16769]},
+ {"t":3.74646, "x":6.00277, "y":0.89859, "heading":1.5708, "vx":-1.70633, "vy":-2.46663, "omega":0.0, "ax":-7.23475, "ay":5.77767, "alpha":0.0, "fx":[-123.06101,-123.06101,-123.06101,-123.06101], "fy":[98.27657,98.27657,98.27657,98.27657]},
+ {"t":3.79088, "x":5.91983, "y":0.79472, "heading":1.5708, "vx":-2.02773, "vy":-2.20996, "omega":0.0, "ax":-6.58885, "ay":6.97295, "alpha":0.0, "fx":[-112.07453,-112.07453,-112.07453,-112.07453], "fy":[118.60791,118.60791,118.60791,118.60791]},
+ {"t":3.83531, "x":5.82325, "y":0.70342, "heading":1.5708, "vx":-2.32044, "vy":-1.90019, "omega":0.0, "ax":-5.55856, "ay":7.92471, "alpha":0.0, "fx":[-94.5495,-94.5495,-94.5495,-94.5495], "fy":[134.79711,134.79711,134.79711,134.79711]},
+ {"t":3.87973, "x":5.71468, "y":0.62683, "heading":1.5708, "vx":-2.56737, "vy":-1.54814, "omega":0.0, "ax":-4.52692, "ay":8.54097, "alpha":0.0, "fx":[-77.00153,-77.00153,-77.00153,-77.00153], "fy":[145.27941,145.27941,145.27941,145.27941]},
+ {"t":3.9157, "x":5.6194, "y":0.57666, "heading":1.5708, "vx":-2.73022, "vy":-1.24091, "omega":0.0, "ax":-3.46436, "ay":8.9513, "alpha":0.0, "fx":[-58.92774,-58.92774,-58.92774,-58.92774], "fy":[152.25908,152.25908,152.25908,152.25908]},
+ {"t":3.95168, "x":5.51895, "y":0.53782, "heading":1.5708, "vx":-2.85483, "vy":-0.91892, "omega":0.0, "ax":-2.37724, "ay":9.11896, "alpha":0.0, "fx":[-40.43619,-40.43619,-40.43619,-40.43619], "fy":[155.11082,155.11082,155.11082,155.11082]},
+ {"t":3.98765, "x":5.41472, "y":0.51066, "heading":1.5708, "vx":-2.94035, "vy":-0.59089, "omega":0.0, "ax":-1.27436, "ay":8.68125, "alpha":0.0, "fx":[-21.67651,-21.67651,-21.67651,-21.67651], "fy":[147.66565,147.66565,147.66565,147.66565]},
+ {"t":4.02362, "x":5.30812, "y":0.49502, "heading":1.5708, "vx":-2.98619, "vy":-0.27861, "omega":0.0, "ax":-0.32057, "ay":5.15843, "alpha":0.0, "fx":[-5.45287,-5.45287,-5.45287,-5.45287], "fy":[87.74335,87.74335,87.74335,87.74335]},
+ {"t":4.05959, "x":5.2005, "y":0.48834, "heading":1.5708, "vx":-2.99772, "vy":-0.09306, "omega":0.0, "ax":-0.02209, "ay":0.85128, "alpha":0.0, "fx":[-0.3757,-0.3757,-0.3757,-0.3757], "fy":[14.48003,14.48003,14.48003,14.48003]},
+ {"t":4.09556, "x":5.09265, "y":0.48554, "heading":1.5708, "vx":-2.99851, "vy":-0.06244, "omega":0.0, "ax":-0.00135, "ay":0.06597, "alpha":0.0, "fx":[-0.02294,-0.02294,-0.02294,-0.02294], "fy":[1.12219,1.12219,1.12219,1.12219]},
+ {"t":4.13153, "x":4.98479, "y":0.48334, "heading":1.5708, "vx":-2.99856, "vy":-0.06006, "omega":0.0, "ax":0.00067, "ay":-0.03315, "alpha":0.0, "fx":[0.0114,0.0114,0.0114,0.0114], "fy":[-0.56391,-0.56391,-0.56391,-0.56391]},
+ {"t":4.16751, "x":4.87692, "y":0.48116, "heading":1.5708, "vx":-2.99854, "vy":-0.06125, "omega":0.0, "ax":0.00095, "ay":-0.04608, "alpha":0.0, "fx":[0.01622,0.01622,0.01622,0.01622], "fy":[-0.78382,-0.78382,-0.78382,-0.78382]},
+ {"t":4.20348, "x":4.76906, "y":0.47892, "heading":1.5708, "vx":-2.9985, "vy":-0.06291, "omega":0.0, "ax":0.00103, "ay":-0.04832, "alpha":0.0, "fx":[0.01747,0.01747,0.01747,0.01747], "fy":[-0.82186,-0.82186,-0.82186,-0.82186]},
+ {"t":4.23945, "x":4.6612, "y":0.47663, "heading":1.5708, "vx":-2.99847, "vy":-0.06465, "omega":0.0, "ax":0.00108, "ay":-0.04926, "alpha":0.0, "fx":[0.0183,0.0183,0.0183,0.0183], "fy":[-0.83791,-0.83791,-0.83791,-0.83791]},
+ {"t":4.27542, "x":4.55334, "y":0.47427, "heading":1.5708, "vx":-2.99843, "vy":-0.06642, "omega":0.0, "ax":0.00112, "ay":-0.05007, "alpha":0.0, "fx":[0.01911,0.01911,0.01911,0.01911], "fy":[-0.85167,-0.85167,-0.85167,-0.85167]},
+ {"t":4.31139, "x":4.44549, "y":0.47185, "heading":1.5708, "vx":-2.99839, "vy":-0.06822, "omega":0.0, "ax":0.00117, "ay":-0.05077, "alpha":0.0, "fx":[0.0199,0.0199,0.0199,0.0199], "fy":[-0.86357,-0.86357,-0.86356,-0.86356]},
+ {"t":4.34736, "x":4.33763, "y":0.46936, "heading":1.5708, "vx":-2.99835, "vy":-0.07005, "omega":0.0, "ax":0.00119, "ay":-0.05038, "alpha":0.0, "fx":[0.02027,0.02027,0.02027,0.02027], "fy":[-0.85701,-0.85701,-0.85701,-0.85701]},
+ {"t":4.38334, "x":4.22977, "y":0.46681, "heading":1.5708, "vx":-2.9983, "vy":-0.07186, "omega":0.0, "ax":0.00099, "ay":-0.0409, "alpha":0.0, "fx":[0.01683,0.01683,0.01683,0.01683], "fy":[-0.69564,-0.69564,-0.69563,-0.69563]},
+ {"t":4.41931, "x":4.12192, "y":0.4642, "heading":1.5708, "vx":-2.99827, "vy":-0.07333, "omega":0.0, "ax":-0.00102, "ay":0.04217, "alpha":0.0, "fx":[-0.01738,-0.01738,-0.01738,-0.01738], "fy":[0.71738,0.71738,0.71738,0.71738]},
+ {"t":4.45528, "x":4.01407, "y":0.46159, "heading":1.5708, "vx":-2.9983, "vy":-0.07182, "omega":0.0, "ax":-0.01402, "ay":0.71333, "alpha":0.0, "fx":[-0.23855,-0.23854,-0.23854,-0.23855], "fy":[12.13346,12.13346,12.13346,12.13346]},
+ {"t":4.49125, "x":3.9062, "y":0.45947, "heading":1.5708, "vx":-2.99881, "vy":-0.04616, "omega":0.0, "ax":0.06053, "ay":4.70506, "alpha":0.0, "fx":[1.02952,1.02953,1.02953,1.02952], "fy":[80.0317,80.0317,80.0317,80.0317]},
+ {"t":4.52722, "x":3.79837, "y":0.46085, "heading":1.5708, "vx":-2.99663, "vy":0.12309, "omega":0.0, "ax":0.80597, "ay":8.62951, "alpha":0.0, "fx":[13.70933,13.70934,13.70934,13.70933], "fy":[146.78542,146.78542,146.78542,146.78542]},
+ {"t":4.56319, "x":3.6911, "y":0.47086, "heading":1.5708, "vx":-2.96764, "vy":0.43351, "omega":0.0, "ax":1.88244, "ay":9.21035, "alpha":0.0, "fx":[32.01969,32.01969,32.01969,32.01969], "fy":[156.6655,156.6655,156.6655,156.6655]},
+ {"t":4.59917, "x":3.58557, "y":0.49241, "heading":1.5708, "vx":-2.89993, "vy":0.76482, "omega":0.0, "ax":2.97736, "ay":9.1167, "alpha":0.0, "fx":[50.64408,50.64408,50.64408,50.64408], "fy":[155.07246,155.07246,155.07246,155.07246]},
+ {"t":4.63514, "x":3.48318, "y":0.52582, "heading":1.5708, "vx":-2.79282, "vy":1.09276, "omega":0.0, "ax":4.0403, "ay":8.77786, "alpha":0.0, "fx":[68.72441,68.72441,68.72441,68.72441], "fy":[149.30887,149.30887,149.30887,149.30887]},
+ {"t":4.67111, "x":3.38533, "y":0.57081, "heading":1.5708, "vx":-2.64749, "vy":1.40852, "omega":0.0, "ax":5.07137, "ay":8.26608, "alpha":0.0, "fx":[86.26256,86.26256,86.26256,86.26256], "fy":[140.60364,140.60364,140.60364,140.60364]},
+ {"t":4.70708, "x":3.29338, "y":0.62683, "heading":1.5708, "vx":-2.46506, "vy":1.70586, "omega":0.0, "ax":6.06839, "ay":7.57754, "alpha":0.0, "fx":[103.22162,103.22162,103.22162,103.22162], "fy":[128.89179,128.89179,128.89179,128.89179]},
+ {"t":4.75217, "x":3.18839, "y":0.71145, "heading":1.5708, "vx":-2.19143, "vy":2.04755, "omega":0.0, "ax":7.0923, "ay":6.56593, "alpha":0.0, "fx":[120.63791,120.63791,120.63791,120.63791], "fy":[111.68467,111.68467,111.68467,111.68467]},
+ {"t":4.79726, "x":3.09679, "y":0.81045, "heading":1.5708, "vx":-1.87163, "vy":2.34361, "omega":0.0, "ax":7.86264, "ay":5.40508, "alpha":0.0, "fx":[133.74123,133.74123,133.74123,133.74123], "fy":[91.93886,91.93886,91.93886,91.93886]},
+ {"t":4.84235, "x":3.02039, "y":0.92162, "heading":1.5708, "vx":-1.51709, "vy":2.58734, "omega":0.0, "ax":7.97984, "ay":3.98627, "alpha":0.0, "fx":[135.73473,135.73473,135.73473,135.73473], "fy":[67.80529,67.80529,67.80529,67.80529]},
+ {"t":4.88745, "x":2.96009, "y":1.04234, "heading":1.5708, "vx":-1.15726, "vy":2.76709, "omega":0.0, "ax":3.45774, "ay":1.33429, "alpha":0.0, "fx":[58.81516,58.81517,58.81517,58.81516], "fy":[22.6959,22.6959,22.6959,22.6959]},
+ {"t":4.93254, "x":2.91142, "y":1.16847, "heading":1.5708, "vx":-1.00135, "vy":2.82725, "omega":0.0, "ax":0.28321, "ay":0.09959, "alpha":0.0, "fx":[4.8174,4.81741,4.81741,4.8174], "fy":[1.69401,1.69401,1.69401,1.69401]},
+ {"t":4.97763, "x":2.86656, "y":1.29606, "heading":1.5708, "vx":-0.98858, "vy":2.83174, "omega":0.0, "ax":0.01586, "ay":0.00553, "alpha":0.0, "fx":[0.26981,0.26981,0.26981,0.26981], "fy":[0.09413,0.09413,0.09414,0.09414]},
+ {"t":5.02272, "x":2.822, "y":1.42375, "heading":1.5708, "vx":-0.98786, "vy":2.83199, "omega":0.0, "ax":-0.00284, "ay":-0.00099, "alpha":0.0, "fx":[-0.04828,-0.04828,-0.04828,-0.04828], "fy":[-0.01686,-0.01686,-0.01686,-0.01686]},
+ {"t":5.06781, "x":2.77745, "y":1.55145, "heading":1.5708, "vx":-0.98799, "vy":2.83195, "omega":0.0, "ax":-0.00403, "ay":-0.00141, "alpha":0.0, "fx":[-0.06853,-0.06852,-0.06852,-0.06853], "fy":[-0.02392,-0.02392,-0.02392,-0.02392]},
+ {"t":5.1129, "x":2.7329, "y":1.67915, "heading":1.5708, "vx":-0.98817, "vy":2.83188, "omega":0.0, "ax":-0.00401, "ay":-0.0014, "alpha":0.0, "fx":[-0.06829,-0.06829,-0.06829,-0.06829], "fy":[-0.02384,-0.02384,-0.02384,-0.02384]},
+ {"t":5.158, "x":2.68833, "y":1.80684, "heading":1.5708, "vx":-0.98835, "vy":2.83182, "omega":0.0, "ax":-0.00394, "ay":-0.00138, "alpha":0.0, "fx":[-0.067,-0.067,-0.067,-0.067], "fy":[-0.0234,-0.0234,-0.0234,-0.0234]},
+ {"t":5.20309, "x":2.64376, "y":1.93453, "heading":1.5708, "vx":-0.98853, "vy":2.83176, "omega":0.0, "ax":-0.00388, "ay":-0.00135, "alpha":0.0, "fx":[-0.06594,-0.06594,-0.06594,-0.06594], "fy":[-0.02303,-0.02303,-0.02303,-0.02303]},
+ {"t":5.24818, "x":2.59918, "y":2.06222, "heading":1.5708, "vx":-0.98871, "vy":2.8317, "omega":0.0, "ax":-0.00383, "ay":-0.00134, "alpha":0.0, "fx":[-0.06516,-0.06516,-0.06516,-0.06516], "fy":[-0.02276,-0.02276,-0.02276,-0.02276]},
+ {"t":5.29327, "x":2.5546, "y":2.1899, "heading":1.5708, "vx":-0.98888, "vy":2.83164, "omega":0.0, "ax":-0.0038, "ay":-0.00133, "alpha":0.0, "fx":[-0.06461,-0.06461,-0.06461,-0.06461], "fy":[-0.02257,-0.02257,-0.02257,-0.02257]},
+ {"t":5.33836, "x":2.51, "y":2.31758, "heading":1.5708, "vx":-0.98905, "vy":2.83158, "omega":0.0, "ax":-0.00378, "ay":-0.00132, "alpha":0.0, "fx":[-0.06427,-0.06427,-0.06427,-0.06427], "fy":[-0.02246,-0.02246,-0.02246,-0.02246]},
+ {"t":5.38345, "x":2.4654, "y":2.44526, "heading":1.5708, "vx":-0.98922, "vy":2.83152, "omega":0.0, "ax":-0.00377, "ay":-0.00132, "alpha":0.0, "fx":[-0.0641,-0.0641,-0.0641,-0.0641], "fy":[-0.0224,-0.0224,-0.0224,-0.0224]},
+ {"t":5.42855, "x":2.42079, "y":2.57294, "heading":1.5708, "vx":-0.98939, "vy":2.83146, "omega":0.0, "ax":-0.00377, "ay":-0.00132, "alpha":0.0, "fx":[-0.06409,-0.06409,-0.06409,-0.06409], "fy":[-0.0224,-0.0224,-0.0224,-0.0224]},
+ {"t":5.47364, "x":2.37618, "y":2.70061, "heading":1.5708, "vx":-0.98956, "vy":2.8314, "omega":0.0, "ax":-0.00378, "ay":-0.00132, "alpha":0.0, "fx":[-0.06422,-0.06422,-0.06422,-0.06422], "fy":[-0.02245,-0.02245,-0.02245,-0.02245]},
+ {"t":5.51873, "x":2.33155, "y":2.82828, "heading":1.5708, "vx":-0.98973, "vy":2.83134, "omega":0.0, "ax":-0.00379, "ay":-0.00133, "alpha":0.0, "fx":[-0.06448,-0.06449,-0.06449,-0.06448], "fy":[-0.02254,-0.02254,-0.02255,-0.02255]},
+ {"t":5.56382, "x":2.28692, "y":2.95595, "heading":1.5708, "vx":-0.9899, "vy":2.83128, "omega":0.0, "ax":-0.00381, "ay":-0.00133, "alpha":0.0, "fx":[-0.06487,-0.06487,-0.06487,-0.06487], "fy":[-0.02268,-0.02268,-0.02268,-0.02268]},
+ {"t":5.60891, "x":2.24228, "y":3.08361, "heading":1.5708, "vx":-0.99007, "vy":2.83122, "omega":0.0, "ax":-0.00384, "ay":-0.00134, "alpha":0.0, "fx":[-0.06537,-0.06537,-0.06537,-0.06537], "fy":[-0.02286,-0.02286,-0.02286,-0.02286]},
+ {"t":5.654, "x":2.19763, "y":3.21128, "heading":1.5708, "vx":-0.99025, "vy":2.83116, "omega":0.0, "ax":-0.00388, "ay":-0.00136, "alpha":0.0, "fx":[-0.06606,-0.06607,-0.06607,-0.06606], "fy":[-0.0231,-0.0231,-0.02311,-0.02311]},
+ {"t":5.6991, "x":2.15298, "y":3.33894, "heading":1.5708, "vx":-0.99042, "vy":2.8311, "omega":0.0, "ax":-0.004, "ay":-0.0014, "alpha":0.0, "fx":[-0.06804,-0.06804,-0.06804,-0.06804], "fy":[-0.0238,-0.0238,-0.02381,-0.02381]},
+ {"t":5.74419, "x":2.10831, "y":3.46659, "heading":1.5708, "vx":-0.9906, "vy":2.83103, "omega":0.0, "ax":-0.00494, "ay":-0.00223, "alpha":0.0, "fx":[-0.08409,-0.08409,-0.08409,-0.08409], "fy":[-0.03798,-0.03798,-0.038,-0.038]},
+ {"t":5.78928, "x":2.06364, "y":3.59425, "heading":1.5708, "vx":-0.99083, "vy":2.83093, "omega":0.0, "ax":2.66279, "ay":-7.62828, "alpha":0.0, "fx":[45.29323,45.29323,45.29323,45.29323], "fy":[-129.75479,-129.75479,-129.75479,-129.75479]},
+ {"t":5.83437, "x":2.02167, "y":3.71414, "heading":1.5708, "vx":-0.87076, "vy":2.48696, "omega":0.0, "ax":3.20661, "ay":-9.16123, "alpha":0.0, "fx":[54.54347,54.54347,54.54347,54.54347], "fy":[-155.82984,-155.82984,-155.82984,-155.82984]},
+ {"t":5.87946, "x":1.98566, "y":3.81697, "heading":1.5708, "vx":-0.72617, "vy":2.07387, "omega":0.0, "ax":3.21622, "ay":-9.18737, "alpha":0.0, "fx":[54.70705,54.70705,54.70705,54.70705], "fy":[-156.27453,-156.27453,-156.27453,-156.27453]},
+ {"t":5.92455, "x":1.95619, "y":3.90115, "heading":1.5708, "vx":-0.58114, "vy":1.65959, "omega":0.0, "ax":3.2197, "ay":-9.19621, "alpha":0.0, "fx":[54.76619,54.76619,54.76619,54.76619], "fy":[-156.42487,-156.42487,-156.42487,-156.42487]},
+ {"t":5.96964, "x":1.93326, "y":3.96663, "heading":1.5708, "vx":-0.43596, "vy":1.24492, "omega":0.0, "ax":3.2216, "ay":-9.20061, "alpha":0.0, "fx":[54.79857,54.79857,54.79857,54.79857], "fy":[-156.49975,-156.49975,-156.49975,-156.49975]},
+ {"t":6.01474, "x":1.91688, "y":4.01341, "heading":1.5708, "vx":-0.29069, "vy":0.83005, "omega":0.0, "ax":3.22287, "ay":-9.20322, "alpha":0.0, "fx":[54.82017,54.82017,54.82017,54.82017], "fy":[-156.54415,-156.54415,-156.54415,-156.54415]},
+ {"t":6.05983, "x":1.90704, "y":4.04148, "heading":1.5708, "vx":-0.14537, "vy":0.41506, "omega":0.0, "ax":3.22383, "ay":-9.20493, "alpha":0.0, "fx":[54.83635,54.83635,54.83635,54.83635], "fy":[-156.57322,-156.57322,-156.57322,-156.57322]},
+ {"t":6.10492, "x":1.90377, "y":4.05084, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+ "splits":[0]
+ },
+ "events":[]
+}
diff --git a/lib/examples/swerve/src/main/deploy/choreo/V1_DoomSpiralAutos.chor b/lib/examples/swerve/src/main/deploy/choreo/V1_DoomSpiralAutos.chor
new file mode 100644
index 00000000..83e3d259
--- /dev/null
+++ b/lib/examples/swerve/src/main/deploy/choreo/V1_DoomSpiralAutos.chor
@@ -0,0 +1,84 @@
+{
+ "name":"V1_DoomSpiralAutos",
+ "version":2,
+ "type":"Swerve",
+ "variables":{
+ "expressions":{},
+ "poses":{}
+ },
+ "config":{
+ "frontLeft":{
+ "x":{
+ "exp":"8.875 in",
+ "val":0.225425
+ },
+ "y":{
+ "exp":"13.125 in",
+ "val":0.333375
+ }
+ },
+ "backLeft":{
+ "x":{
+ "exp":"-8.875 in",
+ "val":-0.225425
+ },
+ "y":{
+ "exp":"13.125 in",
+ "val":0.333375
+ }
+ },
+ "mass":{
+ "exp":"150 lbs",
+ "val":68.0388555
+ },
+ "inertia":{
+ "exp":"7.5 kg m ^ 2",
+ "val":7.5
+ },
+ "gearing":{
+ "exp":"7.03",
+ "val":7.03
+ },
+ "radius":{
+ "exp":"2 in",
+ "val":0.0508
+ },
+ "vmax":{
+ "exp":"5800 RPM",
+ "val":607.3745796940267
+ },
+ "tmax":{
+ "exp":"1.2 N * m",
+ "val":1.2
+ },
+ "cof":{
+ "exp":"1.5",
+ "val":1.5
+ },
+ "bumper":{
+ "front":{
+ "exp":"13.75 in",
+ "val":0.34925
+ },
+ "side":{
+ "exp":"17.5 in",
+ "val":0.4445
+ },
+ "back":{
+ "exp":"13.75 in",
+ "val":0.34925
+ }
+ },
+ "differentialTrackWidth":{
+ "exp":"22 in",
+ "val":0.5588
+ }
+ },
+ "generationFeatures":[],
+ "codegen":{
+ "root":"../../java/frc/robot",
+ "genVars":false,
+ "genTrajData":false,
+ "useChoreoLib":false
+ }
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/Constants.java b/lib/examples/swerve/src/main/java/frc/robot/Constants.java
new file mode 100644
index 00000000..191dd3d4
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/Constants.java
@@ -0,0 +1,29 @@
+package frc.robot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.team190.gompeilib.core.robot.RobotMode;
+
+public final class Constants {
+ public static final boolean TUNING_MODE = true;
+ public static final double LOOP_PERIOD_SECONDS = 0.02;
+
+ public static RobotMode getMode() {
+ switch (RobotConfig.ROBOT) {
+ case V0_FUNKY, V1_DOOMSPIRAL:
+ return RobotBase.isReal() ? RobotMode.REAL : RobotMode.REPLAY;
+
+ case V0_FUNKY_SIM, V1_DOOMSPIRAL_SIM:
+ return RobotMode.SIM;
+
+ default:
+ return RobotMode.REAL;
+ }
+ }
+
+ public static void main(String... args) {
+ if (getMode().equals(RobotMode.SIM)) {
+ System.err.println("Cannot deploy, invalid mode selected: " + RobotConfig.ROBOT);
+ System.exit(1);
+ }
+ }
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/FieldConstants.java b/lib/examples/swerve/src/main/java/frc/robot/FieldConstants.java
new file mode 100644
index 00000000..de1b67a8
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/FieldConstants.java
@@ -0,0 +1,324 @@
+// Copyright (c) 2025-2026 Littleton Robotics
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+
+/**
+ * Contains information for location of field element and other useful reference points.
+ *
+ * NOTE: All constants are defined relative to the field coordinate system, and from the
+ * perspective of the blue alliance station
+ */
+public class FieldConstants {
+
+ // AprilTag related constants
+ public static final int aprilTagCount = AprilTagLayoutType.ANDYMARK.getLayout().getTags().size();
+ public static final double aprilTagWidth = Units.inchesToMeters(6.5);
+
+ // Field dimensions
+ public static final double fieldLength = AprilTagLayoutType.ANDYMARK.getLayout().getFieldLength();
+ public static final double fieldWidth = AprilTagLayoutType.ANDYMARK.getLayout().getFieldWidth();
+
+ /**
+ * Officially defined and relevant vertical lines found on the field (defined by X-axis offset)
+ */
+ public static class LinesVertical {
+ public static final double center = fieldLength / 2.0;
+ public static final double starting =
+ AprilTagLayoutType.ANDYMARK.getLayout().getTagPose(26).get().getX();
+ public static final double allianceZone = starting;
+ public static final double hubCenter =
+ AprilTagLayoutType.ANDYMARK.getLayout().getTagPose(26).get().getX() + Hub.width / 2.0;
+ public static final double neutralZoneNear = center - Units.inchesToMeters(120);
+ public static final double neutralZoneFar = center + Units.inchesToMeters(120);
+ public static final double oppHubCenter =
+ AprilTagLayoutType.ANDYMARK.getLayout().getTagPose(4).get().getX() + Hub.width / 2.0;
+ public static final double oppAllianceZone =
+ AprilTagLayoutType.ANDYMARK.getLayout().getTagPose(10).get().getX();
+ }
+
+ /**
+ * Officially defined and relevant horizontal lines found on the field (defined by Y-axis offset)
+ *
+ *
NOTE: The field element start and end are always left to right from the perspective of the
+ * alliance station
+ */
+ public static class LinesHorizontal {
+
+ public static final double center = fieldWidth / 2.0;
+
+ // Right of hub
+ public static final double rightBumpStart = Hub.nearRightCorner.getY();
+ public static final double rightBumpEnd = rightBumpStart - RightBump.width;
+ public static final double rightTrenchOpenStart = rightBumpEnd - Units.inchesToMeters(12.0);
+ public static final double rightTrenchOpenEnd = 0;
+
+ // Left of hub
+ public static final double leftBumpEnd = Hub.nearLeftCorner.getY();
+ public static final double leftBumpStart = leftBumpEnd + LeftBump.width;
+ public static final double leftTrenchOpenEnd = leftBumpStart + Units.inchesToMeters(12.0);
+ public static final double leftTrenchOpenStart = fieldWidth;
+ }
+
+ /** Hub related constants */
+ public static class Hub {
+
+ // Dimensions
+ public static final double width = Units.inchesToMeters(47.0);
+ public static final double height =
+ Units.inchesToMeters(72.0); // includes the catcher at the top
+ public static final double innerWidth = Units.inchesToMeters(41.7);
+ public static final double innerHeight = Units.inchesToMeters(56.5);
+
+ // Relevant reference points on alliance side
+ public static final Translation3d topCenterPoint =
+ new Translation3d(
+ AprilTagLayoutType.ANDYMARK.getLayout().getTagPose(26).get().getX() + width / 2.0,
+ fieldWidth / 2.0,
+ height);
+ public static final Translation3d innerCenterPoint =
+ new Translation3d(
+ AprilTagLayoutType.ANDYMARK.getLayout().getTagPose(26).get().getX() + width / 2.0,
+ fieldWidth / 2.0,
+ innerHeight);
+
+ public static final Translation2d nearLeftCorner =
+ new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0);
+ public static final Translation2d nearRightCorner =
+ new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0);
+ public static final Translation2d farLeftCorner =
+ new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0);
+ public static final Translation2d farRightCorner =
+ new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0);
+
+ // Relevant reference points on the opposite side
+ public static final Translation3d oppTopCenterPoint =
+ new Translation3d(
+ AprilTagLayoutType.ANDYMARK.getLayout().getTagPose(4).get().getX() + width / 2.0,
+ fieldWidth / 2.0,
+ height);
+ public static final Translation2d oppNearLeftCorner =
+ new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0);
+ public static final Translation2d oppNearRightCorner =
+ new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0);
+ public static final Translation2d oppFarLeftCorner =
+ new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0);
+ public static final Translation2d oppFarRightCorner =
+ new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0);
+
+ // Hub faces
+ public static final Pose2d nearFace =
+ AprilTagLayoutType.ANDYMARK.getLayout().getTagPose(26).get().toPose2d();
+ public static final Pose2d farFace =
+ AprilTagLayoutType.ANDYMARK.getLayout().getTagPose(20).get().toPose2d();
+ public static final Pose2d rightFace =
+ AprilTagLayoutType.ANDYMARK.getLayout().getTagPose(18).get().toPose2d();
+ public static final Pose2d leftFace =
+ AprilTagLayoutType.ANDYMARK.getLayout().getTagPose(21).get().toPose2d();
+ }
+
+ /** Left Bump related constants */
+ public static class LeftBump {
+
+ // Dimensions
+ public static final double width = Units.inchesToMeters(73.0);
+ public static final double height = Units.inchesToMeters(6.513);
+ public static final double depth = Units.inchesToMeters(44.4);
+
+ // Relevant reference points on alliance side
+ public static final Translation2d nearLeftCorner =
+ new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255));
+ public static final Translation2d nearRightCorner = Hub.nearLeftCorner;
+ public static final Translation2d farLeftCorner =
+ new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255));
+ public static final Translation2d farRightCorner = Hub.farLeftCorner;
+
+ // Relevant reference points on opposing side
+ public static final Translation2d oppNearLeftCorner =
+ new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255));
+ public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner;
+ public static final Translation2d oppFarLeftCorner =
+ new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255));
+ public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner;
+ }
+
+ /** Right Bump related constants */
+ public static class RightBump {
+ // Dimensions
+ public static final double width = Units.inchesToMeters(73.0);
+ public static final double height = Units.inchesToMeters(6.513);
+ public static final double depth = Units.inchesToMeters(44.4);
+
+ // Relevant reference points on alliance side
+ public static final Translation2d nearLeftCorner =
+ new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255));
+ public static final Translation2d nearRightCorner = Hub.nearLeftCorner;
+ public static final Translation2d farLeftCorner =
+ new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255));
+ public static final Translation2d farRightCorner = Hub.farLeftCorner;
+
+ // Relevant reference points on opposing side
+ public static final Translation2d oppNearLeftCorner =
+ new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255));
+ public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner;
+ public static final Translation2d oppFarLeftCorner =
+ new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255));
+ public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner;
+ }
+
+ /** Left Trench related constants */
+ public static class LeftTrench {
+ // Dimensions
+ public static final double width = Units.inchesToMeters(65.65);
+ public static final double depth = Units.inchesToMeters(47.0);
+ public static final double height = Units.inchesToMeters(40.25);
+ public static final double openingWidth = Units.inchesToMeters(50.34);
+ public static final double openingHeight = Units.inchesToMeters(22.25);
+
+ // Relevant reference points on alliance side
+ public static final Translation3d openingTopLeft =
+ new Translation3d(LinesVertical.hubCenter, fieldWidth, openingHeight);
+ public static final Translation3d openingTopRight =
+ new Translation3d(LinesVertical.hubCenter, fieldWidth - openingWidth, openingHeight);
+
+ // Relevant reference points on opposing side
+ public static final Translation3d oppOpeningTopLeft =
+ new Translation3d(LinesVertical.oppHubCenter, fieldWidth, openingHeight);
+ public static final Translation3d oppOpeningTopRight =
+ new Translation3d(LinesVertical.oppHubCenter, fieldWidth - openingWidth, openingHeight);
+ }
+
+ public static class RightTrench {
+
+ // Dimensions
+ public static final double width = Units.inchesToMeters(65.65);
+ public static final double depth = Units.inchesToMeters(47.0);
+ public static final double height = Units.inchesToMeters(40.25);
+ public static final double openingWidth = Units.inchesToMeters(50.34);
+ public static final double openingHeight = Units.inchesToMeters(22.25);
+
+ // Relevant reference points on alliance side
+ public static final Translation3d openingTopLeft =
+ new Translation3d(LinesVertical.hubCenter, openingWidth, openingHeight);
+ public static final Translation3d openingTopRight =
+ new Translation3d(LinesVertical.hubCenter, 0, openingHeight);
+
+ // Relevant reference points on opposing side
+ public static final Translation3d oppOpeningTopLeft =
+ new Translation3d(LinesVertical.oppHubCenter, openingWidth, openingHeight);
+ public static final Translation3d oppOpeningTopRight =
+ new Translation3d(LinesVertical.oppHubCenter, 0, openingHeight);
+ }
+
+ /** Tower related constants */
+ public static class Tower {
+ // Dimensions
+ public static final double width = Units.inchesToMeters(49.25);
+ public static final double depth = Units.inchesToMeters(45.0);
+ public static final double height = Units.inchesToMeters(78.25);
+ public static final double innerOpeningWidth = Units.inchesToMeters(32.250);
+ public static final double frontFaceX = Units.inchesToMeters(43.51);
+
+ public static final double uprightHeight = Units.inchesToMeters(72.1);
+
+ // Rung heights from the floor
+ public static final double lowRungHeight = Units.inchesToMeters(27.0);
+ public static final double midRungHeight = Units.inchesToMeters(45.0);
+ public static final double highRungHeight = Units.inchesToMeters(63.0);
+
+ // Relevant reference points on alliance side
+ public static final Translation2d centerPoint =
+ new Translation2d(
+ frontFaceX, AprilTagLayoutType.ANDYMARK.getLayout().getTagPose(31).get().getY());
+ public static final Translation2d leftUpright =
+ new Translation2d(
+ frontFaceX,
+ (AprilTagLayoutType.ANDYMARK.getLayout().getTagPose(31).get().getY())
+ + innerOpeningWidth / 2
+ + Units.inchesToMeters(0.75));
+ public static final Translation2d rightUpright =
+ new Translation2d(
+ frontFaceX,
+ (AprilTagLayoutType.ANDYMARK.getLayout().getTagPose(31).get().getY())
+ - innerOpeningWidth / 2
+ - Units.inchesToMeters(0.75));
+
+ // Relevant reference points on opposing side
+ public static final Translation2d oppCenterPoint =
+ new Translation2d(
+ fieldLength - frontFaceX,
+ AprilTagLayoutType.ANDYMARK.getLayout().getTagPose(15).get().getY());
+ public static final Translation2d oppLeftUpright =
+ new Translation2d(
+ fieldLength - frontFaceX,
+ (AprilTagLayoutType.ANDYMARK.getLayout().getTagPose(15).get().getY())
+ + innerOpeningWidth / 2
+ + Units.inchesToMeters(0.75));
+ public static final Translation2d oppRightUpright =
+ new Translation2d(
+ fieldLength - frontFaceX,
+ (AprilTagLayoutType.ANDYMARK.getLayout().getTagPose(15).get().getY())
+ - innerOpeningWidth / 2
+ - Units.inchesToMeters(0.75));
+ }
+
+ public static class Depot {
+ // Dimensions
+ public static final double width = Units.inchesToMeters(42.0);
+ public static final double depth = Units.inchesToMeters(27.0);
+ public static final double height = Units.inchesToMeters(1.125);
+ public static final double distanceFromCenterY = Units.inchesToMeters(75.93);
+
+ // Relevant reference points on alliance side
+ public static final Translation3d depotCenter =
+ new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY, height);
+ public static final Translation3d leftCorner =
+ new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY + (width / 2), height);
+ public static final Translation3d rightCorner =
+ new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY - (width / 2), height);
+ }
+
+ public static class Outpost {
+ // Dimensions
+ public static final double width = Units.inchesToMeters(31.8);
+ public static final double openingDistanceFromFloor = Units.inchesToMeters(28.1);
+ public static final double height = Units.inchesToMeters(7.0);
+
+ // Relevant reference points on alliance side
+ public static final Translation2d centerPoint =
+ new Translation2d(0, AprilTagLayoutType.ANDYMARK.getLayout().getTagPose(29).get().getY());
+ }
+
+ public enum AprilTagLayoutType {
+ ANDYMARK(AprilTagFields.k2026RebuiltAndymark),
+ WELDED(AprilTagFields.k2026RebuiltWelded);
+
+ private final AprilTagFields name;
+ private volatile AprilTagFieldLayout layout;
+
+ AprilTagLayoutType(AprilTagFields name) {
+ this.name = name;
+ }
+
+ public AprilTagFieldLayout getLayout() {
+ if (layout != null) {
+ return layout;
+ }
+ layout = AprilTagFieldLayout.loadField(name);
+ return layout;
+ }
+ }
+
+ public static final Translation2d FEED_TRANSLATION = new Translation2d(0, 0);
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/Main.java b/lib/examples/swerve/src/main/java/frc/robot/Main.java
new file mode 100644
index 00000000..8e389ea6
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/Main.java
@@ -0,0 +1,11 @@
+package frc.robot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+public final class Main {
+ private Main() {}
+
+ public static void main(String... args) {
+ RobotBase.startRobot(Robot::new);
+ }
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/Robot.java b/lib/examples/swerve/src/main/java/frc/robot/Robot.java
new file mode 100644
index 00000000..1ae16042
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/Robot.java
@@ -0,0 +1,285 @@
+package frc.robot;
+
+import com.ctre.phoenix6.SignalLogger;
+import edu.wpi.first.math.MathShared;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.net.WebServer;
+import edu.wpi.first.wpilibj.*;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.team190.gompeilib.core.GompeiLib;
+import edu.wpi.team190.gompeilib.core.robot.RobotContainer;
+import edu.wpi.team190.gompeilib.core.utility.VirtualSubsystem;
+import edu.wpi.team190.gompeilib.core.utility.phoenix.PhoenixUtil;
+import frc.robot.subsystems.v0_Funky.V0_FunkyRobotContainer;
+import frc.robot.util.*;
+import frc.robot.util.Alert;
+import frc.robot.util.Alert.AlertType;
+import java.lang.reflect.Field;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.function.BiConsumer;
+import org.littletonrobotics.junction.LogFileUtil;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGReader;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends LoggedRobot {
+ private static final double LOW_BATTERY_VOLTAGE = 10.0;
+ private static final double LOW_BATTERY_DISABLED_TIME = 1.5;
+
+ private static final double LOOP_OVERRUN_WARNING_TIMEOUT = 1;
+ private static double startupTimestamp = Double.NEGATIVE_INFINITY;
+ private final Timer disabledTimer = new Timer();
+ private final Alert logReceiverQueueAlert =
+ new Alert("Logging queue exceeded capacity, data will NOT be logged.", AlertType.WARNING);
+ private final Elastic.Notification lowBatteryAlert =
+ new Elastic.Notification(
+ Elastic.NotificationLevel.WARNING,
+ "Low Battery! Replace battery soon!",
+ "Battery voltage is very low, consider turning off the robot or replacing the battery.");
+ private Command autonomousCommand;
+ private RobotContainer robotContainer;
+
+ public Robot() {
+ super(Constants.LOOP_PERIOD_SECONDS);
+ GompeiLib.init(Constants.getMode(), Constants.TUNING_MODE, Constants.LOOP_PERIOD_SECONDS);
+ }
+
+ public static boolean isJitting() {
+ return Timer.getFPGATimestamp() - startupTimestamp <= 45.0;
+ }
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ SignalLogger.enableAutoLogging(false);
+ LiveWindow.disableAllTelemetry();
+ // Record metadata
+ Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
+ Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
+ Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
+ Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
+ Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
+ Logger.recordMetadata("GompeiLib_Version", BuildConstants.VERSION);
+ switch (BuildConstants.DIRTY) {
+ case 0:
+ Logger.recordMetadata("GitDirty", "All changes committed");
+ break;
+ case 1:
+ Logger.recordMetadata("GitDirty", "Uncomitted changes");
+ break;
+ default:
+ Logger.recordMetadata("GitDirty", "Unknown");
+ break;
+ }
+
+ // Set up data receivers & replay source
+ switch (Constants.getMode()) {
+ case REAL:
+ // Running on a real robot, log to a USB stick ("/U/logs")
+ Logger.addDataReceiver(new WPILOGWriter());
+ Logger.addDataReceiver(new NT4Publisher());
+ break;
+
+ case SIM:
+ // Running a physics simulator, log to NT
+ // setUseTiming(false);
+ Logger.addDataReceiver(new NT4Publisher());
+ // setting up maple sim field
+ break;
+
+ case REPLAY:
+ // Replaying a log, set up replay source
+ setUseTiming(false); // Run as fast as possible
+ String logPath = LogFileUtil.findReplayLog();
+ Logger.setReplaySource(new WPILOGReader(logPath));
+ Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
+ break;
+ }
+
+ // Start AdvantageKit logger
+ Logger.start();
+
+ // Start timers
+ disabledTimer.restart();
+
+ RobotController.setBrownoutVoltage(6);
+ // Instantiate our RobotContainer. This will perform all our button bindings,
+ // and put our autonomous chooser on the dashboard.
+ robotContainer =
+ switch (RobotConfig.ROBOT) {
+ case V0_FUNKY, V0_FUNKY_SIM -> new V0_FunkyRobotContainer();
+ default -> new RobotContainer() {};
+ };
+
+ Elastic.selectTab("Autonomous");
+ DriverStation.silenceJoystickConnectionWarning(true);
+
+ try {
+ Field watchdogField = IterativeRobotBase.class.getDeclaredField("m_watchdog");
+ watchdogField.setAccessible(true);
+ Watchdog watchdog = (Watchdog) watchdogField.get(this);
+ watchdog.setTimeout(LOOP_OVERRUN_WARNING_TIMEOUT);
+ } catch (Exception e) {
+ DriverStation.reportWarning("Failed to disable loop overrun warnings.", false);
+ }
+ CommandScheduler.getInstance().setPeriod(LOOP_OVERRUN_WARNING_TIMEOUT);
+
+ // Silence Rotation2d warnings
+ var mathShared = MathSharedStore.getMathShared();
+ MathSharedStore.setMathShared(
+ new MathShared() {
+ @Override
+ public void reportError(String error, StackTraceElement[] stackTrace) {
+ if (error.startsWith("x and y components of Rotation2d are zero")) {
+ return;
+ }
+ mathShared.reportError(error, stackTrace);
+ }
+
+ @Override
+ public void reportUsage(MathUsageId id, int count) {
+ mathShared.reportUsage(id, count);
+ }
+
+ @Override
+ public double getTimestamp() {
+ return mathShared.getTimestamp();
+ }
+ });
+
+ String DEPLOY_DIR =
+ Filesystem.getDeployDirectory().getPath()
+ + RobotConfig.ROBOT.name().toLowerCase().replaceFirst("_sim", "");
+ // try {
+ // var m = Choreo.class.getDeclaredMethod("setChoreoDir", File.class);
+ // m.setAccessible(true);
+ // m.invoke(null, new File(DEPLOY_DIR + "/choreo"));
+ // } catch (NoSuchMethodException | InvocationTargetException | IllegalAccessException e) {
+ // throw new RuntimeException(e);
+ // }
+ // Log active commands
+ Map commandCounts = new HashMap<>();
+ BiConsumer logCommandFunction =
+ (Command command, Boolean active) -> {
+ String name = command.getName();
+ int count = commandCounts.getOrDefault(name, 0) + (active ? 1 : -1);
+ commandCounts.put(name, count);
+ Logger.recordOutput(
+ "CommandsUnique/" + name + "_" + Integer.toHexString(command.hashCode()), active);
+ Logger.recordOutput("CommandsAll/" + name, count > 0);
+ };
+ CommandScheduler.getInstance()
+ .onCommandInitialize((Command command) -> logCommandFunction.accept(command, true));
+ CommandScheduler.getInstance()
+ .onCommandFinish((Command command) -> logCommandFunction.accept(command, false));
+ CommandScheduler.getInstance()
+ .onCommandInterrupt((Command command) -> logCommandFunction.accept(command, false));
+
+ WebServer.start(5800, DEPLOY_DIR);
+
+ startupTimestamp = Timer.getFPGATimestamp();
+ }
+
+ /** This function is called periodically during all modes. */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding
+ // newly-scheduled commands, running already-scheduled commands, removing
+ // finished or interrupted commands, and running subsystem periodic() methods.
+ // This must be called from the robot's periodic block in order for anything in
+ // the Command-based framework to work.
+
+ PhoenixUtil.refreshAll();
+ VirtualSubsystem.periodicAll();
+ robotContainer.robotPeriodic();
+ CommandScheduler.getInstance().run();
+
+ // Check logging fault
+ logReceiverQueueAlert.set(Logger.getReceiverQueueFault());
+
+ // Update low battery alert
+ if (RobotState.isEnabled()) {
+ disabledTimer.reset();
+ }
+ if (RobotController.getBatteryVoltage() < LOW_BATTERY_VOLTAGE
+ && disabledTimer.hasElapsed(LOW_BATTERY_DISABLED_TIME)) {
+ Elastic.sendNotification(lowBatteryAlert);
+ }
+ }
+
+ /** This function is called once when the robot is disabled. */
+ @Override
+ public void disabledInit() {}
+
+ /** This function is called periodically when disabled. */
+ @Override
+ public void disabledPeriodic() {}
+
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+ @Override
+ public void autonomousInit() {
+ Elastic.selectTab("Autonomous");
+
+ autonomousCommand = robotContainer.getAutonomousCommand();
+
+ if (autonomousCommand != null) {
+ CommandScheduler.getInstance().cancelAll();
+ CommandScheduler.getInstance().schedule(autonomousCommand);
+ }
+ }
+
+ /** This function is called periodically during autonomous. */
+ @Override
+ public void autonomousPeriodic() {}
+
+ /** This function is called once when teleop is enabled. */
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (autonomousCommand != null) {
+ CommandScheduler.getInstance().cancel(autonomousCommand);
+ }
+ Elastic.selectTab("Teleoperated");
+ }
+
+ /** This function is called periodically during operator control. */
+ @Override
+ public void teleopPeriodic() {}
+
+ /** This function is called once when test mode is enabled. */
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /** This function is called periodically during test mode. */
+ @Override
+ public void testPeriodic() {}
+
+ /** This function is called once when the robot is first started up. */
+ @Override
+ public void simulationInit() {}
+
+ /** This function is called periodically whilst in simulation. */
+ @Override
+ public void simulationPeriodic() {}
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/RobotConfig.java b/lib/examples/swerve/src/main/java/frc/robot/RobotConfig.java
new file mode 100644
index 00000000..ebea24c8
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/RobotConfig.java
@@ -0,0 +1,12 @@
+package frc.robot;
+
+public final class RobotConfig {
+ public static final RobotType ROBOT = RobotType.V1_DOOMSPIRAL;
+
+ public enum RobotType {
+ V0_FUNKY,
+ V0_FUNKY_SIM,
+ V1_DOOMSPIRAL,
+ V1_DOOMSPIRAL_SIM,
+ }
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/commands/shared/AutoAlignCommand.java b/lib/examples/swerve/src/main/java/frc/robot/commands/shared/AutoAlignCommand.java
new file mode 100644
index 00000000..6d5c6e92
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/commands/shared/AutoAlignCommand.java
@@ -0,0 +1,168 @@
+package frc.robot.commands.shared;
+
+import static edu.wpi.first.units.Units.*;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive.SwerveDrive;
+import edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive.SwerveDriveConstants.AutoAlignConstants;
+import java.util.function.BooleanSupplier;
+import java.util.function.Supplier;
+import org.littletonrobotics.junction.Logger;
+
+public class AutoAlignCommand extends Command {
+ private final SwerveDrive drive;
+ private final Pose2d targetPose;
+ private final BooleanSupplier valid;
+ private final Supplier robotPose;
+
+ private ChassisSpeeds speeds;
+
+ private final ProfiledPIDController alignXController;
+ private final ProfiledPIDController alignYController;
+ private final ProfiledPIDController alignHeadingController;
+
+ /**
+ * Creates a new AutoAlignCommand.
+ *
+ * @param drive The swerve drive subsystem on which this command will run
+ * @param targetPose The pose to which the robot will attempt to align
+ * @param valid A boolean supplier that returns true when auto aligning is possible (e.g. when
+ * tags are visible)
+ * @param robotPose A supplier that returns the robot's current pose
+ * @param constants The swerve drive constants
+ */
+ public AutoAlignCommand(
+ SwerveDrive drive,
+ Pose2d targetPose,
+ BooleanSupplier valid,
+ Supplier robotPose,
+ AutoAlignConstants constants,
+ double maxAccelerationMetersPerSecond) {
+ this.addRequirements(drive);
+
+ this.drive = drive;
+ this.targetPose = targetPose;
+ this.valid = valid;
+ this.robotPose = robotPose;
+
+ alignXController =
+ new ProfiledPIDController(
+ constants.xGains().kP().get(),
+ 0.0,
+ constants.xGains().kD().get(),
+ new TrapezoidProfile.Constraints(
+ constants.xConstraints().maxVelocity().get().in(MetersPerSecond),
+ maxAccelerationMetersPerSecond));
+ alignYController =
+ new ProfiledPIDController(
+ constants.yGains().kP().get(),
+ 0.0,
+ constants.yGains().kD().get(),
+ new TrapezoidProfile.Constraints(
+ constants.yConstraints().maxVelocity().get().in(MetersPerSecond),
+ maxAccelerationMetersPerSecond));
+ alignHeadingController =
+ new ProfiledPIDController(
+ constants.rotationGains().kP().get(),
+ 0.0,
+ constants.rotationGains().kD().get(),
+ new TrapezoidProfile.Constraints(
+ constants.rotationConstraints().maxVelocity().get().in(RadiansPerSecond),
+ Double.POSITIVE_INFINITY));
+
+ alignXController.setTolerance(constants.xConstraints().goalTolerance().get().in(Meters), 0);
+ alignYController.setTolerance(constants.yConstraints().goalTolerance().get().in(Meters), 0);
+
+ alignHeadingController.enableContinuousInput(-Math.PI, Math.PI);
+ alignHeadingController.setTolerance(
+ constants.rotationConstraints().goalTolerance().get().in(Radians), 0);
+ speeds = new ChassisSpeeds();
+ }
+
+ @Override
+ public void initialize() {
+ alignHeadingController.reset(
+ robotPose.get().getRotation().getRadians(),
+ drive.getMeasuredChassisSpeeds().omegaRadiansPerSecond);
+ alignXController.reset(
+ robotPose.get().getX(), drive.getMeasuredChassisSpeeds().vxMetersPerSecond);
+ alignYController.reset(
+ robotPose.get().getY(), drive.getMeasuredChassisSpeeds().vyMetersPerSecond);
+ }
+
+ @Override
+ public void execute() {
+
+ if (valid.getAsBoolean()) {
+ ChassisSpeeds measuredSpeeds = drive.getMeasuredChassisSpeeds();
+
+ double adjustedXSpeed =
+ calculate(
+ alignXController,
+ targetPose.getX(),
+ robotPose.get().getX(),
+ measuredSpeeds.vxMetersPerSecond);
+ double adjustedYSpeed =
+ calculate(
+ alignYController,
+ targetPose.getY(),
+ robotPose.get().getY(),
+ measuredSpeeds.vyMetersPerSecond);
+ double adjustedThetaSpeed =
+ calculate(
+ alignHeadingController,
+ targetPose.getRotation().getRadians(),
+ robotPose.get().getRotation().getRadians(),
+ measuredSpeeds.omegaRadiansPerSecond);
+ speeds =
+ ChassisSpeeds.fromFieldRelativeSpeeds(
+ adjustedXSpeed, adjustedYSpeed, adjustedThetaSpeed, robotPose.get().getRotation());
+
+ } else {
+ speeds = new ChassisSpeeds();
+ }
+ Logger.recordOutput("Drive/Auto Align/speeds", speeds);
+ drive.runVelocity(speeds);
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ drive.runVelocity(new ChassisSpeeds());
+ alignHeadingController.reset(robotPose.get().getRotation().getRadians());
+ alignXController.reset(robotPose.get().getX());
+ alignYController.reset(robotPose.get().getY());
+ }
+
+ @Override
+ public boolean isFinished() {
+ // Return true when the command should end
+ return alignXController.atGoal()
+ && alignYController.atGoal()
+ && alignHeadingController.atGoal();
+ }
+
+ /**
+ * Calculates the PID output for the given setpoint, measurement, and speed. If the controller is
+ * not at the setpoint, calculate the PID output. Otherwise, reset the controller with the given
+ * measurement and speed.
+ *
+ * @param controller The profiled PID controller to use.
+ * @param setpoint The setpoint of the controller.
+ * @param measurement The current measurement.
+ * @param speed The current speed.
+ * @return The calculated PID output.
+ */
+ public static double calculate(
+ ProfiledPIDController controller, double setpoint, double measurement, double speed) {
+ double pidOutput = 0.0;
+
+ if (!controller.atSetpoint()) pidOutput = controller.calculate(measurement, setpoint);
+ else controller.reset(measurement, speed);
+
+ return pidOutput;
+ }
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/commands/shared/DriveCommands.java b/lib/examples/swerve/src/main/java/frc/robot/commands/shared/DriveCommands.java
new file mode 100644
index 00000000..3fcf265c
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/commands/shared/DriveCommands.java
@@ -0,0 +1,399 @@
+package frc.robot.commands.shared;
+
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.team190.gompeilib.core.logging.Trace;
+import edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive.SwerveDrive;
+import edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive.SwerveDriveConstants;
+import edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive.SwerveDriveConstants.AutoAlignConstants;
+import frc.robot.FieldConstants;
+import frc.robot.subsystems.v0_Funky.V0_FunkyRobotState;
+import frc.robot.util.AllianceFlipUtil;
+import java.text.DecimalFormat;
+import java.text.NumberFormat;
+import java.util.List;
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+import org.littletonrobotics.junction.Logger;
+
+public final class DriveCommands {
+
+ /**
+ * A command that drives a SwerveDrive using joystick input.
+ *
+ * @param drive The SwerveDrive to control.
+ * @param driveConstants The constants for the SwerveDrive.
+ * @param xSupplier The supplier of the x-axis joystick input.
+ * @param ySupplier The supplier of the y-axis joystick input.
+ * @param omegaSupplier The supplier of the omega joystick input.
+ * @param rotationSupplier The supplier of the rotation of the robot.
+ * @param hijackXSuppliers A list of pairs of boolean suppliers and double suppliers for hijacking
+ * the x velocity. If the boolean supplier is true, the x velocity will be set to the value of
+ * the double supplier. If multiple boolean suppliers are true, the first one in the list will
+ * take precedence.
+ * @param hijackYSuppliers A list of pairs of boolean suppliers and double suppliers for hijacking
+ * the y velocity. If the boolean supplier is true, the y velocity will be set to the value of
+ * the double supplier. If multiple boolean suppliers are true, the first one in the list will
+ * take precedence.
+ * @param hijackOmegaSuppliers A list of pairs of boolean suppliers and double suppliers for
+ * hijacking the omega velocity. If the boolean supplier is true, the omega velocity will be
+ * set to the value of the double supplier. If multiple boolean suppliers are true, the first
+ * one in the list will take precedence.
+ * @return A command that drives the SwerveDrive.
+ */
+ @Trace
+ public static Command joystickDrive(
+ SwerveDrive drive,
+ SwerveDriveConstants driveConstants,
+ DoubleSupplier xSupplier,
+ DoubleSupplier ySupplier,
+ DoubleSupplier omegaSupplier,
+ Supplier rotationSupplier,
+ List> hijackXSuppliers,
+ List> hijackYSuppliers,
+ List> hijackOmegaSuppliers,
+ BooleanSupplier slowMode) {
+ return Commands.run(
+ () -> {
+ // Apply deadband
+ double linearMagnitude =
+ MathUtil.applyDeadband(
+ Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()),
+ driveConstants.driverDeadband);
+ Rotation2d linearDirection =
+ new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble());
+
+ double omega =
+ MathUtil.applyDeadband(omegaSupplier.getAsDouble(), driveConstants.driverDeadband);
+ linearMagnitude *= linearMagnitude;
+
+ // Calculate new linear velocities
+
+ double fieldRelativeXVel =
+ linearMagnitude * linearDirection.getCos() * drive.getMaxLinearSpeedMetersPerSec();
+ double fieldRelativeYVel =
+ linearMagnitude * linearDirection.getSin() * drive.getMaxLinearSpeedMetersPerSec();
+
+ double angular = omega * drive.getMaxAngularSpeedRadPerSec();
+
+ fieldRelativeXVel =
+ hijackXSuppliers.stream()
+ .filter(pair -> pair.getFirst().getAsBoolean())
+ .map(pair -> pair.getSecond().getAsDouble())
+ .findFirst()
+ .orElse(slowMode.getAsBoolean() ? (fieldRelativeXVel * 0.1) : fieldRelativeXVel);
+
+ fieldRelativeYVel =
+ hijackYSuppliers.stream()
+ .filter(pair -> pair.getFirst().getAsBoolean())
+ .map(pair -> pair.getSecond().getAsDouble())
+ .findFirst()
+ .orElse(slowMode.getAsBoolean() ? (fieldRelativeYVel * 0.1) : fieldRelativeYVel);
+
+ angular =
+ hijackOmegaSuppliers.stream()
+ .filter(pair -> pair.getFirst().getAsBoolean())
+ .map(pair -> pair.getSecond().getAsDouble())
+ .findFirst()
+ .orElse(slowMode.getAsBoolean() ? (angular * 0.1) : angular);
+
+ ChassisSpeeds chassisSpeeds =
+ ChassisSpeeds.fromFieldRelativeSpeeds(
+ fieldRelativeXVel,
+ fieldRelativeYVel,
+ angular,
+ AllianceFlipUtil.apply(rotationSupplier.get()));
+
+ Logger.recordOutput("Drive/JoystickDrive/chassisSpeeds", chassisSpeeds);
+
+ drive.runVelocity(chassisSpeeds);
+ },
+ drive);
+ }
+
+ public static Command joystickDrive(
+ SwerveDrive drive,
+ SwerveDriveConstants driveConstants,
+ DoubleSupplier xSupplier,
+ DoubleSupplier ySupplier,
+ DoubleSupplier omegaSupplier,
+ Supplier rotationSupplier) {
+ return joystickDrive(
+ drive,
+ driveConstants,
+ xSupplier,
+ ySupplier,
+ omegaSupplier,
+ rotationSupplier,
+ List.of(),
+ List.of(),
+ List.of(),
+ () -> false);
+ }
+
+ public static Command joystickDriveRotationLock(
+ SwerveDrive drive,
+ SwerveDriveConstants driveConstants,
+ DoubleSupplier xSupplier,
+ DoubleSupplier ySupplier,
+ DoubleSupplier omegaSupplier,
+ Supplier rotationSupplier,
+ BooleanSupplier pointAtHub,
+ DoubleSupplier hubSetpoint,
+ BooleanSupplier cardinalDirectionAlign,
+ DoubleSupplier cardinalDirectionSetpoint,
+ BooleanSupplier slowMode) {
+ ProfiledPIDController omegaController =
+ new ProfiledPIDController(
+ driveConstants.autoAlignConstants.rotationGains().kP().get(),
+ 0.0,
+ driveConstants.autoAlignConstants.rotationGains().kD().get(),
+ new TrapezoidProfile.Constraints(
+ driveConstants
+ .autoAlignConstants
+ .rotationConstraints()
+ .maxVelocity()
+ .get()
+ .in(RadiansPerSecond),
+ Double.POSITIVE_INFINITY));
+ omegaController.enableContinuousInput(-Math.PI, Math.PI);
+ omegaController.setTolerance(
+ driveConstants.autoAlignConstants.rotationConstraints().goalTolerance().get().in(Radians),
+ 0);
+ return joystickDrive(
+ drive,
+ driveConstants,
+ xSupplier,
+ ySupplier,
+ omegaSupplier,
+ rotationSupplier,
+ List.of(),
+ List.of(),
+ List.of(
+ Pair.of(
+ pointAtHub,
+ () ->
+ AutoAlignCommand.calculate(
+ omegaController,
+ hubSetpoint.getAsDouble(),
+ rotationSupplier.get().getRadians(),
+ drive.getMeasuredChassisSpeeds().omegaRadiansPerSecond)),
+ Pair.of(
+ cardinalDirectionAlign,
+ () ->
+ AutoAlignCommand.calculate(
+ omegaController,
+ cardinalDirectionSetpoint.getAsDouble(),
+ rotationSupplier.get().getRadians(),
+ drive.getMeasuredChassisSpeeds().omegaRadiansPerSecond)),
+ Pair.of(
+ slowMode,
+ () ->
+ AutoAlignCommand.calculate(
+ omegaController,
+ AllianceFlipUtil.apply(Rotation2d.kCCW_90deg).getRadians(),
+ rotationSupplier.get().getRadians(),
+ drive.getMeasuredChassisSpeeds().omegaRadiansPerSecond))),
+ slowMode);
+ }
+
+ public static Command rotateToAngle(
+ SwerveDrive drive,
+ SwerveDriveConstants driveConstants,
+ Supplier currentRotation,
+ Rotation2d targetRotation) {
+ ProfiledPIDController omegaController =
+ new ProfiledPIDController(
+ driveConstants.autoAlignConstants.rotationGains().kP().get(),
+ 0.0,
+ driveConstants.autoAlignConstants.rotationGains().kD().get(),
+ new TrapezoidProfile.Constraints(
+ driveConstants
+ .autoAlignConstants
+ .rotationConstraints()
+ .maxVelocity()
+ .get()
+ .in(RadiansPerSecond),
+ Double.POSITIVE_INFINITY));
+ omegaController.enableContinuousInput(-Math.PI, Math.PI);
+
+ omegaController.setTolerance(
+ driveConstants.autoAlignConstants.rotationConstraints().goalTolerance().get().in(Radians),
+ 0);
+ return Commands.run(
+ () ->
+ drive.runVelocity(
+ ChassisSpeeds.fromFieldRelativeSpeeds(
+ 0.0,
+ 0.0,
+ AutoAlignCommand.calculate(
+ omegaController,
+ targetRotation.getRadians(),
+ currentRotation.get().getRadians(),
+ drive.getMeasuredChassisSpeeds().omegaRadiansPerSecond),
+ AllianceFlipUtil.apply(currentRotation.get()))));
+ }
+
+ public static Command inchMovement(SwerveDrive drive, double velocity, double time) {
+ return Commands.run(() -> drive.runVelocity(new ChassisSpeeds(0.0, velocity, 0.0)))
+ .withTimeout(time);
+ }
+
+ public static Command stop(SwerveDrive drive) {
+ return Commands.run(drive::stopWithX);
+ }
+
+ public static Command feedforwardCharacterization(SwerveDrive drive) {
+ return new KSCharacterization(
+ drive, drive::runCharacterization, drive::getFFCharacterizationVelocity);
+ }
+
+ public static Command autoAlignPoseCommand(
+ SwerveDrive drive,
+ Supplier robotPoseSupplier,
+ Pose2d targetPose,
+ AutoAlignConstants constants) {
+ return new AutoAlignCommand(
+ drive, targetPose, () -> true, robotPoseSupplier, constants, Double.POSITIVE_INFINITY);
+ }
+
+ public static Command autoAlignTowerCommand(
+ SwerveDrive drive, Supplier robotPoseSupplier, AutoAlignConstants constants) {
+ return autoAlignPoseCommand(
+ drive,
+ robotPoseSupplier,
+ new Pose2d(FieldConstants.Tower.centerPoint, new Rotation2d()),
+ constants);
+ }
+
+ public static Command aimAtHub(SwerveDrive drive, SwerveDriveConstants driveConstants) {
+ ProfiledPIDController omegaController =
+ new ProfiledPIDController(
+ driveConstants.autoAlignConstants.rotationGains().kP().get(),
+ 0.0,
+ driveConstants.autoAlignConstants.rotationGains().kD().get(),
+ new TrapezoidProfile.Constraints(
+ driveConstants
+ .autoAlignConstants
+ .rotationConstraints()
+ .maxVelocity()
+ .get()
+ .in(RadiansPerSecond),
+ Double.POSITIVE_INFINITY));
+ omegaController.enableContinuousInput(-Math.PI, Math.PI);
+ omegaController.setTolerance(
+ driveConstants.autoAlignConstants.rotationConstraints().goalTolerance().get().in(Radians),
+ 0);
+ return Commands.run(
+ () -> {
+ drive.runVelocity(
+ ChassisSpeeds.fromFieldRelativeSpeeds(
+ 0.0,
+ 0.0,
+ AutoAlignCommand.calculate(
+ omegaController,
+ (AllianceFlipUtil.shouldFlip()
+ ? FieldConstants.Hub.oppTopCenterPoint.toTranslation2d()
+ : FieldConstants.Hub.topCenterPoint.toTranslation2d())
+ .minus(V0_FunkyRobotState.getGlobalPose().getTranslation())
+ .getAngle()
+ .minus(Rotation2d.kCW_Pi_2)
+ .getRadians(),
+ V0_FunkyRobotState.getHeading().getRadians(),
+ drive.getMeasuredChassisSpeeds().omegaRadiansPerSecond),
+ V0_FunkyRobotState.getHeading()));
+ });
+ }
+
+ public static Command wheelRadiusCharacterization(
+ SwerveDrive drive, SwerveDriveConstants driveConstants) {
+ double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec
+ double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2
+ SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE);
+ WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState();
+
+ return Commands.parallel(
+ // SwerveDrive control sequence
+ Commands.sequence(
+ // Reset acceleration limiter
+ Commands.runOnce(
+ () -> {
+ limiter.reset(0.0);
+ }),
+
+ // Turn in place, accelerating up to full speed
+ Commands.run(
+ () -> {
+ double speed = limiter.calculate(WHEEL_RADIUS_MAX_VELOCITY);
+ drive.runVelocity(new ChassisSpeeds(0.0, 0.0, speed));
+ },
+ drive)),
+
+ // Measurement sequence
+ Commands.sequence(
+ // Wait for modules to fully orient before starting measurement
+ Commands.waitSeconds(1.0),
+
+ // Record starting measurement
+ Commands.runOnce(
+ () -> {
+ state.positions = drive.getWheelRadiusCharacterizationPositions();
+ state.lastAngle = drive.getRawGyroRotation();
+ state.gyroDelta = 0.0;
+ }),
+
+ // Update gyro delta
+ Commands.run(
+ () -> {
+ var rotation = drive.getRawGyroRotation();
+ state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians());
+ state.lastAngle = rotation;
+ })
+
+ // When cancelled, calculate and print results
+ .finallyDo(
+ () -> {
+ double[] positions = drive.getWheelRadiusCharacterizationPositions();
+ double wheelDelta = 0.0;
+ for (int i = 0; i < 4; i++) {
+ wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0;
+ }
+ double wheelRadius =
+ (state.gyroDelta * driveConstants.driveConfig.driveBaseRadius())
+ / wheelDelta;
+
+ NumberFormat formatter = new DecimalFormat("#0.000");
+ System.out.println(
+ "********** Wheel Radius Characterization Results **********");
+ System.out.println(
+ "\tWheel Delta: " + formatter.format(wheelDelta) + " radians");
+ System.out.println(
+ "\tGyro Delta: " + formatter.format(state.gyroDelta) + " radians");
+ System.out.println(
+ "\tWheel Radius: "
+ + formatter.format(wheelRadius)
+ + " meters, "
+ + formatter.format(Units.metersToInches(wheelRadius))
+ + " inches");
+ })));
+ }
+
+ private static class WheelRadiusCharacterizationState {
+ double[] positions = new double[4];
+ Rotation2d lastAngle = new Rotation2d();
+ double gyroDelta = 0.0;
+ }
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/commands/shared/KSCharacterization.java b/lib/examples/swerve/src/main/java/frc/robot/commands/shared/KSCharacterization.java
new file mode 100644
index 00000000..a92b593a
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/commands/shared/KSCharacterization.java
@@ -0,0 +1,55 @@
+package frc.robot.commands.shared;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import edu.wpi.team190.gompeilib.core.utility.tunable.LoggedTunableNumber;
+import java.util.function.DoubleConsumer;
+import java.util.function.DoubleSupplier;
+import org.littletonrobotics.junction.Logger;
+
+public class KSCharacterization extends Command {
+ private static final LoggedTunableNumber currentRampFactor =
+ new LoggedTunableNumber("StaticCharacterization/CurrentRampPerSec", 1.0);
+ private static final LoggedTunableNumber minVelocity =
+ new LoggedTunableNumber("StaticCharacterization/MinStaticVelocity", 0.1);
+
+ private final DoubleConsumer inputConsumer;
+ private final DoubleSupplier velocitySupplier;
+ private final Timer timer = new Timer();
+ private double currentInput = 0.0;
+
+ public KSCharacterization(
+ Subsystem subsystem,
+ DoubleConsumer characterizationInputConsumer,
+ DoubleSupplier velocitySupplier) {
+ inputConsumer = characterizationInputConsumer;
+ this.velocitySupplier = velocitySupplier;
+ addRequirements(subsystem);
+ }
+
+ @Override
+ public void initialize() {
+ timer.restart();
+ }
+
+ @Override
+ public void execute() {
+ currentInput = timer.get() * currentRampFactor.get();
+ inputConsumer.accept(currentInput);
+ }
+
+ @Override
+ public boolean isFinished() {
+ return velocitySupplier.getAsDouble() >= minVelocity.get();
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ System.out.println("********** FF Characterization Results **********\n\n\n\n\n\n\n\n\n");
+ System.out.println(
+ "Static Characterization output: " + currentInput + " amps\n\n\n\n\n\n\n\n\n\n");
+ Logger.recordOutput("KS output", currentInput);
+ inputConsumer.accept(0);
+ }
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/commands/shared/SharedCompositeCommands.java b/lib/examples/swerve/src/main/java/frc/robot/commands/shared/SharedCompositeCommands.java
new file mode 100644
index 00000000..d89a8280
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/commands/shared/SharedCompositeCommands.java
@@ -0,0 +1,35 @@
+package frc.robot.commands.shared;
+
+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.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive.SwerveDrive;
+import frc.robot.util.AllianceFlipUtil;
+import java.util.function.Consumer;
+import java.util.function.Supplier;
+
+/**
+ * A class that holds composite commands, which are sequences of commands for complex robot actions.
+ */
+public class SharedCompositeCommands {
+ /**
+ * Creates a command to reset the robot's heading to the alliance-specific zero.
+ *
+ * @param drive The drive subsystem.
+ * @return A command to reset the heading.
+ */
+ public static Command resetHeading(
+ SwerveDrive drive,
+ Consumer resetHeadingConsumer,
+ Supplier currentRobotTranslation) {
+ return Commands.runOnce(
+ () -> {
+ resetHeadingConsumer.accept(
+ new Pose2d(
+ currentRobotTranslation.get(), AllianceFlipUtil.apply(new Rotation2d())));
+ })
+ .ignoringDisable(true);
+ }
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/commands/v0_Funky/V0_FunkyCompositeCommands.java b/lib/examples/swerve/src/main/java/frc/robot/commands/v0_Funky/V0_FunkyCompositeCommands.java
new file mode 100644
index 00000000..2fa8ec61
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/commands/v0_Funky/V0_FunkyCompositeCommands.java
@@ -0,0 +1,3 @@
+package frc.robot.commands.v0_Funky;
+
+public class V0_FunkyCompositeCommands {}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/commands/v0_Funky/autonomous/V0_FunkyAutonomousTest.java b/lib/examples/swerve/src/main/java/frc/robot/commands/v0_Funky/autonomous/V0_FunkyAutonomousTest.java
new file mode 100644
index 00000000..e65de1b5
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/commands/v0_Funky/autonomous/V0_FunkyAutonomousTest.java
@@ -0,0 +1,3 @@
+package frc.robot.commands.v0_Funky.autonomous;
+
+public class V0_FunkyAutonomousTest {}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/subsystems/v0_Funky/V0_FunkyConstants.java b/lib/examples/swerve/src/main/java/frc/robot/subsystems/v0_Funky/V0_FunkyConstants.java
new file mode 100644
index 00000000..69134d4f
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/subsystems/v0_Funky/V0_FunkyConstants.java
@@ -0,0 +1,162 @@
+package frc.robot.subsystems.v0_Funky;
+
+import static edu.wpi.first.units.Units.*;
+
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.team190.gompeilib.core.utility.control.Gains;
+import edu.wpi.team190.gompeilib.core.utility.control.constraints.AngularPositionConstraints;
+import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints;
+import edu.wpi.team190.gompeilib.core.utility.tunable.LoggedTunableMeasure;
+import edu.wpi.team190.gompeilib.core.utility.tunable.LoggedTunableNumber;
+import edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive.SwerveDriveConstants;
+import edu.wpi.team190.gompeilib.subsystems.vision.VisionConstants.StaticLimelightConfig;
+import edu.wpi.team190.gompeilib.subsystems.vision.camera.CameraType;
+
+public class V0_FunkyConstants {
+ public static final SwerveDriveConstants.DriveConfig DRIVE_CONFIG =
+ SwerveDriveConstants.DriveConfig.builder()
+ .withCanBus(V0_FunkyTunerConstants.kCANBus)
+ .withPigeon2Id(V0_FunkyTunerConstants.DrivetrainConstants.Pigeon2Id)
+ .withMaxLinearVelocityMetersPerSecond(
+ V0_FunkyTunerConstants.kSpeedAt12Volts.in(MetersPerSecond))
+ .withWheelRadiusMeters(V0_FunkyTunerConstants.kWheelRadius.in(Meters))
+ .withDriveModel(DCMotor.getKrakenX60Foc(1))
+ .withTurnModel(DCMotor.getKrakenX44Foc(1))
+ .withFrontLeft(V0_FunkyTunerConstants.FrontLeft)
+ .withFrontRight(V0_FunkyTunerConstants.FrontRight)
+ .withBackLeft(V0_FunkyTunerConstants.BackLeft)
+ .withBackRight(V0_FunkyTunerConstants.BackRight)
+ .withDriveClosedLoopOutputType(V0_FunkyTunerConstants.kDriveClosedLoopOutput)
+ .withSteerClosedLoopOutputType(V0_FunkyTunerConstants.kSteerClosedLoopOutput)
+ .withBumperLength(Units.inchesToMeters(34.5))
+ .withBumperWidth(Units.inchesToMeters(34.5))
+ .build();
+
+ public static final Gains DRIVE_GAINS =
+ Gains.builder()
+ .withKP(
+ new LoggedTunableNumber(
+ "Drive/Teleoperated/Drive Kp", V0_FunkyTunerConstants.driveGains.kP))
+ .withKD(
+ new LoggedTunableNumber(
+ "Drive/Teleoperated/Drive Kd", V0_FunkyTunerConstants.driveGains.kD))
+ .withKS(
+ new LoggedTunableNumber(
+ "Drive/Teleoperated/Drive Ks", V0_FunkyTunerConstants.driveGains.kS))
+ .withKV(
+ new LoggedTunableNumber(
+ "Drive/Teleoperated/Drive Kv", V0_FunkyTunerConstants.driveGains.kV))
+ .build();
+
+ public static final Gains TRANSLATION_AUTO_GAINS =
+ Gains.builder()
+ .withKP(new LoggedTunableNumber("Drive/Auto/Translation Kp", 0.0))
+ .withKD(new LoggedTunableNumber("Drive/Auto/Translation Kd", 0.0))
+ .build();
+
+ public static final Gains ROTATION_AUTO_GAINS =
+ Gains.builder()
+ .withKP(new LoggedTunableNumber("Drive/Auto/Rotation Kp", 0.0))
+ .withKD(new LoggedTunableNumber("Drive/Auto/Rotation Kd", 0.0))
+ .build();
+
+ public static final Gains AUTO_ALIGN_X_GAINS =
+ Gains.builder()
+ .withKP(new LoggedTunableNumber("Drive/Auto Align/X/Kp", 0.0))
+ .withKD(new LoggedTunableNumber("Drive/Auto Align/X/Kd", 0.0))
+ .build();
+
+ public static final LinearConstraints AUTO_ALIGN_X_CONSTRAINTS =
+ LinearConstraints.builder()
+ .withMaxVelocity(
+ new LoggedTunableMeasure<>(
+ "Drive/Auto Align/X/Max Velocity", MetersPerSecond.of(0.0)))
+ .withMaxAcceleration(
+ new LoggedTunableMeasure<>(
+ "Drive/Auto Align/X/Max Acceleration", MetersPerSecondPerSecond.of(0.0)))
+ .withGoalTolerance(
+ new LoggedTunableMeasure<>("Drive/Auto Align/X/Max Velocity", Meters.of(0.0)))
+ .build();
+
+ public static final Gains AUTO_ALIGN_Y_GAINS =
+ Gains.builder()
+ .withKP(new LoggedTunableNumber("Drive/Auto Align/Y/Kp", 0.0))
+ .withKD(new LoggedTunableNumber("Drive/Auto Align/Y/Kd", 0.0))
+ .build();
+
+ public static final LinearConstraints AUTO_ALIGN_Y_CONSTRAINTS =
+ LinearConstraints.builder()
+ .withMaxVelocity(
+ new LoggedTunableMeasure<>(
+ "Drive/Auto Align/Y/Max Velocity", MetersPerSecond.of(0.0)))
+ .withMaxAcceleration(
+ new LoggedTunableMeasure<>(
+ "Drive/Auto Align/Y/Max Acceleration", MetersPerSecondPerSecond.of(0.0)))
+ .withGoalTolerance(
+ new LoggedTunableMeasure<>("Drive/Auto Align/Y/Max Velocity", Meters.of(0.0)))
+ .build();
+
+ public static final Gains AUTO_ALIGN_THETA_GAINS =
+ Gains.builder()
+ .withKP(new LoggedTunableNumber("Drive/Auto Align/Theta/Kp", 0.0))
+ .withKD(new LoggedTunableNumber("Drive/Auto Align/Theta/Kd", 0.0))
+ .build();
+
+ public static final AngularPositionConstraints AUTO_ALIGN_THETA_CONSTRAINTS =
+ AngularPositionConstraints.builder()
+ .withMaxVelocity(
+ new LoggedTunableMeasure<>(
+ "Drive/Auto Align/Theta/Max Velocity", RadiansPerSecond.of(0.0)))
+ .withMaxAcceleration(
+ new LoggedTunableMeasure<>(
+ "Drive/Auto Align/Theta/Max Acceleration", RadiansPerSecondPerSecond.of(0.0)))
+ .withGoalTolerance(
+ new LoggedTunableMeasure<>("Drive/Auto Align/Theta/Max Velocity", Radians.of(0.0)))
+ .build();
+
+ public static final SwerveDriveConstants.AutoAlignConstants AUTO_ALIGN_NEAR_CONSTANTS =
+ SwerveDriveConstants.AutoAlignConstants.builder()
+ .withXGains(AUTO_ALIGN_X_GAINS)
+ .withXConstraints(AUTO_ALIGN_X_CONSTRAINTS)
+ .withYGains(AUTO_ALIGN_Y_GAINS)
+ .withYConstraints(AUTO_ALIGN_Y_CONSTRAINTS)
+ .withRotationGains(AUTO_ALIGN_THETA_GAINS)
+ .withRotationConstraints(AUTO_ALIGN_THETA_CONSTRAINTS)
+ .withLinearThreshold(
+ new LoggedTunableMeasure<>(
+ "Drive/Auto Align/Position Threshold Meters", Inches.of(0.25)))
+ .withAngularThreshold(
+ new LoggedTunableMeasure<>(
+ "Drive/Auto Align/Angular Threshold Meters", Radians.of(0.25)))
+ .build();
+
+ public static final double ODOMETRY_FREQUENCY = 250.0;
+ public static final double DRIVER_DEADBAND = 0.1;
+ public static final double OPERATOR_DEADBAND = 0.1;
+
+ public static final SwerveDriveConstants DRIVE_CONSTANTS =
+ SwerveDriveConstants.builder()
+ .withDriveConfig(DRIVE_CONFIG)
+ .withAutoTranslationGains(TRANSLATION_AUTO_GAINS)
+ .withAutoRotationGains(ROTATION_AUTO_GAINS)
+ .withAutoAlignConstants(AUTO_ALIGN_NEAR_CONSTANTS)
+ .withOdometryFrequency(ODOMETRY_FREQUENCY)
+ .withDriverDeadband(DRIVER_DEADBAND)
+ .withOperatorDeadband(OPERATOR_DEADBAND)
+ .build();
+
+ public static final StaticLimelightConfig LIMELIGHT_CONFIG =
+ StaticLimelightConfig.builder()
+ .key("limelight")
+ .cameraType(CameraType.LIMELIGHT_4)
+ .horizontalFOV(82.0)
+ .verticalFOV(56.2)
+ .megatagXYStdev(0.1)
+ .metatagThetaStdev(0.0015)
+ .megatag2XYStdev(0.001)
+ .robotToCameraTransform(new Transform3d(0.0, 0.0, 0.0, new Rotation3d(0.0, 0.0, 0.0)))
+ .build();
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/subsystems/v0_Funky/V0_FunkyRobotContainer.java b/lib/examples/swerve/src/main/java/frc/robot/subsystems/v0_Funky/V0_FunkyRobotContainer.java
new file mode 100644
index 00000000..41b53588
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/subsystems/v0_Funky/V0_FunkyRobotContainer.java
@@ -0,0 +1,167 @@
+package frc.robot.subsystems.v0_Funky;
+
+import choreo.auto.AutoChooser;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.team190.gompeilib.core.io.components.inertial.GyroIO;
+import edu.wpi.team190.gompeilib.core.io.components.inertial.GyroIOPigeon2;
+import edu.wpi.team190.gompeilib.core.robot.RobotContainer;
+import edu.wpi.team190.gompeilib.core.robot.RobotMode;
+import edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive.SwerveDrive;
+import edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive.SwerveModuleIO;
+import edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive.SwerveModuleIOSim;
+import edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive.SwerveModuleIOTalonFX;
+import edu.wpi.team190.gompeilib.subsystems.vision.Vision;
+import edu.wpi.team190.gompeilib.subsystems.vision.camera.CameraStaticLimelight;
+import edu.wpi.team190.gompeilib.subsystems.vision.io.CameraIOLimelight;
+import frc.robot.Constants;
+import frc.robot.RobotConfig;
+import frc.robot.commands.shared.DriveCommands;
+import frc.robot.commands.shared.SharedCompositeCommands;
+import frc.robot.util.input.XKeysInput;
+import java.util.List;
+
+public class V0_FunkyRobotContainer implements RobotContainer {
+ private SwerveDrive drive;
+ private Vision vision;
+
+ private final CommandXboxController driver = new CommandXboxController(0);
+
+ private final AutoChooser autoChooser = new AutoChooser();
+
+ private final XKeysInput xkeys = new XKeysInput(1);
+
+ public V0_FunkyRobotContainer() {
+ if (Constants.getMode() != RobotMode.REPLAY) {
+ switch (RobotConfig.ROBOT) {
+ case V0_FUNKY:
+ drive =
+ new SwerveDrive(
+ V0_FunkyConstants.DRIVE_CONSTANTS,
+ new GyroIOPigeon2(
+ V0_FunkyConstants.DRIVE_CONSTANTS, V0_FunkyRobotState::setGyroTimestamp),
+ new SwerveModuleIOTalonFX(
+ V0_FunkyConstants.DRIVE_CONSTANTS,
+ V0_FunkyConstants.DRIVE_CONSTANTS.driveConfig.frontLeft()),
+ new SwerveModuleIOTalonFX(
+ V0_FunkyConstants.DRIVE_CONSTANTS,
+ V0_FunkyConstants.DRIVE_CONSTANTS.driveConfig.frontRight()),
+ new SwerveModuleIOTalonFX(
+ V0_FunkyConstants.DRIVE_CONSTANTS,
+ V0_FunkyConstants.DRIVE_CONSTANTS.driveConfig.backLeft()),
+ new SwerveModuleIOTalonFX(
+ V0_FunkyConstants.DRIVE_CONSTANTS,
+ V0_FunkyConstants.DRIVE_CONSTANTS.driveConfig.backRight()),
+ V0_FunkyRobotState::getGlobalPose,
+ V0_FunkyRobotState::resetPose);
+ vision =
+ new Vision(
+ () -> AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark),
+ new CameraStaticLimelight(
+ new CameraIOLimelight(V0_FunkyConstants.LIMELIGHT_CONFIG),
+ V0_FunkyConstants.LIMELIGHT_CONFIG,
+ V0_FunkyRobotState::getHeading,
+ drive::getMeasuredChassisSpeeds,
+ V0_FunkyRobotState::getGyroTimestamp,
+ List.of(),
+ List.of()));
+ break;
+
+ case V0_FUNKY_SIM:
+ drive =
+ new SwerveDrive(
+ V0_FunkyConstants.DRIVE_CONSTANTS,
+ new GyroIO() {},
+ new SwerveModuleIOSim(
+ V0_FunkyConstants.DRIVE_CONSTANTS,
+ V0_FunkyConstants.DRIVE_CONSTANTS.driveConfig.frontLeft()),
+ new SwerveModuleIOSim(
+ V0_FunkyConstants.DRIVE_CONSTANTS,
+ V0_FunkyConstants.DRIVE_CONSTANTS.driveConfig.frontRight()),
+ new SwerveModuleIOSim(
+ V0_FunkyConstants.DRIVE_CONSTANTS,
+ V0_FunkyConstants.DRIVE_CONSTANTS.driveConfig.backLeft()),
+ new SwerveModuleIOSim(
+ V0_FunkyConstants.DRIVE_CONSTANTS,
+ V0_FunkyConstants.DRIVE_CONSTANTS.driveConfig.backRight()),
+ () -> Pose2d.kZero,
+ V0_FunkyRobotState::resetPose);
+ vision =
+ new Vision(
+ () -> AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark));
+
+ break;
+
+ default:
+ break;
+ }
+ }
+ if (drive == null) {
+ drive =
+ new SwerveDrive(
+ V0_FunkyConstants.DRIVE_CONSTANTS,
+ new GyroIOPigeon2(
+ V0_FunkyConstants.DRIVE_CONSTANTS, V0_FunkyRobotState::setGyroTimestamp),
+ new SwerveModuleIO() {},
+ new SwerveModuleIO() {},
+ new SwerveModuleIO() {},
+ new SwerveModuleIO() {},
+ V0_FunkyRobotState::getGlobalPose,
+ V0_FunkyRobotState::resetPose);
+ }
+
+ if (vision == null) {
+ new Vision(() -> AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark));
+ }
+
+ configureButtonBindings();
+ configureAutos();
+ }
+
+ private void configureButtonBindings() {
+ drive.setDefaultCommand(
+ DriveCommands.joystickDrive(
+ drive,
+ V0_FunkyConstants.DRIVE_CONSTANTS,
+ driver::getLeftY,
+ driver::getLeftX,
+ driver::getRightX,
+ V0_FunkyRobotState::getHeading));
+
+ driver
+ .povDown()
+ .onTrue(
+ SharedCompositeCommands.resetHeading(
+ drive,
+ V0_FunkyRobotState::resetPose,
+ () -> V0_FunkyRobotState.getGlobalPose().getTranslation()));
+
+ xkeys
+ .a1()
+ .or(xkeys.a2())
+ .or(xkeys.a3())
+ .or(xkeys.a4())
+ .onTrue(
+ SharedCompositeCommands.resetHeading(
+ drive,
+ V0_FunkyRobotState::resetPose,
+ () -> V0_FunkyRobotState.getGlobalPose().getTranslation()));
+ }
+
+ private void configureAutos() {
+ // Autos here
+ }
+
+ @Override
+ public void robotPeriodic() {
+ V0_FunkyRobotState.periodic(drive.getRawGyroRotation(), drive.getModulePositions());
+ }
+
+ @Override
+ public Command getAutonomousCommand() {
+ return autoChooser.selectedCommand();
+ }
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/subsystems/v0_Funky/V0_FunkyRobotState.java b/lib/examples/swerve/src/main/java/frc/robot/subsystems/v0_Funky/V0_FunkyRobotState.java
new file mode 100644
index 00000000..c49be094
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/subsystems/v0_Funky/V0_FunkyRobotState.java
@@ -0,0 +1,67 @@
+package frc.robot.subsystems.v0_Funky;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.networktables.NetworkTablesJNI;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.team190.gompeilib.core.state.localization.FieldZone;
+import edu.wpi.team190.gompeilib.core.state.localization.Localization;
+import frc.robot.util.NTPrefixes;
+import java.util.HashSet;
+import java.util.List;
+import lombok.Getter;
+import lombok.Setter;
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+public class V0_FunkyRobotState {
+ private static final AprilTagFieldLayout fieldLayout;
+ private static final Localization localization;
+
+ @AutoLogOutput(key = NTPrefixes.ROBOT_STATE + "Hood/Score Angle")
+ @Getter
+ private static final Rotation2d scoreAngle;
+
+ @AutoLogOutput(key = NTPrefixes.ROBOT_STATE + "Hood/Feed Angle")
+ @Getter
+ private static final Rotation2d feedAngle;
+
+ private static final FieldZone globalZone;
+
+ @Getter @Setter private static long gyroTimestamp;
+
+ static {
+ fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark);
+
+ globalZone = new FieldZone(new HashSet<>(fieldLayout.getTags()));
+
+ localization =
+ new Localization(
+ List.of(globalZone), V0_FunkyConstants.DRIVE_CONSTANTS.driveConfig.kinematics(), 2);
+ scoreAngle = Rotation2d.kZero;
+ feedAngle = Rotation2d.kZero;
+
+ gyroTimestamp = NetworkTablesJNI.now();
+ }
+
+ public static void periodic(Rotation2d heading, SwerveModulePosition[] modulePositions) {
+
+ localization.addOdometryObservation(Timer.getTimestamp(), heading, modulePositions);
+ Logger.recordOutput(NTPrefixes.ROBOT_STATE + "/Global Pose", getGlobalPose());
+ }
+
+ public static void resetPose(Pose2d pose) {
+ localization.resetPose(pose);
+ }
+
+ public static Rotation2d getHeading() {
+ return localization.getHeading();
+ }
+
+ public static Pose2d getGlobalPose() {
+ return localization.getEstimatedPose(globalZone);
+ }
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/subsystems/v0_Funky/V0_FunkyTunerConstants.java b/lib/examples/swerve/src/main/java/frc/robot/subsystems/v0_Funky/V0_FunkyTunerConstants.java
new file mode 100644
index 00000000..cebaaaf4
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/subsystems/v0_Funky/V0_FunkyTunerConstants.java
@@ -0,0 +1,304 @@
+package frc.robot.subsystems.v0_Funky;
+
+import static edu.wpi.first.units.Units.*;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.configs.*;
+import com.ctre.phoenix6.hardware.*;
+import com.ctre.phoenix6.signals.*;
+import com.ctre.phoenix6.swerve.*;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants.*;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.units.measure.*;
+
+// Generated by the 2026 Tuner X Swerve Project Generator
+// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
+public class V0_FunkyTunerConstants {
+ // Both sets of gains need to be tuned to your individual robot.
+
+ // The steer motor uses any SwerveModule.SteerRequestType control request with the
+ // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
+ public static final Slot0Configs steerGains =
+ new Slot0Configs()
+ .withKP(100)
+ .withKI(0)
+ .withKD(0.5)
+ .withKS(0.1)
+ .withKV(2.66)
+ .withKA(0)
+ .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
+ // When using closed-loop control, the drive motor uses the control
+ // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
+ public static final Slot0Configs driveGains =
+ new Slot0Configs().withKP(60.0).withKI(0).withKD(0.0).withKS(2.361118000000002).withKV(0.0);
+
+ // The closed-loop output type to use for the steer motors;
+ // This affects the PID/FF gains for the steer motors
+ public static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
+ // The closed-loop output type to use for the drive motors;
+ // This affects the PID/FF gains for the drive motors
+ public static final ClosedLoopOutputType kDriveClosedLoopOutput =
+ ClosedLoopOutputType.TorqueCurrentFOC;
+
+ // The type of motor used for the drive motor
+ private static final DriveMotorArrangement kDriveMotorType =
+ DriveMotorArrangement.TalonFX_Integrated;
+ // The type of motor used for the drive motor
+ private static final SteerMotorArrangement kSteerMotorType =
+ SteerMotorArrangement.TalonFX_Integrated;
+
+ // The remote sensor feedback type to use for the steer motors;
+ // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote*
+ private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
+
+ // The stator current at which the wheels start to slip;
+ // This needs to be tuned to your individual robot
+ private static final Current kSlipCurrent = Amps.of(120);
+
+ // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
+ // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
+ private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
+ private static final TalonFXConfiguration steerInitialConfigs =
+ new TalonFXConfiguration()
+ .withCurrentLimits(
+ new CurrentLimitsConfigs()
+ // Swerve azimuth does not require much torque output, so we can set a relatively
+ // low
+ // stator current limit to help avoid brownouts without impacting performance.
+ .withStatorCurrentLimit(Amps.of(60))
+ .withStatorCurrentLimitEnable(true));
+ private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
+ // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
+ private static final Pigeon2Configuration pigeonConfigs = null;
+
+ // CAN bus that the devices are located on;
+ // All swerve devices must share the same CAN bus
+ public static final CANBus kCANBus = new CANBus("Drive", "./logs/example.hoot");
+
+ // Theoretical free speed (m/s) at 12 V applied output;
+ // This needs to be tuned to your individual robot
+ public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.04);
+
+ // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
+ // This may need to be tuned to your individual robot
+ private static final double kCoupleRatio = 3.5714285714285716;
+
+ private static final double kDriveGearRatio = 6.122448979591837;
+ private static final double kSteerGearRatio = 21.428571428571427;
+ public static final Distance kWheelRadius = Inches.of(2);
+
+ private static final boolean kInvertRightSide = false;
+
+ private static final int kPigeonId = 50;
+
+ // These are only used for simulation
+ private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01);
+ private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01);
+ // Simulated voltage necessary to overcome friction
+ private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
+ private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
+
+ public static final SwerveDrivetrainConstants DrivetrainConstants =
+ new SwerveDrivetrainConstants()
+ .withCANBusName(kCANBus.getName())
+ .withPigeon2Id(kPigeonId)
+ .withPigeon2Configs(pigeonConfigs);
+
+ private static final SwerveModuleConstantsFactory<
+ TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+ ConstantCreator =
+ new SwerveModuleConstantsFactory<
+ TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>()
+ .withDriveMotorGearRatio(kDriveGearRatio)
+ .withSteerMotorGearRatio(kSteerGearRatio)
+ .withCouplingGearRatio(kCoupleRatio)
+ .withWheelRadius(kWheelRadius)
+ .withSteerMotorGains(steerGains)
+ .withDriveMotorGains(driveGains)
+ .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
+ .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
+ .withSlipCurrent(kSlipCurrent)
+ .withSpeedAt12Volts(kSpeedAt12Volts)
+ .withDriveMotorType(kDriveMotorType)
+ .withSteerMotorType(kSteerMotorType)
+ .withFeedbackSource(kSteerFeedbackType)
+ .withDriveMotorInitialConfigs(driveInitialConfigs)
+ .withSteerMotorInitialConfigs(steerInitialConfigs)
+ .withEncoderInitialConfigs(encoderInitialConfigs)
+ .withSteerInertia(kSteerInertia)
+ .withDriveInertia(kDriveInertia)
+ .withSteerFrictionVoltage(kSteerFrictionVoltage)
+ .withDriveFrictionVoltage(kDriveFrictionVoltage);
+
+ // Front Left
+ private static final int kFrontLeftDriveMotorId = 1;
+ private static final int kFrontLeftSteerMotorId = 5;
+ private static final int kFrontLeftEncoderId = 10;
+ private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.463134765625 + 0.25);
+ private static final boolean kFrontLeftSteerMotorInverted = true;
+ private static final boolean kFrontLeftEncoderInverted = false;
+
+ private static final Distance kFrontLeftXPos = Inches.of(11.5);
+ private static final Distance kFrontLeftYPos = Inches.of(11.5);
+
+ // Front Right
+ private static final int kFrontRightDriveMotorId = 2;
+ private static final int kFrontRightSteerMotorId = 6;
+ private static final int kFrontRightEncoderId = 11;
+ private static final Angle kFrontRightEncoderOffset = Rotations.of(0.1376953125 + 0.25);
+ private static final boolean kFrontRightSteerMotorInverted = true;
+ private static final boolean kFrontRightEncoderInverted = false;
+
+ private static final Distance kFrontRightXPos = Inches.of(11.5);
+ private static final Distance kFrontRightYPos = Inches.of(-11.5);
+
+ // Back Left
+ private static final int kBackLeftDriveMotorId = 3;
+ private static final int kBackLeftSteerMotorId = 7;
+ private static final int kBackLeftEncoderId = 12;
+ private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.018798828125 + 0.25);
+ private static final boolean kBackLeftSteerMotorInverted = true;
+ private static final boolean kBackLeftEncoderInverted = false;
+
+ private static final Distance kBackLeftXPos = Inches.of(-11.5);
+ private static final Distance kBackLeftYPos = Inches.of(11.5);
+
+ // Back Right
+ private static final int kBackRightDriveMotorId = 4;
+ private static final int kBackRightSteerMotorId = 8;
+ private static final int kBackRightEncoderId = 13;
+ private static final Angle kBackRightEncoderOffset = Rotations.of(0.477294921875 + 0.25);
+ private static final boolean kBackRightSteerMotorInverted = true;
+ private static final boolean kBackRightEncoderInverted = false;
+
+ private static final Distance kBackRightXPos = Inches.of(-11.5);
+ private static final Distance kBackRightYPos = Inches.of(-11.5);
+
+ public static final SwerveModuleConstants<
+ TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+ FrontLeft =
+ ConstantCreator.createModuleConstants(
+ kFrontLeftSteerMotorId,
+ kFrontLeftDriveMotorId,
+ kFrontLeftEncoderId,
+ kFrontLeftEncoderOffset,
+ kFrontLeftXPos,
+ kFrontLeftYPos,
+ false,
+ kFrontLeftSteerMotorInverted,
+ kFrontLeftEncoderInverted);
+ public static final SwerveModuleConstants<
+ TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+ FrontRight =
+ ConstantCreator.createModuleConstants(
+ kFrontRightSteerMotorId,
+ kFrontRightDriveMotorId,
+ kFrontRightEncoderId,
+ kFrontRightEncoderOffset,
+ kFrontRightXPos,
+ kFrontRightYPos,
+ kInvertRightSide,
+ kFrontRightSteerMotorInverted,
+ kFrontRightEncoderInverted);
+ public static final SwerveModuleConstants<
+ TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+ BackLeft =
+ ConstantCreator.createModuleConstants(
+ kBackLeftSteerMotorId,
+ kBackLeftDriveMotorId,
+ kBackLeftEncoderId,
+ kBackLeftEncoderOffset,
+ kBackLeftXPos,
+ kBackLeftYPos,
+ true,
+ kBackLeftSteerMotorInverted,
+ kBackLeftEncoderInverted);
+ public static final SwerveModuleConstants<
+ TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+ BackRight =
+ ConstantCreator.createModuleConstants(
+ kBackRightSteerMotorId,
+ kBackRightDriveMotorId,
+ kBackRightEncoderId,
+ kBackRightEncoderOffset,
+ kBackRightXPos,
+ kBackRightYPos,
+ true,
+ kBackRightSteerMotorInverted,
+ kBackRightEncoderInverted);
+
+ /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */
+ public static class TunerSwerveDrivetrain extends SwerveDrivetrain {
+ /**
+ * Constructs a CTRE SwerveDrivetrain using the specified constants.
+ *
+ * This constructs the underlying hardware devices, so users should not construct the devices
+ * themselves. If they need the devices, they can access them through getters in the classes.
+ *
+ * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
+ * @param modules Constants for each specific module
+ */
+ public TunerSwerveDrivetrain(
+ SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) {
+ super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules);
+ }
+
+ /**
+ * Constructs a CTRE SwerveDrivetrain using the specified constants.
+ *
+ *
This constructs the underlying hardware devices, so users should not construct the devices
+ * themselves. If they need the devices, they can access them through getters in the classes.
+ *
+ * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
+ * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set
+ * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0.
+ * @param modules Constants for each specific module
+ */
+ public TunerSwerveDrivetrain(
+ SwerveDrivetrainConstants drivetrainConstants,
+ double odometryUpdateFrequency,
+ SwerveModuleConstants... modules) {
+ super(
+ TalonFX::new,
+ TalonFX::new,
+ CANcoder::new,
+ drivetrainConstants,
+ odometryUpdateFrequency,
+ modules);
+ }
+
+ /**
+ * Constructs a CTRE SwerveDrivetrain using the specified constants.
+ *
+ *
This constructs the underlying hardware devices, so users should not construct the devices
+ * themselves. If they need the devices, they can access them through getters in the classes.
+ *
+ * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
+ * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set
+ * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0.
+ * @param odometryStandardDeviation The standard deviation for odometry calculation in the form
+ * [x, y, theta], with units in meters and radians
+ * @param visionStandardDeviation The standard deviation for vision calculation in the form [x,
+ * y, theta], with units in meters and radians
+ * @param modules Constants for each specific module
+ */
+ public TunerSwerveDrivetrain(
+ SwerveDrivetrainConstants drivetrainConstants,
+ double odometryUpdateFrequency,
+ Matrix odometryStandardDeviation,
+ Matrix visionStandardDeviation,
+ SwerveModuleConstants... modules) {
+ super(
+ TalonFX::new,
+ TalonFX::new,
+ CANcoder::new,
+ drivetrainConstants,
+ odometryUpdateFrequency,
+ odometryStandardDeviation,
+ visionStandardDeviation,
+ modules);
+ }
+ }
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/util/Alert.java b/lib/examples/swerve/src/main/java/frc/robot/util/Alert.java
new file mode 100644
index 00000000..e53f04eb
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/util/Alert.java
@@ -0,0 +1,148 @@
+// Copyright (c) 2023 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.util;
+
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.function.Predicate;
+import lombok.Getter;
+
+/** Class for managing persistent alerts to be sent over NetworkTables. */
+public class Alert {
+ private static final Map groups = new HashMap<>();
+
+ private final AlertType type;
+ @Getter private boolean active = false;
+ private double activeStartTime = 0.0;
+ private String text;
+
+ /**
+ * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated,
+ * the appropriate entries will be added to NetworkTables.
+ *
+ * @param text Text to be displayed when the alert is active.
+ * @param type Alert level specifying urgency.
+ */
+ public Alert(String text, AlertType type) {
+ this("Alerts", text, type);
+ }
+
+ /**
+ * Creates a new Alert. If this is the first to be instantiated in its group, the appropriate
+ * entries will be added to NetworkTables.
+ *
+ * @param group Group identifier, also used as NetworkTables title
+ * @param text Text to be displayed when the alert is active.
+ * @param type Alert level specifying urgency.
+ */
+ public Alert(String group, String text, AlertType type) {
+ if (!groups.containsKey(group)) {
+ groups.put(group, new SendableAlerts());
+ SmartDashboard.putData(group, groups.get(group));
+ }
+
+ this.text = text;
+ this.type = type;
+ groups.get(group).alerts.add(this);
+ }
+
+ /**
+ * Sets whether the alert should currently be displayed. When activated, the alert text will also
+ * be sent to the console.
+ */
+ public void set(boolean active) {
+ if (active && !this.active) {
+ activeStartTime = Timer.getTimestamp();
+ switch (type) {
+ case ERROR:
+ DriverStation.reportError(text, false);
+ break;
+ case WARNING:
+ DriverStation.reportWarning(text, false);
+ break;
+ case INFO:
+ System.out.println(text);
+ break;
+ }
+ }
+ this.active = active;
+ }
+
+ /** Updates current alert text. */
+ public void setText(String text) {
+ if (active && !text.equals(this.text)) {
+ switch (type) {
+ case ERROR:
+ DriverStation.reportError(text, false);
+ break;
+ case WARNING:
+ DriverStation.reportWarning(text, false);
+ break;
+ case INFO:
+ System.out.println(text);
+ break;
+ }
+ }
+ this.text = text;
+ }
+
+ /** Represents an alert's level of urgency. */
+ public enum AlertType {
+ /**
+ * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type
+ * for problems which will seriously affect the robot's functionality and thus require immediate
+ * attention.
+ */
+ ERROR,
+
+ /**
+ * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this
+ * type for problems which could affect the robot's functionality but do not necessarily require
+ * immediate attention.
+ */
+ WARNING,
+
+ /**
+ * Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type
+ * for problems which are unlikely to affect the robot's functionality, or any other alerts
+ * which do not fall under "ERROR" or "WARNING".
+ */
+ INFO
+ }
+
+ private static class SendableAlerts implements Sendable {
+ public final List alerts = new ArrayList<>();
+
+ public String[] getStrings(AlertType type) {
+ Predicate activeFilter = (Alert x) -> x.type == type && x.active;
+ Comparator timeSorter =
+ (Alert a1, Alert a2) -> (int) (a2.activeStartTime - a1.activeStartTime);
+ return alerts.stream()
+ .filter(activeFilter)
+ .sorted(timeSorter)
+ .map((Alert a) -> a.text)
+ .toArray(String[]::new);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Alerts");
+ builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null);
+ builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null);
+ builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null);
+ }
+ }
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/util/AllianceFlipUtil.java b/lib/examples/swerve/src/main/java/frc/robot/util/AllianceFlipUtil.java
new file mode 100644
index 00000000..ca3ed684
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/util/AllianceFlipUtil.java
@@ -0,0 +1,58 @@
+package frc.robot.util;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+
+public class AllianceFlipUtil {
+ public static double fieldWidth = Units.feetToMeters(26.0) + Units.inchesToMeters(5.0);
+ public static double fieldLength = Units.feetToMeters(57.0) + Units.inchesToMeters(6.875);
+
+ public static double applyX(double x) {
+ return shouldFlip() ? fieldLength - x : x;
+ }
+
+ public static double applyY(double y) {
+ return shouldFlip() ? fieldWidth - y : y;
+ }
+
+ public static Translation2d apply(Translation2d translation) {
+ return new Translation2d(applyX(translation.getX()), applyY(translation.getY()));
+ }
+
+ public static Rotation2d apply(Rotation2d rotation) {
+ return shouldFlip() ? rotation.rotateBy(Rotation2d.kPi) : rotation;
+ }
+
+ public static Pose2d apply(Pose2d pose) {
+ return new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation()));
+ }
+
+ public static double overrideApplyX(double x) {
+ return fieldLength - x;
+ }
+
+ public static double overrideApplyY(double y) {
+ return fieldWidth - y;
+ }
+
+ public static Translation2d overrideApply(Translation2d translation) {
+ return new Translation2d(
+ overrideApplyX(translation.getX()), overrideApplyY(translation.getY()));
+ }
+
+ public static Rotation2d overrideApply(Rotation2d rotation) {
+ return rotation.rotateBy(Rotation2d.kPi);
+ }
+
+ public static Pose2d overrideApply(Pose2d pose) {
+ return new Pose2d(overrideApply(pose.getTranslation()), overrideApply(pose.getRotation()));
+ }
+
+ public static boolean shouldFlip() {
+ return DriverStation.getAlliance().isPresent()
+ && DriverStation.getAlliance().get() == DriverStation.Alliance.Red;
+ }
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/util/Elastic.java b/lib/examples/swerve/src/main/java/frc/robot/util/Elastic.java
new file mode 100644
index 00000000..d9a1ed7d
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/util/Elastic.java
@@ -0,0 +1,321 @@
+// Copyright (c) 2023-2025 Gold87 and other Elastic contributors
+// This software can be modified and/or shared under the terms
+// defined by the Elastic license:
+// https://github.com/Gold872/elastic_dashboard/blob/main/LICENSE
+
+package frc.robot.util;
+
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.PubSubOption;
+import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.networktables.StringTopic;
+import lombok.Getter;
+import lombok.Setter;
+
+public final class Elastic {
+ private static final StringTopic notificationTopic =
+ NetworkTableInstance.getDefault().getStringTopic("/Elastic/RobotNotifications");
+ private static final StringPublisher notificationPublisher =
+ notificationTopic.publish(PubSubOption.sendAll(true), PubSubOption.keepDuplicates(true));
+ private static final StringTopic selectedTabTopic =
+ NetworkTableInstance.getDefault().getStringTopic("/Elastic/SelectedTab");
+ private static final StringPublisher selectedTabPublisher =
+ selectedTabTopic.publish(PubSubOption.keepDuplicates(true));
+ private static final ObjectMapper objectMapper = new ObjectMapper();
+
+ /**
+ * Sends an notification to the Elastic dashboard. The notification is serialized as a JSON string
+ * before being published.
+ *
+ * @param notification the {@link Notification} object containing notification details
+ */
+ public static void sendNotification(Notification notification) {
+ try {
+ notificationPublisher.set(objectMapper.writeValueAsString(notification));
+ } catch (JsonProcessingException e) {
+ e.printStackTrace();
+ }
+ }
+
+ /**
+ * Selects the tab of the dashboard with the given name. If no tab matches the name, this will
+ * have no effect on the widgets or tabs in view.
+ *
+ * If the given name is a number, Elastic will select the tab whose index equals the number
+ * provided.
+ *
+ * @param tabName the name of the tab to select
+ */
+ public static void selectTab(String tabName) {
+ selectedTabPublisher.set(tabName);
+ }
+
+ /**
+ * Selects the tab of the dashboard at the given index. If this index is greater than or equal to
+ * the number of tabs, this will have no effect.
+ *
+ * @param tabIndex the index of the tab to select.
+ */
+ public static void selectTab(int tabIndex) {
+ selectTab(Integer.toString(tabIndex));
+ }
+
+ /**
+ * Represents the possible levels of notifications for the Elastic dashboard. These levels are
+ * used to indicate the severity or type of notification.
+ */
+ public enum NotificationLevel {
+ /** Informational Message */
+ INFO,
+ /** Warning message */
+ WARNING,
+ /** Error message */
+ ERROR
+ }
+
+ /**
+ * Represents an notification object to be sent to the Elastic dashboard. This object holds
+ * properties such as level, title, description, display time, and dimensions to control how the
+ * notification is displayed on the dashboard.
+ */
+ @Setter
+ @Getter
+ public static class Notification {
+ /**
+ * Updates the level of this notification
+ *
+ * @return the level of this notification
+ * @param level the level to set the notification to
+ */
+ private NotificationLevel level;
+
+ /**
+ * Updates the title of this notification
+ *
+ * @return the title of this notification
+ * @param title the title to set the notification to
+ */
+ private String title;
+
+ /**
+ * Updates the description of this notification
+ *
+ * @param description the description to set the notification to
+ */
+ private String description;
+
+ /**
+ * Updates the display time of the notification in milliseconds
+ *
+ * @return the number of milliseconds the notification is displayed for
+ * @param displayTimeMillis the number of milliseconds to display the notification for
+ */
+ private int displayTimeMillis;
+
+ /**
+ * Updates the width of the notification
+ *
+ * @return the width of the notification
+ * @param width the width to set the notification to
+ */
+ private double width;
+
+ /**
+ * Updates the height of the notification
+ *
+ *
If the height is set to -1, the height will be determined automatically by the dashboard
+ *
+ * @return the height of the notification
+ * @param height the height to set the notification to
+ */
+ private double height;
+
+ /**
+ * Creates a new Notification with all default parameters. This constructor is intended to be
+ * used with the chainable decorator methods
+ *
+ *
Title and description fields are empty.
+ */
+ public Notification() {
+ this(NotificationLevel.INFO, "", "");
+ }
+
+ /**
+ * Creates a new Notification with all properties specified.
+ *
+ * @param level the level of the notification (e.g., INFO, WARNING, ERROR)
+ * @param title the title text of the notification
+ * @param description the descriptive text of the notification
+ * @param displayTimeMillis the time in milliseconds for which the notification is displayed
+ * @param width the width of the notification display area
+ * @param height the height of the notification display area, inferred if below zero
+ */
+ public Notification(
+ NotificationLevel level,
+ String title,
+ String description,
+ int displayTimeMillis,
+ double width,
+ double height) {
+ this.level = level;
+ this.title = title;
+ this.displayTimeMillis = displayTimeMillis;
+ this.description = description;
+ this.height = height;
+ this.width = width;
+ }
+
+ /**
+ * Creates a new Notification with default display time and dimensions.
+ *
+ * @param level the level of the notification
+ * @param title the title text of the notification
+ * @param description the descriptive text of the notification
+ */
+ public Notification(NotificationLevel level, String title, String description) {
+ this(level, title, description, 3000, 350, -1);
+ }
+
+ /**
+ * Creates a new Notification with a specified display time and default dimensions.
+ *
+ * @param level the level of the notification
+ * @param title the title text of the notification
+ * @param description the descriptive text of the notification
+ * @param displayTimeMillis the display time in milliseconds
+ */
+ public Notification(
+ NotificationLevel level, String title, String description, int displayTimeMillis) {
+ this(level, title, description, displayTimeMillis, 350, -1);
+ }
+
+ /**
+ * Creates a new Notification with specified dimensions and default display time. If the height
+ * is below zero, it is automatically inferred based on screen size.
+ *
+ * @param level the level of the notification
+ * @param title the title text of the notification
+ * @param description the descriptive text of the notification
+ * @param width the width of the notification display area
+ * @param height the height of the notification display area, inferred if below zero
+ */
+ public Notification(
+ NotificationLevel level, String title, String description, double width, double height) {
+ this(level, title, description, 3000, width, height);
+ }
+
+ /**
+ * Updates the display time of the notification
+ *
+ * @param seconds the number of seconds to display the notification for
+ */
+ public void setDisplayTimeSeconds(double seconds) {
+ setDisplayTimeMillis((int) Math.round(seconds * 1000));
+ }
+
+ /**
+ * Modifies the notification's level and returns itself to allow for method chaining
+ *
+ * @param level the level to set the notification to
+ * @return the current notification
+ */
+ public Notification withLevel(NotificationLevel level) {
+ this.level = level;
+ return this;
+ }
+
+ /**
+ * Modifies the notification's title and returns itself to allow for method chaining
+ *
+ * @param title the title to set the notification to
+ * @return the current notification
+ */
+ public Notification withTitle(String title) {
+ setTitle(title);
+ return this;
+ }
+
+ /**
+ * Modifies the notification's description and returns itself to allow for method chaining
+ *
+ * @param description the description to set the notification to
+ * @return the current notification
+ */
+ public Notification withDescription(String description) {
+ setDescription(description);
+ return this;
+ }
+
+ /**
+ * Modifies the notification's display time and returns itself to allow for method chaining
+ *
+ * @param seconds the number of seconds to display the notification for
+ * @return the current notification
+ */
+ public Notification withDisplaySeconds(double seconds) {
+ return withDisplayMilliseconds((int) Math.round(seconds * 1000));
+ }
+
+ /**
+ * Modifies the notification's display time and returns itself to allow for method chaining
+ *
+ * @param displayTimeMillis the number of milliseconds to display the notification for
+ * @return the current notification
+ */
+ public Notification withDisplayMilliseconds(int displayTimeMillis) {
+ setDisplayTimeMillis(displayTimeMillis);
+ return this;
+ }
+
+ /**
+ * Modifies the notification's width and returns itself to allow for method chaining
+ *
+ * @param width the width to set the notification to
+ * @return the current notification
+ */
+ public Notification withWidth(double width) {
+ setWidth(width);
+ return this;
+ }
+
+ /**
+ * Modifies the notification's height and returns itself to allow for method chaining
+ *
+ * @param height the height to set the notification to
+ * @return the current notification
+ */
+ public Notification withHeight(double height) {
+ setHeight(height);
+ return this;
+ }
+
+ /**
+ * Modifies the notification's height and returns itself to allow for method chaining
+ *
+ *
This will set the height to -1 to have it automatically determined by the dashboard
+ *
+ * @return the current notification
+ */
+ public Notification withAutomaticHeight() {
+ setHeight(-1);
+ return this;
+ }
+
+ /**
+ * Modifies the notification to disable the auto dismiss behavior
+ *
+ *
This sets the display time to 0 milliseconds
+ *
+ *
The auto dismiss behavior can be re-enabled by setting the display time to a number
+ * greater than 0
+ *
+ * @return the current notification
+ */
+ public Notification withNoAutoDismiss() {
+ setDisplayTimeMillis(0);
+ return this;
+ }
+ }
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/util/InternalLoggedTracer.java b/lib/examples/swerve/src/main/java/frc/robot/util/InternalLoggedTracer.java
new file mode 100644
index 00000000..745e4baa
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/util/InternalLoggedTracer.java
@@ -0,0 +1,31 @@
+// Copyright (c) 2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.util;
+
+import edu.wpi.first.wpilibj.Timer;
+import org.littletonrobotics.junction.Logger;
+
+/** Utility class for logging code execution times. */
+public class InternalLoggedTracer {
+ private InternalLoggedTracer() {}
+
+ private static double startTime = -1.0;
+
+ /** Reset the clock. */
+ public static void reset() {
+ startTime = Timer.getFPGATimestamp();
+ }
+
+ /** Save the time elapsed since the last reset or record. */
+ public static void record(String epochName, String prefix) {
+ double now = Timer.getFPGATimestamp();
+ Logger.recordOutput(
+ "LoggedTracer/" + prefix + "/" + epochName + "MS", (now - startTime) * 1000.0);
+ startTime = now;
+ }
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/util/LTNUpdater.java b/lib/examples/swerve/src/main/java/frc/robot/util/LTNUpdater.java
new file mode 100644
index 00000000..5498c297
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/util/LTNUpdater.java
@@ -0,0 +1 @@
+package frc.robot.util;
diff --git a/lib/examples/swerve/src/main/java/frc/robot/util/NTPrefixes.java b/lib/examples/swerve/src/main/java/frc/robot/util/NTPrefixes.java
new file mode 100644
index 00000000..2f7e564d
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/util/NTPrefixes.java
@@ -0,0 +1,11 @@
+package frc.robot.util;
+
+public class NTPrefixes {
+ public static final String ROBOT_STATE = "RobotState/";
+ public static final String OI_DATA = ROBOT_STATE + "Operator Input Data/";
+ public static final String POSE_DATA = ROBOT_STATE + "Pose Data/";
+
+ public static final String CANIVORE_STATUS = ROBOT_STATE + "CANivore Status/";
+
+ public static final String SUPERSTRUCTURE = "Superstructure/";
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/util/ShotCalculator.java b/lib/examples/swerve/src/main/java/frc/robot/util/ShotCalculator.java
new file mode 100644
index 00000000..419969a4
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/util/ShotCalculator.java
@@ -0,0 +1,65 @@
+package frc.robot.util;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import java.util.function.Function;
+
+public interface ShotCalculator {
+ static final double phaseDelay = 0.3; // TODO: Change this value based on testing
+
+ /**
+ * Calculates a corrected pose for a moving target based on the shooter's current velocity.
+ *
+ * @param initialPose The current pose of the shooter.
+ * @param targetPose The pose of the target.
+ * @param robotVelocityMetersPerSecond The shooter's velocity in meters per second.
+ * @param distanceToTimeFunction A function that converts distance to time.
+ * @return The corrected pose to aim at.
+ */
+ public default Translation2d getAdjustedTargetPose(
+ Pose2d initialPose,
+ Pose2d targetPose,
+ ChassisSpeeds robotVelocityMetersPerSecond,
+ Function distanceToTimeFunction,
+ Transform2d centerToShooterCenter) {
+
+ // Adds phase delay to the initial pose based on robot velocity to account for latency caused by
+ // target pose calculation
+ initialPose =
+ initialPose.exp(
+ new Twist2d(
+ robotVelocityMetersPerSecond.vxMetersPerSecond * phaseDelay,
+ robotVelocityMetersPerSecond.vyMetersPerSecond * phaseDelay,
+ robotVelocityMetersPerSecond.omegaRadiansPerSecond * phaseDelay));
+
+ Pose2d shooterPose = initialPose.plus(centerToShooterCenter);
+ Transform2d shooterToTarget = new Transform2d(shooterPose, targetPose);
+
+ Translation2d shooterRobotFrameVelocityMetersPerSecond =
+ new Translation2d(
+ -centerToShooterCenter.getRotation().getSin(),
+ centerToShooterCenter.getRotation().getCos())
+ .times(robotVelocityMetersPerSecond.omegaRadiansPerSecond)
+ .times(centerToShooterCenter.getTranslation().getNorm());
+
+ Translation2d shooterFieldFrameVelocityMetersPerSecond =
+ shooterRobotFrameVelocityMetersPerSecond
+ .rotateBy(initialPose.getRotation())
+ .plus(
+ new Translation2d(
+ robotVelocityMetersPerSecond.vxMetersPerSecond,
+ robotVelocityMetersPerSecond.vyMetersPerSecond));
+
+ double deltaT = distanceToTimeFunction.apply(shooterToTarget.getTranslation().getNorm());
+
+ double correctedX =
+ targetPose.getX() - shooterFieldFrameVelocityMetersPerSecond.getX() * deltaT;
+ double correctedY =
+ targetPose.getY() - shooterFieldFrameVelocityMetersPerSecond.getY() * deltaT;
+
+ return new Translation2d(correctedX, correctedY);
+ }
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/util/SystemTimeValidReader.java b/lib/examples/swerve/src/main/java/frc/robot/util/SystemTimeValidReader.java
new file mode 100644
index 00000000..6a8ffc85
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/util/SystemTimeValidReader.java
@@ -0,0 +1,40 @@
+// Copyright (c) 2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.util;
+
+import edu.wpi.first.wpilibj.RobotController;
+
+public class SystemTimeValidReader {
+ private static Thread thread = null;
+ private static boolean ready = false;
+
+ public static void start() {
+ if (thread != null) return;
+ thread =
+ new Thread(
+ () -> {
+ while (true) {
+ boolean readyNew = RobotController.isSystemTimeValid();
+ synchronized (SystemTimeValidReader.class) {
+ ready = readyNew;
+ }
+ try {
+ Thread.sleep(3000);
+ } catch (InterruptedException e) {
+ e.printStackTrace();
+ }
+ }
+ });
+ thread.setName("SystemTimeValidReader");
+ thread.start();
+ }
+
+ public static synchronized boolean isValid() {
+ return ready;
+ }
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/util/input/XKeysInput.java b/lib/examples/swerve/src/main/java/frc/robot/util/input/XKeysInput.java
new file mode 100644
index 00000000..879ca577
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/util/input/XKeysInput.java
@@ -0,0 +1,454 @@
+package frc.robot.util.input;
+
+import edu.wpi.first.wpilibj.event.EventLoop;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.button.CommandGenericHID;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
+@SuppressWarnings("MethodName")
+public class XKeysInput extends CommandGenericHID {
+ /**
+ * Construct an instance of a controller.
+ *
+ * @param port The port index on the Driver Station that the controller is plugged into.
+ */
+ public XKeysInput(int port) {
+ super(port);
+ }
+
+ // Rows are lettered a-h from top to bottom
+ public enum Rows {
+ ka(11),
+ kb(12),
+ kc(13),
+ kd(14),
+ ke(15),
+ kf(16),
+ kg(17),
+ kh(18);
+ public final int value;
+
+ Rows(int value) {
+ this.value = value;
+ }
+ }
+
+ // Columns are numbered 1-10 from left to right
+ public enum Cols {
+ k1(1),
+ k2(2),
+ k3(3),
+ k4(4),
+ k5(5),
+ k6(6),
+ k7(7),
+ k8(8),
+ k9(9),
+ k10(10);
+ public final int value;
+
+ Cols(int value) {
+ this.value = value;
+ }
+ }
+
+ public Trigger a1() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ka.value, loop).and(button(Cols.k1.value, loop));
+ }
+
+ public Trigger a2() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ka.value, loop).and(button(Cols.k2.value, loop));
+ }
+
+ public Trigger a3() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ka.value, loop).and(button(Cols.k3.value, loop));
+ }
+
+ public Trigger a4() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ka.value, loop).and(button(Cols.k4.value, loop));
+ }
+
+ public Trigger a5() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ka.value, loop).and(button(Cols.k5.value, loop));
+ }
+
+ public Trigger a6() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ka.value, loop).and(button(Cols.k6.value, loop));
+ }
+
+ public Trigger a7() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ka.value, loop).and(button(Cols.k7.value, loop));
+ }
+
+ public Trigger a8() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ka.value, loop).and(button(Cols.k8.value, loop));
+ }
+
+ public Trigger a9() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ka.value, loop).and(button(Cols.k9.value, loop));
+ }
+
+ public Trigger a10() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ka.value, loop).and(button(Cols.k10.value, loop));
+ }
+
+ public Trigger b1() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kb.value, loop).and(button(Cols.k1.value, loop));
+ }
+
+ public Trigger b2() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kb.value, loop).and(button(Cols.k2.value, loop));
+ }
+
+ public Trigger b3() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kb.value, loop).and(button(Cols.k3.value, loop));
+ }
+
+ public Trigger b4() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kb.value, loop).and(button(Cols.k4.value, loop));
+ }
+
+ public Trigger b5() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kb.value, loop).and(button(Cols.k5.value, loop));
+ }
+
+ public Trigger b6() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kb.value, loop).and(button(Cols.k6.value, loop));
+ }
+
+ public Trigger b7() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kb.value, loop).and(button(Cols.k7.value, loop));
+ }
+
+ public Trigger b8() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kb.value, loop).and(button(Cols.k8.value, loop));
+ }
+
+ public Trigger b9() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kb.value, loop).and(button(Cols.k9.value, loop));
+ }
+
+ public Trigger b10() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kb.value, loop).and(button(Cols.k10.value, loop));
+ }
+
+ public Trigger c1() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kc.value, loop).and(button(Cols.k1.value, loop));
+ }
+
+ public Trigger c2() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kc.value, loop).and(button(Cols.k2.value, loop));
+ }
+
+ public Trigger c3() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kc.value, loop).and(button(Cols.k3.value, loop));
+ }
+
+ public Trigger c4() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kc.value, loop).and(button(Cols.k4.value, loop));
+ }
+
+ public Trigger c5() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kc.value, loop).and(button(Cols.k5.value, loop));
+ }
+
+ public Trigger c6() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kc.value, loop).and(button(Cols.k6.value, loop));
+ }
+
+ public Trigger c7() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kc.value, loop).and(button(Cols.k7.value, loop));
+ }
+
+ public Trigger c8() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kc.value, loop).and(button(Cols.k8.value, loop));
+ }
+
+ public Trigger c9() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kc.value, loop).and(button(Cols.k9.value, loop));
+ }
+
+ public Trigger c10() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kc.value, loop).and(button(Cols.k10.value, loop));
+ }
+
+ public Trigger d1() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kd.value, loop).and(button(Cols.k1.value, loop));
+ }
+
+ public Trigger d2() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kd.value, loop).and(button(Cols.k2.value, loop));
+ }
+
+ public Trigger d3() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kd.value, loop).and(button(Cols.k3.value, loop));
+ }
+
+ public Trigger d4() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kd.value, loop).and(button(Cols.k4.value, loop));
+ }
+
+ public Trigger d5() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kd.value, loop).and(button(Cols.k5.value, loop));
+ }
+
+ public Trigger d6() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kd.value, loop).and(button(Cols.k6.value, loop));
+ }
+
+ public Trigger d7() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kd.value, loop).and(button(Cols.k7.value, loop));
+ }
+
+ public Trigger d8() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kd.value, loop).and(button(Cols.k8.value, loop));
+ }
+
+ public Trigger d9() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kd.value, loop).and(button(Cols.k9.value, loop));
+ }
+
+ public Trigger d10() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kd.value, loop).and(button(Cols.k10.value, loop));
+ }
+
+ public Trigger e1() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ke.value, loop).and(button(Cols.k1.value, loop));
+ }
+
+ public Trigger e2() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ke.value, loop).and(button(Cols.k2.value, loop));
+ }
+
+ public Trigger e3() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ke.value, loop).and(button(Cols.k3.value, loop));
+ }
+
+ public Trigger e4() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ke.value, loop).and(button(Cols.k4.value, loop));
+ }
+
+ public Trigger e5() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ke.value, loop).and(button(Cols.k5.value, loop));
+ }
+
+ public Trigger e6() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ke.value, loop).and(button(Cols.k6.value, loop));
+ }
+
+ public Trigger e7() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ke.value, loop).and(button(Cols.k7.value, loop));
+ }
+
+ public Trigger e8() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ke.value, loop).and(button(Cols.k8.value, loop));
+ }
+
+ public Trigger e9() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ke.value, loop).and(button(Cols.k9.value, loop));
+ }
+
+ public Trigger e10() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.ke.value, loop).and(button(Cols.k10.value, loop));
+ }
+
+ public Trigger f1() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kf.value, loop).and(button(Cols.k1.value, loop));
+ }
+
+ public Trigger f2() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kf.value, loop).and(button(Cols.k2.value, loop));
+ }
+
+ public Trigger f3() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kf.value, loop).and(button(Cols.k3.value, loop));
+ }
+
+ public Trigger f4() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kf.value, loop).and(button(Cols.k4.value, loop));
+ }
+
+ public Trigger f5() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kf.value, loop).and(button(Cols.k5.value, loop));
+ }
+
+ public Trigger f6() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kf.value, loop).and(button(Cols.k6.value, loop));
+ }
+
+ public Trigger f7() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kf.value, loop).and(button(Cols.k7.value, loop));
+ }
+
+ public Trigger f8() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kf.value, loop).and(button(Cols.k8.value, loop));
+ }
+
+ public Trigger f9() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kf.value, loop).and(button(Cols.k9.value, loop));
+ }
+
+ public Trigger f10() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kf.value, loop).and(button(Cols.k10.value, loop));
+ }
+
+ public Trigger g1() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kg.value, loop).and(button(Cols.k1.value, loop));
+ }
+
+ public Trigger g2() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kg.value, loop).and(button(Cols.k2.value, loop));
+ }
+
+ public Trigger g3() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kg.value, loop).and(button(Cols.k3.value, loop));
+ }
+
+ public Trigger g4() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kg.value, loop).and(button(Cols.k4.value, loop));
+ }
+
+ public Trigger g5() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kg.value, loop).and(button(Cols.k5.value, loop));
+ }
+
+ public Trigger g6() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kg.value, loop).and(button(Cols.k6.value, loop));
+ }
+
+ public Trigger g7() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kg.value, loop).and(button(Cols.k7.value, loop));
+ }
+
+ public Trigger g8() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kg.value, loop).and(button(Cols.k8.value, loop));
+ }
+
+ public Trigger g9() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kg.value, loop).and(button(Cols.k9.value, loop));
+ }
+
+ public Trigger g10() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kg.value, loop).and(button(Cols.k10.value, loop));
+ }
+
+ public Trigger h1() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kh.value, loop).and(button(Cols.k1.value, loop));
+ }
+
+ public Trigger h2() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kh.value, loop).and(button(Cols.k2.value, loop));
+ }
+
+ public Trigger h3() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kh.value, loop).and(button(Cols.k3.value, loop));
+ }
+
+ public Trigger h4() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kh.value, loop).and(button(Cols.k4.value, loop));
+ }
+
+ public Trigger h5() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kh.value, loop).and(button(Cols.k5.value, loop));
+ }
+
+ public Trigger h6() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kh.value, loop).and(button(Cols.k6.value, loop));
+ }
+
+ public Trigger h7() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kh.value, loop).and(button(Cols.k7.value, loop));
+ }
+
+ public Trigger h8() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kh.value, loop).and(button(Cols.k8.value, loop));
+ }
+
+ public Trigger h9() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kh.value, loop).and(button(Cols.k9.value, loop));
+ }
+
+ public Trigger h10() {
+ EventLoop loop = CommandScheduler.getInstance().getDefaultButtonLoop();
+ return button(Rows.kh.value, loop).and(button(Cols.k10.value, loop));
+ }
+}
diff --git a/lib/examples/swerve/src/main/java/frc/robot/util/input/XboxElite2Input.java b/lib/examples/swerve/src/main/java/frc/robot/util/input/XboxElite2Input.java
new file mode 100644
index 00000000..c8f91dbd
--- /dev/null
+++ b/lib/examples/swerve/src/main/java/frc/robot/util/input/XboxElite2Input.java
@@ -0,0 +1,102 @@
+package frc.robot.util.input;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.event.EventLoop;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
+public class XboxElite2Input extends CommandXboxController {
+
+ public XboxElite2Input(int port) {
+ super(port);
+ }
+
+ /**
+ * Constructs a Trigger instance around the top left paddle's digital signal.
+ *
+ * @return a Trigger instance representing the top left paddle's digital signal attached to the
+ * {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+ * @see #topLeftPaddle(EventLoop)
+ */
+ public Trigger topLeftPaddle() {
+ return back(CommandScheduler.getInstance().getDefaultButtonLoop());
+ }
+
+ /**
+ * Constructs a Trigger instance around the top left paddle's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to.
+ * @return a Trigger instance representing the top left paddle's digital signal attached to the
+ * given loop.
+ */
+ public Trigger topLeftPaddle(EventLoop loop) {
+ return button(XboxController.Button.kBack.value, loop);
+ }
+
+ /**
+ * Constructs a Trigger instance around the top right paddle's digital signal.
+ *
+ * @return a Trigger instance representing the top right paddle's digital signal attached to the
+ * {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+ * @see #topRightPaddle(EventLoop)
+ */
+ public Trigger topRightPaddle() {
+ return start(CommandScheduler.getInstance().getDefaultButtonLoop());
+ }
+
+ /**
+ * Constructs a Trigger instance around the top right paddle's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to.
+ * @return a Trigger instance representing the top right paddle's digital signal attached to the
+ * given loop.
+ */
+ public Trigger topRightPaddle(EventLoop loop) {
+ return button(XboxController.Button.kStart.value, loop);
+ }
+
+ /**
+ * Constructs a Trigger instance around the bottom left paddle's digital signal.
+ *
+ * @return a Trigger instance representing the bottom left paddle's digital signal attached to the
+ * {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+ * @see #bottomLeftPaddle(EventLoop)
+ */
+ public Trigger bottomLeftPaddle() {
+ return leftStick(CommandScheduler.getInstance().getDefaultButtonLoop());
+ }
+
+ /**
+ * Constructs a Trigger instance around the bottom left paddle's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to.
+ * @return a Trigger instance representing the bottom left paddle's digital signal attached to the
+ * given loop.
+ */
+ public Trigger bottomLeftPaddle(EventLoop loop) {
+ return button(XboxController.Button.kLeftStick.value, loop);
+ }
+
+ /**
+ * Constructs a Trigger instance around the bottom right paddle's digital signal.
+ *
+ * @return a Trigger instance representing the bottom right paddle's digital signal attached to
+ * the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+ * @see #bottomRightPaddle(EventLoop)
+ */
+ public Trigger bottomRightPaddle() {
+ return rightStick(CommandScheduler.getInstance().getDefaultButtonLoop());
+ }
+
+ /**
+ * Constructs a Trigger instance around the bottom right paddle's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to.
+ * @return a Trigger instance representing the bottom right paddle's digital signal attached to
+ * the given loop.
+ */
+ public Trigger bottomRightPaddle(EventLoop loop) {
+ return button(XboxController.Button.kRightStick.value, loop);
+ }
+}
diff --git a/lib/examples/swerve/vendordeps/AdvantageKit.json b/lib/examples/swerve/vendordeps/AdvantageKit.json
new file mode 100644
index 00000000..2faa4db8
--- /dev/null
+++ b/lib/examples/swerve/vendordeps/AdvantageKit.json
@@ -0,0 +1,35 @@
+{
+ "fileName": "AdvantageKit.json",
+ "name": "AdvantageKit",
+ "version": "26.0.0",
+ "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
+ "frcYear": "2026",
+ "mavenUrls": [
+ "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"
+ ],
+ "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
+ "javaDependencies": [
+ {
+ "groupId": "org.littletonrobotics.akit",
+ "artifactId": "akit-java",
+ "version": "26.0.0"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "org.littletonrobotics.akit",
+ "artifactId": "akit-wpilibio",
+ "version": "26.0.0",
+ "skipInvalidPlatforms": false,
+ "isJar": false,
+ "validPlatforms": [
+ "linuxathena",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal",
+ "windowsx86-64"
+ ]
+ }
+ ],
+ "cppDependencies": []
+}
diff --git a/lib/examples/swerve/vendordeps/ChoreoLib.json b/lib/examples/swerve/vendordeps/ChoreoLib.json
new file mode 100644
index 00000000..cf7e7cf3
--- /dev/null
+++ b/lib/examples/swerve/vendordeps/ChoreoLib.json
@@ -0,0 +1,44 @@
+{
+ "fileName": "ChoreoLib2026.json",
+ "name": "ChoreoLib",
+ "version": "2026.0.1",
+ "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca",
+ "frcYear": "2026",
+ "mavenUrls": [
+ "https://frcmaven.wpi.edu/artifactory/sleipnirgroup-mvn-release/",
+ "https://repo1.maven.org/maven2"
+ ],
+ "jsonUrl": "https://choreo.autos/lib/ChoreoLib2026.json",
+ "javaDependencies": [
+ {
+ "groupId": "choreo",
+ "artifactId": "ChoreoLib-java",
+ "version": "2026.0.1"
+ },
+ {
+ "groupId": "com.google.code.gson",
+ "artifactId": "gson",
+ "version": "2.11.0"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "choreo",
+ "artifactId": "ChoreoLib-cpp",
+ "version": "2026.0.1",
+ "libName": "ChoreoLib",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal",
+ "linuxathena",
+ "linuxarm32",
+ "linuxarm64"
+ ]
+ }
+ ]
+}
diff --git a/lib/examples/swerve/vendordeps/Phoenix6-26.1.1.json b/lib/examples/swerve/vendordeps/Phoenix6-26.1.1.json
new file mode 100644
index 00000000..b136b65b
--- /dev/null
+++ b/lib/examples/swerve/vendordeps/Phoenix6-26.1.1.json
@@ -0,0 +1,449 @@
+{
+ "fileName": "Phoenix6-frc2026-latest.json",
+ "name": "CTRE-Phoenix (v6)",
+ "version": "26.1.1",
+ "frcYear": "2026",
+ "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
+ "mavenUrls": [
+ "https://maven.ctr-electronics.com/release/"
+ ],
+ "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-latest.json",
+ "conflictsWith": [
+ {
+ "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af",
+ "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.",
+ "offlineFileName": "Phoenix6-replay-frc2026-latest.json"
+ }
+ ],
+ "javaDependencies": [
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "wpiapi-java",
+ "version": "26.1.1"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "api-cpp",
+ "version": "26.1.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "tools",
+ "version": "26.1.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "api-cpp-sim",
+ "version": "26.1.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "tools-sim",
+ "version": "26.1.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simTalonSRX",
+ "version": "26.1.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simVictorSPX",
+ "version": "26.1.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simPigeonIMU",
+ "version": "26.1.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProTalonFX",
+ "version": "26.1.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProTalonFXS",
+ "version": "26.1.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANcoder",
+ "version": "26.1.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProPigeon2",
+ "version": "26.1.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANrange",
+ "version": "26.1.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANdi",
+ "version": "26.1.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANdle",
+ "version": "26.1.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "wpiapi-cpp",
+ "version": "26.1.1",
+ "libName": "CTRE_Phoenix6_WPI",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "tools",
+ "version": "26.1.1",
+ "libName": "CTRE_PhoenixTools",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "wpiapi-cpp-sim",
+ "version": "26.1.1",
+ "libName": "CTRE_Phoenix6_WPISim",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "tools-sim",
+ "version": "26.1.1",
+ "libName": "CTRE_PhoenixTools_Sim",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simTalonSRX",
+ "version": "26.1.1",
+ "libName": "CTRE_SimTalonSRX",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simVictorSPX",
+ "version": "26.1.1",
+ "libName": "CTRE_SimVictorSPX",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simPigeonIMU",
+ "version": "26.1.1",
+ "libName": "CTRE_SimPigeonIMU",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProTalonFX",
+ "version": "26.1.1",
+ "libName": "CTRE_SimProTalonFX",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProTalonFXS",
+ "version": "26.1.1",
+ "libName": "CTRE_SimProTalonFXS",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANcoder",
+ "version": "26.1.1",
+ "libName": "CTRE_SimProCANcoder",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProPigeon2",
+ "version": "26.1.1",
+ "libName": "CTRE_SimProPigeon2",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANrange",
+ "version": "26.1.1",
+ "libName": "CTRE_SimProCANrange",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANdi",
+ "version": "26.1.1",
+ "libName": "CTRE_SimProCANdi",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANdle",
+ "version": "26.1.1",
+ "libName": "CTRE_SimProCANdle",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ }
+ ]
+}
diff --git a/lib/examples/swerve/vendordeps/WPILibNewCommands.json b/lib/examples/swerve/vendordeps/WPILibNewCommands.json
new file mode 100644
index 00000000..d90630e9
--- /dev/null
+++ b/lib/examples/swerve/vendordeps/WPILibNewCommands.json
@@ -0,0 +1,39 @@
+{
+ "fileName": "WPILibNewCommands.json",
+ "name": "WPILib-New-Commands",
+ "version": "1.0.0",
+ "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
+ "frcYear": "2026",
+ "mavenUrls": [],
+ "jsonUrl": "",
+ "javaDependencies": [
+ {
+ "groupId": "edu.wpi.first.wpilibNewCommands",
+ "artifactId": "wpilibNewCommands-java",
+ "version": "wpilib"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "edu.wpi.first.wpilibNewCommands",
+ "artifactId": "wpilibNewCommands-cpp",
+ "version": "wpilib",
+ "libName": "wpilibNewCommands",
+ "headerClassifier": "headers",
+ "sourcesClassifier": "sources",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "linuxsystemcore",
+ "linuxathena",
+ "linuxarm32",
+ "linuxarm64",
+ "windowsx86-64",
+ "windowsx86",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ]
+}
diff --git a/lib/gradle/libs.versions.toml b/lib/gradle/libs.versions.toml
new file mode 100644
index 00000000..c4681e12
--- /dev/null
+++ b/lib/gradle/libs.versions.toml
@@ -0,0 +1,88 @@
+
+[versions]
+wpilib = "2026.2.1"
+wpilib-jni = "2025.3.2"
+phoenix6 = "26.1.3"
+choreo = "2026.0.2"
+aspectj = "1.9.21"
+advantagekit = "26.0.2"
+quickbuf = "1.4"
+jackson = "2.17.2"
+pathplanner = "2026.1.2"
+
+[libraries]
+# WPILib Core
+wpilib-wpilibj = { group = "edu.wpi.first.wpilibj", name = "wpilibj-java", version.ref = "wpilib" }
+wpilib-wpimath = { group = "edu.wpi.first.wpimath", name = "wpimath-java", version.ref = "wpilib" }
+wpilib-ntcore = { group = "edu.wpi.first.ntcore", name = "ntcore-java", version.ref = "wpilib" }
+wpilib-wpinet = { group = "edu.wpi.first.wpinet", name = "wpinet-java", version.ref = "wpilib" }
+wpilib-cscore = { group = "edu.wpi.first.cscore", name = "cscore-java", version.ref = "wpilib" }
+wpilib-cameraserver = { group = "edu.wpi.first.cameraserver", name = "cameraserver-java", version.ref = "wpilib" }
+wpilib-wpiutil = { group = "edu.wpi.first.wpiutil", name = "wpiutil-java", version.ref = "wpilib" }
+wpilib-wpiunits = { group = "edu.wpi.first.wpiunits", name = "wpiunits-java", version.ref = "wpilib" }
+wpilib-commands = { group = "edu.wpi.first.wpilibNewCommands", name = "wpilibNewCommands-java", version.ref = "wpilib" }
+wpilib-hal = { group = "edu.wpi.first.hal", name = "hal-java", version.ref = "wpilib" }
+wpilib-apriltag = {group = "edu.wpi.first.apriltag", name="apriltag-java", version.ref = "wpilib"}
+
+# WPILib JNI
+wpilib-ntcore-jni = { group = "edu.wpi.first.ntcore", name = "ntcore-jni", version.ref = "wpilib-jni" }
+wpilib-wpiutil-jni = { group = "edu.wpi.first.wpiutil", name = "wpiutil-jni", version.ref = "wpilib-jni" }
+wpilib-hal-jni = { group = "edu.wpi.first.hal", name = "hal-jni", version.ref = "wpilib-jni" }
+
+# CTRE Phoenix 6/Pro
+phoenix6 = { group = "com.ctre.phoenix6", name = "wpiapi-java", version.ref = "phoenix6" }
+
+# Choreo
+choreo = { group = "choreo", name = "ChoreoLib-java", version.ref = "choreo" }
+
+# PathplannerLib
+pathplanner = { group = "com.pathplanner.lib", name = "PathplannerLib-java", version.ref = "pathplanner" }
+
+# AspectJ
+aspectj-rt = { group = "org.aspectj", name = "aspectjrt", version.ref = "aspectj" }
+aspectj-tools = { group = "org.aspectj", name = "aspectjtools", version.ref = "aspectj" }
+
+# AdvantageKit
+advantagekit-java = { group = "org.littletonrobotics.akit", name = "akit-java", version.ref = "advantagekit" }
+advantagekit-autolog = { group = "org.littletonrobotics.akit", name = "akit-autolog", version.ref = "advantagekit" }
+
+# QuickBuf / Jackson (runtime dependencies needed for generated code)
+quickbuf = { group = "us.hebi.quickbuf", name = "quickbuf-runtime", version.ref = "quickbuf" }
+jackson-annotations = { group = "com.fasterxml.jackson.core", name = "jackson-annotations", version.ref = "jackson" }
+jackson-core = { group = "com.fasterxml.jackson.core", name = "jackson-core", version.ref = "jackson" }
+jackson-databind = { group = "com.fasterxml.jackson.core", name = "jackson-databind", version.ref = "jackson" }
+
+[bundles]
+wpilib = [
+ "wpilib-wpilibj",
+ "wpilib-wpimath",
+ "wpilib-ntcore",
+ "wpilib-wpinet",
+ "wpilib-cscore",
+ "wpilib-cameraserver",
+ "wpilib-wpiutil",
+ "wpilib-wpiunits",
+ "wpilib-commands",
+ "wpilib-hal",
+ "wpilib-apriltag"
+]
+
+wpilib-runtime = [
+ "wpilib-ntcore-jni",
+ "wpilib-wpiutil-jni",
+ "wpilib-hal-jni"
+]
+
+aspectj = [
+ "aspectj-rt",
+ "aspectj-tools"
+]
+
+advantagekit = [
+ "advantagekit-java",
+ "advantagekit-autolog",
+ "quickbuf",
+ "jackson-annotations",
+ "jackson-core",
+ "jackson-databind"
+]
diff --git a/lib/gradle/native-utils.gradle b/lib/gradle/native-utils.gradle
new file mode 100644
index 00000000..a99c8348
--- /dev/null
+++ b/lib/gradle/native-utils.gradle
@@ -0,0 +1,47 @@
+// Detect OS for FRC Natives
+def osClassifier = {
+ def osName = System.getProperty("os.name").toLowerCase()
+ if (osName.contains("win")) return "windowsx86-64"
+ if (osName.contains("mac")) return "osxuniversal"
+ return "linuxx86-64"
+}()
+
+configurations {
+ nativeDeps
+}
+
+dependencies {
+ // Uses the version provider from your libs.versions.toml [cite: 4]
+ def wpilibVer = libs.versions.wpilib.jni.get()
+ nativeDeps "edu.wpi.first.ntcore:ntcore-jni:${wpilibVer}:${osClassifier}"
+ nativeDeps "edu.wpi.first.wpiutil:wpiutil-jni:${wpilibVer}:${osClassifier}"
+ nativeDeps "edu.wpi.first.hal:hal-jni:${wpilibVer}:${osClassifier}"
+ nativeDeps "edu.wpi.first.wpimath:wpimath-jni:${wpilibVer}:${osClassifier}"
+
+ def phoenix6Ver = libs.versions.phoenix6.get()
+ nativeDeps "com.ctre.phoenix6:tools:${phoenix6Ver}:${osClassifier}@zip"
+ nativeDeps "com.ctre.phoenix6.sim:tools-sim:${phoenix6Ver}:${osClassifier}@zip"
+ nativeDeps "com.ctre.phoenix6:api-cpp:${phoenix6Ver}:${osClassifier}@zip"
+ nativeDeps "com.ctre.phoenix6.sim:api-cpp-sim:${phoenix6Ver}:${osClassifier}@zip"
+}
+
+def jniDestDir = layout.buildDirectory.dir("jni")
+
+tasks.register('extractNativeLibs', Copy) {
+ from {
+ configurations.nativeDeps.collect { zipTree(it) }
+ }
+ into jniDestDir
+ // Supports Windows (.dll), OSX (.dylib), and Linux (.so)
+ include "**/*.so", "**/*.dll", "**/*.dylib"
+ eachFile { path = name }
+ includeEmptyDirs = false
+}
+
+// Ensure all test tasks depend on extraction and see the library path
+tasks.withType(Test).configureEach {
+ dependsOn extractNativeLibs
+ // .get().asFile converts the DirectoryProperty to a standard File object
+ systemProperty "java.library.path", jniDestDir.get().asFile.absolutePath
+ systemProperty "jni.extract.path", jniDestDir.get().asFile.absolutePath
+}
diff --git a/lib/gradle/wrapper/gradle-wrapper.jar b/lib/gradle/wrapper/gradle-wrapper.jar
new file mode 100644
index 00000000..249e5832
Binary files /dev/null and b/lib/gradle/wrapper/gradle-wrapper.jar differ
diff --git a/lib/gradle/wrapper/gradle-wrapper.properties b/lib/gradle/wrapper/gradle-wrapper.properties
new file mode 100644
index 00000000..99e3a00a
--- /dev/null
+++ b/lib/gradle/wrapper/gradle-wrapper.properties
@@ -0,0 +1,6 @@
+#Thu Sep 18 10:41:29 EDT 2025
+distributionBase=GRADLE_USER_HOME
+distributionPath=wrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.14-bin.zip
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=wrapper/dists
diff --git a/lib/gradlew b/lib/gradlew
new file mode 100755
index 00000000..1b6c7873
--- /dev/null
+++ b/lib/gradlew
@@ -0,0 +1,234 @@
+#!/bin/sh
+
+#
+# Copyright © 2015-2021 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.
+# You may obtain a copy of the License at
+#
+# https://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
+##############################################################################
+#
+# Gradle start up script for POSIX generated by Gradle.
+#
+# Important for running:
+#
+# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is
+# noncompliant, but you have some other compliant shell such as ksh or
+# bash, then to run this script, type that shell name before the whole
+# command line, like:
+#
+# ksh Gradle
+#
+# Busybox and similar reduced shells will NOT work, because this script
+# requires all of these POSIX shell features:
+# * functions;
+# * expansions «$var», «${var}», «${var:-default}», «${var+SET}»,
+# «${var#prefix}», «${var%suffix}», and «$( cmd )»;
+# * compound commands having a testable exit status, especially «case»;
+# * various built-in commands including «command», «set», and «ulimit».
+#
+# Important for patching:
+#
+# (2) This script targets any POSIX shell, so it avoids extensions provided
+# by Bash, Ksh, etc; in particular arrays are avoided.
+#
+# The "traditional" practice of packing multiple parameters into a
+# space-separated string is a well documented source of bugs and security
+# problems, so this is (mostly) avoided, by progressively accumulating
+# options in "$@", and eventually passing that to Java.
+#
+# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS,
+# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly;
+# see the in-line comments for details.
+#
+# There are tweaks for specific operating systems such as AIX, CygWin,
+# Darwin, MinGW, and NonStop.
+#
+# (3) This script is generated from the Groovy template
+# https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+# within the Gradle project.
+#
+# You can find Gradle at https://github.com/gradle/gradle/.
+#
+##############################################################################
+
+# Attempt to set APP_HOME
+
+# Resolve links: $0 may be a link
+app_path=$0
+
+# Need this for daisy-chained symlinks.
+while
+ APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path
+ [ -h "$app_path" ]
+do
+ ls=$( ls -ld "$app_path" )
+ link=${ls#*' -> '}
+ case $link in #(
+ /*) app_path=$link ;; #(
+ *) app_path=$APP_HOME$link ;;
+ esac
+done
+
+APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit
+
+APP_NAME="Gradle"
+APP_BASE_NAME=${0##*/}
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD=maximum
+
+warn () {
+ echo "$*"
+} >&2
+
+die () {
+ echo
+ echo "$*"
+ echo
+ exit 1
+} >&2
+
+# OS specific support (must be 'true' or 'false').
+cygwin=false
+msys=false
+darwin=false
+nonstop=false
+case "$( uname )" in #(
+ CYGWIN* ) cygwin=true ;; #(
+ Darwin* ) darwin=true ;; #(
+ MSYS* | MINGW* ) msys=true ;; #(
+ NONSTOP* ) nonstop=true ;;
+esac
+
+CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
+
+
+# Determine the Java command to use to start the JVM.
+if [ -n "$JAVA_HOME" ] ; then
+ if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
+ # IBM's JDK on AIX uses strange locations for the executables
+ JAVACMD=$JAVA_HOME/jre/sh/java
+ else
+ JAVACMD=$JAVA_HOME/bin/java
+ fi
+ if [ ! -x "$JAVACMD" ] ; then
+ die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+ fi
+else
+ JAVACMD=java
+ which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+fi
+
+# Increase the maximum file descriptors if we can.
+if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
+ case $MAX_FD in #(
+ max*)
+ MAX_FD=$( ulimit -H -n ) ||
+ warn "Could not query maximum file descriptor limit"
+ esac
+ case $MAX_FD in #(
+ '' | soft) :;; #(
+ *)
+ ulimit -n "$MAX_FD" ||
+ warn "Could not set maximum file descriptor limit to $MAX_FD"
+ esac
+fi
+
+# Collect all arguments for the java command, stacking in reverse order:
+# * args from the command line
+# * the main class name
+# * -classpath
+# * -D...appname settings
+# * --module-path (only if needed)
+# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables.
+
+# 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" )
+
+ # Now convert the arguments - kludge to limit ourselves to /bin/sh
+ for arg do
+ if
+ case $arg in #(
+ -*) false ;; # don't mess with options #(
+ /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath
+ [ -e "$t" ] ;; #(
+ *) false ;;
+ esac
+ then
+ arg=$( cygpath --path --ignore --mixed "$arg" )
+ fi
+ # Roll the args list around exactly as many times as the number of
+ # args, so each arg winds up back in the position where it started, but
+ # possibly modified.
+ #
+ # NB: a `for` loop captures its iteration list before it begins, so
+ # changing the positional parameters here affects neither the number of
+ # iterations, nor the values presented in `arg`.
+ shift # remove old arg
+ set -- "$@" "$arg" # push replacement arg
+ done
+fi
+
+# Collect all arguments for the java command;
+# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of
+# shell script including quotes and variable substitutions, so put them in
+# double quotes to make sure that they get re-expanded; and
+# * put everything else in single quotes, so that it's not re-expanded.
+
+set -- \
+ "-Dorg.gradle.appname=$APP_BASE_NAME" \
+ -classpath "$CLASSPATH" \
+ org.gradle.wrapper.GradleWrapperMain \
+ "$@"
+
+# Use "xargs" to parse quoted args.
+#
+# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
+#
+# In Bash we could simply go:
+#
+# readarray ARGS < <( xargs -n1 <<<"$var" ) &&
+# set -- "${ARGS[@]}" "$@"
+#
+# but POSIX shell has neither arrays nor command substitution, so instead we
+# post-process each arg (as a line of input to sed) to backslash-escape any
+# character that might be a shell metacharacter, then use eval to reverse
+# that process (while maintaining the separation between arguments), and wrap
+# the whole thing up as a single "set" statement.
+#
+# This will of course break if any of these variables contains a newline or
+# an unmatched quote.
+#
+
+eval "set -- $(
+ printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
+ xargs -n1 |
+ sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
+ tr '\n' ' '
+ )" '"$@"'
+
+exec "$JAVACMD" "$@"
diff --git a/lib/gradlew.bat b/lib/gradlew.bat
new file mode 100644
index 00000000..ac1b06f9
--- /dev/null
+++ b/lib/gradlew.bat
@@ -0,0 +1,89 @@
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+
+@if "%DEBUG%" == "" @echo off
+@rem ##########################################################################
+@rem
+@rem Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%" == "" set DIRNAME=.
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Resolve any "." and ".." in APP_HOME to make it shorter.
+for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if "%ERRORLEVEL%" == "0" goto execute
+
+echo.
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto execute
+
+echo.
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+
+:end
+@rem End local scope for the variables with windows NT shell
+if "%ERRORLEVEL%"=="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!
+if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1
+exit /b 1
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega
diff --git a/lib/lombok.config b/lib/lombok.config
new file mode 100644
index 00000000..8f7e8aa1
--- /dev/null
+++ b/lib/lombok.config
@@ -0,0 +1 @@
+lombok.addLombokGeneratedAnnotation = true
\ No newline at end of file
diff --git a/lib/settings.gradle b/lib/settings.gradle
new file mode 100644
index 00000000..53311584
--- /dev/null
+++ b/lib/settings.gradle
@@ -0,0 +1,4 @@
+rootProject.name = 'gompeilib'
+
+include(":swerve")
+project(":swerve").projectDir = file("examples/swerve")
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/GompeiLib.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/GompeiLib.java
new file mode 100644
index 00000000..fa26bf97
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/GompeiLib.java
@@ -0,0 +1,69 @@
+package edu.wpi.team190.gompeilib.core;
+
+import edu.wpi.team190.gompeilib.core.robot.RobotMode;
+
+/** Main class for the GompeiLib library. Must be initialized by calling init() in robotInit(). */
+public final class GompeiLib {
+
+ private static boolean initialized = false;
+ private static RobotMode currentMode;
+ private static boolean tuningMode;
+ private static double loopPeriod;
+
+ /**
+ * Initializes the GompeiLib library with global constants from the robot project. This method
+ * should be called once in robotInit().
+ *
+ * @param mode The current robot mode (REAL, SIM, or REPLAY).
+ * @param isTuning Whether tuning mode is enabled.
+ * @param loopPeriodSecs The robot's main loop period in seconds.
+ */
+ public static void init(RobotMode mode, boolean isTuning, double loopPeriodSecs) {
+ if (initialized) {
+ System.err.println("GompeiLib has already been initialized!");
+ return;
+ }
+
+ currentMode = mode;
+ tuningMode = isTuning;
+ loopPeriod = loopPeriodSecs;
+
+ initialized = true;
+ }
+
+ public static void deinit() {
+ if (!initialized) {
+ System.err.println("GompeiLib has already been deinitialized!");
+ return;
+ }
+
+ currentMode = null;
+ tuningMode = false;
+ loopPeriod = 0.0;
+
+ initialized = false;
+ }
+
+ /** Throws an exception if the library has not been initialized. */
+ private static void checkInitialized() {
+ if (!initialized) {
+ throw new IllegalStateException("GompeiLib.init() must be called before using GompeiLib.");
+ }
+ }
+
+ // Public getters for other classes in your library to use.
+ public static RobotMode getMode() {
+ checkInitialized();
+ return currentMode;
+ }
+
+ public static boolean isTuning() {
+ checkInitialized();
+ return tuningMode;
+ }
+
+ public static double getLoopPeriod() {
+ checkInitialized();
+ return loopPeriod;
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/io/components/inertial/GyroIO.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/io/components/inertial/GyroIO.java
new file mode 100644
index 00000000..6dc31cad
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/io/components/inertial/GyroIO.java
@@ -0,0 +1,44 @@
+package edu.wpi.team190.gompeilib.core.io.components.inertial;
+
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+
+import com.ctre.phoenix6.StatusSignal;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import java.util.Queue;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface GyroIO {
+ @AutoLog
+ public static class GyroIOInputs {
+ public boolean connected = false;
+ public Rotation2d yawPosition = new Rotation2d();
+ public double[] odometryYawTimestamps = new double[] {};
+ public Rotation2d[] odometryYawPositions = new Rotation2d[] {};
+ public double yawVelocityRadPerSec = 0.0;
+
+ public Rotation2d pitchPosition = new Rotation2d();
+ public AngularVelocity pitchVelocity = RadiansPerSecond.zero();
+
+ public Rotation2d rollPosition = new Rotation2d();
+ public AngularVelocity rollVelocity = RadiansPerSecond.zero();
+ }
+
+ public default void updateInputs(GyroIOInputs inputs) {}
+
+ public default void updateInputs(
+ GyroIOInputs inputs, Queue yawTimestampQueue, Queue yawPositionQueue) {}
+
+ public default StatusSignal getYaw() {
+ return null;
+ }
+
+ public default StatusSignal getRoll() {
+ return null;
+ }
+
+ public default StatusSignal getPitch() {
+ return null;
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/io/components/inertial/GyroIOPigeon2.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/io/components/inertial/GyroIOPigeon2.java
new file mode 100644
index 00000000..46a935b3
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/io/components/inertial/GyroIOPigeon2.java
@@ -0,0 +1,89 @@
+package edu.wpi.team190.gompeilib.core.io.components.inertial;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.Pigeon2Configuration;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.NetworkTablesJNI;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.team190.gompeilib.core.GompeiLib;
+import edu.wpi.team190.gompeilib.core.logging.Trace;
+import edu.wpi.team190.gompeilib.core.utility.phoenix.PhoenixUtil;
+import edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive.SwerveDriveConstants;
+import java.util.Queue;
+import java.util.function.Consumer;
+import lombok.Getter;
+
+/** IO implementation for Pigeon 2. */
+public class GyroIOPigeon2 implements GyroIO {
+ @Getter private final StatusSignal yaw;
+ private final StatusSignal yawVelocity;
+
+ @Getter private final StatusSignal pitch;
+ private final StatusSignal pitchVelocity;
+
+ @Getter private final StatusSignal roll;
+ private final StatusSignal rollVelocity;
+
+ private final Consumer networktablesTimestampConsumer;
+
+ public GyroIOPigeon2(
+ SwerveDriveConstants driveConstants, Consumer networkTablesTimestampConsumer) {
+ Pigeon2 pigeon =
+ new Pigeon2(driveConstants.driveConfig.pigeon2Id(), driveConstants.driveConfig.canBus());
+ pigeon.getConfigurator().apply(new Pigeon2Configuration());
+ pigeon.getConfigurator().setYaw(0.0);
+
+ yaw = pigeon.getYaw();
+ yawVelocity = pigeon.getAngularVelocityZWorld();
+
+ yaw.setUpdateFrequency(driveConstants.odometryFrequency);
+
+ pitch = pigeon.getPitch();
+ pitchVelocity = pigeon.getAngularVelocityXWorld();
+
+ roll = pigeon.getRoll();
+ rollVelocity = pigeon.getAngularVelocityYWorld();
+
+ this.networktablesTimestampConsumer = networkTablesTimestampConsumer;
+
+ BaseStatusSignal.setUpdateFrequencyForAll(
+ 2 / GompeiLib.getLoopPeriod(), yawVelocity, pitch, pitchVelocity, roll, rollVelocity);
+
+ pigeon.optimizeBusUtilization();
+
+ PhoenixUtil.registerSignals(
+ driveConstants.driveConfig.canBus().isNetworkFD(),
+ yaw,
+ yawVelocity,
+ pitch,
+ pitchVelocity,
+ roll,
+ rollVelocity);
+ }
+
+ @Trace
+ @Override
+ public void updateInputs(
+ GyroIOInputs inputs, Queue yawTimestampQueue, Queue yawPositionQueue) {
+ inputs.connected =
+ BaseStatusSignal.isAllGood(yaw, yawVelocity, pitch, pitchVelocity, roll, rollVelocity);
+ inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
+ networktablesTimestampConsumer.accept(NetworkTablesJNI.now());
+ inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());
+
+ inputs.pitchPosition = new Rotation2d(pitch.getValue());
+ inputs.pitchVelocity = pitchVelocity.getValue();
+
+ inputs.rollPosition = new Rotation2d(roll.getValue());
+ inputs.rollVelocity = rollVelocity.getValue();
+
+ inputs.odometryYawTimestamps =
+ yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray();
+ inputs.odometryYawPositions =
+ yawPositionQueue.stream().map(Rotation2d::fromDegrees).toArray(Rotation2d[]::new);
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/logging/Trace.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/logging/Trace.java
new file mode 100644
index 00000000..a545931a
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/logging/Trace.java
@@ -0,0 +1,14 @@
+package edu.wpi.team190.gompeilib.core.logging;
+
+import java.lang.annotation.ElementType;
+import java.lang.annotation.Retention;
+import java.lang.annotation.RetentionPolicy;
+import java.lang.annotation.Target;
+
+/**
+ * Marks a method to be profiled for execution time. The time spent inside any method with this
+ * annotation will be automatically logged to AdvantageKit.
+ */
+@Target(ElementType.METHOD)
+@Retention(RetentionPolicy.RUNTIME)
+public @interface Trace {}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/logging/TracerAspect.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/logging/TracerAspect.java
new file mode 100644
index 00000000..2365c7b8
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/logging/TracerAspect.java
@@ -0,0 +1,46 @@
+package edu.wpi.team190.gompeilib.core.logging;
+
+import edu.wpi.first.wpilibj.Timer;
+import org.aspectj.lang.ProceedingJoinPoint;
+import org.aspectj.lang.annotation.Around;
+import org.aspectj.lang.annotation.Aspect;
+import org.aspectj.lang.reflect.MethodSignature;
+import org.littletonrobotics.junction.Logger;
+
+/**
+ * An AspectJ aspect that intercepts calls to methods annotated with @Tracer and logs their
+ * execution time to AdvantageKit's logger. This is the core of the automatic profiling system.
+ */
+@Aspect
+public class TracerAspect {
+
+ /**
+ * This is the "advice" that runs "around" any method annotated with @Tracer.
+ *
+ * @param joinPoint The point in the code where the advice is being executed.
+ * @return The return value of the original method.
+ * @throws Throwable If the original method throws an exception.
+ */
+ @Around("execution(@Trace * *.*(..))")
+ public Object profile(ProceedingJoinPoint joinPoint) throws Throwable {
+ // Use FPGA timestamp for high-resolution, synchronized timing
+ double startTime = Timer.getFPGATimestamp();
+
+ // Proceed with the original method call
+ Object result = joinPoint.proceed();
+
+ double endTime = Timer.getFPGATimestamp();
+ double executionTimeMs = (endTime - startTime) * 1000.0;
+
+ // Get method details for logging
+ MethodSignature signature = (MethodSignature) joinPoint.getSignature();
+ String className = signature.getDeclaringType().getSimpleName();
+ String methodName = signature.getName();
+
+ // Log the result to AdvantageKit Logger under the "Tracer/" directory
+ String logKey = "Tracer/" + className + "/" + methodName + "MS";
+ Logger.recordOutput(logKey, executionTimeMs);
+
+ return result;
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/robot/RobotContainer.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/robot/RobotContainer.java
new file mode 100644
index 00000000..42ff76b5
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/robot/RobotContainer.java
@@ -0,0 +1,13 @@
+package edu.wpi.team190.gompeilib.core.robot;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+
+public interface RobotContainer {
+
+ public default void robotPeriodic() {}
+
+ public default Command getAutonomousCommand() {
+ return Commands.none();
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/robot/RobotMode.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/robot/RobotMode.java
new file mode 100644
index 00000000..f1935e5c
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/robot/RobotMode.java
@@ -0,0 +1,7 @@
+package edu.wpi.team190.gompeilib.core.robot;
+
+public enum RobotMode {
+ REAL,
+ SIM,
+ REPLAY
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/robot/RobotState.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/robot/RobotState.java
new file mode 100644
index 00000000..c0a4287f
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/robot/RobotState.java
@@ -0,0 +1,6 @@
+package edu.wpi.team190.gompeilib.core.robot;
+
+public interface RobotState {
+
+ public default void periodic() {}
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/state/localization/EstimationRegion.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/state/localization/EstimationRegion.java
new file mode 100644
index 00000000..58909d5c
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/state/localization/EstimationRegion.java
@@ -0,0 +1,145 @@
+package edu.wpi.team190.gompeilib.core.state.localization;
+
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.team190.gompeilib.core.utility.GeometryUtil;
+import edu.wpi.team190.gompeilib.subsystems.vision.data.VisionMultiTxTyObservation;
+import edu.wpi.team190.gompeilib.subsystems.vision.data.VisionPoseObservation;
+import java.util.Map;
+import java.util.Set;
+import java.util.stream.Collectors;
+import lombok.Getter;
+
+/**
+ * Represents an independent localization context that estimates the robot's field-relative pose
+ * using a constrained subset of vision targets.
+ *
+ * An {@code EstimationRegion} owns its own {@link SwerveDrivePoseEstimator} and is configured
+ * with a fixed set of AprilTags that are considered valid measurement sources for this region. This
+ * allows multiple estimators to run in parallel, each optimized for different areas of the field,
+ * tag groupings, or sensing conditions.
+ *
+ *
The estimator continuously integrates drivetrain odometry and accepts vision updates in two
+ * forms:
+ *
+ *
+ * - Full field-relative pose observations (e.g. solvePNP-based estimates)
+ *
- Angular {@code tx/ty}-based observations that are projected into a 2D field pose using
+ * known AprilTag locations and the camera transform
+ *
+ *
+ * Vision measurements are assumed to originate only from the tags assigned to this region;
+ * observations referencing unknown tags are ignored.
+ *
+ * This abstraction enables higher-level localization logic to dynamically select or weight
+ * estimators based on robot position, visibility, confidence, or game-specific constraints, without
+ * entangling tag-selection logic with the underlying state estimation.
+ */
+public class EstimationRegion {
+ @Getter private final Map aprilTags;
+ private final SwerveDrivePoseEstimator poseEstimator;
+
+ public EstimationRegion(Set aprilTags, SwerveDriveKinematics kinematics) {
+ this.aprilTags =
+ aprilTags.stream()
+ .map(aprilTag -> Map.entry(aprilTag.ID, aprilTag.pose))
+ .collect(Collectors.toMap(Map.Entry::getKey, Map.Entry::getValue));
+
+ SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[4];
+ for (int i = 0; i < swerveModulePositions.length; i++) {
+ swerveModulePositions[i] = new SwerveModulePosition();
+ }
+
+ this.poseEstimator =
+ new SwerveDrivePoseEstimator(
+ kinematics, Rotation2d.kZero, swerveModulePositions, Pose2d.kZero);
+ }
+
+ public void addOdometryObservation(
+ double timestamp, Rotation2d heading, SwerveModulePosition[] modulePositions) {
+ poseEstimator.updateWithTime(timestamp, heading, modulePositions);
+ }
+
+ public void addPoseObservation(VisionPoseObservation observation) {
+ poseEstimator.addVisionMeasurement(
+ observation.pose(), observation.timestamp(), observation.stddevs());
+ }
+
+ public void addTxTyObservation(VisionMultiTxTyObservation observation) {
+
+ // Get odometry-based pose at the timestamp
+ var sample = poseEstimator.sampleAt(observation.timestamp());
+ if (sample.isEmpty()) return;
+
+ // Average tx and ty over four corners
+ double tx = 0.0;
+ double ty = 0.0;
+ for (int j = 0; j < 4; j++) {
+ tx += observation.tx()[j];
+ ty += observation.ty()[j];
+ }
+ tx /= 4.0;
+ ty /= 4.0;
+
+ Pose3d cameraPose = observation.cameraPose();
+
+ // Project 3D distance onto horizontal plane
+ Rotation3d camToTagRotation3d = new Rotation3d(0.0, -cameraPose.getRotation().getY() - ty, 0.0);
+
+ Translation3d camToTag = new Translation3d(observation.distance(), camToTagRotation3d);
+
+ double distance2d = camToTag.toTranslation2d().getNorm();
+
+ // Compute rotation from camera to tag
+ // tx and ty are flipped from WPILib convention.
+ Rotation2d camToTagRotation2d =
+ sample
+ .get()
+ .getRotation()
+ .plus(cameraPose.toPose2d().getRotation().plus(Rotation2d.fromRadians(-tx)));
+ int tagId = observation.tagId();
+
+ Pose3d tagPose3d = aprilTags.get(tagId);
+ if (tagPose3d == null) return;
+ Pose2d tagPose2d = tagPose3d.toPose2d();
+
+ // Compute camera position in field frame
+ Rotation2d tagToCameraRotation = camToTagRotation2d.plus(Rotation2d.kPi);
+
+ Translation2d fieldToCameraTranslation =
+ new Pose2d(tagPose2d.getTranslation(), tagToCameraRotation)
+ .transformBy(GeometryUtil.toTransform2d(distance2d, 0.0))
+ .getTranslation();
+
+ // Compute robot poses
+ Pose2d robotPose =
+ new Pose2d(
+ fieldToCameraTranslation,
+ sample.get().getRotation().plus(cameraPose.toPose2d().getRotation()))
+ .transformBy(new Transform2d(cameraPose.toPose2d(), Pose2d.kZero));
+
+ // Use odometry rotation only
+ robotPose = new Pose2d(robotPose.getTranslation(), sample.get().getRotation());
+
+ double xystdev =
+ LocalizationConstants.XY_STDDEV_COEFFICIENT
+ * Math.pow(observation.distance(), LocalizationConstants.XY_STDDEV_DISTANCE_EXPONENT);
+
+ poseEstimator.addVisionMeasurement(
+ robotPose,
+ observation.timestamp(),
+ VecBuilder.fill(xystdev, xystdev, Double.POSITIVE_INFINITY));
+ }
+
+ public Pose2d getEstimatedPose() {
+ return poseEstimator.getEstimatedPosition();
+ }
+
+ public void resetPose(Pose2d pose) {
+ poseEstimator.resetPose(pose);
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/state/localization/FieldZone.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/state/localization/FieldZone.java
new file mode 100644
index 00000000..4c7fbf67
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/state/localization/FieldZone.java
@@ -0,0 +1,27 @@
+package edu.wpi.team190.gompeilib.core.state.localization;
+
+import edu.wpi.first.apriltag.AprilTag;
+import java.util.Set;
+
+/**
+ * Defines a logical region of the field in terms of the AprilTags that are considered visible,
+ * reliable, or relevant within that area.
+ *
+ * A {@code FieldZone} contains no estimation or stateful logic on its own. Instead, it serves as
+ * a configuration object that describes which vision targets should be associated with a given
+ * localization strategy or {@link EstimationRegion}.
+ *
+ *
By separating field structure from estimation behavior, field zones make it possible to:
+ *
+ *
+ * - Partition the field into overlapping or disjoint regions
+ *
- Restrict vision updates to specific tag subsets
+ *
- Swap or combine localization strategies based on robot position
+ *
+ *
+ * This abstraction is intentionally lightweight and immutable, making it safe to share across
+ * subsystems and reuse throughout the localization stack.
+ *
+ * @param aprilTags the set of AprilTags associated with this field zone
+ */
+public record FieldZone(Set aprilTags) {}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/state/localization/Localization.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/state/localization/Localization.java
new file mode 100644
index 00000000..74626d8b
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/state/localization/Localization.java
@@ -0,0 +1,94 @@
+package edu.wpi.team190.gompeilib.core.state.localization;
+
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.team190.gompeilib.core.utility.GeometryUtil;
+import edu.wpi.team190.gompeilib.subsystems.vision.data.VisionMultiTxTyObservation;
+import edu.wpi.team190.gompeilib.subsystems.vision.data.VisionPoseObservation;
+import java.util.List;
+import java.util.Set;
+import java.util.stream.Collectors;
+
+public class Localization {
+ private final List estimationRegions;
+
+ private final SwerveDrivePoseEstimator globalPoseEstimator;
+
+ public Localization(
+ List estimationZones,
+ SwerveDriveKinematics kinematics,
+ double bufferLengthSeconds) {
+ this.globalPoseEstimator =
+ new SwerveDrivePoseEstimator(
+ kinematics,
+ Rotation2d.kZero,
+ new SwerveModulePosition[] {
+ new SwerveModulePosition(),
+ new SwerveModulePosition(),
+ new SwerveModulePosition(),
+ new SwerveModulePosition()
+ },
+ Pose2d.kZero);
+
+ this.estimationRegions =
+ estimationZones.stream()
+ .map(zone -> new EstimationRegion(zone.aprilTags(), kinematics))
+ .toList();
+ }
+
+ public void addOdometryObservation(
+ double timestamp, Rotation2d rawHeading, SwerveModulePosition[] modulePositions) {
+ globalPoseEstimator.updateWithTime(timestamp, rawHeading, modulePositions);
+ estimationRegions.forEach(
+ region -> region.addOdometryObservation(timestamp, rawHeading, modulePositions));
+ }
+
+ public void addPoseObservations(List poseObservations) {
+ poseObservations.stream()
+ .filter(observation -> !GeometryUtil.isNaN(observation.pose()))
+ .forEach(
+ observation ->
+ globalPoseEstimator.addVisionMeasurement(
+ observation.pose(), observation.timestamp(), observation.stddevs()));
+ estimationRegions.forEach(
+ zone ->
+ poseObservations.stream()
+ .filter(
+ observation -> zone.getAprilTags().keySet().containsAll(observation.tagIds()))
+ .filter(observation -> !GeometryUtil.isNaN(observation.pose()))
+ .forEach(zone::addPoseObservation));
+ }
+
+ public void addTxTyObservations(List txTyObservations) {
+ estimationRegions.forEach(
+ zone ->
+ txTyObservations.stream()
+ .filter(observation -> zone.getAprilTags().containsKey(observation.tagId()))
+ .forEach(zone::addTxTyObservation));
+ }
+
+ public Pose2d getEstimatedPose(FieldZone fieldZone) {
+ Set zoneTagIDs =
+ fieldZone.aprilTags().stream().map(t -> t.ID).collect(Collectors.toSet());
+
+ return estimationRegions.stream()
+ .filter(region -> zoneTagIDs.equals(region.getAprilTags().keySet()))
+ .findFirst()
+ .map(EstimationRegion::getEstimatedPose)
+ .orElseGet(globalPoseEstimator::getEstimatedPosition);
+ }
+
+ public Rotation2d getHeading() {
+ return globalPoseEstimator.getEstimatedPosition().getRotation();
+ }
+
+ public void resetPose(Pose2d pose) {
+ for (EstimationRegion region : estimationRegions) {
+ region.resetPose(pose);
+ }
+ globalPoseEstimator.resetPose(pose);
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/state/localization/LocalizationConstants.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/state/localization/LocalizationConstants.java
new file mode 100644
index 00000000..76cfa574
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/state/localization/LocalizationConstants.java
@@ -0,0 +1,6 @@
+package edu.wpi.team190.gompeilib.core.state.localization;
+
+public class LocalizationConstants {
+ public static final double XY_STDDEV_COEFFICIENT = 0.1;
+ public static final double XY_STDDEV_DISTANCE_EXPONENT = 1.2;
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/GeometryUtil.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/GeometryUtil.java
new file mode 100644
index 00000000..22b19075
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/GeometryUtil.java
@@ -0,0 +1,228 @@
+package edu.wpi.team190.gompeilib.core.utility;
+
+import edu.wpi.first.math.geometry.*;
+
+public class GeometryUtil {
+ /**
+ * Creates a pure translating transform
+ *
+ * @param translation The translation to create the transform with
+ * @return The resulting transform
+ */
+ public static Transform2d toTransform2d(Translation2d translation) {
+ return new Transform2d(translation, new Rotation2d());
+ }
+
+ /**
+ * Creates a pure translating transform
+ *
+ * @param x The x coordinate of the translation
+ * @param y The y coordinate of the translation
+ * @return The resulting transform
+ */
+ public static Transform2d toTransform2d(double x, double y) {
+ return new Transform2d(x, y, new Rotation2d());
+ }
+
+ /**
+ * Creates a pure rotating transform
+ *
+ * @param rotation The rotation to create the transform with
+ * @return The resulting transform
+ */
+ public static Transform2d toTransform2d(Rotation2d rotation) {
+ return new Transform2d(new Translation2d(), rotation);
+ }
+
+ /**
+ * Converts a Pose2d to a Transform2d to be used in a kinematic chain
+ *
+ * @param pose The pose that will represent the transform
+ * @return The resulting transform
+ */
+ public static Transform2d toTransform2d(Pose2d pose) {
+ return new Transform2d(pose.getTranslation(), pose.getRotation());
+ }
+
+ /**
+ * Checks if a pose is (0, 0, 0)
+ *
+ * @param pose The pose to check
+ * @return Whether the pose is zero
+ */
+ public static boolean isZero(Pose2d pose) {
+ return pose.getX() == 0.0 && pose.getY() == 0.0 && pose.getRotation().getDegrees() == 0.0;
+ }
+
+ /**
+ * Checks if any poses are (0, 0, 0)
+ *
+ * @param poses The poses to check
+ * @return Whether any poses are zero
+ */
+ public static boolean isZero(Pose2d[] poses) {
+ for (Pose2d p : poses) {
+ if (isZero(p)) {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ /**
+ * Checks if a translation is (0, 0)
+ *
+ * @param translation The translation to check
+ * @return Whether the translation is zero
+ */
+ public static boolean isZero(Translation2d translation) {
+ return translation.getX() == 0.0 && translation.getY() == 0.0;
+ }
+
+ /**
+ * Checks if a rotation is zero
+ *
+ * @param rotation The rotation to check
+ * @return Whether the rotation is zero
+ */
+ public static boolean isZero(Rotation2d rotation) {
+ return rotation.getDegrees() == 0.0;
+ }
+
+ /**
+ * Checks if a pose is NaN
+ *
+ * @param pose The pose to check
+ * @return Whether the pose is NaN
+ */
+ public static boolean isNaN(Pose2d pose) {
+ return Double.isNaN(pose.getX())
+ || Double.isNaN(pose.getY())
+ || Double.isNaN(pose.getRotation().getDegrees());
+ }
+
+ /**
+ * Checks if any poses are NaN
+ *
+ * @param poses The poses to check
+ * @return Whether any poses are NaN
+ */
+ public static boolean isNaN(Pose2d[] poses) {
+ for (Pose2d p : poses) {
+ if (isNaN(p)) {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ /**
+ * @param rectangle2ds Array of rectangles to check if the pose is in
+ * @param pose The pose to check is contained by the rectangle
+ * @return Whether the pose is contained by the rectangle
+ */
+ public static boolean contains(Rectangle2d[] rectangle2ds, Pose2d pose) {
+ for (Rectangle2d rectangle : rectangle2ds) {
+ if (rectangle.contains(pose.getTranslation())) {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ /**
+ * @param rectangle2ds Array of rectangles to check against
+ * @param pose Center of the target rectangle
+ * @param x Full width of the target rectangle
+ * @param y Full height of the target rectangle
+ * @return Whether ANY part of the target rectangle is inside any rectangle
+ */
+ public static boolean intersects(Rectangle2d[] rectangle2ds, Pose2d pose, double x, double y) {
+ Rectangle2d target = new Rectangle2d(pose, x, y);
+
+ double hx = x / 2.0;
+ double hy = y / 2.0;
+
+ // Target corners (correctly rotated)
+ Translation2d[] corners =
+ new Translation2d[] {
+ new Translation2d(hx, hy),
+ new Translation2d(-hx, hy),
+ new Translation2d(-hx, -hy),
+ new Translation2d(hx, -hy)
+ };
+
+ Translation2d center = pose.getTranslation();
+ Rotation2d rot = pose.getRotation();
+
+ for (Rectangle2d rectangle : rectangle2ds) {
+
+ for (Translation2d corner : corners) {
+ Translation2d worldCorner = center.plus(corner.rotateBy(rot));
+ if (rectangle.contains(worldCorner)) {
+ return true;
+ }
+ }
+
+ if (rectangle.contains(center)) {
+ return true;
+ }
+
+ if (target.contains(rectangle.getCenter().getTranslation())) {
+ return true;
+ }
+ }
+
+ return false;
+ }
+
+ /**
+ * @param rectangle2ds Array of rectangles to check if the translation is in
+ * @param translation2d The translation to check is contained by the rectangle
+ * @return Whether the translation is contained by the rectangle
+ */
+ public static boolean contains(Rectangle2d[] rectangle2ds, Translation2d translation2d) {
+ for (Rectangle2d rectangle : rectangle2ds) {
+ if (rectangle.contains(translation2d)) {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ /**
+ * Gets the center and corners of a rectangle2d
+ *
+ * @param rectangle2d The rectangle2d to get the center and corners of
+ * @return The poses from the rectangle2d
+ */
+ public static Pose2d[] rectanglePose2ds(Rectangle2d rectangle2d) {
+ Pose2d[] poses = new Pose2d[5];
+ poses[0] =
+ rectangle2d
+ .getCenter()
+ .transformBy(
+ new Transform2d(
+ rectangle2d.getXWidth(), rectangle2d.getYWidth(), new Rotation2d()));
+ poses[1] =
+ rectangle2d
+ .getCenter()
+ .transformBy(
+ new Transform2d(
+ -rectangle2d.getXWidth(), rectangle2d.getYWidth(), new Rotation2d()));
+ poses[2] =
+ rectangle2d
+ .getCenter()
+ .transformBy(
+ new Transform2d(
+ -rectangle2d.getXWidth(), -rectangle2d.getYWidth(), new Rotation2d()));
+ poses[3] =
+ rectangle2d
+ .getCenter()
+ .transformBy(
+ new Transform2d(
+ rectangle2d.getXWidth(), -rectangle2d.getYWidth(), new Rotation2d()));
+ poses[4] = rectangle2d.getCenter();
+ return poses;
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/LimelightHelpers.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/LimelightHelpers.java
new file mode 100644
index 00000000..652638a7
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/LimelightHelpers.java
@@ -0,0 +1,1987 @@
+// LimelightHelpers v1.14 (REQUIRES LLOS 2026.0 OR LATER)
+
+package edu.wpi.team190.gompeilib.core.utility;
+
+import com.fasterxml.jackson.annotation.JsonFormat;
+import com.fasterxml.jackson.annotation.JsonFormat.Shape;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.DeserializationFeature;
+import com.fasterxml.jackson.databind.ObjectMapper;
+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.math.util.Units;
+import edu.wpi.first.net.PortForwarder;
+import edu.wpi.first.networktables.DoubleArrayEntry;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.TimestampedDoubleArray;
+import java.util.Arrays;
+import java.util.Map;
+import java.util.concurrent.ConcurrentHashMap;
+
+/**
+ * LimelightHelpers provides static methods and classes for interfacing with Limelight vision
+ * cameras in FRC. This library supports all Limelight features including AprilTag tracking, Neural
+ * Networks, and standard color/retroreflective tracking.
+ */
+public class LimelightHelpers {
+
+ private static final Map doubleArrayEntries = new ConcurrentHashMap<>();
+
+ /** Represents a Color/Retroreflective Target Result extracted from JSON Output */
+ public static class LimelightTarget_Retro {
+
+ @JsonProperty("t6c_ts")
+ private double[] cameraPose_TargetSpace;
+
+ @JsonProperty("t6r_fs")
+ private double[] robotPose_FieldSpace;
+
+ @JsonProperty("t6r_ts")
+ private double[] robotPose_TargetSpace;
+
+ @JsonProperty("t6t_cs")
+ private double[] targetPose_CameraSpace;
+
+ @JsonProperty("t6t_rs")
+ private double[] targetPose_RobotSpace;
+
+ public Pose3d getCameraPose_TargetSpace() {
+ return toPose3D(cameraPose_TargetSpace);
+ }
+
+ public Pose3d getRobotPose_FieldSpace() {
+ return toPose3D(robotPose_FieldSpace);
+ }
+
+ public Pose3d getRobotPose_TargetSpace() {
+ return toPose3D(robotPose_TargetSpace);
+ }
+
+ public Pose3d getTargetPose_CameraSpace() {
+ return toPose3D(targetPose_CameraSpace);
+ }
+
+ public Pose3d getTargetPose_RobotSpace() {
+ return toPose3D(targetPose_RobotSpace);
+ }
+
+ public Pose2d getCameraPose_TargetSpace2D() {
+ return toPose2D(cameraPose_TargetSpace);
+ }
+
+ public Pose2d getRobotPose_FieldSpace2D() {
+ return toPose2D(robotPose_FieldSpace);
+ }
+
+ public Pose2d getRobotPose_TargetSpace2D() {
+ return toPose2D(robotPose_TargetSpace);
+ }
+
+ public Pose2d getTargetPose_CameraSpace2D() {
+ return toPose2D(targetPose_CameraSpace);
+ }
+
+ public Pose2d getTargetPose_RobotSpace2D() {
+ return toPose2D(targetPose_RobotSpace);
+ }
+
+ @JsonProperty("ta")
+ public double ta;
+
+ @JsonProperty("tx")
+ public double tx;
+
+ @JsonProperty("ty")
+ public double ty;
+
+ @JsonProperty("txp")
+ public double tx_pixels;
+
+ @JsonProperty("typ")
+ public double ty_pixels;
+
+ @JsonProperty("tx_nocross")
+ public double tx_nocrosshair;
+
+ @JsonProperty("ty_nocross")
+ public double ty_nocrosshair;
+
+ @JsonProperty("ts")
+ public double ts;
+
+ public LimelightTarget_Retro() {
+ cameraPose_TargetSpace = new double[6];
+ robotPose_FieldSpace = new double[6];
+ robotPose_TargetSpace = new double[6];
+ targetPose_CameraSpace = new double[6];
+ targetPose_RobotSpace = new double[6];
+ }
+ }
+
+ /** Represents an AprilTag/Fiducial Target Result extracted from JSON Output */
+ public static class LimelightTarget_Fiducial {
+
+ @JsonProperty("fID")
+ public double fiducialID;
+
+ @JsonProperty("fam")
+ public String fiducialFamily;
+
+ @JsonProperty("t6c_ts")
+ private double[] cameraPose_TargetSpace;
+
+ @JsonProperty("t6r_fs")
+ private double[] robotPose_FieldSpace;
+
+ @JsonProperty("t6r_ts")
+ private double[] robotPose_TargetSpace;
+
+ @JsonProperty("t6t_cs")
+ private double[] targetPose_CameraSpace;
+
+ @JsonProperty("t6t_rs")
+ private double[] targetPose_RobotSpace;
+
+ public Pose3d getCameraPose_TargetSpace() {
+ return toPose3D(cameraPose_TargetSpace);
+ }
+
+ public Pose3d getRobotPose_FieldSpace() {
+ return toPose3D(robotPose_FieldSpace);
+ }
+
+ public Pose3d getRobotPose_TargetSpace() {
+ return toPose3D(robotPose_TargetSpace);
+ }
+
+ public Pose3d getTargetPose_CameraSpace() {
+ return toPose3D(targetPose_CameraSpace);
+ }
+
+ public Pose3d getTargetPose_RobotSpace() {
+ return toPose3D(targetPose_RobotSpace);
+ }
+
+ public Pose2d getCameraPose_TargetSpace2D() {
+ return toPose2D(cameraPose_TargetSpace);
+ }
+
+ public Pose2d getRobotPose_FieldSpace2D() {
+ return toPose2D(robotPose_FieldSpace);
+ }
+
+ public Pose2d getRobotPose_TargetSpace2D() {
+ return toPose2D(robotPose_TargetSpace);
+ }
+
+ public Pose2d getTargetPose_CameraSpace2D() {
+ return toPose2D(targetPose_CameraSpace);
+ }
+
+ public Pose2d getTargetPose_RobotSpace2D() {
+ return toPose2D(targetPose_RobotSpace);
+ }
+
+ @JsonProperty("ta")
+ public double ta;
+
+ @JsonProperty("tx")
+ public double tx;
+
+ @JsonProperty("ty")
+ public double ty;
+
+ @JsonProperty("txp")
+ public double tx_pixels;
+
+ @JsonProperty("typ")
+ public double ty_pixels;
+
+ @JsonProperty("tx_nocross")
+ public double tx_nocrosshair;
+
+ @JsonProperty("ty_nocross")
+ public double ty_nocrosshair;
+
+ @JsonProperty("ts")
+ public double ts;
+
+ public LimelightTarget_Fiducial() {
+ cameraPose_TargetSpace = new double[6];
+ robotPose_FieldSpace = new double[6];
+ robotPose_TargetSpace = new double[6];
+ targetPose_CameraSpace = new double[6];
+ targetPose_RobotSpace = new double[6];
+ }
+ }
+
+ /** Represents a Barcode Target Result extracted from JSON Output */
+ public static class LimelightTarget_Barcode {
+
+ /** Barcode family type (e.g. "QR", "DataMatrix", etc.) */
+ @JsonProperty("fam")
+ public String family;
+
+ /** Gets the decoded data content of the barcode */
+ @JsonProperty("data")
+ public String data;
+
+ @JsonProperty("txp")
+ public double tx_pixels;
+
+ @JsonProperty("typ")
+ public double ty_pixels;
+
+ @JsonProperty("tx")
+ public double tx;
+
+ @JsonProperty("ty")
+ public double ty;
+
+ @JsonProperty("tx_nocross")
+ public double tx_nocrosshair;
+
+ @JsonProperty("ty_nocross")
+ public double ty_nocrosshair;
+
+ @JsonProperty("ta")
+ public double ta;
+
+ @JsonProperty("pts")
+ public double[][] corners;
+
+ public LimelightTarget_Barcode() {}
+
+ public String getFamily() {
+ return family;
+ }
+ }
+
+ /** Represents a Neural Classifier Pipeline Result extracted from JSON Output */
+ public static class LimelightTarget_Classifier {
+
+ @JsonProperty("class")
+ public String className;
+
+ @JsonProperty("classID")
+ public double classID;
+
+ @JsonProperty("conf")
+ public double confidence;
+
+ @JsonProperty("zone")
+ public double zone;
+
+ @JsonProperty("tx")
+ public double tx;
+
+ @JsonProperty("txp")
+ public double tx_pixels;
+
+ @JsonProperty("ty")
+ public double ty;
+
+ @JsonProperty("typ")
+ public double ty_pixels;
+
+ public LimelightTarget_Classifier() {}
+ }
+
+ /** Represents a Neural Detector Pipeline Result extracted from JSON Output */
+ public static class LimelightTarget_Detector {
+
+ @JsonProperty("class")
+ public String className;
+
+ @JsonProperty("classID")
+ public double classID;
+
+ @JsonProperty("conf")
+ public double confidence;
+
+ @JsonProperty("ta")
+ public double ta;
+
+ @JsonProperty("tx")
+ public double tx;
+
+ @JsonProperty("ty")
+ public double ty;
+
+ @JsonProperty("txp")
+ public double tx_pixels;
+
+ @JsonProperty("typ")
+ public double ty_pixels;
+
+ @JsonProperty("tx_nocross")
+ public double tx_nocrosshair;
+
+ @JsonProperty("ty_nocross")
+ public double ty_nocrosshair;
+
+ public LimelightTarget_Detector() {}
+ }
+
+ /** Represents hardware statistics from the Limelight. */
+ public static class HardwareReport {
+ @JsonProperty("cid")
+ public String cameraId;
+
+ @JsonProperty("cpu")
+ public double cpuUsage;
+
+ @JsonProperty("dfree")
+ public double diskFree;
+
+ @JsonProperty("dtot")
+ public double diskTotal;
+
+ @JsonProperty("ram")
+ public double ramUsage;
+
+ @JsonProperty("temp")
+ public double temperature;
+
+ public HardwareReport() {}
+ }
+
+ /** Represents IMU data from the JSON results. */
+ public static class IMUResults {
+ @JsonProperty("data")
+ public double[] data;
+
+ @JsonProperty("quat")
+ public double[] quaternion;
+
+ @JsonProperty("yaw")
+ public double yaw;
+
+ // Parsed from data array
+ public double robotYaw;
+ public double roll;
+ public double pitch;
+ public double rawYaw;
+ public double gyroZ;
+ public double gyroX;
+ public double gyroY;
+ public double accelZ;
+ public double accelX;
+ public double accelY;
+
+ public IMUResults() {
+ data = new double[0];
+ quaternion = new double[4];
+ }
+
+ public void parseDataArray() {
+ if (data != null && data.length >= 10) {
+ robotYaw = data[0];
+ roll = data[1];
+ pitch = data[2];
+ rawYaw = data[3];
+ gyroZ = data[4];
+ gyroX = data[5];
+ gyroY = data[6];
+ accelZ = data[7];
+ accelX = data[8];
+ accelY = data[9];
+ }
+ }
+ }
+
+ /** Represents capture rewind buffer statistics. */
+ public static class RewindStats {
+ @JsonProperty("bufferUsage")
+ public double bufferUsage;
+
+ @JsonProperty("enabled")
+ public int enabled;
+
+ @JsonProperty("flushing")
+ public int flushing;
+
+ @JsonProperty("frameCount")
+ public int frameCount;
+
+ @JsonProperty("latpen")
+ public int latencyPenalty;
+
+ @JsonProperty("storedSeconds")
+ public double storedSeconds;
+
+ public RewindStats() {}
+ }
+
+ /** Limelight Results object, parsed from a Limelight's JSON results output. */
+ public static class LimelightResults {
+
+ public String error;
+
+ @JsonProperty("pID")
+ public double pipelineID;
+
+ @JsonProperty("tl")
+ public double latency_pipeline;
+
+ @JsonProperty("cl")
+ public double latency_capture;
+
+ public double latency_jsonParse;
+
+ @JsonProperty("ts")
+ public double timestamp_LIMELIGHT_publish;
+
+ @JsonProperty("ts_rio")
+ public double timestamp_RIOFPGA_capture;
+
+ @JsonProperty("ts_nt")
+ public long timestamp_nt;
+
+ @JsonProperty("ts_sys")
+ public long timestamp_sys;
+
+ @JsonProperty("ts_us")
+ public long timestamp_us;
+
+ @JsonProperty("v")
+ @JsonFormat(shape = Shape.NUMBER)
+ public boolean valid;
+
+ @JsonProperty("pTYPE")
+ public String pipelineType;
+
+ @JsonProperty("tx")
+ public double tx;
+
+ @JsonProperty("ty")
+ public double ty;
+
+ @JsonProperty("txnc")
+ public double tx_nocrosshair;
+
+ @JsonProperty("tync")
+ public double ty_nocrosshair;
+
+ @JsonProperty("ta")
+ public double ta;
+
+ @JsonProperty("botpose")
+ public double[] botpose;
+
+ @JsonProperty("botpose_wpired")
+ public double[] botpose_wpired;
+
+ @JsonProperty("botpose_wpiblue")
+ public double[] botpose_wpiblue;
+
+ @JsonProperty("botpose_tagcount")
+ public double botpose_tagcount;
+
+ @JsonProperty("botpose_span")
+ public double botpose_span;
+
+ @JsonProperty("botpose_avgdist")
+ public double botpose_avgdist;
+
+ @JsonProperty("botpose_avgarea")
+ public double botpose_avgarea;
+
+ @JsonProperty("botpose_orb")
+ public double[] botpose_orb;
+
+ @JsonProperty("botpose_orb_wpiblue")
+ public double[] botpose_orb_wpiblue;
+
+ @JsonProperty("botpose_orb_wpired")
+ public double[] botpose_orb_wpired;
+
+ @JsonProperty("t6c_rs")
+ public double[] camerapose_robotspace;
+
+ @JsonProperty("hw")
+ public HardwareReport hardware;
+
+ @JsonProperty("imu")
+ public IMUResults imuResults;
+
+ @JsonProperty("rewind")
+ public RewindStats rewindStats;
+
+ @JsonProperty("PythonOut")
+ public double[] pythonOutput;
+
+ public Pose3d getBotPose3d() {
+ return toPose3D(botpose);
+ }
+
+ public Pose3d getBotPose3d_wpiRed() {
+ return toPose3D(botpose_wpired);
+ }
+
+ public Pose3d getBotPose3d_wpiBlue() {
+ return toPose3D(botpose_wpiblue);
+ }
+
+ public Pose2d getBotPose2d() {
+ return toPose2D(botpose);
+ }
+
+ public Pose2d getBotPose2d_wpiRed() {
+ return toPose2D(botpose_wpired);
+ }
+
+ public Pose2d getBotPose2d_wpiBlue() {
+ return toPose2D(botpose_wpiblue);
+ }
+
+ @JsonProperty("Retro")
+ public LimelightTarget_Retro[] targets_Retro;
+
+ @JsonProperty("Fiducial")
+ public LimelightTarget_Fiducial[] targets_Fiducials;
+
+ @JsonProperty("Classifier")
+ public LimelightTarget_Classifier[] targets_Classifier;
+
+ @JsonProperty("Detector")
+ public LimelightTarget_Detector[] targets_Detector;
+
+ @JsonProperty("Barcode")
+ public LimelightTarget_Barcode[] targets_Barcode;
+
+ public LimelightResults() {
+ botpose = new double[6];
+ botpose_wpired = new double[6];
+ botpose_wpiblue = new double[6];
+ botpose_orb = new double[6];
+ botpose_orb_wpiblue = new double[6];
+ botpose_orb_wpired = new double[6];
+ camerapose_robotspace = new double[6];
+ targets_Retro = new LimelightTarget_Retro[0];
+ targets_Fiducials = new LimelightTarget_Fiducial[0];
+ targets_Classifier = new LimelightTarget_Classifier[0];
+ targets_Detector = new LimelightTarget_Detector[0];
+ targets_Barcode = new LimelightTarget_Barcode[0];
+ pythonOutput = new double[0];
+ pipelineType = "";
+ }
+ }
+
+ /** Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. */
+ public static class RawFiducial {
+ public int id = 0;
+ public double txnc = 0;
+ public double tync = 0;
+ public double ta = 0;
+ public double distToCamera = 0;
+ public double distToRobot = 0;
+ public double ambiguity = 0;
+
+ public RawFiducial(
+ int id,
+ double txnc,
+ double tync,
+ double ta,
+ double distToCamera,
+ double distToRobot,
+ double ambiguity) {
+ this.id = id;
+ this.txnc = txnc;
+ this.tync = tync;
+ this.ta = ta;
+ this.distToCamera = distToCamera;
+ this.distToRobot = distToRobot;
+ this.ambiguity = ambiguity;
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (this == obj) return true;
+ if (obj == null || getClass() != obj.getClass()) return false;
+ RawFiducial other = (RawFiducial) obj;
+ return id == other.id
+ && Double.compare(txnc, other.txnc) == 0
+ && Double.compare(tync, other.tync) == 0
+ && Double.compare(ta, other.ta) == 0
+ && Double.compare(distToCamera, other.distToCamera) == 0
+ && Double.compare(distToRobot, other.distToRobot) == 0
+ && Double.compare(ambiguity, other.ambiguity) == 0;
+ }
+ }
+
+ /** Represents a Limelight Raw Target/Contour result from Limelight's NetworkTables output. */
+ public static class RawTarget {
+ public double txnc = 0;
+ public double tync = 0;
+ public double ta = 0;
+
+ public RawTarget(double txnc, double tync, double ta) {
+ this.txnc = txnc;
+ this.tync = tync;
+ this.ta = ta;
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (this == obj) return true;
+ if (obj == null || getClass() != obj.getClass()) return false;
+ RawTarget other = (RawTarget) obj;
+ return Double.compare(txnc, other.txnc) == 0
+ && Double.compare(tync, other.tync) == 0
+ && Double.compare(ta, other.ta) == 0;
+ }
+ }
+
+ /** Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. */
+ public static class RawDetection {
+ public int classId = 0;
+ public double txnc = 0;
+ public double tync = 0;
+ public double ta = 0;
+ public double corner0_X = 0;
+ public double corner0_Y = 0;
+ public double corner1_X = 0;
+ public double corner1_Y = 0;
+ public double corner2_X = 0;
+ public double corner2_Y = 0;
+ public double corner3_X = 0;
+ public double corner3_Y = 0;
+
+ public RawDetection(
+ int classId,
+ double txnc,
+ double tync,
+ double ta,
+ double corner0_X,
+ double corner0_Y,
+ double corner1_X,
+ double corner1_Y,
+ double corner2_X,
+ double corner2_Y,
+ double corner3_X,
+ double corner3_Y) {
+ this.classId = classId;
+ this.txnc = txnc;
+ this.tync = tync;
+ this.ta = ta;
+ this.corner0_X = corner0_X;
+ this.corner0_Y = corner0_Y;
+ this.corner1_X = corner1_X;
+ this.corner1_Y = corner1_Y;
+ this.corner2_X = corner2_X;
+ this.corner2_Y = corner2_Y;
+ this.corner3_X = corner3_X;
+ this.corner3_Y = corner3_Y;
+ }
+ }
+
+ /** Represents a 3D Pose Estimate. */
+ public static class PoseEstimate {
+ public Pose2d pose;
+ public double timestampSeconds;
+ public double latency;
+ public int tagCount;
+ public double tagSpan;
+ public double avgTagDist;
+ public double avgTagArea;
+
+ public RawFiducial[] rawFiducials;
+ public boolean isMegaTag2;
+
+ /** Instantiates a PoseEstimate object with default values */
+ public PoseEstimate() {
+ this.pose = new Pose2d();
+ this.timestampSeconds = 0;
+ this.latency = 0;
+ this.tagCount = 0;
+ this.tagSpan = 0;
+ this.avgTagDist = 0;
+ this.avgTagArea = 0;
+ this.rawFiducials = new RawFiducial[] {};
+ this.isMegaTag2 = false;
+ }
+
+ public PoseEstimate(
+ Pose2d pose,
+ double timestampSeconds,
+ double latency,
+ int tagCount,
+ double tagSpan,
+ double avgTagDist,
+ double avgTagArea,
+ RawFiducial[] rawFiducials,
+ boolean isMegaTag2) {
+
+ this.pose = pose;
+ this.timestampSeconds = timestampSeconds;
+ this.latency = latency;
+ this.tagCount = tagCount;
+ this.tagSpan = tagSpan;
+ this.avgTagDist = avgTagDist;
+ this.avgTagArea = avgTagArea;
+ this.rawFiducials = rawFiducials;
+ this.isMegaTag2 = isMegaTag2;
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (this == obj) return true;
+ if (obj == null || getClass() != obj.getClass()) return false;
+ PoseEstimate that = (PoseEstimate) obj;
+ // We don't compare the timestampSeconds as it isn't relevant for equality and makes
+ // unit testing harder
+ return Double.compare(that.latency, latency) == 0
+ && tagCount == that.tagCount
+ && Double.compare(that.tagSpan, tagSpan) == 0
+ && Double.compare(that.avgTagDist, avgTagDist) == 0
+ && Double.compare(that.avgTagArea, avgTagArea) == 0
+ && pose.equals(that.pose)
+ && Arrays.equals(rawFiducials, that.rawFiducials);
+ }
+ }
+
+ /** Encapsulates the state of an internal Limelight IMU. */
+ public static class IMUData {
+ public double robotYaw = 0.0;
+ public double Roll = 0.0;
+ public double Pitch = 0.0;
+ public double Yaw = 0.0;
+ public double gyroX = 0.0;
+ public double gyroY = 0.0;
+ public double gyroZ = 0.0;
+ public double accelX = 0.0;
+ public double accelY = 0.0;
+ public double accelZ = 0.0;
+
+ public IMUData() {}
+
+ public IMUData(double[] imuData) {
+ if (imuData != null && imuData.length >= 10) {
+ this.robotYaw = imuData[0];
+ this.Roll = imuData[1];
+ this.Pitch = imuData[2];
+ this.Yaw = imuData[3];
+ this.gyroX = imuData[4];
+ this.gyroY = imuData[5];
+ this.gyroZ = imuData[6];
+ this.accelX = imuData[7];
+ this.accelY = imuData[8];
+ this.accelZ = imuData[9];
+ }
+ }
+ }
+
+ private static ObjectMapper mapper;
+
+ /** Print JSON Parse time to the console in milliseconds */
+ static boolean profileJSON = false;
+
+ static final String sanitizeName(String name) {
+ if ("".equals(name) || name == null) {
+ return "limelight";
+ }
+ return name;
+ }
+
+ /**
+ * Takes a 6-length array of pose data and converts it to a Pose3d object. Array format: [x, y, z,
+ * roll, pitch, yaw] where angles are in degrees.
+ *
+ * @param inData Array containing pose data [x, y, z, roll, pitch, yaw]
+ * @return Pose3d object representing the pose, or empty Pose3d if invalid data
+ */
+ public static Pose3d toPose3D(double[] inData) {
+ if (inData.length < 6) {
+ // System.err.println("Bad LL 3D Pose Data!");
+ return new Pose3d();
+ }
+ return new Pose3d(
+ new Translation3d(inData[0], inData[1], inData[2]),
+ new Rotation3d(
+ Units.degreesToRadians(inData[3]),
+ Units.degreesToRadians(inData[4]),
+ Units.degreesToRadians(inData[5])));
+ }
+
+ /**
+ * Takes a 6-length array of pose data and converts it to a Pose2d object. Uses only x, y, and yaw
+ * components, ignoring z, roll, and pitch. Array format: [x, y, z, roll, pitch, yaw] where angles
+ * are in degrees.
+ *
+ * @param inData Array containing pose data [x, y, z, roll, pitch, yaw]
+ * @return Pose2d object representing the pose, or empty Pose2d if invalid data
+ */
+ public static Pose2d toPose2D(double[] inData) {
+ if (inData.length < 6) {
+ // System.err.println("Bad LL 2D Pose Data!");
+ return new Pose2d();
+ }
+ Translation2d tran2d = new Translation2d(inData[0], inData[1]);
+ Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5]));
+ return new Pose2d(tran2d, r2d);
+ }
+
+ /**
+ * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw].
+ * Translation components are in meters, rotation components are in degrees.
+ *
+ * @param pose The Pose3d object to convert
+ * @return A 6-element array containing [x, y, z, roll, pitch, yaw]
+ */
+ public static double[] pose3dToArray(Pose3d pose) {
+ double[] result = new double[6];
+ result[0] = pose.getTranslation().getX();
+ result[1] = pose.getTranslation().getY();
+ result[2] = pose.getTranslation().getZ();
+ result[3] = Units.radiansToDegrees(pose.getRotation().getX());
+ result[4] = Units.radiansToDegrees(pose.getRotation().getY());
+ result[5] = Units.radiansToDegrees(pose.getRotation().getZ());
+ return result;
+ }
+
+ /**
+ * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw].
+ * Translation components are in meters, rotation components are in degrees. Note: z, roll, and
+ * pitch will be 0 since Pose2d only contains x, y, and yaw.
+ *
+ * @param pose The Pose2d object to convert
+ * @return A 6-element array containing [x, y, 0, 0, 0, yaw]
+ */
+ public static double[] pose2dToArray(Pose2d pose) {
+ double[] result = new double[6];
+ result[0] = pose.getTranslation().getX();
+ result[1] = pose.getTranslation().getY();
+ result[2] = 0;
+ result[3] = Units.radiansToDegrees(0);
+ result[4] = Units.radiansToDegrees(0);
+ result[5] = Units.radiansToDegrees(pose.getRotation().getRadians());
+ return result;
+ }
+
+ private static double extractArrayEntry(double[] inData, int position) {
+ if (inData.length < position + 1) {
+ return 0;
+ }
+ return inData[position];
+ }
+
+ private static PoseEstimate getBotPoseEstimate(
+ String limelightName, String entryName, boolean isMegaTag2) {
+ DoubleArrayEntry poseEntry =
+ LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName);
+
+ TimestampedDoubleArray tsValue = poseEntry.getAtomic();
+ double[] poseArray = tsValue.value;
+ long timestamp = tsValue.timestamp;
+
+ if (poseArray.length == 0) {
+ // Handle the case where no data is available
+ return new PoseEstimate();
+ }
+
+ var pose = toPose2D(poseArray);
+ double latency = extractArrayEntry(poseArray, 6);
+ int tagCount = (int) extractArrayEntry(poseArray, 7);
+ double tagSpan = extractArrayEntry(poseArray, 8);
+ double tagDist = extractArrayEntry(poseArray, 9);
+ double tagArea = extractArrayEntry(poseArray, 10);
+
+ // Convert server timestamp from microseconds to seconds and adjust for latency
+ double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0);
+
+ int valsPerFiducial = 7;
+ int expectedTotalVals = 11 + valsPerFiducial * tagCount;
+ RawFiducial[] rawFiducials;
+
+ if (poseArray.length != expectedTotalVals) {
+ // Array size mismatch - return empty array instead of null-filled array
+ rawFiducials = new RawFiducial[0];
+ } else {
+ rawFiducials = new RawFiducial[tagCount];
+ for (int i = 0; i < tagCount; i++) {
+ int baseIndex = 11 + (i * valsPerFiducial);
+ int id = (int) poseArray[baseIndex];
+ double txnc = poseArray[baseIndex + 1];
+ double tync = poseArray[baseIndex + 2];
+ double ta = poseArray[baseIndex + 3];
+ double distToCamera = poseArray[baseIndex + 4];
+ double distToRobot = poseArray[baseIndex + 5];
+ double ambiguity = poseArray[baseIndex + 6];
+ rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity);
+ }
+ }
+
+ return new PoseEstimate(
+ pose,
+ adjustedTimestamp,
+ latency,
+ tagCount,
+ tagSpan,
+ tagDist,
+ tagArea,
+ rawFiducials,
+ isMegaTag2);
+ }
+
+ /**
+ * Gets the latest raw fiducial/AprilTag detection results from NetworkTables.
+ *
+ * @param limelightName Name/identifier of the Limelight
+ * @return Array of RawFiducial objects containing detection details
+ */
+ public static RawFiducial[] getRawFiducials(String limelightName) {
+ var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials");
+ var rawFiducialArray = entry.getDoubleArray(new double[0]);
+ int valsPerEntry = 7;
+ if (rawFiducialArray.length % valsPerEntry != 0) {
+ return new RawFiducial[0];
+ }
+
+ int numFiducials = rawFiducialArray.length / valsPerEntry;
+ RawFiducial[] rawFiducials = new RawFiducial[numFiducials];
+
+ for (int i = 0; i < numFiducials; i++) {
+ int baseIndex = i * valsPerEntry;
+ int id = (int) extractArrayEntry(rawFiducialArray, baseIndex);
+ double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1);
+ double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2);
+ double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3);
+ double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4);
+ double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5);
+ double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6);
+
+ rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity);
+ }
+
+ return rawFiducials;
+ }
+
+ /**
+ * Gets the latest raw neural detector results from NetworkTables
+ *
+ * @param limelightName Name/identifier of the Limelight
+ * @return Array of RawDetection objects containing detection details
+ */
+ public static RawDetection[] getRawDetections(String limelightName) {
+ var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections");
+ var rawDetectionArray = entry.getDoubleArray(new double[0]);
+ int valsPerEntry = 12;
+ if (rawDetectionArray.length % valsPerEntry != 0) {
+ return new RawDetection[0];
+ }
+
+ int numDetections = rawDetectionArray.length / valsPerEntry;
+ RawDetection[] rawDetections = new RawDetection[numDetections];
+
+ for (int i = 0; i < numDetections; i++) {
+ int baseIndex = i * valsPerEntry; // Starting index for this detection's data
+ int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex);
+ double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1);
+ double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2);
+ double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3);
+ double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4);
+ double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5);
+ double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6);
+ double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7);
+ double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8);
+ double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9);
+ double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10);
+ double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11);
+
+ rawDetections[i] =
+ new RawDetection(
+ classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X,
+ corner2_Y, corner3_X, corner3_Y);
+ }
+
+ return rawDetections;
+ }
+
+ /**
+ * Gets the raw target contours from NetworkTables. Returns ungrouped contours in normalized
+ * screen space (-1 to 1).
+ *
+ * @param limelightName Name/identifier of the Limelight
+ * @return Array of RawTarget objects containing up to 3 contours
+ */
+ public static RawTarget[] getRawTargets(String limelightName) {
+ var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawtargets");
+ var rawTargetArray = entry.getDoubleArray(new double[0]);
+ int valsPerEntry = 3;
+ if (rawTargetArray.length % valsPerEntry != 0) {
+ return new RawTarget[0];
+ }
+
+ int numTargets = rawTargetArray.length / valsPerEntry;
+ RawTarget[] rawTargets = new RawTarget[numTargets];
+
+ for (int i = 0; i < numTargets; i++) {
+ int baseIndex = i * valsPerEntry;
+ double txnc = extractArrayEntry(rawTargetArray, baseIndex);
+ double tync = extractArrayEntry(rawTargetArray, baseIndex + 1);
+ double ta = extractArrayEntry(rawTargetArray, baseIndex + 2);
+
+ rawTargets[i] = new RawTarget(txnc, tync, ta);
+ }
+
+ return rawTargets;
+ }
+
+ /**
+ * Gets the corner coordinates of detected targets from NetworkTables. Requires "send contours" to
+ * be enabled in the Limelight Output tab.
+ *
+ * @param limelightName Name/identifier of the Limelight
+ * @return Array of doubles containing corner coordinates [x0, y0, x1, y1, ...]
+ */
+ public static double[] getCornerCoordinates(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "tcornxy");
+ }
+
+ /**
+ * Prints detailed information about a PoseEstimate to standard output. Includes timestamp,
+ * latency, tag count, tag span, average tag distance, average tag area, and detailed information
+ * about each detected fiducial.
+ *
+ * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available."
+ */
+ public static void printPoseEstimate(PoseEstimate pose) {
+ if (pose == null) {
+ System.out.println("No PoseEstimate available.");
+ return;
+ }
+
+ System.out.printf("Pose Estimate Information:%n");
+ System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds);
+ System.out.printf("Latency: %.3f ms%n", pose.latency);
+ System.out.printf("Tag Count: %d%n", pose.tagCount);
+ System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan);
+ System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist);
+ System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea);
+ System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2);
+ System.out.println();
+
+ if (pose.rawFiducials == null || pose.rawFiducials.length == 0) {
+ System.out.println("No RawFiducials data available.");
+ return;
+ }
+
+ System.out.println("Raw Fiducials Details:");
+ for (int i = 0; i < pose.rawFiducials.length; i++) {
+ RawFiducial fiducial = pose.rawFiducials[i];
+ System.out.printf(" Fiducial #%d:%n", i + 1);
+ System.out.printf(" ID: %d%n", fiducial.id);
+ System.out.printf(" TXNC: %.2f%n", fiducial.txnc);
+ System.out.printf(" TYNC: %.2f%n", fiducial.tync);
+ System.out.printf(" TA: %.2f%n", fiducial.ta);
+ System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera);
+ System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot);
+ System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity);
+ System.out.println();
+ }
+ }
+
+ public static Boolean validPoseEstimate(PoseEstimate pose) {
+ return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0;
+ }
+
+ public static NetworkTable getLimelightNTTable(String tableName) {
+ return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName));
+ }
+
+ public static void Flush() {
+ NetworkTableInstance.getDefault().flush();
+ }
+
+ public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) {
+ return getLimelightNTTable(tableName).getEntry(entryName);
+ }
+
+ public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) {
+ String key = tableName + "/" + entryName;
+ return doubleArrayEntries.computeIfAbsent(
+ key,
+ k -> {
+ NetworkTable table = getLimelightNTTable(tableName);
+ return table.getDoubleArrayTopic(entryName).getEntry(new double[0]);
+ });
+ }
+
+ public static double getLimelightNTDouble(String tableName, String entryName) {
+ return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0);
+ }
+
+ public static void setLimelightNTDouble(String tableName, String entryName, double val) {
+ getLimelightNTTableEntry(tableName, entryName).setDouble(val);
+ }
+
+ public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) {
+ getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val);
+ }
+
+ public static double[] getLimelightNTDoubleArray(String tableName, String entryName) {
+ return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]);
+ }
+
+ public static String getLimelightNTString(String tableName, String entryName) {
+ return getLimelightNTTableEntry(tableName, entryName).getString("");
+ }
+
+ public static String[] getLimelightNTStringArray(String tableName, String entryName) {
+ return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]);
+ }
+
+ /////
+
+ /**
+ * Does the Limelight have a valid target?
+ *
+ * @param limelightName Name of the Limelight camera ("" for default)
+ * @return True if a valid target is present, false otherwise
+ */
+ public static boolean getTV(String limelightName) {
+ return 1.0 == getLimelightNTDouble(limelightName, "tv");
+ }
+
+ /**
+ * Gets the horizontal offset from the crosshair to the target in degrees.
+ *
+ * @param limelightName Name of the Limelight camera ("" for default)
+ * @return Horizontal offset angle in degrees
+ */
+ public static double getTX(String limelightName) {
+ return getLimelightNTDouble(limelightName, "tx");
+ }
+
+ /**
+ * Gets the vertical offset from the crosshair to the target in degrees.
+ *
+ * @param limelightName Name of the Limelight camera ("" for default)
+ * @return Vertical offset angle in degrees
+ */
+ public static double getTY(String limelightName) {
+ return getLimelightNTDouble(limelightName, "ty");
+ }
+
+ /**
+ * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the
+ * most accurate 2d metric if you are using a calibrated camera and you don't need adjustable
+ * crosshair functionality.
+ *
+ * @param limelightName Name of the Limelight camera ("" for default)
+ * @return Horizontal offset angle in degrees
+ */
+ public static double getTXNC(String limelightName) {
+ return getLimelightNTDouble(limelightName, "txnc");
+ }
+
+ /**
+ * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the
+ * most accurate 2d metric if you are using a calibrated camera and you don't need adjustable
+ * crosshair functionality.
+ *
+ * @param limelightName Name of the Limelight camera ("" for default)
+ * @return Vertical offset angle in degrees
+ */
+ public static double getTYNC(String limelightName) {
+ return getLimelightNTDouble(limelightName, "tync");
+ }
+
+ /**
+ * Gets the target area as a percentage of the image (0-100%).
+ *
+ * @param limelightName Name of the Limelight camera ("" for default)
+ * @return Target area percentage (0-100)
+ */
+ public static double getTA(String limelightName) {
+ return getLimelightNTDouble(limelightName, "ta");
+ }
+
+ /**
+ * T2D is an array that contains several targeting metrcis
+ *
+ * @param limelightName Name of the Limelight camera
+ * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty,
+ * txnc, tync, ta, tid, targetClassIndexDetector, targetClassIndexClassifier,
+ * targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels,
+ * targetVerticalExtentPixels, targetSkewDegrees]
+ */
+ public static double[] getT2DArray(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "t2d");
+ }
+
+ /**
+ * Gets the number of targets currently detected.
+ *
+ * @param limelightName Name of the Limelight camera
+ * @return Number of detected targets
+ */
+ public static int getTargetCount(String limelightName) {
+ double[] t2d = getT2DArray(limelightName);
+ if (t2d.length == 17) {
+ return (int) t2d[1];
+ }
+ return 0;
+ }
+
+ /**
+ * Gets the classifier class index from the currently running neural classifier pipeline
+ *
+ * @param limelightName Name of the Limelight camera
+ * @return Class index from classifier pipeline
+ */
+ public static int getClassifierClassIndex(String limelightName) {
+ double[] t2d = getT2DArray(limelightName);
+ if (t2d.length == 17) {
+ return (int) t2d[11];
+ }
+ return 0;
+ }
+
+ /**
+ * Gets the detector class index from the primary result of the currently running neural detector
+ * pipeline.
+ *
+ * @param limelightName Name of the Limelight camera
+ * @return Class index from detector pipeline
+ */
+ public static int getDetectorClassIndex(String limelightName) {
+ double[] t2d = getT2DArray(limelightName);
+ if (t2d.length == 17) {
+ return (int) t2d[10];
+ }
+ return 0;
+ }
+
+ /**
+ * Gets the current neural classifier result class name.
+ *
+ * @param limelightName Name of the Limelight camera
+ * @return Class name string from classifier pipeline
+ */
+ public static String getClassifierClass(String limelightName) {
+ return getLimelightNTString(limelightName, "tcclass");
+ }
+
+ /**
+ * Gets the primary neural detector result class name.
+ *
+ * @param limelightName Name of the Limelight camera
+ * @return Class name string from detector pipeline
+ */
+ public static String getDetectorClass(String limelightName) {
+ return getLimelightNTString(limelightName, "tdclass");
+ }
+
+ /**
+ * Gets the pipeline's processing latency contribution.
+ *
+ * @param limelightName Name of the Limelight camera
+ * @return Pipeline latency in milliseconds
+ */
+ public static double getLatency_Pipeline(String limelightName) {
+ return getLimelightNTDouble(limelightName, "tl");
+ }
+
+ /**
+ * Gets the capture latency.
+ *
+ * @param limelightName Name of the Limelight camera
+ * @return Capture latency in milliseconds
+ */
+ public static double getLatency_Capture(String limelightName) {
+ return getLimelightNTDouble(limelightName, "cl");
+ }
+
+ /**
+ * Gets the active pipeline index.
+ *
+ * @param limelightName Name of the Limelight camera
+ * @return Current pipeline index (0-9)
+ */
+ public static double getCurrentPipelineIndex(String limelightName) {
+ return getLimelightNTDouble(limelightName, "getpipe");
+ }
+
+ /**
+ * Gets the current pipeline type.
+ *
+ * @param limelightName Name of the Limelight camera
+ * @return Pipeline type string (e.g. "retro", "apriltag", etc)
+ */
+ public static String getCurrentPipelineType(String limelightName) {
+ return getLimelightNTString(limelightName, "getpipetype");
+ }
+
+ /**
+ * Gets the full JSON results dump.
+ *
+ * @param limelightName Name of the Limelight camera
+ * @return JSON string containing all current results
+ */
+ public static String getJSONDump(String limelightName) {
+ return getLimelightNTString(limelightName, "json");
+ }
+
+ /**
+ * Switch to getBotPose
+ *
+ * @param limelightName
+ * @return
+ */
+ @Deprecated
+ public static double[] getBotpose(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "botpose");
+ }
+
+ /**
+ * Switch to getBotPose_wpiRed
+ *
+ * @param limelightName
+ * @return
+ */
+ @Deprecated
+ public static double[] getBotpose_wpiRed(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "botpose_wpired");
+ }
+
+ /**
+ * Switch to getBotPose_wpiBlue
+ *
+ * @param limelightName
+ * @return
+ */
+ @Deprecated
+ public static double[] getBotpose_wpiBlue(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
+ }
+
+ public static double[] getBotPose(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "botpose");
+ }
+
+ public static double[] getBotPose_wpiRed(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "botpose_wpired");
+ }
+
+ public static double[] getBotPose_wpiBlue(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
+ }
+
+ public static double[] getBotPose_TargetSpace(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "botpose_targetspace");
+ }
+
+ public static double[] getCameraPose_TargetSpace(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace");
+ }
+
+ public static double[] getTargetPose_CameraSpace(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace");
+ }
+
+ public static double[] getTargetPose_RobotSpace(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace");
+ }
+
+ /**
+ * Gets the average color under the crosshair region as a 3-element array.
+ *
+ * @param limelightName Name of the Limelight camera
+ * @return Array containing [Blue, Green, Red] color values (BGR order)
+ */
+ public static double[] getTargetColor(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "tc");
+ }
+
+ public static double getFiducialID(String limelightName) {
+ return getLimelightNTDouble(limelightName, "tid");
+ }
+
+ /**
+ * Gets the Limelight heartbeat value. Increments once per frame, allowing you to detect if the
+ * Limelight is connected and alive.
+ *
+ * @param limelightName Name of the Limelight camera
+ * @return Heartbeat value that increments each frame
+ */
+ public static double getHeartbeat(String limelightName) {
+ return getLimelightNTDouble(limelightName, "hb");
+ }
+
+ public static String getNeuralClassID(String limelightName) {
+ return getLimelightNTString(limelightName, "tclass");
+ }
+
+ public static String[] getRawBarcodeData(String limelightName) {
+ return getLimelightNTStringArray(limelightName, "rawbarcodes");
+ }
+
+ /////
+ /////
+
+ public static Pose3d getBotPose3d(String limelightName) {
+ double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose");
+ return toPose3D(poseArray);
+ }
+
+ /**
+ * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System.
+ *
+ * @param limelightName Name/identifier of the Limelight
+ * @return Pose3d object representing the robot's position and orientation in Red Alliance field
+ * space
+ */
+ public static Pose3d getBotPose3d_wpiRed(String limelightName) {
+ double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired");
+ return toPose3D(poseArray);
+ }
+
+ /**
+ * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System.
+ *
+ * @param limelightName Name/identifier of the Limelight
+ * @return Pose3d object representing the robot's position and orientation in Blue Alliance field
+ * space
+ */
+ public static Pose3d getBotPose3d_wpiBlue(String limelightName) {
+ double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
+ return toPose3D(poseArray);
+ }
+
+ /**
+ * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system.
+ *
+ * @param limelightName Name/identifier of the Limelight
+ * @return Pose3d object representing the robot's position and orientation relative to the target
+ */
+ public static Pose3d getBotPose3d_TargetSpace(String limelightName) {
+ double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace");
+ return toPose3D(poseArray);
+ }
+
+ /**
+ * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system.
+ *
+ * @param limelightName Name/identifier of the Limelight
+ * @return Pose3d object representing the camera's position and orientation relative to the target
+ */
+ public static Pose3d getCameraPose3d_TargetSpace(String limelightName) {
+ double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace");
+ return toPose3D(poseArray);
+ }
+
+ /**
+ * Gets the target's 3D pose with respect to the camera's coordinate system.
+ *
+ * @param limelightName Name/identifier of the Limelight
+ * @return Pose3d object representing the target's position and orientation relative to the camera
+ */
+ public static Pose3d getTargetPose3d_CameraSpace(String limelightName) {
+ double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace");
+ return toPose3D(poseArray);
+ }
+
+ /**
+ * Gets the target's 3D pose with respect to the robot's coordinate system.
+ *
+ * @param limelightName Name/identifier of the Limelight
+ * @return Pose3d object representing the target's position and orientation relative to the robot
+ */
+ public static Pose3d getTargetPose3d_RobotSpace(String limelightName) {
+ double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace");
+ return toPose3D(poseArray);
+ }
+
+ /**
+ * Gets the camera's 3D pose with respect to the robot's coordinate system.
+ *
+ * @param limelightName Name/identifier of the Limelight
+ * @return Pose3d object representing the camera's position and orientation relative to the robot
+ */
+ public static Pose3d getCameraPose3d_RobotSpace(String limelightName) {
+ double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace");
+ return toPose3D(poseArray);
+ }
+
+ /**
+ * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement)
+ *
+ * @param limelightName
+ * @return
+ */
+ public static Pose2d getBotPose2d_wpiBlue(String limelightName) {
+
+ double[] result = getBotPose_wpiBlue(limelightName);
+ return toPose2D(result);
+ }
+
+ /**
+ * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator
+ * (addVisionMeasurement) in the WPILib Blue alliance coordinate system.
+ *
+ * @param limelightName
+ * @return
+ */
+ public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) {
+ return getBotPoseEstimate(limelightName, "botpose_wpiblue", false);
+ }
+
+ /**
+ * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator
+ * (addVisionMeasurement) in the WPILib Blue alliance coordinate system. Make sure you are calling
+ * setRobotOrientation() before calling this method.
+ *
+ * @param limelightName
+ * @return
+ */
+ public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) {
+ return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true);
+ }
+
+ /**
+ * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement)
+ *
+ * @param limelightName
+ * @return
+ */
+ public static Pose2d getBotPose2d_wpiRed(String limelightName) {
+
+ double[] result = getBotPose_wpiRed(limelightName);
+ return toPose2D(result);
+ }
+
+ /**
+ * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when
+ * you are on the RED alliance
+ *
+ * @param limelightName
+ * @return
+ */
+ public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) {
+ return getBotPoseEstimate(limelightName, "botpose_wpired", false);
+ }
+
+ /**
+ * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when
+ * you are on the RED alliance
+ *
+ * @param limelightName
+ * @return
+ */
+ public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) {
+ return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true);
+ }
+
+ /**
+ * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement)
+ *
+ * @param limelightName
+ * @return
+ */
+ public static Pose2d getBotPose2d(String limelightName) {
+
+ double[] result = getBotPose(limelightName);
+ return toPose2D(result);
+ }
+
+ /**
+ * Gets the current IMU data from NetworkTables. IMU data is formatted as [robotYaw, Roll, Pitch,
+ * Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. Returns all zeros if data is invalid or
+ * unavailable.
+ *
+ * @param limelightName Name/identifier of the Limelight
+ * @return IMUData object containing all current IMU data
+ */
+ public static IMUData getIMUData(String limelightName) {
+ double[] imuData = getLimelightNTDoubleArray(limelightName, "imu");
+ if (imuData == null || imuData.length < 10) {
+ return new IMUData(); // Returns object with all zeros
+ }
+ return new IMUData(imuData);
+ }
+
+ /////
+ /////
+
+ public static void setPipelineIndex(String limelightName, int pipelineIndex) {
+ setLimelightNTDouble(limelightName, "pipeline", pipelineIndex);
+ }
+
+ public static void setPriorityTagID(String limelightName, int ID) {
+ setLimelightNTDouble(limelightName, "priorityid", ID);
+ }
+
+ /**
+ * Sets LED mode to be controlled by the current pipeline.
+ *
+ * @param limelightName Name of the Limelight camera
+ */
+ public static void setLEDMode_PipelineControl(String limelightName) {
+ setLimelightNTDouble(limelightName, "ledMode", 0);
+ }
+
+ public static void setLEDMode_ForceOff(String limelightName) {
+ setLimelightNTDouble(limelightName, "ledMode", 1);
+ }
+
+ public static void setLEDMode_ForceBlink(String limelightName) {
+ setLimelightNTDouble(limelightName, "ledMode", 2);
+ }
+
+ public static void setLEDMode_ForceOn(String limelightName) {
+ setLimelightNTDouble(limelightName, "ledMode", 3);
+ }
+
+ /**
+ * Enables standard side-by-side stream mode.
+ *
+ * @param limelightName Name of the Limelight camera
+ */
+ public static void setStreamMode_Standard(String limelightName) {
+ setLimelightNTDouble(limelightName, "stream", 0);
+ }
+
+ /**
+ * Enables Picture-in-Picture mode with secondary stream in the corner.
+ *
+ * @param limelightName Name of the Limelight camera
+ */
+ public static void setStreamMode_PiPMain(String limelightName) {
+ setLimelightNTDouble(limelightName, "stream", 1);
+ }
+
+ /**
+ * Enables Picture-in-Picture mode with primary stream in the corner.
+ *
+ * @param limelightName Name of the Limelight camera
+ */
+ public static void setStreamMode_PiPSecondary(String limelightName) {
+ setLimelightNTDouble(limelightName, "stream", 2);
+ }
+
+ /**
+ * Sets the crop window for the camera. The crop window in the UI must be completely open.
+ *
+ * @param limelightName Name of the Limelight camera
+ * @param cropXMin Minimum X value (-1 to 1)
+ * @param cropXMax Maximum X value (-1 to 1)
+ * @param cropYMin Minimum Y value (-1 to 1)
+ * @param cropYMax Maximum Y value (-1 to 1)
+ */
+ public static void setCropWindow(
+ String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) {
+ double[] entries = new double[4];
+ entries[0] = cropXMin;
+ entries[1] = cropXMax;
+ entries[2] = cropYMin;
+ entries[3] = cropYMax;
+ setLimelightNTDoubleArray(limelightName, "crop", entries);
+ }
+
+ /**
+ * Sets the keystone modification for the crop window.
+ *
+ * @param limelightName Name of the Limelight camera
+ * @param horizontal Horizontal keystone value (-0.95 to 0.95)
+ * @param vertical Vertical keystone value (-0.95 to 0.95)
+ */
+ public static void setKeystone(String limelightName, double horizontal, double vertical) {
+ double[] entries = new double[2];
+ entries[0] = horizontal;
+ entries[1] = vertical;
+ setLimelightNTDoubleArray(limelightName, "keystone_set", entries);
+ }
+
+ /** Sets 3D offset point for easy 3D targeting. */
+ public static void setFiducial3DOffset(
+ String limelightName, double offsetX, double offsetY, double offsetZ) {
+ double[] entries = new double[3];
+ entries[0] = offsetX;
+ entries[1] = offsetY;
+ entries[2] = offsetZ;
+ setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries);
+ }
+
+ /**
+ * Sets robot orientation values used by MegaTag2 localization algorithm.
+ *
+ * @param limelightName Name/identifier of the Limelight
+ * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC
+ * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second
+ * @param pitch (Unnecessary) Robot pitch in degrees
+ * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second
+ * @param roll (Unnecessary) Robot roll in degrees
+ * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second
+ */
+ public static void SetRobotOrientation(
+ String limelightName,
+ double yaw,
+ double yawRate,
+ double pitch,
+ double pitchRate,
+ double roll,
+ double rollRate) {
+ SetRobotOrientation_INTERNAL(
+ limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true);
+ }
+
+ public static void SetRobotOrientation_NoFlush(
+ String limelightName,
+ double yaw,
+ double yawRate,
+ double pitch,
+ double pitchRate,
+ double roll,
+ double rollRate) {
+ SetRobotOrientation_INTERNAL(
+ limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false);
+ }
+
+ private static void SetRobotOrientation_INTERNAL(
+ String limelightName,
+ double yaw,
+ double yawRate,
+ double pitch,
+ double pitchRate,
+ double roll,
+ double rollRate,
+ boolean flush) {
+
+ double[] entries = new double[6];
+ entries[0] = yaw;
+ entries[1] = yawRate;
+ entries[2] = pitch;
+ entries[3] = pitchRate;
+ entries[4] = roll;
+ entries[5] = rollRate;
+ setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries);
+ if (flush) {
+ Flush();
+ }
+ }
+
+ /**
+ * Configures the IMU mode for MegaTag2 Localization
+ *
+ * @param limelightName Name/identifier of the Limelight
+ * @param mode IMU mode.
+ */
+ public static void SetIMUMode(String limelightName, int mode) {
+ setLimelightNTDouble(limelightName, "imumode_set", mode);
+ }
+
+ /**
+ * Configures the complementary filter alpha value for IMU Assist Modes (Modes 3 and 4)
+ *
+ * @param limelightName Name/identifier of the Limelight
+ * @param alpha Defaults to .001. Higher values will cause the internal IMU to converge onto the
+ * assist source more rapidly.
+ */
+ public static void SetIMUAssistAlpha(String limelightName, double alpha) {
+ setLimelightNTDouble(limelightName, "imuassistalpha_set", alpha);
+ }
+
+ /**
+ * Configures the throttle value. Set to 100-200 while disabled to reduce thermal
+ * output/temperature.
+ *
+ * @param limelightName Name/identifier of the Limelight
+ * @param throttle Defaults to 0. Your Limelgiht will process one frame after skipping
+ * frames.
+ */
+ public static void SetThrottle(String limelightName, int throttle) {
+ setLimelightNTDouble(limelightName, "throttle_set", throttle);
+ }
+
+ /**
+ * Overrides the valid AprilTag IDs that will be used for localization. Tags not in this list will
+ * be ignored for robot pose estimation.
+ *
+ * @param limelightName Name/identifier of the Limelight
+ * @param validIDs Array of valid AprilTag IDs to track
+ */
+ public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) {
+ double[] validIDsDouble = new double[validIDs.length];
+ for (int i = 0; i < validIDs.length; i++) {
+ validIDsDouble[i] = validIDs[i];
+ }
+ setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble);
+ }
+
+ /**
+ * Sets the downscaling factor for AprilTag detection. Increasing downscale can improve
+ * performance at the cost of potentially reduced detection range.
+ *
+ * @param limelightName Name/identifier of the Limelight
+ * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to
+ * 0 for pipeline control.
+ */
+ public static void SetFiducialDownscalingOverride(String limelightName, float downscale) {
+ int d = 0; // pipeline
+ if (downscale == 1.0) {
+ d = 1;
+ }
+ if (downscale == 1.5) {
+ d = 2;
+ }
+ if (downscale == 2) {
+ d = 3;
+ }
+ if (downscale == 3) {
+ d = 4;
+ }
+ if (downscale == 4) {
+ d = 5;
+ }
+ setLimelightNTDouble(limelightName, "fiducial_downscale_set", d);
+ }
+
+ /**
+ * Sets the camera pose relative to the robot.
+ *
+ * @param limelightName Name of the Limelight camera
+ * @param forward Forward offset in meters
+ * @param side Side offset in meters
+ * @param up Up offset in meters
+ * @param roll Roll angle in degrees
+ * @param pitch Pitch angle in degrees
+ * @param yaw Yaw angle in degrees
+ */
+ public static void setCameraPose_RobotSpace(
+ String limelightName,
+ double forward,
+ double side,
+ double up,
+ double roll,
+ double pitch,
+ double yaw) {
+ double[] entries = new double[6];
+ entries[0] = forward;
+ entries[1] = side;
+ entries[2] = up;
+ entries[3] = roll;
+ entries[4] = pitch;
+ entries[5] = yaw;
+ setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries);
+ }
+
+ /////
+ /////
+
+ public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) {
+ setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData);
+ }
+
+ public static double[] getPythonScriptData(String limelightName) {
+ return getLimelightNTDoubleArray(limelightName, "llpython");
+ }
+
+ /////
+ /////
+
+ /**
+ * Triggers a snapshot capture via NetworkTables by incrementing the snapshot counter.
+ * Rate-limited to once per 10 frames on the Limelight.
+ *
+ * @param limelightName Name of the Limelight camera
+ */
+ public static void triggerSnapshot(String limelightName) {
+ double current = getLimelightNTDouble(limelightName, "snapshot");
+ setLimelightNTDouble(limelightName, "snapshot", current + 1);
+ }
+
+ /**
+ * Enables or pauses the rewind buffer recording.
+ *
+ * @param limelightName Name of the Limelight camera
+ * @param enabled True to enable recording, false to pause
+ */
+ public static void setRewindEnabled(String limelightName, boolean enabled) {
+ setLimelightNTDouble(limelightName, "rewind_enable_set", enabled ? 1 : 0);
+ }
+
+ /**
+ * Triggers a rewind capture with the specified duration. Maximum duration is 165 seconds.
+ * Rate-limited on the Limelight.
+ *
+ * @param limelightName Name of the Limelight camera
+ * @param durationSeconds Duration of rewind capture in seconds (max 165)
+ */
+ public static void triggerRewindCapture(String limelightName, double durationSeconds) {
+ double[] currentArray = getLimelightNTDoubleArray(limelightName, "capture_rewind");
+ double counter = (currentArray.length > 0) ? currentArray[0] : 0;
+ double[] entries = new double[2];
+ entries[0] = counter + 1;
+ entries[1] = Math.min(durationSeconds, 165);
+ setLimelightNTDoubleArray(limelightName, "capture_rewind", entries);
+ }
+
+ /**
+ * Gets the latest JSON results output and returns a LimelightResults object.
+ *
+ * @param limelightName Name of the Limelight camera
+ * @return LimelightResults object containing all current target data
+ */
+ public static LimelightResults getLatestResults(String limelightName) {
+
+ long start = System.nanoTime();
+ LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults();
+ if (mapper == null) {
+ mapper =
+ new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false);
+ }
+
+ try {
+ String jsonString = getJSONDump(limelightName);
+ if (jsonString == null || jsonString.isEmpty() || jsonString.isBlank()) {
+ results.error = "lljson error: empty json";
+ } else {
+ results = mapper.readValue(jsonString, LimelightResults.class);
+ if (results.imuResults != null) {
+ results.imuResults.parseDataArray();
+ }
+ }
+ } catch (JsonProcessingException e) {
+ results.error = "lljson error: " + e.getMessage();
+ }
+
+ long end = System.nanoTime();
+ double millis = (end - start) * .000001;
+ results.latency_jsonParse = millis;
+ if (profileJSON) {
+ System.out.printf("lljson: %.2f\r\n", millis);
+ }
+
+ return results;
+ }
+
+ /**
+ * Sets up port forwarding for a Limelight 3A/3G connected via USB. This allows access to the
+ * Limelight web interface and video stream when connected to the robot over USB.
+ *
+ * For usbIndex 0: ports 5800-5809 forward to 172.29.0.1 For usbIndex 1: ports 5810-5819
+ * forward to 172.29.1.1 etc.
+ *
+ *
Call this method once during robot initialization. To access the interface of the camera
+ * with usbIndex0, you would go to roboRIO-(teamnum)-FRC.local:5801. Port 5811 for usb index 1
+ *
+ * @param usbIndex The USB index of the Limelight (0, 1, 2, etc.)
+ */
+ public static void setupPortForwardingUSB(int usbIndex) {
+ String ip = "172.29." + usbIndex + ".1";
+ int basePort = 5800 + (usbIndex * 10);
+
+ for (int i = 0; i < 10; i++) {
+ PortForwarder.add(basePort + i, ip, 5800 + i);
+ }
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/Setpoint.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/Setpoint.java
new file mode 100644
index 00000000..17070f46
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/Setpoint.java
@@ -0,0 +1,91 @@
+package edu.wpi.team190.gompeilib.core.utility;
+
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Unit;
+import edu.wpi.team190.gompeilib.core.logging.Trace;
+import lombok.Getter;
+
+public class Setpoint {
+ @Getter private Measure setpoint;
+ @Getter private Measure newSetpoint;
+ @Getter private Measure offset;
+ private final Measure step;
+ public Measure min;
+ public Measure max;
+
+ public Setpoint(Measure setpoint, Measure step, Measure min, Measure max) {
+ this.setpoint = setpoint;
+ this.offset = setpoint.times(0);
+ this.step = step;
+ this.min = min.minus(setpoint);
+ this.max = max.minus(setpoint);
+ this.newSetpoint = setpoint;
+ }
+
+ public Setpoint(Measure setpoint, Measure step) {
+ this.setpoint = setpoint;
+ this.offset = setpoint.times(0);
+ this.step = step;
+ this.min = step.times(Double.NEGATIVE_INFINITY);
+ this.max = step.times(Double.POSITIVE_INFINITY);
+ this.newSetpoint = setpoint;
+ }
+
+ public void setSetpoint(Measure setpoint) {
+ this.min = min.plus(this.setpoint).minus(setpoint);
+ this.max = max.plus(this.setpoint).minus(setpoint);
+ this.setpoint = setpoint;
+
+ // offset = clamp(offset);
+ newSetpoint = calculateSetpoint();
+ }
+
+ @Trace
+ private Measure calculateSetpoint() {
+ double sign = Math.signum(setpoint.magnitude());
+
+ // Apply offset in the direction of the setpoint's sign
+ // positive offset always increases magnitude, negative always decreases
+ var newMagnitude = setpoint.plus(offset.times(sign));
+
+ // Clamp to zero if we've crossed zero (don't flip sign)
+ if (Math.signum(newMagnitude.magnitude()) != sign && sign != 0) {
+ return setpoint.times(0); // return zero in same unit
+ }
+
+ return newMagnitude;
+ }
+
+ public void increment() {
+ increment(step);
+ }
+
+ public void decrement() {
+ decrement(step);
+ }
+
+ public void increment(Measure step) {
+ Measure next = offset.plus(step);
+ if (next.gt(max)) {
+ offset = max;
+ } else {
+ offset = next;
+ }
+ newSetpoint = calculateSetpoint();
+ }
+
+ public void decrement(Measure step) {
+ Measure next = offset.minus(step);
+ if (next.lt(min)) {
+ offset = min;
+ } else {
+ offset = next;
+ }
+ newSetpoint = calculateSetpoint();
+ }
+
+ public void reset() {
+ offset = offset.times(0);
+ newSetpoint = setpoint;
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/VirtualSubsystem.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/VirtualSubsystem.java
new file mode 100644
index 00000000..c3444e3e
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/VirtualSubsystem.java
@@ -0,0 +1,30 @@
+// Copyright (c) 2023 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package edu.wpi.team190.gompeilib.core.utility;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/** Represents a subsystem unit that requires a periodic callback but not a hardware mutex. */
+public abstract class VirtualSubsystem {
+ private static List subsystems = new ArrayList<>();
+
+ public VirtualSubsystem() {
+ subsystems.add(this);
+ }
+
+ /** Calls {@link #periodic()} on all virtual subsystems. */
+ public static void periodicAll() {
+ for (var subsystem : subsystems) {
+ subsystem.periodic();
+ }
+ }
+
+ /** This method is called periodically once per loop cycle. */
+ public abstract void periodic();
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/CurrentLimits.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/CurrentLimits.java
new file mode 100644
index 00000000..c707ba18
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/CurrentLimits.java
@@ -0,0 +1,18 @@
+package edu.wpi.team190.gompeilib.core.utility.control;
+
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Current;
+import lombok.Builder;
+
+@Builder(setterPrefix = "with")
+public record CurrentLimits(Current supplyCurrentLimit, Current statorCurrentLimit) {
+ @Builder(
+ setterPrefix = "with",
+ builderClassName = "FromDoubles",
+ builderMethodName = "fromDoubles")
+ public CurrentLimits(double supplyCurrentLimit, double statorCurrentLimit) {
+ this(
+ Current.ofBaseUnits(supplyCurrentLimit, Units.Amps),
+ Current.ofBaseUnits(statorCurrentLimit, Units.Amps));
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/Gains.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/Gains.java
new file mode 100644
index 00000000..1cf75598
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/Gains.java
@@ -0,0 +1,96 @@
+package edu.wpi.team190.gompeilib.core.utility.control;
+
+import edu.wpi.team190.gompeilib.core.utility.tunable.LoggedTunableNumber;
+import java.util.function.Consumer;
+import lombok.Builder;
+import lombok.NonNull;
+
+@Builder(setterPrefix = "with")
+public record Gains(
+ LoggedTunableNumber kP,
+ LoggedTunableNumber kI,
+ LoggedTunableNumber kD,
+ LoggedTunableNumber kS,
+ LoggedTunableNumber kV,
+ LoggedTunableNumber kA,
+ LoggedTunableNumber kG) {
+
+ @Builder(
+ setterPrefix = "with",
+ builderMethodName = "fromDoubles",
+ builderClassName = "FromDoubles")
+ public Gains(
+ @NonNull String prefix,
+ double kP,
+ double kI,
+ double kD,
+ double kS,
+ double kV,
+ double kA,
+ double kG) {
+ this(
+ new LoggedTunableNumber(prefix + "/Kp", kP),
+ new LoggedTunableNumber(prefix + "/Ki", kI),
+ new LoggedTunableNumber(prefix + "/Kd", kD),
+ new LoggedTunableNumber(prefix + "/Ks", kS),
+ new LoggedTunableNumber(prefix + "/Kv", kV),
+ new LoggedTunableNumber(prefix + "/Ka", kA),
+ new LoggedTunableNumber(prefix + "/Kg", kG));
+ }
+
+ public Gains {
+ if (kP == null) {
+ kP = new LoggedTunableNumber("");
+ }
+ if (kI == null) {
+ kI = new LoggedTunableNumber("");
+ }
+ if (kD == null) {
+ kD = new LoggedTunableNumber("");
+ }
+ if (kS == null) {
+ kS = new LoggedTunableNumber("");
+ }
+ if (kV == null) {
+ kV = new LoggedTunableNumber("");
+ }
+ if (kA == null) {
+ kA = new LoggedTunableNumber("");
+ }
+ if (kG == null) {
+ kG = new LoggedTunableNumber("");
+ }
+ }
+
+ public double getKP() {
+ return kP.get();
+ }
+
+ public double getKI() {
+ return kI.get();
+ }
+
+ public double getKD() {
+ return kD.get();
+ }
+
+ public double getKS() {
+ return kS.get();
+ }
+
+ public double getKV() {
+ return kV.get();
+ }
+
+ public double getKA() {
+ return kA.get();
+ }
+
+ public double getKG() {
+ return kG.get();
+ }
+
+ public void update(int id, Consumer consumer) {
+ LoggedTunableNumber.ifChanged(id, g -> consumer.accept(this), kP, kI, kD, kS, kV, kA, kG);
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/LinearProfile.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/LinearProfile.java
new file mode 100644
index 00000000..e2e7c58b
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/LinearProfile.java
@@ -0,0 +1,102 @@
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package edu.wpi.team190.gompeilib.core.utility.control;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Twist2d;
+import lombok.Getter;
+import lombok.Setter;
+
+/** Ramps up and down to setpoint for velocity closed loop control */
+public class LinearProfile {
+ private double dv;
+ private double maxVelocity;
+ @Getter private final double period;
+ @Getter private double currentSetpoint = 0;
+ @Getter @Setter private double goal = 0;
+
+ /**
+ * Creates a new LinearProfile
+ *
+ * @param maxAcceleration The max ramp rate in velocity in rps/sec
+ * @param period Period of control loop (0.02)
+ */
+ public LinearProfile(double maxAcceleration, double maxVelocity, double period) {
+ this.period = period;
+ this.maxVelocity = maxVelocity;
+ setMaxAcceleration(maxAcceleration);
+ }
+
+ /** Set the max acceleration constraint in rpm/sec */
+ public void setMaxAcceleration(double maxAcceleration) {
+ dv = maxAcceleration * period;
+ }
+
+ public void setMaxVelocity(double maxVelocity) {
+ this.maxVelocity = maxVelocity;
+ }
+
+ /**
+ * Sets the target setpoint, starting from the current speed
+ *
+ * @param goal Target setpoint
+ * @param currentSpeed Current speed, to be used as the starting setpoint
+ */
+ public void setGoal(double goal, double currentSpeed) {
+ this.goal = goal;
+ currentSetpoint = currentSpeed;
+ }
+
+ /** Resets target setpoint and current setpoint */
+ public void reset() {
+ currentSetpoint = 0;
+ goal = 0;
+ }
+
+ /**
+ * Returns the current setpoint to send to motors
+ *
+ * @return Setpoint to send to motors
+ */
+ public double calculateSetpoint() {
+ if (EqualsUtil.epsilonEquals(goal, currentSetpoint)) {
+ return currentSetpoint;
+ }
+ if (goal > currentSetpoint) {
+ currentSetpoint += dv;
+ if (currentSetpoint > goal) {
+ currentSetpoint = goal;
+ }
+ } else if (goal < currentSetpoint) {
+ currentSetpoint -= dv;
+ if (currentSetpoint < goal) {
+ currentSetpoint = goal;
+ }
+ }
+ return MathUtil.clamp(currentSetpoint, -maxVelocity, maxVelocity);
+ }
+
+ public class EqualsUtil {
+ public static boolean epsilonEquals(double a, double b, double epsilon) {
+ return (a - epsilon <= b) && (a + epsilon >= b);
+ }
+
+ public static boolean epsilonEquals(double a, double b) {
+ return epsilonEquals(a, b, 1e-9);
+ }
+
+ /** Extension methods for wpi geometry objects */
+ public static class GeomExtensions {
+ public static boolean epsilonEquals(Twist2d twist, Twist2d other) {
+ return EqualsUtil.epsilonEquals(twist.dx, other.dx)
+ && EqualsUtil.epsilonEquals(twist.dy, other.dy)
+ && EqualsUtil.epsilonEquals(twist.dtheta, other.dtheta);
+ }
+ }
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/constraints/AngularPositionConstraints.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/constraints/AngularPositionConstraints.java
new file mode 100644
index 00000000..b542df45
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/constraints/AngularPositionConstraints.java
@@ -0,0 +1,49 @@
+package edu.wpi.team190.gompeilib.core.utility.control.constraints;
+
+import edu.wpi.first.units.*;
+import edu.wpi.team190.gompeilib.core.utility.tunable.LoggedTunableMeasure;
+import java.util.function.Consumer;
+import lombok.Builder;
+import lombok.NonNull;
+
+/** Specifically for Angular constraints (Degrees, Radians, Rotations). */
+@Builder(setterPrefix = "with")
+public record AngularPositionConstraints(
+ LoggedTunableMeasure goalTolerance,
+ LoggedTunableMeasure maxVelocity,
+ LoggedTunableMeasure maxAcceleration)
+ implements Constraints,
+ Constraints.PositionConstraints {
+
+ @Builder(
+ setterPrefix = "with",
+ builderClassName = "FromMeasures",
+ builderMethodName = "fromMeasures")
+ public AngularPositionConstraints(
+ @NonNull String prefix,
+ Measure goalTolerance,
+ Measure maxVelocity,
+ Measure maxAcceleration) {
+ this(
+ new LoggedTunableMeasure<>(String.format("%s/Goal Tolerance", prefix), goalTolerance),
+ new LoggedTunableMeasure<>(String.format("%s/Max Velocity", prefix), maxVelocity),
+ new LoggedTunableMeasure<>(String.format("%s/Max Acceleration", prefix), maxAcceleration));
+ }
+
+ public double getGoalTolerance(AngleUnit unit) {
+ return goalTolerance.get(unit);
+ }
+
+ public double getMaxVelocity(AngularVelocityUnit unit) {
+ return maxVelocity.get(unit);
+ }
+
+ public double getMaxAcceleration(AngularAccelerationUnit unit) {
+ return maxAcceleration.get(unit);
+ }
+
+ public void update(int id, Consumer consumer) {
+ LoggedTunableMeasure.ifChanged(
+ id, () -> consumer.accept(this), goalTolerance, maxVelocity, maxAcceleration);
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/constraints/AngularVelocityConstraints.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/constraints/AngularVelocityConstraints.java
new file mode 100644
index 00000000..d66c91fc
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/constraints/AngularVelocityConstraints.java
@@ -0,0 +1,38 @@
+package edu.wpi.team190.gompeilib.core.utility.control.constraints;
+
+import edu.wpi.first.units.AngularAccelerationUnit;
+import edu.wpi.first.units.AngularVelocityUnit;
+import edu.wpi.first.units.Measure;
+import edu.wpi.team190.gompeilib.core.utility.tunable.LoggedTunableMeasure;
+import java.util.function.Consumer;
+import lombok.Builder;
+import lombok.NonNull;
+
+/** Specifically for Angular constraints (Degrees, Radians, Rotations). */
+@Builder(setterPrefix = "with")
+public record AngularVelocityConstraints(
+ LoggedTunableMeasure goalTolerance,
+ LoggedTunableMeasure maxVelocity,
+ LoggedTunableMeasure maxAcceleration)
+ implements Constraints {
+
+ @Builder(
+ setterPrefix = "with",
+ builderClassName = "FromMeasures",
+ builderMethodName = "fromMeasures")
+ public AngularVelocityConstraints(
+ @NonNull String prefix,
+ Measure goalTolerance,
+ Measure maxVelocity,
+ Measure maxAcceleration) {
+ this(
+ new LoggedTunableMeasure<>(String.format("%s/Goal Tolerance", prefix), goalTolerance),
+ new LoggedTunableMeasure<>(String.format("%s/Max Velocity", prefix), maxVelocity),
+ new LoggedTunableMeasure<>(String.format("%s/Max Acceleration", prefix), maxAcceleration));
+ }
+
+ public void update(int id, Consumer consumer) {
+ LoggedTunableMeasure.ifChanged(
+ id, () -> consumer.accept(this), goalTolerance, maxVelocity, maxAcceleration);
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/constraints/Constraints.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/constraints/Constraints.java
new file mode 100644
index 00000000..fd09f865
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/constraints/Constraints.java
@@ -0,0 +1,10 @@
+package edu.wpi.team190.gompeilib.core.utility.control.constraints;
+
+import java.util.function.Consumer;
+
+public interface Constraints> {
+ public void update(int id, Consumer consumer);
+
+ public sealed interface PositionConstraints>
+ extends Constraints permits AngularPositionConstraints, LinearConstraints {}
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/constraints/LinearConstraints.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/constraints/LinearConstraints.java
new file mode 100644
index 00000000..0de82929
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/control/constraints/LinearConstraints.java
@@ -0,0 +1,48 @@
+package edu.wpi.team190.gompeilib.core.utility.control.constraints;
+
+import edu.wpi.first.units.*;
+import edu.wpi.team190.gompeilib.core.utility.tunable.LoggedTunableMeasure;
+import java.util.function.Consumer;
+import lombok.Builder;
+import lombok.NonNull;
+
+/** Specifically for Angular constraints (Degrees, Radians, Rotations). */
+@Builder(setterPrefix = "with")
+public record LinearConstraints(
+ LoggedTunableMeasure goalTolerance,
+ LoggedTunableMeasure maxVelocity,
+ LoggedTunableMeasure maxAcceleration)
+ implements Constraints, Constraints.PositionConstraints {
+
+ @Builder(
+ setterPrefix = "with",
+ builderMethodName = "fromMeasures",
+ builderClassName = "FromMeasures")
+ public LinearConstraints(
+ @NonNull String prefix,
+ Measure goalTolerance,
+ Measure maxVelocity,
+ Measure maxAcceleration) {
+ this(
+ new LoggedTunableMeasure<>(String.format("%s/Goal Tolerance", prefix), goalTolerance),
+ new LoggedTunableMeasure<>(String.format("%s/Max Velocity", prefix), maxVelocity),
+ new LoggedTunableMeasure<>(String.format("%s/Max Acceleration", prefix), maxAcceleration));
+ }
+
+ public double getGoalToleranceMeters(DistanceUnit unit) {
+ return goalTolerance.get(unit);
+ }
+
+ public double getMaxVelocityMetersPerSecond(LinearVelocityUnit unit) {
+ return maxVelocity.get(unit);
+ }
+
+ public double getMaxAccelerationMetersPerSecondSquared(LinearAccelerationUnit unit) {
+ return maxAcceleration.get(unit);
+ }
+
+ public void update(int id, Consumer consumer) {
+ LoggedTunableMeasure.ifChanged(
+ id, () -> consumer.accept(this), goalTolerance, maxVelocity, maxAcceleration);
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/phoenix/GainSlot.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/phoenix/GainSlot.java
new file mode 100644
index 00000000..328ee878
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/phoenix/GainSlot.java
@@ -0,0 +1,17 @@
+package edu.wpi.team190.gompeilib.core.utility.phoenix;
+
+/** An Enum class that handles all possible gain slots for motor control */
+public enum GainSlot {
+ ZERO,
+ ONE,
+ TWO;
+
+ public static GainSlot integerToGainSlot(Integer integer) {
+ return switch (integer) {
+ case 0 -> ZERO;
+ case 1 -> ONE;
+ case 2 -> TWO;
+ default -> null;
+ };
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/phoenix/PhoenixOdometryThread.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/phoenix/PhoenixOdometryThread.java
new file mode 100644
index 00000000..77cd15be
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/phoenix/PhoenixOdometryThread.java
@@ -0,0 +1,167 @@
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package edu.wpi.team190.gompeilib.core.utility.phoenix;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive.SwerveDriveConstants;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Queue;
+import java.util.concurrent.ArrayBlockingQueue;
+import java.util.concurrent.locks.Lock;
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.function.DoubleSupplier;
+
+/**
+ * Provides an interface for asynchronously reading high-frequency measurements to a set of queues.
+ *
+ * This version is intended for Phoenix 6 devices on both the RIO and CANivore buses. When using
+ * a CANivore, the thread uses the "waitForAll" blocking method to enable more consistent sampling.
+ * This also allows Phoenix Pro users to benefit from lower latency between devices using CANivore
+ * time synchronization.
+ */
+public class PhoenixOdometryThread extends Thread {
+ private final SwerveDriveConstants driveConstants;
+ private final Lock signalsLock =
+ new ReentrantLock(); // Prevents conflicts when registering signals
+ private BaseStatusSignal[] phoenixSignals = new BaseStatusSignal[0];
+ private final List genericSignals = new ArrayList<>();
+ private final List> phoenixQueues = new ArrayList<>();
+ private final List> genericQueues = new ArrayList<>();
+ private final List> timestampQueues = new ArrayList<>();
+
+ private static boolean isCANFD;
+ private static PhoenixOdometryThread instance = null;
+
+ public static PhoenixOdometryThread getInstance(SwerveDriveConstants driveConstants) {
+ if (instance == null) {
+ instance = new PhoenixOdometryThread(driveConstants);
+ }
+ return instance;
+ }
+
+ private PhoenixOdometryThread(SwerveDriveConstants driveConstants) {
+ this.driveConstants = driveConstants;
+ isCANFD = driveConstants.driveConfig.canBus().isNetworkFD();
+ setName("PhoenixOdometryThread");
+ setDaemon(true);
+ }
+
+ @Override
+ public void start() {
+ if (timestampQueues.size() > 0) {
+ super.start();
+ }
+ }
+
+ /** Registers a Phoenix signal to be read from the thread. */
+ public Queue registerSignal(StatusSignal signal) {
+ Queue queue = new ArrayBlockingQueue<>(20);
+ signalsLock.lock();
+ driveConstants.reentrantLock.lock();
+ try {
+ BaseStatusSignal[] newSignals = new BaseStatusSignal[phoenixSignals.length + 1];
+ System.arraycopy(phoenixSignals, 0, newSignals, 0, phoenixSignals.length);
+ newSignals[phoenixSignals.length] = signal;
+ phoenixSignals = newSignals;
+ phoenixQueues.add(queue);
+ } finally {
+ signalsLock.unlock();
+ driveConstants.reentrantLock.unlock();
+ }
+ return queue;
+ }
+
+ /** Registers a generic signal to be read from the thread. */
+ public Queue registerSignal(DoubleSupplier signal) {
+ Queue queue = new ArrayBlockingQueue<>(20);
+ signalsLock.lock();
+ driveConstants.reentrantLock.lock();
+ try {
+ genericSignals.add(signal);
+ genericQueues.add(queue);
+ } finally {
+ signalsLock.unlock();
+ driveConstants.reentrantLock.unlock();
+ }
+ return queue;
+ }
+
+ /** Returns a new queue that returns timestamp values for each sample. */
+ public Queue makeTimestampQueue() {
+ Queue queue = new ArrayBlockingQueue<>(20);
+ driveConstants.reentrantLock.lock();
+ try {
+ timestampQueues.add(queue);
+ } finally {
+ driveConstants.reentrantLock.unlock();
+ }
+ return queue;
+ }
+
+ @Override
+ public void run() {
+ while (true) {
+ // Wait for updates from all signals
+ signalsLock.lock();
+ try {
+ if (isCANFD && phoenixSignals.length > 0) {
+ BaseStatusSignal.waitForAll(2.0 / driveConstants.odometryFrequency, phoenixSignals);
+ } else {
+ // "waitForAll" does not support blocking on multiple signals with a bus
+ // that is not CAN FD, regardless of Pro licensing. No reasoning for this
+ // behavior is provided by the documentation.
+ Thread.sleep((long) (1000.0 / driveConstants.odometryFrequency));
+ if (phoenixSignals.length > 0) BaseStatusSignal.refreshAll(phoenixSignals);
+ }
+ } catch (InterruptedException e) {
+ e.printStackTrace();
+ } finally {
+ signalsLock.unlock();
+ }
+
+ // Save new data to queues
+ driveConstants.reentrantLock.lock();
+ try {
+ // Sample timestamp is current FPGA time minus average CAN latency
+ // Default timestamps from Phoenix are NOT compatible with
+ // FPGA timestamps, this solution is imperfect but close
+ double timestamp = RobotController.getFPGATime() / 1e6;
+ double totalLatency = 0.0;
+ for (BaseStatusSignal signal : phoenixSignals) {
+ totalLatency += signal.getTimestamp().getLatency();
+ }
+ if (phoenixSignals.length > 0) {
+ timestamp -= totalLatency / phoenixSignals.length;
+ }
+
+ // Add new samples to queues
+ for (int i = 0; i < phoenixSignals.length; i++) {
+ phoenixQueues.get(i).offer(phoenixSignals[i].getValueAsDouble());
+ }
+ for (int i = 0; i < genericSignals.size(); i++) {
+ genericQueues.get(i).offer(genericSignals.get(i).getAsDouble());
+ }
+ for (int i = 0; i < timestampQueues.size(); i++) {
+ timestampQueues.get(i).offer(timestamp);
+ }
+ } finally {
+ driveConstants.reentrantLock.unlock();
+ }
+ }
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/phoenix/PhoenixUtil.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/phoenix/PhoenixUtil.java
new file mode 100644
index 00000000..8c6c09fe
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/phoenix/PhoenixUtil.java
@@ -0,0 +1,45 @@
+package edu.wpi.team190.gompeilib.core.utility.phoenix;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
+import java.util.function.Supplier;
+
+public class PhoenixUtil {
+ /** Attempts to run the command until no error is produced. */
+ public static void tryUntilOk(int maxAttempts, Supplier command) {
+ for (int i = 0; i < maxAttempts; i++) {
+ var error = command.get();
+ if (error.isOK()) break;
+ }
+ }
+
+ /** Signals for synchronized refresh. */
+ private static BaseStatusSignal[] canivoreSignals = new BaseStatusSignal[0];
+
+ private static BaseStatusSignal[] rioSignals = new BaseStatusSignal[0];
+
+ /** Registers a set of signals for synchronized refresh. */
+ public static void registerSignals(boolean canivore, BaseStatusSignal... signals) {
+ if (canivore) {
+ BaseStatusSignal[] newSignals = new BaseStatusSignal[canivoreSignals.length + signals.length];
+ System.arraycopy(canivoreSignals, 0, newSignals, 0, canivoreSignals.length);
+ System.arraycopy(signals, 0, newSignals, canivoreSignals.length, signals.length);
+ canivoreSignals = newSignals;
+ } else {
+ BaseStatusSignal[] newSignals = new BaseStatusSignal[rioSignals.length + signals.length];
+ System.arraycopy(rioSignals, 0, newSignals, 0, rioSignals.length);
+ System.arraycopy(signals, 0, newSignals, rioSignals.length, signals.length);
+ rioSignals = newSignals;
+ }
+ }
+
+ /** Refresh all registered signals. */
+ public static void refreshAll() {
+ if (canivoreSignals.length > 0) {
+ BaseStatusSignal.refreshAll(canivoreSignals);
+ }
+ if (rioSignals.length > 0) {
+ BaseStatusSignal.refreshAll(rioSignals);
+ }
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/sysid/CustomSysIdRoutine.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/sysid/CustomSysIdRoutine.java
new file mode 100644
index 00000000..8f53a9d4
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/sysid/CustomSysIdRoutine.java
@@ -0,0 +1,151 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.team190.gompeilib.core.utility.sysid;
+
+import static edu.wpi.first.units.Units.*;
+
+import edu.wpi.first.units.*;
+import edu.wpi.first.units.measure.Per;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import java.util.function.Consumer;
+
+/**
+ * A generic SysId characterization routine. Subclass this for specific units.
+ *
+ * @param The unit type for the output (e.g., VoltageUnit, CurrentUnit)
+ */
+public class CustomSysIdRoutine extends SysIdRoutineLog {
+ private final Config config;
+ private final Mechanism mechanism;
+ private final MutableMeasure outputValue;
+ private final Consumer recordState;
+
+ /**
+ * Create a new SysId characterization routine. * @param config Configuration with strongly typed
+ * measures.
+ *
+ * @param mechanism Mechanism interface.
+ * @param initialMutable The mutable measure instance (created by the subclass).
+ */
+ public CustomSysIdRoutine(
+ Config config, Mechanism mechanism, MutableMeasure initialMutable) {
+ super(mechanism.name);
+ this.config = config;
+ this.mechanism = mechanism;
+ outputValue = initialMutable;
+ recordState = config.recordState != null ? config.recordState : this::recordState;
+ }
+
+ /**
+ * @param rampRate We use Measure for ramp rate because it is U/Time.
+ */
+ public record Config(
+ Per rampRate,
+ Measure stepOutput,
+ Measure timeout,
+ Consumer recordState,
+ U outputUnit) {
+ public Config(
+ Per rampRate,
+ Measure stepOutput,
+ Measure timeout,
+ Consumer recordState,
+ U outputUnit) {
+ this.rampRate = rampRate;
+ this.stepOutput = stepOutput;
+ this.timeout = timeout != null ? timeout : Seconds.of(10);
+ this.recordState = recordState;
+ this.outputUnit = outputUnit;
+ }
+ }
+
+ public static class Mechanism {
+ public final Consumer> drive;
+ public final Consumer log;
+ public final Subsystem subsystem;
+ public final String name;
+
+ public Mechanism(
+ Consumer> drive,
+ Consumer log,
+ Subsystem subsystem,
+ String name) {
+ this.drive = drive;
+ this.log = log != null ? log : l -> {};
+ this.subsystem = subsystem;
+ this.name = name != null ? name : subsystem.getName();
+ }
+
+ public Mechanism(Consumer> drive, Subsystem subsystem) {
+ this(drive, null, subsystem, null);
+ }
+ }
+
+ public enum Direction {
+ kForward,
+ kReverse
+ }
+
+ public Command quasistatic(Direction direction) {
+ State state =
+ (direction == Direction.kForward) ? State.kQuasistaticForward : State.kQuasistaticReverse;
+
+ double outputSign = (direction == Direction.kForward) ? 1.0 : -1.0;
+ Timer timer = new Timer();
+
+ double rampRateUnitsPerSec = config.rampRate.magnitude();
+
+ return mechanism
+ .subsystem
+ .runOnce(timer::restart)
+ .andThen(
+ mechanism.subsystem.run(
+ () -> {
+ mechanism.drive.accept(
+ outputValue.mut_replace(
+ outputSign * timer.get() * rampRateUnitsPerSec, config.outputUnit));
+
+ mechanism.log.accept(this);
+ recordState.accept(state);
+ }))
+ .finallyDo(
+ () -> {
+ mechanism.drive.accept(outputValue.mut_replace(0, config.outputUnit));
+ recordState.accept(State.kNone);
+ timer.stop();
+ })
+ .withName("sysid-" + state + "-" + mechanism.name)
+ .withTimeout(config.timeout.in(Seconds));
+ }
+
+ public Command dynamic(Direction direction) {
+ double outputSign = (direction == Direction.kForward) ? 1.0 : -1.0;
+ State state = (direction == Direction.kForward) ? State.kDynamicForward : State.kDynamicReverse;
+
+ // OPTIMIZED: Pre-calculate step magnitude safely
+ double stepMagnitude = config.stepOutput.in(config.outputUnit);
+
+ return mechanism
+ .subsystem
+ .runOnce(() -> outputValue.mut_replace(stepMagnitude * outputSign, config.outputUnit))
+ .andThen(
+ mechanism.subsystem.run(
+ () -> {
+ mechanism.drive.accept(outputValue);
+ mechanism.log.accept(this);
+ recordState.accept(state);
+ }))
+ .finallyDo(
+ () -> {
+ mechanism.drive.accept(outputValue.mut_replace(0, config.outputUnit));
+ recordState.accept(State.kNone);
+ })
+ .withName("sysid-" + state.toString() + "-" + mechanism.name)
+ .withTimeout(config.timeout.in(Seconds));
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/sysid/CustomUnits.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/sysid/CustomUnits.java
new file mode 100644
index 00000000..845eb212
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/sysid/CustomUnits.java
@@ -0,0 +1,13 @@
+package edu.wpi.team190.gompeilib.core.utility.sysid;
+
+import static edu.wpi.first.units.Units.*;
+
+import edu.wpi.first.units.CurrentUnit;
+import edu.wpi.first.units.PerUnit;
+import edu.wpi.first.units.TimeUnit;
+import edu.wpi.first.units.VoltageUnit;
+
+public class CustomUnits {
+ public static final PerUnit ampsPerSecond = Amps.per(Second);
+ public static final PerUnit voltsPerSecond = Volts.per(Second);
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/tunable/LoggedTunableMeasure.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/tunable/LoggedTunableMeasure.java
new file mode 100644
index 00000000..c4179743
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/tunable/LoggedTunableMeasure.java
@@ -0,0 +1,100 @@
+package edu.wpi.team190.gompeilib.core.utility.tunable;
+
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Unit;
+import edu.wpi.team190.gompeilib.core.GompeiLib;
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.function.Consumer;
+import java.util.function.Supplier;
+import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
+
+/**
+ * Class for a tunable measure (unit-safe). Gets value from dashboard in tuning mode, returns
+ * default if not or value not in dashboard.
+ */
+public class LoggedTunableMeasure implements Supplier> {
+ private static final String tableKey = "TunableNumbers";
+
+ private final String key;
+ private final U unit;
+ private boolean hasDefault = false;
+ private Measure defaultValue;
+ private LoggedNetworkNumber dashboardNumber;
+ private final Map> lastHasChangedValues = new HashMap<>();
+
+ /**
+ * Create a new LoggedTunableMeasure with the default value
+ *
+ * @param dashboardKey Key on dashboard
+ * @param defaultValue Default measure value
+ */
+ public LoggedTunableMeasure(String dashboardKey, Measure defaultValue) {
+ this.key = tableKey + "/" + dashboardKey + " (" + defaultValue.unit() + ")";
+ this.unit = defaultValue.unit();
+ initDefault(defaultValue);
+ }
+
+ /** Set the default value. The default value can only be set once. */
+ private void initDefault(Measure defaultValue) {
+ hasDefault = true;
+ this.defaultValue = defaultValue;
+ if (GompeiLib.isTuning()) {
+ // We store the raw double value in the base unit
+ dashboardNumber = new LoggedNetworkNumber(key, defaultValue.in(unit));
+ }
+ }
+
+ /** Get the current measure, from dashboard if available and in tuning mode. */
+ @Override
+ @SuppressWarnings("unchecked") // safe
+ public Measure get() {
+ return (Measure) unit.of(getRawValue());
+ }
+
+ /**
+ * Get the current value converted to the specified unit. The unit must be of the same dimension
+ * (e.g., CurrentUnit for current values).
+ *
+ * @param targetUnit The unit to convert the value to (must be compatible with the base unit)
+ * @return The current value in the specified unit
+ */
+ @SuppressWarnings("unchecked")
+ public double get(Unit targetUnit) {
+ return get().in((U) targetUnit);
+ }
+
+ public double getRawValue() {
+ return GompeiLib.isTuning() ? dashboardNumber.get() : defaultValue.in(unit);
+ }
+
+ /** Checks whether the measure has changed since our last check */
+ public boolean hasChanged(int id) {
+ Measure currentValue = get();
+ Measure lastValue = lastHasChangedValues.get(id);
+ if (!currentValue.equals(lastValue)) {
+ lastHasChangedValues.put(id, currentValue);
+ return true;
+ }
+ return false;
+ }
+
+ /** Runs action if any of the tunableMeasures have changed */
+ @SafeVarargs
+ public static void ifChanged(
+ int id, Consumer[]> action, LoggedTunableMeasure... tunableNumbers) {
+ if (Arrays.stream(tunableNumbers).anyMatch(n -> n.hasChanged(id))) {
+ @SuppressWarnings("unchecked")
+ Measure[] values =
+ Arrays.stream(tunableNumbers).map(LoggedTunableMeasure::get).toArray(Measure[]::new);
+ action.accept(values);
+ }
+ }
+
+ public static void ifChanged(int id, Runnable action, LoggedTunableMeasure... tunableNumbers) {
+ if (Arrays.stream(tunableNumbers).anyMatch(n -> n.hasChanged(id))) {
+ action.run();
+ }
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/tunable/LoggedTunableNumber.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/tunable/LoggedTunableNumber.java
new file mode 100644
index 00000000..07b0db43
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/tunable/LoggedTunableNumber.java
@@ -0,0 +1,123 @@
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package edu.wpi.team190.gompeilib.core.utility.tunable;
+
+import edu.wpi.team190.gompeilib.core.GompeiLib;
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.function.Consumer;
+import java.util.function.DoubleSupplier;
+import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
+
+/**
+ * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or
+ * value not in dashboard.
+ */
+public class LoggedTunableNumber implements DoubleSupplier {
+ private static final String tableKey = "TunableNumbers";
+
+ private final String key;
+ private boolean hasDefault = false;
+ private double defaultValue;
+ private LoggedNetworkNumber dashboardNumber;
+ private final Map lastHasChangedValues = new HashMap<>();
+
+ /**
+ * Create a new LoggedTunableNumber
+ *
+ * @param dashboardKey Key on dashboard
+ */
+ public LoggedTunableNumber(String dashboardKey) {
+ this.key = tableKey + "/" + dashboardKey;
+ }
+
+ /**
+ * Create a new LoggedTunableNumber with the default value
+ *
+ * @param dashboardKey Key on dashboard
+ * @param defaultValue Default value
+ */
+ public LoggedTunableNumber(String dashboardKey, double defaultValue) {
+ this(dashboardKey);
+ initDefault(defaultValue);
+ }
+
+ /**
+ * Set the default value of the number. The default value can only be set once.
+ *
+ * @param defaultValue The default value
+ */
+ public void initDefault(double defaultValue) {
+ if (!hasDefault) {
+ hasDefault = true;
+ this.defaultValue = defaultValue;
+ if (GompeiLib.isTuning()) {
+ dashboardNumber = new LoggedNetworkNumber(key, defaultValue);
+ }
+ }
+ }
+
+ /**
+ * Get the current value, from dashboard if available and in tuning mode.
+ *
+ * @return The current value
+ */
+ public double get() {
+ if (!hasDefault) {
+ return 0.0;
+ } else {
+ return GompeiLib.isTuning() ? dashboardNumber.get() : defaultValue;
+ }
+ }
+
+ /**
+ * Checks whether the number has changed since our last check
+ *
+ * @param id Unique identifier for the caller to avoid conflicts when shared between multiple
+ * objects. Recommended approach is to pass the result of "hashCode()"
+ * @return True if the number has changed since the last time this method was called, false
+ * otherwise.
+ */
+ public boolean hasChanged(int id) {
+ double currentValue = get();
+ Double lastValue = lastHasChangedValues.get(id);
+ if (lastValue == null || currentValue != lastValue) {
+ lastHasChangedValues.put(id, currentValue);
+ return true;
+ }
+
+ return false;
+ }
+
+ /**
+ * Runs action if any of the tunableNumbers have changed
+ *
+ * @param id Unique identifier for the caller to avoid conflicts when shared between multiple *
+ * objects. Recommended approach is to pass the result of "hashCode()"
+ * @param action Callback to run when any of the tunable numbers have changed. Access tunable
+ * numbers in order inputted in method
+ * @param tunableNumbers All tunable numbers to check
+ */
+ public static void ifChanged(
+ int id, Consumer action, LoggedTunableNumber... tunableNumbers) {
+ if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) {
+ action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray());
+ }
+ }
+
+ /** Runs action if any of the tunableNumbers have changed */
+ public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) {
+ ifChanged(id, values -> action.run(), tunableNumbers);
+ }
+
+ @Override
+ public double getAsDouble() {
+ return get();
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/tunable/TunableUpdaterRegistry.java b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/tunable/TunableUpdaterRegistry.java
new file mode 100644
index 00000000..b08556c6
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/core/utility/tunable/TunableUpdaterRegistry.java
@@ -0,0 +1,63 @@
+package edu.wpi.team190.gompeilib.core.utility.tunable;
+
+import edu.wpi.team190.gompeilib.core.utility.control.Gains;
+import edu.wpi.team190.gompeilib.core.utility.control.constraints.Constraints;
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.function.Consumer;
+
+/**
+ * Central registry for tunable objects that need to be polled for runtime updates.
+ *
+ * Each registry maps a tunable object or group of tunable objects to the action that should run
+ * when a value changes. The {@link #periodic()} method should be called regularly to detect changes
+ * and invoke the corresponding update callbacks.
+ */
+public class TunableUpdaterRegistry {
+
+ private TunableUpdaterRegistry() {}
+
+ /** Registered gain sets and their update callbacks. */
+ private static final HashMap> GAINS_UPDATER = new HashMap<>();
+
+ /** Registered constraint sets and their update callbacks. */
+ private static final HashMap, Consumer> CONSTRAINTS_UPDATER = new HashMap<>();
+
+ /** Registered tunable number groups and their update callbacks. */
+ private static final HashMap> NUMBER_UPDATER =
+ new HashMap<>();
+
+ /** Registered tunable measure groups and their update callbacks. */
+ private static final HashMap[], Runnable> MEASURE_UPDATER =
+ new HashMap<>();
+
+ /**
+ * Checks all registered tunables for changes and runs any associated update callbacks.
+ *
+ * This method is intended to be called periodically, once per robot loop, so runtime tuning
+ * changes are detected and applied.
+ */
+ public static void periodic() {
+ GAINS_UPDATER.forEach((k, v) -> k.update(k.hashCode(), v));
+ CONSTRAINTS_UPDATER.forEach((k, v) -> k.update(k.hashCode(), v));
+ NUMBER_UPDATER.forEach((k, v) -> LoggedTunableNumber.ifChanged(Arrays.hashCode(k), v, k));
+ MEASURE_UPDATER.forEach((k, v) -> LoggedTunableMeasure.ifChanged(Arrays.hashCode(k), v, k));
+ }
+
+ public static void registerGains(Gains g, Consumer c) {
+ GAINS_UPDATER.putIfAbsent(g, c);
+ }
+
+ public static void registerConstraints(
+ Constraints constraints, Consumer> consumer) {
+ CONSTRAINTS_UPDATER.putIfAbsent(constraints, consumer);
+ }
+
+ public static void registerNumber(LoggedTunableNumber[] numbers, Consumer consumer) {
+ NUMBER_UPDATER.putIfAbsent(numbers, consumer);
+ }
+
+ public static void registerMeasure(LoggedTunableMeasure[] measures, Runnable consumer) {
+ MEASURE_UPDATER.putIfAbsent(measures, consumer);
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/Arm.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/Arm.java
new file mode 100644
index 00000000..ff81c393
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/Arm.java
@@ -0,0 +1,183 @@
+package edu.wpi.team190.gompeilib.subsystems.arm;
+
+import static edu.wpi.first.units.Units.*;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.AngleUnit;
+import edu.wpi.first.units.VoltageUnit;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.Voltage;
+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.sysid.SysIdRoutine;
+import edu.wpi.team190.gompeilib.core.logging.Trace;
+import edu.wpi.team190.gompeilib.core.utility.Setpoint;
+import edu.wpi.team190.gompeilib.core.utility.control.Gains;
+import edu.wpi.team190.gompeilib.core.utility.control.constraints.AngularPositionConstraints;
+import edu.wpi.team190.gompeilib.core.utility.phoenix.GainSlot;
+import lombok.Getter;
+import org.littletonrobotics.junction.Logger;
+
+public class Arm {
+ public ArmIO io;
+ public ArmIOInputsAutoLogged inputs;
+
+ private final String aKitTopic;
+
+ @Getter private ArmState currentState;
+
+ @Getter private Setpoint voltageGoal;
+ @Getter private Setpoint positionGoal;
+
+ private final SysIdRoutine characterizationRoutine;
+
+ public Arm(
+ ArmIO io,
+ Subsystem subsystem,
+ int index,
+ ArmConstants constants,
+ Setpoint positionGoal,
+ Setpoint voltageGoal) {
+ this.io = io;
+ this.inputs = new ArmIOInputsAutoLogged();
+
+ aKitTopic = subsystem.getName() + "/Arms" + index;
+
+ currentState = ArmState.IDLE;
+
+ this.positionGoal = positionGoal;
+ this.voltageGoal = voltageGoal;
+
+ characterizationRoutine =
+ new SysIdRoutine(
+ new SysIdRoutine.Config(
+ Volts.of(1).per(Second),
+ Volts.of(9),
+ Seconds.of(12),
+ (state) -> Logger.recordOutput(aKitTopic + "/SysIdState", state.toString())),
+ new SysIdRoutine.Mechanism(io::setVoltageGoal, null, subsystem));
+ }
+
+ public Arm(ArmIO io, Subsystem subsystem, int index, ArmConstants constants) {
+ this(
+ io,
+ subsystem,
+ index,
+ constants,
+ new Setpoint<>(
+ Rotation2d.kZero.getMeasure(),
+ constants.positionOffsetStep.getMeasure(),
+ constants.armParameters.maxAngle().getMeasure(),
+ constants.armParameters.minAngle().getMeasure()),
+ new Setpoint<>(Volts.of(0), constants.voltageOffsetStep, Volts.of(-12), Volts.of(12)));
+ }
+
+ public Arm(
+ ArmIO io,
+ Subsystem subsystem,
+ int index,
+ ArmConstants constants,
+ Setpoint positionGoal) {
+ this(
+ io,
+ subsystem,
+ index,
+ constants,
+ positionGoal,
+ new Setpoint<>(Volts.of(0), constants.voltageOffsetStep, Volts.of(-12), Volts.of(12)));
+ }
+
+ @Trace
+ public void periodic() {
+ io.updateInputs(inputs);
+ Logger.processInputs(aKitTopic, inputs);
+
+ Logger.recordOutput(aKitTopic + "/State", currentState.name());
+ Logger.recordOutput(aKitTopic + "/Voltage Goal", voltageGoal.getSetpoint());
+ Logger.recordOutput(
+ aKitTopic + "/Position Goal", new Rotation2d((Angle) positionGoal.getSetpoint()));
+ Logger.recordOutput(aKitTopic + "/Voltage Offset", voltageGoal.getOffset());
+ Logger.recordOutput(aKitTopic + "/Position Offset", positionGoal.getOffset());
+ Logger.recordOutput(aKitTopic + "/At Voltage Goal", atVoltageGoal());
+ Logger.recordOutput(aKitTopic + "/At Position Goal", atPositionGoal());
+
+ switch (currentState) {
+ case OPEN_LOOP_VOLTAGE_CONTROL -> io.setVoltageGoal((Voltage) voltageGoal.getNewSetpoint());
+ case CLOSED_LOOP_POSITION_CONTROL ->
+ io.setPositionGoal(new Rotation2d((Angle) positionGoal.getNewSetpoint()));
+ }
+ }
+
+ public Rotation2d getArmPosition() {
+ return inputs.position;
+ }
+
+ public void setVoltageGoal(Voltage voltageGoal) {
+ currentState = ArmState.OPEN_LOOP_VOLTAGE_CONTROL;
+ this.voltageGoal.setSetpoint(voltageGoal);
+ }
+
+ public void setVoltageGoal(Setpoint voltageGoal) {
+ currentState = ArmState.OPEN_LOOP_VOLTAGE_CONTROL;
+ this.voltageGoal = voltageGoal;
+ }
+
+ public void setPositionGoal(Rotation2d positionGoal) {
+ currentState = ArmState.CLOSED_LOOP_POSITION_CONTROL;
+ this.positionGoal.setSetpoint(positionGoal.getMeasure());
+ }
+
+ public void setPositionGoal(Setpoint positionGoal) {
+ currentState = ArmState.CLOSED_LOOP_POSITION_CONTROL;
+ this.positionGoal = positionGoal;
+ }
+
+ public boolean atVoltageGoal(Voltage voltageReference) {
+ return io.atVoltageGoal(voltageReference);
+ }
+
+ public boolean atPositionGoal(Rotation2d positionReference) {
+ return io.atPositionGoal(positionReference);
+ }
+
+ public boolean atVoltageGoal() {
+ return atVoltageGoal((Voltage) voltageGoal.getNewSetpoint());
+ }
+
+ public boolean atPositionGoal() {
+ return atPositionGoal(new Rotation2d((Angle) positionGoal.getNewSetpoint()));
+ }
+
+ public void setPosition(Rotation2d position) {
+ io.setPosition(position);
+ }
+
+ public void setGainSlot(GainSlot slot) {
+ io.setGainSlot(slot);
+ }
+
+ public Command waitUntilAtGoal() {
+ return Commands.waitUntil(this::atPositionGoal);
+ }
+
+ public void updateGains(Gains gains, GainSlot slot) {
+ io.updateGains(gains, slot);
+ }
+
+ public void updateConstraints(AngularPositionConstraints constraints) {
+ io.updateConstraints(constraints);
+ }
+
+ public Command sysIdRoutine() {
+ return Commands.sequence(
+ Commands.runOnce(() -> currentState = ArmState.IDLE),
+ characterizationRoutine.quasistatic(SysIdRoutine.Direction.kForward),
+ Commands.waitSeconds(1.0),
+ characterizationRoutine.quasistatic(SysIdRoutine.Direction.kReverse),
+ Commands.waitSeconds(1.0),
+ characterizationRoutine.dynamic(SysIdRoutine.Direction.kForward),
+ Commands.waitSeconds(1.0),
+ characterizationRoutine.dynamic(SysIdRoutine.Direction.kReverse));
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/ArmConstants.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/ArmConstants.java
new file mode 100644
index 00000000..3099c33a
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/ArmConstants.java
@@ -0,0 +1,39 @@
+package edu.wpi.team190.gompeilib.subsystems.arm;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.signals.InvertedValue;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.team190.gompeilib.core.utility.control.CurrentLimits;
+import edu.wpi.team190.gompeilib.core.utility.control.Gains;
+import edu.wpi.team190.gompeilib.core.utility.control.constraints.AngularPositionConstraints;
+import lombok.Builder;
+import lombok.NonNull;
+
+@Builder(setterPrefix = "with")
+public class ArmConstants {
+ @NonNull public final Integer armCANID;
+ @NonNull public final CANBus canBus;
+ @NonNull public final ArmParameters armParameters;
+ @NonNull public final Gains slot0Gains;
+ @Builder.Default public final Gains slot1Gains = Gains.builder().build();
+ @Builder.Default public final Gains slot2Gains = Gains.builder().build();
+ @NonNull public final AngularPositionConstraints constraints;
+ @NonNull public final CurrentLimits currentLimits;
+ @NonNull public final Boolean enableFOC;
+ @NonNull public final InvertedValue invertedValue;
+ @NonNull public final Voltage voltageOffsetStep;
+ @NonNull public final Rotation2d positionOffsetStep;
+
+ @Builder(setterPrefix = "with")
+ public record ArmParameters(
+ @NonNull DCMotor motorConfig,
+ @NonNull Rotation2d minAngle,
+ @NonNull Rotation2d maxAngle,
+ @NonNull Boolean continuousOutput,
+ @NonNull Integer numMotors,
+ @NonNull Double gearRatio,
+ @NonNull Double lengthMeters,
+ @NonNull Double momentOfInertia) {}
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/ArmIO.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/ArmIO.java
new file mode 100644
index 00000000..ed9e00c1
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/ArmIO.java
@@ -0,0 +1,98 @@
+package edu.wpi.team190.gompeilib.subsystems.arm;
+
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.measure.*;
+import edu.wpi.team190.gompeilib.core.utility.control.Gains;
+import edu.wpi.team190.gompeilib.core.utility.control.constraints.AngularPositionConstraints;
+import edu.wpi.team190.gompeilib.core.utility.phoenix.GainSlot;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ArmIO {
+ @AutoLog
+ public static class ArmIOInputs {
+ public Rotation2d position = new Rotation2d();
+ public AngularVelocity velocity = RadiansPerSecond.of(0.0);
+ public AngularAcceleration acceleration = RadiansPerSecondPerSecond.of(0.0);
+
+ public double[] appliedVolts = new double[] {};
+ public double[] supplyCurrentAmps = new double[] {};
+ public double[] torqueCurrentAmps = new double[] {};
+ public double[] temperatureCelsius = new double[] {};
+
+ public Rotation2d positionGoal = new Rotation2d();
+ public Rotation2d positionSetpoint = new Rotation2d();
+ public Rotation2d positionError = new Rotation2d();
+
+ public GainSlot gainSlot = GainSlot.ZERO;
+ }
+
+ /**
+ * Updates the inputs for the arm.
+ *
+ * @param inputs The inputs to update.
+ */
+ default void updateInputs(ArmIOInputs inputs) {}
+
+ /**
+ * Sets the voltage for the arm.
+ *
+ * @param voltageGoal The voltage of the arm in volts
+ */
+ default void setVoltageGoal(Voltage voltageGoal) {}
+
+ /**
+ * Sets the position goal of the arm
+ *
+ * @param positionGoal the position goal of the arm
+ */
+ default void setPositionGoal(Rotation2d positionGoal) {}
+
+ /**
+ * Checks if the voltage of the arm matches the volts argument
+ *
+ * @param voltageReference The voltage to check against
+ * @return True if the voltage matches, false otherwise
+ */
+ default boolean atVoltageGoal(Voltage voltageReference) {
+ return false;
+ }
+
+ /**
+ * Checks if the position of the arm matches the positionGoal argument
+ *
+ * @param positionReference the position to check against
+ * @return True if the position matches, false otherwise
+ */
+ default boolean atPositionGoal(Rotation2d positionReference) {
+ return false;
+ }
+
+ /**
+ * Sets the position of the arm.
+ *
+ * @param position The position to set.
+ */
+ default void setPosition(Rotation2d position) {}
+
+ /**
+ * @param gainSlot The CTRE gain slot to set the arm to.
+ */
+ default void setGainSlot(GainSlot gainSlot) {}
+
+ /**
+ * Sets the gains for the arm.
+ *
+ * @param gains the gains to update
+ */
+ default void updateGains(Gains gains, GainSlot gainSlot) {}
+
+ /**
+ * Sets the constraints for the arm.
+ *
+ * @param constraints the constraints to update
+ */
+ default void updateConstraints(AngularPositionConstraints constraints) {}
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/ArmIOSim.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/ArmIOSim.java
new file mode 100644
index 00000000..9b5db8a9
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/ArmIOSim.java
@@ -0,0 +1,164 @@
+package edu.wpi.team190.gompeilib.subsystems.arm;
+
+import static edu.wpi.first.units.Units.*;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.units.measure.*;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import edu.wpi.team190.gompeilib.core.GompeiLib;
+import edu.wpi.team190.gompeilib.core.utility.control.Gains;
+import edu.wpi.team190.gompeilib.core.utility.control.constraints.AngularPositionConstraints;
+import edu.wpi.team190.gompeilib.core.utility.phoenix.GainSlot;
+import java.util.Arrays;
+
+public class ArmIOSim implements ArmIO {
+ private final SingleJointedArmSim armSim;
+
+ private Voltage appliedVolts;
+ private boolean isClosedLoop;
+ private GainSlot gainSlot;
+
+ private final ProfiledPIDController feedback;
+ private ArmFeedforward feedforward;
+
+ private final ArmConstants constants;
+
+ public ArmIOSim(ArmConstants constants) {
+ armSim =
+ new SingleJointedArmSim(
+ LinearSystemId.createSingleJointedArmSystem(
+ constants.armParameters.motorConfig(),
+ constants.armParameters.momentOfInertia(),
+ constants.armParameters.gearRatio()),
+ constants.armParameters.motorConfig(),
+ constants.armParameters.gearRatio(),
+ constants.armParameters.lengthMeters(),
+ constants.armParameters.minAngle().getRadians(),
+ constants.armParameters.maxAngle().getRadians(),
+ true,
+ constants.armParameters.minAngle().getRadians());
+
+ appliedVolts = Volts.of(0.0);
+ isClosedLoop = true;
+ gainSlot = GainSlot.ZERO;
+
+ feedback =
+ new ProfiledPIDController(
+ constants.slot0Gains.kP().get(),
+ 0.0,
+ constants.slot0Gains.kD().get(),
+ new Constraints(
+ constants.constraints.maxVelocity().get().in(RadiansPerSecond),
+ constants.constraints.maxAcceleration().get().in(RadiansPerSecondPerSecond)));
+ if (constants.armParameters.continuousOutput()) {
+ feedback.enableContinuousInput(
+ constants.armParameters.minAngle().getRadians(),
+ constants.armParameters.maxAngle().getRadians());
+ }
+ feedback.setTolerance(constants.constraints.goalTolerance().get().in(Radians));
+ feedforward =
+ new ArmFeedforward(
+ constants.slot0Gains.kS().get(),
+ constants.slot0Gains.kV().get(),
+ constants.slot0Gains.kA().get(),
+ constants.slot0Gains.kG().get());
+
+ this.constants = constants;
+ }
+
+ @Override
+ public void updateInputs(ArmIOInputs inputs) {
+ if (isClosedLoop)
+ appliedVolts =
+ Volts.of(
+ feedback.calculate(armSim.getAngleRads())
+ + feedforward.calculate(
+ feedback.getSetpoint().position, feedback.getSetpoint().velocity));
+
+ appliedVolts = Volts.of(MathUtil.clamp(appliedVolts.in(Volts), -12.0, 12.0));
+ armSim.setInputVoltage(appliedVolts.in(Volts));
+ armSim.update(GompeiLib.getLoopPeriod());
+
+ inputs.position = Rotation2d.fromRadians(armSim.getAngleRads());
+ inputs.velocity = RadiansPerSecond.of(armSim.getVelocityRadPerSec());
+
+ inputs.appliedVolts = new double[constants.armParameters.numMotors()];
+ inputs.supplyCurrentAmps = new double[constants.armParameters.numMotors()];
+ inputs.torqueCurrentAmps = new double[constants.armParameters.numMotors()];
+ inputs.temperatureCelsius = new double[constants.armParameters.numMotors()];
+
+ Arrays.fill(inputs.appliedVolts, appliedVolts.in(Volts));
+ Arrays.fill(inputs.supplyCurrentAmps, armSim.getCurrentDrawAmps());
+ Arrays.fill(inputs.torqueCurrentAmps, armSim.getCurrentDrawAmps());
+
+ inputs.positionGoal = Rotation2d.fromRadians(feedback.getGoal().position);
+ inputs.positionSetpoint = Rotation2d.fromRadians(feedback.getSetpoint().position);
+ inputs.positionError = Rotation2d.fromRadians(feedback.getPositionError());
+
+ inputs.gainSlot = gainSlot;
+ }
+
+ @Override
+ public void setVoltageGoal(Voltage voltageGoal) {
+ isClosedLoop = false;
+ this.appliedVolts = voltageGoal;
+ }
+
+ @Override
+ public void setPositionGoal(Rotation2d rotationGoal) {
+ isClosedLoop = true;
+ feedback.setGoal(rotationGoal.getRadians());
+ }
+
+ @Override
+ public boolean atVoltageGoal(Voltage voltageReference) {
+ return appliedVolts.isNear(voltageReference, Millivolts.of(500));
+ }
+
+ @Override
+ public boolean atPositionGoal(Rotation2d positionReference) {
+ return Math.abs(positionReference.getRadians() - armSim.getAngleRads())
+ < constants.constraints.goalTolerance().get(Radians);
+ }
+
+ @Override
+ public void setPosition(Rotation2d position) {
+ armSim.setState(position.getRadians(), 0);
+ }
+
+ @Override
+ public void setGainSlot(GainSlot gainSlot) {
+ this.gainSlot = gainSlot;
+ switch (gainSlot) {
+ case ZERO:
+ feedback.setPID(constants.slot0Gains.kP().get(), 0.0, constants.slot0Gains.kD().get());
+ break;
+ case ONE:
+ feedback.setPID(constants.slot1Gains.kP().get(), 0.0, constants.slot1Gains.kD().get());
+ break;
+ case TWO:
+ feedback.setPID(constants.slot2Gains.kP().get(), 0.0, constants.slot2Gains.kD().get());
+ break;
+ }
+ }
+
+ @Override
+ public void updateGains(Gains gains, GainSlot gainSlot) {
+ feedback.setPID(gains.kP().get(), gains.kI().get(), gains.kD().get());
+ feedforward = new ArmFeedforward(gains.kS().get(), gains.kG().get(), gains.kV().get());
+ }
+
+ @Override
+ public void updateConstraints(AngularPositionConstraints constraints) {
+ feedback.setConstraints(
+ new Constraints(
+ constraints.maxVelocity().get().in(RadiansPerSecond),
+ constraints.maxAcceleration().get().in(RadiansPerSecondPerSecond)));
+ feedback.setTolerance(constraints.goalTolerance().get().in(Radians));
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/ArmIOTalonFX.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/ArmIOTalonFX.java
new file mode 100644
index 00000000..34c310f2
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/ArmIOTalonFX.java
@@ -0,0 +1,268 @@
+package edu.wpi.team190.gompeilib.subsystems.arm;
+
+import static edu.wpi.first.units.Units.*;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.GravityTypeValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Temperature;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.team190.gompeilib.core.GompeiLib;
+import edu.wpi.team190.gompeilib.core.utility.control.Gains;
+import edu.wpi.team190.gompeilib.core.utility.control.constraints.AngularPositionConstraints;
+import edu.wpi.team190.gompeilib.core.utility.phoenix.GainSlot;
+import edu.wpi.team190.gompeilib.core.utility.phoenix.PhoenixUtil;
+import java.util.ArrayList;
+
+public class ArmIOTalonFX implements ArmIO {
+ protected final TalonFX talonFX;
+ private final TalonFX[] followTalonFX;
+
+ private final StatusSignal positionRotations;
+ private final StatusSignal velocityRotationsPerSecond;
+ private ArrayList> appliedVolts;
+ private ArrayList> supplyCurrentAmps;
+ private ArrayList> torqueCurrentAmps;
+ private ArrayList> temperatureCelsius;
+ private final StatusSignal positionSetpointRotations;
+ private final StatusSignal positionErrorRotations;
+
+ private StatusSignal[] statusSignals;
+
+ private final VoltageOut voltageRequest;
+ private final MotionMagicVoltage positionVoltageRequest;
+
+ private final TalonFXConfiguration config;
+
+ protected final ArmConstants constants;
+
+ public ArmIOTalonFX(ArmConstants constants) {
+ talonFX = new TalonFX(constants.armCANID, constants.canBus);
+ followTalonFX = new TalonFX[constants.armParameters.numMotors() - 1];
+
+ config = new TalonFXConfiguration();
+
+ config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+ config.CurrentLimits.SupplyCurrentLimit = constants.currentLimits.supplyCurrentLimit().in(Amps);
+ config.CurrentLimits.SupplyCurrentLimitEnable = true;
+ config.CurrentLimits.StatorCurrentLimit = constants.currentLimits.statorCurrentLimit().in(Amps);
+ config.CurrentLimits.StatorCurrentLimitEnable = true;
+ config.Feedback.SensorToMechanismRatio = constants.armParameters.gearRatio();
+ config.MotorOutput.Inverted = constants.invertedValue;
+
+ config.Slot0.withKP(constants.slot0Gains.kP().get())
+ .withKD(constants.slot0Gains.kD().get())
+ .withKS(constants.slot0Gains.kS().get())
+ .withKV(constants.slot0Gains.kV().get())
+ .withKA(constants.slot0Gains.kA().get())
+ .withKG(constants.slot0Gains.kG().get())
+ .withGravityType(GravityTypeValue.Arm_Cosine);
+
+ config.Slot1.withKP(constants.slot1Gains.kP().get())
+ .withKD(constants.slot1Gains.kD().get())
+ .withKS(constants.slot1Gains.kS().get())
+ .withKV(constants.slot1Gains.kV().get())
+ .withKA(constants.slot1Gains.kA().get())
+ .withKG(constants.slot1Gains.kG().get())
+ .withGravityType(GravityTypeValue.Arm_Cosine);
+
+ config.Slot2.withKP(constants.slot2Gains.kP().get())
+ .withKD(constants.slot2Gains.kD().get())
+ .withKS(constants.slot2Gains.kS().get())
+ .withKV(constants.slot2Gains.kV().get())
+ .withKA(constants.slot2Gains.kA().get())
+ .withKG(constants.slot2Gains.kG().get())
+ .withGravityType(GravityTypeValue.Arm_Cosine);
+
+ config.MotorOutput.Inverted = constants.invertedValue;
+ config.ClosedLoopGeneral.ContinuousWrap = constants.armParameters.continuousOutput();
+ config.MotionMagic =
+ new MotionMagicConfigs()
+ .withMotionMagicAcceleration(
+ constants.constraints.maxAcceleration().get().in(RotationsPerSecondPerSecond))
+ .withMotionMagicCruiseVelocity(
+ constants.constraints.maxVelocity().get().in(RotationsPerSecond));
+
+ PhoenixUtil.tryUntilOk(5, () -> talonFX.getConfigurator().apply(config, 0.25));
+
+ appliedVolts = new ArrayList<>();
+ supplyCurrentAmps = new ArrayList<>();
+ torqueCurrentAmps = new ArrayList<>();
+ temperatureCelsius = new ArrayList<>();
+
+ positionRotations = talonFX.getPosition();
+ velocityRotationsPerSecond = talonFX.getVelocity();
+ appliedVolts.add(talonFX.getMotorVoltage());
+ supplyCurrentAmps.add(talonFX.getSupplyCurrent());
+ torqueCurrentAmps.add(talonFX.getTorqueCurrent());
+ temperatureCelsius.add(talonFX.getDeviceTemp());
+
+ positionSetpointRotations = talonFX.getClosedLoopReference();
+ positionErrorRotations = talonFX.getClosedLoopError();
+
+ for (int i = 0; i < constants.armParameters.numMotors() - 1; i++) {
+ appliedVolts.add(followTalonFX[i].getMotorVoltage());
+ supplyCurrentAmps.add(followTalonFX[i].getSupplyCurrent());
+ torqueCurrentAmps.add(followTalonFX[i].getTorqueCurrent());
+ temperatureCelsius.add(followTalonFX[i].getDeviceTemp());
+ }
+
+ voltageRequest = new VoltageOut(0).withEnableFOC(constants.enableFOC);
+ positionVoltageRequest = new MotionMagicVoltage(0).withEnableFOC(constants.enableFOC);
+
+ var signalsList = new ArrayList>();
+
+ signalsList.add(positionRotations);
+ signalsList.add(velocityRotationsPerSecond);
+ signalsList.addAll(appliedVolts);
+ signalsList.addAll(supplyCurrentAmps);
+ signalsList.addAll(torqueCurrentAmps);
+ signalsList.addAll(temperatureCelsius);
+ signalsList.add(positionSetpointRotations);
+ signalsList.add(positionErrorRotations);
+
+ statusSignals = new StatusSignal[signalsList.size()];
+
+ for (int i = 0; i < signalsList.size(); i++) {
+ statusSignals[i] = signalsList.get(i);
+ }
+
+ BaseStatusSignal.setUpdateFrequencyForAll(1 / GompeiLib.getLoopPeriod(), statusSignals);
+
+ talonFX.optimizeBusUtilization();
+
+ PhoenixUtil.registerSignals(constants.canBus.isNetworkFD(), statusSignals);
+
+ talonFX.setPosition(0);
+
+ this.constants = constants;
+ }
+
+ @Override
+ public void updateInputs(ArmIOInputs inputs) {
+
+ inputs.position = new Rotation2d(positionRotations.getValue());
+ inputs.velocity = velocityRotationsPerSecond.getValue();
+
+ inputs.appliedVolts = new double[constants.armParameters.numMotors()];
+ inputs.supplyCurrentAmps = new double[constants.armParameters.numMotors()];
+ inputs.torqueCurrentAmps = new double[constants.armParameters.numMotors()];
+ inputs.temperatureCelsius = new double[constants.armParameters.numMotors()];
+
+ for (int i = 0; i < constants.armParameters.numMotors(); i++) {
+ inputs.appliedVolts[i] = appliedVolts.get(i).getValueAsDouble();
+ inputs.supplyCurrentAmps[i] = supplyCurrentAmps.get(i).getValueAsDouble();
+ inputs.torqueCurrentAmps[i] = torqueCurrentAmps.get(i).getValueAsDouble();
+ inputs.temperatureCelsius[i] = temperatureCelsius.get(i).getValueAsDouble();
+ }
+
+ inputs.positionGoal = new Rotation2d(positionVoltageRequest.getPositionMeasure());
+ inputs.positionSetpoint =
+ Rotation2d.fromRotations(positionSetpointRotations.getValueAsDouble());
+ inputs.positionError = Rotation2d.fromRotations(positionErrorRotations.getValueAsDouble());
+
+ inputs.gainSlot = GainSlot.integerToGainSlot(talonFX.getClosedLoopSlot().getValue());
+ }
+
+ @Override
+ public void setVoltageGoal(Voltage voltageGoal) {
+ talonFX.setControl(voltageRequest.withOutput(voltageGoal));
+ }
+
+ @Override
+ public void setPositionGoal(Rotation2d positionGoal) {
+ talonFX.setControl(positionVoltageRequest.withPosition(positionGoal.getRotations()));
+ }
+
+ @Override
+ public boolean atVoltageGoal(Voltage voltageReference) {
+ return appliedVolts.get(0).getValue().isNear(voltageReference, Millivolts.of(500));
+ }
+
+ @Override
+ public boolean atPositionGoal(Rotation2d positionReference) {
+ return Math.abs(positionRotations.getValueAsDouble() - positionReference.getRotations())
+ < constants.constraints.goalTolerance().get().in(Rotations);
+ }
+
+ @Override
+ public void setPosition(Rotation2d position) {
+ talonFX.setPosition(position.getRotations());
+ }
+
+ @Override
+ public void setGainSlot(GainSlot gainSlot) {
+ switch (gainSlot) {
+ case ONE:
+ talonFX.setControl(positionVoltageRequest.withSlot(1));
+ break;
+ case TWO:
+ talonFX.setControl(positionVoltageRequest.withSlot(2));
+ break;
+ default:
+ talonFX.setControl(positionVoltageRequest.withSlot(0));
+ }
+ }
+
+ @Override
+ public void updateGains(Gains gains, GainSlot gainSlot) {
+ switch (gainSlot) {
+ case ZERO:
+ config.Slot0.withKP(gains.kP().get())
+ .withKD(gains.kD().get())
+ .withKS(gains.kS().get())
+ .withKV(gains.kV().get())
+ .withKA(gains.kA().get())
+ .withKG(gains.kG().get());
+ break;
+ case ONE:
+ config.Slot1.withKP(gains.kP().get())
+ .withKD(gains.kD().get())
+ .withKS(gains.kS().get())
+ .withKV(gains.kV().get())
+ .withKA(gains.kA().get())
+ .withKG(gains.kG().get());
+ break;
+ case TWO:
+ default:
+ config.Slot2.withKP(gains.kP().get())
+ .withKD(gains.kD().get())
+ .withKS(gains.kS().get())
+ .withKV(gains.kV().get())
+ .withKA(gains.kA().get())
+ .withKG(gains.kG().get());
+ break;
+ }
+ PhoenixUtil.tryUntilOk(5, () -> talonFX.getConfigurator().apply(config));
+
+ for (int i = 0; i < constants.armParameters.numMotors() - 1; i++) {
+ int finalI = i;
+ PhoenixUtil.tryUntilOk(5, () -> followTalonFX[finalI].getConfigurator().apply(config));
+ }
+ }
+
+ @Override
+ public void updateConstraints(AngularPositionConstraints constraints) {
+ config.MotionMagic =
+ new MotionMagicConfigs()
+ .withMotionMagicAcceleration(
+ constraints.maxAcceleration().get(RotationsPerSecondPerSecond))
+ .withMotionMagicCruiseVelocity(constraints.maxVelocity().get(RotationsPerSecond));
+ PhoenixUtil.tryUntilOk(5, () -> talonFX.getConfigurator().apply(config, 0.25));
+
+ for (int i = 0; i < constants.armParameters.numMotors() - 1; i++) {
+ int finalI = i;
+ PhoenixUtil.tryUntilOk(5, () -> followTalonFX[finalI].getConfigurator().apply(config, 0.25));
+ }
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/ArmIOTalonFXSim.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/ArmIOTalonFXSim.java
new file mode 100644
index 00000000..6a2087e5
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/ArmIOTalonFXSim.java
@@ -0,0 +1,59 @@
+package edu.wpi.team190.gompeilib.subsystems.arm;
+
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+
+import com.ctre.phoenix6.sim.TalonFXSimState;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import edu.wpi.team190.gompeilib.core.GompeiLib;
+import edu.wpi.team190.gompeilib.core.logging.Trace;
+
+public class ArmIOTalonFXSim extends ArmIOTalonFX {
+ private final SingleJointedArmSim armSim;
+
+ private final TalonFXSimState armController;
+
+ public ArmIOTalonFXSim(ArmConstants constants) {
+ super(constants);
+ armSim =
+ new SingleJointedArmSim(
+ LinearSystemId.createSingleJointedArmSystem(
+ constants.armParameters.motorConfig(),
+ constants.armParameters.momentOfInertia(),
+ constants.armParameters.gearRatio()),
+ constants.armParameters.motorConfig(),
+ constants.armParameters.gearRatio(),
+ constants.armParameters.lengthMeters(),
+ constants.armParameters.minAngle().getRadians(),
+ constants.armParameters.maxAngle().getRadians(),
+ true,
+ constants.armParameters.minAngle().getRadians());
+
+ armController = super.talonFX.getSimState();
+ }
+
+ @Override
+ @Trace
+ public void updateInputs(ArmIOInputs inputs) {
+ armController.setSupplyVoltage(RobotController.getBatteryVoltage());
+ double armVoltage = armController.getMotorVoltage();
+
+ armSim.setInputVoltage(armVoltage);
+
+ armSim.update(GompeiLib.getLoopPeriod());
+
+ Angle rotorPosition =
+ Angle.ofBaseUnits(armSim.getAngleRads() * constants.armParameters.gearRatio(), Radians);
+ AngularVelocity rotorVelocity =
+ AngularVelocity.ofBaseUnits(
+ armSim.getVelocityRadPerSec() * constants.armParameters.gearRatio(), RadiansPerSecond);
+ armController.setRawRotorPosition(rotorPosition);
+ armController.setRotorVelocity(rotorVelocity);
+
+ super.updateInputs(inputs);
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/ArmState.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/ArmState.java
new file mode 100644
index 00000000..46c8de97
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/arm/ArmState.java
@@ -0,0 +1,7 @@
+package edu.wpi.team190.gompeilib.subsystems.arm;
+
+public enum ArmState {
+ IDLE,
+ OPEN_LOOP_VOLTAGE_CONTROL,
+ CLOSED_LOOP_POSITION_CONTROL;
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveDrive.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveDrive.java
new file mode 100644
index 00000000..47937c3b
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveDrive.java
@@ -0,0 +1,491 @@
+// Copyright 2021-2024 FRC 6328
+package edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive;
+
+import choreo.auto.AutoFactory;
+import choreo.trajectory.SwerveSample;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.team190.gompeilib.core.GompeiLib;
+import edu.wpi.team190.gompeilib.core.io.components.inertial.GyroIO;
+import edu.wpi.team190.gompeilib.core.io.components.inertial.GyroIOInputsAutoLogged;
+import edu.wpi.team190.gompeilib.core.io.components.inertial.GyroIOPigeon2;
+import edu.wpi.team190.gompeilib.core.logging.Trace;
+import edu.wpi.team190.gompeilib.core.utility.control.Gains;
+import edu.wpi.team190.gompeilib.core.utility.phoenix.PhoenixOdometryThread;
+import java.util.List;
+import java.util.Optional;
+import java.util.Queue;
+import java.util.function.Consumer;
+import java.util.function.Supplier;
+import java.util.stream.IntStream;
+import lombok.Getter;
+import org.littletonrobotics.junction.Logger;
+
+public class SwerveDrive extends SubsystemBase {
+ private final SwerveDriveConstants driveConstants;
+
+ private final GyroIO gyroIO;
+ private final GyroIOInputsAutoLogged gyroInputs;
+ private final SwerveModule[] modules;
+
+ private final LinearFilter xFilter;
+ private final LinearFilter yFilter;
+ private double filteredX;
+ private double filteredY;
+
+ private final SwerveDriveKinematics kinematics;
+ @Getter private Rotation2d rawGyroRotation;
+ private final SwerveModulePosition[] lastModulePositions;
+ @Getter private ChassisSpeeds measuredChassisSpeeds;
+
+ @Getter private final AutoFactory autoFactory;
+
+ private final Supplier robotPoseSupplier;
+
+ private final PIDController autoXController;
+ private final PIDController autoYController;
+ private final PIDController autoHeadingController;
+
+ private final Optional> yawTimestampQueue;
+ private final Optional> yawPositionQueue;
+
+ private RobotConfig config;
+
+ public SwerveDrive(
+ SwerveDriveConstants driveConstants,
+ GyroIO gyroIO,
+ SwerveModuleIO flModuleIO,
+ SwerveModuleIO frModuleIO,
+ SwerveModuleIO blModuleIO,
+ SwerveModuleIO brModuleIO,
+ Supplier robotPoseSupplier,
+ Consumer resetPoseConsumer) {
+ this.driveConstants = driveConstants;
+ this.gyroIO = gyroIO;
+ gyroInputs = new GyroIOInputsAutoLogged();
+ modules = new SwerveModule[4]; // FL, FR, BL, BR
+ modules[0] = new SwerveModule(driveConstants, flModuleIO, 0);
+ modules[1] = new SwerveModule(driveConstants, frModuleIO, 1);
+ modules[2] = new SwerveModule(driveConstants, blModuleIO, 2);
+ modules[3] = new SwerveModule(driveConstants, brModuleIO, 3);
+
+ xFilter = LinearFilter.movingAverage(10);
+ yFilter = LinearFilter.movingAverage(10);
+ filteredX = 0;
+ filteredY = 0;
+
+ kinematics = this.driveConstants.driveConfig.kinematics();
+ rawGyroRotation = new Rotation2d();
+ lastModulePositions = // For delta tracking
+ new SwerveModulePosition[] {
+ new SwerveModulePosition(),
+ new SwerveModulePosition(),
+ new SwerveModulePosition(),
+ new SwerveModulePosition()
+ };
+
+ autoFactory =
+ new AutoFactory(robotPoseSupplier, resetPoseConsumer, this::choreoDrive, true, this);
+
+ this.robotPoseSupplier = robotPoseSupplier;
+
+ boolean isGryoHighFrequency = gyroIO instanceof GyroIOPigeon2;
+
+ if (isGryoHighFrequency) {
+ // Start threads (no-op if no signals have been created)
+ PhoenixOdometryThread.getInstance(driveConstants).start();
+ this.yawTimestampQueue =
+ Optional.of(PhoenixOdometryThread.getInstance(driveConstants).makeTimestampQueue());
+ this.yawPositionQueue =
+ Optional.of(
+ PhoenixOdometryThread.getInstance(driveConstants).registerSignal(gyroIO.getYaw()));
+ } else {
+ this.yawTimestampQueue = Optional.empty();
+ this.yawPositionQueue = Optional.empty();
+ }
+
+ autoHeadingController =
+ new PIDController(
+ driveConstants.autoRotationGains.kP().get(),
+ 0.0,
+ driveConstants.autoRotationGains.kD().get(),
+ GompeiLib.getLoopPeriod());
+ autoXController =
+ new PIDController(
+ driveConstants.autoTranslationGains.kP().get(),
+ 0.0,
+ driveConstants.autoTranslationGains.kD().get(),
+ GompeiLib.getLoopPeriod());
+ autoYController =
+ new PIDController(
+ driveConstants.autoTranslationGains.kP().get(),
+ 0.0,
+ driveConstants.autoTranslationGains.kD().get(),
+ GompeiLib.getLoopPeriod());
+
+ autoHeadingController.enableContinuousInput(-Math.PI, Math.PI);
+ autoHeadingController.setTolerance(Units.degreesToRadians(1.0));
+
+ measuredChassisSpeeds = new ChassisSpeeds();
+
+ try {
+ config = RobotConfig.fromGUISettings();
+ } catch (Exception e) {
+ System.err.println("Error occurred while loading robot config: " + e.getMessage());
+ }
+
+ try {
+ AutoBuilder.configure(
+ this.robotPoseSupplier,
+ resetPoseConsumer, // resetPose
+ () -> getChassisSpeeds(), // get robotRelativeSpeeds
+ (speeds, feedforwards) -> {
+ List> forces =
+ IntStream.range(0, 4)
+ .mapToObj(
+ i ->
+ VecBuilder.fill(
+ feedforwards.robotRelativeForcesXNewtons()[i],
+ feedforwards.robotRelativeForcesYNewtons()[i]))
+ .toList();
+
+ runVelocity(speeds);
+ },
+ new PPHolonomicDriveController(
+ new PIDConstants(
+ driveConstants.autoTranslationGains.kP().getAsDouble(),
+ driveConstants.autoTranslationGains.kI().getAsDouble(),
+ driveConstants.autoTranslationGains.kD().getAsDouble()),
+ new PIDConstants(
+ driveConstants.autoRotationGains.kP().getAsDouble(),
+ driveConstants.autoRotationGains.kI().getAsDouble(),
+ driveConstants.autoRotationGains.kD().getAsDouble())),
+ com.pathplanner.lib.config.RobotConfig.fromGUISettings(),
+ () -> {
+ var alliance = DriverStation.getAlliance();
+ if (alliance.isPresent()) {
+ return alliance.get() == DriverStation.Alliance.Red;
+ }
+ return false;
+ },
+ this);
+ } catch (Exception e) {
+ throw new RuntimeException("Failed to load PathPlanner robot config", e);
+ }
+ }
+
+ @Trace
+ public void periodic() {
+ driveConstants.reentrantLock.lock();
+
+ if (yawTimestampQueue.isPresent() && yawPositionQueue.isPresent()) {
+ gyroIO.updateInputs(gyroInputs, yawTimestampQueue.get(), yawPositionQueue.get());
+ yawTimestampQueue.get().clear();
+ yawPositionQueue.get().clear();
+ } else {
+ gyroIO.updateInputs(gyroInputs);
+ }
+
+ for (int i = 0; i < 4; i++) {
+ modules[i].updateInputs();
+ }
+
+ driveConstants.reentrantLock.unlock();
+
+ Logger.processInputs("Drive/Gyro", gyroInputs);
+
+ for (int i = 0; i < 4; i++) {
+ modules[i].periodic();
+ }
+
+ // Stop moving when disabled
+ if (DriverStation.isDisabled()) {
+ for (var module : modules) {
+ module.stop();
+ }
+
+ Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {});
+ Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {});
+ }
+
+ Logger.recordOutput("SwerveStates/Measured", getModuleStates());
+ Logger.recordOutput("SwerveChassisSpeeds/Measured", measuredChassisSpeeds);
+
+ // Update odometry
+ double[] sampleTimestamps =
+ modules[0].getOdometryTimestamps(); // All signals are sampled together
+ int sampleCount = sampleTimestamps.length;
+ for (int i = 0; i < sampleCount; i++) {
+ // Read wheel positions and deltas from each module
+ SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];
+ SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4];
+ for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) {
+ modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i];
+ moduleDeltas[moduleIndex] =
+ new SwerveModulePosition(
+ modulePositions[moduleIndex].distanceMeters
+ - lastModulePositions[moduleIndex].distanceMeters,
+ modulePositions[moduleIndex].angle);
+ lastModulePositions[moduleIndex] = modulePositions[moduleIndex];
+ }
+
+ // Update gyro angle
+ if (gyroInputs.connected) {
+ // Use the real gyro angle
+ rawGyroRotation = gyroInputs.odometryYawPositions[i];
+ } else {
+ // Use the angle delta from the kinematics and module deltas
+ Twist2d twist = kinematics.toTwist2d(moduleDeltas);
+ rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta));
+ }
+
+ ChassisSpeeds chassisSpeeds = kinematics.toChassisSpeeds(getModuleStates());
+ measuredChassisSpeeds = chassisSpeeds;
+ Translation2d rawFieldRelativeVelocity =
+ new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond)
+ .rotateBy(getRawGyroRotation());
+
+ filteredX = xFilter.calculate(rawFieldRelativeVelocity.getX());
+ filteredY = yFilter.calculate(rawFieldRelativeVelocity.getY());
+ }
+ }
+
+ /**
+ * Runs the drive at the desired velocity.
+ *
+ * @param speeds Speeds in meters/sec
+ */
+ @Trace
+ public void runVelocity(ChassisSpeeds speeds) {
+ // Calculate module setpoints
+ ChassisSpeeds optimizedSpeeds = ChassisSpeeds.discretize(speeds, GompeiLib.getLoopPeriod());
+ SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(optimizedSpeeds);
+ SwerveDriveKinematics.desaturateWheelSpeeds(
+ setpointStates, driveConstants.driveConfig.maxLinearVelocityMetersPerSecond());
+
+ // Log unoptimized setpoints and setpoint speeds
+ Logger.recordOutput("SwerveStates/Setpoints", setpointStates);
+ Logger.recordOutput("SwerveChassisSpeeds/Setpoints", speeds);
+
+ // Send setpoints to modules
+ for (int i = 0; i < 4; i++) {
+ modules[i].runSetpoint(setpointStates[i], new SwerveModuleState());
+ }
+
+ // Log optimized setpoints (runSetpoint mutates each state)
+ Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates);
+ }
+
+ /**
+ * Runs the drive at the desired velocity and torque.
+ *
+ * @param speeds Speeds in meters/sec
+ */
+ @Trace
+ public void runVelocityTorque(ChassisSpeeds speeds, List> forces) {
+ if (forces.size() != 4) {
+ throw new IllegalArgumentException("Forces array must have 4 elements");
+ }
+ // Calculate module setpoints
+ ChassisSpeeds optimizedSpeeds = ChassisSpeeds.discretize(speeds, GompeiLib.getLoopPeriod());
+ SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(optimizedSpeeds);
+ SwerveModuleState[] setpointTorques = new SwerveModuleState[4];
+ SwerveDriveKinematics.desaturateWheelSpeeds(
+ setpointStates, driveConstants.driveConfig.maxLinearVelocityMetersPerSecond());
+
+ // Send setpoints to modules
+ for (int i = 0; i < 4; i++) {
+ Vector wheelDirection =
+ VecBuilder.fill(setpointStates[i].angle.getCos(), setpointStates[i].angle.getSin());
+ setpointTorques[i] =
+ new SwerveModuleState(
+ forces.get(i).dot(wheelDirection)
+ * driveConstants.driveConfig.frontLeft().DriveMotorGearRatio,
+ setpointStates[i].angle);
+
+ setpointStates[i].optimize(modules[i].getAngle());
+ setpointTorques[i].optimize(modules[i].getAngle());
+
+ modules[i].runSetpoint(setpointStates[i], setpointTorques[i]);
+ }
+
+ // Log optimized setpoints (runSetpoint mutates each state)
+ Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates);
+ Logger.recordOutput("SwerveStates/TorquesOptimized", setpointTorques);
+ }
+
+ /** Runs the drive in a straight line with the specified drive current. */
+ @Trace
+ public void runCharacterization(double amps) {
+ for (int i = 0; i < 4; i++) {
+ modules[i].runCharacterization(amps);
+ }
+ }
+
+ /** Stops the drive. */
+ @Trace
+ public void stop() {
+ runVelocity(new ChassisSpeeds());
+ }
+
+ /**
+ * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will
+ * return to their normal orientations the next time a nonzero velocity is requested.
+ */
+ @Trace
+ public void stopWithX() {
+ Rotation2d[] headings = new Rotation2d[4];
+ for (int i = 0; i < 4; i++) {
+ headings[i] = driveConstants.driveConfig.getModuleTranslations()[i].getAngle();
+ }
+ kinematics.resetHeadings(headings);
+ stop();
+ }
+
+ /** Returns the module states (turn angles and drive velocities) for all of the modules. */
+ @Trace
+ private SwerveModuleState[] getModuleStates() {
+ SwerveModuleState[] states = new SwerveModuleState[4];
+ for (int i = 0; i < 4; i++) {
+ states[i] = modules[i].getState();
+ }
+ return states;
+ }
+
+ /** Returns the module positions (turn angles and drive positions) for all of the modules. */
+ @Trace
+ public SwerveModulePosition[] getModulePositions() {
+ SwerveModulePosition[] states = new SwerveModulePosition[4];
+ for (int i = 0; i < 4; i++) {
+ states[i] = modules[i].getPosition();
+ }
+ return states;
+ }
+
+ /** Returns the measured chassis speeds of the robot. */
+ @Trace
+ private ChassisSpeeds getChassisSpeeds() {
+ return kinematics.toChassisSpeeds(getModuleStates());
+ }
+
+ /** Returns the position of each module in radians. */
+ @Trace
+ public double[] getWheelRadiusCharacterizationPositions() {
+ double[] values = new double[4];
+ for (int i = 0; i < 4; i++) {
+ values[i] = modules[i].getWheelRadiusCharacterizationPosition();
+ }
+ return values;
+ }
+
+ /** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */
+ @Trace
+ public double getFFCharacterizationVelocity() {
+ double output = 0.0;
+ for (int i = 0; i < 4; i++) {
+ output += modules[i].getFFCharacterizationVelocity() / 4.0;
+ }
+ return output;
+ }
+
+ /** Returns the maximum linear speed in meters per sec. */
+ @Trace
+ public double getMaxLinearSpeedMetersPerSec() {
+ return driveConstants.driveConfig.maxLinearVelocityMetersPerSecond();
+ }
+
+ /** Returns the maximum angular speed in radians per sec. */
+ @Trace
+ public double getMaxAngularSpeedRadPerSec() {
+ return getMaxLinearSpeedMetersPerSec() / driveConstants.driveConfig.driveBaseRadius();
+ }
+
+ /** Returns the field relative velocity in X and Y. */
+ @Trace
+ public Translation2d getFieldRelativeVelocity() {
+ return new Translation2d(filteredX, filteredY);
+ }
+
+ /** Returns the current yaw velocity */
+ @Trace
+ public double getYawVelocity() {
+ return gyroInputs.yawVelocityRadPerSec;
+ }
+
+ /** Sets PID gains for modules */
+ @Trace
+ public void setPIDGains(double drive_Kp, double drive_Kd, double turn_Kp, double turn_Kd) {
+ for (var module : modules) {
+ module.setPID(drive_Kp, drive_Kd, turn_Kp, turn_Kd);
+ }
+ }
+
+ /** Sets FF gains for modules */
+ @Trace
+ public void setFFGains(double kS, double kV) {
+ for (var module : modules) {
+ module.setFF(kS, kV);
+ }
+ }
+
+ /** Runs a choreo path from swerve samples */
+ @Trace
+ public void choreoDrive(SwerveSample sample) {
+ Pose2d pose = robotPoseSupplier.get();
+ double xFF = sample.vx;
+ double yFF = sample.vy;
+ double rotationFF = sample.omega;
+
+ double xFeedback = autoXController.calculate(pose.getX(), sample.x);
+ double yFeedback = autoYController.calculate(pose.getY(), sample.y);
+ double rotationFeedback =
+ autoHeadingController.calculate(pose.getRotation().getRadians(), sample.heading);
+
+ ChassisSpeeds velocity =
+ ChassisSpeeds.fromFieldRelativeSpeeds(
+ xFF + xFeedback,
+ yFF + yFeedback,
+ rotationFF + rotationFeedback,
+ Rotation2d.fromRadians(sample.heading));
+
+ runVelocity(velocity);
+ Logger.recordOutput("Auto/Setpoint", sample.getPose());
+ }
+
+ public void setAutoControllers(Gains translationGains, Gains rotationGains) {
+ autoXController.setPID(translationGains.kP().get(), 0.0, translationGains.kD().get());
+ autoYController.setPID(translationGains.kP().get(), 0.0, translationGains.kD().get());
+ autoHeadingController.setPID(rotationGains.kP().get(), 0.0, rotationGains.kD().get());
+ }
+
+ /**
+ * Updates current limits.
+ *
+ * @param driveCurrentLimit The drive current limit.
+ * @param turnCurrentLimit The turn current limit.
+ */
+ @Trace
+ public void updateCurrentLimits(double driveCurrentLimit, double turnCurrentLimit) {
+ for (SwerveModule s : modules) {
+ s.updateCurrentLimits(driveCurrentLimit, turnCurrentLimit);
+ }
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveDriveConstants.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveDriveConstants.java
new file mode 100644
index 00000000..04fb8eee
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveDriveConstants.java
@@ -0,0 +1,101 @@
+package edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.units.AngleUnit;
+import edu.wpi.first.units.DistanceUnit;
+import edu.wpi.team190.gompeilib.core.utility.control.Gains;
+import edu.wpi.team190.gompeilib.core.utility.control.constraints.AngularPositionConstraints;
+import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints;
+import edu.wpi.team190.gompeilib.core.utility.tunable.LoggedTunableMeasure;
+import java.util.concurrent.locks.ReentrantLock;
+import lombok.Builder;
+import lombok.NonNull;
+
+@Builder(setterPrefix = "with")
+public class SwerveDriveConstants {
+ @Builder.Default public final ReentrantLock reentrantLock = new ReentrantLock();
+
+ @NonNull public final DriveConfig driveConfig;
+
+ @NonNull public final Gains driveGains;
+ @NonNull public final Gains turnGains;
+
+ @NonNull public final Gains autoTranslationGains;
+ @NonNull public final Gains autoRotationGains;
+
+ @NonNull public final AutoAlignConstants autoAlignConstants;
+
+ @NonNull public final Double odometryFrequency;
+ @NonNull public final Double driverDeadband;
+ @NonNull public final Double operatorDeadband;
+
+ @Builder(setterPrefix = "with")
+ public record DriveConfig(
+ @NonNull CANBus canBus,
+ @NonNull Integer pigeon2Id,
+ @NonNull Double maxLinearVelocityMetersPerSecond,
+ @NonNull Double wheelRadiusMeters,
+ @NonNull DCMotor driveModel,
+ @NonNull DCMotor turnModel,
+ @NonNull
+ SwerveModuleConstants
+ frontLeft,
+ @NonNull
+ SwerveModuleConstants
+ frontRight,
+ @NonNull
+ SwerveModuleConstants
+ backLeft,
+ @NonNull
+ SwerveModuleConstants
+ backRight,
+ @NonNull SwerveModuleConstants.ClosedLoopOutputType driveClosedLoopOutputType,
+ @NonNull SwerveModuleConstants.ClosedLoopOutputType steerClosedLoopOutputType,
+ @NonNull Double bumperWidth,
+ @NonNull Double bumperLength,
+ @NonNull Double robotMassKilograms,
+ @NonNull Double trackWidth,
+ @NonNull Double robotMOI,
+ @NonNull Double moduleCurrentLimit,
+ @NonNull Double wheelCOF) {
+ public Double driveBaseRadius() {
+ return Math.hypot(
+ (Math.abs(frontLeft.LocationX) + Math.abs(frontRight.LocationX)) / 2.0,
+ (Math.abs(frontLeft.LocationY) + Math.abs(backLeft.LocationY)) / 2.0);
+ }
+
+ public Double maxAngularVelocity() {
+ return maxLinearVelocityMetersPerSecond / driveBaseRadius();
+ }
+
+ public Translation2d[] getModuleTranslations() {
+ return new Translation2d[] {
+ new Translation2d(frontLeft.LocationX, frontLeft.LocationY),
+ new Translation2d(frontRight.LocationX, frontRight.LocationY),
+ new Translation2d(backLeft.LocationX, backLeft.LocationY),
+ new Translation2d(backRight.LocationX, backRight.LocationY)
+ };
+ }
+
+ public SwerveDriveKinematics kinematics() {
+ return new SwerveDriveKinematics(getModuleTranslations());
+ }
+ }
+
+ @Builder(setterPrefix = "with")
+ public record AutoAlignConstants(
+ @NonNull Gains xGains,
+ @NonNull LinearConstraints xConstraints,
+ @NonNull Gains yGains,
+ @NonNull LinearConstraints yConstraints,
+ @NonNull Gains rotationGains,
+ @NonNull AngularPositionConstraints rotationConstraints,
+ @NonNull LoggedTunableMeasure linearThreshold,
+ @NonNull LoggedTunableMeasure angularThreshold) {}
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveModule.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveModule.java
new file mode 100644
index 00000000..955d17e1
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveModule.java
@@ -0,0 +1,169 @@
+package edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.team190.gompeilib.core.logging.Trace;
+import lombok.Getter;
+import org.littletonrobotics.junction.Logger;
+
+public class SwerveModule {
+ private final SwerveDriveConstants driveConstants;
+ private final SwerveModuleIO io;
+ private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
+ private final int index;
+
+ private final Alert driveDisconnectedAlert;
+ private final Alert turnDisconnectedAlert;
+ private final Alert turnEncoderDisconnectedAlert;
+ @Getter private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
+
+ public SwerveModule(SwerveDriveConstants driveConstants, SwerveModuleIO io, int index) {
+ this.driveConstants = driveConstants;
+ this.io = io;
+ this.index = index;
+ driveDisconnectedAlert =
+ new Alert(
+ "Disconnected drive motor on module " + Integer.toString(index) + ".",
+ AlertType.kError);
+ turnDisconnectedAlert =
+ new Alert(
+ "Disconnected turn motor on module " + Integer.toString(index) + ".", AlertType.kError);
+ turnEncoderDisconnectedAlert =
+ new Alert(
+ "Disconnected turn encoder on module " + Integer.toString(index) + ".",
+ AlertType.kError);
+ }
+
+ @Trace
+ public void updateInputs() {
+ io.updateInputs(inputs);
+ Logger.processInputs("Drive/Module" + Integer.toString(index), inputs);
+ }
+
+ @Trace
+ public void periodic() {
+ // Calculate positions for odometry
+ int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together
+ odometryPositions = new SwerveModulePosition[sampleCount];
+ for (int i = 0; i < sampleCount; i++) {
+ double positionMeters =
+ inputs.odometryDrivePositionsRadians[i] * driveConstants.driveConfig.wheelRadiusMeters();
+ Rotation2d angle = inputs.odometryTurnPositions[i];
+ odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
+ }
+
+ // Update alerts
+ driveDisconnectedAlert.set(!inputs.driveConnected);
+ turnDisconnectedAlert.set(!inputs.turnConnected);
+ turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected);
+ }
+
+ /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */
+ @Trace
+ public void runSetpoint(SwerveModuleState state, SwerveModuleState torqueFeedforward) {
+ // Optimize veloci[ty setpoint
+ state.optimize(getAngle());
+ state.cosineScale(inputs.turnPosition);
+
+ double wheelTorqueNewtonMeters = torqueFeedforward.speedMetersPerSecond;
+ // Apply setpoints
+ io.setDriveVelocity(
+ state.speedMetersPerSecond / driveConstants.driveConfig.wheelRadiusMeters(),
+ driveConstants
+ .driveConfig
+ .driveModel()
+ .getCurrent(
+ wheelTorqueNewtonMeters
+ / driveConstants.driveConfig.frontLeft().DriveMotorGearRatio));
+ io.setTurnPosition(state.angle);
+ }
+
+ /** Runs the module with the specified output while controlling to zero degrees. */
+ @Trace
+ public void runCharacterization(double amps) {
+ io.setDriveAmps(amps);
+ io.setTurnPosition(new Rotation2d());
+ }
+
+ /** Disables all outputs to motors. */
+ @Trace
+ public void stop() {
+ io.setDriveAmps(0.0);
+ io.setTurnAmps(0.0);
+ }
+
+ /** Returns the current turn angle of the module. */
+ @Trace
+ public Rotation2d getAngle() {
+ return inputs.turnPosition;
+ }
+
+ /** Returns the current drive position of the module in meters. */
+ @Trace
+ public double getPositionMeters() {
+ return inputs.drivePositionRadians * driveConstants.driveConfig.wheelRadiusMeters();
+ }
+
+ /** Returns the current drive velocity of the module in meters per second. */
+ @Trace
+ public double getVelocityMetersPerSec() {
+ return inputs.driveVelocityRadiansPerSecond * driveConstants.driveConfig.wheelRadiusMeters();
+ }
+
+ /** Returns the module position (turn angle and drive position). */
+ @Trace
+ public SwerveModulePosition getPosition() {
+ return new SwerveModulePosition(getPositionMeters(), getAngle());
+ }
+
+ /** Returns the module state (turn angle and drive velocity). */
+ @Trace
+ public SwerveModuleState getState() {
+ return new SwerveModuleState(getVelocityMetersPerSec(), getAngle());
+ }
+
+ /** Returns the timestamps of the samples received this cycle. */
+ @Trace
+ public double[] getOdometryTimestamps() {
+ return inputs.odometryTimestamps;
+ }
+
+ /** Returns the module position in radians. */
+ @Trace
+ public double getWheelRadiusCharacterizationPosition() {
+ return inputs.drivePositionRadians;
+ }
+
+ /** Returns the module velocity in rotations/sec (Phoenix native units). */
+ @Trace
+ public double getFFCharacterizationVelocity() {
+ return Units.radiansToRotations(inputs.driveVelocityRadiansPerSecond);
+ }
+
+ /** Sets module PID gains */
+ @Trace
+ public void setPID(double drive_Kp, double drive_Kd, double turn_Kp, double turn_Kd) {
+ io.setPID(drive_Kp, drive_Kd, turn_Kp, turn_Kd);
+ }
+
+ /** Sets module FF gains */
+ @Trace
+ public void setFF(double kS, double kV) {
+ io.setFeedforward(kS, kV);
+ }
+
+ /**
+ * Updates current limits.
+ *
+ * @param driveCurrentLimit The drive current limit.
+ * @param turnCurrentLimit The turn current limit.
+ */
+ @Trace
+ public void updateCurrentLimits(double driveCurrentLimit, double turnCurrentLimit) {
+ io.updateCurrentLimits(driveCurrentLimit, turnCurrentLimit);
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveModuleIO.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveModuleIO.java
new file mode 100644
index 00000000..90c99ce4
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveModuleIO.java
@@ -0,0 +1,67 @@
+package edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface SwerveModuleIO {
+ @AutoLog
+ public static class ModuleIOInputs {
+ public double drivePositionRadians = 0.0;
+ public double driveVelocityRadiansPerSecond = 0.0;
+ public double driveAppliedVolts = 0.0;
+ public double driveSupplyCurrentAmps = 0.0;
+ public double driveTorqueCurrentAmps = 0.0;
+ public double driveTemperatureCelcius = 0.0;
+ public double driveVelocitySetpointRadiansPerSecond = 0.0;
+ public double driveVelocityErrorRadiansPerSecond = 0.0;
+
+ public Rotation2d turnAbsolutePosition = new Rotation2d();
+ public Rotation2d turnPosition = new Rotation2d();
+ public double turnVelocityRadiansPerSecond = 0.0;
+ public double turnAppliedVolts = 0.0;
+ public double turnSupplyCurrentAmps = 0.0;
+ public double turnTorqueCurrentAmps = 0.0;
+ public double turnTemperatureCelcius = 0.0;
+ public Rotation2d turnPositionGoal = new Rotation2d();
+ public Rotation2d turnPositionSetpoint = new Rotation2d();
+ public Rotation2d turnPositionError = new Rotation2d();
+
+ public boolean driveConnected = false;
+ public boolean turnConnected = false;
+ public boolean turnEncoderConnected = false;
+
+ public double[] odometryTimestamps = new double[] {};
+ public double[] odometryDrivePositionsRadians = new double[] {};
+ public Rotation2d[] odometryTurnPositions = new Rotation2d[] {};
+ }
+
+ /** Updates the set of loggable inputs. */
+ public default void updateInputs(ModuleIOInputs inputs) {}
+
+ /** Run the drive motor at the specified open loop value. */
+ public default void setDriveAmps(double currentAmps) {}
+
+ /** Run the turn motor at the specified open loop value. */
+ public default void setTurnAmps(double currentAmps) {}
+
+ /** Run the drive motor at the specified velocity. */
+ public default void setDriveVelocity(
+ double velocityRadiansPerSecond, double currentFeedforward) {}
+
+ /** Run the turn motor to the specified rotation. */
+ public default void setTurnPosition(Rotation2d position) {}
+
+ /** Sets the module PID gains */
+ public default void setPID(double drive_Kp, double drive_Kd, double turn_Kp, double turn_Kd) {}
+
+ /** Sets the module FF gains */
+ public default void setFeedforward(double drive_Ks, double drive_Kv) {}
+
+ /**
+ * Updates current limits.
+ *
+ * @param driveCurrentLimit The drive current limit.
+ * @param turnCurrentLimit The turn current limit.
+ */
+ public default void updateCurrentLimits(double driveCurrentLimit, double turnCurrentLimit) {}
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveModuleIOSim.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveModuleIOSim.java
new file mode 100644
index 00000000..19bc8c72
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveModuleIOSim.java
@@ -0,0 +1,149 @@
+package edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive;
+
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import edu.wpi.team190.gompeilib.core.GompeiLib;
+
+/**
+ * Physics sim implementation of module IO. Simulation is not vendor-specific, but the sim models
+ * are configured using a set of module constants from Phoenix.
+ *
+ * Simulation is always based on voltage control.
+ */
+public class SwerveModuleIOSim implements SwerveModuleIO {
+ private static final double DRIVE_KP = 0.05;
+ private static final double DRIVE_KD = 0.0;
+ private static final double DRIVE_KS = 0.0;
+ private static final double DRIVE_KV_ROT =
+ 0.91035; // Same units as TunerConstants: (volt * secs) / rotation
+ private static final double DRIVE_KV = 1.0 / Units.rotationsToRadians(1.0 / DRIVE_KV_ROT);
+ private static final double TURN_KP = 8.0;
+ private static final double TURN_KD = 0.0;
+
+ private final DCMotorSim driveSim;
+ private final DCMotorSim turnSim;
+
+ private boolean driveClosedLoop;
+ private boolean turnClosedLoop;
+
+ private final PIDController driveController;
+ private final PIDController turnController;
+
+ private double driveFFVolts;
+ private double driveAppliedVolts;
+ private double turnAppliedVolts;
+
+ public SwerveModuleIOSim(
+ SwerveDriveConstants driveConstants,
+ SwerveModuleConstants
+ constants) {
+ // Create drive and turn sim models
+ driveSim =
+ new DCMotorSim(
+ LinearSystemId.createDCMotorSystem(
+ driveConstants.driveConfig.driveModel(),
+ constants.DriveInertia,
+ constants.DriveMotorGearRatio),
+ driveConstants.driveConfig.driveModel());
+ turnSim =
+ new DCMotorSim(
+ LinearSystemId.createDCMotorSystem(
+ driveConstants.driveConfig.turnModel(),
+ constants.SteerInertia,
+ constants.SteerMotorGearRatio),
+ driveConstants.driveConfig.turnModel());
+
+ driveClosedLoop = false;
+ turnClosedLoop = false;
+
+ driveController = new PIDController(DRIVE_KP, 0, DRIVE_KD);
+ turnController = new PIDController(TURN_KP, 0, TURN_KD);
+
+ driveFFVolts = 0.0;
+ driveAppliedVolts = 0.0;
+ turnAppliedVolts = 0.0;
+
+ // Enable wrapping for turn PID
+ turnController.enableContinuousInput(-Math.PI, Math.PI);
+ }
+
+ @Override
+ public void updateInputs(ModuleIOInputs inputs) {
+ // Run closed-loop control
+ if (driveClosedLoop) {
+ driveAppliedVolts =
+ driveFFVolts + driveController.calculate(driveSim.getAngularVelocityRadPerSec());
+ } else {
+ driveController.reset();
+ }
+ if (turnClosedLoop) {
+ turnAppliedVolts = turnController.calculate(turnSim.getAngularPositionRad());
+ } else {
+ turnController.reset();
+ }
+
+ // Update simulation state
+ driveSim.setInputVoltage(MathUtil.clamp(driveAppliedVolts, -12.0, 12.0));
+ turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0));
+ driveSim.update(GompeiLib.getLoopPeriod());
+ turnSim.update(GompeiLib.getLoopPeriod());
+
+ inputs.drivePositionRadians = driveSim.getAngularPositionRad();
+ inputs.driveVelocityRadiansPerSecond = driveSim.getAngularVelocityRadPerSec();
+ inputs.driveAppliedVolts = driveAppliedVolts;
+ inputs.driveSupplyCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps());
+ inputs.driveVelocitySetpointRadiansPerSecond = driveController.getSetpoint();
+ inputs.driveVelocityErrorRadiansPerSecond = driveController.getError();
+
+ inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad());
+ inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad());
+ inputs.turnVelocityRadiansPerSecond = turnSim.getAngularVelocityRadPerSec();
+ inputs.turnAppliedVolts = turnAppliedVolts;
+ inputs.turnSupplyCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps());
+ inputs.turnPositionGoal = Rotation2d.fromRadians(turnController.getSetpoint());
+ inputs.turnPositionSetpoint = Rotation2d.fromRadians(turnController.getSetpoint());
+ inputs.turnPositionError = Rotation2d.fromRadians(turnController.getError());
+
+ inputs.driveConnected = true;
+ inputs.turnConnected = true;
+ inputs.turnEncoderConnected = true;
+
+ inputs.odometryTimestamps = new double[] {Timer.getTimestamp()};
+ inputs.odometryDrivePositionsRadians = new double[] {inputs.drivePositionRadians};
+ inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition};
+ }
+
+ @Override
+ public void setDriveAmps(double currentAmps) {
+ driveClosedLoop = false;
+ driveAppliedVolts = currentAmps;
+ }
+
+ @Override
+ public void setTurnAmps(double currentAmps) {
+ turnClosedLoop = false;
+ turnAppliedVolts = currentAmps;
+ }
+
+ @Override
+ public void setDriveVelocity(double velocityRadiansPerSecond, double currentFeedforward) {
+ driveClosedLoop = true;
+ driveFFVolts =
+ DRIVE_KS * Math.signum(velocityRadiansPerSecond) + DRIVE_KV * velocityRadiansPerSecond;
+ driveController.setSetpoint(velocityRadiansPerSecond);
+ }
+
+ @Override
+ public void setTurnPosition(Rotation2d position) {
+ turnClosedLoop = true;
+ turnController.setSetpoint(position.getRadians());
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveModuleIOTalonFX.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveModuleIOTalonFX.java
new file mode 100644
index 00000000..55c89eec
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveModuleIOTalonFX.java
@@ -0,0 +1,330 @@
+package edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive;
+
+import static edu.wpi.team190.gompeilib.core.utility.phoenix.PhoenixUtil.tryUntilOk;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.*;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Temperature;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.team190.gompeilib.core.GompeiLib;
+import edu.wpi.team190.gompeilib.core.logging.Trace;
+import edu.wpi.team190.gompeilib.core.utility.phoenix.PhoenixOdometryThread;
+import edu.wpi.team190.gompeilib.core.utility.phoenix.PhoenixUtil;
+import java.util.Queue;
+
+/**
+ * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and
+ * CANcoder. Configured using a set of module constants from Phoenix.
+ *
+ * Device configuration and other behaviors not exposed by TunerConstants can be customized here.
+ */
+public class SwerveModuleIOTalonFX implements SwerveModuleIO {
+ protected final TalonFX driveTalonFX;
+ protected final TalonFX turnTalonFX;
+ protected final CANcoder cancoder;
+
+ private final TalonFXConfiguration driveConfig;
+ private final TalonFXConfiguration turnConfig;
+ private final CANcoderConfiguration cancoderConfig;
+
+ private final StatusSignal drivePositionRotations;
+ private final StatusSignal driveVelocityRotationsPerSecond;
+ private final StatusSignal driveAppliedVolts;
+ private final StatusSignal driveSupplyCurrentAmps;
+ private final StatusSignal driveTorqueCurrentAmps;
+ private final StatusSignal driveTemperatureCelcius;
+ private final StatusSignal driveVelocitySetpointRotationsPerSecond;
+ private final StatusSignal driveVelocityErrorRotationsPerSecond;
+
+ private final StatusSignal turnAbsolutePositionRotations;
+ private final StatusSignal turnPositionRotations;
+ private final StatusSignal turnVelocityRotationsPerSecond;
+ private final StatusSignal turnAppliedVolts;
+ private final StatusSignal turnSupplyCurrentAmps;
+ private final StatusSignal turnTorqueCurrentAmps;
+ private final StatusSignal turnTemperatureCelcius;
+ private Rotation2d turnPositionGoal;
+ private final StatusSignal turnPositionSetpointRotations;
+ private final StatusSignal turnPositionErrorRotations;
+
+ private final Queue timestampQueue;
+ private final Queue drivePositionQueue;
+ private final Queue turnPositionQueue;
+
+ private final TorqueCurrentFOC torqueCurrentRequest;
+ private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest;
+ private final VelocityVoltage velocityVoltageRequest;
+ private final MotionMagicTorqueCurrentFOC positionTorqueCurrentRequest;
+ private final MotionMagicVoltage positionVoltageRequest;
+
+ private final SwerveModuleConstants.ClosedLoopOutputType driveClosedLoopOutputType;
+ private final SwerveModuleConstants.ClosedLoopOutputType turnClosedLoopOutputType;
+
+ public SwerveModuleIOTalonFX(
+ SwerveDriveConstants driveConstants,
+ SwerveModuleConstants
+ constants) {
+ driveTalonFX = new TalonFX(constants.DriveMotorId, driveConstants.driveConfig.canBus());
+ turnTalonFX = new TalonFX(constants.SteerMotorId, driveConstants.driveConfig.canBus());
+ cancoder = new CANcoder(constants.EncoderId, driveConstants.driveConfig.canBus());
+
+ driveConfig = new TalonFXConfiguration();
+ driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+ driveConfig.Slot0 = constants.DriveMotorGains;
+ driveConfig.Feedback.SensorToMechanismRatio = constants.DriveMotorGearRatio;
+ driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = constants.SlipCurrent;
+ driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -constants.SlipCurrent;
+ driveConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod = GompeiLib.getLoopPeriod();
+ driveConfig.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent;
+ driveConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+ driveConfig.MotorOutput.Inverted =
+ constants.DriveMotorInverted
+ ? InvertedValue.Clockwise_Positive
+ : InvertedValue.CounterClockwise_Positive;
+ tryUntilOk(5, () -> driveTalonFX.getConfigurator().apply(driveConfig, 0.25));
+ tryUntilOk(5, () -> driveTalonFX.setPosition(0.0, 0.25));
+
+ turnConfig = new TalonFXConfiguration();
+ turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+ turnConfig.Slot0 = constants.SteerMotorGains;
+ turnConfig.Feedback.FeedbackRemoteSensorID = constants.EncoderId;
+ turnConfig.Feedback.FeedbackSensorSource =
+ switch (constants.FeedbackSource) {
+ case RemoteCANcoder -> FeedbackSensorSourceValue.RemoteCANcoder;
+ case FusedCANcoder -> FeedbackSensorSourceValue.FusedCANcoder;
+ case SyncCANcoder -> FeedbackSensorSourceValue.SyncCANcoder;
+ default ->
+ throw new IllegalArgumentException("Unexpected value: " + constants.FeedbackSource);
+ };
+ turnConfig.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio;
+ turnConfig.TorqueCurrent.PeakForwardTorqueCurrent = 40.0;
+ turnConfig.TorqueCurrent.PeakReverseTorqueCurrent = -40.0;
+ turnConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod = GompeiLib.getLoopPeriod();
+ turnConfig.CurrentLimits.StatorCurrentLimit = 40.0;
+ turnConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+ turnConfig.MotionMagic.MotionMagicCruiseVelocity = 100.0 / constants.SteerMotorGearRatio;
+ turnConfig.MotionMagic.MotionMagicAcceleration =
+ turnConfig.MotionMagic.MotionMagicCruiseVelocity / 0.100;
+ turnConfig.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio;
+ turnConfig.MotionMagic.MotionMagicExpo_kA = 0.1;
+ turnConfig.ClosedLoopGeneral.ContinuousWrap = true;
+ turnConfig.MotorOutput.Inverted =
+ constants.SteerMotorInverted
+ ? InvertedValue.Clockwise_Positive
+ : InvertedValue.CounterClockwise_Positive;
+ tryUntilOk(5, () -> turnTalonFX.getConfigurator().apply(turnConfig, 0.25));
+
+ cancoderConfig = constants.EncoderInitialConfigs;
+ cancoderConfig.MagnetSensor.MagnetOffset = constants.EncoderOffset;
+ tryUntilOk(5, () -> cancoder.getConfigurator().apply(cancoderConfig, 0.25));
+
+ drivePositionRotations = driveTalonFX.getPosition();
+ driveVelocityRotationsPerSecond = driveTalonFX.getVelocity();
+ driveAppliedVolts = driveTalonFX.getMotorVoltage();
+ driveSupplyCurrentAmps = driveTalonFX.getSupplyCurrent();
+ driveTorqueCurrentAmps = driveTalonFX.getTorqueCurrent();
+ driveTemperatureCelcius = driveTalonFX.getDeviceTemp();
+ driveVelocitySetpointRotationsPerSecond = driveTalonFX.getClosedLoopReference();
+ driveVelocityErrorRotationsPerSecond = driveTalonFX.getClosedLoopError();
+
+ turnAbsolutePositionRotations = cancoder.getAbsolutePosition();
+ turnPositionRotations = turnTalonFX.getPosition();
+ turnVelocityRotationsPerSecond = turnTalonFX.getVelocity();
+ turnAppliedVolts = turnTalonFX.getMotorVoltage();
+ turnSupplyCurrentAmps = turnTalonFX.getSupplyCurrent();
+ turnTorqueCurrentAmps = turnTalonFX.getTorqueCurrent();
+ turnTemperatureCelcius = turnTalonFX.getDeviceTemp();
+ turnPositionGoal = new Rotation2d();
+ turnPositionSetpointRotations = turnTalonFX.getClosedLoopReference();
+ turnPositionErrorRotations = turnTalonFX.getClosedLoopError();
+
+ timestampQueue = PhoenixOdometryThread.getInstance(driveConstants).makeTimestampQueue();
+ drivePositionQueue =
+ PhoenixOdometryThread.getInstance(driveConstants)
+ .registerSignal(driveTalonFX.getPosition());
+ turnPositionQueue =
+ PhoenixOdometryThread.getInstance(driveConstants).registerSignal(turnTalonFX.getPosition());
+
+ torqueCurrentRequest = new TorqueCurrentFOC(0.0);
+ velocityTorqueCurrentRequest = new VelocityTorqueCurrentFOC(0.0);
+ velocityVoltageRequest = new VelocityVoltage(0.0);
+ positionTorqueCurrentRequest = new MotionMagicTorqueCurrentFOC(0.0);
+ positionVoltageRequest = new MotionMagicVoltage(0.0);
+
+ driveClosedLoopOutputType = constants.DriveMotorClosedLoopOutput;
+ turnClosedLoopOutputType = constants.SteerMotorClosedLoopOutput;
+
+ // Configure periodic frames
+ BaseStatusSignal.setUpdateFrequencyForAll(
+ driveConstants.odometryFrequency,
+ drivePositionRotations,
+ turnPositionRotations,
+ turnAbsolutePositionRotations);
+ BaseStatusSignal.setUpdateFrequencyForAll(
+ 50.0,
+ driveVelocityRotationsPerSecond,
+ driveAppliedVolts,
+ driveSupplyCurrentAmps,
+ driveTorqueCurrentAmps,
+ driveTemperatureCelcius,
+ driveVelocitySetpointRotationsPerSecond,
+ driveVelocityErrorRotationsPerSecond,
+ turnVelocityRotationsPerSecond,
+ turnAppliedVolts,
+ turnSupplyCurrentAmps,
+ turnTorqueCurrentAmps,
+ turnTemperatureCelcius,
+ turnPositionSetpointRotations,
+ turnPositionErrorRotations);
+
+ driveTalonFX.optimizeBusUtilization();
+ turnTalonFX.optimizeBusUtilization();
+ cancoder.optimizeBusUtilization();
+
+ PhoenixUtil.registerSignals(
+ true,
+ drivePositionRotations,
+ turnPositionRotations,
+ turnAbsolutePositionRotations,
+ driveVelocityRotationsPerSecond,
+ driveAppliedVolts,
+ driveSupplyCurrentAmps,
+ driveTorqueCurrentAmps,
+ driveTemperatureCelcius,
+ driveVelocitySetpointRotationsPerSecond,
+ driveVelocityErrorRotationsPerSecond,
+ turnVelocityRotationsPerSecond,
+ turnAppliedVolts,
+ turnSupplyCurrentAmps,
+ turnTorqueCurrentAmps,
+ turnTemperatureCelcius,
+ turnPositionSetpointRotations,
+ turnPositionErrorRotations);
+ }
+
+ @Override
+ @Trace
+ public void updateInputs(ModuleIOInputs inputs) {
+ inputs.drivePositionRadians =
+ Units.rotationsToRadians(drivePositionRotations.getValueAsDouble());
+ inputs.driveVelocityRadiansPerSecond =
+ Units.rotationsToRadians(driveVelocityRotationsPerSecond.getValueAsDouble());
+ inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
+ inputs.driveSupplyCurrentAmps = driveSupplyCurrentAmps.getValueAsDouble();
+ inputs.driveTorqueCurrentAmps = driveTorqueCurrentAmps.getValueAsDouble();
+ inputs.driveTemperatureCelcius = driveTemperatureCelcius.getValueAsDouble();
+ inputs.driveVelocitySetpointRadiansPerSecond =
+ Units.rotationsToRadians(driveVelocitySetpointRotationsPerSecond.getValueAsDouble());
+ inputs.driveVelocityErrorRadiansPerSecond =
+ Units.rotationsToRadians(driveVelocityErrorRotationsPerSecond.getValueAsDouble());
+
+ inputs.turnAbsolutePosition =
+ Rotation2d.fromRotations(turnAbsolutePositionRotations.getValueAsDouble());
+ inputs.turnPosition = Rotation2d.fromRotations(turnPositionRotations.getValueAsDouble());
+ inputs.turnVelocityRadiansPerSecond =
+ Units.rotationsToRadians(turnVelocityRotationsPerSecond.getValueAsDouble());
+ inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble();
+ inputs.turnSupplyCurrentAmps = turnSupplyCurrentAmps.getValueAsDouble();
+ inputs.turnTorqueCurrentAmps = turnTorqueCurrentAmps.getValueAsDouble();
+ inputs.turnTemperatureCelcius = turnTemperatureCelcius.getValueAsDouble();
+ inputs.turnPositionGoal = turnPositionGoal;
+ inputs.turnPositionSetpoint =
+ Rotation2d.fromRotations(turnPositionSetpointRotations.getValueAsDouble());
+ inputs.turnPositionError =
+ Rotation2d.fromRotations(turnPositionErrorRotations.getValueAsDouble());
+
+ inputs.odometryTimestamps =
+ timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
+ inputs.odometryDrivePositionsRadians =
+ drivePositionQueue.stream().mapToDouble(Units::rotationsToRadians).toArray();
+ inputs.odometryTurnPositions =
+ turnPositionQueue.stream().map(Rotation2d::fromRotations).toArray(Rotation2d[]::new);
+
+ timestampQueue.clear();
+ drivePositionQueue.clear();
+ turnPositionQueue.clear();
+ }
+
+ @Override
+ @Trace
+ public void setDriveAmps(double currentAmps) {
+ driveTalonFX.setControl(torqueCurrentRequest.withOutput(currentAmps));
+ }
+
+ @Override
+ @Trace
+ public void setTurnAmps(double currentAmps) {
+ turnTalonFX.setControl(torqueCurrentRequest.withOutput(currentAmps));
+ }
+
+ @Override
+ @Trace
+ public void setDriveVelocity(double velocityRadiansPerSecond, double currentFeedforward) {
+ if (driveClosedLoopOutputType.equals(SwerveModuleConstants.ClosedLoopOutputType.Voltage)) {
+ driveTalonFX.setControl(
+ velocityVoltageRequest
+ .withVelocity(Units.radiansToRotations(velocityRadiansPerSecond))
+ .withFeedForward(currentFeedforward));
+ } else if (driveClosedLoopOutputType.equals(
+ SwerveModuleConstants.ClosedLoopOutputType.TorqueCurrentFOC)) {
+ driveTalonFX.setControl(
+ velocityTorqueCurrentRequest
+ .withVelocity(Units.radiansToRotations(velocityRadiansPerSecond))
+ .withFeedForward(currentFeedforward));
+ }
+ }
+
+ @Override
+ @Trace
+ public void setTurnPosition(Rotation2d rotation) {
+ turnPositionGoal = rotation;
+ if (turnClosedLoopOutputType.equals(SwerveModuleConstants.ClosedLoopOutputType.Voltage))
+ turnTalonFX.setControl(positionVoltageRequest.withPosition(rotation.getRotations()));
+ if (turnClosedLoopOutputType.equals(
+ SwerveModuleConstants.ClosedLoopOutputType.TorqueCurrentFOC))
+ turnTalonFX.setControl(positionTorqueCurrentRequest.withPosition(rotation.getRotations()));
+ }
+
+ @Override
+ @Trace
+ public void setPID(double drive_Kp, double drive_Kd, double turn_Kp, double turn_Kd) {
+ driveConfig.Slot0.kP = drive_Kp;
+ driveConfig.Slot0.kD = drive_Kd;
+ turnConfig.Slot0.kP = turn_Kp;
+ turnConfig.Slot0.kD = turn_Kd;
+ tryUntilOk(5, () -> driveTalonFX.getConfigurator().apply(driveConfig, 0.25));
+ tryUntilOk(5, () -> turnTalonFX.getConfigurator().apply(turnConfig, 0.25));
+ }
+
+ @Override
+ @Trace
+ public void setFeedforward(double drive_Ks, double drive_Kv) {
+ driveConfig.Slot0.kS = drive_Ks;
+ driveConfig.Slot0.kV = drive_Kv;
+ tryUntilOk(5, () -> driveTalonFX.getConfigurator().apply(driveConfig, 0.25));
+ tryUntilOk(5, () -> turnTalonFX.getConfigurator().apply(turnConfig, 0.25));
+ }
+
+ @Override
+ @Trace
+ public void updateCurrentLimits(double driveCurrentLimit, double turnCurrentLimit) {
+ driveConfig.CurrentLimits.StatorCurrentLimit = driveCurrentLimit;
+ turnConfig.CurrentLimits.StatorCurrentLimit = turnCurrentLimit;
+ tryUntilOk(5, () -> driveTalonFX.getConfigurator().apply(driveConfig, 0.25));
+ tryUntilOk(5, () -> turnTalonFX.getConfigurator().apply(turnConfig, 0.25));
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveModuleIOTalonFXSim.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveModuleIOTalonFXSim.java
new file mode 100644
index 00000000..b112b0af
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/drivebases/swervedrive/SwerveModuleIOTalonFXSim.java
@@ -0,0 +1,87 @@
+package edu.wpi.team190.gompeilib.subsystems.drivebases.swervedrive;
+
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.sim.CANcoderSimState;
+import com.ctre.phoenix6.sim.TalonFXSimState;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import edu.wpi.team190.gompeilib.core.GompeiLib;
+import edu.wpi.team190.gompeilib.core.logging.Trace;
+
+public class SwerveModuleIOTalonFXSim extends SwerveModuleIOTalonFX {
+
+ private final DCMotorSim steerMotorSim;
+ private final DCMotorSim driveMotorSim;
+
+ private final TalonFXSimState steerController;
+ private final TalonFXSimState driveController;
+ private final CANcoderSimState encoderController;
+ private final double offset;
+
+ public SwerveModuleIOTalonFXSim(
+ SwerveDriveConstants driveConstants,
+ SwerveModuleConstants
+ constants) {
+
+ super(driveConstants, constants);
+ driveMotorSim =
+ new DCMotorSim(
+ LinearSystemId.createDCMotorSystem(
+ driveConstants.driveConfig.driveModel(),
+ constants.DriveInertia,
+ constants.DriveMotorGearRatio),
+ driveConstants.driveConfig.driveModel());
+ steerMotorSim =
+ new DCMotorSim(
+ LinearSystemId.createDCMotorSystem(
+ driveConstants.driveConfig.turnModel(),
+ constants.SteerInertia,
+ constants.SteerMotorGearRatio),
+ driveConstants.driveConfig.turnModel());
+
+ steerController = super.turnTalonFX.getSimState();
+ driveController = super.driveTalonFX.getSimState();
+ encoderController = super.cancoder.getSimState();
+
+ offset = constants.EncoderOffset;
+ }
+
+ @Override
+ @Trace
+ public void updateInputs(ModuleIOInputs inputs) {
+ driveController.setSupplyVoltage(RobotController.getBatteryVoltage());
+ double motorVoltageDrive = driveController.getMotorVoltage();
+
+ driveMotorSim.setInputVoltage(motorVoltageDrive);
+
+ driveMotorSim.update(GompeiLib.getLoopPeriod());
+
+ double rotorPositionRotationsDrive =
+ driveMotorSim.getAngularPositionRotations() * driveMotorSim.getGearing();
+ double rotorVelocityRotationsPerSecondDrive =
+ driveMotorSim.getAngularVelocityRadPerSec() / (Math.PI * 2) * driveMotorSim.getGearing();
+ driveController.setRawRotorPosition(rotorPositionRotationsDrive);
+ driveController.setRotorVelocity(rotorVelocityRotationsPerSecondDrive);
+
+ steerController.setSupplyVoltage(RobotController.getBatteryVoltage());
+ double motorVoltageSteer = steerController.getMotorVoltage();
+
+ steerMotorSim.setInputVoltage(motorVoltageSteer);
+
+ steerMotorSim.update(GompeiLib.getLoopPeriod());
+
+ double rotorPositionRotationsSteer =
+ steerMotorSim.getAngularPositionRotations() * steerMotorSim.getGearing();
+ double rotorVelocityRotationsPerSecondSteer =
+ steerMotorSim.getAngularVelocityRadPerSec() / (Math.PI * 2) * steerMotorSim.getGearing();
+ steerController.setRawRotorPosition(rotorPositionRotationsSteer);
+ steerController.setRotorVelocity(rotorVelocityRotationsPerSecondSteer);
+
+ encoderController.setRawPosition(steerMotorSim.getAngularPositionRotations() + offset);
+
+ super.updateInputs(inputs);
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/Elevator.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/Elevator.java
new file mode 100644
index 00000000..68db7016
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/Elevator.java
@@ -0,0 +1,186 @@
+package edu.wpi.team190.gompeilib.subsystems.elevator;
+
+import static edu.wpi.first.units.Units.*;
+
+import edu.wpi.first.units.DistanceUnit;
+import edu.wpi.first.units.VoltageUnit;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.Voltage;
+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.sysid.SysIdRoutine;
+import edu.wpi.team190.gompeilib.core.logging.Trace;
+import edu.wpi.team190.gompeilib.core.utility.Setpoint;
+import edu.wpi.team190.gompeilib.core.utility.control.Gains;
+import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints;
+import edu.wpi.team190.gompeilib.core.utility.phoenix.GainSlot;
+import org.littletonrobotics.junction.Logger;
+
+public class Elevator {
+ public final ElevatorIO io;
+ public final ElevatorIOInputsAutoLogged inputs;
+
+ private final String aKitTopic;
+
+ private ElevatorState currentState;
+
+ private Setpoint voltageGoal;
+ private Setpoint positionGoal;
+
+ private final SysIdRoutine characterizationRoutine;
+
+ public final ElevatorConstants constants;
+
+ public Elevator(
+ ElevatorConstants constants,
+ Subsystem subsystem,
+ int index,
+ ElevatorIO io,
+ Setpoint positionGoal,
+ Setpoint voltageGoal) {
+ this.io = io;
+ this.inputs = new ElevatorIOInputsAutoLogged();
+
+ aKitTopic = subsystem.getName() + "/Elevators" + index;
+
+ currentState = ElevatorState.IDLE;
+
+ this.positionGoal = positionGoal;
+ this.voltageGoal = voltageGoal;
+
+ characterizationRoutine =
+ new SysIdRoutine(
+ new SysIdRoutine.Config(
+ Volts.of(1).per(Second),
+ Volts.of(3),
+ Seconds.of(3),
+ (state) -> Logger.recordOutput(aKitTopic + "/SysIdState", state.toString())),
+ new SysIdRoutine.Mechanism(io::setVoltageGoal, null, subsystem));
+
+ this.constants = constants;
+ }
+
+ public Elevator(ElevatorConstants constants, Subsystem subsystem, int index, ElevatorIO io) {
+ this(
+ constants,
+ subsystem,
+ index,
+ io,
+ new Setpoint<>(
+ Meters.of(0),
+ constants.heightOffsetStep,
+ constants.elevatorParameters.MIN_HEIGHT(),
+ constants.elevatorParameters.MAX_HEIGHT()),
+ new Setpoint<>(Volts.of(0), constants.voltageOffsetStep, Volts.of(-12), Volts.of(12)));
+ }
+
+ public Elevator(
+ ElevatorConstants constants,
+ Subsystem subsystem,
+ int index,
+ ElevatorIO io,
+ Setpoint positionGoal) {
+ this(
+ constants,
+ subsystem,
+ index,
+ io,
+ positionGoal,
+ new Setpoint<>(Volts.of(0), constants.voltageOffsetStep, Volts.of(-12), Volts.of(12)));
+ }
+
+ @Trace
+ public void periodic() {
+ io.updateInputs(inputs);
+ Logger.processInputs(aKitTopic, inputs);
+
+ Logger.recordOutput(aKitTopic + "/State", currentState.name());
+ Logger.recordOutput(aKitTopic + "/Voltage Goal", voltageGoal.getSetpoint());
+ Logger.recordOutput(aKitTopic + "/Position Goal", positionGoal.getSetpoint());
+ Logger.recordOutput(aKitTopic + "/Voltage Offset", voltageGoal.getOffset());
+ Logger.recordOutput(aKitTopic + "/Position Offset", positionGoal.getOffset());
+ Logger.recordOutput(aKitTopic + "/At Voltage Goal", atVoltageGoal());
+ Logger.recordOutput(aKitTopic + "/At Position Goal", atPositionGoal());
+
+ switch (currentState) {
+ case OPEN_LOOP_VOLTAGE_CONTROL -> io.setVoltageGoal((Voltage) voltageGoal.getNewSetpoint());
+ case CLOSED_LOOP_POSITION_CONTROL ->
+ io.setPositionGoal((Distance) positionGoal.getNewSetpoint());
+ }
+ }
+
+ public Distance getElevatorPosition() {
+ return inputs.position;
+ }
+
+ public void setVoltageGoal(Voltage voltageGoal) {
+ currentState = ElevatorState.OPEN_LOOP_VOLTAGE_CONTROL;
+ this.voltageGoal.setSetpoint(voltageGoal);
+ }
+
+ public void setPositionGoal(Distance positionGoal) {
+ currentState = ElevatorState.CLOSED_LOOP_POSITION_CONTROL;
+ this.positionGoal.setSetpoint(positionGoal);
+ }
+
+ public void setVoltageGoal(Setpoint voltageGoal) {
+ currentState = ElevatorState.OPEN_LOOP_VOLTAGE_CONTROL;
+ this.voltageGoal = voltageGoal;
+ }
+
+ public void setPositionGoal(Setpoint positionGoal) {
+ currentState = ElevatorState.CLOSED_LOOP_POSITION_CONTROL;
+ this.positionGoal = positionGoal;
+ }
+
+ public boolean atVoltageGoal(Voltage voltageReference) {
+ return voltageGoal.getNewSetpoint().isNear(voltageReference, Millivolts.of(500));
+ }
+
+ public boolean atPositionGoal(Distance positionReference) {
+ return positionGoal
+ .getNewSetpoint()
+ .isNear(positionReference, constants.constraints.goalTolerance().get());
+ }
+
+ public boolean atVoltageGoal() {
+ return atVoltageGoal((Voltage) voltageGoal.getNewSetpoint());
+ }
+
+ public boolean atPositionGoal() {
+ return atPositionGoal((Distance) positionGoal.getNewSetpoint());
+ }
+
+ public void setPosition(Distance position) {
+ io.setPosition(position);
+ }
+
+ public void setGainSlot(GainSlot gainSlot) {
+ io.setGainSlot(gainSlot);
+ }
+
+ public Command waitUntilAtGoal() {
+ return Commands.waitUntil(this::atPositionGoal);
+ }
+
+ public void updateGains(Gains gains, GainSlot slot) {
+ io.updateGains(gains, slot);
+ }
+
+ public void updateConstraints(LinearConstraints constraints) {
+ io.updateConstraints(constraints);
+ }
+
+ public Command runSysIdRoutine() {
+ return Commands.sequence(
+ Commands.runOnce(() -> currentState = ElevatorState.IDLE),
+ characterizationRoutine.quasistatic(SysIdRoutine.Direction.kForward),
+ Commands.waitSeconds(1.0),
+ characterizationRoutine.quasistatic(SysIdRoutine.Direction.kReverse),
+ Commands.waitSeconds(1.0),
+ characterizationRoutine.dynamic(SysIdRoutine.Direction.kForward),
+ Commands.waitSeconds(1.0),
+ characterizationRoutine.dynamic(SysIdRoutine.Direction.kReverse));
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorConstants.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorConstants.java
new file mode 100644
index 00000000..6b2bd590
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorConstants.java
@@ -0,0 +1,48 @@
+package edu.wpi.team190.gompeilib.subsystems.elevator;
+
+import com.ctre.phoenix6.CANBus;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.team190.gompeilib.core.utility.control.Gains;
+import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints;
+import java.util.Set;
+import lombok.Builder;
+import lombok.NonNull;
+import lombok.Singular;
+
+@Builder(setterPrefix = "with")
+public class ElevatorConstants {
+ @NonNull public final Integer leaderCANID;
+ @NonNull public final CANBus canBus = new CANBus();
+ @NonNull public final Double elevatorGearRatio;
+ @NonNull public final Double drumRadius;
+
+ @NonNull public final Double elevatorSupplyCurrentLimit;
+ @NonNull public final Double elevatorStatorCurrentLimit;
+
+ @NonNull public final ElevatorParameters elevatorParameters;
+ @NonNull public final Gains slot0Gains;
+ @Builder.Default public final Gains slot1Gains = Gains.builder().build();
+ @Builder.Default public final Gains slot2Gains = Gains.builder().build();
+ @NonNull public final LinearConstraints constraints;
+
+ @Singular(value = "alignedFollowerCANID")
+ @NonNull
+ public final Set alignedFollowerCANIDs;
+
+ @Singular(value = "opposedFollowerCANID")
+ @NonNull
+ public final Set opposedFollowerCANIDs;
+
+ @NonNull public final Voltage voltageOffsetStep;
+ @NonNull public final Distance heightOffsetStep;
+
+ @Builder(setterPrefix = "with")
+ public record ElevatorParameters(
+ @NonNull DCMotor ELEVATOR_MOTOR_CONFIG,
+ @NonNull Double CARRIAGE_MASS_KG,
+ @NonNull Distance MIN_HEIGHT,
+ @NonNull Distance MAX_HEIGHT,
+ @NonNull Integer NUM_MOTORS) {}
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIO.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIO.java
new file mode 100644
index 00000000..7078080b
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIO.java
@@ -0,0 +1,97 @@
+package edu.wpi.team190.gompeilib.subsystems.elevator;
+
+import static edu.wpi.first.units.Units.*;
+
+import edu.wpi.first.units.measure.*;
+import edu.wpi.team190.gompeilib.core.utility.control.Gains;
+import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints;
+import edu.wpi.team190.gompeilib.core.utility.phoenix.GainSlot;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ElevatorIO {
+
+ @AutoLog
+ public static class ElevatorIOInputs {
+ public Distance position = Meters.of(0.0);
+ public LinearVelocity velocity = MetersPerSecond.of(0.0);
+ public LinearAcceleration acceleration = MetersPerSecondPerSecond.of(0.0);
+
+ public double[] appliedVolts = new double[] {};
+ public double[] supplyCurrentAmps = new double[] {};
+ public double[] torqueCurrentAmps = new double[] {};
+ public double[] temperatureCelsius = new double[] {};
+
+ public Distance positionGoalMeters = Meters.of(0.0);
+ public Distance positionSetpointMeters = Meters.of(0.0);
+ public Distance positionErrorMeters = Meters.of(0.0);
+
+ public GainSlot gainSlot;
+ }
+
+ /**
+ * Updates the inputs for the elevator.
+ *
+ * @param inputs The inputs to update.
+ */
+ default void updateInputs(ElevatorIOInputs inputs) {}
+
+ /**
+ * Sets the voltage for the elevator.
+ *
+ * @param voltageGoal The voltage of the elevator in volts.
+ */
+ default void setVoltageGoal(Voltage voltageGoal) {}
+
+ /**
+ * Sets the position goal of the elevator.
+ *
+ * @param positionGoal The position goal of the elevator in meters.
+ */
+ default void setPositionGoal(Distance positionGoal) {}
+
+ /**
+ * Checks if the voltage of the elevator matches the volts argument
+ *
+ * @param voltageReference The voltage to check against
+ * @return True if the voltage matches, false otherwise
+ */
+ default boolean atVoltageGoal(Voltage voltageReference) {
+ return false;
+ }
+
+ /**
+ * Checks if the position of the subsystem matches the positionGoal argument
+ *
+ * @param positionReference the position to check against
+ * @return True if the position matches, false otherwise
+ */
+ default boolean atPositionGoal(Distance positionReference) {
+ return false;
+ }
+
+ /**
+ * Sets the position of the elevator.
+ *
+ * @param position The position to set.
+ */
+ default void setPosition(Distance position) {}
+
+ /**
+ * @param gainSlot The CTRE gain slot to set the elevator to.
+ */
+ default void setGainSlot(GainSlot gainSlot) {}
+
+ /**
+ * Sets the gains for the elevator.
+ *
+ * @param gains the gains to update
+ */
+ default void updateGains(Gains gains, GainSlot gainSlot) {}
+
+ /**
+ * Sets the constraints for the elevator.
+ *
+ * @param constraints the constraints to update
+ */
+ default void updateConstraints(LinearConstraints constraints) {}
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOSim.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOSim.java
new file mode 100644
index 00000000..472dc64c
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOSim.java
@@ -0,0 +1,163 @@
+package edu.wpi.team190.gompeilib.subsystems.elevator;
+
+import static edu.wpi.first.units.Units.*;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.units.measure.*;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+import edu.wpi.team190.gompeilib.core.GompeiLib;
+import edu.wpi.team190.gompeilib.core.utility.control.Gains;
+import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints;
+import edu.wpi.team190.gompeilib.core.utility.phoenix.GainSlot;
+import java.util.Arrays;
+
+public class ElevatorIOSim implements ElevatorIO {
+ private final ElevatorSim sim;
+
+ private Voltage appliedVolts;
+ private boolean isClosedLoop;
+ private GainSlot gainSlot;
+
+ private final ProfiledPIDController feedback;
+ private ElevatorFeedforward feedforward;
+
+ private final ElevatorConstants constants;
+
+ public ElevatorIOSim(ElevatorConstants constants) {
+ sim =
+ new ElevatorSim(
+ LinearSystemId.createElevatorSystem(
+ constants.elevatorParameters.ELEVATOR_MOTOR_CONFIG(),
+ constants.elevatorParameters.CARRIAGE_MASS_KG(),
+ constants.drumRadius,
+ constants.elevatorGearRatio),
+ constants.elevatorParameters.ELEVATOR_MOTOR_CONFIG(),
+ constants.elevatorParameters.MIN_HEIGHT().in(Meters),
+ constants.elevatorParameters.MAX_HEIGHT().in(Meters),
+ true,
+ constants.elevatorParameters.MIN_HEIGHT().in(Meters));
+
+ appliedVolts = Volts.of(0.0);
+ isClosedLoop = true;
+ gainSlot = GainSlot.ZERO;
+
+ feedback =
+ new ProfiledPIDController(
+ constants.slot0Gains.kP().get(),
+ 0,
+ constants.slot0Gains.kD().get(),
+ new TrapezoidProfile.Constraints(
+ constants.constraints.maxVelocity().get().in(MetersPerSecond),
+ constants.constraints.maxAcceleration().get().in(MetersPerSecondPerSecond)));
+
+ feedforward =
+ new ElevatorFeedforward(
+ constants.slot0Gains.kS().get(),
+ constants.slot0Gains.kG().get(),
+ constants.slot0Gains.kV().get(),
+ constants.slot0Gains.kA().get());
+
+ this.constants = constants;
+ }
+
+ @Override
+ public void updateInputs(ElevatorIOInputs inputs) {
+ if (isClosedLoop) {
+ appliedVolts =
+ Volts.of(
+ feedback.calculate(sim.getPositionMeters())
+ + feedforward.calculate((feedback.getSetpoint().velocity)));
+ }
+
+ appliedVolts = Volts.of(MathUtil.clamp(appliedVolts.in(Volts), -12, 12));
+
+ sim.setInputVoltage(appliedVolts.in(Volts));
+ sim.update(GompeiLib.getLoopPeriod());
+
+ inputs.position = Meters.of(sim.getPositionMeters());
+ inputs.velocity = MetersPerSecond.of(sim.getVelocityMetersPerSecond());
+ inputs.acceleration =
+ MetersPerSecondPerSecond.of(-1.0); // TODO: Replace with calculation based on velocity
+
+ inputs.appliedVolts = new double[constants.elevatorParameters.NUM_MOTORS()];
+ inputs.supplyCurrentAmps = new double[constants.elevatorParameters.NUM_MOTORS()];
+ inputs.torqueCurrentAmps = new double[constants.elevatorParameters.NUM_MOTORS()];
+ inputs.temperatureCelsius = new double[constants.elevatorParameters.NUM_MOTORS()];
+
+ Arrays.fill(inputs.appliedVolts, appliedVolts.in(Volts));
+ Arrays.fill(inputs.supplyCurrentAmps, sim.getCurrentDrawAmps());
+ Arrays.fill(inputs.torqueCurrentAmps, sim.getCurrentDrawAmps());
+ Arrays.fill(inputs.temperatureCelsius, 0.0);
+
+ inputs.positionGoalMeters = Meters.of(feedback.getGoal().position);
+ inputs.positionSetpointMeters = Meters.of(feedback.getSetpoint().position);
+ inputs.positionErrorMeters = Meters.of(feedback.getPositionError());
+
+ inputs.gainSlot = gainSlot;
+ }
+
+ @Override
+ public void setVoltageGoal(Voltage volts) {
+ isClosedLoop = false;
+ appliedVolts = volts;
+ }
+
+ @Override
+ public void setPositionGoal(Distance position) {
+ isClosedLoop = true;
+ feedback.setGoal(position.in(Meters));
+ }
+
+ @Override
+ public boolean atVoltageGoal(Voltage voltageReference) {
+ return appliedVolts.isNear(voltageReference, Millivolt.of(500));
+ }
+
+ @Override
+ public boolean atPositionGoal(Distance positionReference) {
+ return Meters.of(sim.getPositionMeters())
+ .isNear(positionReference, constants.constraints.goalTolerance().get());
+ }
+
+ @Override
+ public void setPosition(Distance position) {
+ sim.setState(position.in(Meters), sim.getVelocityMetersPerSecond());
+ }
+
+ @Override
+ public void setGainSlot(GainSlot gainSlot) {
+ this.gainSlot = gainSlot;
+ switch (gainSlot) {
+ case ZERO:
+ feedback.setPID(constants.slot0Gains.kP().get(), 0.0, constants.slot0Gains.kD().get());
+ break;
+ case ONE:
+ feedback.setPID(constants.slot1Gains.kP().get(), 0.0, constants.slot1Gains.kD().get());
+ break;
+ case TWO:
+ feedback.setPID(constants.slot2Gains.kP().get(), 0.0, constants.slot2Gains.kD().get());
+ break;
+ }
+ }
+
+ @Override
+ public void updateGains(Gains gains, GainSlot gainSlot) {
+ feedback.setPID(gains.kP().get(), gains.kI().get(), gains.kD().get());
+ feedforward =
+ new ElevatorFeedforward(
+ gains.kS().get(), gains.kG().get(), gains.kV().get(), gains.kA().get());
+ }
+
+ @Override
+ public void updateConstraints(LinearConstraints constraints) {
+ feedback.setConstraints(
+ new TrapezoidProfile.Constraints(
+ constraints.maxVelocity().get().in(MetersPerSecond),
+ constraints.maxAcceleration().get().in(MetersPerSecondPerSecond)));
+ feedback.setTolerance(constraints.goalTolerance().get().in(Meters));
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTalonFX.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTalonFX.java
new file mode 100644
index 00000000..47068d82
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTalonFX.java
@@ -0,0 +1,299 @@
+package edu.wpi.team190.gompeilib.subsystems.elevator;
+
+import static edu.wpi.first.units.Units.*;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.GravityTypeValue;
+import com.ctre.phoenix6.signals.MotorAlignmentValue;
+import edu.wpi.first.units.measure.*;
+import edu.wpi.team190.gompeilib.core.GompeiLib;
+import edu.wpi.team190.gompeilib.core.utility.control.Gains;
+import edu.wpi.team190.gompeilib.core.utility.control.constraints.LinearConstraints;
+import edu.wpi.team190.gompeilib.core.utility.phoenix.GainSlot;
+import edu.wpi.team190.gompeilib.core.utility.phoenix.PhoenixUtil;
+import java.util.ArrayList;
+
+public class ElevatorIOTalonFX implements ElevatorIO {
+
+ // Core hardware components
+ protected final TalonFX talonFX;
+ public final TalonFX[] followTalonFX;
+
+ // Configuration
+ public final TalonFXConfiguration config;
+ protected final ElevatorConstants constants;
+
+ // Sensor inputs
+ private StatusSignal positionRotations;
+ private StatusSignal velocityRotationsPerSecond;
+ private StatusSignal accelerationRotationsPerSecondPerSecond;
+ private ArrayList> appliedVolts;
+ private ArrayList> supplyCurrentAmps;
+ private ArrayList> torqueCurrentAmps;
+ private ArrayList> temperatureCelsius;
+
+ public Distance positionGoalMeters;
+
+ private StatusSignal positionSetpointRotations;
+ private StatusSignal positionErrorRotations;
+
+ private StatusSignal[] statusSignals;
+
+ private MotionMagicVoltage positionVoltageRequest;
+ private VoltageOut voltageRequest;
+
+ public ElevatorIOTalonFX(ElevatorConstants constants) {
+
+ this.constants = constants;
+
+ // Create lead motor
+ talonFX = new TalonFX(constants.leaderCANID, constants.canBus);
+
+ // Create follower motor array (define length)
+ followTalonFX = new TalonFX[constants.elevatorParameters.NUM_MOTORS() - 1];
+
+ config = new TalonFXConfiguration();
+ config.Slot0.withKP(constants.slot0Gains.kP().get())
+ .withKD(constants.slot0Gains.kD().get())
+ .withKS(constants.slot0Gains.kS().get())
+ .withKV(constants.slot0Gains.kV().get())
+ .withKA(constants.slot0Gains.kA().get())
+ .withKG(constants.slot0Gains.kG().get())
+ .withGravityType(GravityTypeValue.Elevator_Static);
+
+ config.Slot1.withKP(constants.slot1Gains.kP().get())
+ .withKD(constants.slot1Gains.kD().get())
+ .withKS(constants.slot1Gains.kS().get())
+ .withKV(constants.slot1Gains.kV().get())
+ .withKA(constants.slot1Gains.kA().get())
+ .withKG(constants.slot1Gains.kG().get())
+ .withGravityType(GravityTypeValue.Elevator_Static);
+
+ config.Slot2.withKP(constants.slot2Gains.kP().get())
+ .withKD(constants.slot2Gains.kD().get())
+ .withKS(constants.slot2Gains.kS().get())
+ .withKV(constants.slot2Gains.kV().get())
+ .withKA(constants.slot2Gains.kA().get())
+ .withKG(constants.slot2Gains.kG().get())
+ .withGravityType(GravityTypeValue.Elevator_Static);
+
+ config.CurrentLimits.withSupplyCurrentLimit(constants.elevatorSupplyCurrentLimit)
+ .withSupplyCurrentLimitEnable(true)
+ .withStatorCurrentLimit(constants.elevatorStatorCurrentLimit)
+ .withStatorCurrentLimitEnable(true);
+
+ config.Feedback.SensorToMechanismRatio =
+ constants.elevatorGearRatio / (2 * Math.PI * constants.drumRadius);
+
+ config.SoftwareLimitSwitch.withForwardSoftLimitThreshold(
+ constants.elevatorParameters.MAX_HEIGHT().in(Meters))
+ .withForwardSoftLimitEnable(true)
+ .withReverseSoftLimitThreshold(constants.elevatorParameters.MIN_HEIGHT().in(Meters))
+ .withReverseSoftLimitEnable(true);
+
+ config.MotionMagic.withMotionMagicAcceleration(
+ constants.constraints.maxAcceleration().get().in(MetersPerSecondPerSecond))
+ .withMotionMagicCruiseVelocity(
+ constants.constraints.maxVelocity().get().in(MetersPerSecond));
+
+ PhoenixUtil.tryUntilOk(5, () -> talonFX.getConfigurator().apply(config));
+
+ final int[] indexHolder = {0}; // mutable index for array insertion
+
+ constants.alignedFollowerCANIDs.forEach(
+ id -> {
+ TalonFX follower = new TalonFX(id, talonFX.getNetwork());
+ followTalonFX[indexHolder[0]++] = follower;
+
+ PhoenixUtil.tryUntilOk(5, () -> follower.getConfigurator().apply(config, 0.25));
+
+ follower.setControl(new Follower(talonFX.getDeviceID(), MotorAlignmentValue.Aligned));
+ });
+
+ constants.opposedFollowerCANIDs.forEach(
+ id -> {
+ TalonFX follower = new TalonFX(id, talonFX.getNetwork());
+ followTalonFX[indexHolder[0]++] = follower;
+
+ PhoenixUtil.tryUntilOk(5, () -> follower.getConfigurator().apply(config, 0.25));
+
+ follower.setControl(new Follower(talonFX.getDeviceID(), MotorAlignmentValue.Opposed));
+ });
+
+ appliedVolts = new ArrayList<>();
+ supplyCurrentAmps = new ArrayList<>();
+ torqueCurrentAmps = new ArrayList<>();
+ temperatureCelsius = new ArrayList<>();
+
+ positionRotations = talonFX.getPosition();
+ velocityRotationsPerSecond = talonFX.getVelocity();
+ accelerationRotationsPerSecondPerSecond = talonFX.getAcceleration();
+ appliedVolts.add(talonFX.getMotorVoltage());
+ supplyCurrentAmps.add(talonFX.getSupplyCurrent());
+ torqueCurrentAmps.add(talonFX.getTorqueCurrent());
+ temperatureCelsius.add(talonFX.getDeviceTemp());
+ positionGoalMeters = Meters.of(0.0);
+ positionSetpointRotations = talonFX.getClosedLoopReference();
+ positionErrorRotations = talonFX.getClosedLoopError();
+
+ for (TalonFX follower : followTalonFX) {
+ appliedVolts.add(follower.getMotorVoltage());
+ supplyCurrentAmps.add(follower.getSupplyCurrent());
+ torqueCurrentAmps.add(follower.getTorqueCurrent());
+ temperatureCelsius.add(follower.getDeviceTemp());
+ }
+
+ var signalsList = new ArrayList>();
+
+ signalsList.add(positionRotations);
+ signalsList.add(velocityRotationsPerSecond);
+ signalsList.add(accelerationRotationsPerSecondPerSecond);
+ signalsList.add(positionSetpointRotations);
+ signalsList.add(positionErrorRotations);
+ signalsList.addAll(appliedVolts);
+ signalsList.addAll(supplyCurrentAmps);
+ signalsList.addAll(torqueCurrentAmps);
+ signalsList.addAll(temperatureCelsius);
+
+ statusSignals = new StatusSignal[signalsList.size()];
+
+ for (int i = 0; i < signalsList.size(); i++) {
+ statusSignals[i] = signalsList.get(i);
+ }
+
+ BaseStatusSignal.setUpdateFrequencyForAll(1 / GompeiLib.getLoopPeriod(), statusSignals);
+
+ talonFX.optimizeBusUtilization();
+ for (TalonFX follower : followTalonFX) {
+ follower.optimizeBusUtilization();
+ }
+
+ positionVoltageRequest = new MotionMagicVoltage(0.0);
+ voltageRequest = new VoltageOut(0.0);
+
+ PhoenixUtil.registerSignals(constants.canBus.isNetworkFD(), statusSignals);
+ }
+
+ @Override
+ public void updateInputs(ElevatorIOInputs inputs) {
+
+ // CTRE status signals are natively in rotations, but setting sensor to mechanism ratio
+ // including the circumference of the drum allows us to transform into meters directly from
+ // status signal object
+ inputs.position = Meters.of(positionRotations.getValueAsDouble());
+ inputs.velocity = MetersPerSecond.of(velocityRotationsPerSecond.getValueAsDouble());
+ inputs.acceleration =
+ MetersPerSecondPerSecond.of(accelerationRotationsPerSecondPerSecond.getValueAsDouble());
+
+ inputs.appliedVolts = new double[appliedVolts.size()];
+ inputs.supplyCurrentAmps = new double[supplyCurrentAmps.size()];
+ inputs.torqueCurrentAmps = new double[torqueCurrentAmps.size()];
+ inputs.temperatureCelsius = new double[temperatureCelsius.size()];
+
+ for (int i = 0; i <= followTalonFX.length; i++) {
+ inputs.appliedVolts[i] = appliedVolts.get(i).getValueAsDouble();
+ inputs.supplyCurrentAmps[i] = supplyCurrentAmps.get(i).getValueAsDouble();
+ inputs.torqueCurrentAmps[i] = torqueCurrentAmps.get(i).getValueAsDouble();
+ inputs.temperatureCelsius[i] = temperatureCelsius.get(i).getValueAsDouble();
+ }
+ inputs.positionGoalMeters = positionGoalMeters;
+ inputs.positionSetpointMeters = Meters.of(positionSetpointRotations.getValueAsDouble());
+ inputs.positionErrorMeters = Meters.of(positionErrorRotations.getValueAsDouble());
+
+ inputs.gainSlot = GainSlot.integerToGainSlot(talonFX.getClosedLoopSlot().getValue());
+ }
+
+ @Override
+ public void setVoltageGoal(Voltage voltageGoal) {
+ talonFX.setControl(voltageRequest.withOutput(voltageGoal).withEnableFOC(true));
+ }
+
+ @Override
+ public void setPositionGoal(Distance positionGoal) {
+ positionGoalMeters = positionGoal;
+ talonFX.setControl(positionVoltageRequest.withPosition(positionGoal.in(Meters)));
+ }
+
+ @Override
+ public boolean atVoltageGoal(Voltage voltageReference) {
+ return appliedVolts.get(0).getValue().isNear(voltageReference, Millivolts.of(500));
+ }
+
+ @Override
+ public boolean atPositionGoal(Distance positionReference) {
+ return Math.abs(positionRotations.getValueAsDouble() - positionReference.in(Meters))
+ <= constants.constraints.goalTolerance().get(Meters);
+ }
+
+ @Override
+ public void setPosition(Distance position) {
+ talonFX.setPosition(position.in(Meters));
+ }
+
+ @Override
+ public void setGainSlot(GainSlot slot) {
+ switch (slot) {
+ case ONE:
+ talonFX.setControl(positionVoltageRequest.withSlot(1));
+ break;
+ case TWO:
+ talonFX.setControl(positionVoltageRequest.withSlot(2));
+ break;
+ default:
+ talonFX.setControl(positionVoltageRequest.withSlot(0));
+ break;
+ }
+ }
+
+ @Override
+ public void updateGains(Gains gains, GainSlot gainSlot) {
+ switch (gainSlot) {
+ case ZERO:
+ config.Slot0.withKP(gains.kP().get())
+ .withKD(gains.kD().get())
+ .withKS(gains.kS().get())
+ .withKV(gains.kV().get())
+ .withKA(gains.kA().get())
+ .withKG(gains.kG().get());
+ break;
+ case ONE:
+ config.Slot1.withKP(gains.kP().get())
+ .withKD(gains.kD().get())
+ .withKS(gains.kS().get())
+ .withKV(gains.kV().get())
+ .withKA(gains.kA().get())
+ .withKG(gains.kG().get());
+ break;
+ case TWO:
+ default:
+ config.Slot2.withKP(gains.kP().get())
+ .withKD(gains.kD().get())
+ .withKS(gains.kS().get())
+ .withKV(gains.kV().get())
+ .withKA(gains.kA().get())
+ .withKG(gains.kG().get());
+ break;
+ }
+ PhoenixUtil.tryUntilOk(5, () -> talonFX.getConfigurator().apply(config, 0.25));
+ for (TalonFX follower : followTalonFX) {
+ PhoenixUtil.tryUntilOk(5, () -> follower.getConfigurator().apply(config, 0.25));
+ }
+ }
+
+ @Override
+ public void updateConstraints(LinearConstraints constraints) {
+ config.MotionMagic.withMotionMagicAcceleration(
+ constraints.maxAcceleration().get().in(MetersPerSecondPerSecond))
+ .withMotionMagicCruiseVelocity(constraints.maxVelocity().get().in(MetersPerSecond));
+ PhoenixUtil.tryUntilOk(5, () -> talonFX.getConfigurator().apply(config, 0.25));
+ for (TalonFX follower : followTalonFX) {
+ PhoenixUtil.tryUntilOk(5, () -> follower.getConfigurator().apply(config, 0.25));
+ }
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTalonFXSim.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTalonFXSim.java
new file mode 100644
index 00000000..0b52d670
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorIOTalonFXSim.java
@@ -0,0 +1,63 @@
+package edu.wpi.team190.gompeilib.subsystems.elevator;
+
+import static edu.wpi.first.units.Units.*;
+import static edu.wpi.first.units.Units.Meters;
+
+import com.ctre.phoenix6.sim.TalonFXSimState;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+import edu.wpi.team190.gompeilib.core.GompeiLib;
+import edu.wpi.team190.gompeilib.core.logging.Trace;
+
+public class ElevatorIOTalonFXSim extends ElevatorIOTalonFX {
+ private final ElevatorSim elevatorSim;
+
+ private final TalonFXSimState elevatorController;
+
+ public ElevatorIOTalonFXSim(ElevatorConstants constants) {
+ super(constants);
+ elevatorSim =
+ new ElevatorSim(
+ LinearSystemId.createElevatorSystem(
+ constants.elevatorParameters.ELEVATOR_MOTOR_CONFIG(),
+ constants.elevatorParameters.CARRIAGE_MASS_KG(),
+ constants.drumRadius,
+ constants.elevatorGearRatio),
+ constants.elevatorParameters.ELEVATOR_MOTOR_CONFIG(),
+ constants.elevatorParameters.MIN_HEIGHT().in(Meters),
+ constants.elevatorParameters.MAX_HEIGHT().in(Meters),
+ true,
+ constants.elevatorParameters.MIN_HEIGHT().in(Meters));
+
+ elevatorController = super.talonFX.getSimState();
+ }
+
+ @Override
+ @Trace
+ public void updateInputs(ElevatorIOInputs inputs) {
+ elevatorController.setSupplyVoltage(RobotController.getBatteryVoltage());
+ double elevatorVoltage = elevatorController.getMotorVoltage();
+
+ elevatorSim.setInputVoltage(elevatorVoltage);
+
+ elevatorSim.update(GompeiLib.getLoopPeriod());
+
+ Angle rotorPosition =
+ Angle.ofBaseUnits(
+ elevatorSim.getPositionMeters() * constants.elevatorGearRatio * constants.drumRadius,
+ Radians);
+ AngularVelocity rotorVelocity =
+ AngularVelocity.ofBaseUnits(
+ elevatorSim.getVelocityMetersPerSecond()
+ * constants.elevatorGearRatio
+ * constants.drumRadius,
+ RadiansPerSecond);
+ elevatorController.setRawRotorPosition(rotorPosition);
+ elevatorController.setRotorVelocity(rotorVelocity);
+
+ super.updateInputs(inputs);
+ }
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorState.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorState.java
new file mode 100644
index 00000000..b09c07e2
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/elevator/ElevatorState.java
@@ -0,0 +1,7 @@
+package edu.wpi.team190.gompeilib.subsystems.elevator;
+
+public enum ElevatorState {
+ IDLE,
+ OPEN_LOOP_VOLTAGE_CONTROL,
+ CLOSED_LOOP_POSITION_CONTROL;
+}
diff --git a/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/generic/flywheel/GenericFlywheel.java b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/generic/flywheel/GenericFlywheel.java
new file mode 100644
index 00000000..591dd735
--- /dev/null
+++ b/lib/src/main/java/edu/wpi/team190/gompeilib/subsystems/generic/flywheel/GenericFlywheel.java
@@ -0,0 +1,265 @@
+package edu.wpi.team190.gompeilib.subsystems.generic.flywheel;
+
+import static edu.wpi.first.units.Units.*;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.*;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import edu.wpi.team190.gompeilib.core.utility.Setpoint;
+import edu.wpi.team190.gompeilib.core.utility.control.Gains;
+import edu.wpi.team190.gompeilib.core.utility.control.constraints.AngularVelocityConstraints;
+import edu.wpi.team190.gompeilib.core.utility.phoenix.GainSlot;
+import edu.wpi.team190.gompeilib.core.utility.sysid.CustomSysIdRoutine;
+import edu.wpi.team190.gompeilib.core.utility.sysid.CustomUnits;
+import java.util.function.Supplier;
+import lombok.Getter;
+import org.littletonrobotics.junction.Logger;
+
+public class GenericFlywheel {
+ private final GenericFlywheelIO io;
+ private final GenericFlywheelIOInputsAutoLogged inputs;
+
+ private final String aKitTopic;
+
+ @Getter private GenericFlywheelState currentState;
+
+ @Getter private Setpoint velocityGoal;
+ @Getter private Setpoint voltageGoal;
+ @Getter private Current currentGoal;
+
+ private final CustomSysIdRoutine voltageCharacterizationRoutine;
+ private final CustomSysIdRoutine