Copy 2025 code and change CODEOWNERS

diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json
new file mode 100644 (file)
index 0000000..a3dc0c4
--- /dev/null
+++ b/.DataLogTool/datalogtool.json
@@ -0,0 +1,6 @@
+{
+  "download": {
+    "localDir": "D:\\loggies",
+    "serverTeam": "972"
+  }
+}

diff --git a/.Glass/glass.json b/.Glass/glass.json
new file mode 100644 (file)
index 0000000..bd13c22
--- /dev/null
+++ b/.Glass/glass.json
@@ -0,0 +1,8 @@
+{
+  "NetworkTables Log": {
+    "visible": true
+  },
+  "NetworkTables Settings": {
+    "mode": "Server"
+  }
+}

diff --git a/.gitattributes b/.gitattributes
index d8c3d837fddca856d58aeecd130dd2b0d999ea04..286213be10c1d20d988b00362d550e7843864f8a 100644 (file)
--- a/.gitattributes
+++ b/.gitattributes
@@ -1,4 +1,4 @@
 *.gradle text eol=lf
 *.java text eol=lf
 *.md text eol=lf
-*.xml text eol=lf
+*.xml text eol=lf
\ No newline at end of file

diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
new file mode 100644 (file)
index 0000000..affeb5c
--- /dev/null
+++ b/.github/CODEOWNERS
@@ -0,0 +1,2 @@
+# Make the pull reviewers code owners for every file, they will get requested for review when a PR is made
+*   @Saara21 @maxwtan @Arnav814
\ No newline at end of file

diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md
index 92b3c80d995bc21eaa1941387a2ac54cd0de65a6..c89f33bc6fdca3138b6deed4d5456dc3083ed95e 100644 (file)
--- a/.github/PULL_REQUEST_TEMPLATE.md
+++ b/.github/PULL_REQUEST_TEMPLATE.md
@@ -1,21 +1,21 @@
 <!--
-PLEASE CHECK THE PR GUIDLINES:
+PLEASE CHECK THE PR GUIDELINES:
  https://docs.google.com/document/d/14ZIhQRlUPK0wl5_AmM80N2D2KJUd6k_7I6isLS89YKU/edit
 These are the things we are going to be checking your PR has, if it doesn't your PR will be rejected!
 -->
 
 ## Overview
 <!-- A general very short overview of the PR -->
-This PR will..
+This PR will...
 
 
-## Design Doc
-<!-- Link your approved design doc. We will be checking that the code fits everything in the design doc. If the PR is only meant to address part of the deisgn doc, note here what is not yet implemented from the Design Doc -->
+## Documentation
+<!-- Link your approved design doc or your project documentation. We will be checking that the code fits everything in the linked file. If the PR is only meant to address part of it, note here what is not yet implemented -->
 
 
 ## Tests Ran
-<!-- Explicity list the exact tests you ran. These should be the tests listed in the Testing and Evaluation section of your design doc, and of every merged design doc. List the actual result of the test and how it was within the measurement specified by the test. -->
+<!-- At a minimum, make sure the code builds and that the code works in the sim. -->
 
 
 ## Additional Notes
-<!-- Anything else that will help the reviewer understand your PR -->
+<!-- Anything else that will help the reviewer understand your PR -->
\ No newline at end of file

diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml
index faa3f6fca4f4800fd5f24acc5b18e734bfaa9b3d..849878f35f647b6bb01fd07f26047c5a3a52cedd 100644 (file)
--- a/.github/workflows/main.yml
+++ b/.github/workflows/main.yml
@@ -18,12 +18,16 @@ jobs:
     runs-on: ubuntu-latest
 
     # This grabs the WPILib docker container
-    container: wpilib/roborio-cross-ubuntu:2022-18.04
+    container: wpilib/roborio-cross-ubuntu:2023-22.04
 
     # Steps represent a sequence of tasks that will be executed as part of the job
     steps:
     # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it
-    - uses: actions/checkout@v2
+    - uses: actions/checkout@v3
+
+    # Declares the repository safe and not under dubious ownership.
+    - name: Add repository to git safe directories
+      run: git config --global --add safe.directory $GITHUB_WORKSPACE
 
     # Grant execute permission for gradlew
     - name: Grant execute permission for gradlew

diff --git a/.gitignore b/.gitignore
index 8965997f16db1dd188eb5be83b56a9994cfde38c..10c4de4fdb54d73702c72e0de7fdaf90f0d92b23 100644 (file)
--- a/.gitignore
+++ b/.gitignore
@@ -160,6 +160,22 @@ bin/
 
 # Simulation GUI and other tools window save file
 *-window.json
+*simgui.json
+simgui-ds.json
+networktables.json
+networktables.json.bck
+
+# Simulation puts log files in root of repo
+*.wpilog
+
+# Don't commit ctre_sim directory
+ctre_sim/*
 
 # File that has git and version data
-src/main/java/frc/robot/util/BuildData.java
\ No newline at end of file
+src/main/java/frc/robot/util/BuildData.java
+
+# Log files from simulation
+logs/
+
+# IntelliJ IDEA files
+.idea/

diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
new file mode 100644 (file)
index 0000000..0c64c43
--- /dev/null
+++ b/.pathplanner/settings.json
@@ -0,0 +1,15 @@
+{
+  "robotWidth": 0.76,
+  "robotLength": 0.91,
+  "holonomicMode": true,
+  "pathFolders": [
+    "Final Stuff",
+    "Stuff"
+  ],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "maxModuleSpeed": 4.5
+}
\ No newline at end of file

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 4ed293b78f728bd074893f985f4e12650a4d0bcc..612cdd0d950720ab872439a19bd542fadb01c613 100644 (file)
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -25,5 +25,36 @@
       }
     },
   ],
-  "java.test.defaultConfig": "WPIlibUnitTests"
+  "java.test.defaultConfig": "WPIlibUnitTests",
+  "java.import.gradle.annotationProcessing.enabled": false,
+  "java.completion.favoriteStaticMembers": [
+    "org.junit.Assert.*",
+    "org.junit.Assume.*",
+    "org.junit.jupiter.api.Assertions.*",
+    "org.junit.jupiter.api.Assumptions.*",
+    "org.junit.jupiter.api.DynamicContainer.*",
+    "org.junit.jupiter.api.DynamicTest.*",
+    "org.mockito.Mockito.*",
+    "org.mockito.ArgumentMatchers.*",
+    "org.mockito.Answers.*",
+    "edu.wpi.first.units.Units.*"
+  ],
+  "java.completion.filteredTypes": [
+    "java.awt.*",
+    "com.sun.*",
+    "sun.*",
+    "jdk.*",
+    "org.graalvm.*",
+    "io.micrometer.shaded.*",
+    "java.beans.*",
+    "java.util.Base64.*",
+    "java.util.Timer",
+    "java.sql.*",
+    "javax.swing.*",
+    "javax.management.*",
+    "javax.smartcardio.*",
+    "edu.wpi.first.math.proto.*",
+    "edu.wpi.first.math.**.proto.*",
+    "edu.wpi.first.math.**.struct.*",
+  ]
 }

diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json
index 5e8c81013997b5db0458d27ee9f012fb45d8b2da..e56da02420c332c2200ac254e583c6b9b356dffb 100644 (file)
--- a/.wpilib/wpilib_preferences.json
+++ b/.wpilib/wpilib_preferences.json
@@ -1,6 +1,6 @@
 {
     "enableCppIntellisense": false,
     "currentLanguage": "java",
-    "projectYear": "2022",
+    "projectYear": "2025",
     "teamNumber": 972
 }
\ No newline at end of file

diff --git a/README.md b/README.md
index b0cddad0c2e0b725e201c2f3c5d5c8d79077724e..f1db6d762c0efc544c496f48ec2aabb3fbed4a2e 100644 (file)
--- a/README.md
+++ b/README.md
@@ -1,6 +1,5 @@
-# Code-Structure-2023
+# FRC2026
 
-[![CI](https://github.com/iron-claw-972/Code-Structure-2023/actions/workflows/main.yml/badge.svg)](https://github.com/iron-claw-972/Code-Structure-2023/actions/workflows/main.yml)
+[![CI](https://github.com/iron-claw-972/FRC2025/actions/workflows/main.yml/badge.svg)](https://github.com/iron-claw-972/FRC2025/actions/workflows/main.yml)
 
-
-Code Structure for our future 2023 repository.
+Code for the 2025-2026 FRC Season.

diff --git a/WPILib-License.md b/WPILib-License.md
index 3d5a824cadb0d7e51c50464780594063a7711eb9..645e54253ad07faea41d725c3fd42f180e89957f 100644 (file)
--- a/WPILib-License.md
+++ b/WPILib-License.md
@@ -1,4 +1,4 @@
-Copyright (c) 2009-2021 FIRST and other WPILib contributors
+Copyright (c) 2009-2024 FIRST and other WPILib contributors
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

diff --git a/build.gradle b/build.gradle
index 281129dd141b7dda25e960ba6ac1954f1f00f9a5..9e033e6225775115b564ef57468e3c3860c276b6 100644 (file)
--- a/build.gradle
+++ b/build.gradle
@@ -1,13 +1,14 @@
-import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO
-
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2022.4.1"
-    id "com.peterabeles.gversion" version "1.10"
+    id "edu.wpi.first.GradleRIO" version "2025.3.1"
+    id "com.peterabeles.gversion" version "1.10.3"
+
 }
 
-sourceCompatibility = JavaVersion.VERSION_11
-targetCompatibility = JavaVersion.VERSION_11
+java {
+    sourceCompatibility = JavaVersion.VERSION_17
+    targetCompatibility = JavaVersion.VERSION_17
+}
 
 def ROBOT_MAIN_CLASS = "frc.robot.Main"
 
@@ -28,12 +29,22 @@ deploy {
                 // getTargetTypeClass is a shortcut to get the class type using a string
 
                 frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
+                    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("-XX:+HeapDumpOnOutOfMemoryError")
+                    jvmArgs.add("-XX:HeapDumpPath=/home/lvuser/frc-usercode.hprof")
+                    jvmArgs.add("-Djava.rmi.server.hostname=10.9.72.2") // Replace TE.AM with team number
                 }
 
                 // Static files artifact
                 frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
                     files = project.fileTree('src/main/deploy')
                     directory = '/home/lvuser/deploy'
+                    deleteOldFiles = false // Change to true to delete files on roboRIO that no
+                                           // longer exist in deploy directory of this project
                 }
             }
         }
@@ -48,9 +59,47 @@ wpi.java.debugJni = false
 // Set this to true to enable desktop support.
 def includeDesktopSupport = true
 
+// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+// Also defines JUnit 5.
+dependencies {
+    annotationProcessor wpi.java.deps.wpilibAnnotations()
+    implementation wpi.java.deps.wpilib()
+    implementation wpi.java.vendor.java()
+
+    roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
+    roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
+
+    roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
+    roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)
+
+    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()
+
+    implementation "gov.nist.math:jama:1.0.3"
+
+    testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
+    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
+    def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
+    annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
+ 
+}
+
+// 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()
 
@@ -67,6 +116,17 @@ dependencies {
     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 groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
+    annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
+}
+
+test {
+    useJUnitPlatform()
+    systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
 }
 
 // Simulation configuration (e.g. environment variables).
@@ -78,21 +138,24 @@ wpi.sim.addDriverstation()
 // knows where to look for our Robot Class.
 jar {
     from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
+    from sourceSets.main.allSource
     manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
     duplicatesStrategy = DuplicatesStrategy.INCLUDE
 }
 
+gversion {
+    srcDir = file("src/main/java")
+    classPackage = "frc.robot.util"
+    className = "BuildData" // This will generate GVersion.java
+}
+
 // Configure jar and deploy tasks
 deployArtifact.jarTask = jar
 wpi.java.configureExecutableTasks(jar)
 wpi.java.configureTestTasks(test)
 
-project.compileJava.dependsOn(createVersionFile)
-gversion {
-    srcDir       = "src/main/java/"
-    classPackage = "frc.robot.util"
-    className    = "BuildData"
-    dateFormat   = "yyyy-MM-dd HH:mm:ss z"
-    timeZone     = "PST"
-    indent       = "  "
-}
\ No newline at end of file
+// Configure string concat to always inline compile
+tasks.withType(JavaCompile) {
+    options.compilerArgs.add '-XDstringConcat=inline'
+}
+

diff --git a/choreo_paths.chor b/choreo_paths.chor
new file mode 100644 (file)
index 0000000..ba97e56
--- /dev/null
+++ b/choreo_paths.chor
@@ -0,0 +1,14676 @@
+{
+    "version": "v0.3.1",
+    "robotConfiguration": {
+        "mass": 73.8410170797483,
+        "rotationalInertia": 5.8275989259364,
+        "motorMaxTorque": 0.6789855072463769,
+        "motorMaxVelocity": 4640,
+        "gearing": 5.903,
+        "wheelbase": 0.5270497153931537,
+        "trackWidth": 0.5270497153931537,
+        "bumperLength": 0.8732515284441746,
+        "bumperWidth": 0.8732515284441746,
+        "wheelRadius": 0.050799972568014815
+    },
+    "paths": {
+        "Center 6": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 2.6305306911468507,
+                    "y": 4.355533123016357,
+                    "heading": -0.6,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 11
+                },
+                {
+                    "x": 2.7529065132141115,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 11
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 2.6305306911468507,
+                    "y": 6.870196914672851,
+                    "heading": 0.6,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "x": 5.372490882873535,
+                    "y": 6.594394683837891,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 8.173408317565917,
+                    "y": 7.465926647186279,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 5.372490882873535,
+                    "y": 6.594394683837891,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "x": 5.372490882873535,
+                    "y": 6.594394683837891,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 8.160777854919433,
+                    "y": 5.798648357391357,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 5.372490882873535,
+                    "y": 6.594394683837891,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 4.251064518955209e-23,
+                    "angularVelocity": -1.474314261329566e-20,
+                    "velocityX": 3.7387078793459146e-19,
+                    "velocityY": -3.5824936616210366e-19,
+                    "timestamp": 0
+                },
+                {
+                    "x": 1.3700049879548286,
+                    "y": 5.56309045957489,
+                    "heading": -0.009280219609018163,
+                    "angularVelocity": -0.12291476177728405,
+                    "velocityX": 0.26496229964597995,
+                    "velocityY": -0.25385449119608827,
+                    "timestamp": 0.07550126180802841
+                },
+                {
+                    "x": 1.4100151408697066,
+                    "y": 5.524757983351538,
+                    "heading": -0.02784117001944216,
+                    "angularVelocity": -0.24583629420151398,
+                    "velocityX": 0.529926943692794,
+                    "velocityY": -0.5077064317258396,
+                    "timestamp": 0.15100252361605682
+                },
+                {
+                    "x": 1.4700306924160145,
+                    "y": 5.467259673777908,
+                    "heading": -0.05568513031548327,
+                    "angularVelocity": -0.36878801266709876,
+                    "velocityX": 0.794894682673056,
+                    "velocityY": -0.7615542866002325,
+                    "timestamp": 0.22650378542408522
+                },
+                {
+                    "x": 1.5500518988129126,
+                    "y": 5.390595964752488,
+                    "heading": -0.09281752021598637,
+                    "angularVelocity": -0.4918115142885552,
+                    "velocityX": 1.0598658152278608,
+                    "velocityY": -1.0153963945708073,
+                    "timestamp": 0.30200504723211363
+                },
+                {
+                    "x": 1.6500789877423618,
+                    "y": 5.29476741780182,
+                    "heading": -0.13924857672911986,
+                    "angularVelocity": -0.6149706031561483,
+                    "velocityX": 1.3248399633873773,
+                    "velocityY": -1.269231065228089,
+                    "timestamp": 0.37750630904014204
+                },
+                {
+                    "x": 1.7701120892018134,
+                    "y": 5.179774707267526,
+                    "heading": -0.19499506458965388,
+                    "angularVelocity": -0.7383517377799143,
+                    "velocityX": 1.5898158333386678,
+                    "velocityY": -1.5230568043574852,
+                    "timestamp": 0.45300757084817045
+                },
+                {
+                    "x": 1.9101511510897655,
+                    "y": 5.04561857790655,
+                    "heading": -0.26008157143235316,
+                    "angularVelocity": -0.8620585309976676,
+                    "velocityX": 1.854791013215372,
+                    "velocityY": -1.7768726793213794,
+                    "timestamp": 0.5285088326561989
+                },
+                {
+                    "x": 2.070195845092578,
+                    "y": 4.892299771059555,
+                    "heading": -0.3345408337552403,
+                    "angularVelocity": -0.9861989129692874,
+                    "velocityX": 2.1197618446397177,
+                    "velocityY": -2.030678735367728,
+                    "timestamp": 0.6040100944642273
+                },
+                {
+                    "x": 2.210251097690708,
+                    "y": 4.758137158014049,
+                    "heading": -0.40081482224653847,
+                    "angularVelocity": -0.8777865018972559,
+                    "velocityX": 1.855005456123874,
+                    "velocityY": -1.776958554502474,
+                    "timestamp": 0.6795113562722557
+                },
+                {
+                    "x": 2.330301718519033,
+                    "y": 4.643136516439017,
+                    "heading": -0.4576616163795054,
+                    "angularVelocity": -0.752925087232411,
+                    "velocityX": 1.5900478740814894,
+                    "velocityY": -1.523161849499088,
+                    "timestamp": 0.7550126180802841
+                },
+                {
+                    "x": 2.4303482300309223,
+                    "y": 4.547297740543023,
+                    "heading": -0.5050607440151612,
+                    "angularVelocity": -0.6277925229405316,
+                    "velocityX": 1.3250972118356075,
+                    "velocityY": -1.2693665456833927,
+                    "timestamp": 0.8305138798883125
+                },
+                {
+                    "x": 2.510391075160622,
+                    "y": 4.470620833949426,
+                    "heading": -0.5429978618626794,
+                    "angularVelocity": -0.5024699844616926,
+                    "velocityX": 1.060152416170463,
+                    "velocityY": -1.0155711938769658,
+                    "timestamp": 0.9060151416963409
+                },
+                {
+                    "x": 2.5704305889794776,
+                    "y": 4.4131058554979345,
+                    "heading": -0.5714637486145556,
+                    "angularVelocity": -0.3770253115008103,
+                    "velocityX": 0.7952120584621968,
+                    "velocityY": -0.7617750627496834,
+                    "timestamp": 0.9815164035043693
+                },
+                {
+                    "x": 2.6104669873911317,
+                    "y": 4.374752877259734,
+                    "heading": -0.5904533440855589,
+                    "angularVelocity": -0.2515136173390948,
+                    "velocityX": 0.5302745603570374,
+                    "velocityY": -0.5079779770531248,
+                    "timestamp": 1.0570176653123977
+                },
+                {
+                    "x": 2.6305003641950035,
+                    "y": 4.355561956718282,
+                    "heading": -0.5999649881801159,
+                    "angularVelocity": -0.1259799355240121,
+                    "velocityX": 0.2653383046075278,
+                    "velocityY": -0.25418012999898976,
+                    "timestamp": 1.1325189271204261
+                },
+                {
+                    "x": 2.6305306911468507,
+                    "y": 4.355533123016357,
+                    "heading": -0.6,
+                    "angularVelocity": -0.0004637249636049969,
+                    "velocityX": 0.0004016747683561972,
+                    "velocityY": -0.00038189695422426704,
+                    "timestamp": 1.2080201889284545
+                },
+                {
+                    "x": 2.610550178329772,
+                    "y": 4.374673695560834,
+                    "heading": -0.5905590496217955,
+                    "angularVelocity": 0.12501971996346353,
+                    "velocityX": -0.2645875697942839,
+                    "velocityY": 0.25346484449015166,
+                    "timestamp": 1.2835358786209563
+                },
+                {
+                    "x": 2.5705586389053807,
+                    "y": 4.412983573513255,
+                    "heading": -0.5716455307114111,
+                    "angularVelocity": 0.2504581364137668,
+                    "velocityX": -0.5295792117801764,
+                    "velocityY": 0.507310177638816,
+                    "timestamp": 1.359051568313458
+                },
+                {
+                    "x": 2.5105557750803515,
+                    "y": 4.470462660598173,
+                    "heading": -0.5432668457654932,
+                    "angularVelocity": 0.3757985269216927,
+                    "velocityX": -0.7945747972290169,
+                    "velocityY": 0.7611542358809293,
+                    "timestamp": 1.4345672580059599
+                },
+                {
+                    "x": 2.430541183538218,
+                    "y": 4.547110911961194,
+                    "heading": -0.5054353006402181,
+                    "angularVelocity": 0.5009759598213868,
+                    "velocityX": -1.0595757235079437,
+                    "velocityY": 1.014997700148553,
+                    "timestamp": 1.5100829476984616
+                },
+                {
+                    "x": 2.330514381369411,
+                    "y": 4.642928388936716,
+                    "heading": -0.45816894222168364,
+                    "angularVelocity": 0.6259144107800899,
+                    "velocityX": -1.3245830446111697,
+                    "velocityY": 1.2688419766235208,
+                    "timestamp": 1.5855986373909634
+                },
+                {
+                    "x": 2.210474854671869,
+                    "y": 4.757915322387685,
+                    "heading": -0.401491937913021,
+                    "angularVelocity": 0.7505328301899918,
+                    "velocityX": -1.5895971709500427,
+                    "velocityY": 1.5226893102505303,
+                    "timestamp": 1.6611143270834652
+                },
+                {
+                    "x": 2.0704221363896043,
+                    "y": 4.892072174009052,
+                    "heading": -0.33543399057621603,
+                    "angularVelocity": 0.8747579159482137,
+                    "velocityX": -1.8546174821756414,
+                    "velocityY": 1.7765427577719441,
+                    "timestamp": 1.736630016775967
+                },
+                {
+                    "x": 1.9103559157267933,
+                    "y": 5.045399678728609,
+                    "heading": -0.2600283702226508,
+                    "angularVelocity": 0.9985424308592702,
+                    "velocityX": -2.1196419090469347,
+                    "velocityY": 2.0304059374138372,
+                    "timestamp": 1.8121457064684687
+                },
+                {
+                    "x": 1.7702658106581808,
+                    "y": 5.1796120110222805,
+                    "heading": -0.19499833457123836,
+                    "angularVelocity": 0.8611460203331687,
+                    "velocityX": -1.8551125685146552,
+                    "velocityY": 1.7772774484372842,
+                    "timestamp": 1.8876613961609705
+                },
+                {
+                    "x": 1.650189127018009,
+                    "y": 5.294652307387748,
+                    "heading": -0.13927528164735775,
+                    "angularVelocity": 0.7379003376753047,
+                    "velocityX": -1.5900892136338995,
+                    "velocityY": 1.5233959569714512,
+                    "timestamp": 1.9631770858534723
+                },
+                {
+                    "x": 1.5501256774020051,
+                    "y": 5.3905199561224375,
+                    "heading": -0.092847301921564,
+                    "angularVelocity": 0.6148123643556385,
+                    "velocityX": -1.3250683404132293,
+                    "velocityY": 1.269506365168085,
+                    "timestamp": 2.0386927755459743
+                },
+                {
+                    "x": 1.4700752292390322,
+                    "y": 5.467214496036427,
+                    "heading": -0.055707914242732186,
+                    "angularVelocity": 0.4918102162618465,
+                    "velocityX": -1.0600505469649628,
+                    "velocityY": 1.0156106661581024,
+                    "timestamp": 2.1142084652384763
+                },
+                {
+                    "x": 1.4100375649931816,
+                    "y": 5.524735600725888,
+                    "heading": -0.027854100312408023,
+                    "angularVelocity": 0.3688480373250149,
+                    "velocityX": -0.795035634188372,
+                    "velocityY": 0.7617106448167867,
+                    "timestamp": 2.1897241549309783
+                },
+                {
+                    "x": 1.3700125191328987,
+                    "y": 5.563083065206659,
+                    "heading": -0.009284821466752706,
+                    "angularVelocity": 0.24589961266683785,
+                    "velocityX": -0.5300229134271852,
+                    "velocityY": 0.5078079090175974,
+                    "timestamp": 2.2652398446234803
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 1.5518121767006254e-21,
+                    "angularVelocity": 0.12295221701027016,
+                    "velocityX": -0.2650114064294312,
+                    "velocityY": 0.2539039085419965,
+                    "timestamp": 2.3407555343159823
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 1.7406173041226286e-22,
+                    "angularVelocity": -1.5911931901870827e-20,
+                    "velocityX": 3.5223689868208494e-19,
+                    "velocityY": -3.372608251853029e-19,
+                    "timestamp": 2.4162712240084843
+                },
+                {
+                    "x": 1.3889696841330885,
+                    "y": 5.58225679397583,
+                    "heading": -6.407798473621889e-19,
+                    "angularVelocity": -7.175883313096834e-18,
+                    "velocityX": 0.4362901951704297,
+                    "velocityY": 2.0950088972512994e-17,
+                    "timestamp": 2.505591783041138
+                },
+                {
+                    "x": 1.466909051129583,
+                    "y": 5.58225679397583,
+                    "heading": -2.0240616740468293e-18,
+                    "angularVelocity": -1.5486712596203492e-17,
+                    "velocityX": 0.8725803761259643,
+                    "velocityY": 4.333390986706464e-17,
+                    "timestamp": 2.594912342073792
+                },
+                {
+                    "x": 1.5838180988942594,
+                    "y": 5.58225679397583,
+                    "heading": -4.31930166451708e-18,
+                    "angularVelocity": -2.5696659484827644e-17,
+                    "velocityX": 1.3088705336241426,
+                    "velocityY": 6.752712300201592e-17,
+                    "timestamp": 2.6842329011064456
+                },
+                {
+                    "x": 1.739696823313371,
+                    "y": 5.58225679397583,
+                    "heading": -7.859560105579577e-18,
+                    "angularVelocity": -3.963542637204652e-17,
+                    "velocityX": 1.7451606450663326,
+                    "velocityY": 9.527288452189843e-17,
+                    "timestamp": 2.7735534601390994
+                },
+                {
+                    "x": 1.9345452126267868,
+                    "y": 5.58225679397583,
+                    "heading": -1.3596874858950657e-17,
+                    "angularVelocity": -6.423285764769935e-17,
+                    "velocityX": 2.1814506248464403,
+                    "velocityY": 1.331744714968596e-16,
+                    "timestamp": 2.862874019171753
+                },
+                {
+                    "x": 2.1683629455887066,
+                    "y": 5.58225679397583,
+                    "heading": -8.610584120256401e-18,
+                    "angularVelocity": 5.582467007271055e-17,
+                    "velocityX": 2.617737008077176,
+                    "velocityY": 4.485060032849567e-16,
+                    "timestamp": 2.952194578204407
+                },
+                {
+                    "x": 2.3632110176723393,
+                    "y": 5.58225679397583,
+                    "heading": -5.705566749802383e-18,
+                    "angularVelocity": 3.252350188947776e-17,
+                    "velocityX": 2.1814470732589166,
+                    "velocityY": 4.562389679160296e-16,
+                    "timestamp": 3.041515137237061
+                },
+                {
+                    "x": 2.5190894141011864,
+                    "y": 5.58225679397583,
+                    "heading": -3.685453907320999e-18,
+                    "angularVelocity": 2.2616437518674518e-17,
+                    "velocityX": 1.7451569730084335,
+                    "velocityY": 4.512114906158933e-16,
+                    "timestamp": 3.1308356962697146
+                },
+                {
+                    "x": 2.6359981302057838,
+                    "y": 5.58225679397583,
+                    "heading": -2.172184802335044e-18,
+                    "angularVelocity": 1.6942002170479532e-17,
+                    "velocityX": 1.3088668204803557,
+                    "velocityY": 4.421516421317778e-16,
+                    "timestamp": 3.2201562553023684
+                },
+                {
+                    "x": 2.7139371636966168,
+                    "y": 5.58225679397583,
+                    "heading": -9.80397662414107e-19,
+                    "angularVelocity": 1.3342808787156727e-17,
+                    "velocityX": 0.8725766423197178,
+                    "velocityY": 4.311146572193661e-16,
+                    "timestamp": 3.309476814335022
+                },
+                {
+                    "x": 2.7529065132141115,
+                    "y": 5.58225679397583,
+                    "heading": 5.080985322169408e-30,
+                    "angularVelocity": 1.0976170246252972e-17,
+                    "velocityX": 0.4362864489377896,
+                    "velocityY": 4.1890359689650406e-16,
+                    "timestamp": 3.398797373367676
+                },
+                {
+                    "x": 2.744525884733328,
+                    "y": 5.58225679397583,
+                    "heading": 9.653430149203684e-19,
+                    "angularVelocity": 9.143954657969524e-18,
+                    "velocityX": -0.07938327169603715,
+                    "velocityY": 4.0355164701809737e-16,
+                    "timestamp": 3.5043690927202595
+                },
+                {
+                    "x": 2.681705118537465,
+                    "y": 5.58225679397583,
+                    "heading": 1.7372399265678157e-18,
+                    "angularVelocity": 7.311587955464892e-18,
+                    "velocityX": -0.5950529799183868,
+                    "velocityY": 3.8819956283656593e-16,
+                    "timestamp": 3.609940812072843
+                },
+                {
+                    "x": 2.564444216810358,
+                    "y": 5.58225679397583,
+                    "heading": 2.315701250129161e-18,
+                    "angularVelocity": 5.4793208552192475e-18,
+                    "velocityX": -1.1107226674549495,
+                    "velocityY": 3.728472544116715e-16,
+                    "timestamp": 3.7155125314254267
+                },
+                {
+                    "x": 2.3927431839196713,
+                    "y": 5.58225679397583,
+                    "heading": 2.700660449129098e-18,
+                    "angularVelocity": 3.646423505928536e-18,
+                    "velocityX": -1.6263923136199703,
+                    "velocityY": 3.5749449791095625e-16,
+                    "timestamp": 3.8210842507780103
+                },
+                {
+                    "x": 2.1666020329683717,
+                    "y": 5.58225679397583,
+                    "heading": 2.8921372367337455e-18,
+                    "angularVelocity": 1.8137128843547566e-18,
+                    "velocityX": -2.1420618356706225,
+                    "velocityY": 3.421403973689333e-16,
+                    "timestamp": 3.926655970130594
+                },
+                {
+                    "x": 1.8944013714726986,
+                    "y": 5.58225679397583,
+                    "heading": 1.9264146661700297e-18,
+                    "angularVelocity": -9.147549897618139e-18,
+                    "velocityX": -2.5783482846063124,
+                    "velocityY": -7.6372503608359e-17,
+                    "timestamp": 4.032227689483178
+                },
+                {
+                    "x": 1.6766408280374865,
+                    "y": 5.58225679397583,
+                    "heading": 1.154320231308916e-18,
+                    "angularVelocity": -7.313458941346627e-18,
+                    "velocityX": -2.062678762557092,
+                    "velocityY": -6.102339516740112e-17,
+                    "timestamp": 4.137799408835762
+                },
+                {
+                    "x": 1.5133204157658162,
+                    "y": 5.58225679397583,
+                    "heading": 5.756870468064279e-19,
+                    "angularVelocity": -5.480948762057906e-18,
+                    "velocityX": -1.5470091163924304,
+                    "velocityY": -4.567313800965292e-17,
+                    "timestamp": 4.243371128188346
+                },
+                {
+                    "x": 1.4044401390253736,
+                    "y": 5.58225679397583,
+                    "heading": 1.9048457565450654e-19,
+                    "angularVelocity": -3.64872783644659e-18,
+                    "velocityX": -1.031339428856028,
+                    "velocityY": -3.032249796435494e-17,
+                    "timestamp": 4.34894284754093
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": -1.3128115287299839e-21,
+                    "angularVelocity": -1.8167496783306206e-18,
+                    "velocityX": -0.5156697206337688,
+                    "velocityY": -1.4971666378034364e-17,
+                    "timestamp": 4.454514566893514
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 2.832404054183828e-22,
+                    "angularVelocity": 1.5118177032219752e-20,
+                    "velocityX": 3.7696639615992387e-19,
+                    "velocityY": 3.7927992176190913e-19,
+                    "timestamp": 4.560086286246098
+                },
+                {
+                    "x": 1.3700047148687937,
+                    "y": 5.602379495184821,
+                    "heading": 0.009288191856822618,
+                    "angularVelocity": 0.12157329926444245,
+                    "velocityX": 0.2618420490159499,
+                    "velocityY": 0.26338637420506966,
+                    "timestamp": 4.636486218739657
+                },
+                {
+                    "x": 1.4100143230250328,
+                    "y": 5.642624726402822,
+                    "heading": 0.027864540168642656,
+                    "angularVelocity": 0.24314613515380012,
+                    "velocityX": 0.5236864333566297,
+                    "velocityY": 0.526770507570724,
+                    "timestamp": 4.712886151233216
+                },
+                {
+                    "x": 1.4700290685888693,
+                    "y": 5.702992212923995,
+                    "heading": 0.05573027757780551,
+                    "angularVelocity": 0.3647351051195263,
+                    "velocityX": 0.7855340129900809,
+                    "velocityY": 0.7901510453070398,
+                    "timestamp": 4.7892860837267754
+                },
+                {
+                    "x": 1.5500492317268397,
+                    "y": 5.7834815634613825,
+                    "heading": 0.09288926238249703,
+                    "angularVelocity": 0.48637457641503007,
+                    "velocityX": 1.047385259728037,
+                    "velocityY": 1.0535264614818005,
+                    "timestamp": 4.865686016220335
+                },
+                {
+                    "x": 1.650075082736049,
+                    "y": 5.884092262166466,
+                    "heading": 0.13934967791469124,
+                    "angularVelocity": 0.6081211594802297,
+                    "velocityX": 1.3092400443893395,
+                    "velocityY": 1.3168951257066361,
+                    "timestamp": 4.942085948713894
+                },
+                {
+                    "x": 1.7701068177219876,
+                    "y": 6.004823674860892,
+                    "heading": 0.19512584033881888,
+                    "angularVelocity": 0.7300551270621883,
+                    "velocityX": 1.571097395878699,
+                    "velocityY": 1.5802554891603338,
+                    "timestamp": 5.018485881207453
+                },
+                {
+                    "x": 1.9101444770585922,
+                    "y": 6.145675079899509,
+                    "heading": 0.26023970526210166,
+                    "angularVelocity": 0.8522764719559472,
+                    "velocityX": 1.8329552758231984,
+                    "velocityY": 1.843606406988539,
+                    "timestamp": 5.094885813701012
+                },
+                {
+                    "x": 2.070187850431952,
+                    "y": 6.306645728097026,
+                    "heading": 0.334721545165819,
+                    "angularVelocity": 0.9748940538657789,
+                    "velocityX": 2.094810402965384,
+                    "velocityY": 2.1069475187178845,
+                    "timestamp": 5.171285746194571
+                },
+                {
+                    "x": 2.2102453512140934,
+                    "y": 6.4475037492072484,
+                    "heading": 0.40094964082687773,
+                    "angularVelocity": 0.8668606568028369,
+                    "velocityX": 1.83321498083718,
+                    "velocityY": 1.8436930048609552,
+                    "timestamp": 5.24768567868813
+                },
+                {
+                    "x": 2.3302979525685337,
+                    "y": 6.568243321341568,
+                    "heading": 0.4577574427913322,
+                    "angularVelocity": 0.743558274338055,
+                    "velocityX": 1.5713705161265887,
+                    "velocityY": 1.5803622882061914,
+                    "timestamp": 5.324085611181689
+                },
+                {
+                    "x": 2.4303461105664526,
+                    "y": 6.66886460713537,
+                    "heading": 0.5051246017989356,
+                    "angularVelocity": 0.6199895400640164,
+                    "velocityX": 1.3095320209393337,
+                    "velocityY": 1.3170337002887431,
+                    "timestamp": 5.400485543675249
+                },
+                {
+                    "x": 2.510390222499523,
+                    "y": 6.7493676560178235,
+                    "heading": 0.5430365397795383,
+                    "angularVelocity": 0.49622999318486116,
+                    "velocityX": 1.0476987259094657,
+                    "velocityY": 1.0537057593505197,
+                    "timestamp": 5.476885476168808
+                },
+                {
+                    "x": 2.570430594098527,
+                    "y": 6.809752452382176,
+                    "heading": 0.5714836779820867,
+                    "angularVelocity": 0.37234506987223925,
+                    "velocityX": 0.7858694325949297,
+                    "velocityY": 0.7903776141352329,
+                    "timestamp": 5.553285408662367
+                },
+                {
+                    "x": 2.6104674242959973,
+                    "y": 6.850018954046101,
+                    "heading": 0.5904606458530037,
+                    "angularVelocity": 0.24838985129361124,
+                    "velocityX": 0.5240427431116611,
+                    "velocityY": 0.5270489167947889,
+                    "timestamp": 5.629685341155926
+                },
+                {
+                    "x": 2.6305007993173484,
+                    "y": 6.870167118249366,
+                    "heading": 0.5999656288193816,
+                    "angularVelocity": 0.1244108817397083,
+                    "velocityX": 0.262217182234288,
+                    "velocityY": 0.26371965976492745,
+                    "timestamp": 5.706085273649485
+                },
+                {
+                    "x": 2.6305306911468507,
+                    "y": 6.870196914672851,
+                    "heading": 0.6,
+                    "angularVelocity": 0.00044988496058163594,
+                    "velocityX": 0.00039125465856706536,
+                    "velocityY": 0.0003900058876048134,
+                    "timestamp": 5.782485206143044
+                },
+                {
+                    "x": 2.610549437302289,
+                    "y": 6.850100761877325,
+                    "heading": 0.5905647509563424,
+                    "angularVelocity": -0.1234749074366408,
+                    "velocityX": -0.26148578140433243,
+                    "velocityY": -0.26298941286856325,
+                    "timestamp": 5.858899509579546
+                },
+                {
+                    "x": 2.570556867464376,
+                    "y": 6.809878732236684,
+                    "heading": 0.5716635252816524,
+                    "angularVelocity": -0.2473519331416332,
+                    "velocityX": -0.523364972778243,
+                    "velocityY": -0.526367811152851,
+                    "timestamp": 5.935313813016047
+                },
+                {
+                    "x": 2.5105527119270885,
+                    "y": 6.74953087982974,
+                    "heading": 0.5433040446606279,
+                    "angularVelocity": -0.37112790859358535,
+                    "velocityX": -0.7852476936749044,
+                    "velocityY": -0.7897455017317754,
+                    "timestamp": 6.011728116452549
+                },
+                {
+                    "x": 2.4305366108384567,
+                    "y": 6.669057196782285,
+                    "heading": 0.5054988442861839,
+                    "angularVelocity": -0.4947398415515119,
+                    "velocityX": -1.047135123794239,
+                    "velocityY": -1.0531232953569503,
+                    "timestamp": 6.08814241988905
+                },
+                {
+                    "x": 2.3305081438358277,
+                    "y": 6.5684575642560175,
+                    "heading": 0.4582658920144913,
+                    "angularVelocity": -0.618116637167829,
+                    "velocityX": -1.3090280550126343,
+                    "velocityY": -1.3165026441661212,
+                    "timestamp": 6.164556723325552
+                },
+                {
+                    "x": 2.210466881094948,
+                    "y": 6.447731698037168,
+                    "heading": 0.4016287033187306,
+                    "angularVelocity": -0.7411856962463141,
+                    "velocityX": -1.5709266111498505,
+                    "velocityY": -1.5798857123544938,
+                    "timestamp": 6.240971026762053
+                },
+                {
+                    "x": 2.0704124606025633,
+                    "y": 6.306879099600228,
+                    "heading": 0.335615479546042,
+                    "angularVelocity": -0.863885696838732,
+                    "velocityX": -1.8328299047934917,
+                    "velocityY": -1.8432753045244243,
+                    "timestamp": 6.3173853301985545
+                },
+                {
+                    "x": 1.9103446925850813,
+                    "y": 6.145899029088003,
+                    "heading": 0.2602568981833521,
+                    "angularVelocity": -0.986184234805082,
+                    "velocityX": -2.0947356819197345,
+                    "velocityY": -2.1066745788763823,
+                    "timestamp": 6.393799633635056
+                },
+                {
+                    "x": 1.7702572862745904,
+                    "y": 6.004990211385915,
+                    "heading": 0.19517700895499335,
+                    "angularVelocity": -0.8516715628041874,
+                    "velocityX": -1.8332615755229642,
+                    "velocityY": -1.844011021041104,
+                    "timestamp": 6.4702139370715575
+                },
+                {
+                    "x": 1.6501829922925522,
+                    "y": 5.8842101837041305,
+                    "heading": 0.13940682943388838,
+                    "angularVelocity": -0.7298395328231781,
+                    "velocityX": -1.571358876310599,
+                    "velocityY": -1.5805944993289158,
+                    "timestamp": 6.546628240508059
+                },
+                {
+                    "x": 1.5501215725938375,
+                    "y": 5.783559506821828,
+                    "heading": 0.09293678090995587,
+                    "angularVelocity": -0.6081328551603944,
+                    "velocityX": -1.3094592922889485,
+                    "velocityY": -1.3171706389490274,
+                    "timestamp": 6.62304254394456
+                },
+                {
+                    "x": 1.4700727644637088,
+                    "y": 5.703038594475421,
+                    "heading": 0.055762182382246936,
+                    "angularVelocity": -0.48648743567491987,
+                    "velocityX": -1.047563146298221,
+                    "velocityY": -1.0537413641847564,
+                    "timestamp": 6.699456847381062
+                },
+                {
+                    "x": 1.4100363343772997,
+                    "y": 5.642647734660285,
+                    "heading": 0.027881288300434102,
+                    "angularVelocity": -0.3648648594301607,
+                    "velocityX": -0.7856700563435488,
+                    "velocityY": -0.79030832055309,
+                    "timestamp": 6.775871150817563
+                },
+                {
+                    "x": 1.3700121102510567,
+                    "y": 5.60238710647987,
+                    "heading": 0.009293825131536333,
+                    "angularVelocity": -0.24324586278985671,
+                    "velocityX": -0.5237792183697898,
+                    "velocityY": -0.5268729330742511,
+                    "timestamp": 6.852285454254065
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": -1.8755644647310815e-21,
+                    "angularVelocity": -0.12162415560405207,
+                    "velocityX": -0.2618895854712081,
+                    "velocityY": -0.2634364457796503,
+                    "timestamp": 6.928699757690566
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": -3.0910168812131e-22,
+                    "angularVelocity": 1.644951644272365e-20,
+                    "velocityX": 3.642271257475659e-19,
+                    "velocityY": 3.6611207460803027e-19,
+                    "timestamp": 7.005114061127068
+                },
+                {
+                    "x": 1.3751389062520347,
+                    "y": 5.5885822288409,
+                    "heading": 8.044474138243886e-19,
+                    "angularVelocity": 1.104684119406405e-17,
+                    "velocityX": 0.3450801575652578,
+                    "velocityY": 0.08682883964891483,
+                    "timestamp": 7.077963530158855
+                },
+                {
+                    "x": 1.4254167183820563,
+                    "y": 5.601233098476921,
+                    "heading": 3.3304984787719056e-18,
+                    "angularVelocity": 3.467494133414787e-17,
+                    "velocityX": 0.6901603099959901,
+                    "velocityY": 0.17365767800588378,
+                    "timestamp": 7.150812999190642
+                },
+                {
+                    "x": 1.5008334359328959,
+                    "y": 5.620209402768863,
+                    "heading": 5.530664896423441e-18,
+                    "angularVelocity": 3.0201543633009046e-17,
+                    "velocityX": 1.0352404561511919,
+                    "velocityY": 0.2604865147838079,
+                    "timestamp": 7.22366246822243
+                },
+                {
+                    "x": 1.6013890583330925,
+                    "y": 5.645511141572933,
+                    "heading": 9.307328997077923e-18,
+                    "angularVelocity": 5.184202645265804e-17,
+                    "velocityX": 1.3803205944619807,
+                    "velocityY": 0.3473153495879261,
+                    "timestamp": 7.296511937254217
+                },
+                {
+                    "x": 1.7270835848479098,
+                    "y": 5.677138314704258,
+                    "heading": 1.388759929837292e-17,
+                    "angularVelocity": 6.287307734871695e-17,
+                    "velocityX": 1.7254007226870975,
+                    "velocityY": 0.4341441818542942,
+                    "timestamp": 7.3693614062860044
+                },
+                {
+                    "x": 1.8779170144977002,
+                    "y": 5.71509092191634,
+                    "heading": 2.114670867599054e-17,
+                    "angularVelocity": 9.964532993924348e-17,
+                    "velocityX": 2.0704808374646535,
+                    "velocityY": 0.520973010736996,
+                    "timestamp": 7.442210875317792
+                },
+                {
+                    "x": 2.0538893459109575,
+                    "y": 5.759368962864081,
+                    "heading": 2.860167806962463e-17,
+                    "angularVelocity": 1.0233388784665959e-16,
+                    "velocityX": 2.415560933415629,
+                    "velocityY": 0.6078018348825661,
+                    "timestamp": 7.515060344349579
+                },
+                {
+                    "x": 2.2550005770304224,
+                    "y": 5.809972437029836,
+                    "heading": 3.0384696860296094e-17,
+                    "angularVelocity": 2.447538484936638e-17,
+                    "velocityX": 2.7606410011267433,
+                    "velocityY": 0.6946306519224411,
+                    "timestamp": 7.5879098133813665
+                },
+                {
+                    "x": 2.4812507044273326,
+                    "y": 5.86690134355086,
+                    "heading": 3.263311662546467e-17,
+                    "angularVelocity": 3.086391424680145e-17,
+                    "velocityX": 3.1057210217714517,
+                    "velocityY": 0.7814594571194984,
+                    "timestamp": 7.660759282413154
+                },
+                {
+                    "x": 2.7326397212441713,
+                    "y": 5.930155680701671,
+                    "heading": 3.492569144669929e-17,
+                    "angularVelocity": 3.147002787608244e-17,
+                    "velocityX": 3.450800948283466,
+                    "velocityY": 0.8682882386309498,
+                    "timestamp": 7.733608751444941
+                },
+                {
+                    "x": 3.009167606908465,
+                    "y": 5.999735443305836,
+                    "heading": 3.599237380066186e-17,
+                    "angularVelocity": 1.464228041869e-17,
+                    "velocityX": 3.7958805923984467,
+                    "velocityY": 0.9551169490858475,
+                    "timestamp": 7.8064582204767285
+                },
+                {
+                    "x": 3.304583058119313,
+                    "y": 6.074067682586061,
+                    "heading": 6.799442288826936e-17,
+                    "angularVelocity": 4.392900801184583e-16,
+                    "velocityX": 4.055148996102434,
+                    "velocityY": 1.0203538923226851,
+                    "timestamp": 7.879307689508516
+                },
+                {
+                    "x": 3.5999985093303932,
+                    "y": 6.1483999218663445,
+                    "heading": 9.124686962806145e-17,
+                    "angularVelocity": 3.1918484854938582e-16,
+                    "velocityX": 4.055148996105624,
+                    "velocityY": 1.0203538923234874,
+                    "timestamp": 7.952157158540303
+                },
+                {
+                    "x": 3.8954139605414735,
+                    "y": 6.222732161146628,
+                    "heading": 6.235845734277902e-17,
+                    "angularVelocity": -3.965493869647151e-16,
+                    "velocityX": 4.055148996105624,
+                    "velocityY": 1.0203538923234874,
+                    "timestamp": 8.025006627572091
+                },
+                {
+                    "x": 4.190829411752554,
+                    "y": 6.297064400426912,
+                    "heading": 1.187079957064157e-17,
+                    "angularVelocity": -6.930408476976731e-16,
+                    "velocityX": 4.055148996105624,
+                    "velocityY": 1.0203538923234874,
+                    "timestamp": 8.097856096603879
+                },
+                {
+                    "x": 4.486244862963634,
+                    "y": 6.3713966397071955,
+                    "heading": 4.1868203670914044e-17,
+                    "angularVelocity": 4.1177244664893084e-16,
+                    "velocityX": 4.055148996105624,
+                    "velocityY": 1.0203538923234874,
+                    "timestamp": 8.170705565635666
+                },
+                {
+                    "x": 4.781660314174713,
+                    "y": 6.445728878987481,
+                    "heading": 5.651608381744153e-17,
+                    "angularVelocity": 2.010705135017266e-16,
+                    "velocityX": 4.0551489961056175,
+                    "velocityY": 1.0203538923235147,
+                    "timestamp": 8.243555034667454
+                },
+                {
+                    "x": 5.077075765372958,
+                    "y": 6.520061118318777,
+                    "heading": 1.7973952569935785e-17,
+                    "angularVelocity": -5.290653694492406e-16,
+                    "velocityX": 4.055148995929428,
+                    "velocityY": 1.0203538930237357,
+                    "timestamp": 8.316404503699241
+                },
+                {
+                    "x": 5.372490882873535,
+                    "y": 6.594394683837891,
+                    "heading": 4.5271640595913275e-29,
+                    "angularVelocity": -2.467272968321232e-16,
+                    "velocityX": 4.055144415282903,
+                    "velocityY": 1.0203720975190471,
+                    "timestamp": 8.389253972731028
+                },
+                {
+                    "x": 5.647242858587802,
+                    "y": 6.679885106259968,
+                    "heading": -9.429977726826593e-18,
+                    "angularVelocity": -1.3703765394515705e-16,
+                    "velocityX": 3.9927311876140994,
+                    "velocityY": 1.242357857334978,
+                    "timestamp": 8.458067013931142
+                },
+                {
+                    "x": 5.921994445476622,
+                    "y": 6.76537677829226,
+                    "heading": -1.6888267565983654e-17,
+                    "angularVelocity": -1.0838483097253208e-16,
+                    "velocityX": 3.9927255371524812,
+                    "velocityY": 1.2423760168319844,
+                    "timestamp": 8.526880055131256
+                },
+                {
+                    "x": 6.196746032348459,
+                    "y": 6.850868450379129,
+                    "heading": 3.8093668702008635e-18,
+                    "angularVelocity": 3.0078069614698596e-16,
+                    "velocityX": 3.9927255369056973,
+                    "velocityY": 1.2423760176250944,
+                    "timestamp": 8.59569309633137
+                },
+                {
+                    "x": 6.471497619219933,
+                    "y": 6.936360122466824,
+                    "heading": -1.5257964645179415e-17,
+                    "angularVelocity": -2.7708892359449947e-16,
+                    "velocityX": 3.9927255369004206,
+                    "velocityY": 1.242376017637102,
+                    "timestamp": 8.664506137531484
+                },
+                {
+                    "x": 6.734791042279063,
+                    "y": 7.0182864892966865,
+                    "heading": -1.5521608741636682e-17,
+                    "angularVelocity": -3.831310052315505e-18,
+                    "velocityX": 3.826214020877989,
+                    "velocityY": 1.190564541270817,
+                    "timestamp": 8.733319178731598
+                },
+                {
+                    "x": 6.975999483549517,
+                    "y": 7.0933408957400905,
+                    "heading": -1.3888820098781766e-17,
+                    "angularVelocity": 2.372789538662461e-17,
+                    "velocityX": 3.5052722138671455,
+                    "velocityY": 1.0907003256132755,
+                    "timestamp": 8.802132219931712
+                },
+                {
+                    "x": 7.19512292364644,
+                    "y": 7.161523335765207,
+                    "heading": -1.2272226828080516e-17,
+                    "angularVelocity": 2.349254214730808e-17,
+                    "velocityX": 3.1843301251531018,
+                    "velocityY": 0.990836022300431,
+                    "timestamp": 8.870945261131826
+                },
+                {
+                    "x": 7.3921613561082085,
+                    "y": 7.2228338073614236,
+                    "heading": -9.91377064737451e-18,
+                    "angularVelocity": 3.427338974634624e-17,
+                    "velocityX": 2.863387942537892,
+                    "velocityY": 0.8909716897691187,
+                    "timestamp": 8.93975830233194
+                },
+                {
+                    "x": 7.567114777704009,
+                    "y": 7.277272309523435,
+                    "heading": -7.430470042956345e-18,
+                    "angularVelocity": 3.6087645031897434e-17,
+                    "velocityX": 2.5424457129720897,
+                    "velocityY": 0.7911073426285686,
+                    "timestamp": 9.008571343532054
+                },
+                {
+                    "x": 7.719983186495353,
+                    "y": 7.3248388416480585,
+                    "heading": -5.288724881656081e-18,
+                    "angularVelocity": 3.1124117230174046e-17,
+                    "velocityX": 2.2215034552359305,
+                    "velocityY": 0.6912429867224752,
+                    "timestamp": 9.077384384732168
+                },
+                {
+                    "x": 7.850766581189917,
+                    "y": 7.3655334033331705,
+                    "heading": -3.488536974859941e-18,
+                    "angularVelocity": 2.6160563105152182e-17,
+                    "velocityX": 1.9005611787195344,
+                    "velocityY": 0.591378624972686,
+                    "timestamp": 9.146197425932282
+                },
+                {
+                    "x": 7.95946496086461,
+                    "y": 7.399355994291541,
+                    "heading": -1.0573272555421988e-18,
+                    "angularVelocity": 3.5330653563536946e-17,
+                    "velocityX": 1.5796188887886835,
+                    "velocityY": 0.4915142590488284,
+                    "timestamp": 9.215010467132396
+                },
+                {
+                    "x": 8.046078324827116,
+                    "y": 7.426306614307746,
+                    "heading": 1.2407808813183412e-18,
+                    "angularVelocity": 3.339640418092168e-17,
+                    "velocityX": 1.2586765887969913,
+                    "velocityY": 0.3916498899944195,
+                    "timestamp": 9.28382350833251
+                },
+                {
+                    "x": 8.110606672538966,
+                    "y": 7.446385263214236,
+                    "heading": 3.1973352292465776e-18,
+                    "angularVelocity": 2.843290041803697e-17,
+                    "velocityX": 0.9377342809802008,
+                    "velocityY": 0.2917855185051375,
+                    "timestamp": 9.352636549532624
+                },
+                {
+                    "x": 8.153050003569383,
+                    "y": 7.459591940876971,
+                    "heading": 1.989163877112771e-18,
+                    "angularVelocity": -1.7557302091088906e-17,
+                    "velocityX": 0.616791966903332,
+                    "velocityY": 0.191921145067957,
+                    "timestamp": 9.421449590732738
+                },
+                {
+                    "x": 8.173408317565917,
+                    "y": 7.465926647186279,
+                    "heading": -4.514694690881201e-29,
+                    "angularVelocity": -2.890678630808383e-17,
+                    "velocityX": 0.295849647704581,
+                    "velocityY": 0.09205677003704135,
+                    "timestamp": 9.490262631932852
+                },
+                {
+                    "x": 8.170410935320641,
+                    "y": 7.464993998545025,
+                    "heading": -1.1479594087562529e-18,
+                    "angularVelocity": -1.587345588598103e-17,
+                    "velocityX": -0.0414464261398678,
+                    "velocityY": -0.012896237403529248,
+                    "timestamp": 9.562582069926373
+                },
+                {
+                    "x": 8.14302049088619,
+                    "y": 7.456471207485986,
+                    "heading": -2.673164473169197e-18,
+                    "angularVelocity": -2.108983568881684e-17,
+                    "velocityX": -0.3787424957160769,
+                    "velocityY": -0.1178492435160072,
+                    "timestamp": 9.634901507919894
+                },
+                {
+                    "x": 8.091236984632978,
+                    "y": 7.440358274124418,
+                    "heading": -4.575611442410796e-18,
+                    "angularVelocity": -2.630616362629961e-17,
+                    "velocityX": -0.7160385601703985,
+                    "velocityY": -0.22280224803477408,
+                    "timestamp": 9.707220945913415
+                },
+                {
+                    "x": 8.015060417013729,
+                    "y": 7.41665519860119,
+                    "heading": -4.947277187842861e-18,
+                    "angularVelocity": -5.139223364033182e-18,
+                    "velocityX": -1.0533346183646357,
+                    "velocityY": -0.3277552506056719,
+                    "timestamp": 9.779540383906935
+                },
+                {
+                    "x": 7.914490788594349,
+                    "y": 7.385361981092388,
+                    "heading": -5.6961729062268445e-18,
+                    "angularVelocity": -1.0355386313000181e-17,
+                    "velocityX": -1.390630668733768,
+                    "velocityY": -0.4327082507417336,
+                    "timestamp": 9.851859821900456
+                },
+                {
+                    "x": 7.789528100102434,
+                    "y": 7.346478621824409,
+                    "heading": -8.528749636934408e-18,
+                    "angularVelocity": -3.9167571116071525e-17,
+                    "velocityX": -1.7279267090420518,
+                    "velocityY": -0.5376612477472918,
+                    "timestamp": 9.924179259893977
+                },
+                {
+                    "x": 7.64017235250811,
+                    "y": 7.3000051210991135,
+                    "heading": -1.201295949426594e-17,
+                    "angularVelocity": -4.817805494591479e-17,
+                    "velocityX": -2.0652227359358726,
+                    "velocityY": -0.642614240578846,
+                    "timestamp": 9.996498697887498
+                },
+                {
+                    "x": 7.466423547169555,
+                    "y": 7.245941479339108,
+                    "heading": -1.5874361845475978e-17,
+                    "angularVelocity": -5.339369965145435e-17,
+                    "velocityX": -2.402518744049449,
+                    "velocityY": -0.7475672275667952,
+                    "timestamp": 10.068818135881019
+                },
+                {
+                    "x": 7.268281686124032,
+                    "y": 7.184287697178302,
+                    "heading": -2.0112928949513346e-17,
+                    "angularVelocity": -5.860896076646912e-17,
+                    "velocityX": -2.7398147239926662,
+                    "velocityY": -0.8525202057893394,
+                    "timestamp": 10.14113757387454
+                },
+                {
+                    "x": 7.045746772766981,
+                    "y": 7.11504377567321,
+                    "heading": -2.557518924430414e-17,
+                    "angularVelocity": -7.552962863536295e-17,
+                    "velocityX": -3.0771106569853086,
+                    "velocityY": -0.9574731694028816,
+                    "timestamp": 10.21345701186806
+                },
+                {
+                    "x": 6.798818813889274,
+                    "y": 7.03820971693686,
+                    "heading": -2.9820267528270197e-17,
+                    "angularVelocity": -5.869899437443598e-17,
+                    "velocityX": -3.4144064960768983,
+                    "velocityY": -1.062426103798449,
+                    "timestamp": 10.285776449861581
+                },
+                {
+                    "x": 6.527497829863459,
+                    "y": 6.953785527308316,
+                    "heading": -3.380514122458171e-17,
+                    "angularVelocity": -5.510100474849542e-17,
+                    "velocityX": -3.751702053466217,
+                    "velocityY": -1.1673789505403607,
+                    "timestamp": 10.358095887855102
+                },
+                {
+                    "x": 6.238746162971835,
+                    "y": 6.863937591938514,
+                    "heading": -5.503987816242723e-18,
+                    "angularVelocity": 3.9133536146805468e-16,
+                    "velocityX": -3.992725536908818,
+                    "velocityY": -1.2423760176047256,
+                    "timestamp": 10.430415325848623
+                },
+                {
+                    "x": 5.949994496080389,
+                    "y": 6.774089656567438,
+                    "heading": 1.3877748170103676e-17,
+                    "angularVelocity": 2.6800175062346127e-16,
+                    "velocityX": -3.992725536906352,
+                    "velocityY": -1.2423760176223397,
+                    "timestamp": 10.502734763842144
+                },
+                {
+                    "x": 5.661242829177896,
+                    "y": 6.684241721231868,
+                    "heading": -1.5868862095572502e-17,
+                    "angularVelocity": -4.1132247554688176e-16,
+                    "velocityX": -3.9927255370591226,
+                    "velocityY": -1.2423760171313674,
+                    "timestamp": 10.575054201835664
+                },
+                {
+                    "x": 5.372490882873535,
+                    "y": 6.594394683837891,
+                    "heading": -6.721600712521686e-29,
+                    "angularVelocity": 2.1942734257626597e-16,
+                    "velocityX": -3.992729400499917,
+                    "velocityY": -1.242363600807112,
+                    "timestamp": 10.647373639829185
+                },
+                {
+                    "x": 5.061138083926854,
+                    "y": 6.516051319215958,
+                    "heading": 2.3088216109691274e-17,
+                    "angularVelocity": 3.007073899622652e-16,
+                    "velocityX": -4.055146014020917,
+                    "velocityY": -1.020365912387457,
+                    "timestamp": 10.724153316287236
+                },
+                {
+                    "x": 4.749785061492521,
+                    "y": 6.437708842785697,
+                    "heading": 4.4477316523156246e-17,
+                    "angularVelocity": 2.785776314809067e-16,
+                    "velocityX": -4.055148924786648,
+                    "velocityY": -1.0203543443304697,
+                    "timestamp": 10.800932992745286
+                },
+                {
+                    "x": 4.438432039050997,
+                    "y": 6.3593663663840205,
+                    "heading": 5.0772646087421463e-17,
+                    "angularVelocity": 8.199213456954237e-17,
+                    "velocityX": -4.0551489248803225,
+                    "velocityY": -1.0203543439581864,
+                    "timestamp": 10.877712669203337
+                },
+                {
+                    "x": 4.127079016609472,
+                    "y": 6.281023889982345,
+                    "heading": 6.919823627727644e-17,
+                    "angularVelocity": 2.399800447194005e-16,
+                    "velocityX": -4.055148924880325,
+                    "velocityY": -1.0203543439581744,
+                    "timestamp": 10.954492345661388
+                },
+                {
+                    "x": 3.8157259941679467,
+                    "y": 6.202681413580669,
+                    "heading": 1.0145407846874318e-16,
+                    "angularVelocity": 4.2010911844854016e-16,
+                    "velocityX": -4.055148924880325,
+                    "velocityY": -1.0203543439581744,
+                    "timestamp": 11.031272022119438
+                },
+                {
+                    "x": 3.5043729717264216,
+                    "y": 6.124338937178994,
+                    "heading": 1.0488814636055141e-16,
+                    "angularVelocity": 4.4726261561856034e-17,
+                    "velocityX": -4.055148924880325,
+                    "velocityY": -1.0203543439581744,
+                    "timestamp": 11.108051698577489
+                },
+                {
+                    "x": 3.1930199492849143,
+                    "y": 6.0459964607773236,
+                    "heading": 1.0202500821302203e-16,
+                    "angularVelocity": -3.729031274317039e-17,
+                    "velocityX": -4.055148924880093,
+                    "velocityY": -1.020354343958116,
+                    "timestamp": 11.18483137503554
+                },
+                {
+                    "x": 2.885850005558275,
+                    "y": 5.968706528343063,
+                    "heading": 8.840325766190533e-17,
+                    "angularVelocity": -1.7741349246854113e-16,
+                    "velocityX": -4.000667336680725,
+                    "velocityY": -1.0066457166759242,
+                    "timestamp": 11.26161105149359
+                },
+                {
+                    "x": 2.606604572932183,
+                    "y": 5.8984429460459165,
+                    "heading": 7.446990987421016e-17,
+                    "angularVelocity": -1.8147182210670663e-16,
+                    "velocityX": -3.636970686880395,
+                    "velocityY": -0.9151325655238358,
+                    "timestamp": 11.33839072795164
+                },
+                {
+                    "x": 2.35528367155804,
+                    "y": 5.835205718956369,
+                    "heading": 6.231835100914448e-17,
+                    "angularVelocity": -1.5826530438188948e-16,
+                    "velocityX": -3.2732737746225524,
+                    "velocityY": -0.8236193483323324,
+                    "timestamp": 11.415170404409691
+                },
+                {
+                    "x": 2.1318873081529506,
+                    "y": 5.778994848764575,
+                    "heading": 5.446711019193275e-17,
+                    "angularVelocity": -1.0225675828957241e-16,
+                    "velocityX": -2.909576774879259,
+                    "velocityY": -0.732106109127788,
+                    "timestamp": 11.491950080867742
+                },
+                {
+                    "x": 1.936415486075461,
+                    "y": 5.72981033631561,
+                    "heading": 4.33435156910529e-17,
+                    "angularVelocity": -1.44876808732947e-16,
+                    "velocityX": -2.545879731393325,
+                    "velocityY": -0.6405928589167442,
+                    "timestamp": 11.568729757325793
+                },
+                {
+                    "x": 1.7688682073406965,
+                    "y": 5.687652182116517,
+                    "heading": 3.180451468220992e-17,
+                    "angularVelocity": -1.5028717938213945e-16,
+                    "velocityX": -2.182182661661835,
+                    "velocityY": -0.5490796021018084,
+                    "timestamp": 11.645509433783843
+                },
+                {
+                    "x": 1.6292454732920727,
+                    "y": 5.652520386505328,
+                    "heading": 2.1943940462114387e-17,
+                    "angularVelocity": -1.2842687902434274e-16,
+                    "velocityX": -1.8184855744333217,
+                    "velocityY": -0.45756634088428094,
+                    "timestamp": 11.722289110241894
+                },
+                {
+                    "x": 1.5175472848891722,
+                    "y": 5.624414949723491,
+                    "heading": 1.6975654570196923e-17,
+                    "angularVelocity": -6.470834628409354e-17,
+                    "velocityX": -1.4547884747069404,
+                    "velocityY": -0.3660530765220469,
+                    "timestamp": 11.799068786699944
+                },
+                {
+                    "x": 1.4337736428516812,
+                    "y": 5.603335871952093,
+                    "heading": 9.958130236130293e-18,
+                    "angularVelocity": -9.139820142132868e-17,
+                    "velocityX": -1.0910913656071621,
+                    "velocityY": -0.2745398098012838,
+                    "timestamp": 11.875848463157995
+                },
+                {
+                    "x": 1.3779245477393556,
+                    "y": 5.589283153331981,
+                    "heading": 4.935318887472694e-18,
+                    "angularVelocity": -6.541850109660993e-17,
+                    "velocityX": -0.7273942492169659,
+                    "velocityY": -0.1830265412461097,
+                    "timestamp": 11.952628139616046
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 1.1330935234644406e-28,
+                    "angularVelocity": -6.427897478789963e-17,
+                    "velocityX": -0.36369712699443685,
+                    "velocityY": -0.09151327122340726,
+                    "timestamp": 12.029407816074096
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 1.3072856105106544e-29,
+                    "angularVelocity": -1.4338555853349923e-28,
+                    "velocityX": -1.6135165350121333e-27,
+                    "velocityY": 9.374994749220142e-27,
+                    "timestamp": 12.106187492532147
+                },
+                {
+                    "x": 1.3749689535834184,
+                    "y": 5.5897009757312865,
+                    "heading": -2.106297966297422e-19,
+                    "angularVelocity": -2.8839388200086933e-18,
+                    "velocityX": 0.34187439625345595,
+                    "velocityY": 0.10192558269393831,
+                    "timestamp": 12.179222951920298
+                },
+                {
+                    "x": 1.4249068603805064,
+                    "y": 5.604589339131964,
+                    "heading": -1.677218384570217e-18,
+                    "angularVelocity": -2.008050062463534e-17,
+                    "velocityX": 0.6837487874443219,
+                    "velocityY": 0.20385116387852864,
+                    "timestamp": 12.25225841130845
+                },
+                {
+                    "x": 1.4998137199393493,
+                    "y": 5.62692188404313,
+                    "heading": -4.467707102380705e-18,
+                    "angularVelocity": -3.820731383236851e-17,
+                    "velocityX": 1.0256231724475782,
+                    "velocityY": 0.3057767432183604,
+                    "timestamp": 12.325293870696601
+                },
+                {
+                    "x": 1.5996895316950535,
+                    "y": 5.656698610296368,
+                    "heading": -4.7388677205012346e-18,
+                    "angularVelocity": -3.712725576261135e-18,
+                    "velocityX": 1.3674975497163229,
+                    "velocityY": 0.40770232025224407,
+                    "timestamp": 12.398329330084753
+                },
+                {
+                    "x": 1.724534294921327,
+                    "y": 5.693919517675143,
+                    "heading": -4.862675168623981e-18,
+                    "angularVelocity": -1.695169019046009e-18,
+                    "velocityX": 1.7093719170406965,
+                    "velocityY": 0.5096278943213377,
+                    "timestamp": 12.471364789472904
+                },
+                {
+                    "x": 1.8743480086497812,
+                    "y": 5.738584605890742,
+                    "heading": -6.31034613617928e-18,
+                    "angularVelocity": -1.982148095742636e-17,
+                    "velocityX": 2.05124627110591,
+                    "velocityY": 0.6115534644373783,
+                    "timestamp": 12.544400248861056
+                },
+                {
+                    "x": 2.0491306715246718,
+                    "y": 5.7906938745389684,
+                    "heading": -8.052005504268843e-18,
+                    "angularVelocity": -2.3846764059595142e-17,
+                    "velocityX": 2.3931206066083033,
+                    "velocityY": 0.7134790290191458,
+                    "timestamp": 12.617435708249207
+                },
+                {
+                    "x": 2.2488822815123832,
+                    "y": 5.850247323013523,
+                    "heading": -1.0500720583751912e-17,
+                    "angularVelocity": -3.352775624225273e-17,
+                    "velocityX": 2.734994914266474,
+                    "velocityY": 0.815404585299506,
+                    "timestamp": 12.690471167637359
+                },
+                {
+                    "x": 2.4736028352235584,
+                    "y": 5.917244950303913,
+                    "heading": -1.4273207870070583e-17,
+                    "angularVelocity": -5.165281793044341e-17,
+                    "velocityX": 3.07686917551763,
+                    "velocityY": 0.9173301277441942,
+                    "timestamp": 12.76350662702551
+                },
+                {
+                    "x": 2.7232923258794886,
+                    "y": 5.99168675438915,
+                    "heading": -1.7375627711302575e-17,
+                    "angularVelocity": -4.247826832535968e-17,
+                    "velocityX": 3.418743343954855,
+                    "velocityY": 1.0192556425175678,
+                    "timestamp": 12.836542086413662
+                },
+                {
+                    "x": 2.997950733144114,
+                    "y": 6.073572729206291,
+                    "heading": -2.1801449098877115e-17,
+                    "angularVelocity": -6.059825493774254e-17,
+                    "velocityX": 3.760617233951168,
+                    "velocityY": 1.1211810742772599,
+                    "timestamp": 12.909577545801813
+                },
+                {
+                    "x": 3.290621803248295,
+                    "y": 6.1608289545479265,
+                    "heading": -2.5658304246841473e-17,
+                    "angularVelocity": -5.280798094786635e-17,
+                    "velocityX": 4.007246241154717,
+                    "velocityY": 1.1947104334335177,
+                    "timestamp": 12.982613005189965
+                },
+                {
+                    "x": 3.5832928733526748,
+                    "y": 6.248085179889621,
+                    "heading": -2.8530592502284846e-17,
+                    "angularVelocity": -3.93273114105059e-17,
+                    "velocityX": 4.007246241157447,
+                    "velocityY": 1.1947104334343266,
+                    "timestamp": 13.055648464578116
+                },
+                {
+                    "x": 3.875963943459746,
+                    "y": 6.335341405222288,
+                    "heading": -2.8145685781283686e-17,
+                    "angularVelocity": 5.2701348673896014e-18,
+                    "velocityX": 4.007246241194295,
+                    "velocityY": 1.194710433310735,
+                    "timestamp": 13.128683923966268
+                },
+                {
+                    "x": 4.168635084046373,
+                    "y": 6.422597394155118,
+                    "heading": -2.4589541837216757e-17,
+                    "angularVelocity": 4.869064936312243e-17,
+                    "velocityX": 4.007247206198914,
+                    "velocityY": 1.1947071965290432,
+                    "timestamp": 13.201719383354419
+                },
+                {
+                    "x": 4.462992171425512,
+                    "y": 6.503984796246935,
+                    "heading": -1.865851348037744e-17,
+                    "angularVelocity": 8.120751764419801e-17,
+                    "velocityX": 4.030331154826551,
+                    "velocityY": 1.1143546268296594,
+                    "timestamp": 13.27475484274257
+                },
+                {
+                    "x": 4.763214801309505,
+                    "y": 6.5599881562762,
+                    "heading": -1.185788123219857e-17,
+                    "angularVelocity": 9.311411614613915e-17,
+                    "velocityX": 4.110642041538187,
+                    "velocityY": 0.7667968477014759,
+                    "timestamp": 13.347790302130722
+                },
+                {
+                    "x": 5.067118338085079,
+                    "y": 6.590198145999091,
+                    "heading": -5.110846102885528e-18,
+                    "angularVelocity": 9.238026550270056e-17,
+                    "velocityX": 4.161040942598275,
+                    "velocityY": 0.41363455472140287,
+                    "timestamp": 13.420825761518874
+                },
+                {
+                    "x": 5.372490882873535,
+                    "y": 6.594394683837891,
+                    "heading": 7.435571865732989e-29,
+                    "angularVelocity": 6.997759918061287e-17,
+                    "velocityX": 4.181154569940233,
+                    "velocityY": 0.05745890932919486,
+                    "timestamp": 13.493861220907025
+                },
+                {
+                    "x": 5.658666545780349,
+                    "y": 6.575364753181748,
+                    "heading": 4.576818453416559e-19,
+                    "angularVelocity": 6.672831036873803e-18,
+                    "velocityX": 4.172334701892643,
+                    "velocityY": -0.2774493094372273,
+                    "timestamp": 13.562450076632574
+                },
+                {
+                    "x": 5.942400266786845,
+                    "y": 6.533486061480876,
+                    "heading": -2.3827590815129613e-18,
+                    "angularVelocity": -4.141257201051082e-17,
+                    "velocityX": 4.136732097439099,
+                    "velocityY": -0.6105757452558301,
+                    "timestamp": 13.631038932358123
+                },
+                {
+                    "x": 6.221870758893712,
+                    "y": 6.469027595137471,
+                    "heading": -4.3182265395820836e-18,
+                    "angularVelocity": -2.821839550453941e-17,
+                    "velocityX": 4.07457580609218,
+                    "velocityY": -0.9397804593989688,
+                    "timestamp": 13.699627788083673
+                },
+                {
+                    "x": 6.495284408168433,
+                    "y": 6.3824041196974175,
+                    "heading": -2.8846127148492644e-18,
+                    "angularVelocity": 2.0901556230520604e-17,
+                    "velocityX": 3.98626929087078,
+                    "velocityY": -1.2629380461844821,
+                    "timestamp": 13.768216643809222
+                },
+                {
+                    "x": 6.753350615235624,
+                    "y": 6.291966909385253,
+                    "heading": -3.3860339612335026e-18,
+                    "angularVelocity": -7.310535233140868e-18,
+                    "velocityX": 3.762509293052168,
+                    "velocityY": -1.3185408818312967,
+                    "timestamp": 13.836805499534771
+                },
+                {
+                    "x": 6.989730454687167,
+                    "y": 6.209128001792428,
+                    "heading": -3.8610537876315765e-18,
+                    "angularVelocity": -6.925612351618996e-18,
+                    "velocityX": 3.4463301209950634,
+                    "velocityY": -1.207760454909701,
+                    "timestamp": 13.90539435526032
+                },
+                {
+                    "x": 7.204423917011262,
+                    "y": 6.133887431043579,
+                    "heading": -4.30969012277514e-18,
+                    "angularVelocity": -6.540950864334822e-18,
+                    "velocityX": 3.130150810259459,
+                    "velocityY": -1.0969795304636218,
+                    "timestamp": 13.97398321098587
+                },
+                {
+                    "x": 7.397430999037374,
+                    "y": 6.066245208513786,
+                    "heading": -4.7319640256573494e-18,
+                    "angularVelocity": -6.156596409336524e-18,
+                    "velocityX": 2.813971453298626,
+                    "velocityY": -0.9861984401730881,
+                    "timestamp": 14.042572066711418
+                },
+                {
+                    "x": 7.568751699180255,
+                    "y": 6.0062013398906515,
+                    "heading": -4.48443110171988e-18,
+                    "angularVelocity": 3.608937943647087e-18,
+                    "velocityX": 2.4977920732254844,
+                    "velocityY": -0.8754172669594278,
+                    "timestamp": 14.111160922436968
+                },
+                {
+                    "x": 7.718386016488768,
+                    "y": 5.953755828586764,
+                    "heading": -5.202509665797113e-18,
+                    "angularVelocity": -1.0469318324083915e-17,
+                    "velocityX": 2.1816126792850916,
+                    "velocityY": -0.7646360439915042,
+                    "timestamp": 14.179749778162517
+                },
+                {
+                    "x": 7.846333950328824,
+                    "y": 5.9089086768771955,
+                    "heading": -5.894233408781161e-18,
+                    "angularVelocity": -1.008507483708594e-17,
+                    "velocityX": 1.8654332760999357,
+                    "velocityY": -0.6538547878538727,
+                    "timestamp": 14.248338633888066
+                },
+                {
+                    "x": 7.952595500247506,
+                    "y": 5.871659886387006,
+                    "heading": -4.927257324037146e-18,
+                    "angularVelocity": 1.4098151580412308e-17,
+                    "velocityX": 1.549253866311417,
+                    "velocityY": -0.5430735080234801,
+                    "timestamp": 14.316927489613615
+                },
+                {
+                    "x": 8.037170665905128,
+                    "y": 5.842009458334993,
+                    "heading": -3.93393020805799e-18,
+                    "angularVelocity": 1.448233981268987e-17,
+                    "velocityX": 1.2330744515704004,
+                    "velocityY": -0.43229221042344845,
+                    "timestamp": 14.385516345339164
+                },
+                {
+                    "x": 8.100059447037491,
+                    "y": 5.8199573936691165,
+                    "heading": -2.914254186351223e-18,
+                    "angularVelocity": 1.4866497055858427e-17,
+                    "velocityX": 0.9168950329774561,
+                    "velocityY": -0.32151089900254415,
+                    "timestamp": 14.454105201064714
+                },
+                {
+                    "x": 8.141261843433236,
+                    "y": 5.8055036931477435,
+                    "heading": -1.470300622541165e-18,
+                    "angularVelocity": 2.1052305779669128e-17,
+                    "velocityX": 0.6007156113029791,
+                    "velocityY": -0.21072957652491553,
+                    "timestamp": 14.522694056790263
+                },
+                {
+                    "x": 8.160777854919433,
+                    "y": 5.798648357391357,
+                    "heading": 3.515192231280365e-30,
+                    "angularVelocity": 2.1436436094366998e-17,
+                    "velocityX": 0.2845361871072536,
+                    "velocityY": -0.09994824500086058,
+                    "timestamp": 14.591282912515812
+                },
+                {
+                    "x": 8.15740465483607,
+                    "y": 5.7998119860439035,
+                    "heading": 1.5702668702365359e-18,
+                    "angularVelocity": 2.1839117027368502e-17,
+                    "velocityX": -0.046914134644165766,
+                    "velocityY": 0.016183632732190804,
+                    "timestamp": 14.663184485654664
+                },
+                {
+                    "x": 8.130199655390193,
+                    "y": 5.809325678962654,
+                    "heading": 3.16948883770298e-18,
+                    "angularVelocity": 2.2241821668858154e-17,
+                    "velocityX": -0.378364453769859,
+                    "velocityY": 0.1323155044240569,
+                    "timestamp": 14.735086058793517
+                },
+                {
+                    "x": 8.079162856808358,
+                    "y": 5.827189435626364,
+                    "heading": 5.55395968868795e-18,
+                    "angularVelocity": 3.3162985827562857e-17,
+                    "velocityX": -0.7098147697446826,
+                    "velocityY": 0.2484473688664962,
+                    "timestamp": 14.806987631932369
+                },
+                {
+                    "x": 8.00429425936746,
+                    "y": 5.8534032553979545,
+                    "heading": 7.967390201909252e-18,
+                    "angularVelocity": 3.3565753958663505e-17,
+                    "velocityX": -1.0412650818684488,
+                    "velocityY": 0.3645792244485113,
+                    "timestamp": 14.878889205071221
+                },
+                {
+                    "x": 7.90559386341362,
+                    "y": 5.887967137481077,
+                    "heading": 1.0409784598773327e-17,
+                    "angularVelocity": 3.396858080066931e-17,
+                    "velocityX": -1.3727153891784034,
+                    "velocityY": 0.4807110689549696,
+                    "timestamp": 14.950790778210074
+                },
+                {
+                    "x": 7.7830616693918495,
+                    "y": 5.930881080851848,
+                    "heading": 1.2881147946027218e-17,
+                    "angularVelocity": 3.437147811049413e-17,
+                    "velocityX": -1.704165690299189,
+                    "velocityY": 0.59684289922138,
+                    "timestamp": 15.022692351348926
+                },
+                {
+                    "x": 7.636697677895493,
+                    "y": 5.982145084145087,
+                    "heading": 1.5381486333713155e-17,
+                    "angularVelocity": 3.4774460120026703e-17,
+                    "velocityX": -2.0356159831677796,
+                    "velocityY": 0.7129747105009819,
+                    "timestamp": 15.094593924487778
+                },
+                {
+                    "x": 7.466501889755232,
+                    "y": 6.04175914544953,
+                    "heading": 1.6910300359972597e-17,
+                    "angularVelocity": 2.1262594954707737e-17,
+                    "velocityX": -2.367066264483349,
+                    "velocityY": 0.8291064951989165,
+                    "timestamp": 15.16649549762663
+                },
+                {
+                    "x": 7.27247430621708,
+                    "y": 6.109723261898264,
+                    "heading": 2.2846387162938302e-17,
+                    "angularVelocity": 8.255851080616128e-17,
+                    "velocityX": -2.6985165284694865,
+                    "velocityY": 0.9452382400241005,
+                    "timestamp": 15.238397070765483
+                },
+                {
+                    "x": 7.05461492935771,
+                    "y": 6.186037428713062,
+                    "heading": 2.7078383309752413e-17,
+                    "angularVelocity": 5.88581857401056e-17,
+                    "velocityX": -3.029966763573451,
+                    "velocityY": 1.0613699183941856,
+                    "timestamp": 15.310298643904336
+                },
+                {
+                    "x": 6.812923763330427,
+                    "y": 6.270701636337379,
+                    "heading": 3.133942989918459e-17,
+                    "angularVelocity": 5.926221643581749e-17,
+                    "velocityX": -3.361416940913657,
+                    "velocityY": 1.1775014638527852,
+                    "timestamp": 15.382200217043188
+                },
+                {
+                    "x": 6.547400820594944,
+                    "y": 6.3637158561012495,
+                    "heading": 3.5629653523192204e-17,
+                    "angularVelocity": 5.966800776000847e-17,
+                    "velocityX": -3.692866944965416,
+                    "velocityY": 1.293632610572301,
+                    "timestamp": 15.45410179018204
+                },
+                {
+                    "x": 6.261992928035358,
+                    "y": 6.458260863280659,
+                    "heading": 1.2977811491721515e-17,
+                    "angularVelocity": -3.1503958874068347e-16,
+                    "velocityX": -3.9694248693059904,
+                    "velocityY": 1.3149226512308523,
+                    "timestamp": 15.526003363320893
+                },
+                {
+                    "x": 5.9696584968278374,
+                    "y": 6.528524677524475,
+                    "heading": 8.30491658549572e-18,
+                    "angularVelocity": -6.499016227646973e-17,
+                    "velocityX": -4.065758486855054,
+                    "velocityY": 0.9772222105367047,
+                    "timestamp": 15.597904936459745
+                },
+                {
+                    "x": 5.672459000416948,
+                    "y": 6.574009641563384,
+                    "heading": 3.9900100019018806e-18,
+                    "angularVelocity": -6.001129593177065e-17,
+                    "velocityX": -4.133421334703643,
+                    "velocityY": 0.6326004015387804,
+                    "timestamp": 15.669806509598597
+                },
+                {
+                    "x": 5.372490882873535,
+                    "y": 6.594394683837891,
+                    "heading": 4.637949774599644e-29,
+                    "angularVelocity": -5.549266626130552e-17,
+                    "velocityX": -4.171927044824572,
+                    "velocityY": 0.28351316090317685,
+                    "timestamp": 15.74170808273745
+                },
+                {
+                    "x": 5.050773644251368,
+                    "y": 6.587292971608332,
+                    "heading": 4.510908674878247e-19,
+                    "angularVelocity": 5.861667091112072e-18,
+                    "velocityX": -4.180530989625617,
+                    "velocityY": -0.0922826771180165,
+                    "timestamp": 15.818664153490314
+                },
+                {
+                    "x": 4.730994022144736,
+                    "y": 6.551328956326083,
+                    "heading": 3.8554219387702585e-18,
+                    "angularVelocity": 4.423732966002132e-17,
+                    "velocityX": -4.155352774358414,
+                    "velocityY": -0.46733175083411,
+                    "timestamp": 15.895620224243178
+                },
+                {
+                    "x": 4.415736028009693,
+                    "y": 6.486793579258048,
+                    "heading": 4.369824303813874e-18,
+                    "angularVelocity": 6.684363689090542e-18,
+                    "velocityX": -4.096596812322474,
+                    "velocityY": -0.8386002096609608,
+                    "timestamp": 15.972576294996042
+                },
+                {
+                    "x": 4.107092112871089,
+                    "y": 6.39573672005434,
+                    "heading": 4.788552904489025e-18,
+                    "angularVelocity": 5.441137996400068e-18,
+                    "velocityX": -4.010650649378687,
+                    "velocityY": -1.1832316581771465,
+                    "timestamp": 16.049532365748906
+                },
+                {
+                    "x": 3.7984505091445313,
+                    "y": 6.304672026515864,
+                    "heading": 1.0411227800571568e-17,
+                    "angularVelocity": 7.306343529532718e-17,
+                    "velocityX": -4.01062061390491,
+                    "velocityY": -1.183333460863932,
+                    "timestamp": 16.12648843650177
+                },
+                {
+                    "x": 3.489808905492053,
+                    "y": 6.213607332726315,
+                    "heading": 1.1712240643296965e-17,
+                    "angularVelocity": 1.6905915673051655e-17,
+                    "velocityX": -4.0106206129422866,
+                    "velocityY": -1.1833334641265096,
+                    "timestamp": 16.203444507254634
+                },
+                {
+                    "x": 3.181167301839591,
+                    "y": 6.12254263893676,
+                    "heading": 1.739759563187972e-17,
+                    "angularVelocity": 7.387792714615859e-17,
+                    "velocityX": -4.010620612942067,
+                    "velocityY": -1.1833334641265585,
+                    "timestamp": 16.280400578007498
+                },
+                {
+                    "x": 2.8759727989575126,
+                    "y": 6.032495012102541,
+                    "heading": 6.160394566309594e-18,
+                    "angularVelocity": -1.460209825642653e-16,
+                    "velocityX": -3.965827515572878,
+                    "velocityY": -1.1701172623976306,
+                    "timestamp": 16.357356648760362
+                },
+                {
+                    "x": 2.5985232218869587,
+                    "y": 5.950633524607093,
+                    "heading": -5.014948305455115e-18,
+                    "angularVelocity": -1.452171708155591e-16,
+                    "velocityX": -3.6052981182154618,
+                    "velocityY": -1.0637430769865972,
+                    "timestamp": 16.434312719513226
+                },
+                {
+                    "x": 2.3488185906123467,
+                    "y": 5.876958182346815,
+                    "heading": -1.021650025995837e-17,
+                    "angularVelocity": -6.759118421281684e-17,
+                    "velocityX": -3.244768461171998,
+                    "velocityY": -0.9573688149552048,
+                    "timestamp": 16.51126879026609
+                },
+                {
+                    "x": 2.1268589117951118,
+                    "y": 5.8114689872871645,
+                    "heading": -1.3728338188268751e-17,
+                    "angularVelocity": -4.5634319606004684e-17,
+                    "velocityX": -2.884238717567016,
+                    "velocityY": -0.8509945273838397,
+                    "timestamp": 16.588224861018954
+                },
+                {
+                    "x": 1.9326441887659624,
+                    "y": 5.754165940410867,
+                    "heading": -1.4828016154735985e-17,
+                    "angularVelocity": -1.4289684436291385e-17,
+                    "velocityX": -2.5237089306813805,
+                    "velocityY": -0.7446202270425196,
+                    "timestamp": 16.66518093177182
+                },
+                {
+                    "x": 1.7661744235233214,
+                    "y": 5.705049042307557,
+                    "heading": -1.1178654192587566e-17,
+                    "angularVelocity": 4.742136554528476e-17,
+                    "velocityX": -2.163179117827389,
+                    "velocityY": -0.638245919039237,
+                    "timestamp": 16.742137002524682
+                },
+                {
+                    "x": 1.6274496173994693,
+                    "y": 5.664118293370325,
+                    "heading": -6.756440882155054e-18,
+                    "angularVelocity": 5.746412553450026e-17,
+                    "velocityX": -1.802649287661176,
+                    "velocityY": -0.531871605927984,
+                    "timestamp": 16.819093073277546
+                },
+                {
+                    "x": 1.5164697713460344,
+                    "y": 5.631373693879946,
+                    "heading": -7.46157239431928e-18,
+                    "angularVelocity": -9.162779561300321e-18,
+                    "velocityX": -1.4421194451290982,
+                    "velocityY": -0.4254972891681831,
+                    "timestamp": 16.89604914403041
+                },
+                {
+                    "x": 1.4332348860767379,
+                    "y": 5.606815244047006,
+                    "heading": -5.695849774050075e-18,
+                    "angularVelocity": 2.2944552690216655e-17,
+                    "velocityX": -1.0815895933226263,
+                    "velocityY": -0.31912296967197257,
+                    "timestamp": 16.973005214783274
+                },
+                {
+                    "x": 1.3777449621466955,
+                    "y": 5.59044294403529,
+                    "heading": -1.8422092172377636e-18,
+                    "angularVelocity": 5.007584871555877e-17,
+                    "velocityX": -0.7210597343027396,
+                    "velocityY": -0.21274864804744426,
+                    "timestamp": 17.04996128553614
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": -9.960155146068815e-29,
+                    "angularVelocity": 2.3938452146369842e-17,
+                    "velocityX": -0.36052986951212246,
+                    "velocityY": -0.10637432472026213,
+                    "timestamp": 17.126917356289002
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": -4.80920215685678e-29,
+                    "angularVelocity": 4.4408560495863134e-29,
+                    "velocityX": -1.71528985939966e-27,
+                    "velocityY": 5.60383945615098e-27,
+                    "timestamp": 17.203873427041867
+                }
+            ],
+            "trajectoryWaypoints": [
+                {
+                    "timestamp": 0,
+                    "isStopPoint": true,
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "timestamp": 1.2080201889284545,
+                    "isStopPoint": false,
+                    "x": 2.6305306911468507,
+                    "y": 4.355533123016357,
+                    "heading": -0.6,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "timestamp": 2.4162712240084843,
+                    "isStopPoint": true,
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 11
+                },
+                {
+                    "timestamp": 3.398797373367676,
+                    "isStopPoint": false,
+                    "x": 2.7529065132141115,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 11
+                },
+                {
+                    "timestamp": 4.560086286246098,
+                    "isStopPoint": true,
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "timestamp": 5.782485206143044,
+                    "isStopPoint": false,
+                    "x": 2.6305306911468507,
+                    "y": 6.870196914672851,
+                    "heading": 0.6,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "timestamp": 7.005114061127068,
+                    "isStopPoint": true,
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "timestamp": 8.389253972731028,
+                    "isStopPoint": false,
+                    "x": 5.372490882873535,
+                    "y": 6.594394683837891,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "timestamp": 9.490262631932852,
+                    "isStopPoint": false,
+                    "x": 8.173408317565917,
+                    "y": 7.465926647186279,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "timestamp": 10.647373639829185,
+                    "isStopPoint": false,
+                    "x": 5.372490882873535,
+                    "y": 6.594394683837891,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "timestamp": 12.106187492532147,
+                    "isStopPoint": true,
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "timestamp": 13.493861220907025,
+                    "isStopPoint": false,
+                    "x": 5.372490882873535,
+                    "y": 6.594394683837891,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "timestamp": 14.591282912515812,
+                    "isStopPoint": false,
+                    "x": 8.160777854919433,
+                    "y": 5.798648357391357,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "timestamp": 15.74170808273745,
+                    "isStopPoint": false,
+                    "x": 5.372490882873535,
+                    "y": 6.594394683837891,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "timestamp": 17.203873427041867,
+                    "isStopPoint": true,
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        2
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        4
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        6
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        10
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": [],
+            "eventMarkers": [],
+            "isTrajectoryStale": false
+        },
+        "Source 3": {
+            "waypoints": [
+                {
+                    "x": 0.7043527960777283,
+                    "y": 4.401524543762207,
+                    "heading": -1.03622,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "x": 3.952295303344726,
+                    "y": 1.2853265047073366,
+                    "heading": -0.6,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "x": 6.344450855255133,
+                    "y": 0.6604639410972595,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 8.144450855255126,
+                    "y": 0.6604639410972595,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "x": 3.952295303344726,
+                    "y": 1.2853265047073366,
+                    "heading": -0.6,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 1.9563456773757935,
+                    "y": 2.7954604625701904,
+                    "heading": -1.0362799738795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 0.7043527960777283,
+                    "y": 4.401524543762207,
+                    "heading": -1.0362799738795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "x": 3.952295303344726,
+                    "y": 1.5853265047073366,
+                    "heading": -0.6,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 20
+                },
+                {
+                    "x": 6.352611541748047,
+                    "y": 1.5393844842910767,
+                    "heading": 0.305878931297,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 8.2424898147583,
+                    "y": 2.4419312477111816,
+                    "heading": 0.6435004368303741,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 27
+                },
+                {
+                    "x": 3.952295303344726,
+                    "y": 1.5853265047073366,
+                    "heading": -0.6,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 1.8480632305145264,
+                    "y": 2.7954604625701904,
+                    "heading": -1.0362799738795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 0.7043527960777284,
+                    "y": 4.401524543762207,
+                    "heading": -1.0362799738795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 2.6081254482269287,
+                    "y": 2.3851616382598877,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 0.7043527960777283,
+                    "y": 4.401524543762207,
+                    "heading": -1.03622,
+                    "angularVelocity": 7.233895438617747e-28,
+                    "velocityX": 4.805672207333151e-25,
+                    "velocityY": 5.304283343813489e-25,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.7175301442807854,
+                    "y": 4.387776624576329,
+                    "heading": -1.031337197748148,
+                    "angularVelocity": 0.07304296649540387,
+                    "velocityX": 0.1971229948804499,
+                    "velocityY": -0.20565829797731885,
+                    "timestamp": 0.06684835633229075
+                },
+                {
+                    "x": 0.7438862242893792,
+                    "y": 4.360280179260933,
+                    "heading": -1.0216329619423168,
+                    "angularVelocity": 0.1451679044671403,
+                    "velocityX": 0.39426668738993087,
+                    "velocityY": -0.41132567536466397,
+                    "timestamp": 0.1336967126645815
+                },
+                {
+                    "x": 0.783422636003079,
+                    "y": 4.319034521528578,
+                    "heading": -1.0071786936695828,
+                    "angularVelocity": 0.216224737088293,
+                    "velocityX": 0.5914343131665276,
+                    "velocityY": -0.6170033190843506,
+                    "timestamp": 0.20054506899687224
+                },
+                {
+                    "x": 0.8361412515663075,
+                    "y": 4.264038862538382,
+                    "heading": -0.9880588752449545,
+                    "angularVelocity": 0.28601777924930255,
+                    "velocityX": 0.7886299447838971,
+                    "velocityY": -0.8226927632568858,
+                    "timestamp": 0.267393425329163
+                },
+                {
+                    "x": 0.9020442909887891,
+                    "y": 4.19529228059345,
+                    "heading": -0.9643753261297147,
+                    "angularVelocity": 0.3542876805753188,
+                    "velocityX": 0.9858587860394051,
+                    "velocityY": -1.0283959953062354,
+                    "timestamp": 0.33424178166145374
+                },
+                {
+                    "x": 0.9811344267458814,
+                    "y": 4.112793681403445,
+                    "heading": -0.9362533192061244,
+                    "angularVelocity": 0.4206835959256975,
+                    "velocityX": 1.1831276054709545,
+                    "velocityY": -1.234115597097437,
+                    "timestamp": 0.4010901379937445
+                },
+                {
+                    "x": 1.0734149329571012,
+                    "y": 4.016541746348792,
+                    "heading": -0.9038507047307363,
+                    "angularVelocity": 0.48471819283514717,
+                    "velocityX": 1.380445403212468,
+                    "velocityY": -1.439854924423246,
+                    "timestamp": 0.46793849432603524
+                },
+                {
+                    "x": 1.1788899063626475,
+                    "y": 3.9065348671821556,
+                    "heading": -0.867372192915153,
+                    "angularVelocity": 0.5456904824144841,
+                    "velocityX": 1.577824485036696,
+                    "velocityY": -1.6456183098924886,
+                    "timestamp": 0.534786850658326
+                },
+                {
+                    "x": 1.297564608530482,
+                    "y": 3.7827710716251666,
+                    "heading": -0.827093163780206,
+                    "angularVelocity": 0.6025432986673191,
+                    "velocityX": 1.7752822758711535,
+                    "velocityY": -1.8514111991293054,
+                    "timestamp": 0.6016352069906168
+                },
+                {
+                    "x": 1.4294460217667793,
+                    "y": 3.6452479711516896,
+                    "heading": -0.7834028226957228,
+                    "angularVelocity": 0.6535739019117648,
+                    "velocityX": 1.9728445166361182,
+                    "velocityY": -2.057239818880132,
+                    "timestamp": 0.6684835633229076
+                },
+                {
+                    "x": 1.5745437873512116,
+                    "y": 3.49396288510523,
+                    "heading": -0.7368919667230378,
+                    "angularVelocity": 0.695766635479982,
+                    "velocityX": 2.170550983530205,
+                    "velocityY": -2.2631085391905352,
+                    "timestamp": 0.7353319196551984
+                },
+                {
+                    "x": 1.7328717195301693,
+                    "y": 3.3289139649585624,
+                    "heading": -0.6885644312079015,
+                    "angularVelocity": 0.7229427642904057,
+                    "velocityX": 2.3684641009262695,
+                    "velocityY": -2.4690049120466098,
+                    "timestamp": 0.8021802759874892
+                },
+                {
+                    "x": 1.904448192532791,
+                    "y": 3.150108401693767,
+                    "heading": -0.6405130662040369,
+                    "angularVelocity": 0.7188114658348472,
+                    "velocityX": 2.56665208265925,
+                    "velocityY": -2.674793713341072,
+                    "timestamp": 0.86902863231978
+                },
+                {
+                    "x": 2.0892282062966747,
+                    "y": 2.957686751038487,
+                    "heading": -0.6000020926775056,
+                    "angularVelocity": 0.6060130083851152,
+                    "velocityX": 2.7641668980667053,
+                    "velocityY": -2.8784799090465154,
+                    "timestamp": 0.9358769886520708
+                },
+                {
+                    "x": 2.2825884833700547,
+                    "y": 2.7558237851902505,
+                    "heading": -0.6000020755746027,
+                    "angularVelocity": 2.558462724502468e-7,
+                    "velocityX": 2.8925210383965143,
+                    "velocityY": -3.019714723348049,
+                    "timestamp": 1.0027253449843616
+                },
+                {
+                    "x": 2.4759485486145203,
+                    "y": 2.553960616434876,
+                    "heading": -0.6000020584735544,
+                    "angularVelocity": 2.558185308983695e-7,
+                    "velocityX": 2.8925178695988683,
+                    "velocityY": -3.0197177586827917,
+                    "timestamp": 1.0695737013166524
+                },
+                {
+                    "x": 2.6693086138453803,
+                    "y": 2.3520974476664738,
+                    "heading": -0.6000020413725061,
+                    "angularVelocity": 2.5581853093723834e-7,
+                    "velocityX": 2.8925178693953844,
+                    "velocityY": -3.0197177588777038,
+                    "timestamp": 1.1364220576489432
+                },
+                {
+                    "x": 2.862668680951388,
+                    "y": 2.1502342806942307,
+                    "heading": -0.6000020242714578,
+                    "angularVelocity": 2.5581853089079124e-7,
+                    "velocityX": 2.892517897446146,
+                    "velocityY": -3.019717732008527,
+                    "timestamp": 1.203270413981234
+                },
+                {
+                    "x": 3.0560576886556707,
+                    "y": 1.948398839231791,
+                    "heading": -0.6000020071702018,
+                    "angularVelocity": 2.5582163847929273e-7,
+                    "velocityX": 2.8929508265391246,
+                    "velocityY": -3.0193029797045874,
+                    "timestamp": 1.2701187703135248
+                },
+                {
+                    "x": 3.2625807834998075,
+                    "y": 1.760024040188964,
+                    "heading": -0.6000019898927613,
+                    "angularVelocity": 2.584572200422514e-7,
+                    "velocityX": 3.0894266691846277,
+                    "velocityY": -2.8179421212161233,
+                    "timestamp": 1.3369671266458156
+                },
+                {
+                    "x": 3.4814847915941782,
+                    "y": 1.5861911074303183,
+                    "heading": -0.6000019721069275,
+                    "angularVelocity": 2.660623943281706e-7,
+                    "velocityX": 3.274635609681117,
+                    "velocityY": -2.6004069852451583,
+                    "timestamp": 1.4038154829781064
+                },
+                {
+                    "x": 3.7117480772714337,
+                    "y": 1.4277118597724805,
+                    "heading": -0.6000019534349248,
+                    "angularVelocity": 2.793188025212715e-7,
+                    "velocityX": 3.4445616662982457,
+                    "velocityY": -2.3707276641188693,
+                    "timestamp": 1.4706638393103972
+                },
+                {
+                    "x": 3.952295303344726,
+                    "y": 1.2853265047073366,
+                    "heading": -0.6,
+                    "angularVelocity": 0.00002922188416015573,
+                    "velocityX": 3.5984015056043734,
+                    "velocityY": -2.12997540818166,
+                    "timestamp": 1.537512195642688
+                },
+                {
+                    "x": 4.073584345445593,
+                    "y": 1.2180047789733814,
+                    "heading": -0.5929917130855997,
+                    "angularVelocity": 0.2082403637392416,
+                    "velocityX": 3.6039155578478117,
+                    "velocityY": -2.000360548251316,
+                    "timestamp": 1.5711669914207824
+                },
+                {
+                    "x": 4.1949158538302544,
+                    "y": 1.1550645250202634,
+                    "heading": -0.5791131956918127,
+                    "angularVelocity": 0.41237859487533596,
+                    "velocityX": 3.605177377532477,
+                    "velocityY": -1.8701719174919447,
+                    "timestamp": 1.6048217871988768
+                },
+                {
+                    "x": 4.3161484807623705,
+                    "y": 1.0965096037793065,
+                    "heading": -0.5584136500132624,
+                    "angularVelocity": 0.6150548591955444,
+                    "velocityX": 3.60223926870563,
+                    "velocityY": -1.739868565153195,
+                    "timestamp": 1.6384765829769712
+                },
+                {
+                    "x": 4.437139610321772,
+                    "y": 1.0423284939440405,
+                    "heading": -0.5308573563764309,
+                    "angularVelocity": 0.8187924781515704,
+                    "velocityX": 3.5950635492536227,
+                    "velocityY": -1.6099075505468483,
+                    "timestamp": 1.6721313787550656
+                },
+                {
+                    "x": 4.55774200997228,
+                    "y": 0.992492159013808,
+                    "heading": -0.4963159496655149,
+                    "angularVelocity": 1.0263442672084038,
+                    "velocityX": 3.5835130436003593,
+                    "velocityY": -1.4808093104719087,
+                    "timestamp": 1.70578617453316
+                },
+                {
+                    "x": 4.67780133943619,
+                    "y": 0.9469498117340087,
+                    "heading": -0.4545524229930145,
+                    "angularVelocity": 1.2409383479214,
+                    "velocityX": 3.5673765562427637,
+                    "velocityY": -1.3532201348089068,
+                    "timestamp": 1.7394409703112543
+                },
+                {
+                    "x": 4.797163762965293,
+                    "y": 0.9056279106107681,
+                    "heading": -0.40521985466870114,
+                    "angularVelocity": 1.4658406679865637,
+                    "velocityX": 3.5466690784911457,
+                    "velocityY": -1.227816130446904,
+                    "timestamp": 1.7730957660893487
+                },
+                {
+                    "x": 4.915796573719093,
+                    "y": 0.8685122267311245,
+                    "heading": -0.34817511875559193,
+                    "angularVelocity": 1.6949957530343842,
+                    "velocityX": 3.5249897677589033,
+                    "velocityY": -1.102834916140014,
+                    "timestamp": 1.806750561867443
+                },
+                {
+                    "x": 5.036950148745924,
+                    "y": 0.8353847117520539,
+                    "heading": -0.2947632712692716,
+                    "angularVelocity": 1.5870501142985936,
+                    "velocityX": 3.5998903640856996,
+                    "velocityY": -0.9843326697775702,
+                    "timestamp": 1.8404053576455375
+                },
+                {
+                    "x": 5.1614000342462845,
+                    "y": 0.8048309238478079,
+                    "heading": -0.24930473318253552,
+                    "angularVelocity": 1.3507298747694276,
+                    "velocityX": 3.69783511155234,
+                    "velocityY": -0.9078583660321533,
+                    "timestamp": 1.8740601534236319
+                },
+                {
+                    "x": 5.288723665341134,
+                    "y": 0.7775918945406379,
+                    "heading": -0.21076034358050552,
+                    "angularVelocity": 1.1452866882977288,
+                    "velocityX": 3.783223999764152,
+                    "velocityY": -0.8093654612190252,
+                    "timestamp": 1.9077149492017262
+                },
+                {
+                    "x": 5.418289324941767,
+                    "y": 0.75437891499721,
+                    "heading": -0.17727721642858435,
+                    "angularVelocity": 0.994899133326939,
+                    "velocityX": 3.8498423955663323,
+                    "velocityY": -0.6897376438259984,
+                    "timestamp": 1.9413697449798206
+                },
+                {
+                    "x": 5.549779888092408,
+                    "y": 0.7347009595308325,
+                    "heading": -0.14854912765415715,
+                    "angularVelocity": 0.8536105511930082,
+                    "velocityX": 3.907037915714417,
+                    "velocityY": -0.5846998922865582,
+                    "timestamp": 1.975024540757915
+                },
+                {
+                    "x": 5.680534523642805,
+                    "y": 0.7195857320422079,
+                    "heading": -0.11505529703891493,
+                    "angularVelocity": 0.995217170120021,
+                    "velocityX": 3.8851709697643155,
+                    "velocityY": -0.4491255150763042,
+                    "timestamp": 2.008679336536009
+                },
+                {
+                    "x": 5.8094562989699625,
+                    "y": 0.7066518872545531,
+                    "heading": -0.0751084136690073,
+                    "angularVelocity": 1.1869596129271087,
+                    "velocityX": 3.8307103741533206,
+                    "velocityY": -0.3843091151981713,
+                    "timestamp": 2.0423341323141035
+                },
+                {
+                    "x": 5.939054292128191,
+                    "y": 0.6923931933660783,
+                    "heading": -0.03929641038115861,
+                    "angularVelocity": 1.064098071608519,
+                    "velocityX": 3.850803137025198,
+                    "velocityY": -0.4236749491065335,
+                    "timestamp": 2.075988928092198
+                },
+                {
+                    "x": 6.072255728678552,
+                    "y": 0.6785723607822262,
+                    "heading": -0.015832129828706364,
+                    "angularVelocity": 0.6972046631085195,
+                    "velocityX": 3.9578738622762195,
+                    "velocityY": -0.410664580316609,
+                    "timestamp": 2.1096437238702923
+                },
+                {
+                    "x": 6.2080495696602656,
+                    "y": 0.6681080409062233,
+                    "heading": 6.6289175254501184e-21,
+                    "angularVelocity": 0.470427154961713,
+                    "velocityX": 4.0349031346700315,
+                    "velocityY": -0.31093101693441927,
+                    "timestamp": 2.1432985196483867
+                },
+                {
+                    "x": 6.344450855255133,
+                    "y": 0.6604639410972595,
+                    "heading": 6.1943375615915975e-21,
+                    "angularVelocity": -3.3470862706211174e-23,
+                    "velocityX": 4.052952408157215,
+                    "velocityY": -0.2271325566604413,
+                    "timestamp": 2.176953315426481
+                },
+                {
+                    "x": 6.601107058435761,
+                    "y": 0.6504176052648494,
+                    "heading": 5.751674625392007e-21,
+                    "angularVelocity": -2.9986081284790827e-23,
+                    "velocityX": 3.7733050076657766,
+                    "velocityY": -0.14769909643853868,
+                    "timestamp": 2.2449722544359716
+                },
+                {
+                    "x": 6.8381422866926505,
+                    "y": 0.6428253473271538,
+                    "heading": 5.309011689192419e-21,
+                    "angularVelocity": -2.9986081284790274e-23,
+                    "velocityX": 3.484841600128703,
+                    "velocityY": -0.11161976426354472,
+                    "timestamp": 2.312991193445462
+                },
+                {
+                    "x": 7.0554956222161564,
+                    "y": 0.6371380022468355,
+                    "heading": 4.8663487529928285e-21,
+                    "angularVelocity": -2.9986081284790274e-23,
+                    "velocityX": 3.1954825918878096,
+                    "velocityY": -0.08361413987250188,
+                    "timestamp": 2.3810101324549526
+                },
+                {
+                    "x": 7.253146101633396,
+                    "y": 0.6331249451031679,
+                    "heading": 4.423685816793239e-21,
+                    "angularVelocity": -2.9986081284790257e-23,
+                    "velocityX": 2.9058153845896255,
+                    "velocityY": -0.0589991141012556,
+                    "timestamp": 2.449029071464443
+                },
+                {
+                    "x": 7.431083352269109,
+                    "y": 0.6306593023093592,
+                    "heading": 3.9810228805936516e-21,
+                    "angularVelocity": -2.998608128479027e-23,
+                    "velocityX": 2.6159956804219875,
+                    "velocityY": -0.03624935686610544,
+                    "timestamp": 2.5170480104739337
+                },
+                {
+                    "x": 7.589301236096254,
+                    "y": 0.6296608053560406,
+                    "heading": 3.538359944394063e-21,
+                    "angularVelocity": -2.998608128479026e-23,
+                    "velocityX": 2.326085736283958,
+                    "velocityY": -0.014679690213620148,
+                    "timestamp": 2.585066949483424
+                },
+                {
+                    "x": 7.727795711567633,
+                    "y": 0.6300740984956901,
+                    "heading": 3.0956970081944744e-21,
+                    "angularVelocity": -2.998608128479027e-23,
+                    "velocityX": 2.0361163741771477,
+                    "velocityY": 0.006076147991541318,
+                    "timestamp": 2.6530858884929147
+                },
+                {
+                    "x": 7.846563921922145,
+                    "y": 0.6318586995087562,
+                    "heading": 2.6530340719948856e-21,
+                    "angularVelocity": -2.998608128479025e-23,
+                    "velocityX": 1.7461050125751612,
+                    "velocityY": 0.02623682519978192,
+                    "timestamp": 2.7211048275024052
+                },
+                {
+                    "x": 7.94560374326088,
+                    "y": 0.6349837154726694,
+                    "heading": 2.2103711357952968e-21,
+                    "angularVelocity": -2.9986081284790257e-23,
+                    "velocityX": 1.456062425862283,
+                    "velocityY": 0.04594332121935558,
+                    "timestamp": 2.7891237665118958
+                },
+                {
+                    "x": 8.024913535772157,
+                    "y": 0.6394247965173088,
+                    "heading": 1.7677081995957073e-21,
+                    "angularVelocity": -2.998608128479025e-23,
+                    "velocityX": 1.1659957309862647,
+                    "velocityY": 0.06529183061823228,
+                    "timestamp": 2.8571427055213863
+                },
+                {
+                    "x": 8.084491995768397,
+                    "y": 0.6451622562559024,
+                    "heading": 1.3250452633961187e-21,
+                    "angularVelocity": -2.9986081284790257e-23,
+                    "velocityX": 0.875909869572164,
+                    "velocityY": 0.08435091493845756,
+                    "timestamp": 2.925161644530877
+                },
+                {
+                    "x": 8.124338062283094,
+                    "y": 0.6521798491863118,
+                    "heading": 8.823823271965344e-22,
+                    "angularVelocity": -2.99860812847192e-23,
+                    "velocityX": 0.5858084100537775,
+                    "velocityY": 0.1031711613353776,
+                    "timestamp": 2.9931805835403673
+                },
+                {
+                    "x": 8.144450855255126,
+                    "y": 0.6604639410972595,
+                    "heading": 4.407410385367316e-22,
+                    "angularVelocity": -1.496603606409474e-23,
+                    "velocityX": 0.29569401206363705,
+                    "velocityY": 0.12179096045282119,
+                    "timestamp": 3.061199522549858
+                },
+                {
+                    "x": 8.145205258312048,
+                    "y": 0.6698373190850176,
+                    "heading": -0.006376457776647946,
+                    "angularVelocity": -0.09515043658591296,
+                    "velocityX": 0.011257312875227653,
+                    "velocityY": 0.13987091878601224,
+                    "timestamp": 3.1282140104013196
+                },
+                {
+                    "x": 8.126895205856489,
+                    "y": 0.6804219813671147,
+                    "heading": -0.01902497003587162,
+                    "angularVelocity": -0.18874295193092017,
+                    "velocityX": -0.27322528370501487,
+                    "velocityY": 0.15794588038273327,
+                    "timestamp": 3.1952284982527814
+                },
+                {
+                    "x": 8.089517204789008,
+                    "y": 0.6922175506286075,
+                    "heading": -0.03782482149772114,
+                    "angularVelocity": -0.28053413619334927,
+                    "velocityX": -0.5577600048265854,
+                    "velocityY": 0.17601521163062275,
+                    "timestamp": 3.2622429861042432
+                },
+                {
+                    "x": 8.03306724564719,
+                    "y": 0.7052235716462792,
+                    "heading": -0.06263444861696359,
+                    "angularVelocity": -0.3702128885060376,
+                    "velocityX": -0.8423545557333496,
+                    "velocityY": 0.19407774997101565,
+                    "timestamp": 3.329257473955705
+                },
+                {
+                    "x": 7.957540677867716,
+                    "y": 0.7194394630709334,
+                    "heading": -0.09328537538182405,
+                    "angularVelocity": -0.457377617102716,
+                    "velocityX": -1.1270185030269546,
+                    "velocityY": 0.21213161333357952,
+                    "timestamp": 3.396271961807167
+                },
+                {
+                    "x": 7.86293205227133,
+                    "y": 0.7348644513578727,
+                    "heading": -0.12957394218488386,
+                    "angularVelocity": -0.5415033072175939,
+                    "velocityX": -1.4117637637712936,
+                    "velocityY": 0.2301739337489082,
+                    "timestamp": 3.4632864496586286
+                },
+                {
+                    "x": 7.749234920592138,
+                    "y": 0.7514974800960112,
+                    "heading": -0.17124964470192677,
+                    "angularVelocity": -0.6218909351276025,
+                    "velocityX": -1.6966052464834631,
+                    "velocityY": 0.24820049024333068,
+                    "timestamp": 3.5303009375100904
+                },
+                {
+                    "x": 7.616441577963973,
+                    "y": 0.7693370841976203,
+                    "heading": -0.2179978979951545,
+                    "angularVelocity": -0.6975842805341689,
+                    "velocityX": -1.9815617023367158,
+                    "velocityY": 0.2662051844841503,
+                    "timestamp": 3.5973154253615522
+                },
+                {
+                    "x": 7.464542732509,
+                    "y": 0.7883812084602433,
+                    "heading": -0.2694129359586248,
+                    "angularVelocity": -0.767222724695476,
+                    "velocityX": -2.2666568129515383,
+                    "velocityY": 0.2841792106929851,
+                    "timestamp": 3.664329913213014
+                },
+                {
+                    "x": 7.2935271024834005,
+                    "y": 0.8086269223991472,
+                    "heading": -0.32495180452365857,
+                    "angularVelocity": -0.8287591287444611,
+                    "velocityX": -2.5519202713994456,
+                    "velocityY": 0.30210950777955076,
+                    "timestamp": 3.731344401064476
+                },
+                {
+                    "x": 7.10338105268464,
+                    "y": 0.8300699017901093,
+                    "heading": -0.3838485504571199,
+                    "angularVelocity": -0.8788658664975039,
+                    "velocityX": -2.837387196336119,
+                    "velocityY": 0.3199752781591175,
+                    "timestamp": 3.7983588889159376
+                },
+                {
+                    "x": 6.894088991530415,
+                    "y": 0.8527032772677037,
+                    "heading": -0.4449335451483212,
+                    "angularVelocity": -0.911519234864506,
+                    "velocityX": -3.1230867811468057,
+                    "velocityY": 0.33773854286197624,
+                    "timestamp": 3.8653733767673994
+                },
+                {
+                    "x": 6.66563899361204,
+                    "y": 0.876514357446011,
+                    "heading": -0.5061812369696652,
+                    "angularVelocity": -0.9139470252626605,
+                    "velocityX": -3.4089643186519996,
+                    "velocityY": 0.3553124248458716,
+                    "timestamp": 3.9323878646188613
+                },
+                {
+                    "x": 6.4180709439467645,
+                    "y": 0.9014720885867831,
+                    "heading": -0.5632031512454849,
+                    "angularVelocity": -0.8508893539887942,
+                    "velocityX": -3.6942466860898726,
+                    "velocityY": 0.3724229184007376,
+                    "timestamp": 3.999402352470323
+                },
+                {
+                    "x": 6.152323100168746,
+                    "y": 0.9276310220541838,
+                    "heading": -0.5999998003509488,
+                    "angularVelocity": -0.5490849857276159,
+                    "velocityX": -3.9655282357309254,
+                    "velocityY": 0.39034743539907785,
+                    "timestamp": 4.066416840321785
+                },
+                {
+                    "x": 5.872690526203783,
+                    "y": 0.9458335250059967,
+                    "heading": -0.5999998233332822,
+                    "angularVelocity": -3.429457439481182e-7,
+                    "velocityX": -4.172718212586672,
+                    "velocityY": 0.27162041426264033,
+                    "timestamp": 4.133431328173247
+                },
+                {
+                    "x": 5.593039502470092,
+                    "y": 0.9637503473437068,
+                    "heading": -0.5999998459733419,
+                    "angularVelocity": -3.378382864464675e-7,
+                    "velocityX": -4.172993522737036,
+                    "velocityY": 0.2673574463095649,
+                    "timestamp": 4.2004458160247085
+                },
+                {
+                    "x": 5.313388500530538,
+                    "y": 0.9816675098474326,
+                    "heading": -0.599999868613403,
+                    "angularVelocity": -3.3783830667742366e-7,
+                    "velocityX": -4.1729931975217545,
+                    "velocityY": 0.2673625223166588,
+                    "timestamp": 4.26746030387617
+                },
+                {
+                    "x": 5.034117072190086,
+                    "y": 1.004758133272553,
+                    "heading": -0.5999998913039959,
+                    "angularVelocity": -3.385923495322561e-7,
+                    "velocityX": -4.167329144698685,
+                    "velocityY": 0.3445616636853435,
+                    "timestamp": 4.334474791727632
+                },
+                {
+                    "x": 4.75708110726362,
+                    "y": 1.0469100197154555,
+                    "heading": -0.599999914385774,
+                    "angularVelocity": -3.444296746456035e-7,
+                    "velocityX": -4.133971232317993,
+                    "velocityY": 0.6289966213922602,
+                    "timestamp": 4.401489279579094
+                },
+                {
+                    "x": 4.483580467446654,
+                    "y": 1.1079272876104984,
+                    "heading": -0.5999999383317541,
+                    "angularVelocity": -3.5732542015957227e-7,
+                    "velocityX": -4.081216593390693,
+                    "velocityY": 0.9105086056956437,
+                    "timestamp": 4.468503767430556
+                },
+                {
+                    "x": 4.214898341412512,
+                    "y": 1.1875239044147397,
+                    "heading": -0.5999999636887432,
+                    "angularVelocity": -3.783807033968074e-7,
+                    "velocityX": -4.009314025195234,
+                    "velocityY": 1.1877523705123063,
+                    "timestamp": 4.5355182552820175
+                },
+                {
+                    "x": 3.952295303344726,
+                    "y": 1.2853265047073366,
+                    "heading": -0.6,
+                    "angularVelocity": -5.418418901139292e-7,
+                    "velocityX": -3.9186009844595686,
+                    "velocityY": 1.459424721850854,
+                    "timestamp": 4.602532743133479
+                },
+                {
+                    "x": 3.8160925290954975,
+                    "y": 1.3417239464495747,
+                    "heading": -0.6007169196903561,
+                    "angularVelocity": -0.020304857924182462,
+                    "velocityX": -3.8575840742138063,
+                    "velocityY": 1.5973086766438414,
+                    "timestamp": 4.637840534649855
+                },
+                {
+                    "x": 3.6823065921813996,
+                    "y": 1.402858783989713,
+                    "heading": -0.6025143699711063,
+                    "angularVelocity": -0.05090803484313769,
+                    "velocityX": -3.7891335359236398,
+                    "velocityY": 1.7314829082919216,
+                    "timestamp": 4.67314832616623
+                },
+                {
+                    "x": 3.5512210452916233,
+                    "y": 1.4685621311984904,
+                    "heading": -0.605901427445284,
+                    "angularVelocity": -0.09592946283844166,
+                    "velocityX": -3.712652116147769,
+                    "velocityY": 1.860873886102604,
+                    "timestamp": 4.708456117682606
+                },
+                {
+                    "x": 3.4231385033293678,
+                    "y": 1.5386145338053958,
+                    "heading": -0.6115606852705346,
+                    "angularVelocity": -0.16028354032355382,
+                    "velocityX": -3.6275999279890074,
+                    "velocityY": 1.984049400949251,
+                    "timestamp": 4.743763909198981
+                },
+                {
+                    "x": 3.2983746783283663,
+                    "y": 1.6127301792840456,
+                    "heading": -0.6203687484454834,
+                    "angularVelocity": -0.2494651406011415,
+                    "velocityX": -3.5336060297947993,
+                    "velocityY": 2.0991300303864997,
+                    "timestamp": 4.779071700715357
+                },
+                {
+                    "x": 3.1772451571651232,
+                    "y": 1.6905419145686205,
+                    "heading": -0.633385976023098,
+                    "angularVelocity": -0.3686786122427844,
+                    "velocityX": -3.4306739663131984,
+                    "velocityY": 2.20381201833274,
+                    "timestamp": 4.8143794922317324
+                },
+                {
+                    "x": 3.060043504806466,
+                    "y": 1.7715964442241068,
+                    "heading": -0.6517847535007312,
+                    "angularVelocity": -0.5210968085925122,
+                    "velocityX": -3.319427450009183,
+                    "velocityY": 2.295655609552751,
+                    "timestamp": 4.849687283748108
+                },
+                {
+                    "x": 2.947017305338435,
+                    "y": 1.8553668515452788,
+                    "heading": -0.6767173903792927,
+                    "angularVelocity": -0.7061511300415926,
+                    "velocityX": -3.2011687679647043,
+                    "velocityY": 2.372575675890688,
+                    "timestamp": 4.8849950752644835
+                },
+                {
+                    "x": 2.8383532557888453,
+                    "y": 1.9412747030571285,
+                    "heading": -0.7091821281066824,
+                    "angularVelocity": -0.9194780056501994,
+                    "velocityX": -3.077622385393145,
+                    "velocityY": 2.4331131408206836,
+                    "timestamp": 4.920302866780859
+                },
+                {
+                    "x": 2.7341728601513613,
+                    "y": 2.028705258017072,
+                    "heading": -0.7499487938265991,
+                    "angularVelocity": -1.1546082031499747,
+                    "velocityX": -2.9506347229099466,
+                    "velocityY": 2.4762396968214184,
+                    "timestamp": 4.955610658297235
+                },
+                {
+                    "x": 2.6345289236594747,
+                    "y": 2.117005920551524,
+                    "heading": -0.7995562186790833,
+                    "angularVelocity": -1.4049993704499368,
+                    "velocityX": -2.8221514915673502,
+                    "velocityY": 2.500883197225741,
+                    "timestamp": 4.99091844981361
+                },
+                {
+                    "x": 2.5393709341735584,
+                    "y": 2.2053997028238324,
+                    "heading": -0.8584340708283553,
+                    "angularVelocity": -1.667559754394875,
+                    "velocityX": -2.695098883253072,
+                    "velocityY": 2.503520568011431,
+                    "timestamp": 5.026226241329986
+                },
+                {
+                    "x": 2.4468481236667095,
+                    "y": 2.294669095310899,
+                    "heading": -0.9198458442656634,
+                    "angularVelocity": -1.7393263866086284,
+                    "velocityX": -2.6204643942099364,
+                    "velocityY": 2.528319916176743,
+                    "timestamp": 5.061534032846361
+                },
+                {
+                    "x": 2.3517474716897926,
+                    "y": 2.3869836058958684,
+                    "heading": -0.9675055780358813,
+                    "angularVelocity": -1.3498361614635055,
+                    "velocityX": -2.6934749496526904,
+                    "velocityY": 2.6145648487290294,
+                    "timestamp": 5.096841824362737
+                },
+                {
+                    "x": 2.2546678426174425,
+                    "y": 2.482949029345561,
+                    "heading": -1.00231711789936,
+                    "angularVelocity": -0.9859449817849257,
+                    "velocityX": -2.7495242523828574,
+                    "velocityY": 2.7179673190600213,
+                    "timestamp": 5.132149615879112
+                },
+                {
+                    "x": 2.156133466715771,
+                    "y": 2.5829110015292103,
+                    "heading": -1.0250587051096054,
+                    "angularVelocity": -0.6440954314488436,
+                    "velocityX": -2.790726116527927,
+                    "velocityY": 2.831159013083336,
+                    "timestamp": 5.167457407395488
+                },
+                {
+                    "x": 2.0565769235323215,
+                    "y": 2.6870555947722727,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": -0.31781281943892153,
+                    "velocityX": -2.8196763067798187,
+                    "velocityY": 2.949620714588201,
+                    "timestamp": 5.202765198911863
+                },
+                {
+                    "x": 1.9563456773757937,
+                    "y": 2.7954604625701904,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 4.542642020492645e-25,
+                    "velocityX": -2.8387854876180527,
+                    "velocityY": 3.0702817463856076,
+                    "timestamp": 5.238072990428239
+                },
+                {
+                    "x": 1.7730322630777298,
+                    "y": 3.0210268959304614,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 2.002705937424186e-25,
+                    "velocityX": -2.533007078205092,
+                    "velocityY": 3.116855220305981,
+                    "timestamp": 5.310442868852725
+                },
+                {
+                    "x": 1.606193927798652,
+                    "y": 3.2314395516520142,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 2.00271753679935e-25,
+                    "velocityX": -2.3053560253408945,
+                    "velocityY": 2.9074617824749667,
+                    "timestamp": 5.382812747277211
+                },
+                {
+                    "x": 1.4543761946227032,
+                    "y": 3.425255597367628,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 2.002717536795154e-25,
+                    "velocityX": -2.0978027942158572,
+                    "velocityY": 2.6781314261547466,
+                    "timestamp": 5.455182625701697
+                },
+                {
+                    "x": 1.317018366640586,
+                    "y": 3.601984297874042,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 2.0027175368278648e-25,
+                    "velocityX": -1.8979972189043135,
+                    "velocityY": 2.44202013812722,
+                    "timestamp": 5.5275525041261835
+                },
+                {
+                    "x": 1.1938248299897818,
+                    "y": 3.761379815843054,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 2.0027175368332326e-25,
+                    "velocityX": -1.7022764074330956,
+                    "velocityY": 2.2025118936096018,
+                    "timestamp": 5.59992238255067
+                },
+                {
+                    "x": 1.0846132178875603,
+                    "y": 3.9032947091678634,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 2.0027175368296866e-25,
+                    "velocityX": -1.5090755225764045,
+                    "velocityY": 1.960966308281009,
+                    "timestamp": 5.672292260975156
+                },
+                {
+                    "x": 0.9892598461227126,
+                    "y": 4.027630760526036,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 2.0027175368205547e-25,
+                    "velocityX": -1.3175836942208314,
+                    "velocityY": 1.7180635654640377,
+                    "timestamp": 5.744662139399642
+                },
+                {
+                    "x": 0.9076753302290267,
+                    "y": 4.134317869708134,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 2.0027175368262682e-25,
+                    "velocityX": -1.1273269717982857,
+                    "velocityY": 1.4741921847142516,
+                    "timestamp": 5.817032017824128
+                },
+                {
+                    "x": 0.8397920613678915,
+                    "y": 4.223303498353317,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 2.0027175368232324e-25,
+                    "velocityX": -0.93800446178679,
+                    "velocityY": 1.2295948339616842,
+                    "timestamp": 5.889401896248614
+                },
+                {
+                    "x": 0.7855571157672845,
+                    "y": 4.2945468080907165,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 2.0027175368176088e-25,
+                    "velocityX": -0.7494132473526036,
+                    "velocityY": 0.9844331825393212,
+                    "timestamp": 5.9617717746731005
+                },
+                {
+                    "x": 0.7449279402703854,
+                    "y": 4.348015145319637,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 2.0027175368190736e-25,
+                    "velocityX": -0.5614100283351063,
+                    "velocityY": 0.7388203268119757,
+                    "timestamp": 6.034141653097587
+                },
+                {
+                    "x": 0.7178695768260748,
+                    "y": 4.383681805542776,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 2.0027175368211496e-25,
+                    "velocityX": -0.37388985629628485,
+                    "velocityY": 0.49283847091654137,
+                    "timestamp": 6.106511531522073
+                },
+                {
+                    "x": 0.7043527960777283,
+                    "y": 4.401524543762207,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 2.0027175368141127e-25,
+                    "velocityX": -0.18677357268811703,
+                    "velocityY": 0.24654923578529575,
+                    "timestamp": 6.178881409946559
+                },
+                {
+                    "x": 0.7043527960777283,
+                    "y": 4.401524543762207,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 6.759494810993815e-26,
+                    "velocityX": -5.770106844992042e-25,
+                    "velocityY": -3.7765094249209375e-25,
+                    "timestamp": 6.251251288371045
+                },
+                {
+                    "x": 0.7167356642093481,
+                    "y": 4.3880630372989495,
+                    "heading": -1.0323294869077149,
+                    "angularVelocity": 0.0603254224142757,
+                    "velocityX": 0.18909105537091309,
+                    "velocityY": -0.20556226852806772,
+                    "timestamp": 6.3167375595610284
+                },
+                {
+                    "x": 0.7415026145057688,
+                    "y": 4.361138808128861,
+                    "heading": -1.0245186649242746,
+                    "angularVelocity": 0.11927419047543625,
+                    "velocityX": 0.3782006494852768,
+                    "velocityY": -0.41114310955310973,
+                    "timestamp": 6.382223830751012
+                },
+                {
+                    "x": 0.7786550164722235,
+                    "y": 4.320750485584969,
+                    "heading": -1.012951720288187,
+                    "angularVelocity": 0.17663159660641478,
+                    "velocityX": 0.5673311564598186,
+                    "velocityY": -0.6167448811785445,
+                    "timestamp": 6.447710101940995
+                },
+                {
+                    "x": 0.8281944252622419,
+                    "y": 4.266896512217522,
+                    "heading": -0.9977506488970705,
+                    "angularVelocity": 0.2321260794803288,
+                    "velocityX": 0.7564854112138795,
+                    "velocityY": -0.8223704356476602,
+                    "timestamp": 6.513196373130978
+                },
+                {
+                    "x": 0.8901226189866902,
+                    "y": 4.199575103915774,
+                    "heading": -0.9790603164668716,
+                    "angularVelocity": 0.2854084083666306,
+                    "velocityX": 0.945666818389875,
+                    "velocityY": -1.0280232341591105,
+                    "timestamp": 6.578682644320962
+                },
+                {
+                    "x": 0.9644416446349294,
+                    "y": 4.118784201180533,
+                    "heading": -0.9570555981446492,
+                    "angularVelocity": 0.336020327961321,
+                    "velocityX": 1.1348794838635836,
+                    "velocityY": -1.233707481998122,
+                    "timestamp": 6.644168915510945
+                },
+                {
+                    "x": 1.0511538736294015,
+                    "y": 4.02452141118641,
+                    "heading": -0.9319517389785453,
+                    "angularVelocity": 0.3833453746858561,
+                    "velocityX": 1.324128361850215,
+                    "velocityY": -1.4394282691811102,
+                    "timestamp": 6.709655186700928
+                },
+                {
+                    "x": 1.150262065510173,
+                    "y": 3.916783943927492,
+                    "heading": -0.9040200178440649,
+                    "angularVelocity": 0.42652789091391524,
+                    "velocityX": 1.5134193790519348,
+                    "velocityY": -1.6451916608041222,
+                    "timestamp": 6.7751414578909115
+                },
+                {
+                    "x": 1.26176942881864,
+                    "y": 3.7955685577539087,
+                    "heading": -0.8736126776022465,
+                    "angularVelocity": 0.4643315261240514,
+                    "velocityX": 1.7027593918879715,
+                    "velocityY": -1.8510045536403001,
+                    "timestamp": 6.840627729080895
+                },
+                {
+                    "x": 1.3856796349802256,
+                    "y": 3.66087156928865,
+                    "heading": -0.8412052975881391,
+                    "angularVelocity": 0.4948728859532958,
+                    "velocityX": 1.8921554687715691,
+                    "velocityY": -2.0568736930292832,
+                    "timestamp": 6.906114000270878
+                },
+                {
+                    "x": 1.521996617397842,
+                    "y": 3.5126891243586433,
+                    "heading": -0.8074753577540704,
+                    "angularVelocity": 0.5150688720115713,
+                    "velocityX": 2.0816116101975704,
+                    "velocityY": -2.2628016870912,
+                    "timestamp": 6.971600271460861
+                },
+                {
+                    "x": 1.6707234642510467,
+                    "y": 3.3510185124370526,
+                    "heading": -0.7734667417988786,
+                    "angularVelocity": 0.5193243612318167,
+                    "velocityX": 2.2711149092873253,
+                    "velocityY": -2.468771071917722,
+                    "timestamp": 7.037086542650845
+                },
+                {
+                    "x": 1.8318569074079185,
+                    "y": 3.1758644206376023,
+                    "heading": -0.741004452502463,
+                    "angularVelocity": 0.4957113713535259,
+                    "velocityX": 2.460568302773024,
+                    "velocityY": -2.6746688827541845,
+                    "timestamp": 7.102572813840828
+                },
+                {
+                    "x": 2.005350740806108,
+                    "y": 2.9872805245573906,
+                    "heading": -0.7141541604245535,
+                    "angularVelocity": 0.4100140623370287,
+                    "velocityX": 2.6493161123018125,
+                    "velocityY": -2.879747047027718,
+                    "timestamp": 7.168059085030811
+                },
+                {
+                    "x": 2.190424030896748,
+                    "y": 2.7863157366197826,
+                    "heading": -0.7119881829198739,
+                    "angularVelocity": 0.03307529143621359,
+                    "velocityX": 2.8261387727165057,
+                    "velocityY": -3.0688079239476664,
+                    "timestamp": 7.2335453562207945
+                },
+                {
+                    "x": 2.3763825220142154,
+                    "y": 2.5853069329321596,
+                    "heading": -0.7119881669601573,
+                    "angularVelocity": 2.4371087893295916e-7,
+                    "velocityX": 2.8396561254492445,
+                    "velocityY": -3.0694800610111423,
+                    "timestamp": 7.299031627410778
+                },
+                {
+                    "x": 2.5753608002161195,
+                    "y": 2.397177338599563,
+                    "heading": -0.7119881508365082,
+                    "angularVelocity": 2.4621418712778765e-7,
+                    "velocityX": 3.0384731728677012,
+                    "velocityY": -2.872809688412548,
+                    "timestamp": 7.364517898600761
+                },
+                {
+                    "x": 2.7864781410109636,
+                    "y": 2.222779610597699,
+                    "heading": -0.71198813425094,
+                    "angularVelocity": 2.5326786554372286e-7,
+                    "velocityX": 3.2238412259321843,
+                    "velocityY": -2.663118922986393,
+                    "timestamp": 7.430004169790744
+                },
+                {
+                    "x": 3.0087889994109913,
+                    "y": 2.0628953375517414,
+                    "heading": -0.7119881168686832,
+                    "angularVelocity": 2.654336029118244e-7,
+                    "velocityX": 3.3947704512763814,
+                    "velocityY": -2.4414930051844577,
+                    "timestamp": 7.495490440980728
+                },
+                {
+                    "x": 3.2412974577495883,
+                    "y": 1.918240900042732,
+                    "heading": -0.7119880410502188,
+                    "angularVelocity": 0.000001157776478274293,
+                    "velocityX": 3.55049163914161,
+                    "velocityY": -2.2089276863748983,
+                    "timestamp": 7.560976712170711
+                },
+                {
+                    "x": 3.479287995502474,
+                    "y": 1.7906960053033385,
+                    "heading": -0.6997868981239171,
+                    "angularVelocity": 0.18631604311237665,
+                    "velocityX": 3.6342050544066913,
+                    "velocityY": -1.9476585308907035,
+                    "timestamp": 7.626462983360694
+                },
+                {
+                    "x": 3.7169780040060765,
+                    "y": 1.6799303664576555,
+                    "heading": -0.6633844580774498,
+                    "angularVelocity": 0.5558789557716548,
+                    "velocityX": 3.6296158596972896,
+                    "velocityY": -1.6914329802706074,
+                    "timestamp": 7.691949254550678
+                },
+                {
+                    "x": 3.952295303344726,
+                    "y": 1.5853265047073366,
+                    "heading": -0.6,
+                    "angularVelocity": 0.9679045229734358,
+                    "velocityX": 3.5933836980268095,
+                    "velocityY": -1.444636563225013,
+                    "timestamp": 7.757435525740661
+                },
+                {
+                    "x": 4.074112481863887,
+                    "y": 1.5402828586317625,
+                    "heading": -0.5592952549143817,
+                    "angularVelocity": 1.1918777883706093,
+                    "velocityX": 3.5669352311031557,
+                    "velocityY": -1.3189253771711302,
+                    "timestamp": 7.791587303282606
+                },
+                {
+                    "x": 4.195092381666415,
+                    "y": 1.49959651079917,
+                    "heading": -0.5111868480910893,
+                    "angularVelocity": 1.408664798317049,
+                    "velocityX": 3.5424188288279144,
+                    "velocityY": -1.1913390974341513,
+                    "timestamp": 7.825739080824551
+                },
+                {
+                    "x": 4.31528397568399,
+                    "y": 1.4633173395591519,
+                    "heading": -0.45584958746005705,
+                    "angularVelocity": 1.6203332480444814,
+                    "velocityX": 3.5193364055489167,
+                    "velocityY": -1.0622923271112426,
+                    "timestamp": 7.859890858366496
+                },
+                {
+                    "x": 4.434745610407602,
+                    "y": 1.4314952831678345,
+                    "heading": -0.39345071655264674,
+                    "angularVelocity": 1.8271046311066104,
+                    "velocityX": 3.4979624289509004,
+                    "velocityY": -0.931783312076039,
+                    "timestamp": 7.894042635908441
+                },
+                {
+                    "x": 4.553564809111248,
+                    "y": 1.4041887809787912,
+                    "heading": -0.32420952229913425,
+                    "angularVelocity": 2.0274550619940936,
+                    "velocityX": 3.479151225956343,
+                    "velocityY": -0.799563131245678,
+                    "timestamp": 7.928194413450386
+                },
+                {
+                    "x": 4.671875409339677,
+                    "y": 1.3814688601488554,
+                    "heading": -0.2484642063556595,
+                    "angularVelocity": 2.2179025923451543,
+                    "velocityX": 3.4642589271706443,
+                    "velocityY": -0.6652632004888026,
+                    "timestamp": 7.962346190992331
+                },
+                {
+                    "x": 4.789867622790203,
+                    "y": 1.3634163593108408,
+                    "heading": -0.1667309654451272,
+                    "angularVelocity": 2.3932353392191157,
+                    "velocityX": 3.4549362271292985,
+                    "velocityY": -0.5285962294595843,
+                    "timestamp": 7.9964979685342765
+                },
+                {
+                    "x": 4.9072638759133165,
+                    "y": 1.3502270995004428,
+                    "heading": -0.08189101561832554,
+                    "angularVelocity": 2.484203046901497,
+                    "velocityX": 3.4374858813403817,
+                    "velocityY": -0.38619541235295707,
+                    "timestamp": 8.030649746076222
+                },
+                {
+                    "x": 5.023862752348719,
+                    "y": 1.3419409683639283,
+                    "heading": 0.004341393754814386,
+                    "angularVelocity": 2.5249757283418135,
+                    "velocityX": 3.414137852479163,
+                    "velocityY": -0.2426266429715844,
+                    "timestamp": 8.064801523618167
+                },
+                {
+                    "x": 5.139798735192581,
+                    "y": 1.3385933374859216,
+                    "heading": 0.09105898183003379,
+                    "angularVelocity": 2.5391822715146737,
+                    "velocityX": 3.394727630252062,
+                    "velocityY": -0.09802215635467636,
+                    "timestamp": 8.098953301160112
+                },
+                {
+                    "x": 5.255334333875942,
+                    "y": 1.3402095329646513,
+                    "heading": 0.17703367140947002,
+                    "angularVelocity": 2.5174294214654838,
+                    "velocityX": 3.3830039605247926,
+                    "velocityY": 0.04732390508062842,
+                    "timestamp": 8.133105078702057
+                },
+                {
+                    "x": 5.370884716650611,
+                    "y": 1.3467609864460404,
+                    "heading": 0.26064984374400246,
+                    "angularVelocity": 2.4483695535857777,
+                    "velocityX": 3.383436854282362,
+                    "velocityY": 0.19183345503298238,
+                    "timestamp": 8.167256856244002
+                },
+                {
+                    "x": 5.486974245992485,
+                    "y": 1.3581084639316627,
+                    "heading": 0.3400681433878616,
+                    "angularVelocity": 2.3254514218569815,
+                    "velocityX": 3.3992236333612103,
+                    "velocityY": 0.33226608693165577,
+                    "timestamp": 8.201408633785947
+                },
+                {
+                    "x": 5.604095520081233,
+                    "y": 1.3740075206335363,
+                    "heading": 0.4136873978805543,
+                    "angularVelocity": 2.1556492748371334,
+                    "velocityX": 3.429434205727671,
+                    "velocityY": 0.46554111809689713,
+                    "timestamp": 8.235560411327892
+                },
+                {
+                    "x": 5.722741961380415,
+                    "y": 1.394099394895898,
+                    "heading": 0.4800808054598774,
+                    "angularVelocity": 1.94406887014239,
+                    "velocityX": 3.474092707281825,
+                    "velocityY": 0.5883112302920394,
+                    "timestamp": 8.269712188869837
+                },
+                {
+                    "x": 5.843764195041286,
+                    "y": 1.4176581219817168,
+                    "heading": 0.5371133918016127,
+                    "angularVelocity": 1.6699741696223076,
+                    "velocityX": 3.5436584087675294,
+                    "velocityY": 0.6898243307214182,
+                    "timestamp": 8.303863966411782
+                },
+                {
+                    "x": 5.967478132998898,
+                    "y": 1.4443969152926162,
+                    "heading": 0.5841910806451285,
+                    "angularVelocity": 1.3784842907721413,
+                    "velocityX": 3.6224743442904956,
+                    "velocityY": 0.7829400176333111,
+                    "timestamp": 8.338015743953727
+                },
+                {
+                    "x": 6.094050696019468,
+                    "y": 1.474151537150211,
+                    "heading": 0.6210751522017017,
+                    "angularVelocity": 1.080004445193888,
+                    "velocityX": 3.7061778955755575,
+                    "velocityY": 0.871246652419496,
+                    "timestamp": 8.372167521495673
+                },
+                {
+                    "x": 6.223075829158687,
+                    "y": 1.5062918435411075,
+                    "heading": 0.6435004368303741,
+                    "angularVelocity": 0.6566359423350606,
+                    "velocityX": 3.7779917306134942,
+                    "velocityY": 0.9411020071040859,
+                    "timestamp": 8.406319299037618
+                },
+                {
+                    "x": 6.352611541748047,
+                    "y": 1.5393844842910769,
+                    "heading": 0.6435004368303741,
+                    "angularVelocity": -1.4718933529794898e-27,
+                    "velocityX": 3.7929420344304243,
+                    "velocityY": 0.9689873597157704,
+                    "timestamp": 8.440471076579563
+                },
+                {
+                    "x": 6.580062653914693,
+                    "y": 1.6130439412495936,
+                    "heading": 0.6435004368303741,
+                    "angularVelocity": 1.0474807729513566e-29,
+                    "velocityX": 3.7848236766813983,
+                    "velocityY": 1.22570540127247,
+                    "timestamp": 8.500566640229724
+                },
+                {
+                    "x": 6.79440770982544,
+                    "y": 1.694856967245299,
+                    "heading": 0.6435004368303741,
+                    "angularVelocity": -1.650371396897613e-30,
+                    "velocityX": 3.566736758781869,
+                    "velocityY": 1.3613821225135523,
+                    "timestamp": 8.560662203879886
+                },
+                {
+                    "x": 6.993347493866424,
+                    "y": 1.7776330405952192,
+                    "heading": 0.6435004368303741,
+                    "angularVelocity": 1.3963817165306325e-30,
+                    "velocityX": 3.310390517328132,
+                    "velocityY": 1.3774073878695936,
+                    "timestamp": 8.620757767530048
+                },
+                {
+                    "x": 7.176977880245271,
+                    "y": 1.8584412636517398,
+                    "heading": 0.6435004368303741,
+                    "angularVelocity": -1.580328410175052e-30,
+                    "velocityX": 3.0556396383571194,
+                    "velocityY": 1.3446620373998812,
+                    "timestamp": 8.68085333118021
+                },
+                {
+                    "x": 7.345549173736612,
+                    "y": 1.9358621673435095,
+                    "heading": 0.6435004368303741,
+                    "angularVelocity": 2.4054088974712413e-30,
+                    "velocityX": 2.805053871740944,
+                    "velocityY": 1.2882964896121982,
+                    "timestamp": 8.740948894830371
+                },
+                {
+                    "x": 7.49926861078026,
+                    "y": 2.009079186830037,
+                    "heading": 0.6435004368303741,
+                    "angularVelocity": 2.3808121740418405e-30,
+                    "velocityX": 2.55791655335002,
+                    "velocityY": 1.2183431694351197,
+                    "timestamp": 8.801044458480533
+                },
+                {
+                    "x": 7.6382952437083365,
+                    "y": 2.0775663187372784,
+                    "heading": 0.6435004368303741,
+                    "angularVelocity": 2.380805137763221e-30,
+                    "velocityX": 2.3134258917580244,
+                    "velocityY": 1.139637067154071,
+                    "timestamp": 8.861140022130694
+                },
+                {
+                    "x": 7.762751887009844,
+                    "y": 2.1409577892840943,
+                    "heading": 0.6435004368303741,
+                    "angularVelocity": 2.3808050924755408e-30,
+                    "velocityX": 2.070978883333475,
+                    "velocityY": 1.0548444293798613,
+                    "timestamp": 8.921235585780856
+                },
+                {
+                    "x": 7.872735273758024,
+                    "y": 2.198985004826962,
+                    "heading": 0.6435004368303741,
+                    "angularVelocity": 2.380805092475582e-30,
+                    "velocityX": 1.8301415290558403,
+                    "velocityY": 0.9655823494836527,
+                    "timestamp": 8.981331149431018
+                },
+                {
+                    "x": 7.968323195692331,
+                    "y": 2.2514425705825416,
+                    "heading": 0.6435004368303741,
+                    "angularVelocity": 2.380807288621953e-30,
+                    "velocityX": 1.5905986420355245,
+                    "velocityY": 0.872902466826909,
+                    "timestamp": 9.04142671308118
+                },
+                {
+                    "x": 8.049579405713123,
+                    "y": 2.2981684302536656,
+                    "heading": 0.6435004368303741,
+                    "angularVelocity": 2.380808241833061e-30,
+                    "velocityX": 1.3521166136957175,
+                    "velocityY": 0.7775259409019353,
+                    "timestamp": 9.101522276731341
+                },
+                {
+                    "x": 8.116557022491735,
+                    "y": 2.339031508309391,
+                    "heading": 0.6435004368303741,
+                    "angularVelocity": 2.6392759987141426e-30,
+                    "velocityX": 1.1145184887275905,
+                    "velocityY": 0.6799682967216062,
+                    "timestamp": 9.161617840381503
+                },
+                {
+                    "x": 8.169300943138342,
+                    "y": 2.3739236285016685,
+                    "heading": 0.6435004368303741,
+                    "angularVelocity": 2.3804976901317442e-30,
+                    "velocityX": 0.8776674590099443,
+                    "velocityY": 0.5806105820954999,
+                    "timestamp": 9.221713404031664
+                },
+                {
+                    "x": 8.207849590345619,
+                    "y": 2.4027540109969827,
+                    "heading": 0.6435004368303741,
+                    "angularVelocity": 2.3808070805983946e-30,
+                    "velocityX": 0.6414557891774174,
+                    "velocityY": 0.47974227620438903,
+                    "timestamp": 9.281808967681826
+                },
+                {
+                    "x": 8.232236204057223,
+                    "y": 2.4254453979327417,
+                    "heading": 0.6435004368303741,
+                    "angularVelocity": 2.3808068323507875e-30,
+                    "velocityX": 0.4057972374394851,
+                    "velocityY": 0.3775883868542062,
+                    "timestamp": 9.341904531331988
+                },
+                {
+                    "x": 8.2424898147583,
+                    "y": 2.4419312477111816,
+                    "heading": 0.6435004368303741,
+                    "angularVelocity": -3.2178293818866526e-30,
+                    "velocityX": 0.1706217577185593,
+                    "velocityY": 0.27432723444296603,
+                    "timestamp": 9.40200009498215
+                },
+                {
+                    "x": 8.2393017204798,
+                    "y": 2.4522166219660964,
+                    "heading": 0.6319049962965602,
+                    "angularVelocity": -0.1961915039244878,
+                    "velocityX": -0.05394163415594769,
+                    "velocityY": 0.1740255609619532,
+                    "timestamp": 9.461102758970478
+                },
+                {
+                    "x": 8.222821560618591,
+                    "y": 2.4565750369075374,
+                    "heading": 0.608972533665731,
+                    "angularVelocity": -0.3880106425550933,
+                    "velocityX": -0.27883954375497194,
+                    "velocityY": 0.0737431216687946,
+                    "timestamp": 9.520205422958806
+                },
+                {
+                    "x": 8.19302770545567,
+                    "y": 2.4550084997233315,
+                    "heading": 0.5749872344457952,
+                    "angularVelocity": -0.5750214444927056,
+                    "velocityX": -0.5041034219507514,
+                    "velocityY": -0.02650535658621403,
+                    "timestamp": 9.579308086947135
+                },
+                {
+                    "x": 8.149895912141313,
+                    "y": 2.447519382114308,
+                    "heading": 0.5302786937880936,
+                    "angularVelocity": -0.7564555916892447,
+                    "velocityX": -0.7297774821600979,
+                    "velocityY": -0.12671370634836995,
+                    "timestamp": 9.638410750935464
+                },
+                {
+                    "x": 8.09339864804559,
+                    "y": 2.4341097073401863,
+                    "heading": 0.4752444136142709,
+                    "angularVelocity": -0.9311641212093363,
+                    "velocityX": -0.9559173865137355,
+                    "velocityY": -0.22688782314059242,
+                    "timestamp": 9.697513414923792
+                },
+                {
+                    "x": 8.023504638887696,
+                    "y": 2.4147802557579743,
+                    "heading": 0.41037330333687494,
+                    "angularVelocity": -1.0976004447144125,
+                    "velocityX": -1.1825864426634942,
+                    "velocityY": -0.32704873651768396,
+                    "timestamp": 9.75661607891212
+                },
+                {
+                    "x": 7.940178760571984,
+                    "y": 2.3895296850369747,
+                    "heading": 0.33626751416330763,
+                    "angularVelocity": -1.2538485437509457,
+                    "velocityX": -1.4098497883643064,
+                    "velocityY": -0.4272323617423738,
+                    "timestamp": 9.815718742900449
+                },
+                {
+                    "x": 7.843382276245969,
+                    "y": 2.358354061171314,
+                    "heading": 0.2536606944782709,
+                    "angularVelocity": -1.3976835240683847,
+                    "velocityX": -1.6377685504181283,
+                    "velocityY": -0.5274825492099173,
+                    "timestamp": 9.874821406888778
+                },
+                {
+                    "x": 7.733073216109231,
+                    "y": 2.321247319783134,
+                    "heading": 0.16343500685588275,
+                    "angularVelocity": -1.526592568487364,
+                    "velocityX": -1.8663974293700505,
+                    "velocityY": -0.6278353441988297,
+                    "timestamp": 9.933924070877106
+                },
+                {
+                    "x": 7.609206539592904,
+                    "y": 2.2782030815405587,
+                    "heading": 0.06664871856515628,
+                    "angularVelocity": -1.6375960364466748,
+                    "velocityX": -2.095788381735004,
+                    "velocityY": -0.7282960756400965,
+                    "timestamp": 9.993026734865435
+                },
+                {
+                    "x": 7.471733974955729,
+                    "y": 2.229217918807126,
+                    "heading": -0.03539885417690381,
+                    "angularVelocity": -1.7266154493850245,
+                    "velocityX": -2.325996078016428,
+                    "velocityY": -0.8288148017000726,
+                    "timestamp": 10.052129398853763
+                },
+                {
+                    "x": 7.320604827398418,
+                    "y": 2.174296101814293,
+                    "heading": -0.14101890697055008,
+                    "angularVelocity": -1.787060779773037,
+                    "velocityX": -2.5570615156561423,
+                    "velocityY": -0.9292612766774592,
+                    "timestamp": 10.111232062842092
+                },
+                {
+                    "x": 7.155773189909133,
+                    "y": 2.113457738944985,
+                    "heading": -0.24782868572534286,
+                    "angularVelocity": -1.8071905993253574,
+                    "velocityX": -2.7889036866736787,
+                    "velocityY": -1.0293675236250293,
+                    "timestamp": 10.17033472683042
+                },
+                {
+                    "x": 6.977229587916178,
+                    "y": 2.046760806821276,
+                    "heading": -0.35216325005396515,
+                    "angularVelocity": -1.7653106863207704,
+                    "velocityX": -3.0209061647070943,
+                    "velocityY": -1.1284928228764857,
+                    "timestamp": 10.229437390818749
+                },
+                {
+                    "x": 6.785127223955647,
+                    "y": 1.9743718586500072,
+                    "heading": -0.44791637926652805,
+                    "angularVelocity": -1.620115283322456,
+                    "velocityX": -3.2503165000898613,
+                    "velocityY": -1.224800089985181,
+                    "timestamp": 10.288540054807077
+                },
+                {
+                    "x": 6.580482239369349,
+                    "y": 1.8965267551833909,
+                    "heading": -0.523907298024649,
+                    "angularVelocity": -1.285744391710116,
+                    "velocityX": -3.4625340175310977,
+                    "velocityY": -1.3171166613063143,
+                    "timestamp": 10.347642718795406
+                },
+                {
+                    "x": 6.365568212552411,
+                    "y": 1.8126473637838463,
+                    "heading": -0.5685150089578763,
+                    "angularVelocity": -0.7547495818807144,
+                    "velocityX": -3.636283245360637,
+                    "velocityY": -1.4192150698335522,
+                    "timestamp": 10.406745382783734
+                },
+                {
+                    "x": 6.1391542252718745,
+                    "y": 1.727320392022171,
+                    "heading": -0.5825906454165231,
+                    "angularVelocity": -0.23815570244729414,
+                    "velocityX": -3.8308592540811275,
+                    "velocityY": -1.4437077113567243,
+                    "timestamp": 10.465848046772063
+                },
+                {
+                    "x": 5.903469838213687,
+                    "y": 1.6529470737325385,
+                    "heading": -0.5825907170739721,
+                    "angularVelocity": -0.0000012124233368564011,
+                    "velocityX": -3.987711740112587,
+                    "velocityY": -1.2583750591059566,
+                    "timestamp": 10.524950710760391
+                },
+                {
+                    "x": 5.663724751126659,
+                    "y": 1.5929402727768625,
+                    "heading": -0.5825907737674252,
+                    "angularVelocity": -9.592368472200333e-7,
+                    "velocityX": -4.056417611469635,
+                    "velocityY": -1.0152977362835247,
+                    "timestamp": 10.58405337474872
+                },
+                {
+                    "x": 5.4207938290604085,
+                    "y": 1.547519030326998,
+                    "heading": -0.5825908335297906,
+                    "angularVelocity": -0.0000010111619577208565,
+                    "velocityX": -4.110321018934508,
+                    "velocityY": -0.7685143001140271,
+                    "timestamp": 10.643156038737049
+                },
+                {
+                    "x": 5.175563573210271,
+                    "y": 1.5168491316802175,
+                    "heading": -0.582590897808758,
+                    "angularVelocity": -0.0000010875815561180186,
+                    "velocityX": -4.149225082283343,
+                    "velocityY": -0.5189258246098157,
+                    "timestamp": 10.702258702725377
+                },
+                {
+                    "x": 4.928928882781426,
+                    "y": 1.5010425173093036,
+                    "heading": -0.582590968507317,
+                    "angularVelocity": -0.0000011961991942253073,
+                    "velocityX": -4.172987709615762,
+                    "velocityY": -0.26744334864559327,
+                    "timestamp": 10.761361366713706
+                },
+                {
+                    "x": 4.6817897858705155,
+                    "y": 1.500156880852723,
+                    "heading": -0.5825910482884903,
+                    "angularVelocity": -0.000001349874404780368,
+                    "velocityX": -4.181522121569922,
+                    "velocityY": -0.014984712986125005,
+                    "timestamp": 10.820464030702034
+                },
+                {
+                    "x": 4.435048362369104,
+                    "y": 1.5141954560145352,
+                    "heading": -0.5825917399367928,
+                    "angularVelocity": -0.000011702489461728115,
+                    "velocityX": -4.174793602368545,
+                    "velocityY": 0.23752863601181112,
+                    "timestamp": 10.879566694690363
+                },
+                {
+                    "x": 4.191238550201924,
+                    "y": 1.5428059629228041,
+                    "heading": -0.5872689519449389,
+                    "angularVelocity": -0.07913707593738419,
+                    "velocityX": -4.125191585532014,
+                    "velocityY": 0.48408151134979194,
+                    "timestamp": 10.938669358678691
+                },
+                {
+                    "x": 3.952295303344726,
+                    "y": 1.5853265047073366,
+                    "heading": -0.6,
+                    "angularVelocity": -0.21540565510845927,
+                    "velocityX": -4.042850706431503,
+                    "velocityY": 0.7194352828652425,
+                    "timestamp": 10.99777202266702
+                },
+                {
+                    "x": 3.8134740215452583,
+                    "y": 1.615040134332196,
+                    "heading": -0.6109765742860589,
+                    "angularVelocity": -0.31488494666214406,
+                    "velocityX": -3.9823656065913373,
+                    "velocityY": 0.8523947850875385,
+                    "timestamp": 11.03263102244587
+                },
+                {
+                    "x": 3.6768690053953224,
+                    "y": 1.6493458750794194,
+                    "heading": -0.6252964897864377,
+                    "angularVelocity": -0.41079536392972205,
+                    "velocityX": -3.918787601955746,
+                    "velocityY": 0.9841286601699233,
+                    "timestamp": 11.06749002222472
+                },
+                {
+                    "x": 3.542588683535768,
+                    "y": 1.688198377556308,
+                    "heading": -0.6428198278038513,
+                    "angularVelocity": -0.5026919340366556,
+                    "velocityX": -3.852099105294062,
+                    "velocityY": 1.1145615973887317,
+                    "timestamp": 11.102349022003569
+                },
+                {
+                    "x": 3.4107408609461936,
+                    "y": 1.7315503308123263,
+                    "heading": -0.6633872651355344,
+                    "angularVelocity": -0.590018000004748,
+                    "velocityX": -3.782318007574331,
+                    "velocityY": 1.2436373255414277,
+                    "timestamp": 11.137208021782419
+                },
+                {
+                    "x": 3.2814309719594807,
+                    "y": 1.7793535620510226,
+                    "heading": -0.6868149980921183,
+                    "angularVelocity": -0.6720712902037491,
+                    "velocityX": -3.709512315530314,
+                    "velocityY": 1.3713311208573407,
+                    "timestamp": 11.172067021561269
+                },
+                {
+                    "x": 3.154759713968678,
+                    "y": 1.8315607122204742,
+                    "heading": -0.7128878595460402,
+                    "angularVelocity": -0.7479520817961316,
+                    "velocityX": -3.633817917737781,
+                    "velocityY": 1.497666327222826,
+                    "timestamp": 11.206926021340118
+                },
+                {
+                    "x": 3.0308199060377405,
+                    "y": 1.8881276343253246,
+                    "heading": -0.7413498638418092,
+                    "angularVelocity": -0.8164894138195471,
+                    "velocityX": -3.5554608197948028,
+                    "velocityY": 1.6227350888929106,
+                    "timestamp": 11.241785021118968
+                },
+                {
+                    "x": 2.909692349791607,
+                    "y": 1.9490166364375032,
+                    "heading": -0.7718912775944233,
+                    "angularVelocity": -0.8761414253527852,
+                    "velocityX": -3.474785765930811,
+                    "velocityY": 1.7467225823593937,
+                    "timestamp": 11.276644020897818
+                },
+                {
+                    "x": 2.791440373924634,
+                    "y": 2.0142005993273084,
+                    "heading": -0.8041312385312992,
+                    "angularVelocity": -0.9248676422562412,
+                    "velocityX": -3.392294002041893,
+                    "velocityY": 1.8699321065819634,
+                    "timestamp": 11.311503020676668
+                },
+                {
+                    "x": 2.6761026118014377,
+                    "y": 2.0836677838154385,
+                    "heading": -0.8375950488175486,
+                    "angularVelocity": -0.9599762040950126,
+                    "velocityX": -3.3086939629626064,
+                    "velocityY": 1.992804869010546,
+                    "timestamp": 11.346362020455517
+                },
+                {
+                    "x": 2.563683426803659,
+                    "y": 2.157426720429071,
+                    "heading": -0.8716858609029794,
+                    "angularVelocity": -0.9779630024299947,
+                    "velocityX": -3.2249687515700507,
+                    "velocityY": 2.1159223466412884,
+                    "timestamp": 11.381221020234367
+                },
+                {
+                    "x": 2.4541405159302982,
+                    "y": 2.2355097870550096,
+                    "heading": -0.9056525586852363,
+                    "angularVelocity": -0.9744025358658038,
+                    "velocityX": -3.1424570862134558,
+                    "velocityY": 2.239968648593113,
+                    "timestamp": 11.416080020013217
+                },
+                {
+                    "x": 2.3473705926939137,
+                    "y": 2.317972680271922,
+                    "heading": -0.9385621397982856,
+                    "angularVelocity": -0.9440770338171433,
+                    "velocityX": -3.062908399946827,
+                    "velocityY": 2.365612718094827,
+                    "timestamp": 11.450939019792067
+                },
+                {
+                    "x": 2.2431987205449455,
+                    "y": 2.404885409895606,
+                    "heading": -0.9692995356278491,
+                    "angularVelocity": -0.8817635624821536,
+                    "velocityX": -2.988378117841816,
+                    "velocityY": 2.493265158927951,
+                    "timestamp": 11.485798019570916
+                },
+                {
+                    "x": 2.141383918660014,
+                    "y": 2.496312439027103,
+                    "heading": -0.996629601543935,
+                    "angularVelocity": -0.7840175016343364,
+                    "velocityX": -2.9207608517415142,
+                    "velocityY": 2.6227668525064347,
+                    "timestamp": 11.520657019349766
+                },
+                {
+                    "x": 2.041650396130837,
+                    "y": 2.5922897101709004,
+                    "heading": -1.0193236199557545,
+                    "angularVelocity": -0.651023223723952,
+                    "velocityX": -2.861055198425048,
+                    "velocityY": 2.7532996285805456,
+                    "timestamp": 11.555516019128616
+                },
+                {
+                    "x": 1.943731375644481,
+                    "y": 2.6928135229166212,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": -0.48642686340414926,
+                    "velocityX": -2.809002584915431,
+                    "velocityY": 2.8837262509956236,
+                    "timestamp": 11.590375018907466
+                },
+                {
+                    "x": 1.8480632305145264,
+                    "y": 2.7954604625701904,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 2.1736687540126838e-29,
+                    "velocityX": -2.7444317317446116,
+                    "velocityY": 2.9446323848869578,
+                    "timestamp": 11.625234018686315
+                },
+                {
+                    "x": 1.672640445377056,
+                    "y": 3.013300327645549,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": -2.795107901873647e-30,
+                    "velocityX": -2.460711972616185,
+                    "velocityY": 3.0557100303929148,
+                    "timestamp": 11.696523461371866
+                },
+                {
+                    "x": 1.5164302211322118,
+                    "y": 3.221006881583978,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 8.418621207211218e-30,
+                    "velocityX": -2.1912111858395145,
+                    "velocityY": 2.913566807565,
+                    "timestamp": 11.767812904057417
+                },
+                {
+                    "x": 1.3764341718321418,
+                    "y": 3.4142598003253495,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 4.531432261764684e-30,
+                    "velocityX": -1.9637697255900248,
+                    "velocityY": 2.7108210060469538,
+                    "timestamp": 11.839102346742967
+                },
+                {
+                    "x": 1.2511473564025137,
+                    "y": 3.591530049792972,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 4.531432118047956e-30,
+                    "velocityX": -1.7574385590620065,
+                    "velocityY": 2.4866269504944793,
+                    "timestamp": 11.910391789428518
+                },
+                {
+                    "x": 1.1397098089961486,
+                    "y": 3.7520669438694583,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 4.5314299990799685e-30,
+                    "velocityX": -1.5631704107703956,
+                    "velocityY": 2.251902778712901,
+                    "timestamp": 11.981681232114068
+                },
+                {
+                    "x": 1.0415712767381338,
+                    "y": 3.895429913411281,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 4.5314368609894865e-30,
+                    "velocityX": -1.3766208369855277,
+                    "velocityY": 2.010998601492499,
+                    "timestamp": 12.052970674799619
+                },
+                {
+                    "x": 0.9563509320037674,
+                    "y": 4.021330695987133,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 4.531437736881817e-30,
+                    "velocityX": -1.1954132550911205,
+                    "velocityY": 1.7660508741972465,
+                    "timestamp": 12.12426011748517
+                },
+                {
+                    "x": 0.8837700445121739,
+                    "y": 4.129566493346561,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 4.531436969781661e-30,
+                    "velocityX": -1.0181155127237986,
+                    "velocityY": 1.5182584304501188,
+                    "timestamp": 12.19554956017072
+                },
+                {
+                    "x": 0.8236159650487832,
+                    "y": 4.219987070970065,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 4.531436969781641e-30,
+                    "velocityX": -0.8438006694584916,
+                    "velocityY": 1.2683585986544863,
+                    "timestamp": 12.26683900285627
+                },
+                {
+                    "x": 0.7757211998785677,
+                    "y": 4.29247675769312,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 4.531437053865754e-30,
+                    "velocityX": -0.6718353148231706,
+                    "velocityY": 1.0168362101356112,
+                    "timestamp": 12.338128445541821
+                },
+                {
+                    "x": 0.7399504493458321,
+                    "y": 4.346943794130791,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 4.5314376706268254e-30,
+                    "velocityX": -0.501767852085982,
+                    "velocityY": 0.7640266831362128,
+                    "timestamp": 12.409417888227372
+                },
+                {
+                    "x": 0.7161921614034822,
+                    "y": 4.383313637078767,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 4.531437521414341e-30,
+                    "velocityX": -0.33326516588360594,
+                    "velocityY": 0.5101715145733307,
+                    "timestamp": 12.480707330912923
+                },
+                {
+                    "x": 0.7043527960777284,
+                    "y": 4.401524543762207,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 4.531437376556036e-30,
+                    "velocityX": -0.16607459505576197,
+                    "velocityY": 0.25545026019864153,
+                    "timestamp": 12.551996773598473
+                },
+                {
+                    "x": 0.7043527960777284,
+                    "y": 4.401524543762207,
+                    "heading": -1.0362799738795865,
+                    "angularVelocity": 1.5152982881175701e-30,
+                    "velocityX": -8.315280281271062e-29,
+                    "velocityY": 1.901615622375989e-28,
+                    "timestamp": 12.623286216284024
+                }
+            ],
+            "trajectoryWaypoints": [
+                {
+                    "timestamp": 0,
+                    "isStopPoint": true,
+                    "x": 0.7043527960777283,
+                    "y": 4.401524543762207,
+                    "heading": -1.03622,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "timestamp": 1.537512195642688,
+                    "isStopPoint": false,
+                    "x": 3.952295303344726,
+                    "y": 1.2853265047073366,
+                    "heading": -0.6,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "timestamp": 2.176953315426481,
+                    "isStopPoint": false,
+                    "x": 6.344450855255133,
+                    "y": 0.6604639410972595,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "timestamp": 3.061199522549858,
+                    "isStopPoint": false,
+                    "x": 8.144450855255126,
+                    "y": 0.6604639410972595,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "timestamp": 4.602532743133479,
+                    "isStopPoint": false,
+                    "x": 3.952295303344726,
+                    "y": 1.2853265047073366,
+                    "heading": -0.6,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "timestamp": 5.238072990428239,
+                    "isStopPoint": false,
+                    "x": 1.9563456773757935,
+                    "y": 2.7954604625701904,
+                    "heading": -1.0362799738795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "timestamp": 6.251251288371045,
+                    "isStopPoint": true,
+                    "x": 0.7043527960777283,
+                    "y": 4.401524543762207,
+                    "heading": -1.0362799738795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "timestamp": 7.757435525740661,
+                    "isStopPoint": false,
+                    "x": 3.952295303344726,
+                    "y": 1.5853265047073366,
+                    "heading": -0.6,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 20
+                },
+                {
+                    "timestamp": 8.440471076579563,
+                    "isStopPoint": false,
+                    "x": 6.352611541748047,
+                    "y": 1.5393844842910767,
+                    "heading": 0.305878931297,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "timestamp": 9.40200009498215,
+                    "isStopPoint": false,
+                    "x": 8.2424898147583,
+                    "y": 2.4419312477111816,
+                    "heading": 0.6435004368303741,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 27
+                },
+                {
+                    "timestamp": 10.99777202266702,
+                    "isStopPoint": false,
+                    "x": 3.952295303344726,
+                    "y": 1.5853265047073366,
+                    "heading": -0.6,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "timestamp": 11.625234018686315,
+                    "isStopPoint": false,
+                    "x": 1.8480632305145264,
+                    "y": 2.7954604625701904,
+                    "heading": -1.0362799738795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "timestamp": 12.623286216284024,
+                    "isStopPoint": true,
+                    "x": 0.7043527960777284,
+                    "y": 4.401524543762207,
+                    "heading": -1.0362799738795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        6
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        2,
+                        3
+                    ],
+                    "type": "ZeroAngularVelocity"
+                },
+                {
+                    "scope": [
+                        5,
+                        6
+                    ],
+                    "type": "ZeroAngularVelocity"
+                },
+                {
+                    "scope": [
+                        8,
+                        9
+                    ],
+                    "type": "ZeroAngularVelocity"
+                },
+                {
+                    "scope": [
+                        11,
+                        12
+                    ],
+                    "type": "ZeroAngularVelocity"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": [],
+            "eventMarkers": [],
+            "isTrajectoryStale": true
+        },
+        "Distance Source 4": {
+            "waypoints": [
+                {
+                    "x": 0.6802782416343689,
+                    "y": 4.38139533996582,
+                    "heading": -1.0378726274695866,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "x": 3.517277717590332,
+                    "y": 1.2412056922912598,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 7.967684268951416,
+                    "y": 0.8080758452415466,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 4.870807647705078,
+                    "y": 1.8692435026168823,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 3.1599457263946533,
+                    "y": 3,
+                    "heading": -0.7431853071795862,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 4.870807647705078,
+                    "y": 1.8692435026168823,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 6.3759331703186035,
+                    "y": 1.8584154844284058,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 11
+                },
+                {
+                    "x": 7.924371719360352,
+                    "y": 2.432312250137329,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 11
+                },
+                {
+                    "x": 6.3759331703186035,
+                    "y": 1.8584154844284058,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 4.870807647705078,
+                    "y": 1.8692435026168823,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 3.1599459648132324,
+                    "y": 3,
+                    "heading": -0.7431853071795862,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 4.5134758949279785,
+                    "y": 3.504307985305786,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 9
+                },
+                {
+                    "x": 5.336422443389893,
+                    "y": 4.143174171447754,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 7.924371719360352,
+                    "y": 4.143174171447754,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 4.5134758949279785,
+                    "y": 3.504307985305786,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 3.959945964813233,
+                    "y": 2.5999999999999996,
+                    "heading": -0.7431853071795862,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 8
+                },
+                {
+                    "x": 3.1599459648132324,
+                    "y": 3,
+                    "heading": -0.7431853071795862,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 0.6802782416343689,
+                    "y": 4.38139533996582,
+                    "heading": -1.0378726274695866,
+                    "angularVelocity": -3.347916590381317e-25,
+                    "velocityX": -7.79504417865945e-25,
+                    "velocityY": 1.5644415406561792e-24,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.6928709180148234,
+                    "y": 4.366129172079192,
+                    "heading": -1.0148952648842788,
+                    "angularVelocity": 0.39308459587811256,
+                    "velocityX": 0.2154288634153295,
+                    "velocityY": -0.2611655455252246,
+                    "timestamp": 0.05845398885188716
+                },
+                {
+                    "x": 0.7181419130531553,
+                    "y": 4.335559528214801,
+                    "heading": -0.9698560026458198,
+                    "angularVelocity": 0.7705079349260691,
+                    "velocityX": 0.43232284972655155,
+                    "velocityY": -0.5229693381892135,
+                    "timestamp": 0.11690797770377433
+                },
+                {
+                    "x": 0.7561908960261392,
+                    "y": 4.2896399224059945,
+                    "heading": -0.9039183675220187,
+                    "angularVelocity": 1.128026271926048,
+                    "velocityX": 0.6509219254376942,
+                    "velocityY": -0.7855683882439376,
+                    "timestamp": 0.1753619665556615
+                },
+                {
+                    "x": 0.8071292746845115,
+                    "y": 4.228302195171338,
+                    "heading": -0.8186581069281054,
+                    "angularVelocity": 1.4585875535363195,
+                    "velocityX": 0.8714269061678875,
+                    "velocityY": -1.0493334747450136,
+                    "timestamp": 0.23381595540754865
+                },
+                {
+                    "x": 0.871072837229944,
+                    "y": 4.15144639527446,
+                    "heading": -0.7161964133960428,
+                    "angularVelocity": 1.7528605924855503,
+                    "velocityX": 1.0939127303605414,
+                    "velocityY": -1.314808474261995,
+                    "timestamp": 0.2922699442594358
+                },
+                {
+                    "x": 0.9481336446587337,
+                    "y": 4.0589407507578334,
+                    "heading": -0.5992730958902776,
+                    "angularVelocity": 2.00026242523893,
+                    "velocityX": 1.3183156349526337,
+                    "velocityY": -1.58253775890335,
+                    "timestamp": 0.350723933111323
+                },
+                {
+                    "x": 1.03842141694975,
+                    "y": 3.9506371946873022,
+                    "heading": -0.47137491438003465,
+                    "angularVelocity": 2.188014608110252,
+                    "velocityX": 1.544595571053174,
+                    "velocityY": -1.852800094531693,
+                    "timestamp": 0.4091779219632102
+                },
+                {
+                    "x": 1.1420524474939961,
+                    "y": 3.8264005587076344,
+                    "heading": -0.33738265270345347,
+                    "angularVelocity": 2.2922689162598577,
+                    "velocityX": 1.7728649931287002,
+                    "velocityY": -2.1253748190643313,
+                    "timestamp": 0.46763191081509736
+                },
+                {
+                    "x": 1.2590920368315979,
+                    "y": 3.6861968307243496,
+                    "heading": -0.20574611612623897,
+                    "angularVelocity": 2.2519684141788834,
+                    "velocityX": 2.0022515423910754,
+                    "velocityY": -2.3985314045639927,
+                    "timestamp": 0.5260858996669845
+                },
+                {
+                    "x": 1.3890539557520782,
+                    "y": 3.530716769695316,
+                    "heading": -0.09362413816745035,
+                    "angularVelocity": 1.9181236415343244,
+                    "velocityX": 2.2233199388630713,
+                    "velocityY": -2.659870850268142,
+                    "timestamp": 0.5845398885188716
+                },
+                {
+                    "x": 1.526819292676983,
+                    "y": 3.3606093766178162,
+                    "heading": -0.02492102014455636,
+                    "angularVelocity": 1.1753366942498389,
+                    "velocityX": 2.356816696872128,
+                    "velocityY": -2.9101075293342884,
+                    "timestamp": 0.6429938773707587
+                },
+                {
+                    "x": 1.6725539368788018,
+                    "y": 3.1759976053864527,
+                    "heading": -8.429679741259334e-7,
+                    "angularVelocity": 0.4263212428449645,
+                    "velocityX": 2.493151401029047,
+                    "velocityY": -3.158240778044072,
+                    "timestamp": 0.7014478662226459
+                },
+                {
+                    "x": 1.8287790494270222,
+                    "y": 2.98801124535308,
+                    "heading": -7.696371008390055e-7,
+                    "angularVelocity": 0.0000012545058896277584,
+                    "velocityX": 2.6726167985570535,
+                    "velocityY": -3.2159714627807388,
+                    "timestamp": 0.759901855074533
+                },
+                {
+                    "x": 1.9850045217267513,
+                    "y": 2.800025184289596,
+                    "heading": -6.963070170753428e-7,
+                    "angularVelocity": 0.0000012544923828803064,
+                    "velocityX": 2.6726229529960572,
+                    "velocityY": -3.215966348161622,
+                    "timestamp": 0.8183558439264201
+                },
+                {
+                    "x": 2.1412299940437136,
+                    "y": 2.6120391232404336,
+                    "heading": -6.229769330118699e-7,
+                    "angularVelocity": 0.0000012544923880093,
+                    "velocityX": 2.6726229532908703,
+                    "velocityY": -3.2159663479166176,
+                    "timestamp": 0.8768098327783073
+                },
+                {
+                    "x": 2.297455466360677,
+                    "y": 2.4240530621912724,
+                    "heading": -5.496468485640347e-7,
+                    "angularVelocity": 0.0000012544923945847663,
+                    "velocityX": 2.6726229532908814,
+                    "velocityY": -3.215966347916608,
+                    "timestamp": 0.9352638216301944
+                },
+                {
+                    "x": 2.453680938677642,
+                    "y": 2.2360670011421124,
+                    "heading": -4.7631676369736977e-7,
+                    "angularVelocity": 0.0000012544924017498851,
+                    "velocityX": 2.6726229532909214,
+                    "velocityY": -3.2159663479165745,
+                    "timestamp": 0.9937178104820815
+                },
+                {
+                    "x": 2.6099064110461754,
+                    "y": 2.0480809401358084,
+                    "heading": -4.029866785187995e-7,
+                    "angularVelocity": 0.0000012544924070857982,
+                    "velocityX": 2.6726229541731215,
+                    "velocityY": -3.2159663471834232,
+                    "timestamp": 1.0521717993339688
+                },
+                {
+                    "x": 2.766132959939523,
+                    "y": 1.8600957737787043,
+                    "heading": -3.2965660566972314e-7,
+                    "angularVelocity": 0.0000012544921961593194,
+                    "velocityX": 2.672641370791651,
+                    "velocityY": -3.2159510419969424,
+                    "timestamp": 1.1106257881858559
+                },
+                {
+                    "x": 2.933382193240376,
+                    "y": 1.6818463665507355,
+                    "heading": -2.5592792501793225e-7,
+                    "angularVelocity": 0.0000012613113681361818,
+                    "velocityX": 2.8612116398871454,
+                    "velocityY": -3.0493968115610293,
+                    "timestamp": 1.169079777037743
+                },
+                {
+                    "x": 3.1152217469834365,
+                    "y": 1.5185076592751214,
+                    "heading": -1.793173771215745e-7,
+                    "angularVelocity": 0.000001310612832436061,
+                    "velocityX": 3.1108151439213563,
+                    "velocityY": -2.7943124239046795,
+                    "timestamp": 1.2275337658896301
+                },
+                {
+                    "x": 3.3103273336088685,
+                    "y": 1.3712698739724987,
+                    "heading": -9.63896176110914e-8,
+                    "angularVelocity": 0.0000014186843556666162,
+                    "velocityX": 3.3377634350976035,
+                    "velocityY": -2.5188663458998373,
+                    "timestamp": 1.2859877547415173
+                },
+                {
+                    "x": 3.517277717590332,
+                    "y": 1.2412056922912598,
+                    "heading": 2.0248701458039248e-25,
+                    "angularVelocity": 0.0000016489827213558838,
+                    "velocityX": 3.5403979787562827,
+                    "velocityY": -2.2250693962186263,
+                    "timestamp": 1.3444417435934044
+                },
+                {
+                    "x": 3.8085805078161967,
+                    "y": 1.1011465689731057,
+                    "heading": 1.2027671540774088e-13,
+                    "angularVelocity": 1.5560191716883039e-12,
+                    "velocityX": 3.768582512599393,
+                    "velocityY": -1.8119440684297683,
+                    "timestamp": 1.4217394514854198
+                },
+                {
+                    "x": 4.113810562883195,
+                    "y": 0.9948051908350006,
+                    "heading": 1.2024608996754215e-13,
+                    "angularVelocity": -3.962011427529581e-16,
+                    "velocityX": 3.9487594573102207,
+                    "velocityY": -1.3757377940192446,
+                    "timestamp": 1.4990371593774352
+                },
+                {
+                    "x": 4.429079571376881,
+                    "y": 0.9235357844694564,
+                    "heading": 1.2023024929371935e-13,
+                    "angularVelocity": -2.0493070564434177e-16,
+                    "velocityX": 4.078633339737774,
+                    "velocityY": -0.9220118980126479,
+                    "timestamp": 1.5763348672694506
+                },
+                {
+                    "x": 4.74851734223509,
+                    "y": 0.8742064147498704,
+                    "heading": 1.2026218341744134e-13,
+                    "angularVelocity": 4.1313157391792204e-16,
+                    "velocityX": 4.132564594340397,
+                    "velocityY": -0.6381737707992456,
+                    "timestamp": 1.653632575161466
+                },
+                {
+                    "x": 5.067955221408222,
+                    "y": 0.8248777464405996,
+                    "heading": 1.2017533765568414e-13,
+                    "angularVelocity": -1.1235231176431118e-15,
+                    "velocityX": 4.132565995609943,
+                    "velocityY": -0.6381646966580525,
+                    "timestamp": 1.7309302830534814
+                },
+                {
+                    "x": 5.387393100583508,
+                    "y": 0.7755490781452744,
+                    "heading": 1.2011087822013848e-13,
+                    "angularVelocity": -8.339113449011661e-16,
+                    "velocityX": 4.132565995637804,
+                    "velocityY": -0.6381646964776388,
+                    "timestamp": 1.8082279909454968
+                },
+                {
+                    "x": 5.706830979759032,
+                    "y": 0.7262204098514907,
+                    "heading": 1.2009725083714761e-13,
+                    "angularVelocity": -1.7629737495368953e-16,
+                    "velocityX": 4.132565995640883,
+                    "velocityY": -0.6381646964576962,
+                    "timestamp": 1.8855256988375122
+                },
+                {
+                    "x": 6.02626887090532,
+                    "y": 0.6768918190768992,
+                    "heading": 1.200782994529034e-13,
+                    "angularVelocity": -2.4517394837846764e-16,
+                    "velocityX": 4.1325661505065865,
+                    "velocityY": -0.6381636935923554,
+                    "timestamp": 1.9628234067295276
+                },
+                {
+                    "x": 6.346278711766802,
+                    "y": 0.6314211922985453,
+                    "heading": 1.2000563974027956e-13,
+                    "angularVelocity": -9.399982820393656e-16,
+                    "velocityX": 4.139965460664552,
+                    "velocityY": -0.588253235682956,
+                    "timestamp": 2.040121114621543
+                },
+                {
+                    "x": 6.669374055375815,
+                    "y": 0.6222974986610686,
+                    "heading": 1.1998245156455226e-13,
+                    "angularVelocity": -2.999852952930381e-16,
+                    "velocityX": 4.179882591866463,
+                    "velocityY": -0.11803317182732602,
+                    "timestamp": 2.1174188225135584
+                },
+                {
+                    "x": 6.957333126631289,
+                    "y": 0.6229872679410849,
+                    "heading": 1.0411186745728865e-13,
+                    "angularVelocity": -2.0531765481888398e-13,
+                    "velocityX": 3.7253248396155745,
+                    "velocityY": 0.008923541186757694,
+                    "timestamp": 2.194716530405574
+                },
+                {
+                    "x": 7.209511372482262,
+                    "y": 0.6307902356507334,
+                    "heading": 8.529380764861957e-14,
+                    "angularVelocity": -2.4344913092323517e-13,
+                    "velocityX": 3.262428508271763,
+                    "velocityY": 0.10094694813653739,
+                    "timestamp": 2.272014238297589
+                },
+                {
+                    "x": 7.425794423178197,
+                    "y": 0.6451047906108485,
+                    "heading": 6.637626916620301e-14,
+                    "angularVelocity": -2.4473608595023864e-13,
+                    "velocityX": 2.7980525761266,
+                    "velocityY": 0.18518731474046324,
+                    "timestamp": 2.3493119461896046
+                },
+                {
+                    "x": 7.606135287145741,
+                    "y": 0.6656663723850257,
+                    "heading": 4.8622136850652897e-14,
+                    "angularVelocity": -2.2968510709725984e-13,
+                    "velocityX": 2.3330687142687285,
+                    "velocityY": 0.266005064508531,
+                    "timestamp": 2.42660965408162
+                },
+                {
+                    "x": 7.750508424686906,
+                    "y": 0.6923262301695847,
+                    "heading": 3.272151062184158e-14,
+                    "angularVelocity": -2.057063095716186e-13,
+                    "velocityX": 1.8677544454856772,
+                    "velocityY": 0.3448984259895882,
+                    "timestamp": 2.5039073619736354
+                },
+                {
+                    "x": 7.858897802002306,
+                    "y": 0.7249890399184212,
+                    "heading": 1.9124643365638934e-14,
+                    "angularVelocity": -1.7590259306521183e-13,
+                    "velocityX": 1.4022327475326948,
+                    "velocityY": 0.42255858083743525,
+                    "timestamp": 2.581205069865651
+                },
+                {
+                    "x": 7.931292420667315,
+                    "y": 0.7635885039265983,
+                    "heading": 8.143934963506789e-15,
+                    "angularVelocity": -1.4205736109888667e-13,
+                    "velocityX": 0.9365687630239243,
+                    "velocityY": 0.49936104265989784,
+                    "timestamp": 2.658502777757666
+                },
+                {
+                    "x": 7.967684268951416,
+                    "y": 0.8080758452415466,
+                    "heading": -2.356364968162154e-28,
+                    "angularVelocity": -1.053580395279536e-13,
+                    "velocityX": 0.4708011308037708,
+                    "velocityY": 0.5755324773290436,
+                    "timestamp": 2.7358004856496816
+                },
+                {
+                    "x": 7.968682020134014,
+                    "y": 0.8574283512733433,
+                    "heading": -5.0773941325982076e-15,
+                    "angularVelocity": -6.686084313137058e-14,
+                    "velocityX": 0.013138725015563199,
+                    "velocityY": 0.6498904906252848,
+                    "timestamp": 2.8117402120246786
+                },
+                {
+                    "x": 7.934920196939559,
+                    "y": 0.9123978556608698,
+                    "heading": -7.250674587273434e-15,
+                    "angularVelocity": -2.861849203858442e-14,
+                    "velocityX": -0.44458710619704805,
+                    "velocityY": 0.7238570246629862,
+                    "timestamp": 2.8876799383996756
+                },
+                {
+                    "x": 7.866392659944353,
+                    "y": 0.9729462284683079,
+                    "heading": -6.561254231060817e-15,
+                    "angularVelocity": 9.078520415102618e-15,
+                    "velocityX": -0.9023937834172842,
+                    "velocityY": 0.7973214508101477,
+                    "timestamp": 2.9636196647746726
+                },
+                {
+                    "x": 7.7630913157264,
+                    "y": 1.039022791795512,
+                    "heading": -3.0532129081230325e-15,
+                    "angularVelocity": 4.6195074572890516e-14,
+                    "velocityX": -1.3603070375555597,
+                    "velocityY": 0.8701185332287369,
+                    "timestamp": 3.0395593911496697
+                },
+                {
+                    "x": 7.6250050093039485,
+                    "y": 1.1105569064458902,
+                    "heading": 3.2228615065427893e-15,
+                    "angularVelocity": 8.264547048371132e-14,
+                    "velocityX": -1.8183671842662383,
+                    "velocityY": 0.9419854148161748,
+                    "timestamp": 3.1154991175246667
+                },
+                {
+                    "x": 7.452117387393709,
+                    "y": 1.1874432876779466,
+                    "heading": 1.2177403879633131e-14,
+                    "angularVelocity": 1.1791644242793474e-13,
+                    "velocityX": -2.2766426765419903,
+                    "velocityY": 1.0124658713199024,
+                    "timestamp": 3.1914388438996637
+                },
+                {
+                    "x": 7.244402183717801,
+                    "y": 1.2695082427410065,
+                    "heading": 2.365643857652256e-14,
+                    "angularVelocity": 1.5115981114028605e-13,
+                    "velocityX": -2.735264052048239,
+                    "velocityY": 1.0806590829392273,
+                    "timestamp": 3.2673785702746607
+                },
+                {
+                    "x": 7.00181043141649,
+                    "y": 1.3564109624551457,
+                    "heading": 3.7383567341129667e-14,
+                    "angularVelocity": 1.8076347413765312e-13,
+                    "velocityX": -3.194530239724227,
+                    "velocityY": 1.1443644040143879,
+                    "timestamp": 3.3433182966496577
+                },
+                {
+                    "x": 6.724221087183376,
+                    "y": 1.447179537855358,
+                    "heading": 5.2548800981821277e-14,
+                    "angularVelocity": 1.9970092551827854e-13,
+                    "velocityX": -3.6553903665961816,
+                    "velocityY": 1.1952712991351762,
+                    "timestamp": 3.4192580230246548
+                },
+                {
+                    "x": 6.415496495875919,
+                    "y": 1.5215059011095828,
+                    "heading": 5.2606937821813504e-14,
+                    "angularVelocity": 7.655655711115095e-16,
+                    "velocityX": -4.065389830125926,
+                    "velocityY": 0.9787546887803473,
+                    "timestamp": 3.4951977493996518
+                },
+                {
+                    "x": 6.102420468620207,
+                    "y": 1.5745971909728977,
+                    "heading": 5.2547023381561494e-14,
+                    "angularVelocity": -7.889736125229508e-16,
+                    "velocityX": -4.122691010364133,
+                    "velocityY": 0.6991240605891275,
+                    "timestamp": 3.571137475774649
+                },
+                {
+                    "x": 5.7893443139235865,
+                    "y": 1.6276877293196694,
+                    "heading": 5.251469887397708e-14,
+                    "angularVelocity": -4.256600481369481e-16,
+                    "velocityX": -4.1226926885491135,
+                    "velocityY": 0.6991141643651176,
+                    "timestamp": 3.647077202149646
+                },
+                {
+                    "x": 5.47626834023763,
+                    "y": 1.6807793350795175,
+                    "heading": 5.2545327836571197e-14,
+                    "angularVelocity": 4.03332538271171e-16,
+                    "velocityX": -4.122690304939467,
+                    "velocityY": 0.6991282204215097,
+                    "timestamp": 3.723016928524643
+                },
+                {
+                    "x": 5.168305647623383,
+                    "y": 1.7582019652150536,
+                    "heading": 5.2570129566972103e-14,
+                    "angularVelocity": 3.265975739589384e-16,
+                    "velocityX": -4.055356890456809,
+                    "velocityY": 1.0195273782422736,
+                    "timestamp": 3.79895665489964
+                },
+                {
+                    "x": 4.870807647705078,
+                    "y": 1.8692435026168823,
+                    "heading": 8.0561270048309e-28,
+                    "angularVelocity": -6.922612455485455e-13,
+                    "velocityX": -3.917554277839153,
+                    "velocityY": 1.4622325191625807,
+                    "timestamp": 3.874896381274637
+                },
+                {
+                    "x": 4.665184835706644,
+                    "y": 1.965165570466657,
+                    "heading": -6.025901704154455e-8,
+                    "angularVelocity": -0.0000011105355093099177,
+                    "velocityX": -3.7894981607646425,
+                    "velocityY": 1.7677829427613814,
+                    "timestamp": 3.929157606184941
+                },
+                {
+                    "x": 4.467801167505074,
+                    "y": 2.0770649526386022,
+                    "heading": -1.1951961632035804e-7,
+                    "angularVelocity": -0.000001092135302451678,
+                    "velocityX": -3.6376559601787903,
+                    "velocityY": 2.062234723910474,
+                    "timestamp": 3.983418831095245
+                },
+                {
+                    "x": 4.282026375471247,
+                    "y": 2.202504243233706,
+                    "heading": -0.0074900260123963456,
+                    "angularVelocity": -0.13803423172184429,
+                    "velocityX": -3.423711726761048,
+                    "velocityY": 2.31176665846485,
+                    "timestamp": 4.0376800560055495
+                },
+                {
+                    "x": 4.109553140955502,
+                    "y": 2.3223494763438204,
+                    "heading": -0.05391384347341113,
+                    "angularVelocity": -0.855561545795458,
+                    "velocityX": -3.178572448389246,
+                    "velocityY": 2.2086717229149047,
+                    "timestamp": 4.0919412809158535
+                },
+                {
+                    "x": 3.9503579748540636,
+                    "y": 2.43525476975937,
+                    "heading": -0.1359907520094868,
+                    "angularVelocity": -1.5126254276742177,
+                    "velocityX": -2.933866059319426,
+                    "velocityY": 2.080773030874018,
+                    "timestamp": 4.146202505826158
+                },
+                {
+                    "x": 3.8058392953370093,
+                    "y": 2.5380816662907524,
+                    "heading": -0.22641050474906085,
+                    "angularVelocity": -1.6663787610589593,
+                    "velocityX": -2.6633877092887084,
+                    "velocityY": 1.8950345610767008,
+                    "timestamp": 4.200463730736462
+                },
+                {
+                    "x": 3.6760560842265666,
+                    "y": 2.630611385450664,
+                    "heading": -0.3168870123533691,
+                    "angularVelocity": -1.6674247172611631,
+                    "velocityX": -2.3918223616399943,
+                    "velocityY": 1.7052641054982942,
+                    "timestamp": 4.254724955646766
+                },
+                {
+                    "x": 3.5609332686690274,
+                    "y": 2.7128111323244637,
+                    "heading": -0.40322451991269215,
+                    "angularVelocity": -1.5911455685352158,
+                    "velocityX": -2.1216405591256335,
+                    "velocityY": 1.5148892604190034,
+                    "timestamp": 4.30898618055707
+                },
+                {
+                    "x": 3.4603899159226676,
+                    "y": 2.7846878010678795,
+                    "heading": -0.4828129395591552,
+                    "angularVelocity": -1.4667641539243872,
+                    "velocityX": -1.8529502957694302,
+                    "velocityY": 1.3246414702622478,
+                    "timestamp": 4.363247405467374
+                },
+                {
+                    "x": 3.3743546742713244,
+                    "y": 2.846257792560545,
+                    "heading": -0.5538376442912536,
+                    "angularVelocity": -1.308940313262462,
+                    "velocityX": -1.585574999340014,
+                    "velocityY": 1.1346959379269979,
+                    "timestamp": 4.417508630377678
+                },
+                {
+                    "x": 3.302766303961918,
+                    "y": 2.8975376071130285,
+                    "heading": -0.6149635904061515,
+                    "angularVelocity": -1.126512462922489,
+                    "velocityX": -1.3193283127637558,
+                    "velocityY": 0.9450544958623966,
+                    "timestamp": 4.471769855287982
+                },
+                {
+                    "x": 3.245572544711835,
+                    "y": 2.938541171103499,
+                    "heading": -0.6651772993510998,
+                    "angularVelocity": -0.925406844905429,
+                    "velocityX": -1.0540447500886079,
+                    "velocityY": 0.7556697081249255,
+                    "timestamp": 4.526031080198286
+                },
+                {
+                    "x": 3.202728904985161,
+                    "y": 2.9692797964201003,
+                    "heading": -0.7036896903252039,
+                    "angularVelocity": -0.7097589676194475,
+                    "velocityX": -0.7895811382344551,
+                    "velocityY": 0.5664933913197379,
+                    "timestamp": 4.58029230510859
+                },
+                {
+                    "x": 3.1741974612408677,
+                    "y": 2.989763077707769,
+                    "heading": -0.7298679174991396,
+                    "angularVelocity": -0.48244814261398083,
+                    "velocityX": -0.5258164332164795,
+                    "velocityY": 0.377493897742418,
+                    "timestamp": 4.634553530018894
+                },
+                {
+                    "x": 3.1599457263946533,
+                    "y": 3,
+                    "heading": -0.7431853071795862,
+                    "angularVelocity": -0.24543105509432933,
+                    "velocityX": -0.2626504445812457,
+                    "velocityY": 0.18865999264028846,
+                    "timestamp": 4.688814754929198
+                },
+                {
+                    "x": 3.1599457263946533,
+                    "y": 3,
+                    "heading": -0.7431853071795862,
+                    "angularVelocity": -2.718854590960149e-28,
+                    "velocityX": -1.846408155148528e-27,
+                    "velocityY": 4.7057945380444184e-27,
+                    "timestamp": 4.743075979839502
+                },
+                {
+                    "x": 3.1707764655908464,
+                    "y": 2.9881322043781373,
+                    "heading": -0.732035938934007,
+                    "angularVelocity": 0.21513876739040086,
+                    "velocityX": 0.2089904853146997,
+                    "velocityY": -0.22900157798098394,
+                    "timestamp": 4.794900057762878
+                },
+                {
+                    "x": 3.1926755779502765,
+                    "y": 2.9645842050264797,
+                    "heading": -0.7101668692970703,
+                    "angularVelocity": 0.4219866616685564,
+                    "velocityX": 0.42256636754461707,
+                    "velocityY": -0.4543833734287422,
+                    "timestamp": 4.846724135686254
+                },
+                {
+                    "x": 3.2259183969876806,
+                    "y": 2.9295838122377513,
+                    "heading": -0.6780905954162056,
+                    "angularVelocity": 0.6189453853533228,
+                    "velocityX": 0.6414550990478732,
+                    "velocityY": -0.6753693300723632,
+                    "timestamp": 4.89854821360963
+                },
+                {
+                    "x": 3.2708261994975825,
+                    "y": 2.8834123046214346,
+                    "heading": -0.6364219103258802,
+                    "angularVelocity": 0.8040410318912782,
+                    "velocityX": 0.8665432036494631,
+                    "velocityY": -0.8909277205970338,
+                    "timestamp": 4.950372291533006
+                },
+                {
+                    "x": 3.327776902619988,
+                    "y": 2.826423972607863,
+                    "heading": -0.5859053180091909,
+                    "angularVelocity": 0.9747706923291543,
+                    "velocityX": 1.0989236162891207,
+                    "velocityY": -1.0996497052553655,
+                    "timestamp": 5.0021963694563825
+                },
+                {
+                    "x": 3.3972187070235824,
+                    "y": 2.759075775607749,
+                    "heading": -0.5274547304559486,
+                    "angularVelocity": 1.127865461295109,
+                    "velocityX": 1.3399525314520095,
+                    "velocityY": -1.2995541782661457,
+                    "timestamp": 5.0540204473797585
+                },
+                {
+                    "x": 3.4796866107524327,
+                    "y": 2.6819736466469406,
+                    "heading": -0.46221377382092116,
+                    "angularVelocity": 1.25889276277117,
+                    "velocityX": 1.591304795635387,
+                    "velocityY": -1.4877665372996447,
+                    "timestamp": 5.105844525303135
+                },
+                {
+                    "x": 3.575819480010563,
+                    "y": 2.5959468307633444,
+                    "heading": -0.3916508199590152,
+                    "angularVelocity": 1.3615862874827402,
+                    "velocityX": 1.8549846540495372,
+                    "velocityY": -1.659977742600455,
+                    "timestamp": 5.157668603226511
+                },
+                {
+                    "x": 3.6863681532781767,
+                    "y": 2.502169766316827,
+                    "heading": -0.31770965056129546,
+                    "angularVelocity": 1.426772503449932,
+                    "velocityX": 2.133152729336816,
+                    "velocityY": -1.8095269265604703,
+                    "timestamp": 5.209492681149887
+                },
+                {
+                    "x": 3.812163930499293,
+                    "y": 2.4023610752274767,
+                    "heading": -0.24304232225652234,
+                    "angularVelocity": 1.4407845020450063,
+                    "velocityX": 2.427361609927921,
+                    "velocityY": -1.925913495980028,
+                    "timestamp": 5.261316759073263
+                },
+                {
+                    "x": 3.953961837508726,
+                    "y": 2.299083056269231,
+                    "heading": -0.17136133589722868,
+                    "angularVelocity": 1.3831598984795583,
+                    "velocityX": 2.736139506796171,
+                    "velocityY": -1.9928578200840514,
+                    "timestamp": 5.313140836996639
+                },
+                {
+                    "x": 4.111973704806276,
+                    "y": 2.196045409033803,
+                    "heading": -0.1079080247435874,
+                    "angularVelocity": 1.224398266139137,
+                    "velocityX": 3.0490048955849653,
+                    "velocityY": -1.9882195952964754,
+                    "timestamp": 5.364964914920015
+                },
+                {
+                    "x": 4.285069061395649,
+                    "y": 2.097867124268194,
+                    "heading": -0.05873651399697375,
+                    "angularVelocity": 0.9488159310680954,
+                    "velocityX": 3.3400566594798016,
+                    "velocityY": -1.8944530939994642,
+                    "timestamp": 5.416788992843391
+                },
+                {
+                    "x": 4.470523752035761,
+                    "y": 2.009092656335386,
+                    "heading": -0.026966372471160117,
+                    "angularVelocity": 0.6130382401166306,
+                    "velocityX": 3.578542987572568,
+                    "velocityY": -1.7129965739875637,
+                    "timestamp": 5.468613070766767
+                },
+                {
+                    "x": 4.6664061186047086,
+                    "y": 1.9323261388767596,
+                    "heading": -0.008453101034109357,
+                    "angularVelocity": 0.3572330117368095,
+                    "velocityX": 3.7797559439179373,
+                    "velocityY": -1.4812905609652864,
+                    "timestamp": 5.520437148690143
+                },
+                {
+                    "x": 4.870807647705078,
+                    "y": 1.8692435026168823,
+                    "heading": 5.290053653169388e-26,
+                    "angularVelocity": 0.16311146040278007,
+                    "velocityX": 3.9441421302774584,
+                    "velocityY": -1.2172457048468346,
+                    "timestamp": 5.572261226613519
+                },
+                {
+                    "x": 5.017762639572388,
+                    "y": 1.8322127421815186,
+                    "heading": 0.001461003033461962,
+                    "angularVelocity": 0.04008169041354506,
+                    "velocityX": 4.031616878161617,
+                    "velocityY": -1.0159153961722736,
+                    "timestamp": 5.608711860765622
+                },
+                {
+                    "x": 5.167108391738545,
+                    "y": 1.8029062751901384,
+                    "heading": 0.0007309116487781689,
+                    "angularVelocity": -0.020029593494512152,
+                    "velocityX": 4.097205868708966,
+                    "velocityY": -0.804004310846535,
+                    "timestamp": 5.645162494917725
+                },
+                {
+                    "x": 5.3178754828089545,
+                    "y": 1.781586355190015,
+                    "heading": 0.00021715295944246574,
+                    "angularVelocity": -0.01409464337964255,
+                    "velocityX": 4.136199398926305,
+                    "velocityY": -0.5848984659953738,
+                    "timestamp": 5.681613129069827
+                },
+                {
+                    "x": 5.469655961827876,
+                    "y": 1.7683146760745116,
+                    "heading": 0.000005188258876374356,
+                    "angularVelocity": -0.00581511695191908,
+                    "velocityX": 4.1640010537421,
+                    "velocityY": -0.36410008835848723,
+                    "timestamp": 5.71806376322193
+                },
+                {
+                    "x": 5.621987131567847,
+                    "y": 1.7631363342262816,
+                    "heading": 0.0000016303066679806213,
+                    "angularVelocity": -0.00009761015936093163,
+                    "velocityX": 4.179108903958118,
+                    "velocityY": -0.14206452010194576,
+                    "timestamp": 5.754514397374033
+                },
+                {
+                    "x": 5.774378851652096,
+                    "y": 1.7660700536906622,
+                    "heading": 0.000001013880704578562,
+                    "angularVelocity": -0.00001691125484483525,
+                    "velocityX": 4.180770064200893,
+                    "velocityY": 0.08048473044772596,
+                    "timestamp": 5.790965031526135
+                },
+                {
+                    "x": 5.92639870788874,
+                    "y": 1.7771075275794117,
+                    "heading": 6.402152729336533e-7,
+                    "angularVelocity": -0.000010251273821071673,
+                    "velocityX": 4.1705682156938835,
+                    "velocityY": 0.3028060867937732,
+                    "timestamp": 5.827415665678238
+                },
+                {
+                    "x": 6.077616038227422,
+                    "y": 1.7962174809956766,
+                    "heading": 3.7516129965530495e-7,
+                    "angularVelocity": -0.000007271587434455114,
+                    "velocityX": 4.148551427327075,
+                    "velocityY": 0.524269436205752,
+                    "timestamp": 5.863866299830341
+                },
+                {
+                    "x": 6.227602479279352,
+                    "y": 1.8233457636897183,
+                    "heading": 1.6965700440858202e-7,
+                    "angularVelocity": -0.000005637879834660427,
+                    "velocityX": 4.114782761420892,
+                    "velocityY": 0.7442472079042579,
+                    "timestamp": 5.9003169339824435
+                },
+                {
+                    "x": 6.3759331703186035,
+                    "y": 1.8584154844284055,
+                    "heading": -5.246500142800949e-26,
+                    "angularVelocity": -0.000004654432175325993,
+                    "velocityX": 4.0693583113065195,
+                    "velocityY": 0.9621155174515489,
+                    "timestamp": 5.936767568134546
+                },
+                {
+                    "x": 6.630268920645483,
+                    "y": 1.9443807518692906,
+                    "heading": -2.614202058840289e-14,
+                    "angularVelocity": -4.071729614915087e-13,
+                    "velocityX": 3.961386241119695,
+                    "velocityY": 1.3389451825660532,
+                    "timestamp": 6.000971292499225
+                },
+                {
+                    "x": 6.86532424088555,
+                    "y": 2.033995146724681,
+                    "heading": -1.479716531157944e-14,
+                    "angularVelocity": 1.7670089062722164e-13,
+                    "velocityX": 3.661085436491938,
+                    "velocityY": 1.3957818762409848,
+                    "timestamp": 6.065175016863903
+                },
+                {
+                    "x": 7.076902889512762,
+                    "y": 2.114538206034381,
+                    "heading": -5.459326745393024e-15,
+                    "angularVelocity": 1.4544076155375644e-13,
+                    "velocityX": 3.2954264058801392,
+                    "velocityY": 1.2544920112766957,
+                    "timestamp": 6.129378741228582
+                },
+                {
+                    "x": 7.265004863010539,
+                    "y": 2.1860098840684987,
+                    "heading": 1.931554215731149e-15,
+                    "angularVelocity": 1.1511607830012644e-13,
+                    "velocityX": 2.9297673204961416,
+                    "velocityY": 1.1132014340501033,
+                    "timestamp": 6.19358246559326
+                },
+                {
+                    "x": 7.429630160202282,
+                    "y": 2.248410165595121,
+                    "heading": 7.392353831711916e-15,
+                    "angularVelocity": 8.50542498899784e-14,
+                    "velocityX": 2.564108216786111,
+                    "velocityY": 0.9719106195800618,
+                    "timestamp": 6.257786189957939
+                },
+                {
+                    "x": 7.570778780498211,
+                    "y": 2.3017390430021187,
+                    "heading": 1.0931792990306175e-14,
+                    "angularVelocity": 5.5128252973146285e-14,
+                    "velocityX": 2.198449103890015,
+                    "velocityY": 0.8306196865479084,
+                    "timestamp": 6.321989914322617
+                },
+                {
+                    "x": 7.6884507235438,
+                    "y": 2.345996511723915,
+                    "heading": 1.255617668617468e-14,
+                    "angularVelocity": 2.5300458998949426e-14,
+                    "velocityX": 1.8327899854720402,
+                    "velocityY": 0.689328682404981,
+                    "timestamp": 6.3861936386872955
+                },
+                {
+                    "x": 7.782645989102363,
+                    "y": 2.3811825687176684,
+                    "heading": 1.2272281762787649e-14,
+                    "angularVelocity": -4.4217827890265095e-15,
+                    "velocityX": 1.467130863367541,
+                    "velocityY": 0.5480376308685124,
+                    "timestamp": 6.450397363051974
+                },
+                {
+                    "x": 7.853364577004646,
+                    "y": 2.407297211810409,
+                    "heading": 1.0083593884875461e-14,
+                    "angularVelocity": -3.408973388335696e-14,
+                    "velocityX": 1.1014717386268686,
+                    "velocityY": 0.4067465454871274,
+                    "timestamp": 6.5146010874166524
+                },
+                {
+                    "x": 7.900606487123601,
+                    "y": 2.4243404393726915,
+                    "heading": 5.993744788639443e-15,
+                    "angularVelocity": -6.370111915946951e-14,
+                    "velocityX": 0.7358126119073701,
+                    "velocityY": 0.26545543472644506,
+                    "timestamp": 6.578804811781331
+                },
+                {
+                    "x": 7.924371719360352,
+                    "y": 2.432312250137329,
+                    "heading": 3.300747525630688e-29,
+                    "angularVelocity": -9.33550950190197e-14,
+                    "velocityX": 0.3701534836478256,
+                    "velocityY": 0.12416430422879107,
+                    "timestamp": 6.643008536146009
+                },
+                {
+                    "x": 7.922491017378158,
+                    "y": 2.4302702205202578,
+                    "heading": -8.756695320067185e-15,
+                    "angularVelocity": -1.2558246039990905e-13,
+                    "velocityX": -0.02697172547062432,
+                    "velocityY": -0.0292853746930814,
+                    "timestamp": 6.7127371852916164
+                },
+                {
+                    "x": 7.892909500405094,
+                    "y": 2.4175537778122855,
+                    "heading": -1.9591167293870617e-14,
+                    "angularVelocity": -1.553804943385475e-13,
+                    "velocityX": -0.4242376316697698,
+                    "velocityY": -0.1823704153714205,
+                    "timestamp": 6.7824658344372235
+                },
+                {
+                    "x": 7.835614963213244,
+                    "y": 2.3941946511690215,
+                    "heading": -3.229380659597313e-14,
+                    "angularVelocity": -1.8217245648308838e-13,
+                    "velocityX": -0.8216785767957127,
+                    "velocityY": -0.33500041847197504,
+                    "timestamp": 6.852194483582831
+                },
+                {
+                    "x": 7.750591807680805,
+                    "y": 2.3602335487177615,
+                    "heading": -4.660838607004271e-14,
+                    "angularVelocity": -2.0528978618498788e-13,
+                    "velocityX": -1.2193432193830964,
+                    "velocityY": -0.48704661380062597,
+                    "timestamp": 6.921923132728438
+                },
+                {
+                    "x": 7.6378194003421855,
+                    "y": 2.3157245966907354,
+                    "heading": -6.217925570771978e-14,
+                    "angularVelocity": -2.23306629749304e-13,
+                    "velocityX": -1.6173037728456214,
+                    "velocityY": -0.6383165681882451,
+                    "timestamp": 6.991651781874045
+                },
+                {
+                    "x": 7.4972691666560305,
+                    "y": 2.2607432816644524,
+                    "heading": -7.852975653361751e-14,
+                    "angularVelocity": -2.3448756036783125e-13,
+                    "velocityX": -2.015674122592836,
+                    "velocityY": -0.7885039463688946,
+                    "timestamp": 7.061380431019652
+                },
+                {
+                    "x": 7.328898919845841,
+                    "y": 2.195402199778693,
+                    "heading": -9.493163002335684e-14,
+                    "angularVelocity": -2.3522431153784627e-13,
+                    "velocityX": -2.414649485874868,
+                    "velocityY": -0.9370765486839444,
+                    "timestamp": 7.131109080165259
+                },
+                {
+                    "x": 7.132640115356717,
+                    "y": 2.1198873257690747,
+                    "heading": -1.1019249654185571e-13,
+                    "angularVelocity": -2.1886077968657218e-13,
+                    "velocityX": -2.8146078676972013,
+                    "velocityY": -1.082982030125505,
+                    "timestamp": 7.200837729310866
+                },
+                {
+                    "x": 6.908362069234848,
+                    "y": 2.034564322766841,
+                    "heading": -1.2195634420509482e-13,
+                    "angularVelocity": -1.6870895689767396e-13,
+                    "velocityX": -3.2164404282884083,
+                    "velocityY": -1.22364342415501,
+                    "timestamp": 7.270566378456473
+                },
+                {
+                    "x": 6.65571980868215,
+                    "y": 1.940480754591585,
+                    "heading": -1.2356989243287506e-13,
+                    "angularVelocity": -2.314039132481892e-14,
+                    "velocityX": -3.6232203498612474,
+                    "velocityY": -1.3492813833061756,
+                    "timestamp": 7.34029502760208
+                },
+                {
+                    "x": 6.3759331703186035,
+                    "y": 1.8584154844284055,
+                    "heading": -5.564026809490533e-25,
+                    "angularVelocity": 1.7721538269625782e-12,
+                    "velocityX": -4.012506219348913,
+                    "velocityY": -1.176923275708542,
+                    "timestamp": 7.410023676747687
+                },
+                {
+                    "x": 6.227730472222822,
+                    "y": 1.8233818050436732,
+                    "heading": 8.669024554559813e-8,
+                    "angularVelocity": 0.0000023803669135590183,
+                    "velocityX": -4.069394391804057,
+                    "velocityY": -0.9619653369627125,
+                    "timestamp": 7.446442535139557
+                },
+                {
+                    "x": 6.077875008438939,
+                    "y": 1.7962757602738582,
+                    "heading": 1.8100925853170345e-7,
+                    "angularVelocity": 0.0000025898399112686397,
+                    "velocityX": -4.114776530648671,
+                    "velocityY": -0.7442859542205248,
+                    "timestamp": 7.482861393531427
+                },
+                {
+                    "x": 5.926790527292132,
+                    "y": 1.777174025334229,
+                    "heading": 2.8543132587338983e-7,
+                    "angularVelocity": 0.0000028672526254968387,
+                    "velocityX": -4.148523260150717,
+                    "velocityY": -0.5245012002873043,
+                    "timestamp": 7.519280251923297
+                },
+                {
+                    "x": 5.774904257799345,
+                    "y": 1.7661306301784576,
+                    "heading": 4.032163201326794e-7,
+                    "angularVelocity": 0.0000032341759039208115,
+                    "velocityX": -4.170539006425706,
+                    "velocityY": -0.30323287558724765,
+                    "timestamp": 7.5556991103151665
+                },
+                {
+                    "x": 5.622645699142081,
+                    "y": 1.7631768121104108,
+                    "heading": 5.387462462721234e-7,
+                    "angularVelocity": 0.0000037214215965017684,
+                    "velocityX": -4.180761434610339,
+                    "velocityY": -0.0811068275744295,
+                    "timestamp": 7.592117968707036
+                },
+                {
+                    "x": 5.470445405221861,
+                    "y": 1.7683209297554319,
+                    "heading": 6.980137884656088e-7,
+                    "angularVelocity": 0.00000437321621890928,
+                    "velocityX": -4.179161583884165,
+                    "velocityY": 0.1412487340945748,
+                    "timestamp": 7.628536827098906
+                },
+                {
+                    "x": 5.3187337669307375,
+                    "y": 1.7815484404811406,
+                    "heading": 8.894300959206496e-7,
+                    "angularVelocity": 0.000005255966713601673,
+                    "velocityX": -4.165743930210383,
+                    "velocityY": 0.363204979776674,
+                    "timestamp": 7.664955685490776
+                },
+                {
+                    "x": 5.167939795459854,
+                    "y": 1.8028219419315428,
+                    "heading": 0.000001125567179046142,
+                    "angularVelocity": 0.0000064839232626305455,
+                    "velocityX": -4.140546357832767,
+                    "velocityY": 0.5841342202849328,
+                    "timestamp": 7.701374543882646
+                },
+                {
+                    "x": 5.018489910628665,
+                    "y": 1.8320812776224336,
+                    "heading": 0.0000014279951382221296,
+                    "angularVelocity": 0.000008304158134827807,
+                    "velocityX": -4.103640021416827,
+                    "velocityY": 0.8034116659028133,
+                    "timestamp": 7.737793402274516
+                },
+                {
+                    "x": 4.870807647705078,
+                    "y": 1.8692435026168823,
+                    "heading": 5.5585714874022985e-25,
+                    "angularVelocity": -0.00003921032128071638,
+                    "velocityX": -4.0551041258491365,
+                    "velocityY": 1.0204115844209116,
+                    "timestamp": 7.774212260666386
+                },
+                {
+                    "x": 4.65506239891728,
+                    "y": 1.942261032325749,
+                    "heading": -0.00511152183249568,
+                    "angularVelocity": -0.0931448282956492,
+                    "velocityX": -3.9314229328313473,
+                    "velocityY": 1.3305636736338056,
+                    "timestamp": 7.829089401201459
+                },
+                {
+                    "x": 4.448768572924891,
+                    "y": 2.030799601998699,
+                    "heading": -0.019590181225012573,
+                    "angularVelocity": -0.2638377155103962,
+                    "velocityX": -3.75919415590793,
+                    "velocityY": 1.6133961939281727,
+                    "timestamp": 7.883966541736533
+                },
+                {
+                    "x": 4.254744422441319,
+                    "y": 2.1321986523330394,
+                    "heading": -0.04966705851292773,
+                    "angularVelocity": -0.5480766124957241,
+                    "velocityX": -3.535609701813157,
+                    "velocityY": 1.8477466089825263,
+                    "timestamp": 7.938843682271607
+                },
+                {
+                    "x": 4.075334891626867,
+                    "y": 2.2420858584907077,
+                    "heading": -0.10369466963717666,
+                    "angularVelocity": -0.984519430084333,
+                    "velocityX": -3.269294446925206,
+                    "velocityY": 2.002422230572227,
+                    "timestamp": 7.99372082280668
+                },
+                {
+                    "x": 3.9133815236402523,
+                    "y": 2.3539668793447595,
+                    "heading": -0.1763064959762628,
+                    "angularVelocity": -1.3231707343183676,
+                    "velocityX": -2.9511991041717054,
+                    "velocityY": 2.0387545663488873,
+                    "timestamp": 8.048597963341754
+                },
+                {
+                    "x": 3.7693508978633568,
+                    "y": 2.462531049535678,
+                    "heading": -0.25707153727623516,
+                    "angularVelocity": -1.4717428880674346,
+                    "velocityX": -2.624601507522793,
+                    "velocityY": 1.9783131761672412,
+                    "timestamp": 8.103475103876828
+                },
+                {
+                    "x": 3.6424532901295272,
+                    "y": 2.564437617081841,
+                    "heading": -0.33942786854657614,
+                    "angularVelocity": -1.5007402074403637,
+                    "velocityX": -2.312394678303705,
+                    "velocityY": 1.8569948534587097,
+                    "timestamp": 8.158352244411903
+                },
+                {
+                    "x": 3.5317645016321078,
+                    "y": 2.6576956051105367,
+                    "heading": -0.419267219922559,
+                    "angularVelocity": -1.4548744813872836,
+                    "velocityX": -2.017029083843642,
+                    "velocityY": 1.6993959072829385,
+                    "timestamp": 8.213229384946978
+                },
+                {
+                    "x": 3.436490735414249,
+                    "y": 2.7410565467131964,
+                    "heading": -0.49379047622142197,
+                    "angularVelocity": -1.3580018122706745,
+                    "velocityX": -1.7361284733297266,
+                    "velocityY": 1.519046743140355,
+                    "timestamp": 8.268106525482052
+                },
+                {
+                    "x": 3.3559896499896893,
+                    "y": 2.8136894224648494,
+                    "heading": -0.5609733911044363,
+                    "angularVelocity": -1.2242422660502008,
+                    "velocityX": -1.4669329458430604,
+                    "velocityY": 1.323554307740043,
+                    "timestamp": 8.322983666017127
+                },
+                {
+                    "x": 3.2897444136235245,
+                    "y": 2.87501111637633,
+                    "heading": -0.6192864512733086,
+                    "angularVelocity": -1.0626111273345644,
+                    "velocityX": -1.2071553969512154,
+                    "velocityY": 1.1174360273434412,
+                    "timestamp": 8.377860806552201
+                },
+                {
+                    "x": 3.2373352351965794,
+                    "y": 2.924594143237033,
+                    "heading": -0.6675358072055315,
+                    "angularVelocity": -0.8792250372700273,
+                    "velocityX": -0.9550275017235731,
+                    "velocityY": 0.9035278875183412,
+                    "timestamp": 8.432737947087276
+                },
+                {
+                    "x": 3.198416700348633,
+                    "y": 2.962113820712204,
+                    "heading": -0.7047663179682083,
+                    "angularVelocity": -0.6784338688143081,
+                    "velocityX": -0.7091939278992236,
+                    "velocityY": 0.683703216117661,
+                    "timestamp": 8.48761508762235
+                },
+                {
+                    "x": 3.172701033774837,
+                    "y": 2.987316560934767,
+                    "heading": -0.7301983883543345,
+                    "angularVelocity": -0.46343650813714726,
+                    "velocityX": -0.46860434641925003,
+                    "velocityY": 0.45925753377136663,
+                    "timestamp": 8.542492228157425
+                },
+                {
+                    "x": 3.1599459648132324,
+                    "y": 3,
+                    "heading": -0.7431853071795862,
+                    "angularVelocity": -0.23665443750574666,
+                    "velocityX": -0.23242954784520237,
+                    "velocityY": 0.23112427035308772,
+                    "timestamp": 8.5973693686925
+                },
+                {
+                    "x": 3.1599459648132324,
+                    "y": 3,
+                    "heading": -0.7431853071795862,
+                    "angularVelocity": -2.558321101390672e-29,
+                    "velocityX": -1.8146718133161694e-27,
+                    "velocityY": -1.261978589298381e-27,
+                    "timestamp": 8.652246509227574
+                },
+                {
+                    "x": 3.174254445852066,
+                    "y": 3.0012413490099736,
+                    "heading": -0.7319622185405995,
+                    "angularVelocity": 0.22829900262778544,
+                    "velocityX": 0.29106176163811776,
+                    "velocityY": 0.025251403602526624,
+                    "timestamp": 8.70140611356139
+                },
+                {
+                    "x": 3.202850559042552,
+                    "y": 3.004179613349041,
+                    "heading": -0.7098801566610144,
+                    "angularVelocity": 0.4491912044213771,
+                    "velocityX": 0.5816994171943644,
+                    "velocityY": 0.059769893978703525,
+                    "timestamp": 8.750565717895206
+                },
+                {
+                    "x": 3.2456916729749725,
+                    "y": 3.009348914628675,
+                    "heading": -0.6773899559870106,
+                    "angularVelocity": 0.6609125747510244,
+                    "velocityX": 0.8714698686651375,
+                    "velocityY": 0.1051534354209254,
+                    "timestamp": 8.799725322229023
+                },
+                {
+                    "x": 3.302701027134029,
+                    "y": 3.0173829462286443,
+                    "heading": -0.6350563836414606,
+                    "angularVelocity": 0.861145505933815,
+                    "velocityX": 1.1596788650278267,
+                    "velocityY": 0.1634275073781015,
+                    "timestamp": 8.848884926562839
+                },
+                {
+                    "x": 3.3737478770473053,
+                    "y": 3.0290415179761236,
+                    "heading": -0.5835964640538578,
+                    "angularVelocity": 1.0467927943066175,
+                    "velocityX": 1.4452282697565253,
+                    "velocityY": 0.2371575586392502,
+                    "timestamp": 8.898044530896655
+                },
+                {
+                    "x": 3.458615323369708,
+                    "y": 3.045243216249596,
+                    "heading": -0.5239353255211855,
+                    "angularVelocity": 1.2136212107718845,
+                    "velocityX": 1.726365528618052,
+                    "velocityY": 0.32957340672345015,
+                    "timestamp": 8.947204135230471
+                },
+                {
+                    "x": 3.5569478978283247,
+                    "y": 3.0671028955065203,
+                    "heading": -0.4572868659527135,
+                    "angularVelocity": 1.3557566313166185,
+                    "velocityX": 2.0002718856501422,
+                    "velocityY": 0.44466751824296896,
+                    "timestamp": 8.996363739564288
+                },
+                {
+                    "x": 3.6681669875857352,
+                    "y": 3.0959670977371183,
+                    "heading": -0.3852650574831731,
+                    "angularVelocity": 1.4650607840632703,
+                    "velocityX": 2.2624081553257263,
+                    "velocityY": 0.5871528589733368,
+                    "timestamp": 9.045523343898104
+                },
+                {
+                    "x": 3.7913406802540934,
+                    "y": 3.1334264012320054,
+                    "heading": -0.31001862368724636,
+                    "angularVelocity": 1.5306558060347588,
+                    "velocityX": 2.5055875517620856,
+                    "velocityY": 0.761993592147766,
+                    "timestamp": 9.09468294823192
+                },
+                {
+                    "x": 3.925009228556469,
+                    "y": 3.1812569342680064,
+                    "heading": -0.23433608155223415,
+                    "angularVelocity": 1.539527080427534,
+                    "velocityX": 2.719072907802623,
+                    "velocityY": 0.9729641579539594,
+                    "timestamp": 9.143842552565737
+                },
+                {
+                    "x": 4.06702482491259,
+                    "y": 3.2412227402695866,
+                    "heading": -0.16161028675015265,
+                    "angularVelocity": 1.479381207143992,
+                    "velocityX": 2.8888677661392754,
+                    "velocityY": 1.219818727473582,
+                    "timestamp": 9.193002156899553
+                },
+                {
+                    "x": 4.214551884068752,
+                    "y": 3.3147260265586795,
+                    "heading": -0.09566981004789035,
+                    "angularVelocity": 1.3413549111277785,
+                    "velocityX": 3.000981418694643,
+                    "velocityY": 1.4951968650921703,
+                    "timestamp": 9.242161761233369
+                },
+                {
+                    "x": 4.364378322917566,
+                    "y": 3.402451984218925,
+                    "heading": -0.04058781830439109,
+                    "angularVelocity": 1.1204726419168822,
+                    "velocityX": 3.047755181905567,
+                    "velocityY": 1.7845130946243346,
+                    "timestamp": 9.291321365567185
+                },
+                {
+                    "x": 4.5134758949279785,
+                    "y": 3.504307985305786,
+                    "heading": 3.3631785914154804e-27,
+                    "angularVelocity": 0.825633543117669,
+                    "velocityX": 3.032928641938876,
+                    "velocityY": 2.0719450953106353,
+                    "timestamp": 9.340480969901002
+                },
+                {
+                    "x": 4.600523359700349,
+                    "y": 3.569285870650025,
+                    "heading": 0.01816599344926272,
+                    "angularVelocity": 0.6249216900973263,
+                    "velocityX": 2.994487967650624,
+                    "velocityY": 2.235280445392861,
+                    "timestamp": 9.369550201673759
+                },
+                {
+                    "x": 4.686546893692203,
+                    "y": 3.6390136130428754,
+                    "heading": 0.030283751359764955,
+                    "angularVelocity": 0.4168585535809847,
+                    "velocityX": 2.9592640997300075,
+                    "velocityY": 2.398678538804583,
+                    "timestamp": 9.398619433446516
+                },
+                {
+                    "x": 4.771736384884454,
+                    "y": 3.7134909076284828,
+                    "heading": 0.035957094180445175,
+                    "angularVelocity": 0.19516658936948508,
+                    "velocityX": 2.930572498723082,
+                    "velocityY": 2.562066144981634,
+                    "timestamp": 9.427688665219273
+                },
+                {
+                    "x": 4.856660629697274,
+                    "y": 3.792667533709231,
+                    "heading": 0.03406209145937424,
+                    "angularVelocity": -0.06518929484909297,
+                    "velocityX": 2.9214478551306744,
+                    "velocityY": 2.723726127325746,
+                    "timestamp": 9.45675789699203
+                },
+                {
+                    "x": 4.94653857071501,
+                    "y": 3.871098031264581,
+                    "heading": 0.0279504475909309,
+                    "angularVelocity": -0.21024442325203974,
+                    "velocityX": 3.0918581447331515,
+                    "velocityY": 2.698058833080388,
+                    "timestamp": 9.485827128764788
+                },
+                {
+                    "x": 5.0396307624325205,
+                    "y": 3.9454940296259062,
+                    "heading": 0.021490360496799883,
+                    "angularVelocity": -0.2222310910942296,
+                    "velocityX": 3.2024304063223243,
+                    "velocityY": 2.5592695033325716,
+                    "timestamp": 9.514896360537545
+                },
+                {
+                    "x": 5.135720972459086,
+                    "y": 4.015693585929082,
+                    "heading": 0.01454272767997795,
+                    "angularVelocity": -0.2390029730105524,
+                    "velocityX": 3.3055641366008026,
+                    "velocityY": 2.414909236403115,
+                    "timestamp": 9.543965592310302
+                },
+                {
+                    "x": 5.234684813317334,
+                    "y": 4.081609056683672,
+                    "heading": 0.007288208288849233,
+                    "angularVelocity": -0.24956006570243372,
+                    "velocityX": 3.404418858808358,
+                    "velocityY": 2.2675339778453596,
+                    "timestamp": 9.57303482408306
+                },
+                {
+                    "x": 5.336422443389893,
+                    "y": 4.143174171447754,
+                    "heading": -2.9790128893571064e-27,
+                    "angularVelocity": -0.25071898513944774,
+                    "velocityX": 3.4998389660885882,
+                    "velocityY": 2.1178789740765374,
+                    "timestamp": 9.602104055855817
+                },
+                {
+                    "x": 5.601458841609157,
+                    "y": 4.267372818196447,
+                    "heading": -0.014576784799321342,
+                    "angularVelocity": -0.2046562124623809,
+                    "velocityX": 3.7210774646787157,
+                    "velocityY": 1.7437332708461397,
+                    "timestamp": 9.67332976957316
+                },
+                {
+                    "x": 5.879046341382613,
+                    "y": 4.363262401476311,
+                    "heading": -0.02679043418253227,
+                    "angularVelocity": -0.17147809050647556,
+                    "velocityX": 3.8972933409281296,
+                    "velocityY": 1.3462776050289633,
+                    "timestamp": 9.744555483290503
+                },
+                {
+                    "x": 6.164615438298519,
+                    "y": 4.429223569887391,
+                    "heading": -0.03945167341125408,
+                    "angularVelocity": -0.1777621952511066,
+                    "velocityX": 4.009353953955124,
+                    "velocityY": 0.926086450643998,
+                    "timestamp": 9.815781197007846
+                },
+                {
+                    "x": 6.44981818392364,
+                    "y": 4.464212612650885,
+                    "heading": -0.051564894166171,
+                    "angularVelocity": -0.17006808528430892,
+                    "velocityX": 4.004210428230155,
+                    "velocityY": 0.4912417291084996,
+                    "timestamp": 9.887006910725189
+                },
+                {
+                    "x": 6.723678067297048,
+                    "y": 4.470383566214521,
+                    "heading": -0.06187724212677704,
+                    "angularVelocity": -0.14478405932905375,
+                    "velocityX": 3.844958078766487,
+                    "velocityY": 0.08663940649474564,
+                    "timestamp": 9.958232624442532
+                },
+                {
+                    "x": 6.976222932545455,
+                    "y": 4.454096398362661,
+                    "heading": -0.06932431769193947,
+                    "angularVelocity": -0.10455599777793567,
+                    "velocityX": 3.5456979237950708,
+                    "velocityY": -0.22866977390349366,
+                    "timestamp": 10.029458338159875
+                },
+                {
+                    "x": 7.201595271822139,
+                    "y": 4.422975711797285,
+                    "heading": -0.07334279579410785,
+                    "angularVelocity": -0.056418923622381,
+                    "velocityX": 3.164199100497136,
+                    "velocityY": -0.436930498006333,
+                    "timestamp": 10.100684051877218
+                },
+                {
+                    "x": 7.3972346520908046,
+                    "y": 4.383240926841446,
+                    "heading": -0.07372930856311687,
+                    "angularVelocity": -0.005426590325831984,
+                    "velocityX": 2.746752121643222,
+                    "velocityY": -0.5578713484504427,
+                    "timestamp": 10.171909765594561
+                },
+                {
+                    "x": 7.562198130742167,
+                    "y": 4.339357119000387,
+                    "heading": -0.07044283334517008,
+                    "angularVelocity": 0.046141695834583545,
+                    "velocityX": 2.316066347976712,
+                    "velocityY": -0.6161231042936308,
+                    "timestamp": 10.243135479311904
+                },
+                {
+                    "x": 7.696221986430261,
+                    "y": 4.294484349506991,
+                    "heading": -0.06350267472615741,
+                    "angularVelocity": 0.09743894805399134,
+                    "velocityX": 1.8816779600126234,
+                    "velocityY": -0.6300080006424584,
+                    "timestamp": 10.314361193029248
+                },
+                {
+                    "x": 7.799317237709317,
+                    "y": 4.250909609152329,
+                    "heading": -0.05294727053269687,
+                    "angularVelocity": 0.14819653805575428,
+                    "velocityX": 1.4474442711544644,
+                    "velocityY": -0.6117838359273932,
+                    "timestamp": 10.38558690674659
+                },
+                {
+                    "x": 7.871602327588269,
+                    "y": 4.210338954621034,
+                    "heading": -0.03881874513325574,
+                    "angularVelocity": 0.1983627072592022,
+                    "velocityX": 1.0148735071411497,
+                    "velocityY": -0.5696068514286532,
+                    "timestamp": 10.456812620463934
+                },
+                {
+                    "x": 7.913232875268372,
+                    "y": 4.174082836279598,
+                    "heading": -0.021157400982541673,
+                    "angularVelocity": 0.2479630351027785,
+                    "velocityX": 0.5844876170046177,
+                    "velocityY": -0.5090313097502501,
+                    "timestamp": 10.528038334181277
+                },
+                {
+                    "x": 7.924371719360352,
+                    "y": 4.143174171447754,
+                    "heading": -1.0097437960393402e-29,
+                    "angularVelocity": 0.2970472302531645,
+                    "velocityX": 0.15638796033947933,
+                    "velocityY": -0.4339537397196697,
+                    "timestamp": 10.59926404789862
+                },
+                {
+                    "x": 7.901061879854102,
+                    "y": 4.117014647255202,
+                    "heading": 0.026867149185379923,
+                    "angularVelocity": 0.3495205221621958,
+                    "velocityX": -0.3032427154636389,
+                    "velocityY": -0.34031487643917635,
+                    "timestamp": 10.676132636384283
+                },
+                {
+                    "x": 7.842409228188353,
+                    "y": 4.098003471379881,
+                    "heading": 0.0576084810480504,
+                    "angularVelocity": 0.39992059784477624,
+                    "velocityX": -0.7630249601459601,
+                    "velocityY": -0.2473204757606027,
+                    "timestamp": 10.753001224869946
+                },
+                {
+                    "x": 7.7483989682142225,
+                    "y": 4.086076859183639,
+                    "heading": 0.09201917280173504,
+                    "angularVelocity": 0.4476560898487505,
+                    "velocityX": -1.2229996911113385,
+                    "velocityY": -0.15515586315814983,
+                    "timestamp": 10.829869813355609
+                },
+                {
+                    "x": 7.619011693227179,
+                    "y": 4.081149603953839,
+                    "heading": 0.12982621784579915,
+                    "angularVelocity": 0.49183998026860576,
+                    "velocityX": -1.6832268880698338,
+                    "velocityY": -0.06409972300609605,
+                    "timestamp": 10.906738401841272
+                },
+                {
+                    "x": 7.45422085196502,
+                    "y": 4.083102110399606,
+                    "heading": 0.1706474007379839,
+                    "angularVelocity": 0.5310515477957343,
+                    "velocityX": -2.143799496108805,
+                    "velocityY": 0.025400576285222472,
+                    "timestamp": 10.983606990326935
+                },
+                {
+                    "x": 7.253987975144995,
+                    "y": 4.091754328818402,
+                    "heading": 0.2139089238500665,
+                    "angularVelocity": 0.5627984585686955,
+                    "velocityX": -2.6048725593207736,
+                    "velocityY": 0.1125585702722947,
+                    "timestamp": 11.060475578812598
+                },
+                {
+                    "x": 7.018252601475449,
+                    "y": 4.106804485403023,
+                    "heading": 0.2586522067646371,
+                    "angularVelocity": 0.5820749905264067,
+                    "velocityX": -3.0667321764795115,
+                    "velocityY": 0.19579072389794971,
+                    "timestamp": 11.137344167298261
+                },
+                {
+                    "x": 6.746907774405913,
+                    "y": 4.127643106983061,
+                    "heading": 0.30295013129710396,
+                    "angularVelocity": 0.5762812275488699,
+                    "velocityX": -3.5299832144068213,
+                    "velocityY": 0.27109410996826905,
+                    "timestamp": 11.214212755783924
+                },
+                {
+                    "x": 6.439749090740071,
+                    "y": 4.152402452088509,
+                    "heading": 0.3409535387391942,
+                    "angularVelocity": 0.49439450093686,
+                    "velocityX": -3.9958933774766843,
+                    "velocityY": 0.3220996455537399,
+                    "timestamp": 11.291081344269587
+                },
+                {
+                    "x": 6.1183816256646795,
+                    "y": 4.146072576447017,
+                    "heading": 0.3409535574726789,
+                    "angularVelocity": 2.4370793128443506e-7,
+                    "velocityX": -4.180738470764694,
+                    "velocityY": -0.08234671360814934,
+                    "timestamp": 11.36794993275525
+                },
+                {
+                    "x": 5.79974773485045,
+                    "y": 4.103769418408104,
+                    "heading": 0.34095356182725745,
+                    "angularVelocity": 5.664964963804072e-8,
+                    "velocityX": -4.145176815282082,
+                    "velocityY": -0.5503308812129776,
+                    "timestamp": 11.444818521240913
+                },
+                {
+                    "x": 5.489237994025465,
+                    "y": 4.026346742277124,
+                    "heading": 0.3360287742583908,
+                    "angularVelocity": -0.06406762067427833,
+                    "velocityX": -4.0394880007832965,
+                    "velocityY": -1.0072082453474474,
+                    "timestamp": 11.521687109726576
+                },
+                {
+                    "x": 5.199567022679866,
+                    "y": 3.922330280425125,
+                    "heading": 0.28775237995316766,
+                    "angularVelocity": -0.6280379964857458,
+                    "velocityX": -3.7683919667605905,
+                    "velocityY": -1.3531725234085639,
+                    "timestamp": 11.59855569821224
+                },
+                {
+                    "x": 4.938883364377543,
+                    "y": 3.798508366694055,
+                    "heading": 0.21191876629762135,
+                    "angularVelocity": -0.9865357898394373,
+                    "velocityX": -3.391289776980151,
+                    "velocityY": -1.6108259065296096,
+                    "timestamp": 11.675424286697902
+                },
+                {
+                    "x": 4.709807126039821,
+                    "y": 3.658502172134417,
+                    "heading": 0.11482976825156727,
+                    "angularVelocity": -1.2630516568437093,
+                    "velocityX": -2.9801020527448228,
+                    "velocityY": -1.8213706966370415,
+                    "timestamp": 11.752292875183565
+                },
+                {
+                    "x": 4.5134758949279785,
+                    "y": 3.504307985305786,
+                    "heading": 1.6717492530612703e-28,
+                    "angularVelocity": -1.4938451520153042,
+                    "velocityX": -2.5541152111627516,
+                    "velocityY": -2.0059453395243487,
+                    "timestamp": 11.829161463669228
+                },
+                {
+                    "x": 4.400933042470042,
+                    "y": 3.3997056562283166,
+                    "heading": -0.08028643635941543,
+                    "angularVelocity": -1.6227759765270617,
+                    "velocityX": -2.2747533155038484,
+                    "velocityY": -2.1142568335677403,
+                    "timestamp": 11.878636214048043
+                },
+                {
+                    "x": 4.302937398056285,
+                    "y": 3.291951339386866,
+                    "heading": -0.16507306663801824,
+                    "angularVelocity": -1.7137353827844055,
+                    "velocityX": -1.9807203404449545,
+                    "velocityY": -2.177965851599104,
+                    "timestamp": 11.928110964426859
+                },
+                {
+                    "x": 4.219869139195696,
+                    "y": 3.1838203852492617,
+                    "heading": -0.2519454962217678,
+                    "angularVelocity": -1.755894247441151,
+                    "velocityX": -1.6790030919722805,
+                    "velocityY": -2.185578569061436,
+                    "timestamp": 11.977585714805674
+                },
+                {
+                    "x": 4.151481762948963,
+                    "y": 3.0784802030686347,
+                    "heading": -0.3381364984876326,
+                    "angularVelocity": -1.7421210133638232,
+                    "velocityX": -1.382268242348026,
+                    "velocityY": -2.1291705642588132,
+                    "timestamp": 12.027060465184489
+                },
+                {
+                    "x": 4.096769953274228,
+                    "y": 2.9790758688649013,
+                    "heading": -0.42082279924897953,
+                    "angularVelocity": -1.671282828680057,
+                    "velocityX": -1.1058531726955891,
+                    "velocityY": -2.0091932438793374,
+                    "timestamp": 12.076535215563304
+                },
+                {
+                    "x": 4.054136857799692,
+                    "y": 2.888285052434389,
+                    "heading": -0.49746057385028863,
+                    "angularVelocity": -1.5490280196365382,
+                    "velocityX": -0.8617142107459757,
+                    "velocityY": -1.8350939769346957,
+                    "timestamp": 12.126009965942119
+                },
+                {
+                    "x": 4.021745584566135,
+                    "y": 2.808098433899372,
+                    "heading": -0.5658222077410688,
+                    "angularVelocity": -1.3817479293448123,
+                    "velocityX": -0.6547031159438974,
+                    "velocityY": -1.6207584256827539,
+                    "timestamp": 12.175484716320934
+                },
+                {
+                    "x": 3.997836195630109,
+                    "y": 2.7398606697094916,
+                    "heading": -0.6241866316468845,
+                    "angularVelocity": -1.1796810182756619,
+                    "velocityX": -0.48326446829864,
+                    "velocityY": -1.3792442340264681,
+                    "timestamp": 12.22495946669975
+                },
+                {
+                    "x": 3.9808795710102305,
+                    "y": 2.684447813014619,
+                    "heading": -0.6715007313655615,
+                    "angularVelocity": -0.9563282150269545,
+                    "velocityX": -0.3427328989039043,
+                    "velocityY": -1.1200229666767407,
+                    "timestamp": 12.274434217078564
+                },
+                {
+                    "x": 3.9695943024140083,
+                    "y": 2.6424340619644666,
+                    "heading": -0.7072413229342775,
+                    "angularVelocity": -0.722400644673479,
+                    "velocityX": -0.22810157726544558,
+                    "velocityY": -0.8491958166229038,
+                    "timestamp": 12.32390896745738
+                },
+                {
+                    "x": 3.962913327171321,
+                    "y": 2.61420036575793,
+                    "heading": -0.7311750968478933,
+                    "angularVelocity": -0.48375734552192345,
+                    "velocityX": -0.13503807884896393,
+                    "velocityY": -0.5706687955039338,
+                    "timestamp": 12.373383717836195
+                },
+                {
+                    "x": 3.959945964813233,
+                    "y": 2.5999999999999996,
+                    "heading": -0.7431853071795862,
+                    "angularVelocity": -0.2427543391272071,
+                    "velocityX": -0.059977308331374074,
+                    "velocityY": -0.2870224841803447,
+                    "timestamp": 12.42285846821501
+                },
+                {
+                    "x": 3.959945964813233,
+                    "y": 2.5999999999999996,
+                    "heading": -0.7431853071795862,
+                    "angularVelocity": -6.99438774695564e-29,
+                    "velocityX": 3.9418487096305405e-23,
+                    "velocityY": -1.9709900600874517e-23,
+                    "timestamp": 12.472333218593825
+                },
+                {
+                    "x": 3.909945960323692,
+                    "y": 2.62500000224477,
+                    "heading": -0.7431853071795862,
+                    "angularVelocity": 4.859579089269393e-17,
+                    "velocityX": -0.522546012525329,
+                    "velocityY": 0.2612730062626647,
+                    "timestamp": 12.568018580822319
+                },
+                {
+                    "x": 3.8099459537390326,
+                    "y": 2.6750000055371,
+                    "heading": -0.7431853071795862,
+                    "angularVelocity": 1.916132911010009e-16,
+                    "velocityX": -1.0450920000267447,
+                    "velocityY": 0.5225460000133729,
+                    "timestamp": 12.663703943050812
+                },
+                {
+                    "x": 3.6599459506462395,
+                    "y": 2.7500000070834965,
+                    "heading": -0.7431853071795862,
+                    "angularVelocity": 2.752945533306333e-16,
+                    "velocityX": -1.5676379291390343,
+                    "velocityY": 0.7838189645695182,
+                    "timestamp": 12.759389305279306
+                },
+                {
+                    "x": 3.4599459789802265,
+                    "y": 2.849999992916503,
+                    "heading": -0.7431853071795862,
+                    "angularVelocity": 3.1955367206396795e-16,
+                    "velocityX": -2.090183566305787,
+                    "velocityY": 1.0450917831528945,
+                    "timestamp": 12.8550746675078
+                },
+                {
+                    "x": 3.309945975887433,
+                    "y": 2.9249999944628997,
+                    "heading": -0.7431853071795862,
+                    "angularVelocity": 2.9470794803491527e-16,
+                    "velocityX": -1.5676379291390345,
+                    "velocityY": 0.783818964569518,
+                    "timestamp": 12.950760029736294
+                },
+                {
+                    "x": 3.2099459693027734,
+                    "y": 2.9749999977552295,
+                    "heading": -0.7431853071795862,
+                    "angularVelocity": 2.1840033759327932e-16,
+                    "velocityX": -1.0450920000267447,
+                    "velocityY": 0.5225460000133729,
+                    "timestamp": 13.046445391964788
+                },
+                {
+                    "x": 3.1599459648132324,
+                    "y": 3,
+                    "heading": -0.7431853071795862,
+                    "angularVelocity": 1.4984441245497084e-16,
+                    "velocityX": -0.522546012525329,
+                    "velocityY": 0.26127300626266475,
+                    "timestamp": 13.142130754193282
+                },
+                {
+                    "x": 3.1599459648132324,
+                    "y": 3,
+                    "heading": -0.7431853071795862,
+                    "angularVelocity": -1.1440552586496935e-29,
+                    "velocityX": -3.941408736271529e-23,
+                    "velocityY": 1.9707570184447103e-23,
+                    "timestamp": 13.237816116421776
+                }
+            ],
+            "trajectoryWaypoints": [
+                {
+                    "timestamp": 0,
+                    "isStopPoint": true,
+                    "x": 0.6802782416343689,
+                    "y": 4.38139533996582,
+                    "heading": -1.0378726274695866,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "timestamp": 1.3444417435934044,
+                    "isStopPoint": false,
+                    "x": 3.517277717590332,
+                    "y": 1.2412056922912598,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "timestamp": 2.7358004856496816,
+                    "isStopPoint": false,
+                    "x": 7.967684268951416,
+                    "y": 0.8080758452415466,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "timestamp": 3.874896381274637,
+                    "isStopPoint": false,
+                    "x": 4.870807647705078,
+                    "y": 1.8692435026168823,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "timestamp": 4.743075979839502,
+                    "isStopPoint": true,
+                    "x": 3.1599457263946533,
+                    "y": 3,
+                    "heading": -0.7431853071795862,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "timestamp": 5.572261226613519,
+                    "isStopPoint": false,
+                    "x": 4.870807647705078,
+                    "y": 1.8692435026168823,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "timestamp": 5.936767568134546,
+                    "isStopPoint": false,
+                    "x": 6.3759331703186035,
+                    "y": 1.8584154844284058,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 11
+                },
+                {
+                    "timestamp": 6.643008536146009,
+                    "isStopPoint": false,
+                    "x": 7.924371719360352,
+                    "y": 2.432312250137329,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 11
+                },
+                {
+                    "timestamp": 7.410023676747687,
+                    "isStopPoint": false,
+                    "x": 6.3759331703186035,
+                    "y": 1.8584154844284058,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "timestamp": 7.774212260666386,
+                    "isStopPoint": false,
+                    "x": 4.870807647705078,
+                    "y": 1.8692435026168823,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "timestamp": 8.652246509227574,
+                    "isStopPoint": true,
+                    "x": 3.1599459648132324,
+                    "y": 3,
+                    "heading": -0.7431853071795862,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "timestamp": 9.340480969901002,
+                    "isStopPoint": false,
+                    "x": 4.5134758949279785,
+                    "y": 3.504307985305786,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 9
+                },
+                {
+                    "timestamp": 9.602104055855817,
+                    "isStopPoint": false,
+                    "x": 5.336422443389893,
+                    "y": 4.143174171447754,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "timestamp": 10.59926404789862,
+                    "isStopPoint": false,
+                    "x": 7.924371719360352,
+                    "y": 4.143174171447754,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "timestamp": 11.829161463669228,
+                    "isStopPoint": false,
+                    "x": 4.5134758949279785,
+                    "y": 3.504307985305786,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "timestamp": 12.472333218593825,
+                    "isStopPoint": true,
+                    "x": 3.959945964813233,
+                    "y": 2.5999999999999996,
+                    "heading": -0.7431853071795862,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 8
+                },
+                {
+                    "timestamp": 13.237816116421776,
+                    "isStopPoint": true,
+                    "x": 3.1599459648132324,
+                    "y": 3,
+                    "heading": -0.7431853071795862,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        4
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        10
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        15
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": [],
+            "eventMarkers": [],
+            "isTrajectoryStale": true
+        },
+        "Distance Center 6": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 2.542736291885376,
+                    "y": 6.991001605987549,
+                    "heading": 0.610560190519,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "x": 7.946028232574463,
+                    "y": 7.456615924835205,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 6.018601417541504,
+                    "y": 6.676982402801514,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 7.891887187957764,
+                    "y": 5.810723304748535,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 6.018601417541504,
+                    "y": 6.676982402801514,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 1.990496039390564,
+                    "y": 6.157227039337158,
+                    "heading": -0.6763337151495863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 11
+                },
+                {
+                    "x": 2.4885952472686768,
+                    "y": 5.583330154418945,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 1.990496039390564,
+                    "y": 4.75,
+                    "heading": -0.6763337151495863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 7
+                },
+                {
+                    "x": 2.477766990661621,
+                    "y": 4.240628242492676,
+                    "heading": -0.6763337151495863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 1.823684249569662e-28,
+                    "angularVelocity": 1.4442982636716686e-28,
+                    "velocityX": -9.525967417566346e-28,
+                    "velocityY": -1.281458100821258e-28,
+                    "timestamp": 0
+                },
+                {
+                    "x": 1.3712979426432907,
+                    "y": 5.607416241994932,
+                    "heading": 0.010815420235994461,
+                    "angularVelocity": 0.14684104373961251,
+                    "velocityX": 0.28916233109823775,
+                    "velocityY": 0.3415900192895115,
+                    "timestamp": 0.07365393190185317
+                },
+                {
+                    "x": 1.413894107001652,
+                    "y": 5.657734961213299,
+                    "heading": 0.03244400597180746,
+                    "angularVelocity": 0.2936514749088203,
+                    "velocityX": 0.5783284511561763,
+                    "velocityY": 0.6831776378947206,
+                    "timestamp": 0.14730786380370633
+                },
+                {
+                    "x": 1.4777889053988098,
+                    "y": 5.733212662791941,
+                    "heading": 0.06488357959528479,
+                    "angularVelocity": 0.44043234062105985,
+                    "velocityX": 0.8675001693365169,
+                    "velocityY": 1.02476133493076,
+                    "timestamp": 0.22096179570555952
+                },
+                {
+                    "x": 1.5629828503604866,
+                    "y": 5.8338489085152085,
+                    "heading": 0.10813427799106332,
+                    "angularVelocity": 0.5872150648170668,
+                    "velocityX": 1.1566788460825288,
+                    "velocityY": 1.3663390823095432,
+                    "timestamp": 0.29461572760741267
+                },
+                {
+                    "x": 1.6694764937636932,
+                    "y": 5.959643075254826,
+                    "heading": 0.16220151122014106,
+                    "angularVelocity": 0.73407124145286,
+                    "velocityX": 1.445865015666968,
+                    "velocityY": 1.707908369470939,
+                    "timestamp": 0.3682696595092658
+                },
+                {
+                    "x": 1.7972703292010448,
+                    "y": 6.110594337397887,
+                    "heading": 0.22709931173769698,
+                    "angularVelocity": 0.8811179368405597,
+                    "velocityX": 1.7350578867621362,
+                    "velocityY": 2.0494664472795554,
+                    "timestamp": 0.441923591411119
+                },
+                {
+                    "x": 1.9463646521461748,
+                    "y": 6.286701685728506,
+                    "heading": 0.3028533408965006,
+                    "angularVelocity": 1.0285130366122057,
+                    "velocityX": 2.0242547695051,
+                    "velocityY": 2.391010822956316,
+                    "timestamp": 0.5155775233129721
+                },
+                {
+                    "x": 2.0954593391167733,
+                    "y": 6.462777598713869,
+                    "heading": 0.3796919635365941,
+                    "angularVelocity": 1.0432385706507032,
+                    "velocityX": 2.024259711881685,
+                    "velocityY": 2.390584025031971,
+                    "timestamp": 0.5892314552148253
+                },
+                {
+                    "x": 2.2232538317965016,
+                    "y": 6.613699108565928,
+                    "heading": 0.4456074038458184,
+                    "angularVelocity": 0.8949344401200363,
+                    "velocityX": 1.735066810146942,
+                    "velocityY": 2.0490625001957663,
+                    "timestamp": 0.6628853871166785
+                },
+                {
+                    "x": 2.3297485254769024,
+                    "y": 6.739466694851569,
+                    "heading": 0.5005708250556786,
+                    "angularVelocity": 0.7462387925616956,
+                    "velocityX": 1.4458792752885092,
+                    "velocityY": 1.7075474864428304,
+                    "timestamp": 0.7365393190185318
+                },
+                {
+                    "x": 2.414943830059909,
+                    "y": 6.840080642329488,
+                    "heading": 0.544560026646676,
+                    "angularVelocity": 0.5972417283793449,
+                    "velocityX": 1.1566973056717835,
+                    "velocityY": 1.3660363388609074,
+                    "timestamp": 0.810193250920385
+                },
+                {
+                    "x": 2.4788400911601216,
+                    "y": 6.915541090400898,
+                    "heading": 0.5775596586986871,
+                    "angularVelocity": 0.448036258213403,
+                    "velocityX": 0.8675200284671415,
+                    "velocityY": 1.0245270839303438,
+                    "timestamp": 0.8838471828222382
+                },
+                {
+                    "x": 2.5214375444702517,
+                    "y": 6.9658480830621485,
+                    "heading": 0.5995607651691287,
+                    "angularVelocity": 0.2987091917884169,
+                    "velocityX": 0.578345951264258,
+                    "velocityY": 0.6830184263385521,
+                    "timestamp": 0.9575011147240914
+                },
+                {
+                    "x": 2.542736291885376,
+                    "y": 6.991001605987549,
+                    "heading": 0.610560190519,
+                    "angularVelocity": 0.14933928258614232,
+                    "velocityX": 0.2891732574916107,
+                    "velocityY": 0.34150957424673456,
+                    "timestamp": 1.0311550466259445
+                },
+                {
+                    "x": 2.542736291885376,
+                    "y": 6.991001605987549,
+                    "heading": 0.610560190519,
+                    "angularVelocity": -1.9066998701598976e-24,
+                    "velocityX": -9.94898252689215e-24,
+                    "velocityY": 6.590062338164459e-24,
+                    "timestamp": 1.1048089785277977
+                },
+                {
+                    "x": 2.5860882804994176,
+                    "y": 6.997986329604455,
+                    "heading": 0.6040705470130452,
+                    "angularVelocity": -0.07648715527237229,
+                    "velocityX": 0.5109479868109409,
+                    "velocityY": 0.08232218600769788,
+                    "timestamp": 1.189655165875792
+                },
+                {
+                    "x": 2.6727971429282147,
+                    "y": 7.011957094513919,
+                    "heading": 0.5914830277795109,
+                    "angularVelocity": -0.1483569224142865,
+                    "velocityX": 1.0219535507607762,
+                    "velocityY": 0.16465990218469131,
+                    "timestamp": 1.274501353223786
+                },
+                {
+                    "x": 2.8028689447073436,
+                    "y": 7.0329156007946185,
+                    "heading": 0.5733232484637217,
+                    "angularVelocity": -0.21403176599211712,
+                    "velocityX": 1.533030603315665,
+                    "velocityY": 0.2470176555457783,
+                    "timestamp": 1.3593475405717803
+                },
+                {
+                    "x": 2.976311299631257,
+                    "y": 7.0608641143026825,
+                    "heading": 0.550333584482233,
+                    "angularVelocity": -0.2709569480971165,
+                    "velocityX": 2.044197392306438,
+                    "velocityY": 0.3294021143629483,
+                    "timestamp": 1.4441937279197745
+                },
+                {
+                    "x": 3.1931336411091458,
+                    "y": 7.095805765761892,
+                    "heading": 0.5236415617438719,
+                    "angularVelocity": -0.3145930721540211,
+                    "velocityX": 2.5554753637732515,
+                    "velocityY": 0.4118234719952363,
+                    "timestamp": 1.5290399152677687
+                },
+                {
+                    "x": 3.4533461169129187,
+                    "y": 7.137744979228221,
+                    "heading": 0.49516279138937963,
+                    "angularVelocity": -0.3356517392783657,
+                    "velocityX": 3.0668729372189607,
+                    "velocityY": 0.4942969716991197,
+                    "timestamp": 1.613886102615763
+                },
+                {
+                    "x": 3.7569462055816967,
+                    "y": 7.1866871771577365,
+                    "heading": 0.4688699375076152,
+                    "angularVelocity": -0.3098884546682701,
+                    "velocityX": 3.5782407926424726,
+                    "velocityY": 0.576834380651426,
+                    "timestamp": 1.6987322899637571
+                },
+                {
+                    "x": 4.1036936295460995,
+                    "y": 7.242619011134652,
+                    "heading": 0.4580775990586799,
+                    "angularVelocity": -0.12719886168450764,
+                    "velocityX": 4.086776728602178,
+                    "velocityY": 0.6592144647291258,
+                    "timestamp": 1.7835784773117513
+                },
+                {
+                    "x": 4.453970286773956,
+                    "y": 7.299020832288215,
+                    "heading": 0.45807758779565816,
+                    "angularVelocity": -1.3274634993655314e-7,
+                    "velocityX": 4.128372389807061,
+                    "velocityY": 0.664753749302052,
+                    "timestamp": 1.8684246646597455
+                },
+                {
+                    "x": 4.804246944243858,
+                    "y": 7.355422651938624,
+                    "heading": 0.45807757653265324,
+                    "angularVelocity": -1.3274615245620227e-7,
+                    "velocityX": 4.128372392659827,
+                    "velocityY": 0.6647537315858247,
+                    "timestamp": 1.9532708520077398
+                },
+                {
+                    "x": 5.1545236017137634,
+                    "y": 7.41182447158901,
+                    "heading": 0.4580775652696484,
+                    "angularVelocity": -1.327461517119493e-7,
+                    "velocityX": 4.128372392659871,
+                    "velocityY": 0.66475373158555,
+                    "timestamp": 2.038117039355734
+                },
+                {
+                    "x": 5.5048002591836696,
+                    "y": 7.468226291239396,
+                    "heading": 0.45807755400664346,
+                    "angularVelocity": -1.3274615213673127e-7,
+                    "velocityX": 4.128372392659871,
+                    "velocityY": 0.6647537315855488,
+                    "timestamp": 2.122963226703728
+                },
+                {
+                    "x": 5.855076916654743,
+                    "y": 7.524628110882535,
+                    "heading": 0.4580775427436386,
+                    "angularVelocity": -1.3274615192395048e-7,
+                    "velocityX": 4.128372392673625,
+                    "velocityY": 0.664753731500135,
+                    "timestamp": 2.2078094140517224
+                },
+                {
+                    "x": 6.205353649404039,
+                    "y": 7.5810294630174075,
+                    "heading": 0.45807753148063285,
+                    "angularVelocity": -1.3274616274715188e-7,
+                    "velocityX": 4.128373279905273,
+                    "velocityY": 0.6647482214320765,
+                    "timestamp": 2.2926556013997166
+                },
+                {
+                    "x": 6.558504100556125,
+                    "y": 7.6150832124269465,
+                    "heading": 0.45807750773089817,
+                    "angularVelocity": -2.7991516688896703e-7,
+                    "velocityX": 4.162243021052327,
+                    "velocityY": 0.4013586287604083,
+                    "timestamp": 2.377501788747711
+                },
+                {
+                    "x": 6.882961359930591,
+                    "y": 7.623821022982591,
+                    "heading": 0.38150693231968036,
+                    "angularVelocity": -0.9024633611073862,
+                    "velocityX": 3.8240641037140963,
+                    "velocityY": 0.10298412726321374,
+                    "timestamp": 2.462347976095705
+                },
+                {
+                    "x": 7.164467598037234,
+                    "y": 7.623239648813218,
+                    "heading": 0.3068146622390825,
+                    "angularVelocity": -0.8803255916998344,
+                    "velocityX": 3.317841931447713,
+                    "velocityY": -0.006852095392192507,
+                    "timestamp": 2.547194163443699
+                },
+                {
+                    "x": 7.402821150081126,
+                    "y": 7.614460764318352,
+                    "heading": 0.23751214172935955,
+                    "angularVelocity": -0.8168018231093954,
+                    "velocityX": 2.8092429311678027,
+                    "velocityY": -0.10346822608374351,
+                    "timestamp": 2.6320403507916934
+                },
+                {
+                    "x": 7.597964025430219,
+                    "y": 7.597880541854749,
+                    "heading": 0.1748380247592176,
+                    "angularVelocity": -0.7386792374427614,
+                    "velocityX": 2.299960451360303,
+                    "velocityY": -0.19541505613681964,
+                    "timestamp": 2.7168865381396876
+                },
+                {
+                    "x": 7.749869224899896,
+                    "y": 7.5737014715455055,
+                    "heading": 0.11942485607561616,
+                    "angularVelocity": -0.6531014582461551,
+                    "velocityX": 1.790359758260474,
+                    "velocityY": -0.2849753308309952,
+                    "timestamp": 2.801732725487682
+                },
+                {
+                    "x": 7.8585212115542395,
+                    "y": 7.542046503417778,
+                    "heading": 0.07165557432406013,
+                    "angularVelocity": -0.563010351374208,
+                    "velocityX": 1.2805759463145991,
+                    "velocityY": -0.3730865123956133,
+                    "timestamp": 2.886578912835676
+                },
+                {
+                    "x": 7.923909903080463,
+                    "y": 7.502998223175264,
+                    "heading": 0.03178626375944312,
+                    "angularVelocity": -0.46990102691467095,
+                    "velocityX": 0.7706733038932455,
+                    "velocityY": -0.46022433609607355,
+                    "timestamp": 2.9714251001836702
+                },
+                {
+                    "x": 7.946028232574463,
+                    "y": 7.456615924835205,
+                    "heading": 4.872348893999645e-24,
+                    "angularVelocity": -0.3746339670994609,
+                    "velocityX": 0.2606873707039118,
+                    "velocityY": -0.5466633185274797,
+                    "timestamp": 3.0562712875316644
+                },
+                {
+                    "x": 7.922039225710158,
+                    "y": 7.400326502118091,
+                    "heading": -0.024206358094073104,
+                    "angularVelocity": -0.2736076214341235,
+                    "velocityX": -0.271150872146958,
+                    "velocityY": -0.6362466836884642,
+                    "timestamp": 3.1447423541167057
+                },
+                {
+                    "x": 7.8509861658143585,
+                    "y": 7.336150571120965,
+                    "heading": -0.039787388549924835,
+                    "angularVelocity": -0.17611441861475394,
+                    "velocityX": -0.8031219995239999,
+                    "velocityY": -0.7253889149786428,
+                    "timestamp": 3.233213420701747
+                },
+                {
+                    "x": 7.732853496113255,
+                    "y": 7.264141046256132,
+                    "heading": -0.04716434286147303,
+                    "angularVelocity": -0.08338267635167741,
+                    "velocityX": -1.3352689671436382,
+                    "velocityY": -0.8139330477678314,
+                    "timestamp": 3.321684487286788
+                },
+                {
+                    "x": 7.5676196522986485,
+                    "y": 7.1843737716186435,
+                    "heading": -0.04693804499155485,
+                    "angularVelocity": 0.002557874327203416,
+                    "velocityX": -1.8676596789502777,
+                    "velocityY": -0.9016199048624852,
+                    "timestamp": 3.4101555538718293
+                },
+                {
+                    "x": 7.355252804098637,
+                    "y": 7.096966555657978,
+                    "heading": -0.040037308652199996,
+                    "angularVelocity": 0.07799992252519808,
+                    "velocityX": -2.400410172470089,
+                    "velocityY": -0.9879751576934634,
+                    "timestamp": 3.4986266204568706
+                },
+                {
+                    "x": 7.09570163072694,
+                    "y": 7.002127255801257,
+                    "heading": -0.028090499343208248,
+                    "angularVelocity": 0.13503634318128263,
+                    "velocityX": -2.933740751528163,
+                    "velocityY": -1.071980970926346,
+                    "timestamp": 3.587097687041912
+                },
+                {
+                    "x": 6.78887318109941,
+                    "y": 6.900320277003001,
+                    "heading": -0.014696086966441679,
+                    "angularVelocity": 0.1513987893871656,
+                    "velocityX": -3.4681219688088203,
+                    "velocityY": -1.1507375544116039,
+                    "timestamp": 3.675568753626953
+                },
+                {
+                    "x": 6.434688295787984,
+                    "y": 6.793487938945866,
+                    "heading": -0.014695897490859833,
+                    "angularVelocity": 0.000002141667204427998,
+                    "velocityX": -4.003397935425274,
+                    "velocityY": -1.2075398452944421,
+                    "timestamp": 3.7640398202119942
+                },
+                {
+                    "x": 6.07023453147431,
+                    "y": 6.729977275374465,
+                    "heading": -0.014695885109107147,
+                    "angularVelocity": 1.3995256487596975e-7,
+                    "velocityX": -4.119468413589764,
+                    "velocityY": -0.7178693105316132,
+                    "timestamp": 3.8525108867970355
+                },
+                {
+                    "x": 5.70577998818262,
+                    "y": 6.666471082103607,
+                    "heading": -0.014695872727371504,
+                    "angularVelocity": 1.39952372261179e-7,
+                    "velocityX": -4.119477218479835,
+                    "velocityY": -0.7178187821417759,
+                    "timestamp": 3.9409819533820767
+                },
+                {
+                    "x": 5.341325440621852,
+                    "y": 6.602964913334049,
+                    "heading": -0.014695860345028063,
+                    "angularVelocity": 1.399592422510824e-7,
+                    "velocityX": -4.119477266733788,
+                    "velocityY": -0.7178185052003817,
+                    "timestamp": 4.029453019967118
+                },
+                {
+                    "x": 5.012545454341452,
+                    "y": 6.545672600238492,
+                    "heading": 0.062448454910025174,
+                    "angularVelocity": 0.8719722529952728,
+                    "velocityX": -3.716243049521347,
+                    "velocityY": -0.6475824843875421,
+                    "timestamp": 4.11792408655216
+                },
+                {
+                    "x": 4.730734074716532,
+                    "y": 6.496564334374803,
+                    "heading": 0.12856972259022606,
+                    "angularVelocity": 0.7473773091301318,
+                    "velocityX": -3.18535076497619,
+                    "velocityY": -0.5550771315329972,
+                    "timestamp": 4.206395153137201
+                },
+                {
+                    "x": 4.495891252721745,
+                    "y": 6.455640399436969,
+                    "heading": 0.183666717807647,
+                    "angularVelocity": 0.6227685201970513,
+                    "velocityX": -2.6544590345709054,
+                    "velocityY": -0.4625685720483159,
+                    "timestamp": 4.294866219722243
+                },
+                {
+                    "x": 4.30801697716886,
+                    "y": 6.422901026579304,
+                    "heading": 0.22774087166689647,
+                    "angularVelocity": 0.4981759072259397,
+                    "velocityX": -2.123567430627664,
+                    "velocityY": -0.3700573997962927,
+                    "timestamp": 4.383337286307285
+                },
+                {
+                    "x": 4.167111258222206,
+                    "y": 6.398346386946046,
+                    "heading": 0.26079453276814246,
+                    "angularVelocity": 0.3736098407887256,
+                    "velocityX": -1.5926757117956818,
+                    "velocityY": -0.2775442930786397,
+                    "timestamp": 4.471808352892326
+                },
+                {
+                    "x": 4.073174110202746,
+                    "y": 6.38197659167395,
+                    "heading": 0.28282976197594306,
+                    "angularVelocity": 0.249067068572296,
+                    "velocityX": -1.061783831091989,
+                    "velocityY": -0.18502993016773903,
+                    "timestamp": 4.560279419477368
+                },
+                {
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "angularVelocity": 0.12453576635098247,
+                    "velocityX": -0.5308918758680198,
+                    "velocityY": -0.09251495826569413,
+                    "timestamp": 4.64875048606241
+                },
+                {
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "angularVelocity": -1.7260583733230727e-24,
+                    "velocityX": -2.8721435656913086e-23,
+                    "velocityY": 9.699536718146445e-24,
+                    "timestamp": 4.737221552647451
+                },
+                {
+                    "x": 4.048121646310105,
+                    "y": 6.381836946527732,
+                    "heading": 0.2879220023652063,
+                    "angularVelocity": -0.09569026109228784,
+                    "velocityX": 0.35391656262382837,
+                    "velocityY": 0.12992033412904042,
+                    "timestamp": 4.799146053846991
+                },
+                {
+                    "x": 4.092109802394514,
+                    "y": 6.39750532464796,
+                    "heading": 0.27630914546149715,
+                    "angularVelocity": -0.1875325061769808,
+                    "velocityX": 0.7103513994027256,
+                    "velocityY": 0.25302388903770934,
+                    "timestamp": 4.861070555046531
+                },
+                {
+                    "x": 4.158348380070515,
+                    "y": 6.420279601349659,
+                    "heading": 0.2593011930834734,
+                    "angularVelocity": -0.27465626768989004,
+                    "velocityX": 1.0696667133831064,
+                    "velocityY": 0.36777489136832514,
+                    "timestamp": 4.922995056246071
+                },
+                {
+                    "x": 4.247041342694878,
+                    "y": 6.449512566443521,
+                    "heading": 0.23726388208727434,
+                    "angularVelocity": -0.35587385557112505,
+                    "velocityX": 1.4322757697888755,
+                    "velocityY": 0.4720742925270236,
+                    "timestamp": 4.984919557445611
+                },
+                {
+                    "x": 4.358419967210315,
+                    "y": 6.484373638974516,
+                    "heading": 0.21066668095834623,
+                    "angularVelocity": -0.42951013918099495,
+                    "velocityX": 1.7986196474403566,
+                    "velocityY": 0.5629608935994745,
+                    "timestamp": 5.046844058645151
+                },
+                {
+                    "x": 4.492739232760998,
+                    "y": 6.523763810645006,
+                    "heading": 0.1801310666057291,
+                    "angularVelocity": -0.4931103805619993,
+                    "velocityX": 2.1690811060046427,
+                    "velocityY": 0.6360999427926483,
+                    "timestamp": 5.108768559844691
+                },
+                {
+                    "x": 4.650257629888207,
+                    "y": 6.566174217155756,
+                    "heading": 0.14651122621435186,
+                    "angularVelocity": -0.5429166120053787,
+                    "velocityX": 2.543716849969224,
+                    "velocityY": 0.6848727997677386,
+                    "timestamp": 5.170693061044231
+                },
+                {
+                    "x": 4.831166826127315,
+                    "y": 6.609441866820435,
+                    "heading": 0.11103492926681961,
+                    "angularVelocity": -0.5728959662221038,
+                    "velocityX": 2.9214477748663974,
+                    "velocityY": 0.6987161596224655,
+                    "timestamp": 5.232617562243771
+                },
+                {
+                    "x": 5.035367402593747,
+                    "y": 6.650325238947526,
+                    "heading": 0.07555553837017441,
+                    "angularVelocity": -0.572945929468525,
+                    "velocityX": 3.297573214331347,
+                    "velocityY": 0.6602131843638516,
+                    "timestamp": 5.294542063443311
+                },
+                {
+                    "x": 5.261782084531844,
+                    "y": 6.683862940799087,
+                    "heading": 0.04296427542789473,
+                    "angularVelocity": -0.5263064265509467,
+                    "velocityX": 3.656302070298779,
+                    "velocityY": 0.5415901816228108,
+                    "timestamp": 5.356466564642851
+                },
+                {
+                    "x": 5.5066279243005445,
+                    "y": 6.7031170566524425,
+                    "heading": 0.017406971953643092,
+                    "angularVelocity": -0.41271714715792507,
+                    "velocityX": 3.953941251456059,
+                    "velocityY": 0.31092888082073744,
+                    "timestamp": 5.418391065842391
+                },
+                {
+                    "x": 5.761751882534451,
+                    "y": 6.701608504256703,
+                    "heading": 0.0031721000803410847,
+                    "angularVelocity": -0.22987463116469584,
+                    "velocityX": 4.119919471160824,
+                    "velocityY": -0.024361155383045233,
+                    "timestamp": 5.480315567041931
+                },
+                {
+                    "x": 6.018601417541504,
+                    "y": 6.676982402801514,
+                    "heading": 1.3856044746201851e-28,
+                    "angularVelocity": -0.051225282705460874,
+                    "velocityX": 4.1477852874325745,
+                    "velocityY": -0.39767944800776983,
+                    "timestamp": 5.542240068241471
+                },
+                {
+                    "x": 6.298785976787896,
+                    "y": 6.621524323291068,
+                    "heading": -1.1062883462477051e-7,
+                    "angularVelocity": -0.0000016196318539939486,
+                    "velocityX": 4.1019670747856205,
+                    "velocityY": -0.8119191749701038,
+                    "timestamp": 5.610544994667785
+                },
+                {
+                    "x": 6.570847588785804,
+                    "y": 6.538762513850569,
+                    "heading": -1.7625665609767657e-7,
+                    "angularVelocity": -9.608065612032315e-7,
+                    "velocityX": 3.983045238931686,
+                    "velocityY": -1.2116521277536396,
+                    "timestamp": 5.678849921094098
+                },
+                {
+                    "x": 6.822289575216137,
+                    "y": 6.436346015436509,
+                    "heading": -1.9920369427970006e-7,
+                    "angularVelocity": -3.359499729024442e-7,
+                    "velocityX": 3.681169127699538,
+                    "velocityY": -1.4994013429550492,
+                    "timestamp": 5.747154847520412
+                },
+                {
+                    "x": 7.046075124873892,
+                    "y": 6.327103284773838,
+                    "heading": -1.9303612449853094e-7,
+                    "angularVelocity": 9.029465521528115e-8,
+                    "velocityX": 3.2762724647565618,
+                    "velocityY": -1.5993389697962725,
+                    "timestamp": 5.815459773946726
+                },
+                {
+                    "x": 7.24149070801809,
+                    "y": 6.220433729890437,
+                    "heading": -1.707914860870973e-7,
+                    "angularVelocity": 3.256666769918985e-7,
+                    "velocityX": 2.860929560549486,
+                    "velocityY": -1.5616670782666768,
+                    "timestamp": 5.88376470037304
+                },
+                {
+                    "x": 7.409535890143074,
+                    "y": 6.121659591172554,
+                    "heading": -1.4083076615117047e-7,
+                    "angularVelocity": 4.3863190407280574e-7,
+                    "velocityX": 2.4602205275232807,
+                    "velocityY": -1.446076350355775,
+                    "timestamp": 5.952069626799354
+                },
+                {
+                    "x": 7.551301991175276,
+                    "y": 6.033880291623994,
+                    "heading": -1.0837809038008853e-7,
+                    "angularVelocity": 4.7511471674142645e-7,
+                    "velocityX": 2.0754886718915797,
+                    "velocityY": -1.285109349224681,
+                    "timestamp": 6.020374553225667
+                },
+                {
+                    "x": 7.6676940976057,
+                    "y": 5.959048466238134,
+                    "heading": -7.68763569270851e-8,
+                    "angularVelocity": 4.6119270016324603e-7,
+                    "velocityX": 1.704007492870772,
+                    "velocityY": -1.0955553179109054,
+                    "timestamp": 6.088679479651981
+                },
+                {
+                    "x": 7.75943039247277,
+                    "y": 5.898484356691668,
+                    "heading": -4.872749406160536e-8,
+                    "angularVelocity": 4.1210589540487035e-7,
+                    "velocityX": 1.3430406804702926,
+                    "velocityY": -0.8866726415670821,
+                    "timestamp": 6.156984406078295
+                },
+                {
+                    "x": 7.827081854833763,
+                    "y": 5.853132007538663,
+                    "heading": -2.56890633614765e-8,
+                    "angularVelocity": 3.372879806112141e-7,
+                    "velocityX": 0.9904331341894409,
+                    "velocityY": -0.6639689335136235,
+                    "timestamp": 6.225289332504609
+                },
+                {
+                    "x": 7.871108582498892,
+                    "y": 5.823696591560291,
+                    "heading": -9.097335168428086e-9,
+                    "angularVelocity": 2.4290675740566603e-7,
+                    "velocityX": 0.6445615267974086,
+                    "velocityY": -0.43094133203006635,
+                    "timestamp": 6.293594258930923
+                },
+                {
+                    "x": 7.891887187957764,
+                    "y": 5.810723304748535,
+                    "heading": 7.7887817466186635e-28,
+                    "angularVelocity": 1.3318710149321612e-7,
+                    "velocityX": 0.30420361379478605,
+                    "velocityY": -0.18993193449599038,
+                    "timestamp": 6.361899185357236
+                },
+                {
+                    "x": 7.888143888902488,
+                    "y": 5.815954260813035,
+                    "heading": 2.765818870905722e-10,
+                    "angularVelocity": 3.82019160949285e-9,
+                    "velocityX": -0.051703022902955066,
+                    "velocityY": 0.07225077056721455,
+                    "timestamp": 6.434299189169424
+                },
+                {
+                    "x": 7.858277690270956,
+                    "y": 5.839675291870993,
+                    "heading": -8.025005255471161e-9,
+                    "angularVelocity": -1.1466279979897161e-7,
+                    "velocityX": -0.41251653396327165,
+                    "velocityY": 0.3276385332726272,
+                    "timestamp": 6.506699192981612
+                },
+                {
+                    "x": 7.801862071405289,
+                    "y": 5.881268564071655,
+                    "heading": -2.3925602238115484e-8,
+                    "angularVelocity": -2.1962149372107496e-7,
+                    "velocityX": -0.7792212140211278,
+                    "velocityY": 0.5744926797042514,
+                    "timestamp": 6.5790991967938
+                },
+                {
+                    "x": 7.718377433983251,
+                    "y": 5.939936873772295,
+                    "heading": -4.617816836254086e-8,
+                    "angularVelocity": -3.0735586951280266e-7,
+                    "velocityX": -1.1531026661076393,
+                    "velocityY": 0.8103357266780017,
+                    "timestamp": 6.651499200605988
+                },
+                {
+                    "x": 7.6071812503843494,
+                    "y": 6.014616392636198,
+                    "heading": -7.314569126794996e-8,
+                    "angularVelocity": -3.7247957852827054e-7,
+                    "velocityX": -1.5358588086176677,
+                    "velocityY": 1.03148501286863,
+                    "timestamp": 6.723899204418176
+                },
+                {
+                    "x": 7.467468982650147,
+                    "y": 6.1038259312718015,
+                    "heading": -1.0259692735622503e-7,
+                    "angularVelocity": -4.06785007424503e-7,
+                    "velocityX": -1.929727353283392,
+                    "velocityY": 1.232175883125927,
+                    "timestamp": 6.796299208230364
+                },
+                {
+                    "x": 7.298232339066578,
+                    "y": 6.205388592850735,
+                    "heading": -1.3134543452628007e-7,
+                    "angularVelocity": -3.9707880740767756e-7,
+                    "velocityX": -2.33752257834935,
+                    "velocityY": 1.402799119215454,
+                    "timestamp": 6.868699212042552
+                },
+                {
+                    "x": 7.098262109849036,
+                    "y": 6.315880698175479,
+                    "heading": -1.545804802746847e-7,
+                    "angularVelocity": -3.209260293504718e-7,
+                    "velocityX": -2.7620195951409254,
+                    "velocityY": 1.52613397108887,
+                    "timestamp": 6.94109921585474
+                },
+                {
+                    "x": 6.866440435797629,
+                    "y": 6.429499058364546,
+                    "heading": -1.6464439215056105e-7,
+                    "angularVelocity": -1.3900430035864317e-7,
+                    "velocityX": -3.201956655316912,
+                    "velocityY": 1.5693142846207861,
+                    "timestamp": 7.013499219666928
+                },
+                {
+                    "x": 6.603388966204105,
+                    "y": 6.536118308127826,
+                    "heading": -1.4954692300634435e-7,
+                    "angularVelocity": 2.085285683600351e-7,
+                    "velocityX": -3.63330739976067,
+                    "velocityY": 1.47264149377475,
+                    "timestamp": 7.085899223479116
+                },
+                {
+                    "x": 6.316248920025813,
+                    "y": 6.6216655877567385,
+                    "heading": -9.596874319673351e-8,
+                    "angularVelocity": 7.400300689016137e-7,
+                    "velocityX": -3.966022528440158,
+                    "velocityY": 1.181592197851674,
+                    "timestamp": 7.1582992272913035
+                },
+                {
+                    "x": 6.018601417541504,
+                    "y": 6.676982402801514,
+                    "heading": 3.619661729671041e-26,
+                    "angularVelocity": 0.000001325535057231274,
+                    "velocityX": -4.111153132759927,
+                    "velocityY": 0.7640443664653904,
+                    "timestamp": 7.230699231103491
+                },
+                {
+                    "x": 5.740842709923353,
+                    "y": 6.700963706149043,
+                    "heading": 0.0018237295021199065,
+                    "angularVelocity": 0.027302725220575197,
+                    "velocityX": -4.158275480494951,
+                    "velocityY": 0.35901976415240183,
+                    "timestamp": 7.297495841602489
+                },
+                {
+                    "x": 5.465613780315343,
+                    "y": 6.698113446403688,
+                    "heading": 0.01705683549279003,
+                    "angularVelocity": 0.2280520804405013,
+                    "velocityX": -4.120402630491888,
+                    "velocityY": -0.04267072421882417,
+                    "timestamp": 7.364292452101486
+                },
+                {
+                    "x": 5.204358543644784,
+                    "y": 6.6722541835752125,
+                    "heading": 0.046262668986232006,
+                    "angularVelocity": 0.4372352620179803,
+                    "velocityX": -3.911204995566067,
+                    "velocityY": -0.3871343565982821,
+                    "timestamp": 7.431089062600483
+                },
+                {
+                    "x": 4.966474467989701,
+                    "y": 6.632584406687531,
+                    "heading": 0.08307800660337901,
+                    "angularVelocity": 0.5511557748532727,
+                    "velocityX": -3.561319562145362,
+                    "velocityY": -0.593889069989206,
+                    "timestamp": 7.49788567309948
+                },
+                {
+                    "x": 4.755212132064066,
+                    "y": 6.587176716637979,
+                    "heading": 0.12225145158563697,
+                    "angularVelocity": 0.5864585746135442,
+                    "velocityX": -3.162770301478206,
+                    "velocityY": -0.6797903323288379,
+                    "timestamp": 7.5646822835984775
+                },
+                {
+                    "x": 4.571183536225408,
+                    "y": 6.5411456783481245,
+                    "heading": 0.16051492442030083,
+                    "angularVelocity": 0.572835545828158,
+                    "velocityX": -2.7550588939152947,
+                    "velocityY": -0.689122366329426,
+                    "timestamp": 7.631478894097475
+                },
+                {
+                    "x": 4.414262802682262,
+                    "y": 6.497671625821495,
+                    "heading": 0.19585610790995653,
+                    "angularVelocity": 0.5290864794731811,
+                    "velocityX": -2.3492319800493826,
+                    "velocityY": -0.6508421939655534,
+                    "timestamp": 7.698275504596472
+                },
+                {
+                    "x": 4.284155725885966,
+                    "y": 6.458840625879828,
+                    "heading": 0.22695656960626623,
+                    "angularVelocity": 0.46559939889130514,
+                    "velocityX": -1.9478095643528162,
+                    "velocityY": -0.581331891717008,
+                    "timestamp": 7.765072115095469
+                },
+                {
+                    "x": 4.180556238781294,
+                    "y": 6.426101360577794,
+                    "heading": 0.2528995134208406,
+                    "angularVelocity": 0.3883871295380163,
+                    "velocityX": -1.5509692232995544,
+                    "velocityY": -0.4901336318932807,
+                    "timestamp": 7.831868725594466
+                },
+                {
+                    "x": 4.103186927937392,
+                    "y": 6.4005094673645,
+                    "heading": 0.27301572769159327,
+                    "angularVelocity": 0.3011562131742396,
+                    "velocityX": -1.1582819886506504,
+                    "velocityY": -0.38313161434557874,
+                    "timestamp": 7.8986653360934636
+                },
+                {
+                    "x": 4.05180613335198,
+                    "y": 6.382864598687039,
+                    "heading": 0.2867974869909083,
+                    "angularVelocity": 0.20632423107040648,
+                    "velocityX": -0.7692126022799183,
+                    "velocityY": -0.26415814433767054,
+                    "timestamp": 7.965461946592461
+                },
+                {
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "angularVelocity": 0.1055455809722198,
+                    "velocityX": -0.383261866992415,
+                    "velocityY": -0.13582880894924632,
+                    "timestamp": 8.032258557091458
+                },
+                {
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "angularVelocity": -2.6570810296558003e-27,
+                    "velocityX": -7.92943357698847e-27,
+                    "velocityY": 1.86287757068553e-26,
+                    "timestamp": 8.099055167590455
+                },
+                {
+                    "x": 4.001912054553803,
+                    "y": 6.377610355104089,
+                    "heading": 0.27793057177759545,
+                    "angularVelocity": -0.248523754884058,
+                    "velocityX": -0.37931188575122854,
+                    "velocityY": 0.05962352837962387,
+                    "timestamp": 8.16310136825578
+                },
+                {
+                    "x": 3.9532989917689947,
+                    "y": 6.385094713412846,
+                    "heading": 0.24611473380028503,
+                    "angularVelocity": -0.49676386181851123,
+                    "velocityX": -0.7590311724943191,
+                    "velocityY": 0.11685873995658545,
+                    "timestamp": 8.227147568921106
+                },
+                {
+                    "x": 3.8803348270353966,
+                    "y": 6.396044749749356,
+                    "heading": 0.19840169352590004,
+                    "angularVelocity": -0.7449784652131087,
+                    "velocityX": -1.1392426713158736,
+                    "velocityY": 0.17097089636479054,
+                    "timestamp": 8.291193769586432
+                },
+                {
+                    "x": 3.7829788345403452,
+                    "y": 6.410188068322517,
+                    "heading": 0.13480855568773095,
+                    "angularVelocity": -0.992925999943011,
+                    "velocityX": -1.520090051926524,
+                    "velocityY": 0.22082993879789267,
+                    "timestamp": 8.355239970251757
+                },
+                {
+                    "x": 3.6611743486827018,
+                    "y": 6.427129765366101,
+                    "heading": 0.0554387772249063,
+                    "angularVelocity": -1.239258186095577,
+                    "velocityX": -1.9018221938586934,
+                    "velocityY": 0.264523060971458,
+                    "timestamp": 8.419286170917083
+                },
+                {
+                    "x": 3.5148377265715247,
+                    "y": 6.446239524609827,
+                    "heading": -0.0393712537936742,
+                    "angularVelocity": -1.4803381002100633,
+                    "velocityX": -2.284860313195802,
+                    "velocityY": 0.29837459591997334,
+                    "timestamp": 8.483332371582408
+                },
+                {
+                    "x": 3.343848310096575,
+                    "y": 6.466345188072757,
+                    "heading": -0.14873635590657566,
+                    "angularVelocity": -1.707597031155534,
+                    "velocityX": -2.6697823555289077,
+                    "velocityY": 0.3139243741871967,
+                    "timestamp": 8.547378572247734
+                },
+                {
+                    "x": 3.14815023920896,
+                    "y": 6.484576821653784,
+                    "heading": -0.2703054170409716,
+                    "angularVelocity": -1.8981463361059787,
+                    "velocityX": -3.0555765815093188,
+                    "velocityY": 0.2846637800780145,
+                    "timestamp": 8.61142477291306
+                },
+                {
+                    "x": 2.932193940008419,
+                    "y": 6.488086466497611,
+                    "heading": -0.39300827574348024,
+                    "angularVelocity": -1.9158491437094674,
+                    "velocityX": -3.3718830618700366,
+                    "velocityY": 0.05479864234519086,
+                    "timestamp": 8.675470973578385
+                },
+                {
+                    "x": 2.7340316065376515,
+                    "y": 6.475211847713753,
+                    "heading": -0.4935227085605528,
+                    "angularVelocity": -1.5694050821579926,
+                    "velocityX": -3.094052908872265,
+                    "velocityY": -0.20102080451476154,
+                    "timestamp": 8.73951717424371
+                },
+                {
+                    "x": 2.5578914607417174,
+                    "y": 6.452000594790772,
+                    "heading": -0.5736763053972732,
+                    "angularVelocity": -1.251496513517863,
+                    "velocityX": -2.750204445636945,
+                    "velocityY": -0.36241420539950464,
+                    "timestamp": 8.803563374909036
+                },
+                {
+                    "x": 2.4046623857559206,
+                    "y": 6.420343190075613,
+                    "heading": -0.6349490103430218,
+                    "angularVelocity": -0.9566953903468849,
+                    "velocityX": -2.392477202301163,
+                    "velocityY": -0.4942901278498211,
+                    "timestamp": 8.867609575574361
+                },
+                {
+                    "x": 2.274706473608676,
+                    "y": 6.381144596576831,
+                    "heading": -0.6781364741434692,
+                    "angularVelocity": -0.6743173420406988,
+                    "velocityX": -2.0290963522775014,
+                    "velocityY": -0.6120362034215643,
+                    "timestamp": 8.931655776239687
+                },
+                {
+                    "x": 2.1682152530987344,
+                    "y": 6.3349342062757374,
+                    "heading": -0.7036879484930157,
+                    "angularVelocity": -0.3989537877986885,
+                    "velocityX": -1.6627250235562485,
+                    "velocityY": -0.7215164962332046,
+                    "timestamp": 8.995701976905012
+                },
+                {
+                    "x": 2.085304191805873,
+                    "y": 6.282058814568287,
+                    "heading": -0.7118496672173015,
+                    "angularVelocity": -0.12743486170139653,
+                    "velocityX": -1.2945508153733418,
+                    "velocityY": -0.8255820198258502,
+                    "timestamp": 9.059748177570338
+                },
+                {
+                    "x": 2.026047892095963,
+                    "y": 6.222762660032223,
+                    "heading": -0.7027306858009142,
+                    "angularVelocity": 0.14238130164876692,
+                    "velocityX": -0.925211786091023,
+                    "velocityY": -0.9258340685330668,
+                    "timestamp": 9.123794378235663
+                },
+                {
+                    "x": 1.990496039390564,
+                    "y": 6.157227039337158,
+                    "heading": -0.6763337151495863,
+                    "angularVelocity": 0.41215513765236195,
+                    "velocityX": -0.5550969821172682,
+                    "velocityY": -1.0232554002308782,
+                    "timestamp": 9.187840578900989
+                },
+                {
+                    "x": 1.978661907650031,
+                    "y": 6.085722412992096,
+                    "heading": -0.632671818562567,
+                    "angularVelocity": 0.6828637108843474,
+                    "velocityX": -0.18508355676508165,
+                    "velocityY": -1.1183186784859962,
+                    "timestamp": 9.251779979825479
+                },
+                {
+                    "x": 1.9906295592630834,
+                    "y": 6.008657807318728,
+                    "heading": -0.5719280710449405,
+                    "angularVelocity": 0.9500205919877586,
+                    "velocityX": 0.18717178203132864,
+                    "velocityY": -1.2052756916565248,
+                    "timestamp": 9.315719380749968
+                },
+                {
+                    "x": 2.026610081409329,
+                    "y": 5.926878777279561,
+                    "heading": -0.49455650916344973,
+                    "angularVelocity": 1.2100764280363472,
+                    "velocityX": 0.5627284839396147,
+                    "velocityY": -1.2790083869528952,
+                    "timestamp": 9.379658781674458
+                },
+                {
+                    "x": 2.086926247462202,
+                    "y": 5.8419986579151795,
+                    "heading": -0.40162648863024836,
+                    "angularVelocity": 1.4534077452953975,
+                    "velocityX": 0.9433332996676658,
+                    "velocityY": -1.3275088308165748,
+                    "timestamp": 9.443598182598947
+                },
+                {
+                    "x": 2.171907866002122,
+                    "y": 5.758194669745824,
+                    "heading": -0.29634456613068483,
+                    "angularVelocity": 1.6465891293523014,
+                    "velocityX": 1.3290962585070243,
+                    "velocityY": -1.3106783447709351,
+                    "timestamp": 9.507537583523437
+                },
+                {
+                    "x": 2.2727395248667923,
+                    "y": 5.693604569796404,
+                    "heading": -0.19652914952300654,
+                    "angularVelocity": 1.5610940228476111,
+                    "velocityX": 1.5769878573580702,
+                    "velocityY": -1.0101768082828597,
+                    "timestamp": 9.571476984447926
+                },
+                {
+                    "x": 2.3578316910702357,
+                    "y": 5.647359218392216,
+                    "heading": -0.11735526130423977,
+                    "angularVelocity": 1.238264467198687,
+                    "velocityX": 1.3308252028187528,
+                    "velocityY": -0.7232684500563616,
+                    "timestamp": 9.635416385372416
+                },
+                {
+                    "x": 2.4228249155353856,
+                    "y": 5.614614683745836,
+                    "heading": -0.05848104065944221,
+                    "angularVelocity": 0.9207815493036267,
+                    "velocityX": 1.0164815985984024,
+                    "velocityY": -0.5121182584280018,
+                    "timestamp": 9.699355786296906
+                },
+                {
+                    "x": 2.4665798931428133,
+                    "y": 5.593575115113347,
+                    "heading": -0.019455306739924745,
+                    "angularVelocity": 0.6103550135792704,
+                    "velocityX": 0.6843194802388106,
+                    "velocityY": -0.3290548289205331,
+                    "timestamp": 9.763295187221395
+                },
+                {
+                    "x": 2.4885952472686768,
+                    "y": 5.583330154418945,
+                    "heading": -2.950378951534863e-27,
+                    "angularVelocity": 0.30427727596166787,
+                    "velocityX": 0.34431592738666283,
+                    "velocityY": -0.16022922558346586,
+                    "timestamp": 9.827234588145885
+                },
+                {
+                    "x": 2.4885952472686768,
+                    "y": 5.583330154418945,
+                    "heading": -1.2597633775469736e-27,
+                    "angularVelocity": 2.52578953066849e-28,
+                    "velocityX": 5.932691751344987e-28,
+                    "velocityY": -9.915062724222499e-27,
+                    "timestamp": 9.891173989070374
+                },
+                {
+                    "x": 2.4743714672940076,
+                    "y": 5.57364847936169,
+                    "heading": -0.01214464618202118,
+                    "angularVelocity": -0.22627917912193768,
+                    "velocityX": -0.26501762245193394,
+                    "velocityY": -0.18038907446510918,
+                    "timestamp": 9.944845061367648
+                },
+                {
+                    "x": 2.4462340281188424,
+                    "y": 5.553844302753565,
+                    "heading": -0.03641057081599083,
+                    "angularVelocity": -0.45212297044421884,
+                    "velocityX": -0.5242570712826018,
+                    "velocityY": -0.36899163293093734,
+                    "timestamp": 9.998516133664921
+                },
+                {
+                    "x": 2.4046457242182946,
+                    "y": 5.523311043473541,
+                    "heading": -0.07278723106010666,
+                    "angularVelocity": -0.6777703274239201,
+                    "velocityX": -0.7748737284434789,
+                    "velocityY": -0.568896017409645,
+                    "timestamp": 10.052187205962195
+                },
+                {
+                    "x": 2.3503574105372382,
+                    "y": 5.481175509134353,
+                    "heading": -0.12134175658440335,
+                    "angularVelocity": -0.9046684451423458,
+                    "velocityX": -1.0115004481439094,
+                    "velocityY": -0.7850697318996741,
+                    "timestamp": 10.105858278259468
+                },
+                {
+                    "x": 2.284734671597692,
+                    "y": 5.426115387645732,
+                    "heading": -0.18232155955050056,
+                    "angularVelocity": -1.136176348934157,
+                    "velocityX": -1.2226835822484632,
+                    "velocityY": -1.0258807795688225,
+                    "timestamp": 10.159529350556742
+                },
+                {
+                    "x": 2.210690375930078,
+                    "y": 5.356070422302002,
+                    "heading": -0.2559264629038059,
+                    "angularVelocity": -1.3714073560077662,
+                    "velocityX": -1.379594118364116,
+                    "velocityY": -1.3050785524791577,
+                    "timestamp": 10.213200422854015
+                },
+                {
+                    "x": 2.135682836612729,
+                    "y": 5.268691707848941,
+                    "heading": -0.33902159970532053,
+                    "angularVelocity": -1.5482294883408945,
+                    "velocityX": -1.3975412844725235,
+                    "velocityY": -1.6280411535116714,
+                    "timestamp": 10.266871495151289
+                },
+                {
+                    "x": 2.0724663596543236,
+                    "y": 5.168263556709831,
+                    "heading": -0.4225586144389228,
+                    "angularVelocity": -1.556462562754615,
+                    "velocityX": -1.1778500829694967,
+                    "velocityY": -1.8711783990984618,
+                    "timestamp": 10.320542567448562
+                },
+                {
+                    "x": 2.0260137993243106,
+                    "y": 5.062857319928912,
+                    "heading": -0.4997989805362741,
+                    "angularVelocity": -1.4391433372065379,
+                    "velocityX": -0.8655046068899471,
+                    "velocityY": -1.9639301446614508,
+                    "timestamp": 10.374213639745836
+                },
+                {
+                    "x": 1.996941498368396,
+                    "y": 4.956716720914925,
+                    "heading": -0.5683761952809107,
+                    "angularVelocity": -1.2777314074293329,
+                    "velocityX": -0.5416754261008351,
+                    "velocityY": -1.9776127897370002,
+                    "timestamp": 10.42788471204311
+                },
+                {
+                    "x": 1.9851616081186387,
+                    "y": 4.852009928485605,
+                    "heading": -0.6273903061238789,
+                    "angularVelocity": -1.0995515520185801,
+                    "velocityX": -0.2194830426437353,
+                    "velocityY": -1.950898089931401,
+                    "timestamp": 10.481555784340383
+                },
+                {
+                    "x": 1.990496039390564,
+                    "y": 4.75,
+                    "heading": -0.6763337151495863,
+                    "angularVelocity": -0.9119141267500614,
+                    "velocityX": 0.09939118120053472,
+                    "velocityY": -1.9006500917401496,
+                    "timestamp": 10.535226856637657
+                },
+                {
+                    "x": 2.0404464839460696,
+                    "y": 4.59880499645476,
+                    "heading": -0.7273828368780748,
+                    "angularVelocity": -0.6071201159183436,
+                    "velocityX": 0.5940537008649031,
+                    "velocityY": -1.7981411818772612,
+                    "timestamp": 10.619310912477696
+                },
+                {
+                    "x": 2.1298849994259657,
+                    "y": 4.462832545778162,
+                    "heading": -0.7498828617388094,
+                    "angularVelocity": -0.26758967126346256,
+                    "velocityX": 1.0636798449642342,
+                    "velocityY": -1.617101474449241,
+                    "timestamp": 10.703394968317735
+                },
+                {
+                    "x": 2.249114137990773,
+                    "y": 4.356577462238403,
+                    "heading": -0.739452965709272,
+                    "angularVelocity": 0.12404130515991267,
+                    "velocityX": 1.4179755885185839,
+                    "velocityY": -1.2636769537128287,
+                    "timestamp": 10.787479024157774
+                },
+                {
+                    "x": 2.359728475062173,
+                    "y": 4.292347233042172,
+                    "heading": -0.7129681413379042,
+                    "angularVelocity": 0.3149803385049905,
+                    "velocityX": 1.315520950628657,
+                    "velocityY": -0.7638811966732779,
+                    "timestamp": 10.871563079997813
+                },
+                {
+                    "x": 2.437807047108505,
+                    "y": 4.256472630548062,
+                    "heading": -0.689567924920021,
+                    "angularVelocity": 0.2782955244499613,
+                    "velocityX": 0.9285776151767543,
+                    "velocityY": -0.42665166583255143,
+                    "timestamp": 10.955647135837852
+                },
+                {
+                    "x": 2.477766990661621,
+                    "y": 4.240628242492676,
+                    "heading": -0.6763337151495863,
+                    "angularVelocity": 0.1573926190669421,
+                    "velocityX": 0.47523805974745176,
+                    "velocityY": -0.1884351069545121,
+                    "timestamp": 11.039731191677891
+                },
+                {
+                    "x": 2.477766990661621,
+                    "y": 4.240628242492676,
+                    "heading": -0.6763337151495863,
+                    "angularVelocity": 1.021747484041298e-28,
+                    "velocityX": -7.166324056649742e-28,
+                    "velocityY": 5.068526190074515e-27,
+                    "timestamp": 11.12381524751793
+                }
+            ],
+            "trajectoryWaypoints": [
+                {
+                    "timestamp": 0,
+                    "isStopPoint": true,
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "timestamp": 1.1048089785277977,
+                    "isStopPoint": true,
+                    "x": 2.542736291885376,
+                    "y": 6.991001605987549,
+                    "heading": 0.610560190519,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "timestamp": 3.0562712875316644,
+                    "isStopPoint": false,
+                    "x": 7.946028232574463,
+                    "y": 7.456615924835205,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "timestamp": 4.737221552647451,
+                    "isStopPoint": true,
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "timestamp": 5.542240068241471,
+                    "isStopPoint": false,
+                    "x": 6.018601417541504,
+                    "y": 6.676982402801514,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 12
+                },
+                {
+                    "timestamp": 6.361899185357236,
+                    "isStopPoint": false,
+                    "x": 7.891887187957764,
+                    "y": 5.810723304748535,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 12
+                },
+                {
+                    "timestamp": 7.230699231103491,
+                    "isStopPoint": false,
+                    "x": 6.018601417541504,
+                    "y": 6.676982402801514,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "timestamp": 8.099055167590455,
+                    "isStopPoint": true,
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "timestamp": 9.187840578900989,
+                    "isStopPoint": false,
+                    "x": 1.990496039390564,
+                    "y": 6.157227039337158,
+                    "heading": -0.6763337151495863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 11
+                },
+                {
+                    "timestamp": 9.891173989070374,
+                    "isStopPoint": true,
+                    "x": 2.4885952472686768,
+                    "y": 5.583330154418945,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 12
+                },
+                {
+                    "timestamp": 10.535226856637657,
+                    "isStopPoint": false,
+                    "x": 1.990496039390564,
+                    "y": 4.75,
+                    "heading": -0.6763337151495863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 7
+                },
+                {
+                    "timestamp": 11.12381524751793,
+                    "isStopPoint": true,
+                    "x": 2.477766990661621,
+                    "y": 4.240628242492676,
+                    "heading": -0.6763337151495863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        1
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        3
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        7
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        9
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": [],
+            "eventMarkers": [],
+            "isTrajectoryStale": false
+        },
+        "Distance Center 7": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 2.6293623447418213,
+                    "y": 6.7744364738464355,
+                    "heading": 0.610560190519,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "x": 7.946028232574463,
+                    "y": 7.456615924835205,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 6.018601417541504,
+                    "y": 6.676982402801514,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 7.891887187957764,
+                    "y": 5.810723304748535,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 6.018601417541504,
+                    "y": 6.676982402801514,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 6.007773399353027,
+                    "y": 6.4279327392578125,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 8.010997772216797,
+                    "y": 4.30559778213501,
+                    "heading": -0.6499245399095859,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 6.007773399353027,
+                    "y": 6.4279327392578125,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 2.9650378227233887,
+                    "y": 6.406276702880859,
+                    "heading": -1.265712572039586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 8
+                },
+                {
+                    "x": 2.60770583152771,
+                    "y": 5.854036331176758,
+                    "heading": -1.374993138249586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 1.990496039390564,
+                    "y": 4.75,
+                    "heading": -0.6763337151495863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 7
+                },
+                {
+                    "x": 2.477766990661621,
+                    "y": 4.240628242492676,
+                    "heading": -0.6763337151495863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 3.125039836628047e-22,
+                    "angularVelocity": -2.4778681964275978e-21,
+                    "velocityX": -1.6966574578954054e-22,
+                    "velocityY": -5.560554740883435e-23,
+                    "timestamp": 0
+                },
+                {
+                    "x": 1.3761104747158472,
+                    "y": 5.6065902507064385,
+                    "heading": 0.012329157086594333,
+                    "angularVelocity": 0.16082740054460393,
+                    "velocityX": 0.3405974752403179,
+                    "velocityY": 0.31741720579612304,
+                    "timestamp": 0.07666079937152849
+                },
+                {
+                    "x": 1.4283317379990463,
+                    "y": 5.655256776007888,
+                    "heading": 0.036989563809549715,
+                    "angularVelocity": 0.32168209730557745,
+                    "velocityX": 0.6811990445092346,
+                    "velocityY": 0.6348293482512852,
+                    "timestamp": 0.15332159874305698
+                },
+                {
+                    "x": 1.5066641753369279,
+                    "y": 5.728255747741762,
+                    "heading": 0.07398796382390593,
+                    "angularVelocity": 0.48262476151660494,
+                    "velocityX": 1.0218056422585864,
+                    "velocityY": 0.9522333752364306,
+                    "timestamp": 0.22998239811458548
+                },
+                {
+                    "x": 1.61110814877825,
+                    "y": 5.825586301435188,
+                    "heading": 0.12333895950365491,
+                    "angularVelocity": 0.6437579060527999,
+                    "velocityX": 1.3624169627444864,
+                    "velocityY": 1.269626125625491,
+                    "timestamp": 0.30664319748611396
+                },
+                {
+                    "x": 1.7416638579543184,
+                    "y": 5.94724735627152,
+                    "heading": 0.18506856130544258,
+                    "angularVelocity": 0.8052303433808673,
+                    "velocityX": 1.7030308873162687,
+                    "velocityY": 1.5870047773271128,
+                    "timestamp": 0.38330399685764244
+                },
+                {
+                    "x": 1.8983311632556052,
+                    "y": 6.093237705653962,
+                    "heading": 0.2592169764309428,
+                    "angularVelocity": 0.9672272626084648,
+                    "velocityX": 2.0436429907548357,
+                    "velocityY": 1.9043676896051687,
+                    "timestamp": 0.4599647962291709
+                },
+                {
+                    "x": 2.0811093927308018,
+                    "y": 6.263556191912837,
+                    "heading": 0.3458391806253537,
+                    "angularVelocity": 1.129941311655328,
+                    "velocityX": 2.384246328940312,
+                    "velocityY": 2.2217155006882288,
+                    "timestamp": 0.5366255956006994
+                },
+                {
+                    "x": 2.237755248322454,
+                    "y": 6.4095214814608905,
+                    "heading": 0.4214192098887476,
+                    "angularVelocity": 0.9859019196643553,
+                    "velocityX": 2.0433631905204215,
+                    "velocityY": 1.9040407971830313,
+                    "timestamp": 0.6132863949722279
+                },
+                {
+                    "x": 2.3682919967752616,
+                    "y": 6.5311594209661115,
+                    "heading": 0.48444328648392837,
+                    "angularVelocity": 0.8221160894728113,
+                    "velocityX": 1.7027835546062429,
+                    "velocityY": 1.586703249932421,
+                    "timestamp": 0.6899471943437564
+                },
+                {
+                    "x": 2.4727205283209286,
+                    "y": 6.628470028391072,
+                    "heading": 0.5348829907239893,
+                    "angularVelocity": 0.657959539341745,
+                    "velocityX": 1.362215531298668,
+                    "velocityY": 1.2693659369941386,
+                    "timestamp": 0.7666079937152849
+                },
+                {
+                    "x": 2.5510415142038663,
+                    "y": 6.70145317297353,
+                    "heading": 0.5727208811580081,
+                    "angularVelocity": 0.493575474613063,
+                    "velocityX": 1.0216562640230678,
+                    "velocityY": 0.9520269183308826,
+                    "timestamp": 0.8432687930868134
+                },
+                {
+                    "x": 2.60325538804063,
+                    "y": 6.750108693952396,
+                    "heading": 0.5979476766685491,
+                    "angularVelocity": 0.3290703425655907,
+                    "velocityX": 0.6811026530484638,
+                    "velocityY": 0.634685802623348,
+                    "timestamp": 0.9199295924583418
+                },
+                {
+                    "x": 2.6293623447418213,
+                    "y": 6.7744364738464355,
+                    "heading": 0.610560190519,
+                    "angularVelocity": 0.1645236412071015,
+                    "velocityX": 0.34055158458062124,
+                    "velocityY": 0.3173431544346181,
+                    "timestamp": 0.9965903918298703
+                },
+                {
+                    "x": 2.6293623447418213,
+                    "y": 6.7744364738464355,
+                    "heading": 0.610560190519,
+                    "angularVelocity": 2.9094056410031188e-21,
+                    "velocityX": -2.0988135133774157e-22,
+                    "velocityY": 3.818316675059001e-22,
+                    "timestamp": 1.073251191201399
+                },
+                {
+                    "x": 2.6720197676214497,
+                    "y": 6.782417422598798,
+                    "heading": 0.6038766101713076,
+                    "angularVelocity": -0.07923422323582437,
+                    "velocityX": 0.5057061621584873,
+                    "velocityY": 0.09461459908935298,
+                    "timestamp": 1.1576033823978742
+                },
+                {
+                    "x": 2.7573390707361796,
+                    "y": 6.798381015719943,
+                    "heading": 0.5908586177943566,
+                    "angularVelocity": -0.15432903629769548,
+                    "velocityX": 1.0114651665183407,
+                    "velocityY": 0.1892492998073053,
+                    "timestamp": 1.2419555735943495
+                },
+                {
+                    "x": 2.885325839743734,
+                    "y": 6.8223294638474,
+                    "heading": 0.5719744160417135,
+                    "angularVelocity": -0.22387328040663998,
+                    "velocityX": 1.5172903891665839,
+                    "velocityY": 0.2839102077582811,
+                    "timestamp": 1.3263077647908248
+                },
+                {
+                    "x": 3.0559871968712695,
+                    "y": 6.854265761931121,
+                    "heading": 0.5478849059076967,
+                    "angularVelocity": -0.2855825058285316,
+                    "velocityX": 2.023200046220811,
+                    "velocityY": 0.37860662101039444,
+                    "timestamp": 1.4106599559873
+                },
+                {
+                    "x": 3.269332249645403,
+                    "y": 6.894194168881417,
+                    "heading": 0.5195930329458099,
+                    "angularVelocity": -0.3354017549584274,
+                    "velocityX": 2.5292176735184513,
+                    "velocityY": 0.47335352388527485,
+                    "timestamp": 1.4950121471837754
+                },
+                {
+                    "x": 3.525371770387385,
+                    "y": 6.942121092388337,
+                    "heading": 0.4888001874449035,
+                    "angularVelocity": -0.36505092593484567,
+                    "velocityX": 3.0353630072941193,
+                    "velocityY": 0.568176390288274,
+                    "timestamp": 1.5793643383802507
+                },
+                {
+                    "x": 3.824109398072704,
+                    "y": 6.998056361458239,
+                    "heading": 0.4590231888113175,
+                    "angularVelocity": -0.35300800383748954,
+                    "velocityX": 3.5415514813301257,
+                    "velocityY": 0.663115780117865,
+                    "timestamp": 1.663716529576726
+                },
+                {
+                    "x": 4.165377594624907,
+                    "y": 7.062001251881552,
+                    "heading": 0.4419288520213311,
+                    "angularVelocity": -0.2026543299885333,
+                    "velocityX": 4.045753781989037,
+                    "velocityY": 0.7580702945151749,
+                    "timestamp": 1.7480687207732013
+                },
+                {
+                    "x": 4.512094110834224,
+                    "y": 7.126817088416808,
+                    "heading": 0.4419288475732292,
+                    "angularVelocity": -5.2732499850975034e-8,
+                    "velocityX": 4.110343919836483,
+                    "velocityY": 0.7683954099578318,
+                    "timestamp": 1.8324209119696766
+                },
+                {
+                    "x": 4.858810627216134,
+                    "y": 7.191632924028826,
+                    "heading": 0.44192884312513186,
+                    "angularVelocity": -5.27324452565932e-8,
+                    "velocityX": 4.110343921882592,
+                    "velocityY": 0.7683953990127874,
+                    "timestamp": 1.916773103166152
+                },
+                {
+                    "x": 5.205527143597466,
+                    "y": 7.256448759643943,
+                    "heading": 0.4419288386770344,
+                    "angularVelocity": -5.273244693132246e-8,
+                    "velocityX": 4.110343921875726,
+                    "velocityY": 0.7683953990495135,
+                    "timestamp": 2.001125294362627
+                },
+                {
+                    "x": 5.55224365998293,
+                    "y": 7.321264595236958,
+                    "heading": 0.44192883422893686,
+                    "angularVelocity": -5.273244840969362e-8,
+                    "velocityX": 4.110343921924708,
+                    "velocityY": 0.768395398787497,
+                    "timestamp": 2.0854774855591027
+                },
+                {
+                    "x": 5.898960176356647,
+                    "y": 7.386080430892807,
+                    "heading": 0.44192882978083925,
+                    "angularVelocity": -5.273244895227244e-8,
+                    "velocityX": 4.110343921785453,
+                    "velocityY": 0.7683953995324063,
+                    "timestamp": 2.1698296767555783
+                },
+                {
+                    "x": 6.24567671797852,
+                    "y": 7.450896131489656,
+                    "heading": 0.44192882533274175,
+                    "angularVelocity": -5.2732447593359085e-8,
+                    "velocityX": 4.110344221103768,
+                    "velocityY": 0.7683937984002956,
+                    "timestamp": 2.2541818679520538
+                },
+                {
+                    "x": 6.595261801688144,
+                    "y": 7.49783974677019,
+                    "heading": 0.44192881540719375,
+                    "angularVelocity": -1.1766793324652698e-7,
+                    "velocityX": 4.144350949880616,
+                    "velocityY": 0.5565192156205201,
+                    "timestamp": 2.3385340591485293
+                },
+                {
+                    "x": 6.9128163797305655,
+                    "y": 7.523763714384209,
+                    "heading": 0.36817463115775373,
+                    "angularVelocity": -0.8743600279173521,
+                    "velocityX": 3.764627492636967,
+                    "velocityY": 0.30733010306319974,
+                    "timestamp": 2.422886250345005
+                },
+                {
+                    "x": 7.188041974628758,
+                    "y": 7.539922522639208,
+                    "heading": 0.2960994016568734,
+                    "angularVelocity": -0.8544559243636105,
+                    "velocityX": 3.262815002127576,
+                    "velocityY": 0.19156358626608547,
+                    "timestamp": 2.5072384415414803
+                },
+                {
+                    "x": 7.420776182221607,
+                    "y": 7.5471672706134445,
+                    "heading": 0.22917449817448735,
+                    "angularVelocity": -0.7933985179650258,
+                    "velocityX": 2.75907720109792,
+                    "velocityY": 0.08588689720415285,
+                    "timestamp": 2.591590632737956
+                },
+                {
+                    "x": 7.610971318020134,
+                    "y": 7.545802222186919,
+                    "heading": 0.168638193399793,
+                    "angularVelocity": -0.7176613187639852,
+                    "velocityX": 2.2547740977530752,
+                    "velocityY": -0.016182726342533954,
+                    "timestamp": 2.6759428239344314
+                },
+                {
+                    "x": 7.758604986098777,
+                    "y": 7.535983661526973,
+                    "heading": 0.1151255004194216,
+                    "angularVelocity": -0.6343960034864801,
+                    "velocityX": 1.7502054894433117,
+                    "velocityY": -0.11639959224149259,
+                    "timestamp": 2.760295015130907
+                },
+                {
+                    "x": 7.863664232769146,
+                    "y": 7.517806741993987,
+                    "heading": 0.06902173017336893,
+                    "angularVelocity": -0.5465628052111479,
+                    "velocityX": 1.2454833144246713,
+                    "velocityY": -0.21548840966854907,
+                    "timestamp": 2.8446472063273824
+                },
+                {
+                    "x": 7.926140623551973,
+                    "y": 7.491335488819979,
+                    "heading": 0.03058497546670338,
+                    "angularVelocity": -0.4556699021266401,
+                    "velocityX": 0.7406611481769909,
+                    "velocityY": -0.31381820434695645,
+                    "timestamp": 2.928999397523858
+                },
+                {
+                    "x": 7.946028232574463,
+                    "y": 7.456615924835205,
+                    "heading": -1.1630164830490874e-22,
+                    "angularVelocity": -0.3625866149163102,
+                    "velocityX": 0.23576873037201065,
+                    "velocityY": -0.41160239576827296,
+                    "timestamp": 3.0133515887203335
+                },
+                {
+                    "x": 7.920037662049596,
+                    "y": 7.411169444194395,
+                    "heading": -0.0232873594647728,
+                    "angularVelocity": -0.2632383110672307,
+                    "velocityX": -0.29379517669185795,
+                    "velocityY": -0.5137231134312821,
+                    "timestamp": 3.101816521390507
+                },
+                {
+                    "x": 7.8471886878468275,
+                    "y": 7.356716657006662,
+                    "heading": -0.03810345649711681,
+                    "angularVelocity": -0.16747988819008414,
+                    "velocityX": -0.8234785468539669,
+                    "velocityY": -0.6155296290198013,
+                    "timestamp": 3.1902814540606808
+                },
+                {
+                    "x": 7.727467357765122,
+                    "y": 7.293295235469457,
+                    "heading": -0.044875887681678404,
+                    "angularVelocity": -0.076554980376365,
+                    "velocityX": -1.3533196314981237,
+                    "velocityY": -0.7169103013242628,
+                    "timestamp": 3.2787463867308544
+                },
+                {
+                    "x": 7.560854391886669,
+                    "y": 7.220959149263091,
+                    "heading": -0.04421375783355388,
+                    "angularVelocity": 0.007484658927997671,
+                    "velocityX": -1.8833786546771185,
+                    "velocityY": -0.8176809049983522,
+                    "timestamp": 3.367211319401028
+                },
+                {
+                    "x": 7.34732146541913,
+                    "y": 7.139792163137446,
+                    "heading": -0.03705684189341484,
+                    "angularVelocity": 0.08090116302718922,
+                    "velocityX": -2.413757858875676,
+                    "velocityY": -0.9175046391349556,
+                    "timestamp": 3.4556762520712017
+                },
+                {
+                    "x": 7.086823282403413,
+                    "y": 7.049941858740973,
+                    "heading": -0.02504724603475039,
+                    "angularVelocity": 0.13575543999383538,
+                    "velocityX": -2.9446490847049964,
+                    "velocityY": -1.0156601229943323,
+                    "timestamp": 3.5441411847413753
+                },
+                {
+                    "x": 6.779279315725815,
+                    "y": 6.9517369192823635,
+                    "heading": -0.011789177436815698,
+                    "angularVelocity": 0.14986806859804128,
+                    "velocityX": -3.476450582110584,
+                    "velocityY": -1.1101002000956572,
+                    "timestamp": 3.632606117411549
+                },
+                {
+                    "x": 6.424632928888111,
+                    "y": 6.846536629082463,
+                    "heading": -0.011789088874469935,
+                    "angularVelocity": 0.000001001101149226084,
+                    "velocityX": -4.008892293627146,
+                    "velocityY": -1.1891750439931048,
+                    "timestamp": 3.7210710500817226
+                },
+                {
+                    "x": 6.061695217745179,
+                    "y": 6.775000553604086,
+                    "heading": -0.011789083969654859,
+                    "angularVelocity": 5.544360832077928e-8,
+                    "velocityX": -4.102616711370633,
+                    "velocityY": -0.808637652447976,
+                    "timestamp": 3.809535982751896
+                },
+                {
+                    "x": 5.698757430246908,
+                    "y": 6.703464865514997,
+                    "heading": -0.011789079064840387,
+                    "angularVelocity": 5.544360147801105e-8,
+                    "velocityX": -4.102617574484819,
+                    "velocityY": -0.8086332734327347,
+                    "timestamp": 3.89800091542207
+                },
+                {
+                    "x": 5.33581964060784,
+                    "y": 6.631929188287501,
+                    "heading": -0.011789074159930215,
+                    "angularVelocity": 5.5444683287550804e-8,
+                    "velocityX": -4.102617598684197,
+                    "velocityY": -0.8086331506542177,
+                    "timestamp": 3.9864658480922435
+                },
+                {
+                    "x": 5.008416015419383,
+                    "y": 6.567395761473697,
+                    "heading": 0.06462490200896195,
+                    "angularVelocity": 0.8637770228547912,
+                    "velocityX": -3.7009424560252184,
+                    "velocityY": -0.7294803134526322,
+                    "timestamp": 4.0749307807624175
+                },
+                {
+                    "x": 4.727784426794771,
+                    "y": 6.512080858011152,
+                    "heading": 0.13012220422079274,
+                    "angularVelocity": 0.7403758781575775,
+                    "velocityX": -3.172235372301678,
+                    "velocityY": -0.625274917336753,
+                    "timestamp": 4.163395713432592
+                },
+                {
+                    "x": 4.493924797623965,
+                    "y": 6.465984740956769,
+                    "heading": 0.18470070193798652,
+                    "angularVelocity": 0.6169506500466159,
+                    "velocityX": -2.6435291602234257,
+                    "velocityY": -0.5210665476482677,
+                    "timestamp": 4.251860646102766
+                },
+                {
+                    "x": 4.30683709599071,
+                    "y": 6.429107629337118,
+                    "heading": 0.22836093103558394,
+                    "angularVelocity": 0.4935314794210844,
+                    "velocityX": -2.1148233089237682,
+                    "velocityY": -0.4168557021022203,
+                    "timestamp": 4.34032557877294
+                },
+                {
+                    "x": 4.166521316526833,
+                    "y": 6.401449688021447,
+                    "heading": 0.26110453763292085,
+                    "angularVelocity": 0.3701309163871272,
+                    "velocityX": -1.5861175183054803,
+                    "velocityY": -0.3126429928883621,
+                    "timestamp": 4.428790511443114
+                },
+                {
+                    "x": 4.072977463595493,
+                    "y": 6.383011025435591,
+                    "heading": 0.2829331362314755,
+                    "angularVelocity": 0.24674860353919897,
+                    "velocityX": -1.0574116783663996,
+                    "velocityY": -0.20842905803818979,
+                    "timestamp": 4.517255444113288
+                },
+                {
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "angularVelocity": 0.12337586761318343,
+                    "velocityX": -0.5287058100920702,
+                    "velocityY": -0.10421452338466086,
+                    "timestamp": 4.605720376783462
+                },
+                {
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "angularVelocity": -3.545196144958051e-23,
+                    "velocityX": -1.2710664244245501e-21,
+                    "velocityY": 5.161280003041427e-22,
+                    "timestamp": 4.694185309453636
+                },
+                {
+                    "x": 4.048121639777245,
+                    "y": 6.381836957223538,
+                    "heading": 0.2879219902009138,
+                    "angularVelocity": -0.09569046695813048,
+                    "velocityX": 0.35391649199799335,
+                    "velocityY": 0.12992051965342163,
+                    "timestamp": 4.7561098045517625
+                },
+                {
+                    "x": 4.09210978411728,
+                    "y": 6.3975053544995015,
+                    "heading": 0.2763091104232377,
+                    "angularVelocity": -0.18753289403933296,
+                    "velocityX": 0.7103512797372235,
+                    "velocityY": 0.25302422330831614,
+                    "timestamp": 4.818034299649889
+                },
+                {
+                    "x": 4.1583483462814845,
+                    "y": 6.420279656151659,
+                    "heading": 0.2593011262084734,
+                    "angularVelocity": -0.27465680887366445,
+                    "velocityX": 1.0696665682819408,
+                    "velocityY": 0.3677753305226003,
+                    "timestamp": 4.879958794748015
+                },
+                {
+                    "x": 4.247041291177504,
+                    "y": 6.4495126487830605,
+                    "heading": 0.2372637765315283,
+                    "angularVelocity": -0.35587451527904157,
+                    "velocityX": 1.4322756246211616,
+                    "velocityY": 0.4720747837358832,
+                    "timestamp": 4.941883289846142
+                },
+                {
+                    "x": 4.358419897352032,
+                    "y": 6.484373747552581,
+                    "heading": 0.21066653247243172,
+                    "angularVelocity": -0.4295108747669278,
+                    "velocityX": 1.7986195284763546,
+                    "velocityY": 0.5629613727859919,
+                    "timestamp": 5.003807784944268
+                },
+                {
+                    "x": 4.492739145472355,
+                    "y": 6.523763939464936,
+                    "heading": 0.18013087413356532,
+                    "angularVelocity": -0.49311113946878693,
+                    "velocityX": 2.16908103824632,
+                    "velocityY": 0.6361003323472623,
+                    "timestamp": 5.065732280042394
+                },
+                {
+                    "x": 4.650257527226494,
+                    "y": 6.56617435469435,
+                    "heading": 0.14651099259101713,
+                    "angularVelocity": -0.5429173300367439,
+                    "velocityX": 2.5437168523462796,
+                    "velocityY": 0.6848730080432686,
+                    "timestamp": 5.127656775140521
+                },
+                {
+                    "x": 4.831166710355601,
+                    "y": 6.609441995709787,
+                    "heading": 0.11103466181531912,
+                    "angularVelocity": -0.5728965689503284,
+                    "velocityX": 2.921447851006871,
+                    "velocityY": 0.6987160887928849,
+                    "timestamp": 5.189581270238647
+                },
+                {
+                    "x": 5.035367274609871,
+                    "y": 6.650325338062944,
+                    "heading": 0.0755552485953242,
+                    "angularVelocity": -0.5729463464138673,
+                    "velocityX": 3.297573342030341,
+                    "velocityY": 0.6602127686042919,
+                    "timestamp": 5.251505765336773
+                },
+                {
+                    "x": 5.261781944494072,
+                    "y": 6.683862994996324,
+                    "heading": 0.042963975482345464,
+                    "angularVelocity": -0.5263066426514128,
+                    "velocityX": 3.6563022358991017,
+                    "velocityY": 0.541589509615474,
+                    "timestamp": 5.3134302604349
+                },
+                {
+                    "x": 5.506627789251162,
+                    "y": 6.703117076456781,
+                    "heading": 0.017406668728306682,
+                    "angularVelocity": -0.4127172407871934,
+                    "velocityX": 3.9539417215934254,
+                    "velocityY": 0.31092835605597996,
+                    "timestamp": 5.375354755533026
+                },
+                {
+                    "x": 5.761751812356917,
+                    "y": 6.701608512470805,
+                    "heading": 0.003171834984879961,
+                    "angularVelocity": -0.22987403806635848,
+                    "velocityX": 4.119920924691961,
+                    "velocityY": -0.024361344950586963,
+                    "timestamp": 5.4372792506311525
+                },
+                {
+                    "x": 6.018601417541504,
+                    "y": 6.676982402801514,
+                    "heading": -1.3559620024912911e-24,
+                    "angularVelocity": -0.051221006806011604,
+                    "velocityX": 4.147786829389234,
+                    "velocityY": -0.39767961983813094,
+                    "timestamp": 5.499203745729279
+                },
+                {
+                    "x": 6.298786009014622,
+                    "y": 6.621524313440803,
+                    "heading": -4.414364457925229e-8,
+                    "angularVelocity": -6.462732937493416e-7,
+                    "velocityX": 4.101968029940426,
+                    "velocityY": -0.8119194148513419,
+                    "timestamp": 5.567508664107004
+                },
+                {
+                    "x": 6.570847718915352,
+                    "y": 6.538762468325385,
+                    "heading": -7.033434259435762e-8,
+                    "angularVelocity": -3.8343795201205294e-7,
+                    "velocityX": 3.983047141586983,
+                    "velocityY": -1.2116527928156882,
+                    "timestamp": 5.6358135824847295
+                },
+                {
+                    "x": 6.822289711502508,
+                    "y": 6.436346018958693,
+                    "heading": -7.949311143160773e-8,
+                    "angularVelocity": -1.3408652048454663e-7,
+                    "velocityX": 3.6811696516008747,
+                    "velocityY": -1.499400801569386,
+                    "timestamp": 5.704118500862455
+                },
+                {
+                    "x": 7.0460752580417605,
+                    "y": 6.327103349848984,
+                    "heading": -7.703336636195349e-8,
+                    "angularVelocity": 3.6011243817786175e-8,
+                    "velocityX": 3.2762728051547096,
+                    "velocityY": -1.5993382571017543,
+                    "timestamp": 5.77242341924018
+                },
+                {
+                    "x": 7.241490832586327,
+                    "y": 6.220433828516969,
+                    "heading": -6.8157674853432e-8,
+                    "angularVelocity": 1.2994220210379365e-7,
+                    "velocityX": 2.8609297717613993,
+                    "velocityY": -1.5616667710827985,
+                    "timestamp": 5.8407283376179056
+                },
+                {
+                    "x": 7.409535997448797,
+                    "y": 6.121659695713664,
+                    "heading": -5.6202483858229566e-8,
+                    "angularVelocity": 1.750267957146284e-7,
+                    "velocityX": 2.460220564691751,
+                    "velocityY": -1.4460764341607861,
+                    "timestamp": 5.909033255995631
+                },
+                {
+                    "x": 7.551302074325524,
+                    "y": 6.033880383414205,
+                    "heading": -4.325249063148238e-8,
+                    "angularVelocity": 1.895909333370981e-7,
+                    "velocityX": 2.0754885628112913,
+                    "velocityY": -1.2851096873294061,
+                    "timestamp": 5.977338174373356
+                },
+                {
+                    "x": 7.667694153636149,
+                    "y": 5.959048535338257,
+                    "heading": -3.068159533861856e-8,
+                    "angularVelocity": 1.840408508117524e-7,
+                    "velocityX": 1.7040072966192434,
+                    "velocityY": -1.095555779191907,
+                    "timestamp": 6.045643092751082
+                },
+                {
+                    "x": 7.7594304227528665,
+                    "y": 5.89848440024899,
+                    "heading": -1.9448297882684093e-8,
+                    "angularVelocity": 1.644581052540686e-7,
+                    "velocityX": 1.343040461733904,
+                    "velocityY": -0.8866731199991837,
+                    "timestamp": 6.113948011128807
+                },
+                {
+                    "x": 7.827081864823745,
+                    "y": 5.853132028276085,
+                    "heading": -1.0253962002990125e-8,
+                    "angularVelocity": 1.3460723031465246e-7,
+                    "velocityX": 0.9904329538433378,
+                    "velocityY": -0.6639693458398959,
+                    "timestamp": 6.182252929506532
+                },
+                {
+                    "x": 7.8711085813221855,
+                    "y": 5.823696596603824,
+                    "heading": -3.6318337075530827e-9,
+                    "angularVelocity": 9.69495089477567e-8,
+                    "velocityX": 0.6445614392652327,
+                    "velocityY": -0.430941612571493,
+                    "timestamp": 6.250557847884258
+                },
+                {
+                    "x": 7.891887187957764,
+                    "y": 5.810723304748535,
+                    "heading": -2.0933118512576364e-26,
+                    "angularVelocity": 5.317089594440484e-8,
+                    "velocityX": 0.30420366686733796,
+                    "velocityY": -0.18993203071478146,
+                    "timestamp": 6.318862766261983
+                },
+                {
+                    "x": 7.888143904798224,
+                    "y": 5.815954272618714,
+                    "heading": 1.1238376512921276e-10,
+                    "angularVelocity": 1.5522619182937645e-9,
+                    "velocityX": -0.05170280503827788,
+                    "velocityY": 0.0722509359902741,
+                    "timestamp": 6.391262767708095
+                },
+                {
+                    "x": 7.858277731902267,
+                    "y": 5.8396753308857665,
+                    "heading": -3.1980355240507115e-9,
+                    "angularVelocity": -4.572402241792647e-8,
+                    "velocityX": -0.41251619198083334,
+                    "velocityY": 0.32763891979626797,
+                    "timestamp": 6.463662769154206
+                },
+                {
+                    "x": 7.801862145221396,
+                    "y": 5.881268642056604,
+                    "heading": -9.54060326150597e-9,
+                    "angularVelocity": -8.760452501062605e-8,
+                    "velocityX": -0.7792207949451817,
+                    "velocityY": 0.5744932367411117,
+                    "timestamp": 6.536062770600318
+                },
+                {
+                    "x": 7.718377542515441,
+                    "y": 5.939936997849849,
+                    "heading": -1.8417788083637822e-8,
+                    "angularVelocity": -1.226130475803823e-7,
+                    "velocityX": -1.1531022242878466,
+                    "velocityY": 0.8103363897984549,
+                    "timestamp": 6.60846277204643
+                },
+                {
+                    "x": 7.607181391684359,
+                    "y": 6.014616563892487,
+                    "heading": -2.9176471451729494e-8,
+                    "angularVelocity": -1.4860059603865473e-7,
+                    "velocityX": -1.5358584062162697,
+                    "velocityY": 1.0314856982181417,
+                    "timestamp": 6.680862773492541
+                },
+                {
+                    "x": 7.4674691498308405,
+                    "y": 6.103826142786331,
+                    "heading": -4.092645392613916e-8,
+                    "angularVelocity": -1.6229257237177443e-7,
+                    "velocityX": -1.9297270588800863,
+                    "velocityY": 1.2321764794472392,
+                    "timestamp": 6.753262774938653
+                },
+                {
+                    "x": 7.298232520371032,
+                    "y": 6.205388827036514,
+                    "heading": -5.239643950278259e-8,
+                    "angularVelocity": -1.5842521198263621e-7,
+                    "velocityX": -2.337522459661463,
+                    "velocityY": 1.402799478198597,
+                    "timestamp": 6.825662776384765
+                },
+                {
+                    "x": 7.0982622904373915,
+                    "y": 6.315880924025181,
+                    "heading": -6.166708665777315e-8,
+                    "angularVelocity": -1.2804761008037847e-7,
+                    "velocityX": -2.762019695296288,
+                    "velocityY": 1.526133905824668,
+                    "timestamp": 6.8980627778308765
+                },
+                {
+                    "x": 6.866440603568656,
+                    "y": 6.429499233364628,
+                    "heading": -6.568320426031161e-8,
+                    "angularVelocity": -5.547123649614453e-8,
+                    "velocityX": -3.2019569369937573,
+                    "velocityY": 1.5693136335641493,
+                    "timestamp": 6.970462779276988
+                },
+                {
+                    "x": 6.60338911605519,
+                    "y": 6.536118404749743,
+                    "heading": -5.966101479819827e-8,
+                    "angularVelocity": 8.317941079871027e-8,
+                    "velocityX": -3.6333077660123805,
+                    "velocityY": 1.472640459330275,
+                    "timestamp": 7.0428627807231
+                },
+                {
+                    "x": 6.316249006337029,
+                    "y": 6.621665638178002,
+                    "heading": -3.828640775690033e-8,
+                    "angularVelocity": 2.9522937312656454e-7,
+                    "velocityX": -3.9660235356747005,
+                    "velocityY": 1.1815915983362697,
+                    "timestamp": 7.115262782169212
+                },
+                {
+                    "x": 6.018601417541504,
+                    "y": 6.676982402801514,
+                    "heading": 7.9356951670816615e-25,
+                    "angularVelocity": 5.288177761349547e-7,
+                    "velocityX": -4.11115445925878,
+                    "velocityY": 0.7640436950085439,
+                    "timestamp": 7.187662783615323
+                },
+                {
+                    "x": 5.740842572720578,
+                    "y": 6.700963658984785,
+                    "heading": 0.0018231984576357557,
+                    "angularVelocity": 0.027294778663288,
+                    "velocityX": -4.158278085090587,
+                    "velocityY": 0.35901910559907296,
+                    "timestamp": 7.2544593852704224
+                },
+                {
+                    "x": 5.465613553779653,
+                    "y": 6.698113364745584,
+                    "heading": 0.01705607864209539,
+                    "angularVelocity": 0.2280487301302209,
+                    "velocityX": -4.12040451342204,
+                    "velocityY": -0.04267124626965841,
+                    "timestamp": 7.321255986925522
+                },
+                {
+                    "x": 5.204358328316174,
+                    "y": 6.672254128794052,
+                    "heading": 0.046261831986706164,
+                    "angularVelocity": 0.43723412001426465,
+                    "velocityX": -3.911205345632041,
+                    "velocityY": -0.3871340054851043,
+                    "timestamp": 7.388052588580621
+                },
+                {
+                    "x": 4.966474282232876,
+                    "y": 6.6325844049513245,
+                    "heading": 0.0830771823140007,
+                    "angularVelocity": 0.5511560381078753,
+                    "velocityX": -3.5613195909516313,
+                    "velocityY": -0.593888354493849,
+                    "timestamp": 7.45484919023572
+                },
+                {
+                    "x": 4.755211974738486,
+                    "y": 6.587176755241801,
+                    "heading": 0.12225070699122965,
+                    "angularVelocity": 0.5864598453600871,
+                    "velocityX": -3.162770294591211,
+                    "velocityY": -0.6797898184099774,
+                    "timestamp": 7.521645791890819
+                },
+                {
+                    "x": 4.571183406962543,
+                    "y": 6.54114573684605,
+                    "heading": 0.16051429938187217,
+                    "angularVelocity": 0.5728374115230364,
+                    "velocityX": -2.7550588385643326,
+                    "velocityY": -0.6891221597384368,
+                    "timestamp": 7.588442393545918
+                },
+                {
+                    "x": 4.414262701798846,
+                    "y": 6.497671687179865,
+                    "heading": 0.1958556202852531,
+                    "angularVelocity": 0.5290886067208012,
+                    "velocityX": -2.3492318662250016,
+                    "velocityY": -0.6508422373141216,
+                    "timestamp": 7.655238995201017
+                },
+                {
+                    "x": 4.284155652887569,
+                    "y": 6.458840678668644,
+                    "heading": 0.2269562205060937,
+                    "angularVelocity": 0.46560153436288354,
+                    "velocityX": -1.9478094047819534,
+                    "velocityY": -0.5813320969788546,
+                    "timestamp": 7.7220355968561165
+                },
+                {
+                    "x": 4.180556191608258,
+                    "y": 6.426101398719019,
+                    "heading": 0.25289929121670346,
+                    "angularVelocity": 0.38838908069852646,
+                    "velocityX": -1.5509690420216515,
+                    "velocityY": -0.49013391607366485,
+                    "timestamp": 7.788832198511216
+                },
+                {
+                    "x": 4.103186902723614,
+                    "y": 6.4005094893734436,
+                    "heading": 0.27301561090145554,
+                    "angularVelocity": 0.301157831181616,
+                    "velocityX": -1.1582818132595463,
+                    "velocityY": -0.38313190658586416,
+                    "timestamp": 7.855628800166315
+                },
+                {
+                    "x": 4.051806124428388,
+                    "y": 6.382864606916464,
+                    "heading": 0.28679744635493554,
+                    "angularVelocity": 0.20632539847822937,
+                    "velocityX": -0.7692124602465237,
+                    "velocityY": -0.2641583856030227,
+                    "timestamp": 7.922425401821414
+                },
+                {
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "angularVelocity": 0.10554620330039294,
+                    "velocityX": -0.3832617841429427,
+                    "velocityY": -0.13582895013430246,
+                    "timestamp": 7.989222003476513
+                },
+                {
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "angularVelocity": 2.005442344245764e-24,
+                    "velocityX": 1.8885981121979073e-24,
+                    "velocityY": 6.519144438430986e-24,
+                    "timestamp": 8.056018605131612
+                },
+                {
+                    "x": 4.0487237842181845,
+                    "y": 6.381905764315973,
+                    "heading": 0.289763925680258,
+                    "angularVelocity": -0.0651795729135273,
+                    "velocityX": 0.35941624407112255,
+                    "velocityY": 0.12950958254097514,
+                    "timestamp": 8.118670877771688
+                },
+                {
+                    "x": 4.09402733620905,
+                    "y": 6.397352444809068,
+                    "heading": 0.28163512088907877,
+                    "angularVelocity": -0.1297447713968433,
+                    "velocityX": 0.7230951102304722,
+                    "velocityY": 0.24654621200788543,
+                    "timestamp": 8.181323150411764
+                },
+                {
+                    "x": 4.162408374331436,
+                    "y": 6.419155700762566,
+                    "heading": 0.2695155327794676,
+                    "angularVelocity": -0.19344211468968972,
+                    "velocityX": 1.0914374729105671,
+                    "velocityY": 0.3480042308880483,
+                    "timestamp": 8.24397542305184
+                },
+                {
+                    "x": 4.254175470208227,
+                    "y": 6.446070164240337,
+                    "heading": 0.25348421107280095,
+                    "angularVelocity": -0.255877736451212,
+                    "velocityX": 1.4647049821157165,
+                    "velocityY": 0.4295847914789773,
+                    "timestamp": 8.306627695691915
+                },
+                {
+                    "x": 4.369627429708199,
+                    "y": 6.476469024717609,
+                    "heading": 0.23366000099411213,
+                    "angularVelocity": -0.3164164561527553,
+                    "velocityX": 1.8427417655416987,
+                    "velocityY": 0.4851996455405065,
+                    "timestamp": 8.369279968331991
+                },
+                {
+                    "x": 4.508985987745745,
+                    "y": 6.508175804754511,
+                    "heading": 0.2102282566283648,
+                    "angularVelocity": -0.3739967183689891,
+                    "velocityX": 2.224317684980605,
+                    "velocityY": 0.5060754973574803,
+                    "timestamp": 8.431932240972067
+                },
+                {
+                    "x": 4.6722331784478675,
+                    "y": 6.538223031668598,
+                    "heading": 0.18348837802396292,
+                    "angularVelocity": -0.426798222596284,
+                    "velocityX": 2.605606849091409,
+                    "velocityY": 0.47958718252252885,
+                    "timestamp": 8.494584513612143
+                },
+                {
+                    "x": 4.858743027816154,
+                    "y": 6.562565877888605,
+                    "heading": 0.15393434470481,
+                    "angularVelocity": -0.4717152638490063,
+                    "velocityX": 2.9769047715116295,
+                    "velocityY": 0.3885389179711166,
+                    "timestamp": 8.557236786252219
+                },
+                {
+                    "x": 5.066570002168237,
+                    "y": 6.575971754133761,
+                    "heading": 0.1223656565639417,
+                    "angularVelocity": -0.5038713970078006,
+                    "velocityX": 3.3171498110851902,
+                    "velocityY": 0.21397270490362202,
+                    "timestamp": 8.619889058892294
+                },
+                {
+                    "x": 5.291589628159925,
+                    "y": 6.572681851877193,
+                    "heading": 0.08994651870042891,
+                    "angularVelocity": -0.5174455210867492,
+                    "velocityX": 3.591563665764864,
+                    "velocityY": -0.05251050150194843,
+                    "timestamp": 8.68254133153237
+                },
+                {
+                    "x": 5.527619341276251,
+                    "y": 6.548104466256015,
+                    "heading": 0.05802485635108468,
+                    "angularVelocity": -0.5095052582166943,
+                    "velocityX": 3.767296909918484,
+                    "velocityY": -0.3922824278437735,
+                    "timestamp": 8.745193604172446
+                },
+                {
+                    "x": 5.768129964554371,
+                    "y": 6.499988313354724,
+                    "heading": 0.027769937254171585,
+                    "angularVelocity": -0.48290218091084397,
+                    "velocityX": 3.8388172231166338,
+                    "velocityY": -0.7679873510368684,
+                    "timestamp": 8.807845876812522
+                },
+                {
+                    "x": 6.007773399353027,
+                    "y": 6.4279327392578125,
+                    "heading": 4.2309997245679596e-24,
+                    "angularVelocity": -0.44323910504738345,
+                    "velocityX": 3.8249759298495123,
+                    "velocityY": -1.1500871566280768,
+                    "timestamp": 8.870498149452597
+                },
+                {
+                    "x": 6.229030281346825,
+                    "y": 6.339362395858419,
+                    "heading": -0.023466940923758877,
+                    "angularVelocity": -0.39811204237675923,
+                    "velocityX": 3.753579534147267,
+                    "velocityY": -1.5025784749406275,
+                    "timestamp": 8.929443718603581
+                },
+                {
+                    "x": 6.443849172872896,
+                    "y": 6.230687798127142,
+                    "heading": -0.039879946371075364,
+                    "angularVelocity": -0.27844341285900365,
+                    "velocityX": 3.6443602906917305,
+                    "velocityY": -1.8436431999310363,
+                    "timestamp": 8.988389287754565
+                },
+                {
+                    "x": 6.650509698951262,
+                    "y": 6.102546806166999,
+                    "heading": -0.049142702906474676,
+                    "angularVelocity": -0.15714084483048257,
+                    "velocityX": 3.505955223691552,
+                    "velocityY": -2.173886753589912,
+                    "timestamp": 9.047334856905549
+                },
+                {
+                    "x": 6.8468725839470075,
+                    "y": 5.955927867022489,
+                    "heading": -0.05303207582244352,
+                    "angularVelocity": -0.06598244740001617,
+                    "velocityX": 3.331257765834417,
+                    "velocityY": -2.487361497332514,
+                    "timestamp": 9.106280426056532
+                },
+                {
+                    "x": 7.030112126915743,
+                    "y": 5.792643403875283,
+                    "heading": -0.05586334395844049,
+                    "angularVelocity": -0.0480319076866473,
+                    "velocityX": 3.108622846602461,
+                    "velocityY": -2.7700888378728914,
+                    "timestamp": 9.165225995207516
+                },
+                {
+                    "x": 7.196822404953559,
+                    "y": 5.61624162712754,
+                    "heading": -0.06598691122862967,
+                    "angularVelocity": -0.17174432982839563,
+                    "velocityX": 2.8282071144448753,
+                    "velocityY": -2.992621486034079,
+                    "timestamp": 9.2241715643585
+                },
+                {
+                    "x": 7.344318297823079,
+                    "y": 5.433198235403824,
+                    "heading": -0.09660333760407158,
+                    "angularVelocity": -0.5194016584524004,
+                    "velocityX": 2.5022388449880117,
+                    "velocityY": -3.1052951792672387,
+                    "timestamp": 9.283117133509483
+                },
+                {
+                    "x": 7.472388333611968,
+                    "y": 5.252274949850564,
+                    "heading": -0.15660945149917227,
+                    "angularVelocity": -1.0179919332257161,
+                    "velocityX": 2.1726829960848693,
+                    "velocityY": -3.0693279946087215,
+                    "timestamp": 9.342062702660467
+                },
+                {
+                    "x": 7.582263019092128,
+                    "y": 5.081723782489176,
+                    "heading": -0.22798352839802205,
+                    "angularVelocity": -1.2108471921957489,
+                    "velocityX": 1.8640024528175283,
+                    "velocityY": -2.8933670472251234,
+                    "timestamp": 9.40100827181145
+                },
+                {
+                    "x": 7.676466487781464,
+                    "y": 4.9254488732019475,
+                    "heading": -0.3020784286066942,
+                    "angularVelocity": -1.2570054251047096,
+                    "velocityX": 1.5981433387815942,
+                    "velocityY": -2.6511731337590945,
+                    "timestamp": 9.459953840962434
+                },
+                {
+                    "x": 7.7568938758586405,
+                    "y": 4.785294368443394,
+                    "heading": -0.37402343822514994,
+                    "angularVelocity": -1.2205329536164857,
+                    "velocityX": 1.364434837691845,
+                    "velocityY": -2.3776936379994624,
+                    "timestamp": 9.518899410113418
+                },
+                {
+                    "x": 7.824872010815036,
+                    "y": 4.662247591209576,
+                    "heading": -0.4407895129961546,
+                    "angularVelocity": -1.1326733414005847,
+                    "velocityX": 1.1532357043202854,
+                    "velocityY": -2.0874644015845227,
+                    "timestamp": 9.577844979264402
+                },
+                {
+                    "x": 7.88135001784302,
+                    "y": 4.556897034423127,
+                    "heading": -0.5003478187596111,
+                    "angularVelocity": -1.0103949562502774,
+                    "velocityX": 0.9581382933689164,
+                    "velocityY": -1.7872514983543952,
+                    "timestamp": 9.636790548415386
+                },
+                {
+                    "x": 7.927030718663331,
+                    "y": 4.469623470627648,
+                    "heading": -0.5512507357361617,
+                    "angularVelocity": -0.863557985947457,
+                    "velocityX": 0.7749641148311569,
+                    "velocityY": -1.4805788637299317,
+                    "timestamp": 9.69573611756637
+                },
+                {
+                    "x": 7.962451814207847,
+                    "y": 4.400689098568436,
+                    "heading": -0.5924140389845107,
+                    "angularVelocity": -0.6983273525260651,
+                    "velocityX": 0.6009119269641297,
+                    "velocityY": -1.1694580789039828,
+                    "timestamp": 9.754681686717353
+                },
+                {
+                    "x": 7.988036352165696,
+                    "y": 4.350283491457437,
+                    "heading": -0.6229985575776359,
+                    "angularVelocity": -0.5188603491941064,
+                    "velocityX": 0.434036660029808,
+                    "velocityY": -0.8551212217815544,
+                    "timestamp": 9.813627255868337
+                },
+                {
+                    "x": 8.004125055669116,
+                    "y": 4.31854932275225,
+                    "heading": -0.6423430672717187,
+                    "angularVelocity": -0.3281758064721266,
+                    "velocityX": 0.27294169409427244,
+                    "velocityY": -0.5383639374810789,
+                    "timestamp": 9.87257282501932
+                },
+                {
+                    "x": 8.010997772216797,
+                    "y": 4.30559778213501,
+                    "heading": -0.6499245399095859,
+                    "angularVelocity": -0.12861819381958664,
+                    "velocityX": 0.1165942859941986,
+                    "velocityY": -0.21972034206788113,
+                    "timestamp": 9.931518394170304
+                },
+                {
+                    "x": 8.008640625334808,
+                    "y": 4.3120713019161,
+                    "heading": -0.6449603194163791,
+                    "angularVelocity": 0.08240933446076017,
+                    "velocityX": -0.03913019295512813,
+                    "velocityY": 0.10746469813501351,
+                    "timestamp": 9.991756964542093
+                },
+                {
+                    "x": 7.996662965285712,
+                    "y": 4.338162984806749,
+                    "heading": -0.6277276549089263,
+                    "angularVelocity": 0.2860735970507537,
+                    "velocityX": -0.19883705697477932,
+                    "velocityY": 0.43313914539493714,
+                    "timestamp": 10.051995534913882
+                },
+                {
+                    "x": 7.9747782083324354,
+                    "y": 4.383759514989304,
+                    "heading": -0.598776455290747,
+                    "angularVelocity": 0.4806090091364049,
+                    "velocityX": -0.36330140005325046,
+                    "velocityY": 0.75693247534156,
+                    "timestamp": 10.11223410528567
+                },
+                {
+                    "x": 7.942638409560382,
+                    "y": 4.448715501539106,
+                    "heading": -0.5587851834199348,
+                    "angularVelocity": 0.6638814902808625,
+                    "velocityX": -0.533541858209585,
+                    "velocityY": 1.0783122200427058,
+                    "timestamp": 10.17247267565746
+                },
+                {
+                    "x": 7.89981288541696,
+                    "y": 4.532838211183571,
+                    "heading": -0.5085984817650532,
+                    "angularVelocity": 0.8331323493424854,
+                    "velocityX": -0.7109319474068704,
+                    "velocityY": 1.3964924652969615,
+                    "timestamp": 10.232711246029249
+                },
+                {
+                    "x": 7.845756103945311,
+                    "y": 4.635862370094525,
+                    "heading": -0.4492930873299135,
+                    "angularVelocity": 0.9845086639525084,
+                    "velocityX": -0.8973782269069894,
+                    "velocityY": 1.710268990035694,
+                    "timestamp": 10.292949816401038
+                },
+                {
+                    "x": 7.779757952213243,
+                    "y": 4.757405734926412,
+                    "heading": -0.382295753262504,
+                    "angularVelocity": 1.1121999352558545,
+                    "velocityX": -1.0956128494539268,
+                    "velocityY": 2.0177000231201787,
+                    "timestamp": 10.353188386772826
+                },
+                {
+                    "x": 7.700864385195838,
+                    "y": 4.896884050165335,
+                    "heading": -0.3095988880574394,
+                    "angularVelocity": 1.2068159114066472,
+                    "velocityX": -1.3096852486783623,
+                    "velocityY": 2.315432029313937,
+                    "timestamp": 10.413426957144615
+                },
+                {
+                    "x": 7.607749192737627,
+                    "y": 5.0533313100207735,
+                    "heading": -0.23416819513379072,
+                    "angularVelocity": 1.252199254698346,
+                    "velocityX": -1.5457736112180178,
+                    "velocityY": 2.597127702232213,
+                    "timestamp": 10.473665527516404
+                },
+                {
+                    "x": 7.4985230728620555,
+                    "y": 5.224975559172228,
+                    "heading": -0.16073992185561656,
+                    "angularVelocity": 1.2189577678384218,
+                    "velocityX": -1.8132256327039729,
+                    "velocityY": 2.8494077480935314,
+                    "timestamp": 10.533904097888193
+                },
+                {
+                    "x": 7.370615754860522,
+                    "y": 5.408140956613431,
+                    "heading": -0.09743106837998239,
+                    "angularVelocity": 1.0509687246044361,
+                    "velocityX": -2.123345843237925,
+                    "velocityY": 3.0406664087596154,
+                    "timestamp": 10.594142668259982
+                },
+                {
+                    "x": 7.222231012592685,
+                    "y": 5.5947592821558745,
+                    "heading": -0.061240832593330456,
+                    "angularVelocity": 0.600781784217115,
+                    "velocityX": -2.4632845924465627,
+                    "velocityY": 3.097987292703733,
+                    "timestamp": 10.654381238631771
+                },
+                {
+                    "x": 7.053607883062888,
+                    "y": 5.775645840558194,
+                    "heading": -0.04890249083117564,
+                    "angularVelocity": 0.20482461130805693,
+                    "velocityX": -2.7992551697203822,
+                    "velocityY": 3.0028361776499053,
+                    "timestamp": 10.71461980900356
+                },
+                {
+                    "x": 6.867274277323916,
+                    "y": 5.943523141012965,
+                    "heading": -0.04597480359183944,
+                    "angularVelocity": 0.04860153920099139,
+                    "velocityX": -3.0932607561721848,
+                    "velocityY": 2.7868739151450646,
+                    "timestamp": 10.774858379375349
+                },
+                {
+                    "x": 6.666800620063063,
+                    "y": 6.094343858364721,
+                    "heading": -0.04319414895342738,
+                    "angularVelocity": 0.04616070104668807,
+                    "velocityX": -3.3279949378535707,
+                    "velocityY": 2.503723385546804,
+                    "timestamp": 10.835096949747138
+                },
+                {
+                    "x": 6.455252510472756,
+                    "y": 6.226027819661062,
+                    "heading": -0.03567675833355503,
+                    "angularVelocity": 0.12479364256945982,
+                    "velocityX": -3.511838150949524,
+                    "velocityY": 2.18604061290953,
+                    "timestamp": 10.895335520118927
+                },
+                {
+                    "x": 6.234971447573829,
+                    "y": 6.337451095987177,
+                    "heading": -0.021378533882876995,
+                    "angularVelocity": 0.23735995662629603,
+                    "velocityX": -3.6568109359063894,
+                    "velocityY": 1.8496998789714996,
+                    "timestamp": 10.955574090490716
+                },
+                {
+                    "x": 6.007773399353027,
+                    "y": 6.4279327392578125,
+                    "heading": 1.0451301322490283e-23,
+                    "angularVelocity": 0.3548977631927474,
+                    "velocityX": -3.771637454517041,
+                    "velocityY": 1.5020549576822213,
+                    "timestamp": 11.015812660862505
+                },
+                {
+                    "x": 5.747502918390015,
+                    "y": 6.502130719218711,
+                    "heading": 0.02823606248628203,
+                    "angularVelocity": 0.41875457589826204,
+                    "velocityX": -3.8599381527595638,
+                    "velocityY": 1.1003922252307357,
+                    "timestamp": 11.08324132698684
+                },
+                {
+                    "x": 5.486616061564071,
+                    "y": 6.54859640899773,
+                    "heading": 0.06008791313844042,
+                    "angularVelocity": 0.47237847762588864,
+                    "velocityX": -3.869079307380576,
+                    "velocityY": 0.6891088382697167,
+                    "timestamp": 11.150669993111176
+                },
+                {
+                    "x": 5.232615422686886,
+                    "y": 6.568179274186433,
+                    "heading": 0.09435854776952027,
+                    "angularVelocity": 0.5082502235456623,
+                    "velocityX": -3.76695333715803,
+                    "velocityY": 0.29042344027083444,
+                    "timestamp": 11.218098659235512
+                },
+                {
+                    "x": 4.994185583794339,
+                    "y": 6.564780827927549,
+                    "heading": 0.12929833940251856,
+                    "angularVelocity": 0.5181741481964114,
+                    "velocityX": -3.5360307803344355,
+                    "velocityY": -0.05040061526083558,
+                    "timestamp": 11.285527325359848
+                },
+                {
+                    "x": 4.7782894744816735,
+                    "y": 6.54517331085893,
+                    "heading": 0.1630448352516845,
+                    "angularVelocity": 0.5004769898152576,
+                    "velocityX": -3.201844582162713,
+                    "velocityY": -0.29078903967139774,
+                    "timestamp": 11.352955991484183
+                },
+                {
+                    "x": 4.588583133594499,
+                    "y": 6.516394405425367,
+                    "heading": 0.19417829885666568,
+                    "angularVelocity": 0.4617244473970761,
+                    "velocityX": -2.813437545054831,
+                    "velocityY": -0.42680520152208584,
+                    "timestamp": 11.420384657608519
+                },
+                {
+                    "x": 4.426380144437834,
+                    "y": 6.483997287524405,
+                    "heading": 0.22180064905808416,
+                    "angularVelocity": 0.4096529234388779,
+                    "velocityX": -2.4055494269687476,
+                    "velocityY": -0.4804650568235182,
+                    "timestamp": 11.487813323732855
+                },
+                {
+                    "x": 4.29190849535161,
+                    "y": 6.4519615961956065,
+                    "heading": 0.24536762088418218,
+                    "angularVelocity": 0.3495096845404179,
+                    "velocityX": -1.994280130623705,
+                    "velocityY": -0.47510492450978464,
+                    "timestamp": 11.55524198985719
+                },
+                {
+                    "x": 4.184982062888447,
+                    "y": 6.423114091223226,
+                    "heading": 0.26454171695590006,
+                    "angularVelocity": 0.2843611949309722,
+                    "velocityX": -1.5857711357658024,
+                    "velocityY": -0.42782256613509534,
+                    "timestamp": 11.622670655981526
+                },
+                {
+                    "x": 4.105281875933628,
+                    "y": 6.39950678354966,
+                    "heading": 0.2791044377060579,
+                    "angularVelocity": 0.21597225018962463,
+                    "velocityX": -1.1819926380844537,
+                    "velocityY": -0.3501078848280095,
+                    "timestamp": 11.690099322105862
+                },
+                {
+                    "x": 4.052465191081701,
+                    "y": 6.382673090712085,
+                    "heading": 0.2889078281274289,
+                    "angularVelocity": 0.1453890605413106,
+                    "velocityX": -0.7832971922436318,
+                    "velocityY": -0.24965187367837952,
+                    "timestamp": 11.757527988230198
+                },
+                {
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "angularVelocity": 0.07325884092771977,
+                    "velocityX": -0.38944343537672876,
+                    "velocityY": -0.13171543471725672,
+                    "timestamp": 11.824956654354533
+                },
+                {
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "angularVelocity": 8.278440610410196e-25,
+                    "velocityX": 6.433403533567858e-24,
+                    "velocityY": -1.0701732524530557e-23,
+                    "timestamp": 11.89238532047887
+                },
+                {
+                    "x": 4.018087490415642,
+                    "y": 6.380018770347843,
+                    "heading": 0.2795341228567479,
+                    "angularVelocity": -0.3355412595403682,
+                    "velocityX": -0.1903063381197778,
+                    "velocityY": 0.1459774304073072,
+                    "timestamp": 11.935043118258669
+                },
+                {
+                    "x": 4.001589705706272,
+                    "y": 6.3921613257192265,
+                    "heading": 0.25120714606109396,
+                    "angularVelocity": -0.66405155141573,
+                    "velocityX": -0.3867472201572967,
+                    "velocityY": 0.28465031022144105,
+                    "timestamp": 11.977700916038469
+                },
+                {
+                    "x": 3.97639313263677,
+                    "y": 6.409754098035882,
+                    "heading": 0.20896838468257176,
+                    "angularVelocity": -0.9901767924485971,
+                    "velocityX": -0.5906674601339826,
+                    "velocityY": 0.4124163279002243,
+                    "timestamp": 12.020358713818268
+                },
+                {
+                    "x": 3.9421738742779104,
+                    "y": 6.432121957225729,
+                    "heading": 0.15224609434940017,
+                    "angularVelocity": -1.3297050782127584,
+                    "velocityX": -0.8021806126865689,
+                    "velocityY": 0.524355694715196,
+                    "timestamp": 12.063016511598068
+                },
+                {
+                    "x": 3.8986904655937336,
+                    "y": 6.458382475722753,
+                    "heading": 0.07940140875468289,
+                    "angularVelocity": -1.7076522789747404,
+                    "velocityX": -1.0193542786395084,
+                    "velocityY": 0.6156088655251506,
+                    "timestamp": 12.105674309377868
+                },
+                {
+                    "x": 3.8458035667226933,
+                    "y": 6.4875155211783255,
+                    "heading": -0.011842050030088008,
+                    "angularVelocity": -2.1389631798568893,
+                    "velocityX": -1.2397944015779776,
+                    "velocityY": 0.6829477134745302,
+                    "timestamp": 12.148332107157668
+                },
+                {
+                    "x": 3.7833094863246672,
+                    "y": 6.518292557296895,
+                    "heading": -0.12260101088673928,
+                    "angularVelocity": -2.59645285554573,
+                    "velocityX": -1.4650095328554629,
+                    "velocityY": 0.7214867555386046,
+                    "timestamp": 12.190989904937467
+                },
+                {
+                    "x": 3.7106930068716886,
+                    "y": 6.548467697277574,
+                    "heading": -0.24923492157262775,
+                    "angularVelocity": -2.9685993482264874,
+                    "velocityX": -1.702302585516201,
+                    "velocityY": 0.7073768818644702,
+                    "timestamp": 12.233647702717267
+                },
+                {
+                    "x": 3.6287066452771013,
+                    "y": 6.573438773987302,
+                    "heading": -0.38557434807314456,
+                    "angularVelocity": -3.196119668537644,
+                    "velocityX": -1.921954856127452,
+                    "velocityY": 0.5853812903945306,
+                    "timestamp": 12.276305500497067
+                },
+                {
+                    "x": 3.5407648103952,
+                    "y": 6.589073554075392,
+                    "heading": -0.5249239622169254,
+                    "angularVelocity": -3.266685609583233,
+                    "velocityX": -2.0615652813551293,
+                    "velocityY": 0.36651634406438516,
+                    "timestamp": 12.318963298276866
+                },
+                {
+                    "x": 3.4508628456235098,
+                    "y": 6.593818697188294,
+                    "heading": -0.660906842350814,
+                    "angularVelocity": -3.1877613756771255,
+                    "velocityX": -2.107515377042373,
+                    "velocityY": 0.11123741402210238,
+                    "timestamp": 12.361621096056666
+                },
+                {
+                    "x": 3.3616615472274485,
+                    "y": 6.587670279666556,
+                    "heading": -0.7900084359706764,
+                    "angularVelocity": -3.0264476916105565,
+                    "velocityX": -2.091090094629868,
+                    "velocityY": -0.14413349590798102,
+                    "timestamp": 12.404278893836466
+                },
+                {
+                    "x": 3.274774187468561,
+                    "y": 6.57100680311711,
+                    "heading": -0.9098799975312181,
+                    "angularVelocity": -2.8100738387697106,
+                    "velocityX": -2.036845882373094,
+                    "velocityY": -0.39063143004857287,
+                    "timestamp": 12.446936691616266
+                },
+                {
+                    "x": 3.191172742303364,
+                    "y": 6.544245662152579,
+                    "heading": -1.018505234653352,
+                    "angularVelocity": -2.546433308227988,
+                    "velocityX": -1.959816247354131,
+                    "velocityY": -0.6273446440595214,
+                    "timestamp": 12.489594489396065
+                },
+                {
+                    "x": 3.111446083115606,
+                    "y": 6.507722354665327,
+                    "heading": -1.1144658869059407,
+                    "angularVelocity": -2.2495453878782246,
+                    "velocityX": -1.8689820698037456,
+                    "velocityY": -0.856192991391337,
+                    "timestamp": 12.532252287175865
+                },
+                {
+                    "x": 3.035976307427136,
+                    "y": 6.461676426153115,
+                    "heading": -1.19697800039683,
+                    "angularVelocity": -1.9342797281008164,
+                    "velocityX": -1.7691906196856966,
+                    "velocityY": -1.079425823852935,
+                    "timestamp": 12.574910084955665
+                },
+                {
+                    "x": 2.9650378227233887,
+                    "y": 6.406276702880859,
+                    "heading": -1.265712572039586,
+                    "angularVelocity": -1.6113014553063978,
+                    "velocityX": -1.662966406984595,
+                    "velocityY": -1.2987009680675718,
+                    "timestamp": 12.617567882735464
+                },
+                {
+                    "x": 2.9126493602591794,
+                    "y": 6.357513140307781,
+                    "heading": -1.310830078079544,
+                    "angularVelocity": -1.357475138846794,
+                    "velocityX": -1.576240390915871,
+                    "velocityY": -1.4671760406243946,
+                    "timestamp": 12.650804223312786
+                },
+                {
+                    "x": 2.8630250654829577,
+                    "y": 6.303097416119462,
+                    "heading": -1.3474650887415063,
+                    "angularVelocity": -1.1022576500783743,
+                    "velocityX": -1.4930733622967893,
+                    "velocityY": -1.6372357258082417,
+                    "timestamp": 12.684040563890107
+                },
+                {
+                    "x": 2.816011074730038,
+                    "y": 6.242970636362372,
+                    "heading": -1.3754971781027352,
+                    "angularVelocity": -0.843416840551841,
+                    "velocityX": -1.4145357141092718,
+                    "velocityY": -1.809067385659103,
+                    "timestamp": 12.717276904467429
+                },
+                {
+                    "x": 2.771398801296672,
+                    "y": 6.177065835385166,
+                    "heading": -1.39470215946782,
+                    "angularVelocity": -0.5778308030153397,
+                    "velocityX": -1.3422739284302563,
+                    "velocityY": -1.982913877774391,
+                    "timestamp": 12.75051324504475
+                },
+                {
+                    "x": 2.7288922086629652,
+                    "y": 6.105306533827174,
+                    "heading": -1.404713958079907,
+                    "angularVelocity": -0.3012304735774339,
+                    "velocityX": -1.278919155820414,
+                    "velocityY": -2.1590614463421836,
+                    "timestamp": 12.783749585622072
+                },
+                {
+                    "x": 2.6880462952478212,
+                    "y": 6.027607098500584,
+                    "heading": -1.4049934257695869,
+                    "angularVelocity": -0.008408497591055405,
+                    "velocityX": -1.228953389742779,
+                    "velocityY": -2.3377855075780865,
+                    "timestamp": 12.816985926199393
+                },
+                {
+                    "x": 2.6481321260568067,
+                    "y": 5.943876209208477,
+                    "heading": -1.394939336924496,
+                    "angularVelocity": 0.30250288300244654,
+                    "velocityX": -1.2009194904643212,
+                    "velocityY": -2.5192571696428567,
+                    "timestamp": 12.850222266776715
+                },
+                {
+                    "x": 2.60770583152771,
+                    "y": 5.854036331176758,
+                    "heading": -1.374993138249586,
+                    "angularVelocity": 0.6001322145711994,
+                    "velocityX": -1.2163280862719847,
+                    "velocityY": -2.7030616629624156,
+                    "timestamp": 12.883458607354036
+                },
+                {
+                    "x": 2.5335974902090035,
+                    "y": 5.699627153541733,
+                    "heading": -1.326359964050082,
+                    "angularVelocity": 0.9256228980196083,
+                    "velocityX": -1.4104853073634442,
+                    "velocityY": -2.9388308050189744,
+                    "timestamp": 12.935999630597472
+                },
+                {
+                    "x": 2.448082198863997,
+                    "y": 5.538609196121231,
+                    "heading": -1.2379059045506466,
+                    "angularVelocity": 1.6835237313443032,
+                    "velocityX": -1.627590900709989,
+                    "velocityY": -3.0646140383385263,
+                    "timestamp": 12.988540653840907
+                },
+                {
+                    "x": 2.3666499769197493,
+                    "y": 5.393860387223226,
+                    "heading": -1.1439329584994455,
+                    "angularVelocity": 1.7885633025417567,
+                    "velocityX": -1.5498788740172182,
+                    "velocityY": -2.7549674513826976,
+                    "timestamp": 13.041081677084343
+                },
+                {
+                    "x": 2.2923433899929266,
+                    "y": 5.264310203060086,
+                    "heading": -1.0550447251034136,
+                    "angularVelocity": 1.6917872532514617,
+                    "velocityX": -1.4142584658571056,
+                    "velocityY": -2.465695872783135,
+                    "timestamp": 13.093622700327778
+                },
+                {
+                    "x": 2.2258000185216607,
+                    "y": 5.149549363728709,
+                    "heading": -0.9737081275164423,
+                    "angularVelocity": 1.5480588798227184,
+                    "velocityX": -1.2665031505563715,
+                    "velocityY": -2.1842140150119014,
+                    "timestamp": 13.146163723571213
+                },
+                {
+                    "x": 2.1672914591200874,
+                    "y": 5.049375589010017,
+                    "heading": -0.9010867791693077,
+                    "angularVelocity": 1.382183746415878,
+                    "velocityX": -1.1135786056257322,
+                    "velocityY": -1.9065821054638286,
+                    "timestamp": 13.198704746814649
+                },
+                {
+                    "x": 2.1169661488037037,
+                    "y": 4.963668228608927,
+                    "heading": -0.8378922046243602,
+                    "angularVelocity": 1.2027663460635734,
+                    "velocityX": -0.9578288965407921,
+                    "velocityY": -1.6312465024517704,
+                    "timestamp": 13.251245770058084
+                },
+                {
+                    "x": 2.07491635973485,
+                    "y": 4.8923475085199595,
+                    "heading": -0.7846102526515801,
+                    "angularVelocity": 1.0141019090152126,
+                    "velocityX": -0.8003229947393125,
+                    "velocityY": -1.3574292178993321,
+                    "timestamp": 13.30378679330152
+                },
+                {
+                    "x": 2.0412044621816885,
+                    "y": 4.835357214512788,
+                    "heading": -0.7415891809457651,
+                    "angularVelocity": 0.8188091713876277,
+                    "velocityX": -0.6416300154065587,
+                    "velocityY": -1.0846818445678552,
+                    "timestamp": 13.356327816544955
+                },
+                {
+                    "x": 2.0158754197230326,
+                    "y": 4.7926558969979665,
+                    "heading": -0.7090843787352074,
+                    "angularVelocity": 0.6186556752036474,
+                    "velocityX": -0.4820812556561803,
+                    "velocityY": -0.8127233707835632,
+                    "timestamp": 13.40886883978839
+                },
+                {
+                    "x": 1.9989634783303936,
+                    "y": 4.76421184486368,
+                    "heading": -0.6872854235406406,
+                    "angularVelocity": 0.4148939980397224,
+                    "velocityX": -0.3218807009959066,
+                    "velocityY": -0.5413684465659858,
+                    "timestamp": 13.461409863031825
+                },
+                {
+                    "x": 1.990496039390564,
+                    "y": 4.75,
+                    "heading": -0.6763337151495863,
+                    "angularVelocity": 0.20844109450081444,
+                    "velocityX": -0.1611586226746669,
+                    "velocityY": -0.27049044701382907,
+                    "timestamp": 13.51395088627526
+                },
+                {
+                    "x": 1.990496039390564,
+                    "y": 4.75,
+                    "heading": -0.6763337151495863,
+                    "angularVelocity": -2.917607263748552e-24,
+                    "velocityX": -1.8295999549572517e-24,
+                    "velocityY": 1.1040980204698261e-23,
+                    "timestamp": 13.566491909518696
+                },
+                {
+                    "x": 2.0311019529427297,
+                    "y": 4.707552352551894,
+                    "heading": -0.6763337151495863,
+                    "angularVelocity": -5.2261319194150544e-17,
+                    "velocityX": 0.41398186246377605,
+                    "velocityY": -0.4327585469834974,
+                    "timestamp": 13.664578123514891
+                },
+                {
+                    "x": 2.1123137791008166,
+                    "y": 4.622657058644844,
+                    "heading": -0.6763337151495863,
+                    "angularVelocity": -1.825824738863161e-16,
+                    "velocityX": 0.8279637152804861,
+                    "velocityY": -0.8655170838823737,
+                    "timestamp": 13.762664337511087
+                },
+                {
+                    "x": 2.2341315150260925,
+                    "y": 4.495314121246338,
+                    "heading": -0.6763337151495863,
+                    "angularVelocity": -9.603465419020808e-17,
+                    "velocityX": 1.2419455391559997,
+                    "velocityY": -1.2982755905273877,
+                    "timestamp": 13.860750551507282
+                },
+                {
+                    "x": 2.3559492509513684,
+                    "y": 4.367971183847831,
+                    "heading": -0.6763337151495863,
+                    "angularVelocity": -2.328687477765739e-16,
+                    "velocityX": 1.2419455391559997,
+                    "velocityY": -1.2982755905273877,
+                    "timestamp": 13.958836765503477
+                },
+                {
+                    "x": 2.4371610771094554,
+                    "y": 4.283075889940782,
+                    "heading": -0.6763337151495863,
+                    "angularVelocity": -1.6799417860231906e-16,
+                    "velocityX": 0.8279637152804862,
+                    "velocityY": -0.8655170838823736,
+                    "timestamp": 14.056922979499673
+                },
+                {
+                    "x": 2.477766990661621,
+                    "y": 4.240628242492676,
+                    "heading": -0.6763337151495863,
+                    "angularVelocity": -1.0297253977874022e-16,
+                    "velocityX": 0.4139818624637761,
+                    "velocityY": -0.4327585469834973,
+                    "timestamp": 14.155009193495868
+                },
+                {
+                    "x": 2.477766990661621,
+                    "y": 4.240628242492676,
+                    "heading": -0.6763337151495863,
+                    "angularVelocity": -1.0483209862050802e-28,
+                    "velocityX": -1.5321576073165576e-26,
+                    "velocityY": -1.2874611866638058e-26,
+                    "timestamp": 14.253095407492063
+                }
+            ],
+            "trajectoryWaypoints": [
+                {
+                    "timestamp": 0,
+                    "isStopPoint": true,
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "timestamp": 1.073251191201399,
+                    "isStopPoint": true,
+                    "x": 2.6293623447418213,
+                    "y": 6.7744364738464355,
+                    "heading": 0.610560190519,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "timestamp": 3.0133515887203335,
+                    "isStopPoint": false,
+                    "x": 7.946028232574463,
+                    "y": 7.456615924835205,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "timestamp": 4.694185309453636,
+                    "isStopPoint": true,
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "timestamp": 5.499203745729279,
+                    "isStopPoint": false,
+                    "x": 6.018601417541504,
+                    "y": 6.676982402801514,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 12
+                },
+                {
+                    "timestamp": 6.318862766261983,
+                    "isStopPoint": false,
+                    "x": 7.891887187957764,
+                    "y": 5.810723304748535,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 12
+                },
+                {
+                    "timestamp": 7.187662783615323,
+                    "isStopPoint": false,
+                    "x": 6.018601417541504,
+                    "y": 6.676982402801514,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "timestamp": 8.056018605131612,
+                    "isStopPoint": true,
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "timestamp": 8.870498149452597,
+                    "isStopPoint": false,
+                    "x": 6.007773399353027,
+                    "y": 6.4279327392578125,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "timestamp": 9.931518394170304,
+                    "isStopPoint": false,
+                    "x": 8.010997772216797,
+                    "y": 4.30559778213501,
+                    "heading": -0.6499245399095859,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "timestamp": 11.015812660862505,
+                    "isStopPoint": false,
+                    "x": 6.007773399353027,
+                    "y": 6.4279327392578125,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "timestamp": 11.89238532047887,
+                    "isStopPoint": true,
+                    "x": 4.026205539703369,
+                    "y": 6.373791694641113,
+                    "heading": 0.293847574053,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "timestamp": 12.617567882735464,
+                    "isStopPoint": false,
+                    "x": 2.9650378227233887,
+                    "y": 6.406276702880859,
+                    "heading": -1.265712572039586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 8
+                },
+                {
+                    "timestamp": 12.883458607354036,
+                    "isStopPoint": false,
+                    "x": 2.60770583152771,
+                    "y": 5.854036331176758,
+                    "heading": -1.374993138249586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "timestamp": 13.566491909518696,
+                    "isStopPoint": true,
+                    "x": 1.990496039390564,
+                    "y": 4.75,
+                    "heading": -0.6763337151495863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 7
+                },
+                {
+                    "timestamp": 14.253095407492063,
+                    "isStopPoint": true,
+                    "x": 2.477766990661621,
+                    "y": 4.240628242492676,
+                    "heading": -0.6763337151495863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        1
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        3
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        7
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        11
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        14
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": [],
+            "eventMarkers": [],
+            "isTrajectoryStale": false
+        },
+        "Distance Center 6 Wing First": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 2.629361867904663,
+                    "y": 4.316425800323486,
+                    "heading": -0.4973420499689195,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 2.510251045227051,
+                    "y": 5.334280490875244,
+                    "heading": 0.5713370823927012,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 3.0191783905029297,
+                    "y": 5.832379341125488,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 11
+                },
+                {
+                    "x": 2.445281744003296,
+                    "y": 6.449589252471924,
+                    "heading": 0.5224037961561394,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 6
+                },
+                {
+                    "x": 2.7051594257354736,
+                    "y": 6.882719039916992,
+                    "heading": 0.5224037961561394,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 6.776577949523926,
+                    "y": 6.9043755531311035,
+                    "heading": 0.5224037961561394,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 8.021825790405273,
+                    "y": 7.337504863739014,
+                    "heading": 0.5224037961561394,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 3.733842372894287,
+                    "y": 6.697348880767819,
+                    "heading": 0.5224037961561394,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 5.7778338432312015,
+                    "y": 6.801184844970702,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 8.03265380859375,
+                    "y": 5.864864349365234,
+                    "heading": -0.302884711661745,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 5.802036285400391,
+                    "y": 6.762963199615477,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 3.733842372894287,
+                    "y": 5.897348880767822,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": -8.23399117094977e-21,
+                    "angularVelocity": 3.616025777443299e-20,
+                    "velocityX": -8.860914587038282e-20,
+                    "velocityY": 8.140056679610733e-20,
+                    "timestamp": 0
+                },
+                {
+                    "x": 1.376109320489114,
+                    "y": 5.556422100544237,
+                    "heading": -0.010099266800899679,
+                    "angularVelocity": -0.12998961247130109,
+                    "velocityX": 0.33605810393746355,
+                    "velocityY": -0.33252332606842316,
+                    "timestamp": 0.07769287567596511
+                },
+                {
+                    "x": 1.4283280795481978,
+                    "y": 5.50475284033716,
+                    "heading": -0.03029817312262219,
+                    "angularVelocity": -0.2599840222926798,
+                    "velocityX": 0.672117727716421,
+                    "velocityY": -0.6650450219216428,
+                    "timestamp": 0.15538575135193022
+                },
+                {
+                    "x": 1.5066564357182666,
+                    "y": 5.42724921578407,
+                    "heading": -0.0605984277415001,
+                    "angularVelocity": -0.39000042610408264,
+                    "velocityX": 1.0081793921073796,
+                    "velocityY": -0.9975641122660442,
+                    "timestamp": 0.23307862702789534
+                },
+                {
+                    "x": 1.6110945641629029,
+                    "y": 5.323911514336438,
+                    "heading": -0.10100431936436054,
+                    "angularVelocity": -0.5200720306889132,
+                    "velocityX": 1.3442433110626257,
+                    "velocityY": -1.3300795027670849,
+                    "timestamp": 0.31077150270386045
+                },
+                {
+                    "x": 1.74164261984874,
+                    "y": 5.194740113776595,
+                    "heading": -0.15152427748693714,
+                    "angularVelocity": -0.6502521329404897,
+                    "velocityX": 1.6803092246233249,
+                    "velocityY": -1.6625900307588073,
+                    "timestamp": 0.38846437837982556
+                },
+                {
+                    "x": 1.8983006855277498,
+                    "y": 5.039735479549906,
+                    "heading": -0.2121726212286143,
+                    "angularVelocity": -0.7806165393417044,
+                    "velocityX": 2.0163762033006187,
+                    "velocityY": -1.9950945679134022,
+                    "timestamp": 0.46615725405579067
+                },
+                {
+                    "x": 2.0810686976676913,
+                    "y": 4.8588981712823704,
+                    "heading": -0.282971977534701,
+                    "angularVelocity": -0.9112721815237043,
+                    "velocityX": 2.3524423642421857,
+                    "velocityY": -2.327591902013722,
+                    "timestamp": 0.5438501297317557
+                },
+                {
+                    "x": 2.2377244681281647,
+                    "y": 4.703905740759624,
+                    "heading": -0.3441858501541506,
+                    "angularVelocity": -0.7878955706924178,
+                    "velocityX": 2.0163466610997847,
+                    "velocityY": -1.9949374916842595,
+                    "timestamp": 0.6215430054077209
+                },
+                {
+                    "x": 2.3682705543010854,
+                    "y": 4.574745653480951,
+                    "heading": -0.3952223440590563,
+                    "angularVelocity": -0.6569005647025403,
+                    "velocityX": 1.6802838746423994,
+                    "velocityY": -1.6624444153330409,
+                    "timestamp": 0.6992358810836861
+                },
+                {
+                    "x": 2.4727071860687984,
+                    "y": 4.471417690375359,
+                    "heading": -0.43606485774485665,
+                    "angularVelocity": -0.5256918775428373,
+                    "velocityX": 1.344224047045041,
+                    "velocityY": -1.3299541586868706,
+                    "timestamp": 0.7769287567596512
+                },
+                {
+                    "x": 2.5510345452184433,
+                    "y": 4.393921749583127,
+                    "heading": -0.4667026260025969,
+                    "angularVelocity": -0.39434462930065384,
+                    "velocityX": 1.0081665592650493,
+                    "velocityY": -0.9974652130968322,
+                    "timestamp": 0.8546216324356164
+                },
+                {
+                    "x": 2.6032527543826127,
+                    "y": 4.34225778871809,
+                    "heading": -0.4871292250327995,
+                    "angularVelocity": -0.2629146990953991,
+                    "velocityX": 0.6721106499128248,
+                    "velocityY": -0.6649768130673013,
+                    "timestamp": 0.9323145081115816
+                },
+                {
+                    "x": 2.629361867904663,
+                    "y": 4.316425800323486,
+                    "heading": -0.4973420499689195,
+                    "angularVelocity": -0.1314512411500224,
+                    "velocityX": 0.3360554400244373,
+                    "velocityY": -0.33248850901518073,
+                    "timestamp": 1.0100073837875467
+                },
+                {
+                    "x": 2.629361867904663,
+                    "y": 4.316425800323486,
+                    "heading": -0.4973420499689195,
+                    "angularVelocity": -4.651997800205279e-20,
+                    "velocityX": 8.78848662911863e-20,
+                    "velocityY": -7.782125385278791e-20,
+                    "timestamp": 1.087700259463512
+                },
+                {
+                    "x": 2.6233052396026286,
+                    "y": 4.326393541424155,
+                    "heading": -0.47896226252358143,
+                    "angularVelocity": 0.4000703563411541,
+                    "velocityX": -0.13183381201916095,
+                    "velocityY": 0.21696647721963866,
+                    "timestamp": 1.133641647406949
+                },
+                {
+                    "x": 2.6115515837229393,
+                    "y": 4.3466736424263335,
+                    "heading": -0.44330520584356,
+                    "angularVelocity": 0.7761423473736202,
+                    "velocityX": -0.25584024353292506,
+                    "velocityY": 0.4414342254341193,
+                    "timestamp": 1.1795830353503862
+                },
+                {
+                    "x": 2.5946459289354515,
+                    "y": 4.3776713184474,
+                    "heading": -0.39137970233868447,
+                    "angularVelocity": 1.1302554369669013,
+                    "velocityX": -0.36798310944156426,
+                    "velocityY": 0.6747222364990407,
+                    "timestamp": 1.2255244232938234
+                },
+                {
+                    "x": 2.573337356420698,
+                    "y": 4.419766771585406,
+                    "heading": -0.32351956642109647,
+                    "angularVelocity": 1.4771024332381333,
+                    "velocityX": -0.46382082624470733,
+                    "velocityY": 0.9162860553937363,
+                    "timestamp": 1.2714658112372605
+                },
+                {
+                    "x": 2.548515626258525,
+                    "y": 4.473258670814533,
+                    "heading": -0.23928333849781427,
+                    "angularVelocity": 1.8335586209757855,
+                    "velocityX": -0.5402912552997571,
+                    "velocityY": 1.1643509615988623,
+                    "timestamp": 1.3174071991806977
+                },
+                {
+                    "x": 2.521204362807651,
+                    "y": 4.538447030993977,
+                    "heading": -0.13846900348834276,
+                    "angularVelocity": 2.19441204374569,
+                    "velocityX": -0.594480590888976,
+                    "velocityY": 1.4189462508120771,
+                    "timestamp": 1.3633485871241349
+                },
+                {
+                    "x": 2.4928952875938295,
+                    "y": 4.615794515250989,
+                    "heading": -0.02317453113785117,
+                    "angularVelocity": 2.509599241808752,
+                    "velocityX": -0.6161998250613465,
+                    "velocityY": 1.6836122659646693,
+                    "timestamp": 1.409289975067572
+                },
+                {
+                    "x": 2.4664488301334497,
+                    "y": 4.705652347771511,
+                    "heading": 0.1004658369360928,
+                    "angularVelocity": 2.691263229273123,
+                    "velocityX": -0.57565647544085,
+                    "velocityY": 1.9559233306393553,
+                    "timestamp": 1.4552313630110092
+                },
+                {
+                    "x": 2.4465017409069274,
+                    "y": 4.806636359026755,
+                    "heading": 0.223493186130365,
+                    "angularVelocity": 2.6779197299337794,
+                    "velocityX": -0.4341856029922523,
+                    "velocityY": 2.1981053637207353,
+                    "timestamp": 1.5011727509544464
+                },
+                {
+                    "x": 2.437142466873225,
+                    "y": 4.914004319219758,
+                    "heading": 0.3340193354303189,
+                    "angularVelocity": 2.405807796578398,
+                    "velocityX": -0.20372205657403003,
+                    "velocityY": 2.337063919905811,
+                    "timestamp": 1.5471141388978835
+                },
+                {
+                    "x": 2.4390218774995867,
+                    "y": 5.023216279374713,
+                    "heading": 0.42300384320052303,
+                    "angularVelocity": 1.9369137884941887,
+                    "velocityX": 0.040908877822231356,
+                    "velocityY": 2.377202018568005,
+                    "timestamp": 1.5930555268413207
+                },
+                {
+                    "x": 2.4523438963207767,
+                    "y": 5.131375051574103,
+                    "heading": 0.4925726441961787,
+                    "angularVelocity": 1.514294715721443,
+                    "velocityX": 0.2899785883176208,
+                    "velocityY": 2.3542774182737958,
+                    "timestamp": 1.6389969147847578
+                },
+                {
+                    "x": 2.4764740733075925,
+                    "y": 5.235554065862188,
+                    "heading": 0.5432225068747625,
+                    "angularVelocity": 1.1024887350147914,
+                    "velocityX": 0.5252383105300292,
+                    "velocityY": 2.2676505641568747,
+                    "timestamp": 1.684938302728195
+                },
+                {
+                    "x": 2.510251045227051,
+                    "y": 5.334280490875244,
+                    "heading": 0.5713370823927012,
+                    "angularVelocity": 0.6119661763931314,
+                    "velocityX": 0.7352187957630835,
+                    "velocityY": 2.148964788234253,
+                    "timestamp": 1.7308796906716322
+                },
+                {
+                    "x": 2.5650236180534773,
+                    "y": 5.445977034086529,
+                    "heading": 0.5690314952034347,
+                    "angularVelocity": -0.041009635460105165,
+                    "velocityX": 0.9742434618308231,
+                    "velocityY": 1.9867539777170269,
+                    "timestamp": 1.7871003120792386
+                },
+                {
+                    "x": 2.631436378611181,
+                    "y": 5.546741566981676,
+                    "heading": 0.5272160744085959,
+                    "angularVelocity": -0.7437737212413915,
+                    "velocityX": 1.1812882692313675,
+                    "velocityY": 1.792305569954328,
+                    "timestamp": 1.843320933486845
+                },
+                {
+                    "x": 2.707043830480548,
+                    "y": 5.633772829267234,
+                    "heading": 0.4487228756397609,
+                    "angularVelocity": -1.3961638417289928,
+                    "velocityX": 1.3448348662175675,
+                    "velocityY": 1.5480309556625385,
+                    "timestamp": 1.8995415548944514
+                },
+                {
+                    "x": 2.7862462006186286,
+                    "y": 5.702826720944244,
+                    "heading": 0.3492973403296841,
+                    "angularVelocity": -1.768488729237435,
+                    "velocityX": 1.4087779208958542,
+                    "velocityY": 1.228266247296711,
+                    "timestamp": 1.9557621763020578
+                },
+                {
+                    "x": 2.8600121003633747,
+                    "y": 5.75337861080909,
+                    "heading": 0.24738242695849674,
+                    "angularVelocity": -1.8127674653805728,
+                    "velocityX": 1.3120790538748128,
+                    "velocityY": 0.8991698881863764,
+                    "timestamp": 2.0119827977096643
+                },
+                {
+                    "x": 2.9223194382842412,
+                    "y": 5.7887086014295965,
+                    "heading": 0.1556886936267466,
+                    "angularVelocity": -1.6309626438839835,
+                    "velocityX": 1.1082648387880867,
+                    "velocityY": 0.6284169355646319,
+                    "timestamp": 2.0682034191172707
+                },
+                {
+                    "x": 2.9703569068277655,
+                    "y": 5.812107898641809,
+                    "heading": 0.08110867474881293,
+                    "angularVelocity": -1.326559846025525,
+                    "velocityX": 0.854445705878034,
+                    "velocityY": 0.416204884015149,
+                    "timestamp": 2.124424040524877
+                },
+                {
+                    "x": 3.00283548948403,
+                    "y": 5.82606180657968,
+                    "heading": 0.028064925087883746,
+                    "angularVelocity": -0.943492767117523,
+                    "velocityX": 0.5776987490193435,
+                    "velocityY": 0.2481991053905678,
+                    "timestamp": 2.1806446619324835
+                },
+                {
+                    "x": 3.0191783905029297,
+                    "y": 5.832379341125488,
+                    "heading": -3.6672255294112824e-21,
+                    "angularVelocity": -0.4991927229763187,
+                    "velocityX": 0.2906922870953705,
+                    "velocityY": 0.11237041476304822,
+                    "timestamp": 2.23686528334009
+                },
+                {
+                    "x": 3.0191783905029297,
+                    "y": 5.832379341125488,
+                    "heading": -2.1726460551285392e-21,
+                    "angularVelocity": 3.7108122990500546e-22,
+                    "velocityX": 2.1269801588032454e-22,
+                    "velocityY": 1.2368753944018978e-21,
+                    "timestamp": 2.2930859047476964
+                },
+                {
+                    "x": 3.002861357001422,
+                    "y": 5.839481859090172,
+                    "heading": 0.011550492706860791,
+                    "angularVelocity": 0.21199493252787568,
+                    "velocityX": -0.2994788624170613,
+                    "velocityY": 0.1303578864481573,
+                    "timestamp": 2.3475706632706013
+                },
+                {
+                    "x": 2.970454672747075,
+                    "y": 5.854191308674451,
+                    "heading": 0.034599552401506535,
+                    "angularVelocity": 0.42303683304306455,
+                    "velocityX": -0.5947843971947421,
+                    "velocityY": 0.26997365837814763,
+                    "timestamp": 2.402055421793506
+                },
+                {
+                    "x": 2.922320531881221,
+                    "y": 5.877237889129693,
+                    "heading": 0.06905281952724152,
+                    "angularVelocity": 0.6323468812154363,
+                    "velocityX": -0.8834423088361375,
+                    "velocityY": 0.4229913296863304,
+                    "timestamp": 2.456540180316411
+                },
+                {
+                    "x": 2.8591099926637993,
+                    "y": 5.909758325766861,
+                    "heading": 0.11477832447476202,
+                    "angularVelocity": 0.8392347912911837,
+                    "velocityX": -1.1601508555984257,
+                    "velocityY": 0.5968721807493298,
+                    "timestamp": 2.511024938839316
+                },
+                {
+                    "x": 2.782225597166992,
+                    "y": 5.953695739861746,
+                    "heading": 0.171638299210357,
+                    "angularVelocity": 1.043594140399681,
+                    "velocityX": -1.4111174864524112,
+                    "velocityY": 0.8064166068830917,
+                    "timestamp": 2.565509697362221
+                },
+                {
+                    "x": 2.695621918494965,
+                    "y": 6.012601850588778,
+                    "heading": 0.23913912308030816,
+                    "angularVelocity": 1.238893696143934,
+                    "velocityX": -1.5895028448298905,
+                    "velocityY": 1.0811484224944983,
+                    "timestamp": 2.619994455885126
+                },
+                {
+                    "x": 2.6121558310066813,
+                    "y": 6.089315201605884,
+                    "heading": 0.31084169357843383,
+                    "angularVelocity": 1.316011531334634,
+                    "velocityX": -1.5319162597223552,
+                    "velocityY": 1.4079781776927003,
+                    "timestamp": 2.6744792144080307
+                },
+                {
+                    "x": 2.5439162846102246,
+                    "y": 6.175738160019639,
+                    "heading": 0.3773551147962972,
+                    "angularVelocity": 1.2207711481349772,
+                    "velocityX": -1.2524520296399875,
+                    "velocityY": 1.5861859491847345,
+                    "timestamp": 2.7289639729309356
+                },
+                {
+                    "x": 2.4931765430661446,
+                    "y": 6.266142072193765,
+                    "heading": 0.43533450902171794,
+                    "angularVelocity": 1.0641396933244534,
+                    "velocityX": -0.9312648696561509,
+                    "velocityY": 1.6592514058059242,
+                    "timestamp": 2.7834487314538405
+                },
+                {
+                    "x": 2.460287773937943,
+                    "y": 6.357897509213886,
+                    "heading": 0.48380582267307914,
+                    "angularVelocity": 0.8896306960961281,
+                    "velocityX": -0.6036324656624045,
+                    "velocityY": 1.684056963958949,
+                    "timestamp": 2.8379334899767454
+                },
+                {
+                    "x": 2.445281744003296,
+                    "y": 6.449589252471924,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 0.7084178131547396,
+                    "velocityX": -0.275417021961089,
+                    "velocityY": 1.6828879441485027,
+                    "timestamp": 2.8924182484996503
+                },
+                {
+                    "x": 2.4640368595524906,
+                    "y": 6.587540353459253,
+                    "heading": 0.5579085954415691,
+                    "angularVelocity": 0.42638133020810537,
+                    "velocityX": 0.22523239891554875,
+                    "velocityY": 1.6566710733888894,
+                    "timestamp": 2.975688309287961
+                },
+                {
+                    "x": 2.52273710591792,
+                    "y": 6.71371975523591,
+                    "heading": 0.5680417649597526,
+                    "angularVelocity": 0.12169043017686823,
+                    "velocityX": 0.704938195189473,
+                    "velocityY": 1.5153033465104557,
+                    "timestamp": 3.0589583700762715
+                },
+                {
+                    "x": 2.605078049581534,
+                    "y": 6.805600488381497,
+                    "heading": 0.5525667893518443,
+                    "angularVelocity": -0.1858408107476787,
+                    "velocityX": 0.9888421226560808,
+                    "velocityY": 1.1034065818586072,
+                    "timestamp": 3.142228430864582
+                },
+                {
+                    "x": 2.670460197759116,
+                    "y": 6.858702767349351,
+                    "heading": 0.5337797940272959,
+                    "angularVelocity": -0.22561524690499246,
+                    "velocityX": 0.785181943649555,
+                    "velocityY": 0.6377115432021956,
+                    "timestamp": 3.2254984916528926
+                },
+                {
+                    "x": 2.7051594257354736,
+                    "y": 6.882719039916992,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -0.13661570273230195,
+                    "velocityX": 0.41670712916338404,
+                    "velocityY": 0.28841425525909853,
+                    "timestamp": 3.308768552441203
+                },
+                {
+                    "x": 2.7051594257354736,
+                    "y": 6.882719039916992,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 6.900030919446935e-23,
+                    "velocityX": -5.778219378902074e-20,
+                    "velocityY": -2.504372507974792e-19,
+                    "timestamp": 3.3920386132295137
+                },
+                {
+                    "x": 2.7398288490574805,
+                    "y": 6.881986068889246,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 5.395309096135092e-17,
+                    "velocityX": 0.4600360054616196,
+                    "velocityY": -0.009725949595165711,
+                    "timestamp": 3.4674010259763555
+                },
+                {
+                    "x": 2.809167695484875,
+                    "y": 6.880520128147862,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 1.1380978370170907e-16,
+                    "velocityX": 0.9200720080488729,
+                    "velocityY": -0.019451881753150334,
+                    "timestamp": 3.542763438723197
+                },
+                {
+                    "x": 2.9131759647111264,
+                    "y": 6.878321218057033,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 1.7975067781041165e-16,
+                    "velocityX": 1.380108006568699,
+                    "velocityY": -0.029177809078592246,
+                    "timestamp": 3.618125851470039
+                },
+                {
+                    "x": 3.0518536562841723,
+                    "y": 6.875389337051742,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 2.5102040552411234e-16,
+                    "velocityX": 1.8401439990900241,
+                    "velocityY": -0.03890375717055886,
+                    "timestamp": 3.6934882642168807
+                },
+                {
+                    "x": 3.2252007695055918,
+                    "y": 6.8717244798411565,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 3.2835275620118203e-16,
+                    "velocityX": 2.3001799823438382,
+                    "velocityY": -0.04862977546772752,
+                    "timestamp": 3.7688506769637224
+                },
+                {
+                    "x": 3.433217303234589,
+                    "y": 6.8673266340826435,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 3.215116027866161e-16,
+                    "velocityX": 2.7602159504601937,
+                    "velocityY": -0.05835595754194546,
+                    "timestamp": 3.844213089710564
+                },
+                {
+                    "x": 3.6759032554627358,
+                    "y": 6.8621957741400355,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 4.116551365644819e-16,
+                    "velocityX": 3.220251891926296,
+                    "velocityY": -0.06808247978795974,
+                    "timestamp": 3.919575502457406
+                },
+                {
+                    "x": 3.9532586221839736,
+                    "y": 6.85633184925441,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 5.073242166542338e-16,
+                    "velocityX": 3.680287780235135,
+                    "velocityY": -0.07780967556497664,
+                    "timestamp": 3.9949379152042477
+                },
+                {
+                    "x": 4.265283392553291,
+                    "y": 6.849734762645486,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 5.186061419797297e-16,
+                    "velocityX": 4.140323524639203,
+                    "velocityY": -0.08753815554028055,
+                    "timestamp": 4.070300327951089
+                },
+                {
+                    "x": 4.580344671204565,
+                    "y": 6.84307401284669,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 2.9918936261002046e-16,
+                    "velocityX": 4.180615603558647,
+                    "velocityY": -0.08838291604556786,
+                    "timestamp": 4.14566274069793
+                },
+                {
+                    "x": 4.895405891408077,
+                    "y": 6.836410498973634,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -6.759041033244585e-16,
+                    "velocityX": 4.180614828002794,
+                    "velocityY": -0.08841959313908884,
+                    "timestamp": 4.221025153444772
+                },
+                {
+                    "x": 5.2104670226750835,
+                    "y": 6.829742781371814,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 1.2122501066067013e-16,
+                    "velocityX": 4.180613647885222,
+                    "velocityY": -0.08847537331665184,
+                    "timestamp": 4.296387566191613
+                },
+                {
+                    "x": 5.525528609681589,
+                    "y": 6.8230966331486265,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -5.738963451554141e-16,
+                    "velocityX": 4.18061969519029,
+                    "velocityY": -0.0881891646106545,
+                    "timestamp": 4.371749978938454
+                },
+                {
+                    "x": 5.840589424991367,
+                    "y": 6.8164140027083135,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 2.6947897759150035e-16,
+                    "velocityX": 4.180609455381084,
+                    "velocityY": -0.08867325496545927,
+                    "timestamp": 4.4471123916852955
+                },
+                {
+                    "x": 6.155679411271227,
+                    "y": 6.811288045591518,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -2.0026083836087547e-16,
+                    "velocityX": 4.18099653123254,
+                    "velocityY": -0.06801742314188503,
+                    "timestamp": 4.522474804432137
+                },
+                {
+                    "x": 6.4694252556395035,
+                    "y": 6.840809560208346,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -1.0762473033828276e-16,
+                    "velocityX": 4.163160824245574,
+                    "velocityY": 0.3917273019906568,
+                    "timestamp": 4.597837217178978
+                },
+                {
+                    "x": 6.776577949523926,
+                    "y": 6.9043755531311035,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -3.677462960461617e-16,
+                    "velocityX": 4.0756748979921005,
+                    "velocityY": 0.843470778148919,
+                    "timestamp": 4.673199629925819
+                },
+                {
+                    "x": 7.013543953390312,
+                    "y": 6.961987444002269,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -4.893185861598382e-16,
+                    "velocityX": 3.6868185744404176,
+                    "velocityY": 0.8963504718263726,
+                    "timestamp": 4.737473474456494
+                },
+                {
+                    "x": 7.225303695322754,
+                    "y": 7.018671437407212,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -4.901113542018955e-16,
+                    "velocityX": 3.2946487560952082,
+                    "velocityY": 0.8819138456528895,
+                    "timestamp": 4.801747318987169
+                },
+                {
+                    "x": 7.411968501618956,
+                    "y": 7.072813587578339,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -4.1553599222426735e-16,
+                    "velocityX": 2.9042109999677495,
+                    "velocityY": 0.8423667600167797,
+                    "timestamp": 4.866021163517844
+                },
+                {
+                    "x": 7.573636696281331,
+                    "y": 7.123581388206941,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -4.213277119698898e-16,
+                    "velocityX": 2.5153030107794323,
+                    "velocityY": 0.7898671846893066,
+                    "timestamp": 4.930295008048518
+                },
+                {
+                    "x": 7.710381873366203,
+                    "y": 7.1704684590736765,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -4.2912506340016083e-16,
+                    "velocityX": 2.12754002943782,
+                    "velocityY": 0.729489129040025,
+                    "timestamp": 4.994568852579193
+                },
+                {
+                    "x": 7.822259372652904,
+                    "y": 7.213134689148531,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -4.3865920867652994e-16,
+                    "velocityX": 1.7406380480835721,
+                    "velocityY": 0.6638194803251983,
+                    "timestamp": 5.058842697109868
+                },
+                {
+                    "x": 7.909311852563935,
+                    "y": 7.25133601958431,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -4.515086254769384e-16,
+                    "velocityX": 1.3543997647361083,
+                    "velocityY": 0.5943526595417427,
+                    "timestamp": 5.123116541640543
+                },
+                {
+                    "x": 7.971573038400992,
+                    "y": 7.284888837484052,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -4.833939499970947e-16,
+                    "velocityX": 0.9686861940760827,
+                    "velocityY": 0.5220291106708211,
+                    "timestamp": 5.187390386171217
+                },
+                {
+                    "x": 8.009070195209654,
+                    "y": 7.3136500159409,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -5.006358797639765e-16,
+                    "velocityX": 0.5833968246720782,
+                    "velocityY": 0.4474787320855215,
+                    "timestamp": 5.251664230701892
+                },
+                {
+                    "x": 8.021825790405273,
+                    "y": 7.337504863739014,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -4.469977199106365e-16,
+                    "velocityX": 0.19845701293833062,
+                    "velocityY": 0.3711439384449641,
+                    "timestamp": 5.315938075232567
+                },
+                {
+                    "x": 7.983783192652999,
+                    "y": 7.362254574962456,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -4.2160773073279116e-16,
+                    "velocityX": -0.3881238839919007,
+                    "velocityY": 0.25250520772193735,
+                    "timestamp": 5.41395471198525
+                },
+                {
+                    "x": 7.888245908562877,
+                    "y": 7.375375716980563,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -4.616597613928156e-16,
+                    "velocityX": -0.9747047772224879,
+                    "velocityY": 0.1338664787204887,
+                    "timestamp": 5.511971348737933
+                },
+                {
+                    "x": 7.7352139386258125,
+                    "y": 7.376868290112506,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -5.113708650512922e-16,
+                    "velocityX": -1.561285665444697,
+                    "velocityY": 0.015227752975333887,
+                    "timestamp": 5.609987985490616
+                },
+                {
+                    "x": 7.524687283587215,
+                    "y": 7.36673229479228,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -5.254329700677736e-16,
+                    "velocityX": -2.147866546061973,
+                    "velocityY": -0.10341096834204828,
+                    "timestamp": 5.7080046222432985
+                },
+                {
+                    "x": 7.256665944815729,
+                    "y": 7.344967731118788,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -3.7838755078538407e-16,
+                    "velocityX": -2.7344474127158738,
+                    "velocityY": -0.22204968865039343,
+                    "timestamp": 5.806021258995981
+                },
+                {
+                    "x": 6.931149925685574,
+                    "y": 7.311574596140671,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -3.394615825938055e-16,
+                    "velocityX": -3.321028244944788,
+                    "velocityY": -0.3406884390695356,
+                    "timestamp": 5.904037895748664
+                },
+                {
+                    "x": 6.548139239864235,
+                    "y": 7.26655286347746,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -4.742302930350804e-16,
+                    "velocityX": -3.9076089377332557,
+                    "velocityY": -0.45932745863144137,
+                    "timestamp": 6.002054532501347
+                },
+                {
+                    "x": 6.146412026065515,
+                    "y": 7.185302300694055,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -2.3651181209873e-16,
+                    "velocityX": -4.09856150045591,
+                    "velocityY": -0.828946651050856,
+                    "timestamp": 6.10007116925403
+                },
+                {
+                    "x": 5.744684992223911,
+                    "y": 7.1040508481538485,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -2.6117867815425296e-16,
+                    "velocityX": -4.098559664470492,
+                    "velocityY": -0.8289557286608469,
+                    "timestamp": 6.198087806006713
+                },
+                {
+                    "x": 5.34295787439086,
+                    "y": 7.022799810888984,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -3.2687293678006866e-16,
+                    "velocityX": -4.098560521380605,
+                    "velocityY": -0.8289514918766033,
+                    "timestamp": 6.296104442759396
+                },
+                {
+                    "x": 4.941230796975172,
+                    "y": 6.941548573791956,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -1.6274431602051954e-16,
+                    "velocityX": -4.098560109028538,
+                    "velocityY": -0.8289535306342138,
+                    "timestamp": 6.394121079512079
+                },
+                {
+                    "x": 4.596262688532624,
+                    "y": 6.871777230633822,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 5.785170797428673e-16,
+                    "velocityX": -3.5194852615987555,
+                    "velocityY": -0.7118316386858059,
+                    "timestamp": 6.492137716264762
+                },
+                {
+                    "x": 4.308789253876432,
+                    "y": 6.813634450082659,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 3.9995305722921473e-16,
+                    "velocityX": -2.9329044964228768,
+                    "velocityY": -0.5931929770031786,
+                    "timestamp": 6.590154353017445
+                },
+                {
+                    "x": 4.0788105028099615,
+                    "y": 6.767120224861494,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 3.6861351582812966e-16,
+                    "velocityX": -2.3463236312296227,
+                    "velocityY": -0.4745543895627656,
+                    "timestamp": 6.688170989770128
+                },
+                {
+                    "x": 3.906326438276487,
+                    "y": 6.732234554149373,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 2.21916201762866e-16,
+                    "velocityX": -1.7597427360080535,
+                    "velocityY": -0.3559158104980193,
+                    "timestamp": 6.7861876265228105
+                },
+                {
+                    "x": 3.79133706145364,
+                    "y": 6.708977438989503,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 1.8561143927305445e-16,
+                    "velocityX": -1.1731618287718801,
+                    "velocityY": -0.23727722079009275,
+                    "timestamp": 6.884204263275493
+                },
+                {
+                    "x": 3.733842372894287,
+                    "y": 6.697348880767819,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": 5.508639517425219e-17,
+                    "velocityX": -0.5865809158951689,
+                    "velocityY": -0.11863861694240564,
+                    "timestamp": 6.982220900028176
+                },
+                {
+                    "x": 3.733842372894287,
+                    "y": 6.697348880767819,
+                    "heading": 0.5224037961561394,
+                    "angularVelocity": -1.6835319404035148e-20,
+                    "velocityX": 3.846585110796379e-19,
+                    "velocityY": 3.193403425909451e-20,
+                    "timestamp": 7.080237536780859
+                },
+                {
+                    "x": 3.751331338841791,
+                    "y": 6.699587280336841,
+                    "heading": 0.513975805961829,
+                    "angularVelocity": -0.15604839616653143,
+                    "velocityX": 0.3238168322219289,
+                    "velocityY": 0.04144507227375776,
+                    "timestamp": 7.134246359693184
+                },
+                {
+                    "x": 3.7863260736878988,
+                    "y": 6.704004062694685,
+                    "heading": 0.4973370967850019,
+                    "angularVelocity": -0.308073908661883,
+                    "velocityX": 0.6479447793727443,
+                    "velocityY": 0.08177890425448403,
+                    "timestamp": 7.188255182605508
+                },
+                {
+                    "x": 3.838845900665831,
+                    "y": 6.71052756994018,
+                    "heading": 0.4727433220581201,
+                    "angularVelocity": -0.4553658717355572,
+                    "velocityX": 0.9724305057192445,
+                    "velocityY": 0.12078595484453863,
+                    "timestamp": 7.242264005517833
+                },
+                {
+                    "x": 3.9089133882112788,
+                    "y": 6.719070782800993,
+                    "heading": 0.4405046382901893,
+                    "angularVelocity": -0.5969151340377453,
+                    "velocityX": 1.2973340977860786,
+                    "velocityY": 0.15818180067878418,
+                    "timestamp": 7.2962728284301575
+                },
+                {
+                    "x": 3.9965552657179173,
+                    "y": 6.729525850718403,
+                    "heading": 0.40100871332347776,
+                    "angularVelocity": -0.7312865349949899,
+                    "velocityX": 1.622732597763012,
+                    "velocityY": 0.19358074021316565,
+                    "timestamp": 7.350281651342482
+                },
+                {
+                    "x": 4.101803555150174,
+                    "y": 6.741755679434534,
+                    "heading": 0.354754655561301,
+                    "angularVelocity": -0.8564166976433442,
+                    "velocityX": 1.9487240002085033,
+                    "velocityY": 0.22644131193126646,
+                    "timestamp": 7.404290474254807
+                },
+                {
+                    "x": 4.224696877453653,
+                    "y": 6.755580236078661,
+                    "heading": 0.30240542478108134,
+                    "angularVelocity": -0.9692718329596139,
+                    "velocityX": 2.2754304885144183,
+                    "velocityY": 0.2559684862336096,
+                    "timestamp": 7.458299297167131
+                },
+                {
+                    "x": 4.365281618112227,
+                    "y": 6.770752614262487,
+                    "heading": 0.24487468968297588,
+                    "angularVelocity": -1.0652099415589684,
+                    "velocityX": 2.602995826937264,
+                    "velocityY": 0.28092406695210137,
+                    "timestamp": 7.512308120079456
+                },
+                {
+                    "x": 4.5236114777315315,
+                    "y": 6.786913235329627,
+                    "heading": 0.18348645255620313,
+                    "angularVelocity": -1.1366334946130574,
+                    "velocityX": 2.9315554585652097,
+                    "velocityY": 0.2992218714593103,
+                    "timestamp": 7.56631694299178
+                },
+                {
+                    "x": 4.69973843079224,
+                    "y": 6.803490840028783,
+                    "heading": 0.12031512412262027,
+                    "angularVelocity": -1.169648309798064,
+                    "velocityX": 3.2610774233429534,
+                    "velocityY": 0.3069425291135703,
+                    "timestamp": 7.620325765904105
+                },
+                {
+                    "x": 4.89365495499135,
+                    "y": 6.819447149799665,
+                    "heading": 0.059092961452105006,
+                    "angularVelocity": -1.1335585441271512,
+                    "velocityX": 3.590460108969724,
+                    "velocityY": 0.2954389470176964,
+                    "timestamp": 7.674334588816429
+                },
+                {
+                    "x": 5.104830717151077,
+                    "y": 6.832438699160781,
+                    "heading": 0.008851181851826708,
+                    "angularVelocity": -0.9302513347094954,
+                    "velocityX": 3.9100234141844274,
+                    "velocityY": 0.24054494544725308,
+                    "timestamp": 7.728343411728754
+                },
+                {
+                    "x": 5.328140039231948,
+                    "y": 6.839579388098938,
+                    "heading": 4.9993181720020666e-8,
+                    "angularVelocity": -0.16388307282707454,
+                    "velocityX": 4.134682261884916,
+                    "velocityY": 0.1322133783539373,
+                    "timestamp": 7.7823522346410785
+                },
+                {
+                    "x": 5.5537444848612765,
+                    "y": 6.829254816135378,
+                    "heading": 1.8581535238630143e-8,
+                    "angularVelocity": -5.816021306799029e-7,
+                    "velocityX": 4.177177606621186,
+                    "velocityY": -0.19116454325104273,
+                    "timestamp": 7.836361057553403
+                },
+                {
+                    "x": 5.7778338432312015,
+                    "y": 6.801184844970702,
+                    "heading": 7.562116538308675e-20,
+                    "angularVelocity": -3.4404629163368187e-7,
+                    "velocityX": 4.149125018586378,
+                    "velocityY": -0.5197293636679055,
+                    "timestamp": 7.890369880465728
+                },
+                {
+                    "x": 6.023879514181097,
+                    "y": 6.7480895192913355,
+                    "heading": -8.873359130944661e-9,
+                    "angularVelocity": -1.474096821803706e-7,
+                    "velocityX": 4.087461537509469,
+                    "velocityY": -0.8820521031648062,
+                    "timestamp": 7.950565106449386
+                },
+                {
+                    "x": 6.264312553803963,
+                    "y": 6.673594285856147,
+                    "heading": -1.6744083268388366e-8,
+                    "angularVelocity": -1.3075329494712452e-7,
+                    "velocityX": 3.9942210647757475,
+                    "velocityY": -1.237560491182664,
+                    "timestamp": 8.010760332433046
+                },
+                {
+                    "x": 6.497275551280226,
+                    "y": 6.578274673466115,
+                    "heading": -2.399265114525153e-8,
+                    "angularVelocity": -1.204176536996037e-7,
+                    "velocityX": 3.8701241447204757,
+                    "velocityY": -1.5835078418994266,
+                    "timestamp": 8.070955558416705
+                },
+                {
+                    "x": 6.720968856148328,
+                    "y": 6.462867168278596,
+                    "heading": -3.0899254081227276e-8,
+                    "angularVelocity": -1.1473672244136315e-7,
+                    "velocityX": 3.7161303278241413,
+                    "velocityY": -1.9172202330272603,
+                    "timestamp": 8.131150784400363
+                },
+                {
+                    "x": 6.933377464225438,
+                    "y": 6.349413240298023,
+                    "heading": -0.03079337544261274,
+                    "angularVelocity": -0.5115579190900974,
+                    "velocityX": 3.528662026034608,
+                    "velocityY": -1.88476621071864,
+                    "timestamp": 8.191346010384022
+                },
+                {
+                    "x": 7.127357335376337,
+                    "y": 6.247833133054634,
+                    "heading": -0.07153202857540313,
+                    "angularVelocity": -0.676775482890449,
+                    "velocityX": 3.2225125494762574,
+                    "velocityY": -1.6875110207404995,
+                    "timestamp": 8.25154123636768
+                },
+                {
+                    "x": 7.302666826825636,
+                    "y": 6.158120559909072,
+                    "heading": -0.11291500541703978,
+                    "angularVelocity": -0.6874793833795204,
+                    "velocityX": 2.9123487549808393,
+                    "velocityY": -1.490360268269716,
+                    "timestamp": 8.31173646235134
+                },
+                {
+                    "x": 7.459290055702994,
+                    "y": 6.080235640166216,
+                    "heading": -0.15221505882089248,
+                    "angularVelocity": -0.6528765821814755,
+                    "velocityX": 2.601921104505488,
+                    "velocityY": -1.2938720383573232,
+                    "timestamp": 8.371931688334998
+                },
+                {
+                    "x": 7.597228420785523,
+                    "y": 6.0141535807269655,
+                    "heading": -0.18812935690621163,
+                    "angularVelocity": -0.5966303390084257,
+                    "velocityX": 2.2915166913730753,
+                    "velocityY": -1.097795686607935,
+                    "timestamp": 8.432126914318657
+                },
+                {
+                    "x": 7.7164854369458595,
+                    "y": 5.959858078865012,
+                    "heading": -0.21989239705905567,
+                    "angularVelocity": -0.5276670970795383,
+                    "velocityX": 1.981170669459914,
+                    "velocityY": -0.9019901657432539,
+                    "timestamp": 8.492322140302315
+                },
+                {
+                    "x": 7.817064511998192,
+                    "y": 5.917337690294524,
+                    "heading": -0.24699919876355111,
+                    "angularVelocity": -0.4503148092151716,
+                    "velocityX": 1.6708812602454004,
+                    "velocityY": -0.706374764371373,
+                    "timestamp": 8.552517366285974
+                },
+                {
+                    "x": 7.898968592472621,
+                    "y": 5.886583965823989,
+                    "heading": -0.26909119151069666,
+                    "angularVelocity": -0.36700572821410926,
+                    "velocityX": 1.360640800595448,
+                    "velocityY": -0.5108997261491135,
+                    "timestamp": 8.612712592269633
+                },
+                {
+                    "x": 7.9622001639044475,
+                    "y": 5.867590422826931,
+                    "heading": -0.2859005975102692,
+                    "angularVelocity": -0.27924815838611156,
+                    "velocityX": 1.0504416321817922,
+                    "velocityY": -0.31553238129239974,
+                    "timestamp": 8.672907818253291
+                },
+                {
+                    "x": 8.006761315879022,
+                    "y": 5.860351935159993,
+                    "heading": -0.29722007092856884,
+                    "angularVelocity": -0.18804603244404322,
+                    "velocityX": 0.7402771772411256,
+                    "velocityY": -0.12025019507199955,
+                    "timestamp": 8.73310304423695
+                },
+                {
+                    "x": 8.03265380859375,
+                    "y": 5.864864349365234,
+                    "heading": -0.302884711661745,
+                    "angularVelocity": -0.09410448487582672,
+                    "velocityX": 0.4301419637789344,
+                    "velocityY": 0.07496299135859619,
+                    "timestamp": 8.793298270220609
+                },
+                {
+                    "x": 8.038999044372272,
+                    "y": 5.883116303873676,
+                    "heading": -0.30232446533447555,
+                    "angularVelocity": 0.008705413942196032,
+                    "velocityX": 0.09859574498610282,
+                    "velocityY": 0.28360885474165515,
+                    "timestamp": 8.857654351503994
+                },
+                {
+                    "x": 8.023741230476212,
+                    "y": 5.914373930337575,
+                    "heading": -0.29538057133457873,
+                    "angularVelocity": 0.10789802395394477,
+                    "velocityX": -0.23708425982112472,
+                    "velocityY": 0.48569810094960236,
+                    "timestamp": 8.92201043278738
+                },
+                {
+                    "x": 7.986572081097167,
+                    "y": 5.958123497607152,
+                    "heading": -0.2823371032309085,
+                    "angularVelocity": 0.20267654343704639,
+                    "velocityX": -0.5775545781815625,
+                    "velocityY": 0.6798047114915214,
+                    "timestamp": 8.986366514070765
+                },
+                {
+                    "x": 7.927131820563721,
+                    "y": 6.013727378429472,
+                    "heading": -0.26354639848302325,
+                    "angularVelocity": 0.2919802507107623,
+                    "velocityX": -0.9236152877566656,
+                    "velocityY": 0.8640035209333707,
+                    "timestamp": 9.05072259535415
+                },
+                {
+                    "x": 7.8449983231200475,
+                    "y": 6.080375877201426,
+                    "heading": -0.23945459436001962,
+                    "angularVelocity": 0.3743516330168969,
+                    "velocityX": -1.2762352182695424,
+                    "velocityY": 1.0356208371120963,
+                    "timestamp": 9.115078676637536
+                },
+                {
+                    "x": 7.73967594274609,
+                    "y": 6.157012398666797,
+                    "heading": -0.21064125546050952,
+                    "angularVelocity": 0.4477174235117473,
+                    "velocityX": -1.6365567678084803,
+                    "velocityY": 1.1908201981396094,
+                    "timestamp": 9.179434757920921
+                },
+                {
+                    "x": 7.610589170044395,
+                    "y": 6.24221277947117,
+                    "heading": -0.1778829463049905,
+                    "angularVelocity": 0.5090165296309906,
+                    "velocityX": -2.005820897224518,
+                    "velocityY": 1.3238901298107526,
+                    "timestamp": 9.243790839204307
+                },
+                {
+                    "x": 7.457098123742525,
+                    "y": 6.333983891334329,
+                    "heading": -0.1422582406491948,
+                    "angularVelocity": 0.5535561666492045,
+                    "velocityX": -2.3850278519288124,
+                    "velocityY": 1.4259897438293916,
+                    "timestamp": 9.308146920487692
+                },
+                {
+                    "x": 7.278589562677723,
+                    "y": 6.429422311009697,
+                    "heading": -0.10532315073359272,
+                    "angularVelocity": 0.5739176341853679,
+                    "velocityX": -2.7737636833224153,
+                    "velocityY": 1.4829743789885954,
+                    "timestamp": 9.372503001771078
+                },
+                {
+                    "x": 7.0748043390181765,
+                    "y": 6.524168352293141,
+                    "heading": -0.06939014472798437,
+                    "angularVelocity": 0.5583467061547885,
+                    "velocityX": -3.166526295505748,
+                    "velocityY": 1.4722158247367187,
+                    "timestamp": 9.436859083054463
+                },
+                {
+                    "x": 6.846786882119966,
+                    "y": 6.6118039531436965,
+                    "heading": -0.037838987740982835,
+                    "angularVelocity": 0.4902591388072404,
+                    "velocityX": -3.543059992950117,
+                    "velocityY": 1.3617299111899137,
+                    "timestamp": 9.501215164337848
+                },
+                {
+                    "x": 6.5986490653165255,
+                    "y": 6.684332838455106,
+                    "heading": -0.014752410202795902,
+                    "angularVelocity": 0.35873187238556964,
+                    "velocityX": -3.855701152946053,
+                    "velocityY": 1.12699350030396,
+                    "timestamp": 9.565571245621234
+                },
+                {
+                    "x": 6.337948603586263,
+                    "y": 6.7352009376864705,
+                    "heading": -0.00346493782708955,
+                    "angularVelocity": 0.1753909211159554,
+                    "velocityX": -4.050906402804312,
+                    "velocityY": 0.7904163556412387,
+                    "timestamp": 9.62992732690462
+                },
+                {
+                    "x": 6.071046358370447,
+                    "y": 6.761675318964879,
+                    "heading": -0.0003587026360581851,
+                    "angularVelocity": 0.04826638181018736,
+                    "velocityX": -4.14727310757996,
+                    "velocityY": 0.41137342035837426,
+                    "timestamp": 9.694283408188005
+                },
+                {
+                    "x": 5.802036285400391,
+                    "y": 6.762963199615477,
+                    "heading": 3.48426610312228e-20,
+                    "angularVelocity": 0.0055737178042065665,
+                    "velocityX": -4.180025688411564,
+                    "velocityY": 0.020011794144629762,
+                    "timestamp": 9.75863948947139
+                },
+                {
+                    "x": 5.504490144089293,
+                    "y": 6.7332005959035195,
+                    "heading": 1.0813799079954769e-8,
+                    "angularVelocity": 1.512165560586682e-7,
+                    "velocityX": -4.160785901890718,
+                    "velocityY": -0.4161903138202568,
+                    "timestamp": 9.83015149425296
+                },
+                {
+                    "x": 5.212483549504086,
+                    "y": 6.672709068505996,
+                    "heading": 1.7927215293047143e-8,
+                    "angularVelocity": 9.947163745193156e-8,
+                    "velocityX": -4.08332272989868,
+                    "velocityY": -0.8458933235376672,
+                    "timestamp": 9.901663499034528
+                },
+                {
+                    "x": 4.936047112225373,
+                    "y": 6.585152370785529,
+                    "heading": 2.0960766953807052e-8,
+                    "angularVelocity": 4.2420173648314827e-8,
+                    "velocityX": -3.86559484834855,
+                    "velocityY": -1.2243636294060563,
+                    "timestamp": 9.973175503816098
+                },
+                {
+                    "x": 4.685471386719961,
+                    "y": 6.480097764592836,
+                    "heading": 2.0521148140147226e-8,
+                    "angularVelocity": -6.147482719554469e-9,
+                    "velocityX": -3.5039672887200504,
+                    "velocityY": -1.469048539662375,
+                    "timestamp": 10.044687508597667
+                },
+                {
+                    "x": 4.465637952492511,
+                    "y": 6.36957828843063,
+                    "heading": 1.806553462983808e-8,
+                    "angularVelocity": -3.433847950126585e-8,
+                    "velocityX": -3.0740773510534587,
+                    "velocityY": -1.545467456824671,
+                    "timestamp": 10.116199513379236
+                },
+                {
+                    "x": 4.276834572623958,
+                    "y": 6.2625357492286335,
+                    "heading": 1.4753077547904654e-8,
+                    "angularVelocity": -4.632029394280808e-8,
+                    "velocityX": -2.640163430535123,
+                    "velocityY": -1.4968471311768274,
+                    "timestamp": 10.187711518160805
+                },
+                {
+                    "x": 4.117926572195921,
+                    "y": 6.164505408559498,
+                    "heading": 1.1239712389074229e-8,
+                    "angularVelocity": -4.912972541552041e-8,
+                    "velocityX": -2.2221164252549563,
+                    "velocityY": -1.3708235556892094,
+                    "timestamp": 10.259223522942374
+                },
+                {
+                    "x": 3.9876518132215875,
+                    "y": 6.078928620338262,
+                    "heading": 7.894590787280817e-9,
+                    "angularVelocity": -4.6777063683539706e-8,
+                    "velocityX": -1.8217187362073228,
+                    "velocityY": -1.1966772359777518,
+                    "timestamp": 10.330735527723943
+                },
+                {
+                    "x": 3.8849194916806877,
+                    "y": 6.008061273344668,
+                    "heading": 4.950126193053713e-9,
+                    "angularVelocity": -4.1174409852415885e-8,
+                    "velocityX": -1.4365744864053387,
+                    "velocityY": -0.9909853207171924,
+                    "timestamp": 10.402247532505513
+                },
+                {
+                    "x": 3.808836355920425,
+                    "y": 5.953465817607232,
+                    "heading": 2.5714210248063034e-9,
+                    "angularVelocity": -3.3263018922920817e-8,
+                    "velocityX": -1.0639211694967192,
+                    "velocityY": -0.763444625894576,
+                    "timestamp": 10.473759537287082
+                },
+                {
+                    "x": 3.758675804955618,
+                    "y": 5.916276486245191,
+                    "heading": 8.864982180144823e-10,
+                    "angularVelocity": -2.3561398005174934e-8,
+                    "velocityX": -0.7014283981832174,
+                    "velocityY": -0.5200431938055047,
+                    "timestamp": 10.545271542068651
+                },
+                {
+                    "x": 3.733842372894287,
+                    "y": 5.897348880767822,
+                    "heading": 2.776883269501392e-22,
+                    "angularVelocity": -1.2396495116174843e-8,
+                    "velocityX": -0.34726242310201455,
+                    "velocityY": -0.26467731586021115,
+                    "timestamp": 10.61678354685022
+                },
+                {
+                    "x": 3.733842372894287,
+                    "y": 5.897348880767822,
+                    "heading": 1.3627894416387573e-22,
+                    "angularVelocity": -7.174234051514954e-23,
+                    "velocityX": 3.367103597351517e-20,
+                    "velocityY": -6.161027541818125e-20,
+                    "timestamp": 10.68829555163179
+                }
+            ],
+            "trajectoryWaypoints": [
+                {
+                    "timestamp": 0,
+                    "isStopPoint": true,
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "timestamp": 1.087700259463512,
+                    "isStopPoint": true,
+                    "x": 2.629361867904663,
+                    "y": 4.316425800323486,
+                    "heading": -0.4973420499689195,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "timestamp": 1.7308796906716322,
+                    "isStopPoint": false,
+                    "x": 2.510251045227051,
+                    "y": 5.334280490875244,
+                    "heading": 0.5713370823927012,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "timestamp": 2.2930859047476964,
+                    "isStopPoint": true,
+                    "x": 3.0191783905029297,
+                    "y": 5.832379341125488,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 11
+                },
+                {
+                    "timestamp": 2.8924182484996503,
+                    "isStopPoint": false,
+                    "x": 2.445281744003296,
+                    "y": 6.449589252471924,
+                    "heading": 0.5224037961561394,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 6
+                },
+                {
+                    "timestamp": 3.3920386132295137,
+                    "isStopPoint": true,
+                    "x": 2.7051594257354736,
+                    "y": 6.882719039916992,
+                    "heading": 0.5224037961561394,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "timestamp": 4.673199629925819,
+                    "isStopPoint": false,
+                    "x": 6.776577949523926,
+                    "y": 6.9043755531311035,
+                    "heading": 0.5224037961561394,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "timestamp": 5.315938075232567,
+                    "isStopPoint": false,
+                    "x": 8.021825790405273,
+                    "y": 7.337504863739014,
+                    "heading": 0.5224037961561394,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "timestamp": 7.080237536780859,
+                    "isStopPoint": true,
+                    "x": 3.733842372894287,
+                    "y": 6.697348880767819,
+                    "heading": 0.5224037961561394,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "timestamp": 7.890369880465728,
+                    "isStopPoint": false,
+                    "x": 5.7778338432312015,
+                    "y": 6.801184844970702,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "timestamp": 8.793298270220609,
+                    "isStopPoint": false,
+                    "x": 8.03265380859375,
+                    "y": 5.864864349365234,
+                    "heading": -0.302884711661745,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "timestamp": 9.75863948947139,
+                    "isStopPoint": false,
+                    "x": 5.802036285400391,
+                    "y": 6.762963199615477,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "timestamp": 10.68829555163179,
+                    "isStopPoint": true,
+                    "x": 3.733842372894287,
+                    "y": 5.897348880767822,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        1
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        3
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        5
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        8
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": [],
+            "eventMarkers": [],
+            "isTrajectoryStale": false
+        },
+        "Distance Center 6 Wing First Subwoofer": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 2.629361867904663,
+                    "y": 4.2189717292785645,
+                    "heading": -0.44684263169879895,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 2.6500000000000012,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 2.629361867904663,
+                    "y": 6.918971729278555,
+                    "heading": 0.44684263169879895,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 8.08679485321045,
+                    "y": 7.44578742980957,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 4.056788444519045,
+                    "y": 6.276337623596191,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 6.321792125701904,
+                    "y": 6.503730297088623,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 7.90271520614624,
+                    "y": 5.908177375793457,
+                    "heading": -0.34555536227067424,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 6.321792125701904,
+                    "y": 6.503730297088623,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 4.056788444519045,
+                    "y": 6.276337623596191,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": -1.3105569434022965e-22,
+                    "angularVelocity": -4.761453764625247e-22,
+                    "velocityX": 1.1722115722360089e-20,
+                    "velocityY": -1.2492293356601218e-20,
+                    "timestamp": 0
+                },
+                {
+                    "x": 1.376106022093243,
+                    "y": 5.554437182407712,
+                    "heading": -0.009091989168136132,
+                    "angularVelocity": -0.11487456261093582,
+                    "velocityX": 0.32984177763681827,
+                    "velocityY": -0.3514924679072023,
+                    "timestamp": 0.07914710586476341
+                },
+                {
+                    "x": 1.428318135425877,
+                    "y": 5.498798018189392,
+                    "heading": -0.027275691769830347,
+                    "angularVelocity": -0.22974564139798406,
+                    "velocityX": 0.6596844289145241,
+                    "velocityY": -0.7029841914041539,
+                    "timestamp": 0.15829421172952682
+                },
+                {
+                    "x": 1.5066364383487854,
+                    "y": 5.415339395500696,
+                    "heading": -0.05455120903188366,
+                    "angularVelocity": -0.34461799915537367,
+                    "velocityX": 0.9895283228262182,
+                    "velocityY": -1.05447472496721,
+                    "timestamp": 0.2374413175942902
+                },
+                {
+                    "x": 1.6110610489670512,
+                    "y": 5.304061450304077,
+                    "heading": -0.09091971001300213,
+                    "angularVelocity": -0.4595051276197058,
+                    "velocityX": 1.3193737089602908,
+                    "velocityY": -1.4059635406853233,
+                    "timestamp": 0.31658842345905364
+                },
+                {
+                    "x": 1.7415920897935755,
+                    "y": 5.164964366004136,
+                    "heading": -0.1363843221081841,
+                    "angularVelocity": -0.5744317697840544,
+                    "velocityX": 1.6492206430082152,
+                    "velocityY": -1.7574500391411858,
+                    "timestamp": 0.39573552932381706
+                },
+                {
+                    "x": 1.8982296650833075,
+                    "y": 4.998048375309143,
+                    "heading": -0.19095112736368186,
+                    "angularVelocity": -0.6894352567829158,
+                    "velocityX": 1.9790688942862253,
+                    "velocityY": -2.108933597397716,
+                    "timestamp": 0.4748826351885805
+                },
+                {
+                    "x": 2.080973829240046,
+                    "y": 4.803313756359354,
+                    "heading": -0.25463009738549497,
+                    "angularVelocity": -0.8045647320398506,
+                    "velocityX": 2.308917832939953,
+                    "velocityY": -2.4604136414353124,
+                    "timestamp": 0.5540297410533439
+                },
+                {
+                    "x": 2.237634329672691,
+                    "y": 4.636382002307393,
+                    "heading": -0.3095162598785737,
+                    "angularVelocity": -0.6934702399208541,
+                    "velocityX": 1.979358546607204,
+                    "velocityY": -2.109132762696247,
+                    "timestamp": 0.6331768469181073
+                },
+                {
+                    "x": 2.368188294580038,
+                    "y": 4.497268491668379,
+                    "heading": -0.35527028560397067,
+                    "angularVelocity": -0.5780884244027288,
+                    "velocityX": 1.6495102819100074,
+                    "velocityY": -1.7576575810202477,
+                    "timestamp": 0.7123239527828706
+                },
+                {
+                    "x": 2.47263578406431,
+                    "y": 4.385973104721263,
+                    "heading": -0.39188377719785317,
+                    "angularVelocity": -0.46260051070525543,
+                    "velocityX": 1.3196627765864057,
+                    "velocityY": -1.4061839119838944,
+                    "timestamp": 0.791471058647634
+                },
+                {
+                    "x": 2.5509768566083473,
+                    "y": 4.302495763635934,
+                    "heading": -0.41935051793107386,
+                    "angularVelocity": -0.34703405049519326,
+                    "velocityX": 0.9898160101760007,
+                    "velocityY": -1.0547112263076903,
+                    "timestamp": 0.8706181645123974
+                },
+                {
+                    "x": 2.603211554320217,
+                    "y": 4.246836425701493,
+                    "heading": -0.437666590123682,
+                    "angularVelocity": -0.2314180915712106,
+                    "velocityX": 0.6599697757884107,
+                    "velocityY": -0.7032390802709192,
+                    "timestamp": 0.9497652703771607
+                },
+                {
+                    "x": 2.6293398948866087,
+                    "y": 4.218995077070589,
+                    "heading": -0.44683039365411925,
+                    "angularVelocity": -0.11578191559013186,
+                    "velocityX": 0.33012376486686545,
+                    "velocityY": -0.3517671091912785,
+                    "timestamp": 1.028912376241924
+                },
+                {
+                    "x": 2.629361867904663,
+                    "y": 4.2189717292785645,
+                    "heading": -0.44684263169879895,
+                    "angularVelocity": -0.00015462403262836384,
+                    "velocityX": 0.00027762250829401467,
+                    "velocityY": -0.0002949923660397582,
+                    "timestamp": 1.1080594821066876
+                },
+                {
+                    "x": 2.6032711516040385,
+                    "y": 4.24677311445227,
+                    "heading": -0.4377041053238575,
+                    "angularVelocity": 0.11544864826739264,
+                    "velocityX": -0.32960871432123445,
+                    "velocityY": 0.35121990204749487,
+                    "timestamp": 1.1872161159182462
+                },
+                {
+                    "x": 2.5510677157973753,
+                    "y": 4.302399261550015,
+                    "heading": -0.4194176623258515,
+                    "angularVelocity": 0.23101592522919745,
+                    "velocityX": -0.6594953990961658,
+                    "velocityY": 0.7027351267888616,
+                    "timestamp": 1.2663727497298047
+                },
+                {
+                    "x": 2.472751515756506,
+                    "y": 4.38585023239311,
+                    "heading": -0.3919884490131159,
+                    "angularVelocity": 0.34651818795167477,
+                    "velocityX": -0.9893826489300932,
+                    "velocityY": 1.0542511325299573,
+                    "timestamp": 1.3455293835413633
+                },
+                {
+                    "x": 2.36832250186272,
+                    "y": 4.497126133012537,
+                    "heading": -0.35542377520119517,
+                    "angularVelocity": 0.46192810445890287,
+                    "velocityX": -1.3192705256061201,
+                    "velocityY": 1.4057684777795487,
+                    "timestamp": 1.4246860173529219
+                },
+                {
+                    "x": 2.2377806374279694,
+                    "y": 4.636227121930257,
+                    "heading": -0.30973274156337427,
+                    "angularVelocity": 0.5772230505227592,
+                    "velocityX": -1.6491588657688505,
+                    "velocityY": 1.7572878256655662,
+                    "timestamp": 1.5038426511644805
+                },
+                {
+                    "x": 2.081125922237724,
+                    "y": 4.803153414756678,
+                    "heading": -0.2549254844011435,
+                    "angularVelocity": 0.6923899428657577,
+                    "velocityX": -1.9790472086417852,
+                    "velocityY": 2.1088098973967053,
+                    "timestamp": 1.582999284976039
+                },
+                {
+                    "x": 1.8983584218600063,
+                    "y": 4.997905279612994,
+                    "heading": -0.19101192332640735,
+                    "angularVelocity": 0.8074315189664284,
+                    "velocityX": -2.3089347231821,
+                    "velocityY": 2.4603353563511114,
+                    "timestamp": 1.6621559187875976
+                },
+                {
+                    "x": 1.7416842694507195,
+                    "y": 5.164862529299444,
+                    "heading": -0.13643643316957763,
+                    "angularVelocity": 0.6894619885776455,
+                    "velocityX": -1.9792927625278673,
+                    "velocityY": 2.1092009809804435,
+                    "timestamp": 1.7413125525991562
+                },
+                {
+                    "x": 1.6111226785905355,
+                    "y": 5.303993796816868,
+                    "heading": -0.09095798078133475,
+                    "angularVelocity": 0.574537473340636,
+                    "velocityX": -1.649408073251331,
+                    "velocityY": 1.757670340664581,
+                    "timestamp": 1.8204691864107148
+                },
+                {
+                    "x": 1.506673537960871,
+                    "y": 5.415298936436349,
+                    "heading": -0.054575132238388,
+                    "angularVelocity": 0.4596310731146078,
+                    "velocityX": -1.319524790282489,
+                    "velocityY": 1.4061378593290832,
+                    "timestamp": 1.8996258202222733
+                },
+                {
+                    "x": 1.428336751560515,
+                    "y": 5.498777850544818,
+                    "heading": -0.027287678660803165,
+                    "angularVelocity": 0.34472731170637844,
+                    "velocityX": -0.9896427201142192,
+                    "velocityY": 1.054604144830112,
+                    "timestamp": 1.978782454033832
+                },
+                {
+                    "x": 1.3761122507008792,
+                    "y": 5.55443047925558,
+                    "heading": -0.009095885873172527,
+                    "angularVelocity": 0.2298201920882369,
+                    "velocityX": -0.6597615177012465,
+                    "velocityY": 0.7030696737717484,
+                    "timestamp": 2.0579390878453907
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 3.968181320363232e-22,
+                    "angularVelocity": 0.11490996313494475,
+                    "velocityX": -0.32988076227498664,
+                    "velocityY": 0.3515348414953293,
+                    "timestamp": 2.1370957216569493
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 1.7752085844076005e-22,
+                    "angularVelocity": -5.239816034386896e-22,
+                    "velocityX": 2.9480236062895993e-20,
+                    "velocityY": -1.2346716393304613e-20,
+                    "timestamp": 2.216252355468508
+                },
+                {
+                    "x": 1.4019931563006072,
+                    "y": 5.58225679397583,
+                    "heading": 6.677169422600811e-24,
+                    "angularVelocity": -1.8513645761030444e-21,
+                    "velocityX": 0.563430207687053,
+                    "velocityY": 1.0793744639684502e-16,
+                    "timestamp": 2.3085320302023935
+                },
+                {
+                    "x": 1.5059794683382701,
+                    "y": 5.58225679397583,
+                    "heading": -1.5919770654572408e-22,
+                    "angularVelocity": -1.7975194292009434e-21,
+                    "velocityX": 1.1268604092671195,
+                    "velocityY": 2.1588928379491377e-16,
+                    "timestamp": 2.400811704936279
+                },
+                {
+                    "x": 1.661958935173697,
+                    "y": 5.58225679397583,
+                    "heading": -1.0739397610061377e-22,
+                    "angularVelocity": 5.613814545355455e-22,
+                    "velocityX": 1.6902906006684286,
+                    "velocityY": 3.2384452847940683e-16,
+                    "timestamp": 2.493091379670165
+                },
+                {
+                    "x": 1.8699315549281479,
+                    "y": 5.58225679397583,
+                    "heading": 5.877231323874057e-22,
+                    "angularVelocity": 7.532725689717569e-21,
+                    "velocityX": 2.253720771710548,
+                    "velocityY": 4.3180658827193484e-16,
+                    "timestamp": 2.5853710544040505
+                },
+                {
+                    "x": 2.1298973219641684,
+                    "y": 5.58225679397583,
+                    "heading": 3.2033613274405696e-21,
+                    "angularVelocity": 2.834468774110536e-20,
+                    "velocityX": 2.817150881661696,
+                    "velocityY": 5.397890976977118e-16,
+                    "timestamp": 2.677650729137936
+                },
+                {
+                    "x": 2.3379041602219917,
+                    "y": 5.58225679397583,
+                    "heading": 1.310690511155894e-21,
+                    "angularVelocity": -2.0510155439556265e-20,
+                    "velocityX": 2.2540915847142853,
+                    "velocityY": 5.581506402100161e-16,
+                    "timestamp": 2.7699304038718218
+                },
+                {
+                    "x": 2.4939178511908273,
+                    "y": 5.58225679397583,
+                    "heading": 3.997113631994639e-23,
+                    "angularVelocity": -1.377030243024098e-20,
+                    "velocityX": 1.6906614746827489,
+                    "velocityY": 4.50249833557945e-16,
+                    "timestamp": 2.8622100786057074
+                },
+                {
+                    "x": 2.597938389238784,
+                    "y": 5.58225679397583,
+                    "heading": -6.030574852797699e-22,
+                    "angularVelocity": -6.968254669755187e-21,
+                    "velocityX": 1.127231303620532,
+                    "velocityY": 3.4232862627505307e-16,
+                    "timestamp": 2.954489753339593
+                },
+                {
+                    "x": 2.6499657724881525,
+                    "y": 5.58225679397583,
+                    "heading": -6.166353516647707e-22,
+                    "angularVelocity": -1.4713443371706887e-22,
+                    "velocityX": 0.5638011122102904,
+                    "velocityY": 2.344006170727818e-16,
+                    "timestamp": 3.0467694280734787
+                },
+                {
+                    "x": 2.6500000000000012,
+                    "y": 5.58225679397583,
+                    "heading": 7.150164834326212e-28,
+                    "angularVelocity": 6.6822560720639164e-21,
+                    "velocityX": 0.00037091062519919635,
+                    "velocityY": 1.2646920658154274e-16,
+                    "timestamp": 3.1390491028073644
+                },
+                {
+                    "x": 2.5980273838116625,
+                    "y": 5.58225679397583,
+                    "heading": 1.2475447557817004e-21,
+                    "angularVelocity": 1.3517380299097123e-20,
+                    "velocityX": -0.5631334791901095,
+                    "velocityY": 1.852154451951177e-17,
+                    "timestamp": 3.2313409272307405
+                },
+                {
+                    "x": 2.494047919980239,
+                    "y": 5.58225679397583,
+                    "heading": 3.2324491441975756e-21,
+                    "angularVelocity": 2.1506824119623174e-20,
+                    "velocityX": -1.1266378628992357,
+                    "velocityY": -8.942818834696392e-17,
+                    "timestamp": 3.3236327516541166
+                },
+                {
+                    "x": 2.3380616094450217,
+                    "y": 5.58225679397583,
+                    "heading": 6.132702376489969e-21,
+                    "angularVelocity": 3.142480817163132e-20,
+                    "velocityX": -1.6901422364309449,
+                    "velocityY": -1.9738137273676251e-16,
+                    "timestamp": 3.415924576077493
+                },
+                {
+                    "x": 2.1300684540847508,
+                    "y": 5.58225679397583,
+                    "heading": 1.0303595448942247e-20,
+                    "angularVelocity": 4.519243978206457e-20,
+                    "velocityX": -2.253646589606145,
+                    "velocityY": -3.05341460883141e-16,
+                    "timestamp": 3.508216400500869
+                },
+                {
+                    "x": 1.8700684595368808,
+                    "y": 5.58225679397583,
+                    "heading": 1.681410681468819e-20,
+                    "angularVelocity": 7.054266238709e-20,
+                    "velocityX": -2.8171508816984203,
+                    "velocityY": -4.133222662368775e-16,
+                    "timestamp": 3.600508224924245
+                },
+                {
+                    "x": 1.6620410811667898,
+                    "y": 5.58225679397583,
+                    "heading": 7.817959688103422e-21,
+                    "angularVelocity": -9.747502065767468e-20,
+                    "velocityX": -2.2540174026227233,
+                    "velocityY": -4.316713876129951e-16,
+                    "timestamp": 3.692800049347621
+                },
+                {
+                    "x": 1.5060205419917154,
+                    "y": 5.58225679397583,
+                    "heading": 3.3227072373995285e-21,
+                    "angularVelocity": -4.870694497734212e-20,
+                    "velocityX": -1.6905131104500808,
+                    "velocityY": -3.237733503311185e-16,
+                    "timestamp": 3.7850918737709973
+                },
+                {
+                    "x": 1.4020068476435492,
+                    "y": 5.58225679397583,
+                    "heading": 9.77517677073401e-22,
+                    "angularVelocity": -2.5410592286057495e-20,
+                    "velocityX": -1.1270087572547884,
+                    "velocityY": -2.158546447821665e-16,
+                    "timestamp": 3.8773836981943735
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 4.719104857778951e-25,
+                    "angularVelocity": -1.0586486204388865e-20,
+                    "velocityX": -0.5635043837141495,
+                    "velocityY": -1.0792904871510697e-16,
+                    "timestamp": 3.9696755226177496
+                },
+                {
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 3.507762761410347e-25,
+                    "angularVelocity": -1.3163866439134166e-24,
+                    "velocityX": 1.7883460029357865e-20,
+                    "velocityY": -7.181910053514036e-24,
+                    "timestamp": 4.061967347041126
+                },
+                {
+                    "x": 1.3761091905494292,
+                    "y": 5.609537510382012,
+                    "heading": 0.009092277255100973,
+                    "angularVelocity": 0.11546531077977794,
+                    "velocityX": 0.331567737808145,
+                    "velocityY": 0.346445264454243,
+                    "timestamp": 4.140712012375655
+                },
+                {
+                    "x": 1.4283276412673929,
+                    "y": 5.664098878978288,
+                    "heading": 0.02727672011448041,
+                    "angularVelocity": 0.23092920367528352,
+                    "velocityX": 0.6631363597283135,
+                    "velocityY": 0.6928897134108606,
+                    "timestamp": 4.219456677710184
+                },
+                {
+                    "x": 1.5066554501194507,
+                    "y": 5.745940797463734,
+                    "heading": 0.0545537157896761,
+                    "angularVelocity": 0.3463980138758046,
+                    "velocityX": 0.9947062257398623,
+                    "velocityY": 1.0393328632201235,
+                    "timestamp": 4.298201343044713
+                },
+                {
+                    "x": 1.6110927331941791,
+                    "y": 5.85506311882236,
+                    "heading": 0.09092486765955961,
+                    "angularVelocity": 0.4618871858222897,
+                    "velocityX": 1.3262775659919341,
+                    "velocityY": 1.3857741460331916,
+                    "timestamp": 4.376946008379242
+                },
+                {
+                    "x": 1.7416396083512349,
+                    "y": 5.991465645233681,
+                    "heading": 0.13639392962694408,
+                    "angularVelocity": 0.5774240296053013,
+                    "velocityX": 1.657850402975963,
+                    "velocityY": 1.7322129166699285,
+                    "timestamp": 4.455690673713772
+                },
+                {
+                    "x": 1.8982961713119855,
+                    "y": 6.155148124011134,
+                    "heading": 0.19096791231543717,
+                    "angularVelocity": 0.6930499032111946,
+                    "velocityX": 1.9894244555517604,
+                    "velocityY": 2.0786484783710053,
+                    "timestamp": 4.534435339048301
+                },
+                {
+                    "x": 2.081062461891351,
+                    "y": 6.346110236484044,
+                    "heading": 0.25465858240789885,
+                    "angularVelocity": 0.8088252051346765,
+                    "velocityX": 2.3209990137481022,
+                    "velocityY": 2.425079993186225,
+                    "timestamp": 4.61318000438283
+                },
+                {
+                    "x": 2.2377196372080883,
+                    "y": 6.509785226593951,
+                    "heading": 0.30954671204597756,
+                    "angularVelocity": 0.6970393410766176,
+                    "velocityX": 1.9894322320275923,
+                    "velocityY": 2.078553377737655,
+                    "timestamp": 4.691924669717359
+                },
+                {
+                    "x": 2.3682671510821507,
+                    "y": 6.646180841197734,
+                    "heading": 0.3553020329707184,
+                    "angularVelocity": 0.5810593102448243,
+                    "velocityX": 1.657858514217577,
+                    "velocityY": 1.7321251417392771,
+                    "timestamp": 4.770669335051888
+                },
+                {
+                    "x": 2.472705075371397,
+                    "y": 6.755297231195074,
+                    "heading": 0.39191475956413135,
+                    "angularVelocity": 0.46495500917899774,
+                    "velocityX": 1.3262857089501277,
+                    "velocityY": 1.3856988220571407,
+                    "timestamp": 4.8494140003864175
+                },
+                {
+                    "x": 2.5510334762030915,
+                    "y": 6.837134483375156,
+                    "heading": 0.4193781488302824,
+                    "angularVelocity": 0.3487650769671707,
+                    "velocityX": 0.9947137434508653,
+                    "velocityY": 1.039273604534536,
+                    "timestamp": 4.928158665720947
+                },
+                {
+                    "x": 2.603252400434301,
+                    "y": 6.891692644090699,
+                    "heading": 0.437688018711337,
+                    "angularVelocity": 0.23252203566132756,
+                    "velocityX": 0.6631423730022795,
+                    "velocityY": 0.6928489756577794,
+                    "timestamp": 5.006903331055476
+                },
+                {
+                    "x": 2.629361867904663,
+                    "y": 6.918971729278555,
+                    "heading": 0.44684263169879895,
+                    "angularVelocity": 0.11625692926080337,
+                    "velocityX": 0.3315712545026574,
+                    "velocityY": 0.3464245491674547,
+                    "timestamp": 5.085647996390005
+                },
+                {
+                    "x": 2.629361867904663,
+                    "y": 6.918971729278555,
+                    "heading": 0.44684263169879895,
+                    "angularVelocity": 1.7104653429303553e-24,
+                    "velocityX": 8.676311589571829e-24,
+                    "velocityY": 9.986643357418398e-25,
+                    "timestamp": 5.164392661724534
+                },
+                {
+                    "x": 2.647240076574808,
+                    "y": 6.9229927793221195,
+                    "heading": 0.43944656124636106,
+                    "angularVelocity": -0.13452107233246685,
+                    "velocityX": 0.32517210553324816,
+                    "velocityY": 0.073135588315622,
+                    "timestamp": 5.219373419193956
+                },
+                {
+                    "x": 2.6830022691087487,
+                    "y": 6.931034450131843,
+                    "heading": 0.4248111822038071,
+                    "angularVelocity": -0.26619093144894534,
+                    "velocityX": 0.6504492513372576,
+                    "velocityY": 0.14626336885583588,
+                    "timestamp": 5.274354176663378
+                },
+                {
+                    "x": 2.736655222701382,
+                    "y": 6.94309618304354,
+                    "heading": 0.40312445923850565,
+                    "angularVelocity": -0.3944420550655893,
+                    "velocityX": 0.9758496619926198,
+                    "velocityY": 0.21938098831042505,
+                    "timestamp": 5.3293349341328
+                },
+                {
+                    "x": 2.8082070403706108,
+                    "y": 6.959177291873429,
+                    "heading": 0.3746181200720642,
+                    "angularVelocity": -0.518478472805611,
+                    "velocityX": 1.3013974518088953,
+                    "velocityY": 0.29248612732978096,
+                    "timestamp": 5.384315691602222
+                },
+                {
+                    "x": 2.897667587057907,
+                    "y": 6.979276960170721,
+                    "heading": 0.3395861567651547,
+                    "angularVelocity": -0.6371677095644406,
+                    "velocityX": 1.6271246669719133,
+                    "velocityY": 0.365576416593952,
+                    "timestamp": 5.439296449071644
+                },
+                {
+                    "x": 3.005049084237076,
+                    "y": 7.003394214511703,
+                    "heading": 0.29841332172862295,
+                    "angularVelocity": -0.748858999613284,
+                    "velocityX": 1.9530741685196018,
+                    "velocityY": 0.4386490010508486,
+                    "timestamp": 5.494277206541065
+                },
+                {
+                    "x": 3.1303669283648046,
+                    "y": 7.03152781229272,
+                    "heading": 0.251621978066968,
+                    "angularVelocity": -0.8510494546692724,
+                    "velocityX": 2.279303703617138,
+                    "velocityY": 0.5116989848068959,
+                    "timestamp": 5.549257964010487
+                },
+                {
+                    "x": 3.2736407888898387,
+                    "y": 7.063675864480488,
+                    "heading": 0.1999566397529945,
+                    "angularVelocity": -0.9396985544025636,
+                    "velocityX": 2.605890990219942,
+                    "velocityY": 0.5847146104825557,
+                    "timestamp": 5.604238721479909
+                },
+                {
+                    "x": 3.4348957619080127,
+                    "y": 7.099834631071478,
+                    "heading": 0.14455793072354875,
+                    "angularVelocity": -1.0076017788633853,
+                    "velocityX": 2.932934729170615,
+                    "velocityY": 0.6576622122948987,
+                    "timestamp": 5.659219478949331
+                },
+                {
+                    "x": 3.6141609000051536,
+                    "y": 7.139994322888153,
+                    "heading": 0.08739671753641093,
+                    "angularVelocity": -1.0396585245106666,
+                    "velocityX": 3.260506881827549,
+                    "velocityY": 0.730431766768768,
+                    "timestamp": 5.714200236418753
+                },
+                {
+                    "x": 3.8114358161798583,
+                    "y": 7.184118841743803,
+                    "heading": 0.03277975478683384,
+                    "angularVelocity": -0.9933832355793388,
+                    "velocityX": 3.5880719956326845,
+                    "velocityY": 0.8025447608682011,
+                    "timestamp": 5.769180993888175
+                },
+                {
+                    "x": 4.025725584987085,
+                    "y": 7.231688135484132,
+                    "heading": 1.1487831117686327e-8,
+                    "angularVelocity": -0.5962039231131655,
+                    "velocityX": 3.8975412247895487,
+                    "velocityY": 0.8651989519566878,
+                    "timestamp": 5.824161751357597
+                },
+                {
+                    "x": 4.250000000000002,
+                    "y": 7.282256793975824,
+                    "heading": -1.3446001300471768e-25,
+                    "angularVelocity": -2.0894275827457326e-7,
+                    "velocityX": 4.079143782943505,
+                    "velocityY": 0.9197519426649702,
+                    "timestamp": 5.879142508827019
+                },
+                {
+                    "x": 4.555578615051821,
+                    "y": 7.331690581992348,
+                    "heading": 1.1852293345406531e-17,
+                    "angularVelocity": 1.6010580396806402e-16,
+                    "velocityX": 4.127885452839213,
+                    "velocityY": 0.6677725612361392,
+                    "timestamp": 5.953170390303632
+                },
+                {
+                    "x": 4.861157269791372,
+                    "y": 7.381124124675592,
+                    "heading": 2.2033445333656944e-17,
+                    "angularVelocity": 1.3753131637930226e-16,
+                    "velocityX": 4.127885988957933,
+                    "velocityY": 0.667769247170231,
+                    "timestamp": 6.027198271780246
+                },
+                {
+                    "x": 5.166735924531274,
+                    "y": 7.430557667356667,
+                    "heading": -3.355740357093402e-17,
+                    "angularVelocity": -7.5094474940707e-16,
+                    "velocityX": 4.127885988962678,
+                    "velocityY": 0.6677692471409031,
+                    "timestamp": 6.101226153256859
+                },
+                {
+                    "x": 5.472314579271176,
+                    "y": 7.47999121003774,
+                    "heading": -3.9827748951312203e-17,
+                    "angularVelocity": -8.470248310915055e-17,
+                    "velocityX": 4.12788598896268,
+                    "velocityY": 0.6677692471408927,
+                    "timestamp": 6.175254034733473
+                },
+                {
+                    "x": 5.77789323401108,
+                    "y": 7.529424752718813,
+                    "heading": -2.9096619403201524e-17,
+                    "angularVelocity": 1.4496064636808375e-16,
+                    "velocityX": 4.12788598896268,
+                    "velocityY": 0.6677692471408905,
+                    "timestamp": 6.249281916210086
+                },
+                {
+                    "x": 6.083471888751526,
+                    "y": 7.578858295396524,
+                    "heading": -1.7014104780220546e-17,
+                    "angularVelocity": 1.632157287494049e-16,
+                    "velocityX": 4.127885988970029,
+                    "velocityY": 0.6677692470954667,
+                    "timestamp": 6.3233097976867
+                },
+                {
+                    "x": 6.389050604827326,
+                    "y": 7.628291458921789,
+                    "heading": -2.8399965998049225e-17,
+                    "angularVelocity": -1.5380503927329875e-16,
+                    "velocityX": 4.127886817513971,
+                    "velocityY": 0.6677641253435272,
+                    "timestamp": 6.397337679163313
+                },
+                {
+                    "x": 6.6974520045085875,
+                    "y": 7.654947886751572,
+                    "heading": 7.456276940362482e-19,
+                    "angularVelocity": 3.937110330692362e-16,
+                    "velocityX": 4.166016824062293,
+                    "velocityY": 0.36008632555834863,
+                    "timestamp": 6.471365560639927
+                },
+                {
+                    "x": 6.983237000162054,
+                    "y": 7.656945566167564,
+                    "heading": 6.6435514939088325e-18,
+                    "angularVelocity": 7.96716545476509e-17,
+                    "velocityX": 3.860504852401468,
+                    "velocityY": 0.02698550027564136,
+                    "timestamp": 6.5453934421165405
+                },
+                {
+                    "x": 7.236389344029046,
+                    "y": 7.6515490284862535,
+                    "heading": 7.190524978488895e-18,
+                    "angularVelocity": 7.388749666939387e-18,
+                    "velocityX": 3.4196891605896758,
+                    "velocityY": -0.07289871834323541,
+                    "timestamp": 6.619421323593154
+                },
+                {
+                    "x": 7.456702532389806,
+                    "y": 7.639737294256418,
+                    "heading": 8.941088425266997e-18,
+                    "angularVelocity": 2.3647353022640597e-17,
+                    "velocityX": 2.976083929003971,
+                    "velocityY": -0.15955791242746284,
+                    "timestamp": 6.693449205069768
+                },
+                {
+                    "x": 7.6441107685462475,
+                    "y": 7.621856856041679,
+                    "heading": 1.0945616358890774e-17,
+                    "angularVelocity": 2.7078012954548576e-17,
+                    "velocityX": 2.53158988773232,
+                    "velocityY": -0.24153653810000633,
+                    "timestamp": 6.767477086546381
+                },
+                {
+                    "x": 7.79858185865481,
+                    "y": 7.598084937291478,
+                    "heading": 9.68583076902552e-18,
+                    "angularVelocity": -1.7017717713022894e-17,
+                    "velocityX": 2.0866609583763616,
+                    "velocityY": -0.3211211543006168,
+                    "timestamp": 6.841504968022995
+                },
+                {
+                    "x": 7.920096730975915,
+                    "y": 7.568529170908052,
+                    "heading": 8.319598760311882e-18,
+                    "angularVelocity": -1.8455641056660778e-17,
+                    "velocityX": 1.6414743998785648,
+                    "velocityY": -0.3992518196372258,
+                    "timestamp": 6.915532849499608
+                },
+                {
+                    "x": 8.0086427775389,
+                    "y": 7.533261859454375,
+                    "heading": 6.577363737030138e-18,
+                    "angularVelocity": -2.3534849148015073e-17,
+                    "velocityX": 1.1961175275690947,
+                    "velocityY": -0.476405791307416,
+                    "timestamp": 6.989560730976222
+                },
+                {
+                    "x": 8.064211046315053,
+                    "y": 7.49233491761698,
+                    "heading": 4.5104421550156944e-18,
+                    "angularVelocity": -2.792085280315562e-17,
+                    "velocityX": 0.7506397274614434,
+                    "velocityY": -0.5528584773876515,
+                    "timestamp": 7.063588612452835
+                },
+                {
+                    "x": 8.08679485321045,
+                    "y": 7.44578742980957,
+                    "heading": -4.5362375322406465e-30,
+                    "angularVelocity": -6.092896439943622e-17,
+                    "velocityX": 0.30507163577996627,
+                    "velocityY": -0.6287831946415446,
+                    "timestamp": 7.137616493929449
+                },
+                {
+                    "x": 8.056348015754137,
+                    "y": 7.3720119192300455,
+                    "heading": -3.562838906898219e-18,
+                    "angularVelocity": -3.5333147474799246e-17,
+                    "velocityX": -0.30194533800340373,
+                    "velocityY": -0.7316415542426236,
+                    "timestamp": 7.2384520877413605
+                },
+                {
+                    "x": 7.964689737823336,
+                    "y": 7.2878795305099695,
+                    "heading": -7.250663421617498e-18,
+                    "angularVelocity": -3.6572646376797225e-17,
+                    "velocityX": -0.9089873373659229,
+                    "velocityY": -0.8343520927442343,
+                    "timestamp": 7.339287681553272
+                },
+                {
+                    "x": 7.811816492885734,
+                    "y": 7.1934111312196,
+                    "heading": -9.984265294683441e-18,
+                    "angularVelocity": -2.710949348016387e-17,
+                    "velocityX": -1.516064309818574,
+                    "velocityY": -0.9368556847751761,
+                    "timestamp": 7.440123275365184
+                },
+                {
+                    "x": 7.597723004977704,
+                    "y": 7.088638022017575,
+                    "heading": -1.433836387576785e-17,
+                    "angularVelocity": -4.318017494077071e-17,
+                    "velocityX": -2.1231936047045,
+                    "velocityY": -1.039048864009848,
+                    "timestamp": 7.5409588691770955
+                },
+                {
+                    "x": 7.322400517715837,
+                    "y": 6.973612368749597,
+                    "heading": -2.1940207343875766e-17,
+                    "angularVelocity": -7.538849309728229e-17,
+                    "velocityX": -2.730409737809707,
+                    "velocityY": -1.140724707612031,
+                    "timestamp": 7.641794462989007
+                },
+                {
+                    "x": 6.985831656638794,
+                    "y": 6.848438495527462,
+                    "heading": -2.9554201439643655e-17,
+                    "angularVelocity": -7.550899248834429e-17,
+                    "velocityX": -3.337798175759688,
+                    "velocityY": -1.2413659551172047,
+                    "timestamp": 7.742630056800919
+                },
+                {
+                    "x": 6.587965404213338,
+                    "y": 6.713429306112849,
+                    "heading": -2.3579747883783766e-17,
+                    "angularVelocity": 5.924945081446257e-17,
+                    "velocityX": -3.945692561374678,
+                    "velocityY": -1.3389040943860189,
+                    "timestamp": 7.84346565061283
+                },
+                {
+                    "x": 6.1730951719461284,
+                    "y": 6.638125788985514,
+                    "heading": 1.9461532946570554e-17,
+                    "angularVelocity": 4.2684610863345274e-16,
+                    "velocityX": -4.1143232918434265,
+                    "velocityY": -0.7467949984784092,
+                    "timestamp": 7.944301244424742
+                },
+                {
+                    "x": 5.757475578384333,
+                    "y": 6.567074520342679,
+                    "heading": 3.073583715260816e-17,
+                    "angularVelocity": 1.1180877485527298e-16,
+                    "velocityX": -4.1217548075042965,
+                    "velocityY": -0.704624884496314,
+                    "timestamp": 8.045136838236653
+                },
+                {
+                    "x": 5.341855982130269,
+                    "y": 6.496023267448575,
+                    "heading": 4.9911384157422675e-17,
+                    "angularVelocity": 1.901664509492479e-16,
+                    "velocityX": -4.12175483420387,
+                    "velocityY": -0.7046247283140503,
+                    "timestamp": 8.145972432048564
+                },
+                {
+                    "x": 4.974693840732125,
+                    "y": 6.433255942720096,
+                    "heading": 4.3755003466781107e-17,
+                    "angularVelocity": -6.105364641788702e-17,
+                    "velocityX": -3.6411958071374304,
+                    "velocityY": -0.6224719105196076,
+                    "timestamp": 8.246808025860476
+                },
+                {
+                    "x": 4.668725379751608,
+                    "y": 6.380949837101729,
+                    "heading": 4.297922243446681e-17,
+                    "angularVelocity": -7.693523718012518e-18,
+                    "velocityX": -3.034329936621781,
+                    "velocityY": -0.5187266087403004,
+                    "timestamp": 8.347643619672388
+                },
+                {
+                    "x": 4.423950607483419,
+                    "y": 6.339104952011474,
+                    "heading": 3.647497975951615e-17,
+                    "angularVelocity": -6.450343999932714e-17,
+                    "velocityX": -2.4274639838465006,
+                    "velocityY": -0.41498129289849484,
+                    "timestamp": 8.4484792134843
+                },
+                {
+                    "x": 4.240369526692457,
+                    "y": 6.307721287922,
+                    "heading": 2.807259348278656e-17,
+                    "angularVelocity": -8.332758264591161e-17,
+                    "velocityX": -1.8205980036513219,
+                    "velocityY": -0.3112359723691862,
+                    "timestamp": 8.549314807296211
+                },
+                {
+                    "x": 4.1179821387611755,
+                    "y": 6.286798845069638,
+                    "heading": 1.6554566502834792e-17,
+                    "angularVelocity": -1.1422580603516648e-16,
+                    "velocityX": -1.2137320097461906,
+                    "velocityY": -0.20749064949612547,
+                    "timestamp": 8.650150401108123
+                },
+                {
+                    "x": 4.056788444519045,
+                    "y": 6.276337623596191,
+                    "heading": -1.081005308476118e-27,
+                    "angularVelocity": -1.6417383860510177e-16,
+                    "velocityX": -0.6068660076150864,
+                    "velocityY": -0.10374532521681326,
+                    "timestamp": 8.750985994920034
+                },
+                {
+                    "x": 4.056788444519045,
+                    "y": 6.276337623596191,
+                    "heading": -8.68734990058906e-28,
+                    "angularVelocity": -7.610530353701758e-29,
+                    "velocityX": -1.0690268508350096e-22,
+                    "velocityY": -4.0812108972482127e-23,
+                    "timestamp": 8.851821588731946
+                },
+                {
+                    "x": 4.0828323692232145,
+                    "y": 6.281987827191528,
+                    "heading": 5.917096277980191e-10,
+                    "angularVelocity": 8.956304425960182e-9,
+                    "velocityX": 0.39420909706230056,
+                    "velocityY": 0.08552327204275713,
+                    "timestamp": 8.917887858887182
+                },
+                {
+                    "x": 4.134945642418738,
+                    "y": 6.293169746044328,
+                    "heading": 1.7586803299010682e-9,
+                    "angularVelocity": 1.766363833407003e-8,
+                    "velocityX": 0.788803016623651,
+                    "velocityY": 0.16925306705715784,
+                    "timestamp": 8.983954129042418
+                },
+                {
+                    "x": 4.213159185837416,
+                    "y": 6.309735593389541,
+                    "heading": 3.480401916232622e-9,
+                    "angularVelocity": 2.6060523505958926e-8,
+                    "velocityX": 1.183864977315949,
+                    "velocityY": 0.25074591476540037,
+                    "timestamp": 9.050020399197654
+                },
+                {
+                    "x": 4.317511365530878,
+                    "y": 6.33149590597647,
+                    "heading": 5.730589257299653e-9,
+                    "angularVelocity": 3.405954862866893e-8,
+                    "velocityX": 1.57950765872307,
+                    "velocityY": 0.3293709866744319,
+                    "timestamp": 9.11608666935289
+                },
+                {
+                    "x": 4.448050927235057,
+                    "y": 6.358199069694473,
+                    "heading": 8.474350324606633e-9,
+                    "angularVelocity": 4.153043695156349e-8,
+                    "velocityX": 1.975888171035693,
+                    "velocityY": 0.4041875476738608,
+                    "timestamp": 9.182152939508127
+                },
+                {
+                    "x": 4.604841555340528,
+                    "y": 6.389494861933726,
+                    "heading": 1.1663148228371095e-8,
+                    "angularVelocity": 4.826665553045075e-8,
+                    "velocityX": 2.3732326304642357,
+                    "velocityY": 0.4737030282732438,
+                    "timestamp": 9.248219209663363
+                },
+                {
+                    "x": 4.7879690180112195,
+                    "y": 6.42486273284508,
+                    "heading": 1.5224903472771874e-8,
+                    "angularVelocity": 5.3911856020201134e-8,
+                    "velocityX": 2.7718753040000252,
+                    "velocityY": 0.5353393014052362,
+                    "timestamp": 9.314285479818599
+                },
+                {
+                    "x": 4.9975511936263155,
+                    "y": 6.463449108142169,
+                    "heading": 1.9041584229582254e-8,
+                    "angularVelocity": 5.777048935625297e-8,
+                    "velocityX": 3.1723022220361643,
+                    "velocityY": 0.5840556036601274,
+                    "timestamp": 9.380351749973835
+                },
+                {
+                    "x": 5.233736795312115,
+                    "y": 6.503606531218817,
+                    "heading": 2.2885938036083167e-8,
+                    "angularVelocity": 5.8189357405342333e-8,
+                    "velocityX": 3.574980109075873,
+                    "velocityY": 0.6078354806815993,
+                    "timestamp": 9.446418020129071
+                },
+                {
+                    "x": 5.496423817757033,
+                    "y": 6.540955963494301,
+                    "heading": 2.615742950768295e-8,
+                    "angularVelocity": 4.9518331576957005e-8,
+                    "velocityX": 3.9761140113961715,
+                    "velocityY": 0.565332842125389,
+                    "timestamp": 9.512484290284307
+                },
+                {
+                    "x": 5.772317890968588,
+                    "y": 6.555158418696577,
+                    "heading": 2.6157432845131755e-8,
+                    "angularVelocity": 5.051668270867061e-14,
+                    "velocityX": 4.176020116820391,
+                    "velocityY": 0.21497286238354563,
+                    "timestamp": 9.578550560439544
+                },
+                {
+                    "x": 6.0482967296543855,
+                    "y": 6.542711256144356,
+                    "heading": 2.61574082135951e-8,
+                    "angularVelocity": -3.7283074403740065e-13,
+                    "velocityX": 4.1773031538988095,
+                    "velocityY": -0.1884041966191396,
+                    "timestamp": 9.64461683059478
+                },
+                {
+                    "x": 6.321792125701904,
+                    "y": 6.503730297088623,
+                    "heading": 1.1564358585154957e-28,
+                    "angularVelocity": -3.9592681942136435e-7,
+                    "velocityX": 4.139712979178139,
+                    "velocityY": -0.5900281484657701,
+                    "timestamp": 9.710683100750016
+                },
+                {
+                    "x": 6.547355460734675,
+                    "y": 6.4527161195833775,
+                    "heading": -0.007269533201866749,
+                    "angularVelocity": -0.1301585532473417,
+                    "velocityX": 4.038635843353985,
+                    "velocityY": -0.9133917343524579,
+                    "timestamp": 9.76653446833592
+                },
+                {
+                    "x": 6.760403627607179,
+                    "y": 6.388539344746577,
+                    "heading": -0.033469969753863994,
+                    "angularVelocity": -0.46911002692456527,
+                    "velocityX": 3.814555955945363,
+                    "velocityY": -1.1490636238779843,
+                    "timestamp": 9.822385835921823
+                },
+                {
+                    "x": 6.955177537271062,
+                    "y": 6.320001202518741,
+                    "heading": -0.07019350986549461,
+                    "angularVelocity": -0.6575226659427229,
+                    "velocityX": 3.4873615111448264,
+                    "velocityY": -1.227152443893485,
+                    "timestamp": 9.878237203507727
+                },
+                {
+                    "x": 7.130957377141255,
+                    "y": 6.252260176669711,
+                    "heading": -0.11078396487947455,
+                    "angularVelocity": -0.7267584800237601,
+                    "velocityX": 3.1472790634862915,
+                    "velocityY": -1.212880342541995,
+                    "timestamp": 9.93408857109363
+                },
+                {
+                    "x": 7.288010376164827,
+                    "y": 6.187988784317964,
+                    "heading": -0.15176321891304065,
+                    "angularVelocity": -0.7337197960378785,
+                    "velocityX": 2.811981260476969,
+                    "velocityY": -1.1507577187414648,
+                    "timestamp": 9.989939938679534
+                },
+                {
+                    "x": 7.426691779416035,
+                    "y": 6.128716395109355,
+                    "heading": -0.19110286527521025,
+                    "angularVelocity": -0.7043631707256224,
+                    "velocityX": 2.48304400134704,
+                    "velocityY": -1.0612522444941654,
+                    "timestamp": 10.045791306265437
+                },
+                {
+                    "x": 7.547307972917185,
+                    "y": 6.075411733859649,
+                    "heading": -0.22748863294074106,
+                    "angularVelocity": -0.6514749636088404,
+                    "velocityX": 2.1595924811622984,
+                    "velocityY": -0.9544020774015809,
+                    "timestamp": 10.101642673851341
+                },
+                {
+                    "x": 7.650107021696187,
+                    "y": 6.028737089255784,
+                    "heading": -0.26000406925250097,
+                    "angularVelocity": -0.5821779791112268,
+                    "velocityX": 1.8405824820831433,
+                    "velocityY": -0.835693853549357,
+                    "timestamp": 10.157494041437245
+                },
+                {
+                    "x": 7.735288937949401,
+                    "y": 5.989171589665674,
+                    "heading": -0.2879751600685273,
+                    "angularVelocity": -0.500812997515315,
+                    "velocityX": 1.5251536342811545,
+                    "velocityY": -0.708406997004223,
+                    "timestamp": 10.213345409023148
+                },
+                {
+                    "x": 7.803016656542128,
+                    "y": 5.957077010153221,
+                    "heading": -0.3108860017100809,
+                    "angularVelocity": -0.41021093362333316,
+                    "velocityX": 1.2126420805820985,
+                    "velocityY": -0.574642679305731,
+                    "timestamp": 10.269196776609052
+                },
+                {
+                    "x": 7.853424723852741,
+                    "y": 5.932735740203229,
+                    "heading": -0.3283294175539037,
+                    "angularVelocity": -0.3123185088886745,
+                    "velocityX": 0.9025395346511673,
+                    "velocityY": -0.4358222726158591,
+                    "timestamp": 10.325048144194955
+                },
+                {
+                    "x": 7.886625758394815,
+                    "y": 5.916374096245245,
+                    "heading": -0.3399762524437299,
+                    "angularVelocity": -0.20853267150374424,
+                    "velocityX": 0.5944533854253174,
+                    "velocityY": -0.2929497461779804,
+                    "timestamp": 10.380899511780859
+                },
+                {
+                    "x": 7.90271520614624,
+                    "y": 5.908177375793457,
+                    "heading": -0.34555536227067424,
+                    "angularVelocity": -0.0998920898107501,
+                    "velocityX": 0.28807616441402983,
+                    "velocityY": -0.14675952991089913,
+                    "timestamp": 10.436750879366762
+                },
+                {
+                    "x": 7.900517279512601,
+                    "y": 5.908892154044228,
+                    "heading": -0.34435114666329186,
+                    "angularVelocity": 0.020228619667652558,
+                    "velocityX": -0.03692114738981263,
+                    "velocityY": 0.012006967268082137,
+                    "timestamp": 10.496281169973198
+                },
+                {
+                    "x": 7.878897174609603,
+                    "y": 5.918918511140869,
+                    "heading": -0.33622484089703686,
+                    "angularVelocity": 0.1365070736842074,
+                    "velocityX": -0.3631782187312475,
+                    "velocityY": 0.168424460799739,
+                    "timestamp": 10.555811460579633
+                },
+                {
+                    "x": 7.83776699530333,
+                    "y": 5.938088764329109,
+                    "heading": -0.3214499320108964,
+                    "angularVelocity": 0.24819144565948495,
+                    "velocityX": -0.6909117843585054,
+                    "velocityY": 0.3220251907550373,
+                    "timestamp": 10.615341751186069
+                },
+                {
+                    "x": 7.777022436898006,
+                    "y": 5.966198255304121,
+                    "heading": -0.3003583171650395,
+                    "angularVelocity": 0.3543005523909993,
+                    "velocityX": -1.0203974780992662,
+                    "velocityY": 0.4721880355137041,
+                    "timestamp": 10.674872041792504
+                },
+                {
+                    "x": 7.69653792964951,
+                    "y": 6.002991760583309,
+                    "heading": -0.27336046232249744,
+                    "angularVelocity": 0.45351458169470893,
+                    "velocityX": -1.3519925138715216,
+                    "velocityY": 0.6180635925739989,
+                    "timestamp": 10.73440233239894
+                },
+                {
+                    "x": 7.596159781373443,
+                    "y": 6.048142382340488,
+                    "heading": -0.24097637486036755,
+                    "angularVelocity": 0.543993438167913,
+                    "velocityX": -1.6861692972352893,
+                    "velocityY": 0.7584478640575794,
+                    "timestamp": 10.793932623005375
+                },
+                {
+                    "x": 7.4756963505533935,
+                    "y": 6.101216994676396,
+                    "heading": -0.20388575895135275,
+                    "angularVelocity": 0.6230545077333302,
+                    "velocityX": -2.0235653075583584,
+                    "velocityY": 0.8915564126302368,
+                    "timestamp": 10.85346291361181
+                },
+                {
+                    "x": 7.334904005956902,
+                    "y": 6.161615825528181,
+                    "heading": -0.16301475379345282,
+                    "angularVelocity": 0.6865581327009607,
+                    "velocityX": -2.3650538769799136,
+                    "velocityY": 1.0145898875429862,
+                    "timestamp": 10.912993204218246
+                },
+                {
+                    "x": 7.173468437929862,
+                    "y": 6.2284574380514455,
+                    "heading": -0.11969881454856235,
+                    "angularVelocity": 0.7276285535251157,
+                    "velocityX": -2.7118222737113333,
+                    "velocityY": 1.1228168356369232,
+                    "timestamp": 10.972523494824681
+                },
+                {
+                    "x": 6.990989423393413,
+                    "y": 6.3003338750456575,
+                    "heading": -0.07602350478150598,
+                    "angularVelocity": 0.7336653210010509,
+                    "velocityX": -3.0653136861509287,
+                    "velocityY": 1.2073926779460713,
+                    "timestamp": 11.032053785431117
+                },
+                {
+                    "x": 6.7870402492258135,
+                    "y": 6.374708055282701,
+                    "heading": -0.03565897410889504,
+                    "angularVelocity": 0.6780502877008852,
+                    "velocityX": -3.425973098568256,
+                    "velocityY": 1.2493501959992728,
+                    "timestamp": 11.091584076037552
+                },
+                {
+                    "x": 6.561897153690842,
+                    "y": 6.4462208450798215,
+                    "heading": -0.0063889390098301095,
+                    "angularVelocity": 0.4916830541374962,
+                    "velocityX": -3.781992213399873,
+                    "velocityY": 1.2012840701535195,
+                    "timestamp": 11.151114366643988
+                },
+                {
+                    "x": 6.321792125701904,
+                    "y": 6.503730297088623,
+                    "heading": -2.3188011153160923e-28,
+                    "angularVelocity": 0.10732248985761594,
+                    "velocityX": -4.0333253129286994,
+                    "velocityY": 0.9660536077172232,
+                    "timestamp": 11.210644657250423
+                },
+                {
+                    "x": 6.026159037797862,
+                    "y": 6.542710336847052,
+                    "heading": 7.2737432191713975e-9,
+                    "angularVelocity": 1.0199984022231118e-7,
+                    "velocityX": -4.145668443609958,
+                    "velocityY": 0.5466178427552496,
+                    "timestamp": 11.281955976986984
+                },
+                {
+                    "x": 5.728075284165582,
+                    "y": 6.550738026435919,
+                    "heading": 7.273746219720962e-9,
+                    "angularVelocity": 4.207676391895168e-14,
+                    "velocityX": -4.180034175969023,
+                    "velocityY": 0.11257244457854902,
+                    "timestamp": 11.353267296723544
+                },
+                {
+                    "x": 5.430772694641847,
+                    "y": 6.527726353002143,
+                    "heading": 7.273745820396832e-9,
+                    "angularVelocity": -5.5997299201229045e-15,
+                    "velocityX": -4.1690798967405485,
+                    "velocityY": -0.3226931365004294,
+                    "timestamp": 11.424578616460105
+                },
+                {
+                    "x": 5.155259555560304,
+                    "y": 6.482595213861262,
+                    "heading": 6.079879033454249e-9,
+                    "angularVelocity": -1.6741616777714582e-8,
+                    "velocityX": -3.863526016617648,
+                    "velocityY": -0.6328748269924842,
+                    "timestamp": 11.495889936196665
+                },
+                {
+                    "x": 4.910749929550947,
+                    "y": 6.439148378340905,
+                    "heading": 4.8501339645893866e-9,
+                    "angularVelocity": -1.7244738611033904e-8,
+                    "velocityX": -3.428763160079285,
+                    "velocityY": -0.6092558051212502,
+                    "timestamp": 11.567201255933226
+                },
+                {
+                    "x": 4.69702658726096,
+                    "y": 6.39973389360689,
+                    "heading": 3.7031081606903725e-9,
+                    "angularVelocity": -1.6084764776985806e-8,
+                    "velocityX": -2.997046514908499,
+                    "velocityY": -0.5527100729536375,
+                    "timestamp": 11.638512575669786
+                },
+                {
+                    "x": 4.513967146494363,
+                    "y": 6.3651959253494725,
+                    "heading": 2.681611531111107e-9,
+                    "angularVelocity": -1.4324466765625498e-8,
+                    "velocityX": -2.5670460376116413,
+                    "velocityY": -0.48432658917276944,
+                    "timestamp": 11.709823895406346
+                },
+                {
+                    "x": 4.36149950762511,
+                    "y": 6.335968042251882,
+                    "heading": 1.8076673867467656e-9,
+                    "angularVelocity": -1.2255335444539114e-8,
+                    "velocityX": -2.138056614749246,
+                    "velocityY": -0.4098631634579714,
+                    "timestamp": 11.781135215142907
+                },
+                {
+                    "x": 4.239576710769736,
+                    "y": 6.31231402870262,
+                    "heading": 1.0946867890651993e-9,
+                    "angularVelocity": -9.998140552095568e-9,
+                    "velocityX": -1.7097257112304496,
+                    "velocityY": -0.33170068422020343,
+                    "timestamp": 11.852446534879467
+                },
+                {
+                    "x": 4.148165858183431,
+                    "y": 6.294411248448443,
+                    "heading": 5.516922206255535e-10,
+                    "angularVelocity": -7.614423214232743e-9,
+                    "velocityX": -1.2818561334160725,
+                    "velocityY": -0.25105102977076493,
+                    "timestamp": 11.923757854616028
+                },
+                {
+                    "x": 4.087242656413975,
+                    "y": 6.282387124354374,
+                    "heading": 1.851682282397909e-10,
+                    "angularVelocity": -5.139772952453801e-9,
+                    "velocityX": -0.8543272231465969,
+                    "velocityY": -0.16861452204907645,
+                    "timestamp": 11.995069174352588
+                },
+                {
+                    "x": 4.056788444519045,
+                    "y": 6.276337623596191,
+                    "heading": 6.063226678000312e-28,
+                    "angularVelocity": -2.5966176046641755e-9,
+                    "velocityX": -0.4270599956281157,
+                    "velocityY": -0.08483226478671645,
+                    "timestamp": 12.066380494089149
+                },
+                {
+                    "x": 4.056788444519045,
+                    "y": 6.276337623596191,
+                    "heading": 2.9642655774633174e-28,
+                    "angularVelocity": -1.8888378589965254e-28,
+                    "velocityX": -6.1162792213078806e-27,
+                    "velocityY": -8.775569155171093e-28,
+                    "timestamp": 12.13769181382571
+                }
+            ],
+            "trajectoryWaypoints": [
+                {
+                    "timestamp": 0,
+                    "isStopPoint": true,
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "timestamp": 1.1080594821066876,
+                    "isStopPoint": false,
+                    "x": 2.629361867904663,
+                    "y": 4.2189717292785645,
+                    "heading": -0.44684263169879895,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "timestamp": 2.216252355468508,
+                    "isStopPoint": true,
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "timestamp": 3.1390491028073644,
+                    "isStopPoint": false,
+                    "x": 2.6500000000000012,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "timestamp": 4.061967347041126,
+                    "isStopPoint": true,
+                    "x": 1.35,
+                    "y": 5.58225679397583,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "timestamp": 5.164392661724534,
+                    "isStopPoint": true,
+                    "x": 2.629361867904663,
+                    "y": 6.918971729278555,
+                    "heading": 0.44684263169879895,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "timestamp": 5.879142508827019,
+                    "isStopPoint": false,
+                    "x": 4.250000000000002,
+                    "y": 7.282256793975824,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "timestamp": 7.137616493929449,
+                    "isStopPoint": false,
+                    "x": 8.08679485321045,
+                    "y": 7.44578742980957,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "timestamp": 8.851821588731946,
+                    "isStopPoint": true,
+                    "x": 4.056788444519045,
+                    "y": 6.276337623596191,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "timestamp": 9.710683100750016,
+                    "isStopPoint": false,
+                    "x": 6.321792125701904,
+                    "y": 6.503730297088623,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "timestamp": 10.436750879366762,
+                    "isStopPoint": false,
+                    "x": 7.90271520614624,
+                    "y": 5.908177375793457,
+                    "heading": -0.34555536227067424,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "timestamp": 11.210644657250423,
+                    "isStopPoint": false,
+                    "x": 6.321792125701904,
+                    "y": 6.503730297088623,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "timestamp": 12.13769181382571,
+                    "isStopPoint": true,
+                    "x": 4.056788444519045,
+                    "y": 6.276337623596191,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        2
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        4
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        5
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        7
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": [],
+            "eventMarkers": [],
+            "isTrajectoryStale": false
+        }
+    },
+    "splitTrajectoriesAtStopPoints": true,
+    "usesObstacles": true
+}
\ No newline at end of file

diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar
index 7454180f2ae8848c63b8b4dea2cb829da983f2fa..a4b76b9530d66f5e68d973ea569d8e19de379189 100644 (file)
Binary files a/gradle/wrapper/gradle-wrapper.jar and b/gradle/wrapper/gradle-wrapper.jar differ

diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties
index f959987ac8f80296c9462e867d85a083db254da6..34bd9ce95ff15872ceca0b74e9049f2d33016031 100644 (file)
--- a/gradle/wrapper/gradle-wrapper.properties
+++ b/gradle/wrapper/gradle-wrapper.properties
@@ -1,5 +1,7 @@
 distributionBase=GRADLE_USER_HOME
 distributionPath=permwrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-7.3.3-bin.zip
+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/gradlew b/gradlew
index c53aefaa5fc898b78c7265ea99a3bdc09349a7d0..f5feea6d6b116baaca5a2642d4d9fa1f47d574a7 100644 (file)
--- a/gradlew
+++ b/gradlew
@@ -1,7 +1,7 @@
 #!/bin/sh
 
 #
-# Copyright © 2015-2021 the original authors.
+# 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.
@@ -15,6 +15,8 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 #
+# SPDX-License-Identifier: Apache-2.0
+#
 
 ##############################################################################
 #
@@ -32,10 +34,10 @@
 #       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».
+#         * 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:
 #
@@ -55,7 +57,7 @@
 #       Darwin, MinGW, and NonStop.
 #
 #   (3) This script is generated from the Groovy template
-#       https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+#       https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
 #       within the Gradle project.
 #
 #       You can find Gradle at https://github.com/gradle/gradle/.
@@ -80,13 +82,12 @@ do
     esac
 done
 
-APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit
-
-APP_NAME="Gradle"
+# This is normally unused
+# shellcheck disable=SC2034
 APP_BASE_NAME=${0##*/}
-
-# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
-DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036)
+APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s
+' "$PWD" ) || exit
 
 # Use the maximum available, or set MAX_FD != -1 to use that value.
 MAX_FD=maximum
@@ -133,22 +134,29 @@ location of your Java installation."
     fi
 else
     JAVACMD=java
-    which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+    if ! command -v java >/dev/null 2>&1
+    then
+        die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
 
 Please set the JAVA_HOME variable in your environment to match the
 location of your Java installation."
+    fi
 fi
 
 # Increase the maximum file descriptors if we can.
 if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
     case $MAX_FD in #(
       max*)
+        # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked.
+        # shellcheck disable=SC2039,SC3045
         MAX_FD=$( ulimit -H -n ) ||
             warn "Could not query maximum file descriptor limit"
     esac
     case $MAX_FD in  #(
       '' | soft) :;; #(
       *)
+        # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked.
+        # shellcheck disable=SC2039,SC3045
         ulimit -n "$MAX_FD" ||
             warn "Could not set maximum file descriptor limit to $MAX_FD"
     esac
@@ -193,11 +201,15 @@ if "$cygwin" || "$msys" ; then
     done
 fi
 
-# Collect all arguments for the java command;
-#   * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of
-#     shell script including quotes and variable substitutions, so put them in
-#     double quotes to make sure that they get re-expanded; and
-#   * put everything else in single quotes, so that it's not re-expanded.
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+
+# Collect all arguments for the java command:
+#   * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments,
+#     and any embedded shellness will be escaped.
+#   * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be
+#     treated as '${Hostname}' itself on the command line.
 
 set -- \
         "-Dorg.gradle.appname=$APP_BASE_NAME" \
@@ -205,6 +217,12 @@ set -- \
         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.

diff --git a/gradlew.bat b/gradlew.bat
index 107acd32c4e687021ef32db511e8a206129b88ec..9d21a21834d5195c278ba17baec3115b2aaab06e 100644 (file)
--- a/gradlew.bat
+++ b/gradlew.bat
@@ -13,8 +13,10 @@
 @rem See the License for the specific language governing permissions and
 @rem limitations under the License.
 @rem
+@rem SPDX-License-Identifier: Apache-2.0
+@rem
 
-@if "%DEBUG%" == "" @echo off
+@if "%DEBUG%"=="" @echo off
 @rem ##########################################################################
 @rem
 @rem  Gradle startup script for Windows
@@ -25,7 +27,8 @@
 if "%OS%"=="Windows_NT" setlocal
 
 set DIRNAME=%~dp0
-if "%DIRNAME%" == "" set DIRNAME=.
+if "%DIRNAME%"=="" set DIRNAME=.
+@rem This is normally unused
 set APP_BASE_NAME=%~n0
 set APP_HOME=%DIRNAME%
 
@@ -40,13 +43,13 @@ if defined JAVA_HOME goto findJavaFromJavaHome
 
 set JAVA_EXE=java.exe
 %JAVA_EXE% -version >NUL 2>&1
-if "%ERRORLEVEL%" == "0" goto execute
+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.
+echo. 1>&2
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
 
 goto fail
 
@@ -56,11 +59,11 @@ 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.
+echo. 1>&2
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
 
 goto fail
 
@@ -75,13 +78,15 @@ set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
 
 :end
 @rem End local scope for the variables with windows NT shell
-if "%ERRORLEVEL%"=="0" goto mainEnd
+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!
-if  not "" == "%GRADLE_EXIT_CONSOLE%" exit 1
-exit /b 1
+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

diff --git a/settings.gradle b/settings.gradle
index c3636946357b1afcfae2cffee600f2be8380a058..4d2ffedff86be81b0b39f96171538821a127ea6c 100644 (file)
--- a/settings.gradle
+++ b/settings.gradle
@@ -4,7 +4,7 @@ pluginManagement {
     repositories {
         mavenLocal()
         gradlePluginPortal()
-        String frcYear = '2022'
+        String frcYear = '2025'
         File frcHome
         if (OperatingSystem.current().isWindows()) {
             String publicFolder = System.getenv('PUBLIC')
@@ -25,3 +25,7 @@ pluginManagement {
         }
     }
 }
+
+Properties props = System.getProperties();
+props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true");
+rootProject.name = '2025RobotCode'
\ No newline at end of file

diff --git a/src/main/deploy/example.txt b/src/main/deploy/example.txt
deleted file mode 100644 (file)
index bb82515..0000000
--- a/src/main/deploy/example.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-Files placed in this directory will be deployed to the RoboRIO into the
-'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function
-to get a proper path relative to the deploy directory.
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path
deleted file mode 100644 (file)
index 1926665..0000000
--- a/src/main/deploy/pathplanner/New Path.path
+++ /dev/null
@@ -1,74 +0,0 @@
-{
-  "waypoints": [
-    {
-      "anchorPoint": {
-        "x": 1.0,
-        "y": 3.0
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 2.0,
-        "y": 3.0
-      },
-      "holonomicAngle": 0,
-      "isReversal": false,
-      "velOverride": null,
-      "isLocked": false,
-      "isStopPoint": false,
-      "stopEvent": {
-        "names": [],
-        "executionBehavior": "parallel",
-        "waitBehavior": "none",
-        "waitTime": 0
-      }
-    },
-    {
-      "anchorPoint": {
-        "x": 5.334010668021336,
-        "y": 5.955334987593052
-      },
-      "prevControl": {
-        "x": 5.334010668021336,
-        "y": 4.955334987593052
-      },
-      "nextControl": {
-        "x": 5.334010668021336,
-        "y": 4.955334987593052
-      },
-      "holonomicAngle": 0,
-      "isReversal": true,
-      "velOverride": null,
-      "isLocked": false,
-      "isStopPoint": false,
-      "stopEvent": {
-        "names": [],
-        "executionBehavior": "parallel",
-        "waitBehavior": "none",
-        "waitTime": 0
-      }
-    },
-    {
-      "anchorPoint": {
-        "x": 5.0,
-        "y": 3.0
-      },
-      "prevControl": {
-        "x": 4.0,
-        "y": 3.0
-      },
-      "nextControl": null,
-      "holonomicAngle": 0,
-      "isReversal": false,
-      "velOverride": null,
-      "isLocked": false,
-      "isStopPoint": false,
-      "stopEvent": {
-        "names": [],
-        "executionBehavior": "parallel",
-        "waitBehavior": "none",
-        "waitTime": 0
-      }
-    }
-  ],
-  "markers": []
-}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/autos/Align Right Side.auto b/src/main/deploy/pathplanner/autos/Align Right Side.auto
new file mode 100644 (file)
index 0000000..4e318b1
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Align Right Side.auto
@@ -0,0 +1,88 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "AP1"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Station R P2"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Station R P3"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/autos/Left Lollipop J, L, A.auto b/src/main/deploy/pathplanner/autos/Left Lollipop J, L, A.auto
new file mode 100644 (file)
index 0000000..02ddf6a
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Left Lollipop J, L, A.auto
@@ -0,0 +1,118 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "P  1 - Align After"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive To 11/20 Right"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Official Lollipop 1"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Lollipop 2"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive To 6/19 Right"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Lollipop 3"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Lollipop 4"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive To 6/19 Right"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/autos/Left Side Alt.auto b/src/main/deploy/pathplanner/autos/Left Side Alt.auto
new file mode 100644 (file)
index 0000000..8d405d2
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Left Side Alt.auto
@@ -0,0 +1,158 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "P  1"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive To 11/20 Right"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Gronud  P2"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "parallel",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Drive To 6/19 Left"
+                      }
+                    },
+                    {
+                      "type": "sequential",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "wait",
+                            "data": {
+                              "waitTime": 1.0
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "L4"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "GroundB #6"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "parallel",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Drive To 6/19 Right"
+                      }
+                    },
+                    {
+                      "type": "sequential",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "wait",
+                            "data": {
+                              "waitTime": 1.0
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "L4"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/autos/Left Side Sweep.auto b/src/main/deploy/pathplanner/autos/Left Side Sweep.auto
new file mode 100644 (file)
index 0000000..9054c73
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Left Side Sweep.auto
@@ -0,0 +1,99 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "P  1 - Align After"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive To 11/20 Right"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Sweep  P2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Drive To Left Station"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LSweep P3"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive To 6/19 Right"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "GrondP4"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Drive To Left Station"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/autos/Left Side.auto b/src/main/deploy/pathplanner/autos/Left Side.auto
new file mode 100644 (file)
index 0000000..2cf0435
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Left Side.auto
@@ -0,0 +1,124 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "P  1"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive To 11/20 Right"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Gronud  P2"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "P 5"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive To 6/19 Left"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "GroundB #6"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "P  3"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive To 6/19 Right"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "3d intake peice"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/autos/Right Side.auto b/src/main/deploy/pathplanner/autos/Right Side.auto
new file mode 100644 (file)
index 0000000..206b69e
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Right Side.auto
@@ -0,0 +1,118 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "P  1 Mirrored"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive To 9/22 Left"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Gronud  P2 Mirrored"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "P 5 Mirrored"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive To 8/17 Right"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "GroundB #6 Mirrored"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "P  3 Mirrored"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive To 8/17 Left"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/autos/Station Left Side - Align After.auto b/src/main/deploy/pathplanner/autos/Station Left Side - Align After.auto
new file mode 100644 (file)
index 0000000..2f6ff4b
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Station Left Side - Align After.auto
@@ -0,0 +1,144 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "P  1 - Align After"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive To 11/20 Right"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Station P2"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Station Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "StationB Alt Opt Left - Align After"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive To 6/19 Left"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": " Station P4"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Station Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": " StationB Alt Opt - Align After"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive To 6/19 Right"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/autos/Station Left Side.auto b/src/main/deploy/pathplanner/autos/Station Left Side.auto
new file mode 100644 (file)
index 0000000..65980d3
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Station Left Side.auto
@@ -0,0 +1,144 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "P  1"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive To 11/20 Right"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Station P2"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Station Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "StationP 3"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive To 6/19 Left"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": " Station P4"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Station Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "StationB Alt Opt"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive To 6/19 Right"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/autos/Station Right Side.auto b/src/main/deploy/pathplanner/autos/Station Right Side.auto
new file mode 100644 (file)
index 0000000..bfc5082
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Station Right Side.auto
@@ -0,0 +1,142 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "AP1"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive to 9/22 Left"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Station R P2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Drive to Right Station Intake"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Station Intake"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Station R P3"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive to 8/17 Right"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Station R P4"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Drive to Right Station Intake"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Station Intake"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Station R P5"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Drive to 8/17 Left"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoral"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Lower Elevator"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
new file mode 100644 (file)
index 0000000..23e0db9
--- /dev/null
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/3d intake peice.path b/src/main/deploy/pathplanner/paths/3d intake peice.path
new file mode 100644 (file)
index 0000000..69fa8f5
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/3d intake peice.path
@@ -0,0 +1,80 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.66,
+        "y": 5.12
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.909326741052695,
+        "y": 5.101664891709005
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.722,
+        "y": 7.129
+      },
+      "prevControl": {
+        "x": 1.5210513367419152,
+        "y": 7.294369927869808
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.6049966239027695,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 1.5,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "IntakeCoral",
+      "waypointRelativePos": 0.0,
+      "endWaypointRelativePos": 1.0,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "IntakeCoral"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 3.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -145.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": -150.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/4th peice.path b/src/main/deploy/pathplanner/paths/4th peice.path
new file mode 100644 (file)
index 0000000..90f5af4
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/4th peice.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.9300204918032788,
+        "y": 7.243698770491803
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.930020491803279,
+        "y": 7.243698770491803
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.6202868852459016,
+        "y": 5.589395491803279
+      },
+      "prevControl": {
+        "x": 2.6202868852459016,
+        "y": 5.589395491803279
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -145.66978280449666
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -141.34019174590986
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/AP1.path b/src/main/deploy/pathplanner/paths/AP1.path
new file mode 100644 (file)
index 0000000..a693998
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/AP1.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.18063524590164,
+        "y": 0.48
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.483621180258631,
+        "y": 1.147118232765553
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.81183,
+        "y": 2.768
+      },
+      "prevControl": {
+        "x": 5.174076079601306,
+        "y": 2.4481272403373495
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "MoveElevator",
+      "waypointRelativePos": 0.4514601420678732,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 2.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 29.999999999999996
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Center to G.path b/src/main/deploy/pathplanner/paths/Center to G.path
new file mode 100644 (file)
index 0000000..056673f
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Center to G.path
@@ -0,0 +1,61 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.229,
+        "y": 3.8608
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.903565973322253,
+        "y": 3.8608
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.74,
+        "y": 3.8608
+      },
+      "prevControl": {
+        "x": 6.308787430236012,
+        "y": 3.8608
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "MoveElevator",
+      "waypointRelativePos": 0.7829518547750566,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 0.5,
+    "maxAcceleration": 0.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Center to H.path b/src/main/deploy/pathplanner/paths/Center to H.path
new file mode 100644 (file)
index 0000000..5734ea0
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Center to H.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.229,
+        "y": 4.191
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.903565973322253,
+        "y": 4.191
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.74,
+        "y": 4.191
+      },
+      "prevControl": {
+        "x": 6.308787430236012,
+        "y": 4.191
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.5,
+    "maxAcceleration": 0.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/GrondP4.path b/src/main/deploy/pathplanner/paths/GrondP4.path
new file mode 100644 (file)
index 0000000..2863bb1
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/GrondP4.path
@@ -0,0 +1,82 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.721,
+        "y": 5.077
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.7080822290916022,
+        "y": 4.827333960669942
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.260710227277713,
+        "y": 7.240838068181818
+      },
+      "prevControl": {
+        "x": 3.769261363646335,
+        "y": 7.031434659090908
+      },
+      "nextControl": {
+        "x": 2.5266509333291625,
+        "y": 7.543097777451788
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.8076988636413492,
+        "y": 6.572741477272728
+      },
+      "prevControl": {
+        "x": 2.1270340909140755,
+        "y": 7.342904545454545
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake",
+      "waypointRelativePos": 0.4,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "IntakeCoral"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -53.616
+  },
+  "reversed": false,
+  "folder": "Ground LSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -145.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Gronud  P2 Mirrored.path b/src/main/deploy/pathplanner/paths/Gronud  P2 Mirrored.path
new file mode 100644 (file)
index 0000000..9b881a0
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Gronud  P2 Mirrored.path
@@ -0,0 +1,101 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.0,
+        "y": 2.81
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.967147352430555,
+        "y": 2.1109704137731478
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.6396306818181814,
+        "y": 1.826
+      },
+      "prevControl": {
+        "x": 3.976501550755575,
+        "y": 1.7392387033253078
+      },
+      "nextControl": {
+        "x": 2.8861082175925916,
+        "y": 2.020070167824074
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.722,
+        "y": 0.921
+      },
+      "prevControl": {
+        "x": 2.7759913917824064,
+        "y": 2.1777043547453694
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.25,
+      "rotationDegrees": -39.29328640235812
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.6961512491559774,
+      "maxWaypointRelativePos": 2.0,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 1.5,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake",
+      "waypointRelativePos": 0.55,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "IntakeCoral"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 3.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -36.0
+  },
+  "reversed": false,
+  "folder": "Ground LSide",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 29.999999999999996
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Gronud  P2.path b/src/main/deploy/pathplanner/paths/Gronud  P2.path
new file mode 100644 (file)
index 0000000..4345b4a
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Gronud  P2.path
@@ -0,0 +1,101 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.0,
+        "y": 5.24
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.856164772727273,
+        "y": 5.775014204545455
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.6396306818181814,
+        "y": 6.223735795454545
+      },
+      "prevControl": {
+        "x": 3.9487500000000004,
+        "y": 6.383281249999999
+      },
+      "nextControl": {
+        "x": 3.061864563544763,
+        "y": 5.925533927958591
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.722,
+        "y": 7.129
+      },
+      "prevControl": {
+        "x": 2.562698863636364,
+        "y": 6.044247159090909
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.25,
+      "rotationDegrees": -145.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.6961512491559774,
+      "maxWaypointRelativePos": 2.0,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 1.5,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake",
+      "waypointRelativePos": 0.55,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "IntakeCoral"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -144.0
+  },
+  "reversed": false,
+  "folder": "Ground LSide",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 150.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/GroundB #2.path b/src/main/deploy/pathplanner/paths/GroundB #2.path
new file mode 100644 (file)
index 0000000..c1fc6d2
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/GroundB #2.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.0073368,
+        "y": 5.1909832
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.6414852850066137,
+        "y": 5.481452904381226
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.885,
+        "y": 7.011
+      },
+      "prevControl": {
+        "x": 2.279280354370602,
+        "y": 6.697959071416107
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake",
+      "waypointRelativePos": 0.6463314097279476,
+      "endWaypointRelativePos": 1.0,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "IntakeCoral"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 0.5,
+    "maxAcceleration": 0.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -144.0
+  },
+  "reversed": false,
+  "folder": "Ground BSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -150.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/GroundB #3.path b/src/main/deploy/pathplanner/paths/GroundB #3.path
new file mode 100644 (file)
index 0000000..049b70a
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/GroundB #3.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.885,
+        "y": 7.011
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.1167904466171583,
+        "y": 6.862521975540034
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.7213752,
+        "y": 5.0258832
+      },
+      "prevControl": {
+        "x": 3.3903399058878056,
+        "y": 5.426007692748621
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -150.0
+  },
+  "reversed": false,
+  "folder": "Ground BSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -144.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/GroundB #5.path b/src/main/deploy/pathplanner/paths/GroundB #5.path
new file mode 100644 (file)
index 0000000..b81a9be
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/GroundB #5.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.722,
+        "y": 7.129
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.8649319141897391,
+        "y": 6.267760789902279
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.1273,
+        "y": 4.191
+      },
+      "prevControl": {
+        "x": 2.1273,
+        "y": 4.191
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "MoveElevator",
+      "waypointRelativePos": 0.6806256151574805,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.5,
+    "rotation": -90.89200324814249
+  },
+  "reversed": false,
+  "folder": "Ground BSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -142.86256094570606
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/GroundB #6 Mirrored.path b/src/main/deploy/pathplanner/paths/GroundB #6 Mirrored.path
new file mode 100644 (file)
index 0000000..302abdd
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/GroundB #6 Mirrored.path
@@ -0,0 +1,85 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.98,
+        "y": 2.81
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.661208032083275,
+        "y": 2.7146988994001573
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.722,
+        "y": 0.921
+      },
+      "prevControl": {
+        "x": 1.2995983363740153,
+        "y": 0.5281644353113872
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.3291032261499149,
+      "rotationDegrees": -32.65592295916657
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.5509790681971605,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 1.5,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake",
+      "waypointRelativePos": 0.11073598919649108,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "IntakeCoral"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 3.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -35.0
+  },
+  "reversed": false,
+  "folder": "Ground LSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -29.999999999999996
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/GroundB #6.path b/src/main/deploy/pathplanner/paths/GroundB #6.path
new file mode 100644 (file)
index 0000000..2a7adb3
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/GroundB #6.path
@@ -0,0 +1,85 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.98,
+        "y": 5.24
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.316855850468241,
+        "y": 5.760793783082248
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.722,
+        "y": 7.129
+      },
+      "prevControl": {
+        "x": 2.338563708774429,
+        "y": 6.649638232899023
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.3291032261499149,
+      "rotationDegrees": -136.13455862983048
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.5509790681971605,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 1.5,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake",
+      "waypointRelativePos": 0.11073598919649108,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "IntakeCoral"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -142.62124693823438
+  },
+  "reversed": false,
+  "folder": "Ground LSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -150.82019924107797
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/GrounddB #4.path b/src/main/deploy/pathplanner/paths/GrounddB #4.path
new file mode 100644 (file)
index 0000000..0cc0a83
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/GrounddB #4.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.7213752,
+        "y": 5.0258832
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.2108522727272732,
+        "y": 5.555639204545454
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.722,
+        "y": 7.129
+      },
+      "prevControl": {
+        "x": 2.061030502628888,
+        "y": 6.716198757492258
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake",
+      "waypointRelativePos": 0.3779527559055112,
+      "endWaypointRelativePos": 1.0,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "IntakeCoral"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -144.0
+  },
+  "reversed": false,
+  "folder": "Ground BSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -150.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/LSweep P3.path b/src/main/deploy/pathplanner/paths/LSweep P3.path
new file mode 100644 (file)
index 0000000..94ffe3f
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/LSweep P3.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.788,
+        "y": 6.603
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.9316477272727273,
+        "y": 6.26396875
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.721,
+        "y": 5.025
+      },
+      "prevControl": {
+        "x": 3.5675633667260422,
+        "y": 5.222375782631895
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Elevator",
+      "waypointRelativePos": 0.16647919010123857,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -150.0
+  },
+  "reversed": false,
+  "folder": "Ground LSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -56.976000000000006
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Lollipop 2.path b/src/main/deploy/pathplanner/paths/Lollipop 2.path
new file mode 100644 (file)
index 0000000..d258477
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Lollipop 2.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.239,
+        "y": 5.843
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.189204371946254,
+        "y": 5.904804878359121
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.6657752058351885,
+        "y": 5.122
+      },
+      "prevControl": {
+        "x": 3.1011126071083357,
+        "y": 5.555954762962963
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Elevator",
+      "waypointRelativePos": 0.4830139640748033,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.5,
+    "rotation": -150.68614751980144
+  },
+  "reversed": false,
+  "folder": "Lollipop Path",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -89.56435434461116
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Lollipop 3.path b/src/main/deploy/pathplanner/paths/Lollipop 3.path
new file mode 100644 (file)
index 0000000..f776ef5
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Lollipop 3.path
@@ -0,0 +1,80 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6657752058351885,
+        "y": 5.122185271626888
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.232096205428022,
+        "y": 5.337027431134217
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.224,
+        "y": 4.023
+      },
+      "prevControl": {
+        "x": 2.2404897088951454,
+        "y": 4.642129432338781
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.6121951664719626,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 0.5,
+        "maxAcceleration": 2.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake",
+      "waypointRelativePos": 0.6682840182086613,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "IntakeCoral"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -48.652092538649285
+  },
+  "reversed": false,
+  "folder": "Lollipop Path",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -149.20719589802195
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Lollipop 4.path b/src/main/deploy/pathplanner/paths/Lollipop 4.path
new file mode 100644 (file)
index 0000000..1774ff5
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Lollipop 4.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.224,
+        "y": 4.023
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9751816342630288,
+        "y": 4.332334397903094
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.923609664351851,
+        "y": 4.208198061342593
+      },
+      "prevControl": {
+        "x": 2.412264463822535,
+        "y": 4.184931100328749
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Elevator",
+      "waypointRelativePos": 0.402936988901869,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.5,
+    "rotation": -89.04406269082419
+  },
+  "reversed": false,
+  "folder": "Lollipop Path",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -43.54279799705628
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Lollipop 5.path b/src/main/deploy/pathplanner/paths/Lollipop 5.path
new file mode 100644 (file)
index 0000000..b9155fd
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Lollipop 5.path
@@ -0,0 +1,80 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.239,
+        "y": 4.191
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0626564776043614,
+        "y": 3.9678578908798716
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.197960792824073,
+        "y": 2.2002983940972216
+      },
+      "prevControl": {
+        "x": 0.9319482350521202,
+        "y": 1.9434278342202775
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.6121951664719626,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 0.5,
+        "maxAcceleration": 2.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake",
+      "waypointRelativePos": 0.6682840182086613,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "IntakeCoral"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -48.652
+  },
+  "reversed": false,
+  "folder": "Lollipop Path",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Lollipop 6.path b/src/main/deploy/pathplanner/paths/Lollipop 6.path
new file mode 100644 (file)
index 0000000..98904f0
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Lollipop 6.path
@@ -0,0 +1,80 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.198,
+        "y": 2.2
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.0216564776043615,
+        "y": 1.976857890879872
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.23,
+        "y": 4.191
+      },
+      "prevControl": {
+        "x": 2.9639874422280474,
+        "y": 3.9341294401230558
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.6121951664719626,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 0.5,
+        "maxAcceleration": 2.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake",
+      "waypointRelativePos": 0.6682840182086613,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "IntakeCoral"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": "Lollipop Path",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -36.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Official Lollipop 1.path b/src/main/deploy/pathplanner/paths/Official Lollipop 1.path
new file mode 100644 (file)
index 0000000..81b32a1
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Official Lollipop 1.path
@@ -0,0 +1,80 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.026909205835189,
+        "y": 5.287285271626888
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.9877799051430065,
+        "y": 5.857346791883403
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.239,
+        "y": 5.843
+      },
+      "prevControl": {
+        "x": 1.9968631781709456,
+        "y": 5.892619841156129
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.5537930782710281,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 0.5,
+        "maxAcceleration": 2.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake",
+      "waypointRelativePos": 0.40029835137795283,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "IntakeCoral"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -89.24717948106847
+  },
+  "reversed": false,
+  "folder": "Lollipop Path",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 150.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/P  1 - Align After.path b/src/main/deploy/pathplanner/paths/P  1 - Align After.path
new file mode 100644 (file)
index 0000000..bf6da05
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/P  1 - Align After.path
@@ -0,0 +1,88 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.065,
+        "y": 7.315
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.311723981584821,
+        "y": 6.540661185128348
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.9713092,
+        "y": 5.1909832
+      },
+      "prevControl": {
+        "x": 5.454483001977311,
+        "y": 5.695001290538291
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "backdrive",
+      "waypointRelativePos": 0,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "backdrive"
+        }
+      }
+    },
+    {
+      "name": "lower intake",
+      "waypointRelativePos": 0.14206787687450353,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "lower intake"
+        }
+      }
+    },
+    {
+      "name": "MoveElevator",
+      "waypointRelativePos": 0.4625098658247798,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.5,
+    "rotation": 150.0
+  },
+  "reversed": false,
+  "folder": "Ground LSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 150.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/P  1 Mirrored.path b/src/main/deploy/pathplanner/paths/P  1 Mirrored.path
new file mode 100644 (file)
index 0000000..772e6f8
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/P  1 Mirrored.path
@@ -0,0 +1,88 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.065,
+        "y": 0.735
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.163789350689934,
+        "y": 1.5557182436227601
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.094774590163934,
+        "y": 2.712
+      },
+      "prevControl": {
+        "x": 5.687902753152638,
+        "y": 2.1760455531838283
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "backdrive",
+      "waypointRelativePos": 0,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "backdrive"
+        }
+      }
+    },
+    {
+      "name": "lower intake",
+      "waypointRelativePos": 0.14206787687450353,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "lower intake"
+        }
+      }
+    },
+    {
+      "name": "MoveElevator",
+      "waypointRelativePos": 0.4625098658247798,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 2.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 29.999999999999996
+  },
+  "reversed": false,
+  "folder": "Ground LSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 29.999999999999996
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/P  1.path b/src/main/deploy/pathplanner/paths/P  1.path
new file mode 100644 (file)
index 0000000..9a24b89
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/P  1.path
@@ -0,0 +1,88 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.065,
+        "y": 7.315
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.407825499545448,
+        "y": 6.60471106063858
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.094774590163934,
+        "y": 5.33765368852459
+      },
+      "prevControl": {
+        "x": 5.760558198478665,
+        "y": 6.031726466482178
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "backdrive",
+      "waypointRelativePos": 0,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "backdrive"
+        }
+      }
+    },
+    {
+      "name": "lower intake",
+      "waypointRelativePos": 0.14206787687450353,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "lower intake"
+        }
+      }
+    },
+    {
+      "name": "MoveElevator",
+      "waypointRelativePos": 0.4625098658247798,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 2.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 150.0
+  },
+  "reversed": false,
+  "folder": "Ground LSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 150.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/P  3 Mirrored.path b/src/main/deploy/pathplanner/paths/P  3 Mirrored.path
new file mode 100644 (file)
index 0000000..023d3dc
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/P  3 Mirrored.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.722,
+        "y": 0.921
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9852053519873245,
+        "y": 0.9368665670159784
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.5843237704918027,
+        "y": 2.796
+      },
+      "prevControl": {
+        "x": 3.3462031912644794,
+        "y": 2.7198515282593014
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Elevator",
+      "waypointRelativePos": 0.16647919010123857,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 3.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -29.999999999999996
+  },
+  "reversed": false,
+  "folder": "Ground LSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -35.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/P  3.path b/src/main/deploy/pathplanner/paths/P  3.path
new file mode 100644 (file)
index 0000000..2a95b2f
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/P  3.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.722,
+        "y": 7.1288
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.513128546999364,
+        "y": 6.385951337740788
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.5843237704918027,
+        "y": 5.25373975409836
+      },
+      "prevControl": {
+        "x": 3.1610940855407414,
+        "y": 5.679961652038816
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Elevator",
+      "waypointRelativePos": 0.16647919010123857,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -150.0
+  },
+  "reversed": false,
+  "folder": "Ground LSide",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": -145.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/P 5 Mirrored.path b/src/main/deploy/pathplanner/paths/P 5 Mirrored.path
new file mode 100644 (file)
index 0000000..ac07b82
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/P 5 Mirrored.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.722,
+        "y": 0.921
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.0053161494766774,
+        "y": 1.1670302077595403
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.8720286885245896,
+        "y": 2.7
+      },
+      "prevControl": {
+        "x": 3.5346593444490164,
+        "y": 2.4590517448283564
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "MoveElevator",
+      "waypointRelativePos": 0.39370078740157555,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 3.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -29.999999999999996
+  },
+  "reversed": false,
+  "folder": "Ground LSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -35.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/P 5.path b/src/main/deploy/pathplanner/paths/P 5.path
new file mode 100644 (file)
index 0000000..8631720
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/P 5.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.722,
+        "y": 7.129
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.58836769386574,
+        "y": 6.410185185185185
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.8720286885245896,
+        "y": 5.3496413934426235
+      },
+      "prevControl": {
+        "x": 3.2181991167653305,
+        "y": 5.822776882447252
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "MoveElevator",
+      "waypointRelativePos": 0.39370078740157555,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 3.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -151.83
+  },
+  "reversed": false,
+  "folder": "Ground LSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -145.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Station P2.path b/src/main/deploy/pathplanner/paths/Station P2.path
new file mode 100644 (file)
index 0000000..922df85
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Station P2.path
@@ -0,0 +1,71 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.9713092,
+        "y": 5.1909832
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.461440347544805,
+        "y": 7.189854101642059
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.65,
+        "y": 7.4
+      },
+      "prevControl": {
+        "x": 3.1784221311513603,
+        "y": 5.374715163937142
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.4991119005328578,
+      "rotationDegrees": 61.586
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Station",
+      "waypointRelativePos": 0.6590141796083744,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Station Setpoint"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 3.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 36.0
+  },
+  "reversed": false,
+  "folder": "Station Left",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 150.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Station P4.path b/src/main/deploy/pathplanner/paths/Station P4.path
new file mode 100644 (file)
index 0000000..df31b0a
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Station P4.path
@@ -0,0 +1,71 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.9517367941648103,
+        "y": 5.287285271626888
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.4418679417096154,
+        "y": 7.286156173268947
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.356,
+        "y": 7.331
+      },
+      "prevControl": {
+        "x": 2.8844221311513616,
+        "y": 5.305715163937142
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.8063943161634158,
+      "rotationDegrees": 61.586
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "MoveElevator",
+      "waypointRelativePos": 0.6861642294713134,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Station Setpoint"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 3.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 36.0
+  },
+  "reversed": false,
+  "folder": "Station Left",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -150.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Station R P2.path b/src/main/deploy/pathplanner/paths/Station R P2.path
new file mode 100644 (file)
index 0000000..34fb495
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Station R P2.path
@@ -0,0 +1,71 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.812,
+        "y": 2.768
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.6667827868852463,
+        "y": 1.1836495901639348
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.525653409090909,
+        "y": 0.7293892045454539
+      },
+      "prevControl": {
+        "x": 3.111195051053366,
+        "y": 2.9091389109644723
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.6287744227353493,
+      "rotationDegrees": 131.817128535173
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Station",
+      "waypointRelativePos": 0.6634844868735067,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Station Setpoint"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 2.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 145.0
+  },
+  "reversed": false,
+  "folder": "Station RSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 29.999999999999996
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Station R P3.path b/src/main/deploy/pathplanner/paths/Station R P3.path
new file mode 100644 (file)
index 0000000..ce2a120
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Station R P3.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.526,
+        "y": 0.729
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0456919758517573,
+        "y": 2.2265382848833575
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.007,
+        "y": 2.861
+      },
+      "prevControl": {
+        "x": 3.745218130977533,
+        "y": 2.6169235964968895
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Elevator",
+      "waypointRelativePos": 0.16647919010123857,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 2.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -29.999999999999996
+  },
+  "reversed": false,
+  "folder": "Station RSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 144.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Station R P4.path b/src/main/deploy/pathplanner/paths/Station R P4.path
new file mode 100644 (file)
index 0000000..dca9db2
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Station R P4.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.007,
+        "y": 2.861
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.966570514239714,
+        "y": 1.77435496905575
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.526,
+        "y": 0.729
+      },
+      "prevControl": {
+        "x": 2.4772300707441195,
+        "y": 1.765652915018721
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Station",
+      "waypointRelativePos": 0.6587112171837709,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Station Setpoint"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 2.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 144.0
+  },
+  "reversed": false,
+  "folder": "Station RSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -29.999999999999996
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Station R P5.path b/src/main/deploy/pathplanner/paths/Station R P5.path
new file mode 100644 (file)
index 0000000..1c32f18
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Station R P5.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.526,
+        "y": 0.729
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.9585445192827198,
+        "y": 1.9643408880242017
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.721,
+        "y": 3.026
+      },
+      "prevControl": {
+        "x": 3.1963255727771056,
+        "y": 2.590703696539521
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "MoveElevator",
+      "waypointRelativePos": 0.39370078740157555,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 2.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -29.999999999999996
+  },
+  "reversed": false,
+  "folder": "Station RSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 144.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/StationB #5.path b/src/main/deploy/pathplanner/paths/StationB #5.path
new file mode 100644 (file)
index 0000000..0100ceb
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/StationB #5.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.722,
+        "y": 7.403
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.8649319141897391,
+        "y": 6.541760789902279
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.2385,
+        "y": 4.191
+      },
+      "prevControl": {
+        "x": 2.2385,
+        "y": 4.191
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "MoveElevator",
+      "waypointRelativePos": 0.6806256151574805,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 0.5,
+    "maxAcceleration": 0.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.89200324814249
+  },
+  "reversed": false,
+  "folder": "Station Left",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 36.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/StationB Alt Opt - Align After.path b/src/main/deploy/pathplanner/paths/StationB Alt Opt - Align After.path
new file mode 100644 (file)
index 0000000..da5d433
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/StationB Alt Opt - Align After.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.356,
+        "y": 7.331
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.334068287037036,
+        "y": 6.521408420138889
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.666,
+        "y": 5.122185271626888
+      },
+      "prevControl": {
+        "x": 2.836426070601852,
+        "y": 5.99030882486763
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "MoveElevator",
+      "waypointRelativePos": 0.6806256151574805,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -149.28470582855732
+  },
+  "reversed": false,
+  "folder": "Station Left",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 36.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/StationB Alt Opt Left - Align After.path b/src/main/deploy/pathplanner/paths/StationB Alt Opt Left - Align After.path
new file mode 100644 (file)
index 0000000..d148283
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/StationB Alt Opt Left - Align After.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.356,
+        "y": 7.331
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.320208548409597,
+        "y": 6.4855863560267855
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.9517367941648103,
+        "y": 5.287285271626888
+      },
+      "prevControl": {
+        "x": 3.2517059326171873,
+        "y": 5.978196498325892
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "MoveElevator",
+      "waypointRelativePos": 0.6806256151574805,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -149.28470582855732
+  },
+  "reversed": false,
+  "folder": "Station Left",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 36.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/StationB Alt Opt.path b/src/main/deploy/pathplanner/paths/StationB Alt Opt.path
new file mode 100644 (file)
index 0000000..c01326c
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/StationB Alt Opt.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.65,
+        "y": 7.4
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.334068287037036,
+        "y": 6.521408420138889
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.5380135672433033,
+        "y": 5.395889718191963
+      },
+      "prevControl": {
+        "x": 2.708439637845155,
+        "y": 6.264013271432705
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "MoveElevator",
+      "waypointRelativePos": 0.6806256151574805,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -149.28470582855732
+  },
+  "reversed": false,
+  "folder": "Station Left",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 36.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/StationP 3.path b/src/main/deploy/pathplanner/paths/StationP 3.path
new file mode 100644 (file)
index 0000000..3b53624
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/StationP 3.path
@@ -0,0 +1,71 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.65,
+        "y": 7.4
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.9294370334836404,
+        "y": 6.130971138637706
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.8229949951171873,
+        "y": 5.580079432896205
+      },
+      "prevControl": {
+        "x": 3.6027199899253537,
+        "y": 5.6983119245714535
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.3836589698046169,
+      "rotationDegrees": 150.75582343932
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "MoveElevator",
+      "waypointRelativePos": 0.45374746792707615,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -151.83
+  },
+  "reversed": false,
+  "folder": "Station Left",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 36.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Sweep  P2.path b/src/main/deploy/pathplanner/paths/Sweep  P2.path
new file mode 100644 (file)
index 0000000..996f28a
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Sweep  P2.path
@@ -0,0 +1,71 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.9713092,
+        "y": 5.191
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.267840909084701,
+        "y": 7.908934659084283
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.7777840909028838,
+        "y": 6.602656249993373
+      },
+      "prevControl": {
+        "x": 2.183778409084702,
+        "y": 7.44026988635701
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5275310834813515,
+      "rotationDegrees": -77.19714447023723
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake",
+      "waypointRelativePos": 0.3,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "IntakeCoral"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -56.976000000000006
+  },
+  "reversed": false,
+  "folder": "Ground LSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 150.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Sweep P2.path b/src/main/deploy/pathplanner/paths/Sweep P2.path
new file mode 100644 (file)
index 0000000..1d79a3e
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Sweep P2.path
@@ -0,0 +1,87 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.157,
+        "y": 2.996
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.011782786885246,
+        "y": 1.411649590163935
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.831931818176833,
+        "y": 0.5299573863636362
+      },
+      "prevControl": {
+        "x": 3.285943759901909,
+        "y": 0.6018426104701067
+      },
+      "nextControl": {
+        "x": 1.6353409090859239,
+        "y": 0.34049715909090883
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.688,
+        "y": 1.368
+      },
+      "prevControl": {
+        "x": 1.3261818181768328,
+        "y": 0.709875
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.2984014209591429,
+      "rotationDegrees": -36.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake",
+      "waypointRelativePos": 0.55,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "IntakeCoral"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -127.476
+  },
+  "reversed": false,
+  "folder": "Sweep Right Side",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 29.999999999999996
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Sweep P3.path b/src/main/deploy/pathplanner/paths/Sweep P3.path
new file mode 100644 (file)
index 0000000..f55b8e1
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Sweep P3.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.688,
+        "y": 1.368
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.2076919758517572,
+        "y": 2.8655382848833577
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.0114204545454544,
+        "y": 2.1852414772727276
+      },
+      "prevControl": {
+        "x": 2.749638585522988,
+        "y": 1.9411650737696169
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Elevator",
+      "waypointRelativePos": 0.16647919010123857,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -29.999999999999996
+  },
+  "reversed": false,
+  "folder": "Sweep Right Side",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -127.476
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Sweep P4.path b/src/main/deploy/pathplanner/paths/Sweep P4.path
new file mode 100644 (file)
index 0000000..4dd6a62
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Sweep P4.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.721,
+        "y": 3.0259168
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.6805705142397143,
+        "y": 1.93927176905575
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.722,
+        "y": 0.923
+      },
+      "prevControl": {
+        "x": 2.6732300707441192,
+        "y": 1.959652915018721
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake",
+      "waypointRelativePos": 0.19856459330143417,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "IntakeCoral"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -36.0
+  },
+  "reversed": false,
+  "folder": "Sweep Right Side",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -29.999999999999996
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/Sweep RP5.path b/src/main/deploy/pathplanner/paths/Sweep RP5.path
new file mode 100644 (file)
index 0000000..f528baf
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Sweep RP5.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.722,
+        "y": 0.923
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.1545445192827195,
+        "y": 2.1583408880242017
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.00733,
+        "y": 2.86082
+      },
+      "prevControl": {
+        "x": 3.482655572777105,
+        "y": 2.4255236965395213
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "MoveElevator",
+      "waypointRelativePos": 0.39370078740157555,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -29.999999999999996
+  },
+  "reversed": false,
+  "folder": "Sweep Right Side",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -36.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/paths/SweepGroundB #5.path b/src/main/deploy/pathplanner/paths/SweepGroundB #5.path
new file mode 100644 (file)
index 0000000..a6493ce
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/SweepGroundB #5.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.808,
+        "y": 6.57
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.599744318181818,
+        "y": 6.393252840909091
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.2385,
+        "y": 4.191
+      },
+      "prevControl": {
+        "x": 2.2385,
+        "y": 4.191
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "MoveElevator",
+      "waypointRelativePos": 0.6806256151574805,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 2.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.89200324814249
+  },
+  "reversed": false,
+  "folder": "Ground BSide",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -53.616
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file

diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
new file mode 100644 (file)
index 0000000..c606f42
--- /dev/null
+++ b/src/main/deploy/pathplanner/settings.json
@@ -0,0 +1,41 @@
+{
+  "robotWidth": 0.832,
+  "robotLength": 0.832,
+  "holonomicMode": true,
+  "pathFolders": [
+    "Ground BSide",
+    "Ground LSide",
+    "Station RSide",
+    "Lollipop Path",
+    "Station Left",
+    "Sweep Right Side"
+  ],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 58.9670081,
+  "robotMOI": 7.504,
+  "robotTrackwidth": 0.546,
+  "driveWheelRadius": 0.05,
+  "driveGearing": 5.903,
+  "maxDriveSpeed": 5.41,
+  "driveMotorType": "krakenX60",
+  "driveCurrentLimit": 52.0,
+  "wheelCOF": 0.9,
+  "flModuleX": 0.2635,
+  "flModuleY": 0.2635,
+  "frModuleX": 0.2635,
+  "frModuleY": -0.2635,
+  "blModuleX": -0.2635,
+  "blModuleY": 0.2635,
+  "brModuleX": -0.2635,
+  "brModuleY": -0.2635,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": [
+    "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.0,\"y\":-0.55},\"size\":{\"width\":0.241,\"length\":0.5969},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}"
+  ]
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java
index 8776e5dda74b59755899a1903dc2099ec5fc7da3..e9c01924a131a585bbb1116a40f1f1b8843c5f55 100644 (file)
--- a/src/main/java/frc/robot/Main.java
+++ b/src/main/java/frc/robot/Main.java
@@ -12,14 +12,15 @@ import edu.wpi.first.wpilibj.RobotBase;
  * call.
  */
 public final class Main {
-  private Main() {}
+    private Main() {
+    }
 
-  /**
-   * Main initialization function. Do not perform any initialization here.
-   *
-   * <p>If you change your main robot class, change the parameter type.
-   */
-  public static void main(String... args) {
-    RobotBase.startRobot(Robot::new);
-  }
+    /**
+     * Main initialization function. Do not perform any initialization here.
+     *
+     * <p>If you change your main robot class, change the parameter type.
+     */
+    public static void main(String... args) {
+        RobotBase.startRobot(Robot::new);
+    }
 }

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 477e6914b17b0a62bc034e0eef5fd19517fde21b..a7d87ff4a85a71c0147f696fa8ede2857a9c84a2 100644 (file)
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,15 +4,26 @@
 
 package frc.robot;
 
+import java.util.Optional;
+
+import org.littletonrobotics.junction.LogFileUtil;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGReader;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+
+import au.grapplerobotics.CanBridge;
+import edu.wpi.first.net.PortForwarder;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import frc.robot.controls.Driver;
-import frc.robot.controls.Operator;
-import frc.robot.subsystems.Drivetrain;
-import frc.robot.util.PathGroupLoader;
-import frc.robot.util.ShuffleboardManager;
+import frc.robot.constants.Constants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.util.BuildData;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -20,119 +31,191 @@ import frc.robot.util.ShuffleboardManager;
  * the package after creating this project, you must also update the build.gradle file in the
  * project.
  */
-public class Robot extends TimedRobot {
-  private Command m_autoCommand;
-  public static ShuffleboardManager shuffleboard;
-  public static Drivetrain drive;
-
-  private static boolean isTestMode = false;
-
-  /**
-   * This function is run when the robot is first started up and should be used for any
-   * initialization code.
-   */
-  @Override
-  public void robotInit() {
-
-    // This is really annoying so it's disabled
-    DriverStation.silenceJoystickConnectionWarning(true);
-
-    // load paths before auto starts
-    PathGroupLoader.loadPathGroups();
-
-    // make subsystems
-    shuffleboard = new ShuffleboardManager();
-    drive = new Drivetrain();
-
-    shuffleboard.setup();
-
-    Driver.configureControls();
-    Operator.configureControls();
-  }
-
-  /**
-   * This function is called every robot packet, no matter the mode. Use this for items like
-   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
-   *
-   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
-   * SmartDashboard integrated updating.
-   */
-  @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.
-    CommandScheduler.getInstance().run();
-  }
-
-  /** This function is called once each time the robot enters Disabled mode. */
-  @Override
-  public void disabledInit() {
-    CommandScheduler.getInstance().cancelAll();
-    isTestMode = false;
-  }
-
-  @Override
-  public void disabledPeriodic() {
-    m_autoCommand = getAutonomousCommand();
-  }
-
-  /** This autonomous runs the autonomous command selected by your {@link Robot} class. */
-  @Override
-  public void autonomousInit() {
-    isTestMode = false;
-    if (m_autoCommand != null) {
-      m_autoCommand.schedule();
+public class Robot extends LoggedRobot {
+    private Command autoCommand;
+    private RobotContainer robotContainer;
+
+    public Robot(){
+        CanBridge.runTCP();
+        PortForwarder.add(5800,"10.9.72.12",5800);
+        PortForwarder.add(1182,"10.9.72.12",1182);
+
+        // Set up data receivers & replay source
+        switch (Constants.CURRENT_MODE) {
+            case REAL:
+            // Running on a real robot, log to a USB stick ("/U/logs")
+            Logger.addDataReceiver(new WPILOGWriter());
+            Logger.addDataReceiver(new NT4Publisher());
+            break;
+    
+            case SIM:
+            // Running a physics simulator, log to NT
+            Logger.addDataReceiver(new NT4Publisher());
+            break;
+    
+            case REPLAY:
+            // Replaying a log, set up replay source
+            setUseTiming(false); // Run as fast as possible
+            String logPath = LogFileUtil.findReplayLog();
+            Logger.setReplaySource(new WPILOGReader(logPath));
+            Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
+            break;
+        }
+        Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may be added.
+    }
+    
+    /**
+     * This function is run when the robot is first started up and should be used for any
+     * initialization code.
+     */
+    @Override
+    public void robotInit() {
+        // To Set the Robot Identity
+        //   SimGUI: Persistent Values, Preferences, RobotId, then restart Simulation
+        //     changes networktables.json, networktables.json.bck (both Untracked)
+        //   Uncomment the next line, set the desired RobotId, deploy, and then comment the line out
+        //  RobotId.setRobotId(RobotId.SwerveCompetition);
+        DriveConstants.update(RobotId.getRobotId());
+        RobotController.setBrownoutVoltage(6.0);
+        // obtain this robot's identity
+        RobotId robotId = RobotId.getRobotId();
+
+          // Record metadata
+        Logger.recordMetadata("ProjectName", BuildData.MAVEN_NAME);
+        Logger.recordMetadata("BuildDate", BuildData.BUILD_DATE);
+        Logger.recordMetadata("GitSHA", BuildData.GIT_SHA);
+        Logger.recordMetadata("GitDate", BuildData.GIT_DATE);
+        Logger.recordMetadata("GitBranch", BuildData.GIT_BRANCH);
+        switch (BuildData.DIRTY) {
+        case 0:
+            Logger.recordMetadata("GitDirty", "All changes committed");
+            break;
+        case 1:
+            Logger.recordMetadata("GitDirty", "Uncomitted changes");
+            break;
+        default:
+            Logger.recordMetadata("GitDirty", "Unknown");
+            break;
+        }
+
+        robotContainer = new RobotContainer(robotId);
+        
+    }
+
+    /**
+     * This function is called every robot packet, no matter the mode. Use this for items like
+     * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+     *
+     * <p>This runs after the mode-specific periodic functions, but before
+     * LiveWindow and SmartDashboard integrated updating.
+     */
+    @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.
+
+        CommandScheduler.getInstance().run();
+
+        robotContainer.logComponents();
+    }
+
+    /**
+     * This function is called once each time the robot enters Disabled mode.
+     */
+    @Override
+    public void disabledInit() {
+        CommandScheduler.getInstance().cancelAll();
+    }
+
+    /**
+     * This function is called periodically when the robot is disabled
+     */
+    @Override
+    public void disabledPeriodic() {
     }
-  }
-
-  /** This function is called periodically during autonomous. */
-  @Override
-  public void autonomousPeriodic() {}
-
-  @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 (m_autoCommand != null) {
-      m_autoCommand.cancel();
+
+    /**
+     * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+     */
+    @Override
+    public void autonomousInit() {
+        // Disable vision if the constant is false.
+        robotContainer.setVisionEnabled(VisionConstants.ENABLED_AUTO);
+
+        // Get the autonomous command.
+        // This access is fast (about 14 microseconds) because the value is already resident in the Network Tables.
+        // There was a problem last year because the operation also installed about over a dozen items (taking more than 20 ms).
+        autoCommand = robotContainer.getAutoCommand();
+
+        // If there is an autonomous command, then schedule it
+        if (autoCommand != null) {
+            autoCommand.schedule();
+        }
     }
-    isTestMode = false;
-  }
 
-  /** This function is called periodically during operator control. */
-  @Override
-  public void teleopPeriodic() {}
+    /**
+     * This function is called periodically during autonomous.
+     */
+    @Override
+    public void autonomousPeriodic() {
+    }
 
-  @Override
-  public void testInit() {
-    // Cancels all running commands at the start of test mode.
-    CommandScheduler.getInstance().cancelAll();
 
-    // it may be needed to disable LiveWindow (we don't use it anyway)
-    //LiveWindow.setEnabled(false)
+    /**
+     * This function is called once each time the robot enters Teleop mode.
+     */
+    @Override
+    public void teleopInit() {
+        robotContainer.setVisionEnabled(true);
+
+        // 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 (autoCommand != null) {
+            autoCommand.cancel();
+        }
+    }
 
-    isTestMode = true;
+    /**
+     * This function is called periodically during operator control.
+     */
+    @Override
+    public void teleopPeriodic() {
+    }
 
-  }
+    /**
+     * This function is called once each time the robot enters Test mode.
+     */
+    @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 periodically during test mode.
+     */
+    @Override
+    public void testPeriodic() {
+    }
 
-  /**
-   * Use this to pass the autonomous command to the main {@link Robot} class.
-   *
-   * @return the command to run in autonomous
-   */
-  public Command getAutonomousCommand() {
-    return shuffleboard.getAutonomousCommand();
-  }
+    @Override
+    public void simulationPeriodic() {
+    }
 
-  public static boolean isTestMode() {
-    return isTestMode;
-  }
+       /**
+       * Gets the set Alliance; defaults to red if not set.
+       * This method replaces {@link edu.first.wpilibj.DriverStation.getAlliance}.
+       * The .get() is not necessary, so DriverStation.getAlliance().get() becomes Robot.getAlliance()
+       */
+       public static Alliance getAlliance() {
+               Optional<Alliance> dsAlliance = DriverStation.getAlliance();
+               if (dsAlliance.isPresent())
+                       return dsAlliance.get();
+               else
+                       return Alliance.Red; // default to Red alliance
+       }
 }

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
new file mode 100644 (file)
index 0000000..06b0fd4
--- /dev/null
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -0,0 +1,435 @@
+package frc.robot;
+
+import java.io.IOException;
+import java.util.function.BooleanSupplier;
+
+import org.json.simple.parser.ParseException;
+import org.littletonrobotics.junction.Logger;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+import com.pathplanner.lib.commands.PathPlannerAuto;
+
+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.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.DoNothing;
+import frc.robot.commands.drive_comm.DefaultDriveCommand;
+import frc.robot.commands.drive_comm.DriveToPose;
+import frc.robot.commands.gpm.IntakeCoral;
+import frc.robot.commands.gpm.MoveArm;
+import frc.robot.commands.gpm.MoveElevator;
+import frc.robot.commands.gpm.OuttakeCoral;
+import frc.robot.commands.gpm.StationIntake;
+import frc.robot.constants.ArmConstants;
+import frc.robot.constants.AutoConstants;
+import frc.robot.constants.Constants;
+import frc.robot.constants.ElevatorConstants;
+import frc.robot.constants.FieldConstants;
+import frc.robot.constants.IntakeConstants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.controls.BaseDriverConfig;
+import frc.robot.controls.Operator;
+import frc.robot.controls.PS5ControllerDriverConfig;
+import frc.robot.subsystems.arm.Arm;
+import frc.robot.subsystems.climb.Climb;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.drivetrain.GyroIOPigeon2;
+import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.subsystems.indexer.Indexer;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.outtake.Outtake;
+import frc.robot.subsystems.outtake.OuttakeAlpha;
+import frc.robot.subsystems.outtake.OuttakeComp;
+import frc.robot.util.PathGroupLoader;
+import frc.robot.util.Vision.DetectedObject;
+import frc.robot.util.Vision.Vision;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in
+ * the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of
+ * the robot (including
+ * subsystems, commands, and trigger mappings) should be declared here.
+ */
+public class RobotContainer {
+  // The robot's subsystems are defined here...
+  private Drivetrain drive = null;
+  private Vision vision = null;
+  private Intake intake = null;
+  private Indexer indexer = null;
+  private Outtake outtake = null;
+  private Elevator elevator = null;
+  private Climb climb = null;
+  private Arm arm = null;
+  private Command auto = new DoNothing();
+
+  // Dashboard inputs
+  // private final LoggedDashboardChooser<Command> autoChooser;
+
+  // Controllers are defined here
+  private BaseDriverConfig driver = null;
+  @SuppressWarnings("unused")
+  private Operator operator = null;
+
+  /**
+   * The container for the robot. Contains subsystems, OI devices, and commands.
+   * <p>
+   * Different robots may have different subsystems.
+   */
+  public RobotContainer(RobotId robotId) {
+    // dispatch on the robot
+    switch (robotId) {
+      case TestBed1:
+        break;
+
+      case TestBed2:
+        break;
+
+      default:
+      case SwerveCompetition:
+        outtake = new OuttakeComp();
+        elevator = new Elevator();
+        climb = new Climb();
+        arm = new Arm();
+        // Arm can move if the elevator is within tolerance of its safe setpoint or higher
+        arm.setElevatorStowed(() -> elevator.getPosition() < ElevatorConstants.SAFE_SETPOINT - 0.025);
+        // Elevator can only move down if the arm is in the intake setpoint
+        elevator.setArmStowed(() -> arm.canMoveElevator());
+
+      case BetaBot:
+        indexer = new Indexer();
+        intake = new Intake();
+        //SmartDashboard.putData("commadn schedule", CommandScheduler.getInstance());
+        vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS);
+        // fall-through
+        //  SmartDashboard.putData("RunIntakeAndIndexer", new RunIntakeAndIndexer(intake, indexer));
+
+      case Vivace:
+      case Phil:
+        if (robotId == RobotId.Phil) {
+          outtake = new OuttakeAlpha();
+        }
+        if (outtake != null) {
+          //SmartDashboard.putData("OuttakeCoralBasic", new OutakeMotors(intake, outtake));
+          // SmartDashboard.putData("OuttakeCoralBasic", new OutakeMotors(intake, outtake));
+          // SmartDashboard.putData("l4 outake", new ScoreL4(elevator, outtake));
+        }
+      case Vertigo:
+        drive = new Drivetrain(vision, new GyroIOPigeon2());
+        driver = new PS5ControllerDriverConfig(drive, elevator, intake, indexer, outtake, climb, arm);
+        //operator = new Operator(drive, elevator, intake, indexer, outtake, climb);
+
+        // Detected objects need access to the drivetrain
+        DetectedObject.setDrive(drive);
+        
+        //SignalLogger.start();
+        
+
+        driver.configureControls();
+        //operator.configureControls();
+        
+        initializeAutoBuilder();
+        registerCommands();
+        PathGroupLoader.loadPathGroups();
+        // Load the auto command
+        try {
+          PathPlannerAuto.getPathGroupFromAutoFile("Left Side");
+          auto = new PathPlannerAuto("Left Side");
+        } catch (IOException | ParseException e) {
+            e.printStackTrace();
+        }
+        drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
+        break;
+      }
+
+    // This is really annoying so it's disabled
+    DriverStation.silenceJoystickConnectionWarning(true);
+
+    //addPaths(); 
+    // TODO: verify this claim.
+    // LiveWindow is causing periodic loop overruns
+    LiveWindow.disableAllTelemetry();
+    LiveWindow.setEnabled(false);
+    
+  }
+
+  /**
+   * Sets whether the drivetrain uses vision toupdate odometry
+   */
+  public void setVisionEnabled(boolean enabled) {
+    if (drive != null)
+      drive.setVisionEnabled(enabled);
+  }
+
+
+  public void initializeAutoBuilder() {
+    AutoBuilder.configure(
+        () -> drive.getPose(),
+        (pose) -> {
+          drive.resetOdometry(pose);
+        },
+        () -> drive.getChassisSpeeds(),
+        (chassisSpeeds) -> {
+          Logger.recordOutput("Auto/ChassisSpeeds", chassisSpeeds);
+          drive.setChassisSpeeds(chassisSpeeds, false); // problem??
+        },
+        AutoConstants.AUTO_CONTROLLER,
+        AutoConstants.CONFIG,
+        getAllianceColorBooleanSupplier(),
+        drive);
+  }
+
+  public void registerCommands() {
+    if(intake != null && indexer != null && elevator != null && arm != null){
+      NamedCommands.registerCommand("IntakeCoral", new IntakeCoral(intake, indexer, elevator, outtake, arm));
+      NamedCommands.registerCommand("lower intake", new InstantCommand(() -> intake.setAngle(IntakeConstants.INTAKE_SAFE_POINT)));
+    }
+    if(elevator != null && outtake != null && arm != null){
+      NamedCommands.registerCommand("OuttakeCoral", new OuttakeCoral(outtake, elevator, arm).withTimeout(1.5));
+      NamedCommands.registerCommand("L4", 
+        new ParallelCommandGroup(
+          new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT),
+          new MoveArm(arm, ArmConstants.L4_SETPOINT_RIGHT)
+        )
+      );
+      NamedCommands.registerCommand("backdrive", new InstantCommand(() -> outtake.setMotor(0.02)));
+
+      NamedCommands.registerCommand("Lower Elevator", new SequentialCommandGroup(
+        new InstantCommand(()->arm.setSetpoint(ArmConstants.INTAKE_SETPOINT)),
+        new InstantCommand(()->elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT))
+      ));
+      
+      NamedCommands.registerCommand("Score L4", new SequentialCommandGroup(
+        new ParallelCommandGroup(
+          new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT),
+          new MoveArm(arm, ArmConstants.L4_SETPOINT_RIGHT)
+        ),
+        new OuttakeCoral(outtake, elevator, arm)
+      ));
+
+      NamedCommands.registerCommand("Score L3", new SequentialCommandGroup(
+        new ParallelCommandGroup(
+          new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT),
+          new MoveArm(arm, ArmConstants.L2_L3_SETPOINT)
+        ),
+        new OuttakeCoral(outtake, elevator, arm)
+      ));
+
+      NamedCommands.registerCommand("Score L2", new SequentialCommandGroup(
+        new ParallelCommandGroup(
+          new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT),
+          new MoveArm(arm, ArmConstants.L2_L3_SETPOINT)
+        ),
+        new OuttakeCoral(outtake, elevator, arm)
+      ));
+      
+      NamedCommands.registerCommand("L3", 
+        new ParallelCommandGroup(
+          new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT),
+          new MoveArm(arm, ArmConstants.L2_L3_SETPOINT)
+        )
+      );
+      NamedCommands.registerCommand("L2", 
+        new ParallelCommandGroup(
+          new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT),
+          new MoveArm(arm, ArmConstants.L2_L3_SETPOINT)
+        )
+      );
+      NamedCommands.registerCommand("Station Setpoint", 
+        new ParallelCommandGroup(
+          new MoveElevator(elevator, ElevatorConstants.STATION_INTAKE_SETPOINT),
+          new MoveArm(arm, ArmConstants.STATION_INTAKE_SETPOINT),
+          new StationIntake(outtake)
+        )
+      );
+
+      //NamedCommands.registerCommand("L1", new MoveElevator(elevator, ElevatorConstants.L1_SETPOINT));
+
+      NamedCommands.registerCommand("Station Intake", new StationIntake(outtake));
+    
+      Pose2d blueStationRight = new Pose2d(1.722, 0.923, Rotation2d.fromDegrees(-36));
+      Pose2d blueStationLeft = new Pose2d(blueStationRight.getX(), FieldConstants.FIELD_WIDTH-blueStationRight.getY(), Rotation2d.fromDegrees(-144));
+
+      Pose2d blueStationIntakeLeft = new Pose2d(1.65, 7.4, Rotation2d.fromDegrees(-144-180));
+      Pose2d blueStationIntakeRight = new Pose2d(1.526, 0.729, Rotation2d.fromDegrees(-144-180));
+      
+      Pose2d redStationRight = new Pose2d(FieldConstants.FIELD_LENGTH-blueStationRight.getX(), blueStationLeft.getY(), blueStationRight.getRotation().plus(new Rotation2d(Math.PI)));
+      Pose2d redStationLeft = new Pose2d(FieldConstants.FIELD_LENGTH-blueStationLeft.getX(), blueStationRight.getY(), blueStationLeft.getRotation().plus(new Rotation2d(Math.PI)));
+      NamedCommands.registerCommand("Drive To Left Station", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? redStationLeft : blueStationLeft));
+      NamedCommands.registerCommand("Drive To Right Station Intake", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? redStationRight : blueStationIntakeRight));
+      NamedCommands.registerCommand("Drive To Left Station Intake", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? redStationLeft : blueStationIntakeLeft));
+      
+      NamedCommands.registerCommand("Drive To Right Station", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? redStationRight : blueStationRight));
+      NamedCommands.registerCommand("Drive To 6/19 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_6_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_19_LEFT.l4Pose).withTimeout(1));
+      NamedCommands.registerCommand("Drive To 6/19 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_6_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_19_RIGHT.l4Pose).withTimeout(1));
+      NamedCommands.registerCommand("Drive To 7/18 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_7_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_18_LEFT.l4Pose));
+      NamedCommands.registerCommand("Drive To 7/18 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_7_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_18_RIGHT.l4Pose));
+      NamedCommands.registerCommand("Drive To 10/21 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_9_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_22_RIGHT.l4Pose));
+      NamedCommands.registerCommand("Drive To 11/20 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_11_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_20_LEFT.l4Pose));
+      NamedCommands.registerCommand("Drive To 11/20 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_11_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_20_RIGHT.l4Pose));
+      NamedCommands.registerCommand("Drive To 9/22 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get() 
+      == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_9_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_22_LEFT.l4Pose)); 
+      
+
+      NamedCommands.registerCommand("Drive To 8/17 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_8_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_17_LEFT.l4Pose).withTimeout(1));
+      NamedCommands.registerCommand("Drive To 8/17 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_8_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_17_RIGHT.l4Pose).withTimeout(1));
+    }
+  }
+
+  // public void addPaths(){
+  //       try {
+  //           PathPlannerAuto.getPathGroupFromAutoFile("Left Side");
+  //       } 
+  //       catch (IOException | ParseException e) {
+  //           e.printStackTrace();
+  //       }
+  //       autoChooser.addDefaultOption("Left Side", new PathPlannerAuto("Left Side"));
+
+  //       autoChooser.addOption("Station Right Side", new PathPlannerAuto("Station Right Side"));
+  //       autoChooser.addOption("Left Side Lollipop", new PathPlannerAuto("Left Side Lollipop"));
+  //       autoChooser.addOption("Left Side Ground", new PathPlannerAuto("Left Side Ground"));
+
+  //       if(intake != null && indexer != null && arm != null && elevator != null){
+  //         autoChooser.addOption("One peice blue", 
+  //         new SequentialCommandGroup(
+  //           new InstantCommand(()->{
+  //             drive.resetOdometry(new Pose2d(7.229,4.191, Rotation2d.fromDegrees(90.0)));
+  //             intake.setAngle(IntakeConstants.INTAKE_SAFE_POINT);
+  //           }),
+  //           new DriveToPose(drive, () -> VisionConstants.REEF.BLUE_BRANCH_21_RIGHT.pose).withTimeout(8),
+  //           new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT),
+  //           new MoveArm(arm, ArmConstants.L4_SETPOINT),
+  //           new OuttakeCoral(outtake, elevator, arm),
+  //           new SequentialCommandGroup(new WaitCommand(0.1),
+  //           new MoveArm(arm, ArmConstants.INTAKE_SETPOINT),
+  //           new InstantCommand(()->elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT))),
+  //           new InstantCommand(()-> intake.stow())
+  //           ));
+  //           autoChooser.addOption("One peice red", 
+  //           new SequentialCommandGroup(
+  //             new InstantCommand(()->{
+  //               drive.resetOdometry(new Pose2d(FieldConstants.FIELD_LENGTH-7.229,FieldConstants.FIELD_WIDTH-4.191, Rotation2d.fromDegrees(-90.0)));
+  //               intake.setAngle(IntakeConstants.INTAKE_SAFE_POINT);
+  //             }),
+  //             new DriveToPose(drive, () -> VisionConstants.REEF.RED_BRANCH_10_RIGHT.pose).withTimeout(8),
+  //             new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT),
+  //             new MoveArm(arm, ArmConstants.L4_SETPOINT),
+  //             new OuttakeCoral(outtake, elevator, arm),
+  //             new SequentialCommandGroup(new WaitCommand(0.1),
+  //             new MoveArm(arm, ArmConstants.INTAKE_SETPOINT),
+  //             new InstantCommand(()->elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT))),
+  //             new InstantCommand(()-> intake.stow())
+  //             ));
+  //           }
+  //         // autoChooser.addOption("#1", new FollowPathCommand("#1", true, drive)
+  //       // .andThen(new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT))
+  //       // .andThen(new OuttakeCoral(outtake, elevator, arm))
+  //       // .andThen(new FollowPathCommand("#2", true, drive))
+  //       // .andThen(new FollowPathCommand("#3", true, drive))
+  //       // .andThen(new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT))
+  //       // .andThen(new OuttakeCoral(outtake, elevator, arm))
+  //       // .andThen(new FollowPathCommand("#4", true, drive))
+  //       // .andThen(new FollowPathCommand("#5", true, drive))
+  //       // .andThen(new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT))
+  //       // .andThen(new OuttakeCoral(outtake, elevator, arm)));    
+
+        
+  //       if(elevator != null && outtake != null) {
+  //        autoChooser.addOption("WaitTest", new FollowPathCommand("Tester", true, drive)
+  //        .andThen(new OuttakeCoralBasic(outtake, ()->true, ()->false))
+  //        .andThen(new WaitCommand(3))
+  //        .andThen(new FollowPathCommand("Next Tester", true, drive))
+  //        );
+
+  //         autoChooser.addOption("Center to G", new FollowPathCommand("Center to G", true, drive)
+  //        .andThen(new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT))
+  //        .andThen(new OuttakeCoral(outtake, elevator, arm)));
+
+  //        autoChooser.addOption("Center to H", new FollowPathCommand("Center to H", true, drive)
+  //        .andThen(new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT))
+  //        .andThen(new OuttakeCoral(outtake, elevator, arm)));
+  //       }
+  // }
+
+  public static BooleanSupplier getAllianceColorBooleanSupplier() {
+    return () -> {
+      // Boolean supplier that controls when the path will be mirrored for the red
+      // alliance
+      // This will flip the path being followed to the red side of the field.
+      // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+
+      var alliance = DriverStation.getAlliance();
+      if (alliance.isPresent()) {
+        return alliance.get() == DriverStation.Alliance.Red;
+      }
+      return false;
+    };
+  }
+
+  public boolean brownout() {
+    if(RobotController.getBatteryVoltage() < 6.0) {
+      return true;
+    }
+    else {
+      return false;
+    }
+  }
+
+  public Command getAutoCommand(){
+    return auto;
+  }
+
+  // Logged psoitins of subsystems
+  public double elevatorHeightLogged(){
+    return elevator == null ? 0 : elevator.getPosition();
+  }
+  public double armAngleLogged(){
+    return arm == null ? 0 : arm.getAngle() + 90;
+  }
+  public double intakeAngleLogged(){
+    return intake == null ? -90 : -intake.getPivotAngle();
+  }
+  public double climbAngleLogged(){
+    return climb == null ? 0 : climb.getEstimatedClimbAngle();
+  }
+
+  public void logComponents(){
+    if(!Constants.LOG_MECHANISMS) return;
+    
+    Logger.recordOutput(
+      "ComponentPoses", 
+      new Pose3d[] {
+        //intake
+        new Pose3d(0,-0.25,0.27, new Rotation3d(Units.degreesToRadians(intakeAngleLogged()), 0.0, 0.0)),
+        //climb
+        new Pose3d(0,0,0, new Rotation3d(0.0,climbAngleLogged(), 0.0)),
+        //arm
+        new Pose3d(0,0.110, 0.388 + elevatorHeightLogged(), new Rotation3d(Units.degreesToRadians(armAngleLogged()), 0.0, 0.0)),
+        //elevator 1
+        new Pose3d(0,0,0, new Rotation3d(0.0, 0.0, 0.0)),
+        //elevator 2
+        new Pose3d(0,0, elevatorHeightLogged()/3, new Rotation3d(0.0, 0.0, 0.0)),
+        //elevator 3
+        new Pose3d(0,0, elevatorHeightLogged()*2/3, new Rotation3d(0.0, 0.0, 0.0)),
+        //elevator 4
+        new Pose3d(0,0, elevatorHeightLogged(), new Rotation3d(0.0, 0.0, 0.0)),
+        //indexer
+        new Pose3d(0,0,0, new Rotation3d(0.0, 0.0, 0.0)),
+      }
+    );
+  }
+}
+
+

diff --git a/src/main/java/frc/robot/RobotId.java b/src/main/java/frc/robot/RobotId.java
new file mode 100644 (file)
index 0000000..24d04e0
--- /dev/null
+++ b/src/main/java/frc/robot/RobotId.java
@@ -0,0 +1,90 @@
+package frc.robot;
+
+import edu.wpi.first.wpilibj.Preferences;
+
+/**
+ * Set of known Robot Names.
+ * <p>The name of a robot in the RoboRIO's persistent memory.
+ * At deploy time, that name is used to set the corresponding RobotId.
+ * <p>Note that the RobotId is determined at Deploy time.
+ */
+public enum RobotId {
+    Default,
+    SwerveCompetition, Vertigo, Vivace, Phil, BetaBot,
+    ClassBot1, ClassBot2, ClassBot3, ClassBot4,
+    TestBed1, TestBed2;
+
+    /** The key used to access the RobotId name in the RoboRIO's persistent memory. */
+    public static final String ROBOT_ID_KEY = "RobotId";
+
+    /**
+     * Is this robot a classbot?
+     * @return true if a classbot
+     * @deprecated this method is not needed....
+     */
+    @Deprecated
+    public boolean isClassBot() {
+        return this == ClassBot1 || this == ClassBot2 || this == ClassBot3 || this == ClassBot4;
+    }
+
+    /**
+     * Whether this robot is a swerve bot
+     * @return true if a swerve bot
+     * @deprecated this method is not needed....
+     */
+    @Deprecated
+    public boolean isSwerveBot() {
+        return this == SwerveCompetition || this == Phil || this == Vertigo || this == Vivace || this == BetaBot;
+    }
+    
+    /**
+     * Determine the Robot Identity from the RoboRIO's onboard Preferences (flash memory).
+     * @returns the RobotId
+     */
+    public static RobotId getRobotId() {
+        // assume a default identity
+        RobotId robotId = RobotId.Default;
+
+        // check whether Preferences has an entry for the RobotId
+        if (!Preferences.containsKey(ROBOT_ID_KEY)) {
+            // There is no such key. Set it to the default identity.
+            // This step guarantees persistent memory will have a key.
+            setRobotId(RobotId.Default);
+        }
+
+        // Remove the "Default" key if present.
+        // This key was the result of a programming error in 2023.
+        if (Preferences.containsKey("Default")) {
+            Preferences.remove("Default");
+        }
+
+        // get the RobotId string from the RoboRIO's Preferences
+        String strId = Preferences.getString(ROBOT_ID_KEY, RobotId.Default.name());
+
+        // match that string to a RobotId by looking at all possible RobotId enums
+        for (RobotId rid : RobotId.values()) {
+            // does the preference string match the RobotId enum?
+            if (strId.equals(rid.name())) {
+                // yes, this instance is the desired RobotId
+                robotId = rid;
+                break;
+            }
+        }
+
+        // report the RobotId to the SmartDashboard.
+        //SmartDashboard.putString("RobotID", robotId.name());
+
+        // return the robot identity
+        return robotId;
+    }
+    
+    /**
+     * Set the RobotId in the RoboRIO's preferences (flash memory).
+     * <p>
+     * Calling it after the robot has been constructed (robotInit()) does not affect the robot.
+     */
+    static void setRobotId(RobotId robotId) {
+        // Set the robot identity in the RoboRIO Preferences
+        Preferences.setString(ROBOT_ID_KEY, robotId.name());
+    }
+}

diff --git a/src/main/java/frc/robot/commands/DoNothing.java b/src/main/java/frc/robot/commands/DoNothing.java
index 4648cefefbb7499dc4344a4a0b891c97fae01177..c6c0c08c6f340fc11b982c97d66a2a6b12d58dae 100644 (file)
--- a/src/main/java/frc/robot/commands/DoNothing.java
+++ b/src/main/java/frc/robot/commands/DoNothing.java
@@ -2,4 +2,8 @@ package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 
-public class DoNothing extends InstantCommand {}
+/**
+ * Does nothing. Can be used to more clearly mark commands intended not to do anything.
+ */
+public class DoNothing extends InstantCommand {
+}

diff --git a/src/main/java/frc/robot/commands/SupplierCommand.java b/src/main/java/frc/robot/commands/SupplierCommand.java
new file mode 100644 (file)
index 0000000..b173bd1
--- /dev/null
+++ b/src/main/java/frc/robot/commands/SupplierCommand.java
@@ -0,0 +1,50 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+
+import java.util.function.Supplier;
+
+/**
+ * Runs the given command when this command is initialized, and ends when it ends.
+ * Useful for commands that are not created yet because the constructor parameters are not available until initialization.
+ */
+public class SupplierCommand extends Command {
+
+    private final Supplier<Command> commandSupplier;
+    private Command command;
+
+    /**
+     * Runs the given command when this command is initialized, and ends when it ends.
+     * Useful for commands that are not created yet because the constructor parameters are not available until initialization.
+     *
+     * @param commandSupplier A Supplier to the command to run
+     * @param Subsystem       all subsystems that may be required to run the command supplied
+     */
+    public SupplierCommand(Supplier<Command> commandSupplier, Subsystem... Subsystem) {
+        addRequirements(Subsystem);
+        this.commandSupplier = commandSupplier;
+    }
+
+    @Override
+    public final void initialize() {
+        command = commandSupplier.get();
+        command.initialize();
+    }
+
+    @Override
+    public final void execute() {
+        command.execute();
+    }
+
+    @Override
+    public final void end(boolean interrupted) {
+        command.end(interrupted);
+    }
+
+    @Override
+    public final boolean isFinished() {
+        return command.isFinished();
+    }
+
+}

diff --git a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java
new file mode 100644 (file)
index 0000000..98e5d01
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java
@@ -0,0 +1,37 @@
+package frc.robot.commands.auto_comm;
+
+import choreo.auto.AutoFactory;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.DoNothing;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+public class ChoreoPathCommand extends SequentialCommandGroup {
+    private static AutoFactory factory;
+
+    public ChoreoPathCommand(String pathName, boolean resetOdemetry, Drivetrain drive) {
+        if(factory == null){
+            factory = new AutoFactory(
+                drive::getPose,
+                drive::resetOdometry,
+                sample -> drive.setChassisSpeeds(sample.getChassisSpeeds(), false),
+                true,
+                drive,
+                (trajectory, bool)->{
+                    if(bool){
+                        
+                    }else{
+                        
+                    }
+                }
+            );
+        }
+
+        var command = factory.trajectoryCmd(pathName);
+
+        addCommands(
+            resetOdemetry ? new InstantCommand(()->factory.resetOdometry(pathName)) : new DoNothing(),
+            command
+        );
+    }
+}

diff --git a/src/main/java/frc/robot/commands/auto_comm/FollowPathCommand.java b/src/main/java/frc/robot/commands/auto_comm/FollowPathCommand.java
new file mode 100644 (file)
index 0000000..a624ab2
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auto_comm/FollowPathCommand.java
@@ -0,0 +1,48 @@
+// 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 frc.robot.commands.auto_comm;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathPlannerPath;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.RobotContainer;
+import frc.robot.commands.SupplierCommand;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.PathGroupLoader;
+
+/** Add your docs here. */
+public class FollowPathCommand extends SequentialCommandGroup {
+    Drivetrain drive;
+    PathPlannerPath path;
+    
+
+    public FollowPathCommand(String name, Drivetrain drive){
+        this(name, false, drive);
+    }
+    
+    public FollowPathCommand(String pathName, boolean resetOdemetry, Drivetrain drive){
+        this.drive = drive;
+        this.path = PathGroupLoader.getPathGroup(pathName);
+        addCommands(
+            new InstantCommand(()->resetOdemetry(resetOdemetry)),
+            new SupplierCommand(()->AutoBuilder.followPath(path), drive) // "problem" (254)
+            // or pp's interaction with the drivetrain
+            // or pp config
+            );
+    }
+
+    public void resetOdemetry(boolean resetOdemetry){
+        if (resetOdemetry){
+            if(RobotContainer.getAllianceColorBooleanSupplier().getAsBoolean()){
+                drive.resetOdometry(new Pose2d(path.getAllPathPoints().get(0).flip().position, path.getIdealStartingState().flip().rotation()));
+            }else{
+                drive.resetOdometry(new Pose2d(path.getAllPathPoints().get(0).position, path.getIdealStartingState().rotation()));
+            }
+        }
+    }
+}

diff --git a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
new file mode 100644 (file)
index 0000000..0ea7bf8
--- /dev/null
+++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
@@ -0,0 +1,71 @@
+package frc.robot.commands.drive_comm;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Robot;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.controls.BaseDriverConfig;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.Vision.DriverAssist;
+
+
+/**
+ * Default drive command. Drives robot using driver controls.
+ */
+public class DefaultDriveCommand extends Command {
+    private final Drivetrain swerve;
+    private final BaseDriverConfig driver;
+
+    public DefaultDriveCommand(
+            Drivetrain swerve,
+            BaseDriverConfig driver) {
+        this.swerve = swerve;
+        this.driver = driver;
+
+        addRequirements(swerve);
+    }
+
+    @Override
+    public void initialize() {
+        swerve.setStateDeadband(true);
+    }
+
+    @Override
+    public void execute() {
+        double forwardTranslation = driver.getForwardTranslation();
+        double sideTranslation = driver.getSideTranslation();
+        double rotation = -driver.getRotation();
+
+        double slowFactor = driver.getIsSlowMode() ? DriveConstants.SLOW_DRIVE_FACTOR : 1;
+
+        forwardTranslation *= slowFactor;
+        sideTranslation *= slowFactor;
+        rotation *= driver.getIsSlowMode() ? DriveConstants.SLOW_ROT_FACTOR : 1;
+
+        int allianceReversal = Robot.getAlliance() == Alliance.Red ? 1 : -1;
+        forwardTranslation *= allianceReversal;
+        sideTranslation *= allianceReversal;
+
+        ChassisSpeeds driverInput = new ChassisSpeeds(forwardTranslation, sideTranslation, rotation);
+        ChassisSpeeds corrected = DriverAssist.calculate(swerve, driverInput, swerve.getDesiredPose(), true);
+
+        // If the driver is pressing the align button or a command set the drivetrain to
+        // align, then align to speaker
+        if (driver.getIsAlign() || swerve.getIsAlign()) {
+            swerve.driveHeading(
+                    forwardTranslation,
+                    sideTranslation,
+                    swerve.getAlignAngle(),
+                    true);
+        } else {
+            swerve.drive(
+                    corrected.vxMetersPerSecond,
+                    corrected.vyMetersPerSecond,
+                    corrected.omegaRadiansPerSecond,
+                    true,
+                    false);
+        }
+    }
+}
+

diff --git a/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java b/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java
new file mode 100644 (file)
index 0000000..bab477d
--- /dev/null
+++ b/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java
@@ -0,0 +1,207 @@
+// 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.commands.drive_comm;
+
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.Constants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.GeomUtil;
+
+public class DriveToPose extends Command {
+  protected static boolean updateTarget = false;
+  private static final double drivekP = 5.0;
+  private static final double drivekD = 0.0;
+  private static final double thetakP = 7.0;
+  private static final double thetakD = 0.0;
+  private static final double driveMaxVelocity = DriveConstants.MAX_SPEED;
+  private static final double driveMaxAcceleration = 2.6;
+  private static final double thetaMaxVelocity = 5.0;
+  private static final double thetaMaxAcceleration = 5.0;
+  private static final double driveTolerance = 0.015;
+  private static final double thetaTolerance = Units.degreesToRadians(1.0);
+  private static final double ffMinRadius = 0.05;
+  private static final double ffMaxRadius = 0.1;
+
+  private final Drivetrain drive;
+  private final Supplier<Pose2d> target;
+  private Pose2d targetPose;
+
+  private final ProfiledPIDController driveController =
+      new ProfiledPIDController(
+          drivekP, 0.0, drivekD, new TrapezoidProfile.Constraints(driveMaxVelocity, driveMaxAcceleration), Constants.LOOP_TIME);
+  private final ProfiledPIDController thetaController =
+      new ProfiledPIDController(
+          thetakP, 0.0, thetakD, new TrapezoidProfile.Constraints(thetaMaxVelocity, thetaMaxAcceleration), Constants.LOOP_TIME);
+
+  private Translation2d lastSetpointTranslation = new Translation2d();
+  private double driveErrorAbs = 0.0;
+  private double thetaErrorAbs = 0.0;
+  private boolean running = false;
+  private Supplier<Pose2d> robot;
+
+  private Supplier<Translation2d> linearFF = () -> Translation2d.kZero;
+  private DoubleSupplier omegaFF = () -> 0.0;
+
+  private Debouncer debouncer = new Debouncer(0.2);
+
+  public DriveToPose(Drivetrain drive, Supplier<Pose2d> target) {
+    this.drive = drive;
+    this.target = target;
+    robot = drive::getPose;
+
+    // Set tolerance
+    driveController.setTolerance(driveTolerance);
+    thetaController.setTolerance(thetaTolerance);
+
+    // Enable continuous input for theta controller
+    thetaController.enableContinuousInput(-Math.PI, Math.PI);
+
+    addRequirements(drive);
+  }
+
+  public DriveToPose(
+      Drivetrain drive,
+      Supplier<Pose2d> target,
+      Supplier<Translation2d> linearFF,
+      DoubleSupplier omegaFF) {
+    this(drive, target);
+    this.linearFF = linearFF;
+    this.omegaFF = omegaFF;
+  }
+
+  @Override
+  public void initialize() {
+    drive.setVisionEnabled(VisionConstants.ENABLED_GO_TO_POSE);
+
+    targetPose = target.get();
+    Pose2d currentPose = robot.get();
+    ChassisSpeeds fieldVelocity = ChassisSpeeds.fromRobotRelativeSpeeds(drive.getChassisSpeeds(), currentPose.getRotation());
+    Translation2d linearFieldVelocity =
+        new Translation2d(fieldVelocity.vxMetersPerSecond, fieldVelocity.vyMetersPerSecond);
+    
+    thetaController.reset(
+        currentPose.getRotation().getRadians(), fieldVelocity.omegaRadiansPerSecond);
+        lastSetpointTranslation = currentPose.getTranslation();
+    
+    if(targetPose != null){
+        driveController.reset(
+            currentPose.getTranslation().getDistance(target.get().getTranslation()),
+            -linearFieldVelocity
+                .rotateBy(
+                    targetPose
+                        .getTranslation()
+                        .minus(currentPose.getTranslation())
+                        .getAngle()
+                        .unaryMinus())
+                .getX());
+        }
+  }
+
+  @Override
+  public void execute() {
+    running = true;
+
+    // Get current pose and target pose
+    Pose2d currentPose = robot.get();
+    if(updateTarget){
+        targetPose = target.get();
+    }
+    if(targetPose == null){
+        return;
+    }
+
+    // Calculate drive speed
+    double currentDistance = currentPose.getTranslation().getDistance(targetPose.getTranslation());
+    double ffScaler =
+        MathUtil.clamp(
+            (currentDistance - ffMinRadius) / (ffMaxRadius - ffMinRadius),
+            0.0,
+            1.0);
+    driveErrorAbs = currentDistance;
+    driveController.reset(
+        lastSetpointTranslation.getDistance(targetPose.getTranslation()),
+        driveController.getSetpoint().velocity);
+    double driveVelocityScalar =
+        driveController.getSetpoint().velocity * ffScaler
+            + driveController.calculate(driveErrorAbs, 0.0);
+    if (currentDistance < driveController.getPositionTolerance()) driveVelocityScalar = 0.0;
+    lastSetpointTranslation =
+        new Pose2d(
+                targetPose.getTranslation(),
+                currentPose.getTranslation().minus(targetPose.getTranslation()).getAngle())
+            .transformBy(GeomUtil.toTransform2d(driveController.getSetpoint().position, 0.0))
+            .getTranslation();
+
+    // Calculate theta speed
+    double thetaVelocity =
+        thetaController.getSetpoint().velocity * ffScaler
+            + thetaController.calculate(
+                currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians());
+    thetaErrorAbs =
+        Math.abs(currentPose.getRotation().minus(targetPose.getRotation()).getRadians());
+    if (thetaErrorAbs < thetaController.getPositionTolerance()) thetaVelocity = 0.0;
+
+    Translation2d driveVelocity =
+        new Pose2d(
+                new Translation2d(),
+                currentPose.getTranslation().minus(targetPose.getTranslation()).getAngle())
+            .transformBy(GeomUtil.toTransform2d(driveVelocityScalar, 0.0))
+            .getTranslation();
+
+    // Scale feedback velocities by input ff
+    final double linearS = linearFF.get().getNorm() * 3.0;
+    final double thetaS = Math.abs(omegaFF.getAsDouble()) * 3.0;
+    driveVelocity =
+        driveVelocity.interpolate(linearFF.get().times(DriveConstants.MAX_SPEED), linearS);
+    thetaVelocity =
+        MathUtil.interpolate(
+            thetaVelocity, omegaFF.getAsDouble() * DriveConstants.MAX_ANGULAR_SPEED, thetaS);
+
+    // Command speeds
+    drive.drive(driveVelocity.getX(), driveVelocity.getY(), thetaVelocity, true, false);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    drive.stop();
+    drive.setVisionEnabled(true);
+    running = false;
+  }
+
+  /** Checks if the robot is stopped at the final pose. */
+  public boolean atGoal() {
+    return running && (driveController.atGoal() && thetaController.atGoal() || targetPose == null);
+  }
+
+  /** Checks if the robot pose is within the allowed drive and theta tolerances. */
+  public boolean withinTolerance(double driveTolerance, Rotation2d thetaTolerance) {
+    return running
+        && (Math.abs(driveErrorAbs) < driveTolerance
+        && Math.abs(thetaErrorAbs) < thetaTolerance.getRadians()
+        || targetPose == null);
+  }
+
+  @Override
+  public boolean isFinished(){
+    return debouncer.calculate(withinTolerance(driveTolerance, new Rotation2d(thetaTolerance)));
+  }
+}

diff --git a/src/main/java/frc/robot/commands/drive_comm/GoToPose.java b/src/main/java/frc/robot/commands/drive_comm/GoToPose.java
new file mode 100644 (file)
index 0000000..e2afa7a
--- /dev/null
+++ b/src/main/java/frc/robot/commands/drive_comm/GoToPose.java
@@ -0,0 +1,89 @@
+package frc.robot.commands.drive_comm;
+
+import java.util.function.Supplier;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathConstraints;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.DoNothing;
+import frc.robot.commands.SupplierCommand;
+import frc.robot.constants.AutoConstants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/**
+* Moves the robot to a pose using PathPlanner
+*/
+public class GoToPose extends SequentialCommandGroup {
+
+  private Drivetrain drive;
+  private Supplier<Pose2d> poseSupplier;
+  private double maxSpeed;
+  private double maxAccel;
+
+  /**
+   * Uses PathPlanner to go to a pose
+   * @param poseSupplier The supplier for the pose to use
+   * @param drive The drivetrain
+   */
+  public GoToPose(Supplier<Pose2d> poseSupplier, Drivetrain drive) {
+    this(poseSupplier, AutoConstants.MAX_AUTO_SPEED, AutoConstants.MAX_AUTO_ACCEL, drive);
+  }
+  public GoToPose(Pose2d pose, Drivetrain drive){
+    this(()->pose, drive);
+  }
+
+  /**
+   * Uses PathPlanner to go to a pose
+   * @param poseSupplier The supplier for the pose to use
+   * @param maxSpeed The maximum speed to use
+   * @param maxAccel The maximum acceleration to use
+   * @param drive The drivetrain
+   */
+  public GoToPose(Supplier<Pose2d> poseSupplier, double maxSpeed, double maxAccel, Drivetrain drive) {
+    this.poseSupplier = poseSupplier;
+    this.maxSpeed = maxSpeed;
+    this.maxAccel = maxAccel;
+    this.drive = drive;
+    addCommands(
+      new InstantCommand(()->drive.setVisionEnabled(VisionConstants.ENABLED_GO_TO_POSE)),
+      new SupplierCommand(() -> createCommand(), drive).handleInterrupt(()->drive.setVisionEnabled(true)),
+      new InstantCommand(()->drive.setVisionEnabled(true))
+    );
+  }
+
+  /**
+   * Creates the PathPlanner command and schedules it
+   */
+  public Command createCommand() {
+    Pose2d pose = poseSupplier.get();
+    if(pose==null){
+      return new DoNothing();
+    }
+    Command command = AutoBuilder.pathfindToPose(
+      pose,
+      new PathConstraints(maxSpeed, maxAccel, DriveConstants.MAX_ANGULAR_SPEED, DriveConstants.MAX_ANGULAR_ACCEL),
+      0
+    );
+
+    // get the distance to the pose.
+    double dist = drive.getPose().minus(pose).getTranslation().getNorm();
+
+    // if greater than 3m or less than 2 cm, don't run it. If the path is too small pathplanner makes weird paths.
+    if (dist > 3) {
+      command = new DoNothing();
+      DriverStation.reportWarning("Alignment Path too long, doing nothing, GoToPose.java", false);
+    } else if (dist < 0.02) {
+      command = new DoNothing();
+      DriverStation.reportWarning("Alignment Path too short, doing nothing, GoToPose.java", false);
+    }
+
+    return command;
+  }
+}

diff --git a/src/main/java/frc/robot/commands/drive_comm/GoToPosePID.java b/src/main/java/frc/robot/commands/drive_comm/GoToPosePID.java
new file mode 100644 (file)
index 0000000..cb49203
--- /dev/null
+++ b/src/main/java/frc/robot/commands/drive_comm/GoToPosePID.java
@@ -0,0 +1,61 @@
+package frc.robot.commands.drive_comm;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.VisionConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/**
+ * Runs the chassis PIDs to move the robot to a specific pose. 
+ */
+public class GoToPosePID extends Command {
+
+  private Drivetrain drive; 
+  
+  private Supplier<Pose2d> poseSupplier;
+  private Pose2d pose;
+  
+  /**
+   * Runs the chassis PIDs to move the robot to a specific pose. 
+   * @param pose The pose supplier to go to
+   * @param drive The drivetrain
+   */
+  public GoToPosePID(Supplier<Pose2d> pose, Drivetrain drive) {
+    this.drive = drive;
+    this.poseSupplier = pose;
+
+    addRequirements(drive);
+  }
+
+  public GoToPosePID(Pose2d pose, Drivetrain drive){
+    this(()->pose, drive);
+  }
+
+  @Override
+  public void initialize() {
+    pose = poseSupplier.get();
+    drive.setVisionEnabled(VisionConstants.ENABLED_GO_TO_POSE);
+  }
+
+  @Override
+  public void execute() {
+    if(pose == null) {
+      return;
+    }
+
+    drive.driveWithPID(pose.getX(), pose.getY(), pose.getRotation().getRadians()); 
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    drive.stop();
+    drive.setVisionEnabled(true);
+  }
+
+  @Override
+    public boolean isFinished() {
+        return pose == null || drive.getXController().atSetpoint() && drive.getYController().atSetpoint() && drive.getRotationController().atSetpoint();
+    }
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/commands/drive_comm/SetFormationX.java b/src/main/java/frc/robot/commands/drive_comm/SetFormationX.java
new file mode 100644 (file)
index 0000000..22dc905
--- /dev/null
+++ b/src/main/java/frc/robot/commands/drive_comm/SetFormationX.java
@@ -0,0 +1,27 @@
+package frc.robot.commands.drive_comm;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/**
+ * Sets the robot's wheels to an X formation to prevent being pushed around by other bots.
+ */
+public class SetFormationX extends SequentialCommandGroup {
+    public SetFormationX(Drivetrain drive) {
+        addRequirements(drive);
+        addCommands(
+            new InstantCommand(() -> drive.setStateDeadband(false), drive),
+            new RunCommand(() -> drive.setModuleStates(new SwerveModuleState[]{
+                new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(45))),
+                new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(-45))),
+                new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(-45))),
+                new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(45)))
+            }, false), drive)
+        );
+    }
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/commands/drive_comm/SimplePresetSteerAngles.java b/src/main/java/frc/robot/commands/drive_comm/SimplePresetSteerAngles.java
new file mode 100644 (file)
index 0000000..74c2e43
--- /dev/null
+++ b/src/main/java/frc/robot/commands/drive_comm/SimplePresetSteerAngles.java
@@ -0,0 +1,51 @@
+package frc.robot.commands.drive_comm;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/**
+ * Attempts to set all four modules to a constant angle. Determines if the modules are able to reach the angle requested in a certain time.
+ */
+public class SimplePresetSteerAngles extends InstantCommand {
+
+    /**
+     * sets the angle of module steer to 0 to remove initial turn time and drift
+     *
+     * @param drive drivetrain to be used
+     */
+    public SimplePresetSteerAngles(Drivetrain drive) {
+        this(drive, new Rotation2d());
+    }
+
+    /**
+     * sets the angle of module steer to a angle to remove initial turn time and drift
+     *
+     * @param angle angle to set module steer to in radians
+     * @param drive drivetrain to be used
+     */
+    public SimplePresetSteerAngles(Drivetrain drive, double angle) {
+        this(drive, new Rotation2d(angle));
+    }
+
+    /**
+     * sets the angle of module steer to a angle to remove initial turn time and drift
+     *
+     * @param rotation rotation to set module steer to
+     * @param drive    drivetrain to be used
+     */
+    public SimplePresetSteerAngles(Drivetrain drive, Rotation2d rotation) {
+        super(() -> {
+            drive.setStateDeadband(false);
+            drive.setModuleStates(new SwerveModuleState[]{
+                    new SwerveModuleState(0, rotation),
+                    new SwerveModuleState(0, rotation),
+                    new SwerveModuleState(0, rotation),
+                    new SwerveModuleState(0, rotation)
+            }, true);
+            drive.setStateDeadband(true);
+        }, drive);
+        drive.setStateDeadband(true);
+    }
+}

diff --git a/src/main/java/frc/robot/commands/drive_comm/SysIDDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/SysIDDriveCommand.java
new file mode 100644 (file)
index 0000000..17bb229
--- /dev/null
+++ b/src/main/java/frc/robot/commands/drive_comm/SysIDDriveCommand.java
@@ -0,0 +1,60 @@
+// 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 frc.robot.commands.drive_comm;
+
+
+import com.ctre.phoenix6.SignalLogger;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.SysId;
+
+/**
+ * A command to run all 4 SysId routines on the drivetrain
+*/
+public class SysIDDriveCommand extends SequentialCommandGroup {
+
+    private Config config = new Config();
+    private SysId sysId;
+    public SysIDDriveCommand(Drivetrain drive) {
+        config = new Config(
+            Units.Volts.of(0.5).per(Units.Seconds),
+            Units.Volts.of(3),
+            Units.Seconds.of(5),
+            (x)->SignalLogger.writeString("state", x.toString())
+        );
+        Rotation2d[] angles = {
+            Rotation2d.fromDegrees(0),//-45-180
+            Rotation2d.fromDegrees(0),//45
+            Rotation2d.fromDegrees(0),//45+180
+            Rotation2d.fromDegrees(0),//-45
+        };
+        sysId = new SysId(
+            "Drivetrain",
+            x ->{
+                    drive.setAngleMotors(angles);
+                    drive.setDriveVoltages(x);
+                },
+            drive,
+            config
+        );
+        addCommands(
+            sysId.runQuasisStatic(Direction.kForward),
+            new WaitCommand(0.5),
+            sysId.runQuasisStatic(Direction.kReverse),
+            new WaitCommand(0.5),
+            sysId.runDynamic(Direction.kForward),
+            new WaitCommand(0.5),
+            sysId.runDynamic(Direction.kReverse)
+            );
+    }
+
+    
+
+}

diff --git a/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java b/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java
new file mode 100644 (file)
index 0000000..393712a
--- /dev/null
+++ b/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java
@@ -0,0 +1,51 @@
+package frc.robot.commands.drive_comm;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.Trajectory.State;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/**
+ * Sets all module angles to a given trajectory's initial angle.
+ */
+public class TrajectoryPresetSteerAngles extends InstantCommand {
+    /*
+     * make sure to add wait command after called to give time to correct
+     */
+    public TrajectoryPresetSteerAngles(Drivetrain drive, Trajectory trajectory) {
+        super(
+                () -> {
+
+                    // 0.01 is the time between trajectory samples, in seconds
+                    // Can be replaced for any small number, but it should be the same as the time between all uses
+                    double time = 0.01;
+
+                    drive.setStateDeadband(false);
+
+                    Pose2d initialPose = trajectory.getInitialPose();
+                    State sample = trajectory.sample(time);
+                    Pose2d nextPose = sample.poseMeters;
+
+                    double xVelocity = sample.velocityMetersPerSecond * nextPose.getRotation().getCos();
+                    double yVelocity = sample.velocityMetersPerSecond * nextPose.getRotation().getSin();
+                    double angularVelo = (nextPose.getRotation().getRadians() - initialPose.getRotation().getRadians()) / time;
+
+                    ChassisSpeeds chassisSpeeds = new ChassisSpeeds(xVelocity, yVelocity, angularVelo);
+                    chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds, initialPose.getRotation());
+
+                    SwerveModuleState[] swerveModuleStates = DriveConstants.KINEMATICS.toSwerveModuleStates(chassisSpeeds);
+                    for (SwerveModuleState swerveModuleState : swerveModuleStates) {
+                        swerveModuleState.speedMetersPerSecond = 0;
+                    }
+                    drive.setModuleStates(swerveModuleStates, true);
+                    drive.setStateDeadband(true);
+                },
+                drive
+             );
+
+    }
+}

diff --git a/src/main/java/frc/robot/commands/gpm/IntakeAlgae.java b/src/main/java/frc/robot/commands/gpm/IntakeAlgae.java
new file mode 100644 (file)
index 0000000..0b38ce3
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/IntakeAlgae.java
@@ -0,0 +1,26 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.IntakeConstants;
+import frc.robot.subsystems.intake.Intake;
+
+public class IntakeAlgae extends Command {
+    private final Intake intake;
+
+    public IntakeAlgae(Intake intake) {
+        this.intake = intake;
+        addRequirements(intake);
+    }
+
+    @Override
+    public void initialize() {
+        intake.setAngle(IntakeConstants.ALGAE_SETPOINT);
+        intake.setSpeed(IntakeConstants.ALGAE_INTAKE_POWER);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        intake.deactivate();
+        intake.setSpeed(-0.05);
+    }
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/commands/gpm/IntakeAlgaeArm.java b/src/main/java/frc/robot/commands/gpm/IntakeAlgaeArm.java
new file mode 100644 (file)
index 0000000..5ddef9b
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/IntakeAlgaeArm.java
@@ -0,0 +1,23 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.outtake.Outtake;
+
+public class IntakeAlgaeArm extends Command {
+    private final Outtake outtake;
+
+    public IntakeAlgaeArm(Outtake outtake) {
+        this.outtake = outtake;
+        addRequirements(outtake);
+    }
+
+    @Override
+    public void initialize() {
+        outtake.intakeAlgaeReef();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        outtake.setMotor(-0.1);
+    }
+}

diff --git a/src/main/java/frc/robot/commands/gpm/IntakeCoral.java b/src/main/java/frc/robot/commands/gpm/IntakeCoral.java
new file mode 100644 (file)
index 0000000..2b02329
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/IntakeCoral.java
@@ -0,0 +1,28 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.constants.ArmConstants;
+import frc.robot.constants.ElevatorConstants;
+import frc.robot.subsystems.arm.Arm;
+import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.subsystems.indexer.Indexer;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.outtake.Outtake;
+
+/**
+ * Intake a coral.
+ */
+public class IntakeCoral extends SequentialCommandGroup {
+       public IntakeCoral(Intake intake, Indexer indexer, Elevator elevator, Outtake outtake, Arm arm) {
+               addCommands(
+                       new ParallelCommandGroup(
+                                       new InstantCommand(() -> {
+                                               intake.unstow(); 
+                                               elevator.setSetpoint(ElevatorConstants.INTAKE_SETPOINT); 
+                                               arm.setSetpoint(ArmConstants.INTAKE_SETPOINT);}),
+                               new IntakeCoralHelper(intake, indexer, outtake, arm, elevator)));
+       }
+
+}

diff --git a/src/main/java/frc/robot/commands/gpm/IntakeCoralHelper.java b/src/main/java/frc/robot/commands/gpm/IntakeCoralHelper.java
new file mode 100644 (file)
index 0000000..3f5159a
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/IntakeCoralHelper.java
@@ -0,0 +1,106 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.ArmConstants;
+import frc.robot.constants.ElevatorConstants;
+import frc.robot.subsystems.arm.Arm;
+import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.subsystems.indexer.Indexer;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.outtake.Outtake;
+
+/**
+ * All the intake code except the elevator stuff.
+ * Don't call this directly; use {@link frc.robot.commands.gpm.IntakeCoral}
+ * instead.
+ */
+public class IntakeCoralHelper extends Command {
+       private Intake intake;
+       private Indexer indexer;
+       private Outtake outtake;
+       private Arm arm;
+       private Elevator elevator;
+
+       private enum Phase {
+               Acquiring, Intaking, Indexing, Detected, InOuttake, Done
+       };
+
+       private Phase phase;
+
+       public IntakeCoralHelper(Intake intake, Indexer indexer, Outtake outtake, Arm arm, Elevator elevator) {
+               this.intake = intake;
+               this.indexer = indexer;
+               this.outtake = outtake;
+               this.arm = arm;
+               this.elevator = elevator;
+               addRequirements(intake, indexer, arm, elevator);
+               if(outtake != null){
+                       addRequirements(outtake);
+               }
+       }
+
+       @Override
+       public void initialize() {
+               intake.activate();
+               intake.unstow();
+               indexer.run();
+               phase = Phase.Acquiring;
+               if(outtake != null) {
+                       outtake.setMotor(0.7);
+               }
+       }
+
+       @Override
+       public void execute() {
+               if(outtake != null && outtake.coralLoaded()){
+                       phase = Phase.InOuttake;
+               }
+               switch (phase) {
+                       case Acquiring:
+                       case Intaking:
+                       case Indexing:
+                               if (!indexer.isIndexerClear()) {
+                                       phase = Phase.Detected;
+                                       intake.setSpeed(0.1);
+                                       indexer.slow();
+                               }
+                               break;
+                       case Detected:
+                               if(indexer.isIndexerClear()){
+                                       phase = Phase.InOuttake;
+                                       intake.deactivate();
+                                       intake.stow();
+                               }
+                               break;
+                       case InOuttake:
+                               if(outtake == null || outtake.coralLoaded()){
+                                       phase = Phase.Done;
+                                       elevator.setSetpoint(ElevatorConstants.INTAKE_STOW_SETPOINT);
+                                       arm.setSetpoint(ArmConstants.STOW_SETPOINT);
+                                       indexer.stop();
+                                       if(outtake != null){
+                                               outtake.setMotor(0.1);
+                                       }
+                               }
+                               break;
+                       case Done:
+                               break;
+               }
+       }
+
+       @Override
+       public boolean isFinished() {
+               return phase == Phase.Done && elevator.getPosition() > ElevatorConstants.SAFE_SETPOINT-0.025;
+       }
+
+       @Override
+       public void end(boolean interrupted) {
+               // in case it's interrupted
+               intake.deactivate();
+               intake.stow();
+               indexer.stop();
+               if(outtake != null){
+                       outtake.setMotor(0.02);
+               }
+       }
+}

diff --git a/src/main/java/frc/robot/commands/gpm/MoveArm.java b/src/main/java/frc/robot/commands/gpm/MoveArm.java
new file mode 100644 (file)
index 0000000..1a70774
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/MoveArm.java
@@ -0,0 +1,25 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.arm.Arm;
+
+public class MoveArm extends Command {
+    private Arm arm;
+    private double setpoint;
+
+    public MoveArm(Arm arm, double setpoint) {
+        this.arm = arm;
+        this.setpoint = setpoint;
+        addRequirements(arm);
+    }
+
+    @Override
+    public void initialize() {
+        arm.setSetpoint(setpoint);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return arm.atSetpoint();
+    }
+}

diff --git a/src/main/java/frc/robot/commands/gpm/MoveElevator.java b/src/main/java/frc/robot/commands/gpm/MoveElevator.java
new file mode 100644 (file)
index 0000000..200f6a8
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/MoveElevator.java
@@ -0,0 +1,47 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.elevator.Elevator;
+
+/**
+ * Moves the elevator to a position
+ */
+public class MoveElevator extends Command {
+    private Elevator elevator;
+    private double setpoint;
+
+    /**
+     * Creates a command to move the elevator to a position
+     * 
+     * @param elevator The elevator subsystem
+     * @param setpoint The setpoint to move to
+     */
+    public MoveElevator(Elevator elevator, double setpoint) {
+        this.elevator = elevator;
+        this.setpoint = setpoint;
+        if(elevator != null){
+            addRequirements(elevator);
+        }
+    }
+
+    /**
+     * Sets the elevator setpoint
+     */
+    @Override
+    public void initialize() {
+        if(elevator != null){
+            elevator.setSetpoint(setpoint);
+        }
+    }
+
+    /**
+     * Returns whether the elevator is at the setpoint
+     * 
+     * @return True if the elevator is within about 1 inch of the setpoint, false
+     *         otherwise
+     */
+    @Override
+    public boolean isFinished() {
+        return elevator == null || elevator.atSetpoint();
+    }
+}

diff --git a/src/main/java/frc/robot/commands/gpm/NetSetpoint.java b/src/main/java/frc/robot/commands/gpm/NetSetpoint.java
new file mode 100644 (file)
index 0000000..57e92ac
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/NetSetpoint.java
@@ -0,0 +1,42 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.constants.ArmConstants;
+import frc.robot.constants.ElevatorConstants;
+import frc.robot.constants.FieldConstants;
+import frc.robot.subsystems.arm.Arm;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.elevator.Elevator;
+
+public class NetSetpoint extends SequentialCommandGroup {
+    public NetSetpoint(Elevator elevator, Arm arm, Drivetrain drive){
+        this(true, elevator, arm, drive);
+    }
+    public NetSetpoint(boolean chooseClosestSide, Elevator elevator, Arm arm, Drivetrain drive){
+        addCommands(
+            // new InstantCommand(()->{
+            //     if(chooseClosestSide){
+            //         drive.setAlignAngle(
+            //             useSetpoint1(drive, chooseClosestSide) == (drive.getPose().getX() < FieldConstants.FIELD_LENGTH/2)
+            //             ? Math.PI / 2 : -Math.PI / 2
+            //         );
+            //         drive.setIsAlign(false);
+            //     }
+            // }),
+            new ParallelCommandGroup(
+            new MoveArm(arm, ArmConstants.ALGAE_STOW_SETPOINT),
+            new MoveElevator(elevator, ElevatorConstants.NET_SETPOINT)),
+            new ConditionalCommand(
+                new MoveArm(arm, ArmConstants.ALGAE_NET_SETPOINT_1),
+                new MoveArm(arm, ArmConstants.ALGAE_NET_SETPOINT_2),
+                ()->useSetpoint1(drive, chooseClosestSide))
+        );
+    }
+    private static boolean useSetpoint1(Drivetrain drive, boolean chooseClosestSide){
+        boolean positive = MathUtil.angleModulus(drive.getYaw().getRadians()) > 0;
+        return !chooseClosestSide || positive == (drive.getPose().getX() < FieldConstants.FIELD_LENGTH/2);
+    }
+}

diff --git a/src/main/java/frc/robot/commands/gpm/OutakeMotors.java b/src/main/java/frc/robot/commands/gpm/OutakeMotors.java
new file mode 100644 (file)
index 0000000..29b2953
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/OutakeMotors.java
@@ -0,0 +1,27 @@
+// 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 frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.outtake.Outtake;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class OutakeMotors extends SequentialCommandGroup {
+  /** Creates a new OutakeMotors. */
+  public OutakeMotors(Intake intake, Outtake outtake) {
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    addCommands(
+      new InstantCommand(()->outtake.outtake(), outtake),
+      new WaitCommand(0.5),
+      new InstantCommand(()->outtake.stop(), outtake)
+    );
+  }
+}

diff --git a/src/main/java/frc/robot/commands/gpm/OuttakeAlgae.java b/src/main/java/frc/robot/commands/gpm/OuttakeAlgae.java
new file mode 100644 (file)
index 0000000..1bae55f
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/OuttakeAlgae.java
@@ -0,0 +1,16 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import frc.robot.constants.IntakeConstants;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.outtake.Outtake;
+
+public class OuttakeAlgae extends ConditionalCommand {
+    public OuttakeAlgae(Outtake outtake, Intake intake){
+        super(
+            new OuttakeAlgaeArm(outtake),
+            new OuttakeAlgaeIntake(intake),
+            ()-> intake.isAtSetpoint(IntakeConstants.STOW_SETPOINT)
+        );
+    }
+}

diff --git a/src/main/java/frc/robot/commands/gpm/OuttakeAlgaeArm.java b/src/main/java/frc/robot/commands/gpm/OuttakeAlgaeArm.java
new file mode 100644 (file)
index 0000000..6a2ec85
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/OuttakeAlgaeArm.java
@@ -0,0 +1,34 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.outtake.Outtake;
+
+public class OuttakeAlgaeArm extends Command{
+    private final Outtake outtake;
+
+    private final Timer timer = new Timer();
+    private final double EJECTION_TIME = 0.25;
+
+    public OuttakeAlgaeArm(Outtake outtake) {
+        this.outtake = outtake;
+        addRequirements(outtake);
+    }
+
+    @Override
+    public void initialize() {
+        outtake.outtakeAlgae();
+        timer.restart();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return timer.hasElapsed(EJECTION_TIME);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        outtake.stop();
+        timer.stop();
+    }
+}

diff --git a/src/main/java/frc/robot/commands/gpm/OuttakeAlgaeIntake.java b/src/main/java/frc/robot/commands/gpm/OuttakeAlgaeIntake.java
new file mode 100644 (file)
index 0000000..dba0acf
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/OuttakeAlgaeIntake.java
@@ -0,0 +1,38 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.IntakeConstants;
+import frc.robot.subsystems.intake.Intake;
+
+public class OuttakeAlgaeIntake extends Command {
+    private Intake intake;
+
+    private final Timer timer = new Timer();
+    private final double EJECTION_TIME = 1;
+
+    public OuttakeAlgaeIntake(Intake intake) {
+        this.intake = intake;
+        addRequirements(intake);
+    }
+
+    @Override
+    public void initialize() {
+        intake.setSpeed(IntakeConstants.ALGAE_OUTTAKE_POWER);
+        intake.setAngle(50);
+        timer.restart();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return timer.hasElapsed(EJECTION_TIME);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        intake.deactivate();
+        intake.stow();
+        timer.stop();
+    }
+
+}

diff --git a/src/main/java/frc/robot/commands/gpm/OuttakeCoral.java b/src/main/java/frc/robot/commands/gpm/OuttakeCoral.java
new file mode 100644 (file)
index 0000000..1ea8e57
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/OuttakeCoral.java
@@ -0,0 +1,24 @@
+package frc.robot.commands.gpm;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.constants.ElevatorConstants;
+import frc.robot.subsystems.arm.Arm;
+import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.subsystems.outtake.Outtake;
+
+public class OuttakeCoral extends SequentialCommandGroup {
+    public OuttakeCoral(Outtake outtake, Elevator elevator, Arm arm){
+        BooleanSupplier l4Supplier = () -> elevator.getSetpoint() > ElevatorConstants.L3_SETPOINT + 0.001;
+        BooleanSupplier l1Supplier = () -> elevator.getSetpoint() < ElevatorConstants.L1_SETPOINT + 0.001;
+        addCommands(
+            new ConditionalCommand(
+                new ScoreL4(outtake, arm),
+                new OuttakeCoralBasic(outtake, l4Supplier, l1Supplier),
+                l4Supplier)
+            //new InstantCommand(()->elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT), elevator)
+        );
+    }
+}

diff --git a/src/main/java/frc/robot/commands/gpm/OuttakeCoralBasic.java b/src/main/java/frc/robot/commands/gpm/OuttakeCoralBasic.java
new file mode 100644 (file)
index 0000000..7279dc4
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/OuttakeCoralBasic.java
@@ -0,0 +1,54 @@
+package frc.robot.commands.gpm;
+
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.outtake.Outtake;
+
+
+/**
+ * Command to eject coral.
+ * Wants coral to be present.
+ */
+public class OuttakeCoralBasic extends Command {
+    public static final double L1_SPEED = -0.2;
+    public static final double L4_SPEED = -0.8;
+    public static final double OUTTAKE_SPEED = -0.45;
+
+    private Outtake outtake;
+
+    // counter to measure time in ticks (every 20 milliseconds);
+    private Timer timer = new Timer();
+
+    private BooleanSupplier l4Supplier;
+    private BooleanSupplier l1Supplier;
+
+    public OuttakeCoralBasic(Outtake outtake, BooleanSupplier l4Supplier, BooleanSupplier l1Supplier){
+        this.outtake = outtake;
+        this.l4Supplier = l4Supplier;
+        this.l1Supplier = l1Supplier;
+        addRequirements(outtake);
+    }
+
+
+    @Override
+    public void initialize(){
+        timer.restart();
+        boolean l4 = l4Supplier.getAsBoolean();
+        boolean l1 = !l4 && l1Supplier.getAsBoolean();
+        outtake.setMotor(l4 ? L4_SPEED : l1 ? L1_SPEED : OUTTAKE_SPEED);
+    }
+
+    public boolean isFinished(){
+        return timer.hasElapsed(0.25);
+    }
+
+
+    public void end(boolean interrupted){
+        outtake.stop();
+    }
+}
+
+

diff --git a/src/main/java/frc/robot/commands/gpm/RemoveAlgae.java b/src/main/java/frc/robot/commands/gpm/RemoveAlgae.java
new file mode 100644 (file)
index 0000000..c63db70
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/RemoveAlgae.java
@@ -0,0 +1,22 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.outtake.Outtake;
+
+public class RemoveAlgae extends Command {
+    private Outtake outtake;
+    public RemoveAlgae(Outtake outtake){
+        this.outtake = outtake;
+        addRequirements(outtake);
+    }
+
+    @Override
+    public void initialize(){
+        outtake.removeAlgae();
+    }
+
+    @Override
+    public void end(boolean interrupted){
+        outtake.stop();
+    }
+}

diff --git a/src/main/java/frc/robot/commands/gpm/ResetClimb.java b/src/main/java/frc/robot/commands/gpm/ResetClimb.java
new file mode 100644 (file)
index 0000000..4479bc8
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/ResetClimb.java
@@ -0,0 +1,23 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.climb.Climb;
+
+public class ResetClimb extends Command {
+    private Climb climb;
+
+    public ResetClimb(Climb climb){
+        this.climb = climb;
+        addRequirements(climb);
+    }
+
+    @Override
+    public void initialize(){
+        climb.reset(true);
+    }
+    @Override
+    public void end(boolean interrupted){
+        climb.reset(false);
+    }
+
+}

diff --git a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java
new file mode 100644 (file)
index 0000000..254d08d
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java
@@ -0,0 +1,45 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.indexer.Indexer;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.outtake.Outtake;
+
+public class ReverseMotors extends Command {
+    private Intake intake;
+    private Indexer indexer;
+    private Outtake outtake;
+
+    public ReverseMotors(Intake intake, Indexer indexer, Outtake outtake) {
+        this.intake = intake;
+        this.indexer = indexer;
+        this.outtake = outtake;
+        addRequirements(intake, indexer);
+        if(outtake != null){
+            addRequirements(outtake);
+        }
+    }
+
+    @Override
+    public void initialize() {
+        intake.setSpeed(-.5);
+        indexer.reverse();
+        if(outtake != null){
+            outtake.reverse();
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        intake.deactivate();
+        indexer.stop();
+        if(outtake != null){
+            outtake.stop();;
+        }
+    }
+}

diff --git a/src/main/java/frc/robot/commands/gpm/RunIntakeAndIndexer.java b/src/main/java/frc/robot/commands/gpm/RunIntakeAndIndexer.java
new file mode 100644 (file)
index 0000000..4e2cd3c
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/RunIntakeAndIndexer.java
@@ -0,0 +1,29 @@
+// 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 frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.subsystems.indexer.Indexer;
+import frc.robot.subsystems.intake.Intake;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class RunIntakeAndIndexer extends SequentialCommandGroup {
+  /** Creates a new RunIntakeAndIndexer. */
+  public RunIntakeAndIndexer(Intake intake, Indexer indexer ) {
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    addCommands(
+      new InstantCommand(() -> intake.activate()),
+      new InstantCommand(() -> indexer.run()),
+      new WaitCommand(3),
+      new InstantCommand(() -> intake.deactivate()),
+      new InstantCommand(() -> indexer.stop())
+    );
+  }
+}

diff --git a/src/main/java/frc/robot/commands/gpm/ScoreL4.java b/src/main/java/frc/robot/commands/gpm/ScoreL4.java
new file mode 100644 (file)
index 0000000..7b0cb59
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/ScoreL4.java
@@ -0,0 +1,22 @@
+// 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 frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.arm.Arm;
+import frc.robot.subsystems.outtake.Outtake;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class ScoreL4 extends SequentialCommandGroup {
+  /** Creates a new ScoreL4. */
+  public ScoreL4(Outtake outtake, Arm arm) {
+    addCommands(
+      new OuttakeCoralBasic(outtake, ()->true, ()->false)
+    );
+  }
+}
+

diff --git a/src/main/java/frc/robot/commands/gpm/StationIntake.java b/src/main/java/frc/robot/commands/gpm/StationIntake.java
new file mode 100644 (file)
index 0000000..1224d33
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/StationIntake.java
@@ -0,0 +1,30 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.outtake.Outtake;
+
+public class StationIntake extends Command {
+    private Outtake outtake;
+
+    public StationIntake(Outtake outtake) {
+        this.outtake = outtake;
+        addRequirements(outtake);
+    }
+
+    @Override
+    public void initialize() {
+        outtake.setMotor(0.7);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return outtake.coralLoaded();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        outtake.setMotor(0.02);
+    }
+
+
+}

diff --git a/src/main/java/frc/robot/commands/test_comm/PoseTransform.java b/src/main/java/frc/robot/commands/test_comm/PoseTransform.java
new file mode 100644 (file)
index 0000000..94b969b
--- /dev/null
+++ b/src/main/java/frc/robot/commands/test_comm/PoseTransform.java
@@ -0,0 +1,59 @@
+package frc.robot.commands.test_comm;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.TestConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/**
+ * Tests the odometry of the robot by driving a certain distance and calculating the error.
+ */
+public class PoseTransform extends Command {
+
+    private final Drivetrain drive;
+
+    private double startTime;
+    private Pose2d finalPose;
+    private final Transform2d distanceToMove;
+    private Pose2d error;
+
+    public PoseTransform(Drivetrain drive, Transform2d poseTransform) {
+        this.drive = drive;
+        // finalPose is position after robot moves from current position-- startPose-- by the values that are inputted-- distanceToMove
+        distanceToMove = poseTransform;
+
+        addRequirements(drive);
+    }
+
+    @Override
+    public void initialize() {
+        startTime = Timer.getFPGATimestamp();
+        finalPose = drive.getPose().transformBy(distanceToMove);
+    }
+
+    @Override
+    public void execute() {
+        drive.driveWithPID(finalPose.getX(), finalPose.getY(), finalPose.getRotation().getRadians());
+    }
+
+    @Override
+    public boolean isFinished() {
+        double errorMarginMeters = TestConstants.POSE_TRANSFORM_TRANSLATION_ERROR;
+        double errorMarginRadians = Units.degreesToRadians(10);
+        error = drive.getPose().relativeTo(finalPose);
+        // if robot thinks its precision is < 0.1 to the target we inputted, it will stop, so then we can see how off it is
+        return Math.abs(error.getX()) < errorMarginMeters && Math.abs(error.getY()) < errorMarginMeters && Math.abs(error.getRotation().getRadians()) < errorMarginRadians;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        drive.stop();
+        System.out.println(Timer.getFPGATimestamp() - startTime);
+        System.out.println(error.getX());
+        System.out.println(error.getY());
+        System.out.println(error.getRotation().getRadians());
+    }
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/commands/test_comm/PoseTransformTest.java b/src/main/java/frc/robot/commands/test_comm/PoseTransformTest.java
new file mode 100644 (file)
index 0000000..3e1a535
--- /dev/null
+++ b/src/main/java/frc/robot/commands/test_comm/PoseTransformTest.java
@@ -0,0 +1,67 @@
+package frc.robot.commands.test_comm;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.TestConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/**
+ * Tests the odometry of the robot by driving a certain distance and calculating the error.
+ */
+public class PoseTransformTest extends Command {
+
+    private final Drivetrain drive;
+
+    private double startTime;
+    private Pose2d finalPose;
+    private Pose2d error;
+    private double x;
+    private double y;
+    private double rotation;
+
+    public PoseTransformTest(Drivetrain drive, double x, double y, double rotation) {
+        this.drive = drive;
+        // finalPose is position after robot moves from current position-- startPose-- by the values that are inputted-- distanceToMove
+        this.x =x;
+        this.y =y;
+        this.rotation = rotation;
+        addRequirements(drive);
+    }
+
+    @Override
+    public void initialize() {
+        startTime = Timer.getFPGATimestamp();
+        finalPose = drive.getPose().transformBy(new Transform2d(
+               new Translation2d(x, y),
+               Rotation2d.fromDegrees(rotation)
+       ));
+    }
+
+    @Override
+    public void execute() {
+        drive.driveWithPID(finalPose.getX(), finalPose.getY(), finalPose.getRotation().getRadians());
+    }
+
+    @Override
+    public boolean isFinished() {
+        double errorMarginMeters = TestConstants.POSE_TRANSFORM_TRANSLATION_ERROR;
+        double errorMarginRadians = Units.degreesToRadians(10);
+        error = drive.getPose().relativeTo(finalPose);
+        // if robot thinks its precision is < 0.1 to the target we inputted, it will stop, so then we can see how off it is
+        return Math.abs(error.getX()) < errorMarginMeters && Math.abs(error.getY()) < errorMarginMeters && Math.abs(error.getRotation().getRadians()) < errorMarginRadians;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        drive.stop();
+        System.out.println(Timer.getFPGATimestamp() - startTime);
+        System.out.println(error.getX());
+        System.out.println(error.getY());
+        System.out.println(error.getRotation().getRadians());
+    }
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/commands/vision/AcquireGamePiece.java b/src/main/java/frc/robot/commands/vision/AcquireGamePiece.java
new file mode 100644 (file)
index 0000000..ba4f07a
--- /dev/null
+++ b/src/main/java/frc/robot/commands/vision/AcquireGamePiece.java
@@ -0,0 +1,28 @@
+package frc.robot.commands.vision;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.gpm.IntakeCoral;
+import frc.robot.subsystems.arm.Arm;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.subsystems.indexer.Indexer;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.outtake.Outtake;
+import frc.robot.util.Vision.DetectedObject;
+
+public class AcquireGamePiece extends SequentialCommandGroup {
+    /**
+     * Intakes a game piece
+     * 
+     * @param gamePiece The supplier for the game piece to intake
+     * @param drive The drivetrain
+     * @param intake The intake
+     * @param index The indexer
+     * @param arm The arm
+     */
+    public AcquireGamePiece(Supplier<DetectedObject> gamePiece, Drivetrain drive, Intake intake, Indexer indexer, Elevator elevator, Outtake outtake, Arm arm){
+        addCommands(new IntakeCoral(intake, indexer, elevator, outtake, arm).deadlineFor(new DriveToCoral(gamePiece, drive)));
+    }
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/commands/vision/AimAtTag.java b/src/main/java/frc/robot/commands/vision/AimAtTag.java
new file mode 100644 (file)
index 0000000..50586e0
--- /dev/null
+++ b/src/main/java/frc/robot/commands/vision/AimAtTag.java
@@ -0,0 +1,88 @@
+package frc.robot.commands.vision;
+
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.FieldConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/**
+ * Aims the robot at the closest April tag
+ */
+public class AimAtTag extends Command {
+  Drivetrain m_drive;
+  PIDController m_pid;
+
+  /**
+   * Aims the robot at the closest April tag
+   * @param drive The drivetrain
+   */
+  public AimAtTag(Drivetrain drive){
+    m_drive = drive;
+    // Copy drive PID and changetolerance
+    m_pid = new PIDController(
+      m_drive.getRotationController().getP(),
+      m_drive.getRotationController().getI(),
+      m_drive.getRotationController().getD()
+    );
+    m_pid.setTolerance(Units.degreesToRadians(1));
+    addRequirements(drive);
+  }
+
+  /**
+   * Gets the closest tag and sets the setpoint to aim at it
+   */
+  @Override
+  public void initialize(){
+    double dist = Double.POSITIVE_INFINITY;
+    Translation2d closest = new Translation2d();
+    Translation2d driveTranslation = m_drive.getPose().getTranslation();
+    for(AprilTag tag : FieldConstants.APRIL_TAGS){
+      Translation2d translation = tag.pose.toPose2d().getTranslation();
+      double dist2 = driveTranslation.getDistance(translation);
+      if(dist2 < dist){
+        dist = dist2;
+        closest = translation;
+      }
+    }
+    m_pid.reset();
+    m_pid.setSetpoint(Math.atan2(closest.getY() - driveTranslation.getY(), closest.getX() - driveTranslation.getX()));
+  }
+  
+  /**
+   * Runs the PID
+   */
+  @Override
+  public void execute() {
+    double angle = m_drive.getPose().getRotation().getRadians();
+    // If the distance between the angles is more than 180 degrees, use an identical angle ±360 degrees
+    if(angle - m_pid.getSetpoint() > Math.PI){
+      angle -= 2*Math.PI;
+    }else if(angle - m_pid.getSetpoint() < -Math.PI){
+      angle += 2*Math.PI;
+    }
+    double speed = m_pid.calculate(angle);
+    m_drive.drive(0, 0, speed, true, false);
+  }
+
+  /**
+   * Stops the drivetrain
+   * @param interrupted If the command is interrupted
+   */
+  @Override
+  public void end(boolean interrupted) {
+    m_drive.stop();
+  }
+
+  /**
+   * Returns if the command is finished
+   * @return If the PID is at the setpoint
+   */
+  @Override
+  public boolean isFinished() {
+    return m_pid.atSetpoint();
+  }
+}
+

diff --git a/src/main/java/frc/robot/commands/vision/CalculateStdDevs.java b/src/main/java/frc/robot/commands/vision/CalculateStdDevs.java
new file mode 100644 (file)
index 0000000..eea25aa
--- /dev/null
+++ b/src/main/java/frc/robot/commands/vision/CalculateStdDevs.java
@@ -0,0 +1,124 @@
+package frc.robot.commands.vision;
+
+import java.util.ArrayList;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.MathUtils;
+import frc.robot.util.Vision.Vision;
+
+/**
+ * Calculates standard deviations for vision
+ */
+public class CalculateStdDevs extends Command {
+  private final Vision m_vision;
+  private ArrayList<Pose2d> m_poses;
+  private int m_arrayLength;
+  private Timer m_endTimer;
+  private Drivetrain m_drive;
+
+  /**
+   * Constructor for CalculateStdDevs
+   * @param posesToUse the amount of poses to take the standard deviation of. More poses will take more time.
+   * @param vision The vision
+   */
+  public CalculateStdDevs(int posesToUse, Vision vision, Drivetrain drive) {
+    m_vision = vision;
+    m_drive = drive;
+    m_arrayLength = posesToUse;
+    m_endTimer = new Timer();
+  }
+
+  /**
+   * Resets the pose array
+   */
+  @Override
+  public void initialize() {
+    // create the ArrayList of poses to store
+    // an ArrayList prevents issues if the command ends early, and makes checking if the command has finished easy
+    m_poses = new ArrayList<Pose2d>();
+
+    m_drive.setVisionEnabled(false);
+  }
+
+  /**
+   * Adds a pose to the array
+   */
+  @Override
+  public void execute() {
+    Pose2d pose = m_vision.getPose2d(m_drive.getPose());
+    // If the pose exists, add it to the first open spot in the array
+    if (pose != null) {
+      // if we see a pose, reset the timer (it will be started the next time it doesn't get a pose)
+      m_endTimer.stop();
+      m_endTimer.reset();
+      // add the pose to our data
+      m_poses.add(pose);
+      if(m_poses.size()%10==0){
+        System.out.printf("%.0f%% done\n", ((double)m_poses.size())/m_arrayLength * 100);
+      }
+    } else {
+      m_endTimer.start();
+      // If kStdDevCommandEndTime seconds have passed since it saw an April tag, stop the command
+      // Prevents it from running forever
+      if (m_endTimer.hasElapsed(10)) {
+        cancel();
+      }
+    }
+  }
+
+  /**
+   * Calculates the standard deviation
+   */
+  @Override
+  public void end(boolean interrupted) {
+    m_drive.setVisionEnabled(true);
+
+    // If the array is empty, don't try to calculate std devs
+    if (m_poses.size() == 0) {
+      System.out.println("There are no poses in the array\nTry again where the robot can see an April tag.");
+      return;
+    }
+    
+    // create arrays of the poses by X, Y, and Rotation for calculations
+    double[] xArray = new double[m_poses.size()];
+    double[] yArray = new double[m_poses.size()];
+    double[] rotArray = new double[m_poses.size()];
+
+    // copy the values into the arrays
+    for (int i = 0; i < m_poses.size(); i++) {
+      xArray[i] = m_poses.get(i).getX();
+      yArray[i] = m_poses.get(i).getY();
+      rotArray[i] = m_poses.get(i).getRotation().getRadians();
+    }
+
+    // Calculate the standard deviations
+    double stdDevX = MathUtils.stdDev(xArray);
+    double stdDevY = MathUtils.stdDev(yArray);
+    double stdDevRot = MathUtils.stdDev(rotArray);
+    
+    // Find distance to tag
+    double distance;
+    try{
+      distance = m_vision.getEstimatedPoses(m_drive.getPose()).get(0).targetsUsed.get(0).getBestCameraToTarget().getTranslation().getNorm();
+    }catch(Exception e){
+        System.out.println("Could not see a target");
+        distance = -1;
+    }
+    
+    // Print and log values
+    System.out.printf("Standard deviation values:\nX: %.5f\nY: %.5f\nRotation: %.5f\nDistance: %.5f\n",
+      stdDevX, stdDevY, stdDevRot, distance);
+  }
+
+  /**
+   * Returns if the command is finished
+   * @return If the array is full
+   */
+  @Override
+  public boolean isFinished() {
+    return m_poses.size() >= m_arrayLength;
+  }
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/commands/vision/DriveToCoral.java b/src/main/java/frc/robot/commands/vision/DriveToCoral.java
new file mode 100644 (file)
index 0000000..db4c0b4
--- /dev/null
+++ b/src/main/java/frc/robot/commands/vision/DriveToCoral.java
@@ -0,0 +1,59 @@
+package frc.robot.commands.vision;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import frc.robot.commands.drive_comm.DriveToPose;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.Vision.DetectedObject;
+
+/**
+ * Moves toward the detected object
+ * <p>Only works with the front camera
+ */
+public class DriveToCoral extends DriveToPose {
+  private static final boolean constantUpdate = false;
+
+  private static Pose2d tempPose;
+
+  private Supplier<DetectedObject> objectSupplier;
+
+  /**
+   * Moves toward the detected object
+   * @param detectedObject The supplier for the detected object to use
+   * @param drive The drivetrain
+   */
+  public DriveToCoral(Supplier<DetectedObject> detectedObject, Drivetrain drive) {
+    super(drive,
+      constantUpdate
+        ? () -> getPose(detectedObject)
+        : () -> tempPose);
+    objectSupplier = detectedObject;
+    updateTarget = constantUpdate;
+  }
+
+  public static Pose2d getPose(Supplier<DetectedObject> supplier){
+    DetectedObject object = supplier.get();
+    if(object == null) return null;
+    return new Pose2d(object.pose.toPose2d().getTranslation(), new Rotation2d(object.getAngle()+Math.PI/2));
+  }
+
+  @Override
+  public void initialize(){
+    // Set the static variable so the super class has access to it
+    if(constantUpdate){
+      tempPose = getPose(objectSupplier);
+    }
+    super.initialize();
+  }
+
+  @Override
+  public void execute(){
+    // Set the static variable so the super class has access to it
+    if(constantUpdate){
+      tempPose = getPose(objectSupplier);
+    }
+    super.execute();
+  }
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/commands/vision/GoToPose2.java b/src/main/java/frc/robot/commands/vision/GoToPose2.java
new file mode 100644 (file)
index 0000000..cec124a
--- /dev/null
+++ b/src/main/java/frc/robot/commands/vision/GoToPose2.java
@@ -0,0 +1,78 @@
+package frc.robot.commands.vision;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+public class GoToPose2 extends Command {
+    private static final double MIN_ACCEL = 2;
+    private final Supplier<Pose2d> poseSupplier;
+    private final Drivetrain drive;
+    private Pose2d pose;
+    private double vx;
+    private double vy;
+    private Pose2d error;
+
+    public GoToPose2(Supplier<Pose2d> poseSupplier, Drivetrain drive){
+        this.poseSupplier = poseSupplier;
+        this.drive = drive;
+        addRequirements(drive);
+    }
+
+    @Override
+    public void initialize(){
+        pose = poseSupplier.get();
+        ChassisSpeeds v = drive.getChassisSpeeds();
+        vx = v.vxMetersPerSecond;
+        vy = v.vyMetersPerSecond;
+        error = null;
+    }
+
+    @Override
+    public void execute(){
+        if(pose == null){
+            return;
+        }
+        Pose2d drivePose = drive.getPose();
+        error = drivePose.relativeTo(pose);
+        double ax = calcAccel(vx, error.getX());
+        double ay = calcAccel(vy, error.getY());
+        if(Math.abs(ax) < MIN_ACCEL && Math.abs(error.getX()) > 0.01){
+            ax = -Math.signum(error.getX())*MIN_ACCEL;
+        }
+        if(Math.abs(ay) < MIN_ACCEL && Math.abs(error.getY()) > 0.01){
+            ay = -Math.signum(error.getY())*MIN_ACCEL;
+        }
+        vx += ax*Constants.LOOP_TIME;
+        vy += ay*Constants.LOOP_TIME;
+        Translation2d v = new Translation2d(vx, vy).rotateBy(pose.getRotation());
+        drive.driveHeading(v.getX(), v.getY(), pose.getRotation().getRadians(), true);
+    }
+
+    @Override
+    public void end(boolean interrupted){
+        drive.stop();
+    }
+
+    @Override
+    public boolean isFinished(){
+        return pose == null || error != null && error.getTranslation().getNorm() < 0.01;
+    }
+
+    private double calcAccel(double v, double x){
+        if(Math.abs(x) < 0.001 || Math.abs(Math.signum(v) - Math.signum(x)) < 0.5){
+            return 0;
+        }
+        double a = v*v/2/x;
+        double a2 = -v/Constants.LOOP_TIME;
+        if(Math.abs(a2) < Math.abs(a)){
+            return a2;
+        }
+        return a;
+    }
+}

diff --git a/src/main/java/frc/robot/commands/vision/ReturnData.java b/src/main/java/frc/robot/commands/vision/ReturnData.java
new file mode 100644 (file)
index 0000000..ae65672
--- /dev/null
+++ b/src/main/java/frc/robot/commands/vision/ReturnData.java
@@ -0,0 +1,84 @@
+package frc.robot.commands.vision;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.VisionConstants;
+import frc.robot.util.Vision.DetectedObject;
+import frc.robot.util.Vision.Vision;
+
+/**
+ * Adds data from object detection vision to SmartDashboard
+ */
+public class ReturnData extends Command{
+  private final Vision m_vision; 
+  private final Timer timer = new Timer();
+
+  /**
+   * Adds data from object detection vision to Smartdashboard
+   * @param vision The vision
+   */
+  public ReturnData(Vision vision){
+    m_vision = vision;
+  }
+
+  @Override
+  public void initialize(){
+    timer.reset();
+    timer.start();
+  }
+  
+  /**
+   * Adds the data to SmartDashboard
+   */
+  @Override
+  public void execute() {
+    if(timer.advanceIfElapsed(2)){
+        double[] xOffset = m_vision.getHorizontalOffset();
+        double[] yOffset = m_vision.getVerticalOffset();
+        // long[] objectClass = m_vision.getDetectedObjectClass();
+
+        // //put the offsets and area on SmartDashboard for testing 
+        // SmartDashboard.putNumberArray("Object X offsets degrees", xOffset); 
+        // SmartDashboard.putNumberArray("Object Y offsets degrees", yOffset); 
+        // SmartDashboard.putNumberArray("Object Distances", m_vision.getDistance()); 
+
+        DetectedObject bestGamePiece = m_vision.getBestGamePiece(Math.PI, false);
+        if(bestGamePiece!=null){
+          // SmartDashboard.putString("Vision best game piece", bestGamePiece.toString());
+          System.out.println("\n\nBest game piece: "+bestGamePiece);
+        }
+        
+        if ((xOffset.length != 0) == (yOffset.length != 0)) {
+          for (int i = 0; i < xOffset.length; i++) {
+            System.out.printf("\nx: %.2f, y: %.2f, type: %s\n", xOffset[i], yOffset[i], DetectedObject.getType(0));
+            DetectedObject object = new DetectedObject(Units.degreesToRadians(xOffset[i]), Units.degreesToRadians(yOffset[i]), 0, VisionConstants.OBJECT_DETECTION_CAMERAS.get(0));
+            System.out.printf("Object: %s\nDistance: %.2f, Angle: %.2f\n", object, object.getDistance(), Units.radiansToDegrees(object.getAngle()));
+          }
+        }else {
+          System.out.println("One of the arrays is empty!");
+        }
+      }
+    }
+
+  /**
+   * Does nothing
+   * @param interrupted If the command is interrupted
+   */
+  @Override
+  public void end(boolean interrupted) {
+
+  }
+
+  /**
+   * Returns if the command is finished
+   * @retrun Always false (command never finishes)
+   */
+  @Override
+  public boolean isFinished() {
+    return false; 
+  }
+
+
+}
+

diff --git a/src/main/java/frc/robot/commands/vision/TestVisionDistance.java b/src/main/java/frc/robot/commands/vision/TestVisionDistance.java
new file mode 100644 (file)
index 0000000..1810c13
--- /dev/null
+++ b/src/main/java/frc/robot/commands/vision/TestVisionDistance.java
@@ -0,0 +1,113 @@
+package frc.robot.commands.vision;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.Vision.Vision;
+
+/**
+ * Gathers data on the distance limits of the camera used for vision.
+ */
+public class TestVisionDistance extends Command {
+  private final Drivetrain m_drive;
+  private final Vision m_vision;
+  private Translation2d m_visionStartTranslation, m_driveStartTranslation;
+  private Pose2d m_currentPose = null;
+  private double m_driveDistance;
+  private double m_visionDistance;
+
+  private double m_speed;
+
+  private final Timer m_endTimer = new Timer();
+  private final Timer m_printTimer = new Timer();
+
+  // How many seconds of not seeing april tag before ending the command
+  private static final double kEndDelay = 0.25;
+
+  // How many seconds between each data print
+  private static final double kPrintDelay = 1;
+
+  /**
+   * Constructor for TestVisionDistance
+   * @param speed What speed to move at, negative if backward
+   * @param drive The drivetrain
+   * @param vision The vision
+   */
+  public TestVisionDistance(double speed, Drivetrain drive, Vision vision){
+    addRequirements(drive);
+    m_drive = drive;
+    m_speed = speed;
+    m_vision = vision;
+  }
+
+  /**
+   * Starts the timers and disables vision for odometry
+   */
+  @Override
+  public void initialize() {
+
+    m_endTimer.reset();
+    m_printTimer.restart();
+
+    m_drive.setVisionEnabled(false);
+
+    m_currentPose = m_vision.getPose2d(m_drive.getPose());
+    m_visionStartTranslation = m_currentPose.getTranslation();
+    m_driveStartTranslation = m_drive.getPose().getTranslation();
+    m_driveDistance = 0;
+    m_visionDistance = 0;
+  }
+
+  /**
+   * Drives the robot, finds the pose from the drivetrain and vision, and someimes prints the distances
+   */
+  @Override
+  public void execute() {
+    m_drive.drive(m_speed, 0, 0, false, false);
+    Pose2d newestPose = m_vision.getPose2d(m_currentPose, m_drive.getPose());
+
+    // If the camera can see the apriltag
+    if (newestPose != null) {
+      //update current pose
+      m_currentPose = newestPose;
+      // reset the timer
+      m_endTimer.reset();
+      m_driveDistance = m_drive.getPose().getTranslation().getDistance(m_driveStartTranslation);
+      m_visionDistance = m_currentPose.getTranslation().getDistance(m_visionStartTranslation);
+      SmartDashboard.putNumber("Vision test drive distance", m_driveDistance);
+      SmartDashboard.putNumber("Vision test vision distnace", m_visionDistance);
+      SmartDashboard.putNumber("Vision test error", m_visionDistance - m_driveDistance);
+      SmartDashboard.putNumber("Vision test % error", (m_visionDistance-m_driveDistance) / m_driveDistance * 100);
+      // If kPrintDelay seconds have passed, print the data
+      if (m_printTimer.advanceIfElapsed(kPrintDelay)) {
+        System.out.printf("\nDrive dist: %.2f\nVision dist: %.2f\nError: %.2f\n %% error: %.2f\n",
+          m_driveDistance, m_visionDistance,
+          m_visionDistance-m_driveDistance, (m_visionDistance-m_driveDistance) / m_driveDistance * 100
+        );
+      }
+    } else {
+      m_endTimer.start();
+    }
+  }
+
+  /**
+   * Re-enables vision and stops the robot
+   */
+  @Override
+  public void end(boolean interrupted) {
+    m_drive.setVisionEnabled(true);
+    m_drive.stop();
+  }
+
+  /**
+   * Returns if the command is finished
+   * @return If the end delay has elapsed
+   */
+  @Override
+  public boolean isFinished() {
+    return m_endTimer.hasElapsed(kEndDelay);
+  }
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java
new file mode 100644 (file)
index 0000000..bc0be45
--- /dev/null
+++ b/src/main/java/frc/robot/constants/ArmConstants.java
@@ -0,0 +1,52 @@
+package frc.robot.constants;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+
+public class ArmConstants {
+    // Degrees
+    public static final double START_ANGLE = -86.5;
+    public static final double MIN_ANGLE = -86.5;
+    public static final double MAX_ANGLE = 180;
+    public static final double OFFSET = 0 - START_ANGLE;
+
+    public static final double GEAR_RATIO = 29.36;
+    public static final double ENCODER_GEAR_RATIO = 50.0/24.0;
+    public static final DCMotor MOTOR = DCMotor.getKrakenX60(1);
+
+    public static final double MASS = 2.46; // kilograms
+    public static final double LENGTH = 0.138*2; // meters
+    public static final double MOI = 0.0261057394; // kg*m^2
+    public static final double CENTER_OF_MASS_LENGTH = 0.138; // meters
+
+    public static final double MAX_VELOCITY = 21; // rad/s
+    public static final double MAX_ACCELERATION = 120; // rad/s^2
+
+    public static final double INTAKE_SETPOINT = START_ANGLE;
+    public static final double STATION_INTAKE_SETPOINT = 75.5;
+
+    public static final double TOLERANCE = 3.0;
+
+    //Dunk L4 = 6.4
+    public static final double L1_SETPOINT = 50;
+
+    //4 in offset
+    // public static final double L4_SETPOINT = 11;
+    // Original L4: 7.5 degrees
+    public static final double L4_SETPOINT_RIGHT = 7.5;
+    public static final double L4_SETPOINT_LEFT = 6;
+    public static final double L2_L3_SETPOINT = 21.25;
+
+    //touching reef
+    public static final double L4_SETPOINT_ALT = 4.5;
+    public static final double L2_L3_SETPOINT_ALT = 12.23;
+
+    public static final double ALGAE_SETPOINT = -19.37;
+    public static final double ALGAE_NET_SETPOINT_1 = 85.0;
+    public static final double ALGAE_NET_SETPOINT_2 = 25;
+    public static final double ALGAE_STOW_SETPOINT  = 50;
+    public static final double ALGAE_LOLI_SETPOINT  = -19;
+    
+    public static final double PROCESSOR_SETPOINT = -65.0;
+
+    public static final double STOW_SETPOINT = -14.0;
+}

diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java
index 956250b138bba8dd1794caa08a2bec91deab152a..939cb0784ab00512d3cffa12997d5b890bbce4a5 100644 (file)
--- a/src/main/java/frc/robot/constants/AutoConstants.java
+++ b/src/main/java/frc/robot/constants/AutoConstants.java
@@ -1,10 +1,39 @@
 package frc.robot.constants;
 
+import com.pathplanner.lib.config.ModuleConfig;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import frc.robot.constants.swerve.DriveConstants;
+
+/**
+ * Container class for auto constants.
+ */
 public class AutoConstants {
 
-  // Pathplanner output folder should be src/main/deploy/pathplanner
-  public final String kTrajectoryDirectory = "pathplanner/";
+    // Pathplanner output folder should be src/main/deploy/pathplanner
+    public static final String TRAJECTORY_DIRECTORY = "pathplanner/paths/";
+
+    public static final double MAX_AUTO_SPEED = 5.2; // m/s
+    public static final double MAX_AUTO_ACCEL = 4.8; // m/s^2
+
+    public static RobotConfig  CONFIG;
+    public static final PPHolonomicDriveController AUTO_CONTROLLER = new PPHolonomicDriveController( 
+            new PIDConstants(4.0, 0.0, 0), // Translation PID constants
+            new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
+        );
+   
 
-  public final double kMaxAutoSpeed = 1.0; // m/s
-  public final double kMaxAutoAccel = 1.0; // m/s^2
+        
+    static {
+        try{
+            CONFIG = RobotConfig.fromGUISettings();
+        }catch(Exception e){
+            e.printStackTrace();
+            // Although these values are probably wrong and auto might not work correctly, at least it won't cause NullPointerExceptions
+            CONFIG = new RobotConfig(50, 0.5, new ModuleConfig(DriveConstants.WHEEL_RADIUS, MAX_AUTO_SPEED, DriveConstants.COSF, DCMotor.getKrakenX60(1).withReduction(DriveConstants.DRIVE_GEAR_RATIO), DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT, 1), DriveConstants.MODULE_LOCATIONS);
+        }
+    }
 }

diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java
index e19f2d75872fc761c108c516473e7aee618f5e0e..bc90b34943247ec3386ad0dfdc165cd3ad9c8e8a 100644 (file)
--- a/src/main/java/frc/robot/constants/Constants.java
+++ b/src/main/java/frc/robot/constants/Constants.java
@@ -1,18 +1,79 @@
 package frc.robot.constants;
 
-public final class Constants {
-  public static final double kGravitationalAccel = 9.8;
-  public static final double kMaxVoltage = 12.0;
-  public static final double kLoopTime = 0.02;
-  
-  public static final double kCancoderResolution = 4096;
-
-  // CAN bus names
-  // public static final String kCanivoreCAN = ""; // replace this if using Canivore
-  public static final String kRioCAN = "rio";
-
-  public static final OIConstants oi = new OIConstants();
-  public static final DriveConstants drive = new DriveConstants();
-  public static final AutoConstants auto = new AutoConstants();
-  public static final FalconConstants falcon = new FalconConstants();
+import edu.wpi.first.wpilibj.RobotBase;
+
+public class Constants {
+
+    // constants:   
+
+    public static final double GRAVITY_ACCELERATION = 9.8;
+    public static final double ROBOT_VOLTAGE = 12.0;
+    public static final double LOOP_TIME = 0.02;
+
+    // CAN bus names
+    public static final String CANIVORE_CAN = "CANivore";
+    public static final String RIO_CAN = "rio";
+
+    // Logging 
+    public static final boolean USE_TELEMETRY = true;
+
+    public static enum Mode {
+        /** Running on a real robot. */
+        REAL,
+
+        /** Running a physics simulator. */
+        SIM,
+
+        /** Replaying from a log file. */
+        REPLAY
+    }
+
+    // Kraken Speed
+    public static double MAX_RPM = 5800.0; // Rotations per minute
+
+    /*
+     * Talon Stator / Supply Limits explanation
+     * Supply current is current that's being drawn at the input bus voltage. Stator
+     * current is current that's being drawn by the motor.
+     * Supply limiting (supported by Talon FX and SRX) is useful for preventing
+     * breakers from tripping in the PDP.
+     * Stator limiting (supported by Talon FX) is useful for limiting
+     * acceleration/heat.
+     */
+
+    // These are the default values
+
+    // Stator
+    public static final boolean TALONFX_STATOR_LIMIT_ENABLE = false; // enabled?
+    public static final double TALONFX_STATOR_CURRENT_LIMIT = 100; // Limit(amp)
+    public static final double TALONFX_STATOR_TRIGGER_THRESHOLD = 100; // Trigger Threshold(amp)
+    public static final double TALONFX_STATOR_TRIGGER_DURATION = 0; // Trigger Threshold Time(s)
+
+    // Supply
+    public static final boolean TALONFX_SUPPLY_LIMIT_ENABLE = false; // enabled?
+    public static final double TALONFX_SUPPLY_CURRENT_LIMIT = 40; // Limit(amp), current to hold after trigger hit
+    public static final double TALONFX_SUPPLY_TRIGGER_THRESHOLD = 55; // (amp), amps to activate trigger
+    public static final double TALONFX_SUPPLY_TRIGGER_DURATION = 3; // (s), how long after trigger before reducing
+
+    // OIConstants:
+
+    public static final int DRIVER_JOY = 0;
+    public static final int OPERATOR_JOY = 1;
+    public static final int TEST_JOY = 2;
+    public static final int MANUAL_JOY = 3;
+    public static final double DEFAULT_DEADBAND = 0.00005;
+
+    public static final double TRANSLATIONAL_DEADBAND = 0.01;
+    
+    public static final double ROTATION_DEADBAND = 0.01;
+    
+    public static final double HEADING_DEADBAND = 0.05;
+    public static final double HEADING_SLEWRATE = 10;
+
+    //Modes
+    public static final Mode SIM_MODE = Mode.REPLAY;
+    public static final Mode CURRENT_MODE = RobotBase.isReal() ? Mode.REAL : SIM_MODE;
+
+    // Enables 3D logs of mechanisms
+    public static final boolean LOG_MECHANISMS = true;
 }

diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java
deleted file mode 100644 (file)
index d19eb8f..0000000
--- a/src/main/java/frc/robot/constants/DriveConstants.java
+++ /dev/null
@@ -1,9 +0,0 @@
-package frc.robot.constants;
-
-public class DriveConstants {
-
-  public final int kLeftMotor1 = -1;
-  public final int kLeftMotor2 = -1;
-  public final int kRightMotor1 = -1;
-  public final int kRightMotor2 = -1;
-}

diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java
new file mode 100644 (file)
index 0000000..be34322
--- /dev/null
+++ b/src/main/java/frc/robot/constants/ElevatorConstants.java
@@ -0,0 +1,59 @@
+// 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 frc.robot.constants;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+
+/** Stores constants for the elevator. */
+public class ElevatorConstants {
+    public static final double ANGLE = Units.degreesToRadians(3.5); // radians, from vertical
+    public static final DCMotor MOTOR = DCMotor.getKrakenX60(1);
+    public static final int NUMBER_OF_STAGES = 3;
+    public static final double GEARING = 8.333/NUMBER_OF_STAGES;
+    public static final double MIN_HEIGHT = 0.0; // meters
+    public static final double MAX_HEIGHT = Units.inchesToMeters(66);//Units.inchesToMeters(48); // meters
+    public static final double START_HEIGHT = MIN_HEIGHT; // meters
+    public static final double CARRIAGE_MASS = 3; // kilograms 2.49475803
+    public static final double DRUM_RADIUS = Units.inchesToMeters(1.281/2); // meters
+    public static final double SPRING_FORCE = 0; //Newtons
+
+    public static final double BOTTOM_LIMIT_SWITCH_HEIGHT = 0;//0.015; // meters
+    public static final double TOP_LIMIT_SWITCH_HEIGHT = MAX_HEIGHT; // meters
+    public static final double SIM_LIMIT_SWITCH_TRIGGER_DISTANCE = 0.01; // meters
+
+    public static final double STOW_SETPOINT = 0;
+    public static final double INTAKE_SETPOINT = 0.036;
+    public static final double SAFE_SETPOINT = 0.225;
+    public static final double INTAKE_STOW_SETPOINT = 0.58;
+    
+    //4 inch offset
+    public static final double L1_SETPOINT = 0.0;
+    public static final double L2_SETPOINT = 0.523;
+    public static final double L3_SETPOINT = 0.9192;
+    // public static final double L4_SETPOINT = 1.675;
+    public static final double L4_SETPOINT = 1.675;
+
+    //touching reef
+    public static final double L1_SETPOINT_ALT = 0.27;
+    public static final double L2_SETPOINT_ALT = 0.588;
+    public static final double L3_SETPOINT_ALT = 0.98-0.0254-0.01;
+    public static final double L4_SETPOINT_ALT = 1.675;
+    //Dunk L4 = 1.5
+
+    public static final double BOTTOM_ALGAE_SETPOINT = 0.405;
+    public static final double TOP_ALGAE_SETPOINT = 0.799;
+
+    public static final double NET_SETPOINT = MAX_HEIGHT;
+    public static final double STATION_INTAKE_SETPOINT = 0.233;
+
+
+    public static final double CENTER_OF_MASS_HEIGHT_STOWED = Units.inchesToMeters(9.44);
+    public static final double CENTER_OF_MASS_HEIGHT_EXTENDED = Units.inchesToMeters(10+14.767);
+
+
+    // The x distance from the center of the robot to the outtake.
+    public static final double OUTTAKE_X = Units.inchesToMeters(-7.25);
+}

diff --git a/src/main/java/frc/robot/constants/FalconConstants.java b/src/main/java/frc/robot/constants/FalconConstants.java
deleted file mode 100644 (file)
index 5f2edbb..0000000
--- a/src/main/java/frc/robot/constants/FalconConstants.java
+++ /dev/null
@@ -1,36 +0,0 @@
-package frc.robot.constants;
-
-public class FalconConstants {
-
-  // Stored in hex though not really a hex. 21.0 = 0x2100, ex. 1.2 = 0x0102
-  public final int kFirmwareVersion = 0x2100;
-  public final boolean kBreakOnWrongFirmware = true;
-
-  public final double kResolution = 2048;
-  public final double kMaxRpm = 6380.0; // Rotations per minute
-
-  /*
-   * Talon Stator / Supply Limits explanation
-   * Supply current is current that’s being drawn at the input bus voltage. Stator
-   * current is current that’s being drawn by the motor.
-   * Supply limiting (supported by Talon FX and SRX) is useful for preventing
-   * breakers from tripping in the PDP.
-   * Stator limiting (supported by Talon FX) is useful for limiting
-   * acceleration/heat.
-   */
-
-  // These are the default values
-
-  // Stator
-  public final boolean kStatorLimitEnable = false; // enabled?
-  public final double kStatorCurrentLimit = 100; // Limit(amp)
-  public final double kStatorTriggerThreshold = 100; // Trigger Threshold(amp)
-  public final double kStatorTriggerDuration = 0; // Trigger Threshold Time(s)
-
-  // Supply
-  public final boolean kSupplyLimitEnable = false; // enabled?
-  public final double kSupplyCurrentLimit = 40; // Limit(amp), current to hold after trigger hit
-  public final double kSupplyTriggerThreshold = 55; // (amp), amps to activate trigger
-  public final double kSupplyTriggerDuration = 3; // (s), how long after trigger before reducing
-
-}

diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java
new file mode 100644 (file)
index 0000000..03c90e0
--- /dev/null
+++ b/src/main/java/frc/robot/constants/FieldConstants.java
@@ -0,0 +1,46 @@
+package frc.robot.constants;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.util.Units;
+
+public class FieldConstants {
+  /** Width of the field [meters] */
+  public static final double FIELD_LENGTH = Units.inchesToMeters(57*12 + 6+7.0/8.0);
+  /** Height of the field [meters] */
+  public static final double FIELD_WIDTH = Units.inchesToMeters(26*12 + 5);
+
+  /** 
+   * ArrayList of April Tags to use when the field layout is not available.
+   * <p>
+   * Obtain AprilTag ID i with APRIL_TAGS.get(i-1).
+   */
+  public static final ArrayList<AprilTag> APRIL_TAGS = new ArrayList<AprilTag>(List.of(
+    new AprilTag(1 , new Pose3d(Units.inchesToMeters(657.37), Units.inchesToMeters(25.80), Units.inchesToMeters(58.50), new Rotation3d(0, 0, Units.degreesToRadians(126)))),
+    new AprilTag(2 , new Pose3d(Units.inchesToMeters(657.37), Units.inchesToMeters(291.20), Units.inchesToMeters(58.50), new Rotation3d(0, 0, Units.degreesToRadians(234)))),
+    new AprilTag(3 , new Pose3d(Units.inchesToMeters(455.15), Units.inchesToMeters(317.15), Units.inchesToMeters(51.25), new Rotation3d(0, 0, Units.degreesToRadians(270)))),
+    new AprilTag(4 , new Pose3d(Units.inchesToMeters(365.20), Units.inchesToMeters(241.64), Units.inchesToMeters(73.54), new Rotation3d(0, Units.degreesToRadians(30), 0))),
+    new AprilTag(5 , new Pose3d(Units.inchesToMeters(365.20), Units.inchesToMeters(75.39), Units.inchesToMeters(73.54), new Rotation3d(0, Units.degreesToRadians(30), 0))),
+    new AprilTag(6 , new Pose3d(Units.inchesToMeters(530.49), Units.inchesToMeters(130.17), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(300)))),
+    new AprilTag(7 , new Pose3d(Units.inchesToMeters(546.87), Units.inchesToMeters(158.50), Units.inchesToMeters(12.13), new Rotation3d(0, 0, 0))),
+    new AprilTag(8 , new Pose3d(Units.inchesToMeters(530.49), Units.inchesToMeters(186.83), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(60)))),
+    new AprilTag(9 , new Pose3d(Units.inchesToMeters(497.77), Units.inchesToMeters(186.83), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(120)))),
+    new AprilTag(10, new Pose3d(Units.inchesToMeters(481.39), Units.inchesToMeters(158.50), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(180)))),
+    new AprilTag(11, new Pose3d(Units.inchesToMeters(497.77), Units.inchesToMeters(130.17), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(240)))),
+    new AprilTag(12, new Pose3d(Units.inchesToMeters(33.51), Units.inchesToMeters(25.80), Units.inchesToMeters(58.50), new Rotation3d(0, 0, Units.degreesToRadians(54)))),
+    new AprilTag(13, new Pose3d(Units.inchesToMeters(33.51), Units.inchesToMeters(291.20), Units.inchesToMeters(58.50), new Rotation3d(0, 0, Units.degreesToRadians(306)))),
+    new AprilTag(14, new Pose3d(Units.inchesToMeters(325.68), Units.inchesToMeters(241.64), Units.inchesToMeters(73.54), new Rotation3d(0, Units.degreesToRadians(30), Units.degreesToRadians(180)))),
+    new AprilTag(15, new Pose3d(Units.inchesToMeters(325.68), Units.inchesToMeters(75.39), Units.inchesToMeters(73.54), new Rotation3d(0, Units.degreesToRadians(30), Units.degreesToRadians(180)))),
+    new AprilTag(16, new Pose3d(Units.inchesToMeters(235.73), Units.inchesToMeters(-0.15), Units.inchesToMeters(51.25), new Rotation3d(0, 0, Units.degreesToRadians(90)))),
+    new AprilTag(17, new Pose3d(Units.inchesToMeters(160.39), Units.inchesToMeters(130.17), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(240)))),
+    new AprilTag(18, new Pose3d(Units.inchesToMeters(144.00), Units.inchesToMeters(158.50), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(180)))),
+    new AprilTag(19, new Pose3d(Units.inchesToMeters(160.39), Units.inchesToMeters(186.83), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(120)))),
+    new AprilTag(20, new Pose3d(Units.inchesToMeters(193.10), Units.inchesToMeters(186.83), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(60)))),
+    new AprilTag(21, new Pose3d(Units.inchesToMeters(209.49), Units.inchesToMeters(158.50), Units.inchesToMeters(12.13), new Rotation3d(0, 0, 0))),
+    new AprilTag(22, new Pose3d(Units.inchesToMeters(193.10), Units.inchesToMeters(130.17), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(300))))
+  ));
+}

diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java
new file mode 100644 (file)
index 0000000..7302cf0
--- /dev/null
+++ b/src/main/java/frc/robot/constants/IdConstants.java
@@ -0,0 +1,50 @@
+package frc.robot.constants;
+
+import edu.wpi.first.wpilibj.I2C;
+
+public class IdConstants {
+    // Drivetrain
+    public static final int DRIVE_FRONT_LEFT_ID = 1;
+    public static final int STEER_FRONT_LEFT_ID = 2;
+    public static final int ENCODER_FRONT_LEFT_ID = 3;
+    public static final int DRIVE_FRONT_RIGHT_ID = 10;
+    public static final int STEER_FRONT_RIGHT_ID = 11;
+    public static final int ENCODER_FRONT_RIGHT_ID = 12;
+    public static final int DRIVE_BACK_LEFT_ID = 7;
+    public static final int STEER_BACK_LEFT_ID = 8;
+    public static final int ENCODER_BACK_LEFT_ID = 9;
+    public static final int DRIVE_BACK_RIGHT_ID = 4;
+    public static final int STEER_BACK_RIGHT_ID = 5;
+    public static final int ENCODER_BACK_RIGHT_ID = 6;
+    public static final int PIGEON = 13;
+
+    // LEDs
+    public static final int CANDLE_ID = 1;
+
+    // Elevator
+    public static final int ELEVATOR_RIGHT_MOTOR = 50;
+    public static final int ELEVATOR_BOTTOM_LIMIT_SWITCH = 29;
+    public static final int ELEVATOR_TOP_LIMIT_SWITCH = 30;
+
+       // Indexer
+       public static final int INDEXER_MOTOR = 56;
+       public static final int INDEXER_SENSOR = 24;
+
+    // Climb
+    public static final int CLIMB_MOTOR = 31;
+
+    // Intake
+    public static final int INTAKE_ROLLER = 51;
+    public static final int INTAKE_PIVOT = 55; //55
+    public static final int INTAKE_LASER_CAN = 25;
+
+    // Outtake
+    public static final int OUTTAKE_MOTOR_ALPHA = 14;
+    public static final int OUTTAKE_MOTOR_COMP = 30; 
+    public static final int OUTTAKE_DIO_EJECTING = 3;
+    public static final I2C.Port i2cPort = I2C.Port.kMXP;
+
+    //Arm
+    public static final int ARM_MOTOR = 29;
+    public static final int ARM_ABSOLUTE_ENCODER = 5;
+}

diff --git a/src/main/java/frc/robot/constants/IndexerConstants.java b/src/main/java/frc/robot/constants/IndexerConstants.java
new file mode 100644 (file)
index 0000000..d0fdd1b
--- /dev/null
+++ b/src/main/java/frc/robot/constants/IndexerConstants.java
@@ -0,0 +1,16 @@
+package frc.robot.constants;
+
+public class IndexerConstants {
+       public static final double SPEED = 1;
+
+       public static final double MOMENT_OF_INERTIA = 0.000326;
+       public static final double GEAR_RATIO = 1.0;
+       public static final int MEASUREMENT_THRESHOLD = 15; // in millimeters
+
+       // this stuff is for sim, all in meters
+       public static final double WHEEL_CIRCUMFERENCE = Math.PI * 0.1;
+       public static final double START_SIM_POS_AT = -1.0; // start coral here
+       public static final double START_SIM_SENSOR_POS_AT = -0.5; // activate the sensor here
+       public static final double END_SIM_SENSOR_POS_AT = 0.5; // deactivate the sensor here
+}
+

diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java
new file mode 100644 (file)
index 0000000..1939452
--- /dev/null
+++ b/src/main/java/frc/robot/constants/IntakeConstants.java
@@ -0,0 +1,23 @@
+package frc.robot.constants;
+
+/**
+ * Storage class for intake constants
+ */
+public class IntakeConstants {
+    public static final double PIVOT_GEAR_RATIO = 75;
+    public static final double MOMENT_OFiNERTIA = 0.600984136;
+    public static final double CENTER_OF_MASS_DIST = 0.265;
+    public static final double MASS = 6.45688739;
+    public static final double ARM_LENGTH = CENTER_OF_MASS_DIST*2;
+    public static final double DETECT_CORAL_DIST = 0.49;
+    public static final double INTAKE_MOTOR_POWER = 1;
+    public static final double INTAKE_SETPOINT = -30.41;
+    public static final double STOW_SETPOINT = 90;
+    public static final double STATION_SETPOINT = 70;
+    public static final double INTAKE_SAFE_POINT = 30;
+
+    public static final double ALGAE_SETPOINT = 25;
+    public static final double ALGAE_INTAKE_POWER = -0.5;
+    public static final double ALGAE_OUTTAKE_POWER = 0.5;
+   
+}

diff --git a/src/main/java/frc/robot/constants/OIConstants.java b/src/main/java/frc/robot/constants/OIConstants.java
deleted file mode 100644 (file)
index 5ea0f3d..0000000
--- a/src/main/java/frc/robot/constants/OIConstants.java
+++ /dev/null
@@ -1,9 +0,0 @@
-package frc.robot.constants;
-
-// OI stands for Operator Input
-public class OIConstants {
-  public final int kDriverJoy = 0;
-  public final int kOperatorJoy = 1;
-
-  public final double kDeadband = 0.05;
-}

diff --git a/src/main/java/frc/robot/constants/TestConstants.java b/src/main/java/frc/robot/constants/TestConstants.java
new file mode 100644 (file)
index 0000000..51667d6
--- /dev/null
+++ b/src/main/java/frc/robot/constants/TestConstants.java
@@ -0,0 +1,8 @@
+package frc.robot.constants;
+
+/**
+ * Container class for test constants.
+ */
+public class TestConstants {
+    public static final double POSE_TRANSFORM_TRANSLATION_ERROR = 0.6;
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java
new file mode 100644 (file)
index 0000000..c599707
--- /dev/null
+++ b/src/main/java/frc/robot/constants/VisionConstants.java
@@ -0,0 +1,391 @@
+package frc.robot.constants;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.util.Units;
+import frc.robot.constants.swerve.DriveConstants;
+
+/**
+ * Container class for vision constants.
+ */
+public class VisionConstants {
+    /**
+     * If April tag vision is enabled on the robot
+     */
+    public static final boolean ENABLED = true;
+
+    /**
+     * If object detection should be enabled
+     */
+    public static final boolean OBJECT_DETECTION_ENABLED = false;
+
+    /** If odometry should be updated using vision during auto */
+    public static final boolean ENABLED_AUTO = true;
+
+    /**
+     * If odometry should be updated using vision while running the GoToPose,
+     * GoToPosePID, and DriveToPose commands in teleop
+     */
+    public static final boolean ENABLED_GO_TO_POSE = true;
+
+    /** If vision should be simulated */
+    public static final boolean ENABLED_SIM = false;
+
+    /** If vision should only return values if it can see 2 good targets */
+    public static final boolean ONLY_USE_2_TAGS = false;
+
+    /** PoseStrategy to use in pose estimation */
+    public static final PoseStrategy POSE_STRATEGY = PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR;
+
+    /** Fallback PoseStrategy if MultiTag doesn't work */
+    public static final PoseStrategy MULTITAG_FALLBACK_STRATEGY = PoseStrategy.LOWEST_AMBIGUITY;
+
+    /**
+     * Any April tags we always want to ignore. To ignore a tag, put its id in this
+     * array.
+     */
+    public static final int[] TAGS_TO_IGNORE = {12, 13, 16, 1, 2, 3};
+
+    /**
+     * If multiple cameras return different poses, they will be ignored if the
+     * difference between them is greater than this
+     */
+    public static final double MAX_POSE_DIFFERENCE = 0.2;
+
+    /**
+     * The maximum distance to the tag to use
+     */
+    public static final double MAX_DISTANCE = 2;
+
+    /** If vision should use manual calculations */
+    public static final boolean USE_MANUAL_CALCULATIONS = true;
+
+    // <ol start="0"> did not work
+    /**
+     * Which version of driver assist to use. This would be an enum, except there is
+     * no short and descriptive name for all of these.
+     * <p>
+     * The options are:
+     * <p>
+     * 0: Disable driver assist
+     * <p>
+     * 1: Completely remove the component of the driver's input that is not toward
+     * the object
+     * <p>
+     * 2: Interpolate between the next achievable driver speed and a speed
+     * calculated using trapezoid profiles
+     * <p>
+     * 3-5: Add a speed perpendicular to the driver input; there are 3 similar but
+     * different calculations for this
+     */
+    public static final int DRIVER_ASSIST_MODE = 5;
+
+    /**
+     * The number to multiply the distance to the April tag by.
+     * <p>
+     * Only affects manual calculations.
+     * <p>
+     * To find this, set it to 1 and measure the actual distance and the calculated
+     * distance.
+     * <p>
+     * This should not be needed, and it is only here because it improved the
+     * accuracy of vision in the 2023 fall semester
+     */
+    public static final double DISTANCE_SCALE = 1;
+
+    /**
+     * The standard deviations to use for vision
+     */
+    public static final Matrix<N3, N1> VISION_STD_DEVS = VecBuilder.fill(
+            0.3, // x in meters (default=0.9)
+            0.3, // y in meters (default=0.9)
+            0.9 // heading in radians. The gyroscope is very accurate, so as long as it is reset
+                // correctly it is unnecessary to correct it with vision
+    );
+
+    /**
+     * The standard deviations to use for vision when the wheels slip
+     */
+    public static final Matrix<N3, N1> VISION_STD_DEVS_2 = VecBuilder.fill(
+            0.01, // x in meters (default=0.9)
+            0.01, // y in meters (default=0.9)
+            0.9 // heading in radians. The gyroscope is very accurate, so as long as it is reset
+                // correctly it is unnecessary to correct it with vision
+    );
+
+    /**
+     * The highest ambiguity to use. Ambiguities higher than this will be ignored.
+     * <p>
+     * Only affects calculations using PhotonVision, not manual calculations.
+     */
+    public static final double HIGHEST_AMBIGUITY = 0.01;
+
+    /**
+     * The camera poses
+     * <p>
+     * Everything is in meters and radians
+     * <p>
+     * 0 for all numbers is center of the robot, on the ground, looking straight
+     * toward the front
+     * <p>
+     * + X: Front of Robot
+     * <p>
+     * + Y: Left of Robot
+     * <p>
+     * + Z: Top of Robot
+     * <p>
+     * + Pitch: Down
+     * <p>
+     * + Yaw: Counterclockwise
+     */
+    public static final ArrayList<Pair<String, Transform3d>> APRIL_TAG_CAMERAS = new ArrayList<Pair<String, Transform3d>>(
+            List.of(
+                new Pair<String, Transform3d>(
+                        "CameraFront",
+                        new Transform3d(
+                                new Translation3d(Units.inchesToMeters(10.485), Units.inchesToMeters(10.217),
+                                        Units.inchesToMeters(11.012)),
+                                new Rotation3d(0, Units.degreesToRadians(-11),
+                                        Math.PI/2 + Units.degreesToRadians(20)))),
+                new Pair<String, Transform3d>(
+                        "CameraBack",
+                        new Transform3d(
+                                new Translation3d(Units.inchesToMeters(-9.538), Units.inchesToMeters(7.474),
+                                        Units.inchesToMeters(8.719)),
+                                new Rotation3d(0, Units.degreesToRadians(-19.5),
+                                        Math.PI/2-Units.degreesToRadians(25))))));
+
+    /**
+     * The transformations from the robot to object detection cameras
+     */
+    public static final ArrayList<Transform3d> OBJECT_DETECTION_CAMERAS = new ArrayList<>(List.of(
+            new Transform3d(
+                    new Translation3d(Units.inchesToMeters(10), 0, Units.inchesToMeters(24)),
+                    new Rotation3d(0, Units.degreesToRadians(20), 0))));
+
+    // Poses to potentially align to
+    public static final Pose2d RED_PROCESSOR_POSE = new Pose2d(
+            FieldConstants.APRIL_TAGS.get(2).pose.getX(),
+            FieldConstants.APRIL_TAGS.get(2).pose.getY() - DriveConstants.ROBOT_WIDTH_WITH_BUMPERS / 2,
+            new Rotation2d(Math.PI / 2));
+
+    public static final Pose2d BLUE_PROCESSOR_POSE = new Pose2d(
+            FieldConstants.APRIL_TAGS.get(15).pose.getX(),
+            FieldConstants.APRIL_TAGS.get(15).pose.getY() + DriveConstants.ROBOT_WIDTH_WITH_BUMPERS / 2,
+            new Rotation2d(-Math.PI / 2));
+
+    public static final Pose2d BLUE_CAGE_LEFT_POSE = new Pose2d(
+            FieldConstants.FIELD_LENGTH / 2,
+            FieldConstants.FIELD_WIDTH / 2 + Units.inchesToMeters(1019.0 / 8),
+            new Rotation2d());
+
+    public static final Pose2d BLUE_CAGE_MIDDLE_POSE = new Pose2d(
+            FieldConstants.FIELD_LENGTH / 2,
+            FieldConstants.FIELD_WIDTH / 2 + Units.inchesToMeters(675.0 / 8),
+            new Rotation2d());
+
+    public static final Pose2d BLUE_CAGE_RIGHT_POSE = new Pose2d(
+            FieldConstants.FIELD_LENGTH / 2,
+            FieldConstants.FIELD_WIDTH / 2 + Units.inchesToMeters(41.5),
+            new Rotation2d());
+
+    public static final Pose2d RED_CAGE_RIGHT_POSE = new Pose2d(
+            FieldConstants.FIELD_LENGTH / 2,
+            FieldConstants.FIELD_WIDTH / 2 + Units.inchesToMeters(-41.5),
+            new Rotation2d(Math.PI));
+
+    public static final Pose2d RED_CAGE_MIDDLE_POSE = new Pose2d(
+            FieldConstants.FIELD_LENGTH / 2,
+            FieldConstants.FIELD_WIDTH / 2 - Units.inchesToMeters(-675.0 / 8),
+            new Rotation2d(Math.PI));
+
+    public static final Pose2d RED_CAGE_LEFT_POSE = new Pose2d(
+            FieldConstants.FIELD_LENGTH / 2,
+            FieldConstants.FIELD_WIDTH / 2 - Units.inchesToMeters(-1019.0 / 8),
+            new Rotation2d(Math.PI));
+
+   /**
+    * The distance between the AprilTag and algae setpoint in the directin parallel to the face of the reef
+    * Positive is to the left
+    */
+    public static final double ALGAE_X_OFFSET = 0.02;
+
+    /**
+     * Stores all of the alignment poses for both reefs.
+     * Each branch is in the format (RED or BLUE)_BRANCH_(tag ID)_(LEFT or RIGHT)
+     * Each algae setpoint is in the form (RED or BLUE)_ALGAE_(tag ID)
+     * Left and right refer to the position of the branch when looking directly at the AprilTag
+     */
+    public enum REEF {
+        RED_BRANCH_6_LEFT(5, 0.1651, .0587),
+        RED_BRANCH_6_RIGHT(5, -0.1651, .0587),
+        RED_BRANCH_7_LEFT(6, 0.1651, .0587),
+        RED_BRANCH_7_RIGHT(6, -0.1651, .0587),
+        RED_BRANCH_8_LEFT(7, 0.1651, .0587),
+        RED_BRANCH_8_RIGHT(7, -0.1651, .0587),
+        RED_BRANCH_9_LEFT(8, 0.1651, .0587),
+        RED_BRANCH_9_RIGHT(8, -0.1651, .0587),
+        RED_BRANCH_10_LEFT(9, 0.1651, .0587),
+        RED_BRANCH_10_RIGHT(9, -0.1651, .0587),
+        RED_BRANCH_11_LEFT(10, 0.1651, .0587),
+        RED_BRANCH_11_RIGHT(10, -0.1651, .0587),
+
+        BLUE_BRANCH_17_LEFT(16, 0.1651, .0587),
+        BLUE_BRANCH_17_RIGHT(16, -0.1651, .0587),
+        BLUE_BRANCH_18_LEFT(17, 0.1651, .0587),
+        BLUE_BRANCH_18_RIGHT(17, -0.1651, .0587),
+        BLUE_BRANCH_19_LEFT(18, 0.1651, .0587),
+        BLUE_BRANCH_19_RIGHT(18, -0.1651, .0587),
+        BLUE_BRANCH_20_LEFT(19, 0.1651, .0587),
+        BLUE_BRANCH_20_RIGHT(19, -0.1651, .0587),
+        BLUE_BRANCH_21_LEFT(20, 0.1651, .0587),
+        BLUE_BRANCH_21_RIGHT(20, -0.1651, .0587),
+        BLUE_BRANCH_22_LEFT(21, 0.1651, .0587),
+        BLUE_BRANCH_22_RIGHT(21, -0.1651, .0587),
+
+        RED_ALGAE_6(5, ALGAE_X_OFFSET, 0, true),
+        RED_ALGAE_7(6, ALGAE_X_OFFSET, 0, true),
+        RED_ALGAE_8(7, ALGAE_X_OFFSET, 0, true),
+        RED_ALGAE_9(8, ALGAE_X_OFFSET, 0, true),
+        RED_ALGAE_10(9, ALGAE_X_OFFSET, 0, true),
+        RED_ALGAE_11(10, ALGAE_X_OFFSET, 0, true),
+        BLUE_ALGAE_17(16, ALGAE_X_OFFSET, 0, true),
+        BLUE_ALGAE_18(17, ALGAE_X_OFFSET, 0, true),
+        BLUE_ALGAE_19(18, ALGAE_X_OFFSET, 0, true),
+        BLUE_ALGAE_20(19, ALGAE_X_OFFSET, 0, true),
+        BLUE_ALGAE_21(20, ALGAE_X_OFFSET, 0, true),
+        BLUE_ALGAE_22(21, ALGAE_X_OFFSET, 0, true);
+
+        /**
+         * The pose to align to for scoring on this branch or intaking this algae
+         */
+        public final Pose2d pose;
+        /**
+         * The pose to align to for scoring on L4, null for algae
+         */
+        public final Pose2d l4Pose;
+        /**
+         * The pose to align to for scoring on L1, null for algae
+         */
+        public final Pose2d l1Pose;
+        /**
+         * The ID of the AprilTag on the smae side of hte reef ast his branch
+         */
+        public final int aprilTagId;
+        private final int aprilTagIndex;
+        /**
+         * The horizontal (parallel to the closest face of the reef) distance, in meters, between the AprilTag and branch
+         */
+        public final double xOffset;
+        /**
+         * The y (normal to the closest face of the reef) distance, in meters, between the AprilTag and branch
+         */
+        public final double yOffset;
+
+        public final boolean isAlgae;
+
+        private REEF(int aprilTagIndex, double xOffset, double yOffset) {
+            this(aprilTagIndex, xOffset, yOffset, false);
+        }
+        private REEF(int aprilTagIndex, double xOffset, double yOffset, boolean isAlgae) {
+            this.aprilTagIndex = aprilTagIndex;
+            aprilTagId = aprilTagIndex+1;
+            this.xOffset = xOffset;
+            this.yOffset = yOffset;
+            this.isAlgae = isAlgae;
+            pose = getPose();
+            if(isAlgae){
+                l4Pose = null;
+                l1Pose = null;
+            }else{
+                l4Pose = pose.transformBy(new Transform2d(0, Units.inchesToMeters(2), new Rotation2d()));
+                l1Pose = getL1Pose();
+            }
+        }
+
+        /**
+         * Calculates the Pose2d to align to the branch based on the AprilTag's pose and
+         * offsets.
+         *
+         * @return The calculated Pose2d for this reef branch.
+         */
+        private Pose2d getPose() {
+            Pose3d basePose3d = FieldConstants.APRIL_TAGS.get(aprilTagIndex).pose;
+            double adjustedYOffset = DriveConstants.ROBOT_WIDTH_WITH_BUMPERS / 2.0 + (isAlgae ? 0 : Units.inchesToMeters(4.5));
+
+            // Apply both X and Y offsets to calculate the reef branch pose
+            Transform3d transform = new Transform3d(adjustedYOffset, -xOffset, 0, new Rotation3d(0, 0, 0));
+
+            Pose3d branchPose3d = basePose3d.plus(transform);
+
+            // Convert the calculated branch Pose3d to Pose2d
+            return branchPose3d.toPose2d().transformBy(new Transform2d(0, 0, new Rotation2d(Math.PI/2)));
+        }
+        /**
+         * Calculates the Pose2d to align to for scoring on L1 near this branch based on the AprilTag's pose and
+         * offsets.
+         *
+         * @return The calculated Pose2d for this reef branch.
+         */
+        private Pose2d getL1Pose() {
+                Pose3d basePose3d = FieldConstants.APRIL_TAGS.get(aprilTagIndex).pose;
+                double adjustedYOffset = DriveConstants.ROBOT_WIDTH_WITH_BUMPERS / 2.0;
+    
+                // Apply both X and Y offsets to calculate the reef branch pose
+                Transform3d transform = new Transform3d(adjustedYOffset, -Math.signum(xOffset)*Units.inchesToMeters(37.04/2-1), 0, new Rotation3d(0, 0, 0));
+    
+                Pose3d branchPose3d = basePose3d.plus(transform);
+    
+                // Convert the calculated branch Pose3d to Pose2d
+                return branchPose3d.toPose2d().transformBy(new Transform2d(0, 0, new Rotation2d(Math.PI/2)));
+            }
+    
+        /**
+         * Finds the appropriate reef branch based on the AprilTag ID and whether the
+         * pose is left or right.
+         *
+         * @param aprilTagId The ID of the AprilTag.
+         * @param isLeftPose True if the pose is left, false if it's right.
+         * @return The matching REEF, or null if no match is found.
+         */
+        public static REEF fromAprilTagIdAndPose(int aprilTagId, boolean isLeftPose) {
+            for (REEF branch : values()) {
+                if (!branch.isAlgae && branch.aprilTagId == aprilTagId &&
+                        ((isLeftPose && branch.xOffset > 0) || (!isLeftPose && branch.xOffset < 0))) {
+                    return branch;
+                }
+            }
+            return null;
+        }
+
+        /**
+         * Finds the appropriate reef algae position based on the AprilTag ID
+         *
+         * @param aprilTagId The ID of the AprilTag.
+         * @return The matching REEF, or null if no match is found.
+         */
+        public static REEF fromAprilTagIdAlgae(int aprilTagId) {
+            for (REEF branch : values()) {
+                if (branch.isAlgae && branch.aprilTagId == aprilTagId) {
+                    return branch;
+                }
+            }
+            return null;
+        }
+    }
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/constants/swerve/DriveConstants.java b/src/main/java/frc/robot/constants/swerve/DriveConstants.java
new file mode 100644 (file)
index 0000000..3e57260
--- /dev/null
+++ b/src/main/java/frc/robot/constants/swerve/DriveConstants.java
@@ -0,0 +1,240 @@
+package frc.robot.constants.swerve;
+
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.util.Units;
+import frc.robot.RobotId;
+import frc.robot.constants.Constants;
+import frc.robot.util.SwerveStuff.ModuleLimits;
+import lib.COTSFalconSwerveConstants;
+
+/**
+ * Global constants are, by default, for the competition robot.
+ * Global constants get changed in the update method if the RobotId detected is not the competition robot.
+ */
+public class DriveConstants {
+    /**
+     * The robot's width with its bumpers on.
+     * <p>
+     * The frame width is 26.5 inches, and each bumper is 3.25 inches.
+     */
+    public static final double ROBOT_WIDTH_WITH_BUMPERS = 0.832;
+
+    public static double ROBOT_MASS = 25;
+
+    /** Radius of the drive wheels [meters]. */
+    public static final double WHEEL_RADIUS = Units.inchesToMeters(1.95);
+
+    public static double WHEEL_MOI = 0.000326 * ROBOT_MASS;
+
+    
+    /** Distance between the left and right wheels [meters]. */
+    public static double TRACK_WIDTH = Units.inchesToMeters(20.75);//22.75 swerve bot, 20.75 comp bot
+
+    // Mk4i gear ratios
+    // https://www.swervedrivespecialties.com/products/mk4i-swerve-module
+    //   standard gear ratios
+    // https://www.swervedrivespecialties.com/products/kit-adapter-16t-drive-pinion-gear-mk4i
+    //   changes 14-tooth pinion to 16-tooth pinion -- (50.0 / 14.0) becomes (50.0 / 16.0).
+    /** Drive gear ratio for an Mk4i with L2-Plus gearing */
+    public static double DRIVE_GEAR_RATIO = (50.0 / 16.0) * (17.0 / 27.0) * (45.0 / 15.0);
+    // all MK4i modules have the same steering gear ratio
+    public static final double STEER_GEAR_RATIO = 150.0 / 7.0;
+
+    /** Theoretical maximum speed of the robot based on maximum motor RPM, gear ratio, and wheel radius */
+    public static final double MAX_SPEED = 4.5;
+
+    // Need to convert tangential velocity (the m/s of the edge of the robot) to angular velocity (the radians/s of the robot)
+    // To do so, divide by the radius. The radius is the diagonal of the square chassis, diagonal = sqrt(2) * side_length.
+    public static final double MAX_ANGULAR_SPEED = MAX_SPEED / ((TRACK_WIDTH / 2) * Math.sqrt(2));
+
+    public static final double COSF = 0.9;
+    
+    // The maximum acceleration of the robot, limited by friction
+    public static final double MAX_LINEAR_ACCEL = COSF * Constants.GRAVITY_ACCELERATION;
+    // The maximum amount a drive motor can accelerate, independant of friction
+    // This does nothing if greater than LINEAR_ACCEL
+    public static final double MAX_DRIVE_ACCEL = MAX_LINEAR_ACCEL;
+    // The maximum angular acceleration of the robot
+    public static final double MAX_ANGULAR_ACCEL = MAX_LINEAR_ACCEL / TRACK_WIDTH * Math.sqrt(2);
+
+    /**
+     * If this is false, Drivetrain will use the previous setpoint to calculate the new setpoint.
+     * <p> If this is true, Drivetrain will use the actual current setpoint instead.
+     */
+    public static final boolean USE_ACTUAL_SPEED = false;
+
+    /**
+     * Disables the deadband and optimization for the modules.
+     * SwerveSetpointGenerator adds its own optimization and deadband, and the controllers also have a deadband.
+     * Setting this to true fixes bugs caused by using hte actual current state.
+     */
+    public static final boolean DISABLE_DEADBAND_AND_OPTIMIZATION = false;
+
+    public static final Rotation2d STARTING_HEADING = new Rotation2d();
+
+    public static final Translation2d[] MODULE_LOCATIONS = {
+        new Translation2d(DriveConstants.TRACK_WIDTH / 2, DriveConstants.TRACK_WIDTH / 2),
+        new Translation2d(DriveConstants.TRACK_WIDTH / 2, -DriveConstants.TRACK_WIDTH / 2),
+        new Translation2d(-DriveConstants.TRACK_WIDTH / 2, DriveConstants.TRACK_WIDTH / 2),
+        new Translation2d(-DriveConstants.TRACK_WIDTH / 2, -DriveConstants.TRACK_WIDTH / 2)
+    };
+
+    public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics(MODULE_LOCATIONS);
+
+    public static double STEER_OFFSET_FRONT_LEFT = 302.646;
+    public static double STEER_OFFSET_FRONT_RIGHT = 103.039+180;
+    public static double STEER_OFFSET_BACK_LEFT = 165.49+90;
+    public static double STEER_OFFSET_BACK_RIGHT = 73.563;
+
+    // Heading PID.
+    public static final double HEADING_P = 5.5;
+    public static final double HEADING_D = 0;
+
+    public static final double HEADING_TOLERANCE = Units.degreesToRadians(1.5);
+
+    // Translational PID
+    // TODO: Tune this better (low priority since we aren't using it in 2025)
+    public static final double TRANSLATIONAL_P = 1;
+    public static final double TRANSLATIONAL_D = 0.001;
+
+    //The PIDs for PathPlanner Command
+    public static final double PATH_PLANNER_HEADING_P = 3.5;
+    public static final double PATH_PLANNER_HEADING_D = 0;
+
+    public static final double PATH_PLANNER_TRANSLATIONAL_P = 6;
+    public static final double PATH_PLANNER_TRANSLATIONAL_D = 0;
+
+    // CAN
+    public static String DRIVE_MOTOR_CAN = Constants.CANIVORE_CAN;
+    public static String STEER_MOTOR_CAN = Constants.CANIVORE_CAN;
+    public static String STEER_ENCODER_CAN = Constants.CANIVORE_CAN;
+    public static String PIGEON_CAN = Constants.CANIVORE_CAN;
+
+
+    public static COTSFalconSwerveConstants MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+    /* Swerve Current Limiting */
+    public static final int STEER_CONTINUOUS_CURRENT_LIMIT = 15;
+    public static final int STEER_PEAK_CURRENT_LIMIT = 15;
+    public static final double STEER_PEAK_CURRENT_DURATION = 0.01;
+    public static final boolean STEER_ENABLE_CURRENT_LIMIT = true;
+
+    public static final int DRIVE_CONTINUOUS_CURRENT_LIMIT = 60;
+    public static final int DRIVE_PEAK_CURRENT_LIMIT = 60;
+    public static final double DRIVE_PEAK_CURRENT_DURATION = 0.01;
+    public static final boolean DRIVE_ENABLE_CURRENT_LIMIT = true;
+
+    /* Motor inversions */
+    public static final InvertedValue INVERT_DRIVE_MOTOR = InvertedValue.CounterClockwise_Positive;
+    public static final InvertedValue INVERT_STEER_MOTOR = InvertedValue.Clockwise_Positive;
+
+    /* Neutral Modes */
+    public static final NeutralModeValue DRIVE_NEUTRAL_MODE = NeutralModeValue.Brake;
+    public static final NeutralModeValue STEER_NEUTRAL_MODE = NeutralModeValue.Brake;
+
+    /* Drive Motor PID Values */
+    public static final double[] P_VALUES = {
+        0.1,
+        0.1,
+        0.1,
+        0.1
+    };
+    public static final double[] I_VALUES = {
+        0,
+        0,
+        0,
+        0
+    };
+    public static final double[] D_VALUES = {
+        0,
+        0,
+        0,
+        0
+    };
+    /* Drive Motor Characterization Values
+     * Divide SYSID values by 12 to convert from volts to percent output for CTRE */
+    public static final double[] S_VALUES = {
+        0.11,
+        0.11,
+        0.11,
+        0.11
+    };
+    public static final double[] V_VALUES = {
+        0.11079,
+        0.10718,
+        0.11009,
+        0.1164
+    };
+    public static final double[] A_VALUES = {
+        0.005482,
+        0.0049593,
+        0.010156,
+        0.0065708
+    };
+    /* Ramp values for drive motors in open loop driving. */
+    // Open loop prevents throttle from changing too quickly.
+    // It will limit it to time given (in seconds) to go from zero to full throttle.
+    // A small open loop ramp (0.25) helps with tread wear, tipping, etc
+    public static final double OPEN_LOOP_RAMP = 0.25;
+
+    public static final double WHEEL_CIRCUMFERENCE = 2*Math.PI*WHEEL_RADIUS;
+
+    public static final boolean INVERT_GYRO = false; // Make sure gyro is CCW+ CW-
+
+    public static final double SLOW_DRIVE_FACTOR = 0.2;
+    public static final double SLOW_ROT_FACTOR = 0.1;
+
+    public static final ModuleLimits MODULE_LIMITS = new ModuleLimits(MAX_SPEED, MAX_DRIVE_ACCEL, COSF, Units.rotationsPerMinuteToRadiansPerSecond(Constants.MAX_RPM / STEER_GEAR_RATIO));
+
+    /**
+     * Updates the constants if the RobotId is not the competition robot.
+     */
+    public static void update(RobotId robotId) {
+        if(robotId == RobotId.BetaBot) {
+            STEER_OFFSET_FRONT_LEFT = 193.884-180;
+            STEER_OFFSET_FRONT_RIGHT = 110.914;
+            STEER_OFFSET_BACK_LEFT = 128.054+180;
+            STEER_OFFSET_BACK_RIGHT = 107.43;
+        } else if (robotId == RobotId.Vivace) {
+            STEER_OFFSET_FRONT_LEFT = 100.184+180;
+            STEER_OFFSET_FRONT_RIGHT = 224.293;
+            STEER_OFFSET_BACK_LEFT = 304.795-180;
+            STEER_OFFSET_BACK_RIGHT = 201.177-180;
+
+            ROBOT_MASS = 50;
+            WHEEL_MOI = 0.000326 * ROBOT_MASS;
+
+        } else if (robotId == RobotId.Vertigo) {
+            STEER_OFFSET_FRONT_LEFT = 4.185;
+            STEER_OFFSET_FRONT_RIGHT = 101.519+90;
+            STEER_OFFSET_BACK_LEFT = 38.997+180;
+            STEER_OFFSET_BACK_RIGHT = 242.847-90;
+            
+            DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
+
+            ROBOT_MASS = 20;
+
+            WHEEL_MOI = 0.000326 * ROBOT_MASS;
+            
+            // Falcon Speed
+            Constants.MAX_RPM = 6080.0;
+        } else if (robotId == RobotId.Phil) {
+            ROBOT_MASS = 30;
+            WHEEL_MOI = 0.000326 * ROBOT_MASS;
+
+            STEER_OFFSET_FRONT_LEFT = 121.463+180;
+            STEER_OFFSET_FRONT_RIGHT = 284.242;
+            STEER_OFFSET_BACK_LEFT = 157.676;
+            STEER_OFFSET_BACK_RIGHT = 77.199;
+
+            DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
+        }
+        
+        MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+    }
+}

diff --git a/src/main/java/frc/robot/constants/swerve/ModuleConstants.java b/src/main/java/frc/robot/constants/swerve/ModuleConstants.java
new file mode 100644 (file)
index 0000000..7074482
--- /dev/null
+++ b/src/main/java/frc/robot/constants/swerve/ModuleConstants.java
@@ -0,0 +1,145 @@
+package frc.robot.constants.swerve;
+
+import frc.robot.constants.IdConstants;
+
+/**
+ * Container class for module constants, defined using constants from {@link DriveConstants}
+ * .
+ *
+ * @see DriveConstants
+ */
+public enum ModuleConstants {
+
+    FRONT_LEFT(
+            IdConstants.DRIVE_FRONT_LEFT_ID,
+            IdConstants.STEER_FRONT_LEFT_ID,
+            IdConstants.ENCODER_FRONT_LEFT_ID,
+            DriveConstants.STEER_OFFSET_FRONT_LEFT,
+            ModuleType.FRONT_LEFT,
+            DriveConstants.S_VALUES[0],
+            DriveConstants.V_VALUES[0],
+            DriveConstants.A_VALUES[0],
+            DriveConstants.P_VALUES[0],
+            DriveConstants.I_VALUES[0],
+            DriveConstants.D_VALUES[0]
+    ),
+    FRONT_RIGHT(
+            IdConstants.DRIVE_FRONT_RIGHT_ID,
+            IdConstants.STEER_FRONT_RIGHT_ID,
+            IdConstants.ENCODER_FRONT_RIGHT_ID,
+            DriveConstants.STEER_OFFSET_FRONT_RIGHT,
+            ModuleType.FRONT_RIGHT,
+            DriveConstants.S_VALUES[1],
+            DriveConstants.V_VALUES[1],
+            DriveConstants.A_VALUES[1],
+            DriveConstants.P_VALUES[1],
+            DriveConstants.I_VALUES[1],
+            DriveConstants.D_VALUES[1]
+    ),
+    BACK_LEFT(
+            IdConstants.DRIVE_BACK_LEFT_ID,
+            IdConstants.STEER_BACK_LEFT_ID,
+            IdConstants.ENCODER_BACK_LEFT_ID,
+            DriveConstants.STEER_OFFSET_BACK_LEFT,
+            ModuleType.BACK_LEFT,
+            DriveConstants.S_VALUES[2],
+            DriveConstants.V_VALUES[2],
+            DriveConstants.A_VALUES[2],
+            DriveConstants.P_VALUES[2],
+            DriveConstants.I_VALUES[2],
+            DriveConstants.D_VALUES[2]
+    ),
+    BACK_RIGHT(
+            IdConstants.DRIVE_BACK_RIGHT_ID,
+            IdConstants.STEER_BACK_RIGHT_ID,
+            IdConstants.ENCODER_BACK_RIGHT_ID,
+            DriveConstants.STEER_OFFSET_BACK_RIGHT,
+            ModuleType.BACK_RIGHT,
+            DriveConstants.S_VALUES[3],
+            DriveConstants.V_VALUES[3],
+            DriveConstants.A_VALUES[3],
+            DriveConstants.P_VALUES[3],
+            DriveConstants.I_VALUES[3],
+            DriveConstants.D_VALUES[3]
+    ),
+
+    NONE(0, 0, 0, 0.0, ModuleType.NONE,0,0,0,0,0,0);
+
+    private final int drivePort;
+    private final int steerPort;
+    private final int encoderPort;
+    private final double steerOffset;
+    private final double ks;
+    private final double kv;
+    private final double ka;
+    private final double driveP;
+    private final double driveI;
+    private final double driveD;
+    private final ModuleType type;
+
+    ModuleConstants(
+            int drivePort,
+            int steerPort,
+            int encoderPort,
+            double steerOffset,
+            ModuleType type,
+            double ks,
+            double kv,
+            double ka,
+            double driveP,
+            double driveI,
+            double driveD
+
+                   ) {
+        this.drivePort = drivePort;
+        this.steerPort = steerPort;
+        this.encoderPort = encoderPort;
+        this.steerOffset = steerOffset;
+        this.type = type;
+        this.ks =ks;
+        this.kv= kv;
+        this.ka = ka;
+        this.driveP =driveP;
+        this.driveI = driveI;
+        this.driveD = driveD;
+    }
+
+    public int getDrivePort() {
+        return drivePort;
+    }
+
+    public int getSteerPort() {
+        return steerPort;
+    }
+
+    public int getEncoderPort() {
+        return encoderPort;
+    }
+
+    public double getSteerOffset() {
+        return steerOffset;
+    }
+
+    public ModuleType getType() {
+        return type;
+    }
+    public double getDriveS(){
+        return ks;
+    }
+    public double getDriveV(){
+        return kv;
+    }
+    public double getDriveA(){
+        return ka;
+    }
+    public double getDriveP(){
+        return driveP;
+    }
+    public double getDriveI(){
+        return driveI;
+    }
+    public double getDriveD(){
+        return driveD;
+    }
+
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/constants/swerve/ModuleType.java b/src/main/java/frc/robot/constants/swerve/ModuleType.java
new file mode 100644 (file)
index 0000000..da71218
--- /dev/null
+++ b/src/main/java/frc/robot/constants/swerve/ModuleType.java
@@ -0,0 +1,31 @@
+package frc.robot.constants.swerve;
+
+/**
+ * Represents the type for a module on the robot.
+ * <p/>
+ * IDs:
+ * 0 - FRONT_LEFT
+ * 1 - FRONT_RIGHT
+ * 2 - BACK_LEFT
+ * 3 - BACK_RIGHT
+ */
+public enum ModuleType {
+    FRONT_LEFT,
+    FRONT_RIGHT,
+    BACK_LEFT,
+    BACK_RIGHT,
+    NONE;
+
+    public final byte id;
+
+    ModuleType() {
+        this.id = id();
+    }
+
+    private byte id() {
+        if (this == NONE)
+            return -1;
+        // This is a trick that relies on the order the enums are defined.
+        return (byte) this.ordinal();
+    }
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/controls/BaseDriverConfig.java b/src/main/java/frc/robot/controls/BaseDriverConfig.java
new file mode 100644 (file)
index 0000000..b9bd052
--- /dev/null
+++ b/src/main/java/frc/robot/controls/BaseDriverConfig.java
@@ -0,0 +1,79 @@
+package frc.robot.controls;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.RobotController;
+import frc.robot.constants.Constants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.DynamicSlewRateLimiter;
+import frc.robot.util.MathUtils;
+
+/**
+ * Abstract class for different controller types.
+ */
+public abstract class BaseDriverConfig {
+
+    private final Drivetrain drive;
+
+    private double previousHeading = 0;
+
+    private final DynamicSlewRateLimiter headingLimiter = new DynamicSlewRateLimiter(Constants.HEADING_SLEWRATE);
+
+    /**
+     * @param drive               the drivetrain instance
+     * @param controllerTab       the shuffleboard controller tab
+     * @param shuffleboardUpdates whether to update the shuffleboard
+     */
+    public BaseDriverConfig(Drivetrain drive) {
+        headingLimiter.setContinuousLimits(-Math.PI, Math.PI);
+        headingLimiter.enableContinuous(true);
+        this.drive = drive;
+    }
+
+    public double getForwardTranslation() {
+        double forward = getRawForwardTranslation();
+        return forward * DriveConstants.MAX_SPEED * Math.min(1,RobotController.getBatteryVoltage()/12) * MathUtil.applyDeadband(Math.sqrt(forward*forward + Math.pow(getRawSideTranslation(), 2)), Constants.TRANSLATIONAL_DEADBAND);
+    }
+
+    public double getSideTranslation() {
+        double side = getRawSideTranslation();
+        return side * DriveConstants.MAX_SPEED * Math.min(1,RobotController.getBatteryVoltage()/12) * MathUtil.applyDeadband(Math.sqrt(side*side + Math.pow(getRawForwardTranslation(), 2)), Constants.TRANSLATIONAL_DEADBAND);
+    }
+
+    public double getRotation() {
+        return MathUtils.expoMS(MathUtil.applyDeadband(getRawRotation(), Constants.ROTATION_DEADBAND), 2)
+                * DriveConstants.MAX_ANGULAR_SPEED * Math.min(1, RobotController.getBatteryVoltage()/12);
+    }
+
+    public double getHeading() {
+        if (getRawHeadingMagnitude() <= Constants.HEADING_DEADBAND)
+            return headingLimiter.calculate(previousHeading, 1e-6);
+        previousHeading = headingLimiter.calculate(getRawHeadingAngle(),
+                MathUtils.expoMS(getRawHeadingMagnitude(), 2));
+        return previousHeading;
+    }
+
+    protected Drivetrain getDrivetrain() {
+        return drive;
+    }
+
+    /**
+     * Configures the controls for the controller.
+     */
+    public abstract void configureControls();
+
+    public abstract double getRawSideTranslation();
+
+    public abstract double getRawForwardTranslation();
+
+    public abstract double getRawRotation();
+
+    public abstract double getRawHeadingAngle();
+
+    public abstract double getRawHeadingMagnitude();
+
+    public abstract boolean getIsSlowMode();
+
+    public abstract boolean getIsAlign();
+
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/controls/Driver.java b/src/main/java/frc/robot/controls/Driver.java
deleted file mode 100644 (file)
index 8433b48..0000000
--- a/src/main/java/frc/robot/controls/Driver.java
+++ /dev/null
@@ -1,31 +0,0 @@
-package frc.robot.controls;
-
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.Robot;
-import frc.robot.commands.DoNothing;
-import frc.robot.constants.Constants;
-import frc.robot.util.TestType;
-import lib.controllers.GameController;
-import lib.controllers.GameController.Button;
-
-public class Driver {
-  private static GameController driver = new GameController(Constants.oi.kDriverJoy);
-
-  public static void configureControls() {
-    
-    // example button binding implementation
-    driver.get(Button.A).whenPressed(new DoNothing());
-
-    // example test type implementation
-    // tests drivetrain, when in TEST_DRIVE test mode and 
-    driver.get(Button.A).and(Robot.shuffleboard.isTestTypeTrigger(TestType.TEST_DRIVE)).whenActive(
-      new SequentialCommandGroup(
-        new RunCommand(() -> Robot.drive.tankDrive(0.5, 0.5), Robot.drive).withTimeout(1),
-        new RunCommand(() -> Robot.drive.tankDrive(-0.5, -0.5), Robot.drive).withTimeout(1),
-        new RunCommand(() -> Robot.drive.tankDrive(0.5, -0.5), Robot.drive).withTimeout(1),
-        new RunCommand(() -> Robot.drive.tankDrive(-0.5, 0.5), Robot.drive).withTimeout(1)
-      )
-    );
-  }
-}

diff --git a/src/main/java/frc/robot/controls/Ex3DProDriverConfig.java b/src/main/java/frc/robot/controls/Ex3DProDriverConfig.java
new file mode 100644 (file)
index 0000000..84a80fa
--- /dev/null
+++ b/src/main/java/frc/robot/controls/Ex3DProDriverConfig.java
@@ -0,0 +1,66 @@
+package frc.robot.controls;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.commands.drive_comm.SetFormationX;
+import frc.robot.constants.Constants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import lib.controllers.Ex3DProController;
+import lib.controllers.Ex3DProController.Ex3DProAxis;
+import lib.controllers.Ex3DProController.Ex3DProButton;
+
+/**
+ * Driver controls for the Ex3D Pro controller.
+ */
+public class Ex3DProDriverConfig extends BaseDriverConfig {
+
+    private final Ex3DProController kDriver = new Ex3DProController(Constants.DRIVER_JOY);
+    private final BooleanSupplier slowModeSupplier = kDriver.get(Ex3DProButton.B11);
+
+    public Ex3DProDriverConfig(Drivetrain drive) {
+        super(drive);
+    }
+
+    @Override
+    public void configureControls() {
+        kDriver.get(Ex3DProButton.B1).whileTrue(new SetFormationX(super.getDrivetrain()));
+        kDriver.get(Ex3DProButton.B2).onTrue(new InstantCommand(() -> super.getDrivetrain().setYaw(DriveConstants.STARTING_HEADING)));
+    }
+
+    @Override
+    public double getRawSideTranslation() {
+        return -kDriver.get(Ex3DProAxis.X);
+    }
+
+    @Override
+    public double getRawForwardTranslation() {
+        return -kDriver.get(Ex3DProAxis.Y);
+    }
+
+    @Override
+    public double getRawRotation() {
+        return kDriver.get(Ex3DProAxis.Z);
+    }
+
+    @Override
+    public double getRawHeadingAngle() {
+        return kDriver.get(Ex3DProAxis.Z) * Math.PI;
+    }
+
+    @Override
+    public double getRawHeadingMagnitude() {
+        return kDriver.get(Ex3DProAxis.SLIDER);
+    }
+
+    @Override
+    public boolean getIsSlowMode() {
+        return slowModeSupplier.getAsBoolean();
+    }
+
+    @Override
+    public boolean getIsAlign() {
+        return false;
+    }
+}

diff --git a/src/main/java/frc/robot/controls/GameControllerDriverConfig.java b/src/main/java/frc/robot/controls/GameControllerDriverConfig.java
new file mode 100644 (file)
index 0000000..3cf0260
--- /dev/null
+++ b/src/main/java/frc/robot/controls/GameControllerDriverConfig.java
@@ -0,0 +1,322 @@
+package frc.robot.controls;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.StartEndCommand;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.Robot;
+import frc.robot.commands.drive_comm.DriveToPose;
+import frc.robot.commands.gpm.IntakeAlgae;
+import frc.robot.commands.gpm.IntakeAlgaeArm;
+import frc.robot.commands.gpm.IntakeCoral;
+import frc.robot.commands.gpm.MoveArm;
+import frc.robot.commands.gpm.MoveElevator;
+import frc.robot.commands.gpm.NetSetpoint;
+import frc.robot.commands.gpm.OuttakeAlgae;
+import frc.robot.commands.gpm.OuttakeCoral;
+import frc.robot.commands.gpm.ResetClimb;
+import frc.robot.commands.gpm.ReverseMotors;
+import frc.robot.commands.gpm.StationIntake;
+import frc.robot.constants.ArmConstants;
+import frc.robot.constants.Constants;
+import frc.robot.constants.ElevatorConstants;
+import frc.robot.constants.FieldConstants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.subsystems.arm.Arm;
+import frc.robot.subsystems.climb.Climb;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.subsystems.indexer.Indexer;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.outtake.Outtake;
+import lib.controllers.GameController;
+import lib.controllers.GameController.Axis;
+import lib.controllers.GameController.Button;
+import lib.controllers.GameController.DPad;
+
+/**
+ * Driver controls for the generic game controller.
+ */
+public class GameControllerDriverConfig extends BaseDriverConfig {
+  public static final boolean singleAlignmentButton = true;
+
+  private final GameController driver = new GameController(Constants.DRIVER_JOY);
+  private final BooleanSupplier slowModeSupplier = driver.get(Button.RIGHT_JOY);
+  private final Elevator elevator;
+  private final Intake intake;
+  private final Indexer indexer;
+  private final Outtake outtake;
+  private final Climb climb;
+  private final Arm arm;
+  private int alignmentDirection = 0;
+  private Pose2d alignmentPose = null;
+
+  public GameControllerDriverConfig(Drivetrain drive, Elevator elevator, Intake intake, Indexer indexer,
+      Outtake outtake, Climb climb, Arm arm) {
+    super(drive);
+    this.elevator = elevator;
+    this.intake = intake;
+    this.indexer = indexer;
+    this.outtake = outtake;
+    this.climb = climb;
+    this.arm = arm;
+  }
+
+  @Override
+  public void configureControls() {
+    Trigger menu = driver.get(Button.LEFT_JOY);
+
+    // Elevator setpoints
+    if (elevator != null && arm != null) {
+      driver.get(Button.BACK).and(menu.negate()).onTrue(
+          new SequentialCommandGroup(
+              new MoveElevator(elevator, ElevatorConstants.L1_SETPOINT),
+              new MoveArm(arm, ArmConstants.L1_SETPOINT)));
+
+      driver.get(driver.LEFT_TRIGGER_BUTTON).onTrue(
+          new SequentialCommandGroup(
+              new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT),
+              new MoveArm(arm, ArmConstants.L4_SETPOINT_RIGHT)));
+
+      Command l2Coral = new SequentialCommandGroup(
+          new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT),
+          new MoveArm(arm, ArmConstants.L2_L3_SETPOINT));
+      Command l3Coral = new SequentialCommandGroup(
+          new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT),
+          new MoveArm(arm, ArmConstants.L2_L3_SETPOINT));
+      Command l2Algae = new ParallelCommandGroup(
+          new MoveElevator(elevator, ElevatorConstants.BOTTOM_ALGAE_SETPOINT),
+          new MoveArm(arm, ArmConstants.ALGAE_SETPOINT)).andThen(new IntakeAlgaeArm(outtake));
+      Command l3Algae = new ParallelCommandGroup(
+          new MoveElevator(elevator, ElevatorConstants.TOP_ALGAE_SETPOINT),
+          new MoveArm(arm, ArmConstants.ALGAE_SETPOINT)).andThen(new IntakeAlgaeArm(outtake));
+      driver.get(Button.RB).whileTrue(new ConditionalCommand(l2Algae, new InstantCommand(l2Coral::schedule), menu));
+      driver.get(Button.LB).whileTrue(new ConditionalCommand(l3Algae, new InstantCommand(l3Coral::schedule), menu));
+
+      // Processor setpoint
+      driver.get(Button.Y).and(menu.negate()).onTrue(
+          new ParallelCommandGroup(
+              new MoveElevator(elevator, ElevatorConstants.SAFE_SETPOINT + 0.001),
+              new MoveArm(arm, ArmConstants.PROCESSOR_SETPOINT)));
+      driver.get(DPad.UP).onTrue(new NetSetpoint(elevator, arm, getDrivetrain()));
+    }
+    // Intake/outtake
+    Trigger r3 = driver.get(Button.RIGHT_JOY);
+    
+    if (intake != null && indexer != null && elevator != null && outtake != null && arm != null) {// && elevator != null){
+      boolean toggle = true;
+      Command intakeCoral = new IntakeCoral(intake, indexer, elevator, outtake, arm);
+      Command intakeAlgae = new IntakeAlgae(intake);
+      driver.get(Button.A).onTrue(new InstantCommand(() -> {
+        if (r3.getAsBoolean())
+          return;
+        if (menu.getAsBoolean()) {
+          intakeAlgae.schedule();
+        } else {
+          if (toggle) {
+            if (intakeCoral.isScheduled()) {
+              intakeCoral.cancel();
+            } else {
+              intakeCoral.schedule();
+            }
+          } else {
+            intakeCoral.schedule();
+          }
+        }
+      })).onFalse(new InstantCommand(() -> {
+        if (!toggle) {
+          intakeCoral.cancel();
+        }
+        intakeAlgae.cancel();
+      }));
+      // On true, run the command to start intaking
+      // On false, run the command to finish intaking if it has a coral
+      Command startIntake = new StationIntake(outtake);
+      // Command finishIntake = new FinishStationIntake(intake, indexer, elevator,
+      // outtake);
+      driver.get(Button.A).and(r3).and(menu.negate()).onTrue(startIntake)
+          .onFalse(new InstantCommand(() -> {
+            if (!startIntake.isScheduled()) {
+              // finishIntake.schedule();
+            } else {
+              startIntake.cancel();
+            }
+          }));
+    }
+
+    if (intake != null && outtake != null && arm != null && elevator != null) {
+      driver.get(DPad.DOWN).and(menu).onTrue(new SequentialCommandGroup(
+          new OuttakeAlgae(outtake, intake),
+          new InstantCommand(() -> {
+            arm.setSetpoint(ArmConstants.START_ANGLE);
+            elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT);
+            getDrivetrain().setIsAlign(false);
+          }, elevator)));
+    }
+
+    if (outtake != null && elevator != null && arm != null) {
+      driver.get(DPad.DOWN).and(menu.negate())
+          .onTrue(new OuttakeCoral(outtake, elevator, arm)
+              .alongWith(new InstantCommand(() -> getDrivetrain().setDesiredPose(() -> null)))
+              .andThen(new SequentialCommandGroup(new MoveArm(arm, ArmConstants.INTAKE_SETPOINT),
+                  new MoveElevator(elevator, ElevatorConstants.STOW_SETPOINT))));
+    }
+    driver.get(DPad.DOWN).and(menu.negate()).onTrue(new InstantCommand(() -> {
+    }, getDrivetrain()));
+    if (intake != null && indexer != null && outtake != null) {
+      driver.get(Button.B).and(menu.negate()).whileTrue(new ReverseMotors(intake, indexer, outtake));
+    }
+
+    // Climb
+    if (climb != null) {
+      driver.get(Button.X).and(menu.negate())
+          .toggleOnTrue(new StartEndCommand(() -> climb.extend(), () -> climb.climb(), climb));
+      if (intake != null) {
+        driver.get(Button.X).and(menu.negate()).onTrue(new InstantCommand(() -> intake.setAngle(65), intake));
+      }
+      driver.get(Button.X).and(menu).whileTrue(new ResetClimb(climb));
+      driver.get(driver.RIGHT_TRIGGER_BUTTON).and(menu).onTrue(new InstantCommand(() -> climb.stow(), climb));
+    }
+
+    // Alignment
+    if (singleAlignmentButton) {
+      driver.get(DPad.LEFT).toggleOnTrue(new InstantCommand(() -> {
+        setAlignmentDirection();
+        setAlignmentPose(false, true);
+      }).andThen(new DriveToPose(getDrivetrain(), () -> alignmentPose)));
+      driver.get(DPad.RIGHT).toggleOnTrue(new InstantCommand(() -> {
+        setAlignmentDirection();
+        setAlignmentPose(false, false);
+      }).andThen(new DriveToPose(getDrivetrain(), () -> alignmentPose)));
+    } else {
+      driver.get(DPad.LEFT).onTrue(new InstantCommand(() -> setAlignmentPose(false, true))
+          .andThen(new DriveToPose(getDrivetrain(), () -> alignmentPose)));
+      driver.get(DPad.RIGHT).onTrue(new InstantCommand(() -> setAlignmentPose(false, false))
+          .andThen(new DriveToPose(getDrivetrain(), () -> alignmentPose)));
+    }
+
+    // Reset yaw to be away from driver
+    driver.get(Button.START).onTrue(new InstantCommand(() -> super.getDrivetrain().setYaw(
+        new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
+
+    // Cancel commands
+    driver.get(driver.RIGHT_TRIGGER_BUTTON).and(menu.negate()).onTrue(new InstantCommand(() -> {
+      if (elevator != null) {
+        if (outtake != null && outtake.coralLoaded()) {
+          elevator.setSetpoint(ElevatorConstants.INTAKE_STOW_SETPOINT);
+        } else {
+          elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT);
+        }
+      }
+      if (outtake != null) {
+        outtake.stop();
+      }
+      if (intake != null) {
+        intake.stow();
+        intake.deactivate();
+      }
+      if (indexer != null) {
+        indexer.stop();
+      }
+      if (climb != null) {
+        climb.stow();
+      }
+      if (arm != null) {
+        if (outtake != null && outtake.coralLoaded()) {
+          arm.setSetpoint(ArmConstants.STOW_SETPOINT);
+        } else {
+          arm.setSetpoint(ArmConstants.START_ANGLE);
+        }
+      }
+      getDrivetrain().setIsAlign(false);
+      getDrivetrain().setDesiredPose(() -> null);
+      CommandScheduler.getInstance().cancelAll();
+    }));
+  }
+
+  private void setAlignmentDirection() {
+    Translation2d drivePose = getDrivetrain().getPose().getTranslation();
+    int closestDirection = 0;
+    double closestDist = 20;
+    boolean isRed = Robot.getAlliance() == Alliance.Red;
+    int start = isRed ? 5 : 16;
+    for (int i = 0; i < 6; i++) {
+      double dist = FieldConstants.APRIL_TAGS.get(start + i).pose.toPose2d().getTranslation().getDistance(drivePose);
+      if (dist < closestDist) {
+        closestDist = dist;
+        closestDirection = i;
+      }
+    }
+    if (isRed) {
+      closestDirection = (8 - closestDirection) % 6;
+    }
+    alignmentDirection = closestDirection;
+  }
+
+  /**
+   * Sets the drivetrain's alignment pose to the selected position
+   * 
+   * @param isAlgae True for algae, false for branches
+   * @param isLeft  True for left branch, false for right, ignored for algae
+   */
+  private void setAlignmentPose(boolean isAlgae, boolean isLeft) {
+    int id = Robot.getAlliance() == Alliance.Blue ? alignmentDirection + 17
+        : (8 - alignmentDirection) % 6 + 6;
+    if (isAlgae) {
+      alignmentPose = VisionConstants.REEF.fromAprilTagIdAlgae(id).pose;
+    } else {
+      alignmentPose = VisionConstants.REEF.fromAprilTagIdAndPose(id, isLeft).pose;
+    }
+  }
+
+  @Override
+  public double getRawForwardTranslation() {
+    return driver.get(Axis.LEFT_Y);
+  }
+
+  @Override
+  public double getRawSideTranslation() {
+    return driver.get(Axis.LEFT_X);
+  }
+
+  @Override
+  public double getRawRotation() {
+    return driver.get(Axis.RIGHT_X);
+  }
+
+  @Override
+  public double getRawHeadingAngle() {
+    return Math.atan2(driver.get(Axis.RIGHT_X), -driver.get(Axis.RIGHT_Y)) - Math.PI / 2;
+  }
+
+  @Override
+  public double getRawHeadingMagnitude() {
+    return Math.hypot(driver.get(Axis.RIGHT_X), driver.get(Axis.RIGHT_Y));
+  }
+
+  @Override
+  public boolean getIsSlowMode() {
+    return slowModeSupplier.getAsBoolean();
+  }
+
+  @Override
+  public boolean getIsAlign() {
+    return false;
+    // return kDriver.LEFT_TRIGGER_BUTTON.getAsBoolean();
+  }
+
+  public GameController getGameController() {
+    return driver;
+  }
+
+}

diff --git a/src/main/java/frc/robot/controls/MadCatzDriverConfig.java b/src/main/java/frc/robot/controls/MadCatzDriverConfig.java
new file mode 100644 (file)
index 0000000..43233a2
--- /dev/null
+++ b/src/main/java/frc/robot/controls/MadCatzDriverConfig.java
@@ -0,0 +1,67 @@
+package frc.robot.controls;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.commands.drive_comm.SetFormationX;
+import frc.robot.constants.Constants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import lib.controllers.MadCatzController;
+import lib.controllers.MadCatzController.MadCatzAxis;
+import lib.controllers.MadCatzController.MadCatzButton;
+
+/**
+ * Driver controls for the MadCatz controller.
+ */
+public class MadCatzDriverConfig extends BaseDriverConfig {
+
+    private final MadCatzController kDriver = new MadCatzController(Constants.DRIVER_JOY);
+    private final BooleanSupplier slowModeSupplier = kDriver.get(MadCatzButton.B6);
+
+    public MadCatzDriverConfig(Drivetrain drive) {
+        super(drive);
+    }
+
+    @Override
+    public void configureControls() {
+        kDriver.get(MadCatzButton.B1).whileTrue(new SetFormationX(super.getDrivetrain()));
+        kDriver.get(MadCatzButton.B2).onTrue(new InstantCommand(() -> super.getDrivetrain().setYaw(DriveConstants.STARTING_HEADING)));
+    }
+
+    @Override
+    public double getRawSideTranslation() {
+        return kDriver.get(MadCatzAxis.X);
+    }
+
+    @Override
+    public double getRawForwardTranslation() {
+        return -kDriver.get(MadCatzAxis.Y);
+    }
+
+    @Override
+    public double getRawRotation() {
+        return kDriver.get(MadCatzAxis.ZROTATE);
+    }
+
+    @Override
+    public double getRawHeadingAngle() {
+        return kDriver.get(MadCatzAxis.ZROTATE) * Math.PI;
+    }
+
+    @Override
+    public double getRawHeadingMagnitude() {
+        return kDriver.get(MadCatzAxis.SLIDER);
+    }
+
+    @Override
+    public boolean getIsSlowMode() {
+        return slowModeSupplier.getAsBoolean();
+    }
+
+    @Override
+    public boolean getIsAlign() {
+        return false;
+    }
+
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/controls/Operator.java b/src/main/java/frc/robot/controls/Operator.java
index 0da08c5eb04347dfb0d2fcf21ae7e3f99dca3cd2..31ae3d1e7aa4e058f8d3fa6a6f100c49742d2455 100644 (file)
--- a/src/main/java/frc/robot/controls/Operator.java
+++ b/src/main/java/frc/robot/controls/Operator.java
@@ -1,14 +1,167 @@
+// 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 frc.robot.controls;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.Robot;
 import frc.robot.commands.DoNothing;
+import frc.robot.commands.gpm.IntakeAlgae;
+import frc.robot.commands.gpm.IntakeCoral;
+import frc.robot.commands.gpm.MoveElevator;
+import frc.robot.commands.gpm.OuttakeAlgaeIntake;
+import frc.robot.commands.gpm.OuttakeCoral;
+import frc.robot.commands.gpm.RemoveAlgae;
+import frc.robot.commands.gpm.ReverseMotors;
+import frc.robot.commands.gpm.StationIntake;
 import frc.robot.constants.Constants;
+import frc.robot.constants.ElevatorConstants;
+import frc.robot.constants.IntakeConstants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.subsystems.arm.Arm;
+import frc.robot.subsystems.climb.Climb;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.subsystems.indexer.Indexer;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.outtake.Outtake;
 import lib.controllers.GameController;
 import lib.controllers.GameController.Button;
+import lib.controllers.GameController.DPad;
 
+/** 
+ * Controls for the operator, which are almost a duplicate of most of the driver's controls
+ */
 public class Operator {
-  private static GameController operator = new GameController(Constants.oi.kOperatorJoy);
 
-  public static void configureControls() {
-    operator.get(Button.A).whenPressed(new DoNothing());
-  }
+    private final GameController driver = new GameController(Constants.OPERATOR_JOY);
+
+    private final Drivetrain drive;
+    private final Elevator elevator;
+    private final Intake intake;
+    private final Indexer indexer;
+    private final Outtake outtake;
+    private final Climb climb;
+    private final Arm arm;
+    private int alignmentDirection = 0;
+    
+    public Operator(Drivetrain drive, Elevator elevator, Intake intake, Indexer indexer, Outtake outtake, Climb climb, Arm arm) {
+        this.drive = drive;
+        this.elevator = elevator;
+        this.intake = intake;
+        this.indexer = indexer;
+        this.outtake = outtake;
+        this.climb = climb;
+        this.arm = arm;
+    }
+
+    public void configureControls() {
+        // TODO: Update to match new driver controls, except the ones that move the drivetrain
+        Trigger menu = driver.get(Button.LEFT_JOY);
+
+        // Elevator setpoints
+        if(elevator != null && outtake != null) {
+            driver.get(Button.BACK).onTrue(new MoveElevator(elevator, ElevatorConstants.L1_SETPOINT).deadlineFor(new RemoveAlgae(outtake)));
+            driver.get(Button.LB).onTrue(new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT).deadlineFor(new RemoveAlgae(outtake)));
+            driver.get(Button.RB).and(menu.negate()).onTrue(new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT).deadlineFor(new RemoveAlgae(outtake)));
+            driver.get(driver.LEFT_TRIGGER_BUTTON).onTrue(new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT).deadlineFor(new RemoveAlgae(outtake)));
+            driver.get(Button.Y).and(menu.negate()).onTrue(new MoveElevator(elevator, ElevatorConstants.STOW_SETPOINT));
+        }
+
+        // Intake/outtake
+        Trigger r3 = driver.get(Button.RIGHT_JOY);
+        if(intake != null && indexer != null){// && elevator != null){
+            driver.get(Button.A).and(menu.negate()).and(r3.negate()).whileTrue(new IntakeCoral(intake, indexer, elevator, outtake, arm));
+            // On true, run the command to start intaking
+            // On false, run the command to finish intaking if it has a coral
+            Command startIntake = new StationIntake(outtake);
+            Command finishIntake = new DoNothing();
+            driver.get(Button.A).and(r3).and(menu.negate()).onTrue(startIntake)
+                .onFalse(new InstantCommand(()->{
+                    if(!startIntake.isScheduled()){
+                        finishIntake.schedule();
+                    }else{
+                        startIntake.cancel();
+                    }
+            }));
+        }
+        if(intake != null){
+            driver.get(Button.A).and(menu).whileTrue(new IntakeAlgae(intake));
+            driver.get(DPad.DOWN).and(menu).onTrue(new OuttakeAlgaeIntake(intake));
+        }
+        if(outtake != null && elevator != null){
+            driver.get(DPad.DOWN).and(menu.negate()).onTrue(new OuttakeCoral(outtake, elevator, arm));
+        }
+        if(intake != null && indexer != null){
+            driver.get(Button.B).and(menu.negate()).whileTrue(new ReverseMotors(intake, indexer, outtake));
+        }
+
+        // Climb
+        if(climb != null){
+            driver.get(Button.X).and(menu.negate()).onTrue(new InstantCommand(()->climb.extend(), climb))
+                .onFalse(new InstantCommand(()->climb.climb(), climb));
+            if(intake != null){
+                driver.get(Button.X).and(menu.negate()).onTrue(new InstantCommand(()->intake.setAngle(IntakeConstants.ALGAE_SETPOINT), intake));
+            }
+        }
+
+        // Alignment
+        driver.get(Button.B).and(menu).onTrue(new InstantCommand(()->alignmentDirection = 0));
+        driver.get(Button.Y).and(menu).onTrue(new InstantCommand(()->alignmentDirection = 2));
+        driver.get(Button.X).and(menu).onTrue(new InstantCommand(()->alignmentDirection = 3));
+        driver.get(Button.RB).onTrue(new InstantCommand(()->alignmentDirection = 4));
+        driver.get(DPad.UP).onTrue(new InstantCommand(()->alignmentDirection = 5));
+        driver.get(DPad.LEFT).onTrue(new InstantCommand(()->setAlignmentPose(true)));
+        driver.get(DPad.RIGHT).onTrue(new InstantCommand(()->setAlignmentPose(false)));
+
+        // Cancel commands
+        driver.get(Button.START).onTrue(new InstantCommand(()->{
+            if(elevator != null){
+                elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT);
+            }
+            if(outtake != null){
+                outtake.stop();
+            }
+            if(intake != null){
+                intake.stow();
+                intake.deactivate();
+            }
+            if(indexer != null){
+                indexer.stop();
+            }
+            if(climb != null){
+                climb.stow();
+            }
+            drive.setDesiredPose(()->null);
+            CommandScheduler.getInstance().cancelAll();
+        }));
+    }
+
+    /**
+     * Sets the drivetrain's alignmetn pose to the selected position
+     * @param isLeft True for left branch, false for right
+     */
+    private void setAlignmentPose(boolean isLeft){
+        Pose2d pose = VisionConstants.REEF.fromAprilTagIdAndPose(
+            Robot.getAlliance() == Alliance.Blue ? alignmentDirection + 17
+            : (8-alignmentDirection) % 6 + 6,
+        isLeft).pose;
+        drive.setDesiredPose(pose);
+    }
+
+    public Trigger getRightTrigger(){
+        return new Trigger(driver.RIGHT_TRIGGER_BUTTON);
+    }
+    public Trigger getLeftTrigger(){
+        return new Trigger(driver.LEFT_TRIGGER_BUTTON);
+    }
+    public GameController getGameController(){
+        return driver;
+    }
 }

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
new file mode 100644 (file)
index 0000000..eaea0f9
--- /dev/null
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -0,0 +1,486 @@
+package frc.robot.controls;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.StartEndCommand;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.Robot;
+import frc.robot.commands.DoNothing;
+import frc.robot.commands.drive_comm.DriveToPose;
+import frc.robot.commands.gpm.IntakeAlgae;
+import frc.robot.commands.gpm.IntakeAlgaeArm;
+import frc.robot.commands.gpm.IntakeCoral;
+import frc.robot.commands.gpm.MoveArm;
+import frc.robot.commands.gpm.MoveElevator;
+import frc.robot.commands.gpm.OuttakeAlgae;
+import frc.robot.commands.gpm.NetSetpoint;
+import frc.robot.commands.gpm.OuttakeCoral;
+import frc.robot.commands.gpm.ResetClimb;
+import frc.robot.commands.gpm.ReverseMotors;
+import frc.robot.commands.gpm.StationIntake;
+import frc.robot.constants.ArmConstants;
+import frc.robot.constants.Constants;
+import frc.robot.constants.ElevatorConstants;
+import frc.robot.constants.FieldConstants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.subsystems.arm.Arm;
+import frc.robot.subsystems.climb.Climb;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.subsystems.indexer.Indexer;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.outtake.Outtake;
+import lib.controllers.PS5Controller;
+import lib.controllers.PS5Controller.DPad;
+import lib.controllers.PS5Controller.PS5Axis;
+import lib.controllers.PS5Controller.PS5Button;
+
+/**
+ * Driver controls for the PS5 controller
+ */
+public class PS5ControllerDriverConfig extends BaseDriverConfig {
+    private final boolean autoOuttake = true;
+
+    private final PS5Controller driver = new PS5Controller(Constants.DRIVER_JOY);
+    private final Elevator elevator;
+    private final Intake intake;
+    private final Indexer indexer;
+    private final Outtake outtake;
+    private final Climb climb;
+    private final Arm arm;
+    private final BooleanSupplier slowModeSupplier = ()->false;
+    private Pose2d alignmentPose = null;
+    // 0 == not selected, -1 == left, 1 == right
+    private byte selectedDirection = 0;
+
+    public PS5ControllerDriverConfig(Drivetrain drive, Elevator elevator, Intake intake, Indexer indexer, Outtake outtake, Climb climb, Arm arm) {
+        super(drive);
+        this.elevator = elevator;
+        this.intake = intake;
+        this.indexer = indexer;
+        this.outtake = outtake;
+        this.climb = climb;
+        this.arm = arm;
+    }
+
+    public void configureControls() {
+        Trigger menu = driver.get(PS5Button.LEFT_JOY);
+
+        // Elevator setpoints
+        if(elevator != null && arm != null && outtake != null) {
+            driver.get(PS5Button.OPTIONS).and(menu.negate()).onTrue(
+                new SequentialCommandGroup(
+                    new InstantCommand(()->setAlignmentPose(false, true)),
+                    new ConditionalCommand(
+                        new ParallelCommandGroup(
+                            new MoveElevator(elevator, ElevatorConstants.L1_SETPOINT),
+                            new MoveArm(arm, ArmConstants.L1_SETPOINT),
+                            new DriveToPose(getDrivetrain(), ()->alignmentPose)
+                        ),
+                        // This is instant so it doesn't requre the drivetrain for more than 1 frame
+                        new InstantCommand(()->{
+                            elevator.setSetpoint(ElevatorConstants.L1_SETPOINT);
+                            arm.setSetpoint(ArmConstants.L1_SETPOINT);
+                        }),
+                        ()->selectedDirection != 0
+                    ),
+                    new ConditionalCommand(
+                        new SequentialCommandGroup(
+                            new OuttakeCoral(outtake, elevator, arm),
+                            new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT),
+                            new InstantCommand(()->{
+                                elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT);
+                                arm.setSetpoint(ArmConstants.INTAKE_SETPOINT);
+                                alignmentPose = null;
+                                selectedDirection = 0;
+                            }, elevator, arm)
+                        ),
+                        new DoNothing(),
+                        () -> selectedDirection != 0 && autoOuttake
+                    )
+                )
+            );
+
+            driver.get(PS5Button.LEFT_TRIGGER).onTrue(
+                new SequentialCommandGroup(
+                    new InstantCommand(()->setAlignmentPose(true)),
+                    new ConditionalCommand(
+                        new ParallelCommandGroup(
+                            new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT),
+                            new ConditionalCommand(
+                                new MoveArm(arm, ArmConstants.L4_SETPOINT_RIGHT),
+                                new MoveArm(arm, ArmConstants.L4_SETPOINT_LEFT),
+                                () -> selectedDirection >= 0
+                            ),
+                            new DriveToPose(getDrivetrain(), ()->alignmentPose)
+                        ),
+                        // This is instant so it doesn't requre the drivetrain for more than 1 frame
+                        new InstantCommand(()->{
+                            elevator.setSetpoint(ElevatorConstants.L4_SETPOINT);
+                            arm.setSetpoint(ArmConstants.L4_SETPOINT_RIGHT);
+                        }),
+                        ()->selectedDirection != 0
+                    ),
+                    new ConditionalCommand(
+                        new WaitCommand(0.5),
+                        new DoNothing(),
+                        () -> selectedDirection < 0
+                    ),
+                    new ConditionalCommand(
+                        new OuttakeCoral(outtake, elevator, arm)
+                        .andThen(new InstantCommand(()->{
+                            elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT);
+                            arm.setSetpoint(ArmConstants.INTAKE_SETPOINT);
+                            alignmentPose = null;
+                            selectedDirection = 0;
+                        }, elevator, arm)),
+                        new DoNothing(),
+                        () -> selectedDirection != 0 && autoOuttake
+                    )
+                )
+            );
+
+            Command l2Coral = new SequentialCommandGroup(
+                new SequentialCommandGroup(
+                    new InstantCommand(()->setAlignmentPose(false)),
+                    new ConditionalCommand(
+                        new ParallelCommandGroup(
+                            new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT),
+                            new MoveArm(arm, ArmConstants.L2_L3_SETPOINT),
+                            new DriveToPose(getDrivetrain(), ()->alignmentPose)
+                        ),
+                        // This is instant so it doesn't requre the drivetrain for more than 1 frame
+                        new InstantCommand(()->{
+                            elevator.setSetpoint(ElevatorConstants.L2_SETPOINT);
+                            arm.setSetpoint(ArmConstants.L2_L3_SETPOINT);
+                        }),
+                        ()->selectedDirection != 0
+                    ),
+                    new ConditionalCommand(
+                        new OuttakeCoral(outtake, elevator, arm)
+                        .andThen(new InstantCommand(()->{
+                            elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT);
+                            arm.setSetpoint(ArmConstants.INTAKE_SETPOINT);
+                            alignmentPose = null;
+                            selectedDirection = 0;
+                        }, elevator, arm)),
+                        new DoNothing(),
+                        () -> selectedDirection != 0 && autoOuttake
+                    )
+                )
+            );
+            Command l3Coral = new SequentialCommandGroup(
+                new SequentialCommandGroup(
+                    new InstantCommand(()->setAlignmentPose(false)),
+                    new ConditionalCommand(
+                        new ParallelCommandGroup(
+                            new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT),
+                            new MoveArm(arm, ArmConstants.L2_L3_SETPOINT),
+                            new DriveToPose(getDrivetrain(), ()->alignmentPose)
+                        ),
+                        // This is instant so it doesn't requre the drivetrain for more than 1 frame
+                        new InstantCommand(()->{
+                            elevator.setSetpoint(ElevatorConstants.L3_SETPOINT);
+                            arm.setSetpoint(ArmConstants.L2_L3_SETPOINT);
+                        }),
+                        ()->selectedDirection != 0
+                    ),
+                    new ConditionalCommand(
+                        new OuttakeCoral(outtake, elevator, arm)
+                        .andThen(new InstantCommand(()->{
+                            elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT);
+                            arm.setSetpoint(ArmConstants.INTAKE_SETPOINT);
+                            alignmentPose = null;
+                            selectedDirection = 0;
+                        }, elevator, arm)),
+                        new DoNothing(),
+                        () -> selectedDirection != 0 && autoOuttake
+                    )
+                )
+            );
+            Command l2Algae = new ParallelCommandGroup(
+                new MoveElevator(elevator, ElevatorConstants.BOTTOM_ALGAE_SETPOINT),
+                new MoveArm(arm, ArmConstants.ALGAE_SETPOINT)).andThen(new IntakeAlgaeArm(outtake));
+            Command l3Algae = new ParallelCommandGroup(
+                new MoveElevator(elevator, ElevatorConstants.TOP_ALGAE_SETPOINT),
+                new MoveArm(arm, ArmConstants.ALGAE_SETPOINT)).andThen(new IntakeAlgaeArm(outtake));
+            driver.get(PS5Button.RB).whileTrue(new ConditionalCommand(l2Algae, new InstantCommand(l2Coral::schedule), menu));
+            driver.get(PS5Button.LB).whileTrue(new ConditionalCommand(l3Algae, new InstantCommand(l3Coral::schedule), menu));
+    
+            //Processor setpoint
+            driver.get(PS5Button.TRIANGLE).and(menu.negate()).onTrue(
+                new ParallelCommandGroup(
+                    new MoveElevator(elevator, ElevatorConstants.SAFE_SETPOINT + 0.001),
+                    new MoveArm(arm, ArmConstants.PROCESSOR_SETPOINT)
+                )
+            );
+            driver.get(DPad.UP).onTrue(new NetSetpoint(elevator, arm, getDrivetrain()));
+        }
+
+        // Intake/outtake
+        Trigger r3 = driver.get(PS5Button.RIGHT_JOY);
+
+        if(intake != null && indexer != null && elevator != null && outtake != null && arm != null){
+            boolean toggle = true;
+            Command intakeCoral = new IntakeCoral(intake, indexer, elevator, outtake, arm);
+            Command intakeAlgae = new IntakeAlgae(intake);
+            driver.get(PS5Button.CROSS).onTrue(new InstantCommand(()->{
+                if(r3.getAsBoolean()) return;
+                if(menu.getAsBoolean()){
+                    intakeAlgae.schedule();
+                }else{
+                    if(toggle){
+                        if(intakeCoral.isScheduled()){
+                            intakeCoral.cancel();
+                        }else{
+                            intakeCoral.schedule();
+                        }
+                    }else{
+                        intakeCoral.schedule();
+                    }
+                }
+            })).onFalse(new InstantCommand(()->{
+                if(!toggle){
+                    intakeCoral.cancel();
+                }
+                intakeAlgae.cancel();
+            }));
+            // On true, run the command to start intaking
+            // On false, run the command to finish intaking if it has a coral
+            Command startIntake = new StationIntake(outtake);
+            // Command finishIn6take = new FinishStationIntake(intake, indexer, elevator, outtake);
+            // driver.get(PS5Button.CROSS).and(r3).and(menu.negate()).onTrue(startIntake)
+            //     .onFalse(new InstantCommand(()->{
+            //         if(!startIntake.isScheduled()){
+            //             // finishIntake.schedule();
+            //         }else{
+            //             startIntake.cancel();
+            //         }
+            // }));
+            driver.get(PS5Button.CROSS).and(r3).onTrue(
+            new SequentialCommandGroup(
+            new MoveElevator(elevator, ElevatorConstants.STATION_INTAKE_SETPOINT),
+            new MoveArm(arm, ArmConstants.STATION_INTAKE_SETPOINT)).
+            andThen(startIntake));
+        }
+
+        if(intake != null && outtake != null && arm != null && elevator != null){
+            Command algae = new SequentialCommandGroup(
+                new OuttakeAlgae(outtake, intake),
+                // Only move the arm and elevator in sequence when scoring in the net
+                new ConditionalCommand(
+                    new SequentialCommandGroup(
+                        new MoveArm(arm, ArmConstants.ALGAE_STOW_SETPOINT),
+                        new InstantCommand(()-> elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT)),
+                        new WaitCommand(0.25),
+                        new InstantCommand(()-> arm.setSetpoint(ArmConstants.INTAKE_SETPOINT))
+                    ),
+                    new InstantCommand(()->{
+                        elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT);
+                        arm.setSetpoint(ArmConstants.INTAKE_SETPOINT);
+                    }),
+                    // 1 meter is in between L3 and net setpoints
+                    () -> elevator.getSetpoint() > 1
+                )
+            );
+            Command coral = new OuttakeCoral(outtake, elevator, arm).alongWith(new InstantCommand(()->getDrivetrain().setDesiredPose(()->null)))
+                .andThen(
+                    new ConditionalCommand(
+                        new SequentialCommandGroup(new MoveArm(arm, ArmConstants.INTAKE_SETPOINT), new MoveElevator(elevator, ElevatorConstants.STOW_SETPOINT), new InstantCommand(()->selectedDirection = 0)),
+                        new DoNothing(),
+                        ()->!arm.canMoveElevator()
+                    ));
+            Command cancelAlign = new InstantCommand(()->{}, getDrivetrain());
+
+            driver.get(DPad.DOWN).onTrue(new InstantCommand(()->{
+                if(menu.getAsBoolean()){
+                    algae.schedule();
+                }else{
+                    coral.schedule();
+                }
+                cancelAlign.schedule();
+            }));
+        }
+        if(intake != null && indexer != null && outtake != null){
+            driver.get(PS5Button.CIRCLE).and(menu.negate()).whileTrue(new ReverseMotors(intake, indexer, outtake));
+        }
+
+        // Climb
+        if(climb != null){
+            driver.get(PS5Button.SQUARE).and(menu.negate()).toggleOnTrue(new StartEndCommand(()->climb.extend(), ()->climb.climb(), climb));
+            if(intake != null){
+                driver.get(PS5Button.SQUARE).and(menu.negate()).onTrue(new InstantCommand(()->intake.setAngle(65), intake));
+            }
+            driver.get(PS5Button.PS).and(menu).whileTrue(new ResetClimb(climb));
+            driver.get(PS5Button.RIGHT_TRIGGER).and(menu).onTrue(new InstantCommand(()->climb.stow(), climb));
+        }
+
+        // Alignment
+        driver.get(DPad.LEFT).toggleOnTrue(new InstantCommand(()->{
+            selectedDirection = -1;
+        }));
+        driver.get(DPad.RIGHT).toggleOnTrue(new InstantCommand(()->{
+            selectedDirection = 1;
+        }));
+        driver.get(PS5Button.TOUCHPAD).toggleOnTrue(new InstantCommand(()->{
+            setAlignmentPose(true, false, false, false);
+        }).andThen(new DriveToPose(getDrivetrain(), ()->alignmentPose)));
+
+        // Reset the yaw. Mainly useful for testing/driver practice
+        driver.get(PS5Button.CREATE).and(menu.negate()).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
+                new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)
+        )));
+        driver.get(PS5Button.CREATE).and(menu).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
+                new Rotation2d(Robot.getAlliance() == Alliance.Blue ? Math.PI*5/6 : -Math.PI/6)
+        )));
+
+        // Cancel commands
+        driver.get(PS5Button.RIGHT_TRIGGER).and(menu.negate()).onTrue(new InstantCommand(()->{
+            if(elevator != null){
+                if(outtake != null && outtake.coralLoaded()){
+                    elevator.setSetpoint(ElevatorConstants.INTAKE_STOW_SETPOINT);
+                }else{
+                    elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT);
+                }
+            }
+            if(outtake != null){
+                outtake.stop();
+            }
+            if(intake != null){
+                intake.stow();
+                intake.deactivate();
+            }
+            if(indexer != null){
+                indexer.stop();
+            }
+            if(climb != null){
+                climb.stow();
+            }
+            if(arm != null){
+                if(outtake != null && outtake.coralLoaded()){
+                    arm.setSetpoint(ArmConstants.STOW_SETPOINT);
+                }else{
+                    arm.setSetpoint(ArmConstants.START_ANGLE);
+                }
+            }
+            getDrivetrain().setIsAlign(false);
+            getDrivetrain().setDesiredPose(()->null);
+            alignmentPose = null;
+            selectedDirection = 0;
+            CommandScheduler.getInstance().cancelAll();
+        }));
+
+        driver.get(PS5Button.MUTE).and(menu).onTrue(new FunctionalCommand(
+            ()->getDrivetrain().setStateDeadband(false),
+            getDrivetrain()::alignWheels,
+            interrupted->getDrivetrain().setStateDeadband(true),
+            ()->false, getDrivetrain()).withTimeout(2));
+    }
+
+    /**
+     * Sets the drivetrain's alignmetn pose to the nearest reef branch or algae location
+     * @param isAlgae True for algae, false for branches
+     * @param isLeft True for left branch, false for right, ignored for algae
+     * @param l4 If the robot should align to the L4 scoring pose
+     */
+    private void setAlignmentPose(boolean isAlgae, boolean isLeft, boolean l4, boolean l1){
+        Translation2d drivePose = getDrivetrain().getPose().getTranslation();
+        int closestId = 0;
+        double closestDist = 20;
+        boolean isRed = Robot.getAlliance() == Alliance.Red;
+        int start = isRed ? 5 : 16;
+        for(int i = 0; i < 6; i++){
+            double dist = FieldConstants.APRIL_TAGS.get(start+i).pose.toPose2d().getTranslation().getDistance(drivePose);
+            if(dist < closestDist){
+                closestDist = dist;
+                closestId = start+i+1;
+            }
+        }
+        if(isAlgae){
+            alignmentPose = VisionConstants.REEF.fromAprilTagIdAlgae(closestId).pose;
+        }else{
+            if(l4){
+                alignmentPose = VisionConstants.REEF.fromAprilTagIdAndPose(closestId, isLeft).l4Pose;
+            }else if(l1){
+                alignmentPose = VisionConstants.REEF.fromAprilTagIdAndPose(closestId, isLeft).l1Pose;
+            }else{
+                alignmentPose = VisionConstants.REEF.fromAprilTagIdAndPose(closestId, isLeft).pose;
+            }
+        }
+    }
+
+    /**
+     * Sets the drivetrain's alignmetn pose to the nearest reef branch with the selected direction
+     * @param l4 If the robot should align to the L4 scoring pose
+     * @param l1 If the robot should align to the L1 scoring pose
+     */
+    private void setAlignmentPose(boolean l4, boolean l1){
+        if(selectedDirection == 0){
+            alignmentPose = null;
+            return;
+        }
+        setAlignmentPose(false, selectedDirection < 0, l4, l1);
+    }
+    /**
+     * Sets the drivetrain's alignmetn pose to the nearest reef branch with the selected direction
+     * @param l4 If the robot should align to the L4 scoring pose
+     */
+    private void setAlignmentPose(boolean l4){
+        setAlignmentPose(l4, false);
+    }
+    
+    @Override
+    public double getRawSideTranslation() {
+        return driver.get(PS5Axis.LEFT_X);
+    }
+
+    @Override
+    public double getRawForwardTranslation() {
+        return driver.get(PS5Axis.LEFT_Y);
+    }
+
+    @Override
+    public double getRawRotation() {
+        return driver.get(PS5Axis.RIGHT_X);
+    }
+
+    @Override
+    public double getRawHeadingAngle() {
+        return Math.atan2(driver.get(PS5Axis.RIGHT_X), -driver.get(PS5Axis.RIGHT_Y)) - Math.PI / 2;
+    }
+
+    @Override
+    public double getRawHeadingMagnitude() {
+        return Math.hypot(driver.get(PS5Axis.RIGHT_X), driver.get(PS5Axis.RIGHT_Y));
+    }
+
+    @Override
+    public boolean getIsSlowMode() {
+        return slowModeSupplier.getAsBoolean();
+    }
+
+    @Override
+    public boolean getIsAlign() {
+        return false;
+    }
+
+    public void startRumble(){
+        driver.rumbleOn();
+    }
+
+    public void endRumble(){
+        driver.rumbleOff();
+    }
+}

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
deleted file mode 100644 (file)
index b3646ea..0000000
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ /dev/null
@@ -1,64 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc.robot.subsystems;
-
-import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.Constants;
-import frc.robot.util.MotorFactory;
-
-public class Drivetrain extends SubsystemBase {
-
-  private final WPI_TalonFX m_leftMotor1;
-  private final WPI_TalonFX m_leftMotor2;
-  private final WPI_TalonFX m_rightMotor1;
-  private final WPI_TalonFX m_rightMotor2;
-
-  public Drivetrain() {
-
-    m_leftMotor1 = MotorFactory.createTalonFX(Constants.drive.kLeftMotor1, Constants.kRioCAN);
-    m_leftMotor2 = MotorFactory.createTalonFX(Constants.drive.kLeftMotor2, Constants.kRioCAN);
-    m_rightMotor1 = MotorFactory.createTalonFX(Constants.drive.kRightMotor1, Constants.kRioCAN);
-    m_rightMotor2 = MotorFactory.createTalonFX(Constants.drive.kRightMotor2, Constants.kRioCAN);
-
-    SupplyCurrentLimitConfiguration supplyCurrentLimit = new SupplyCurrentLimitConfiguration(true, 40, 45, 1);
-
-    m_leftMotor1.configSupplyCurrentLimit(supplyCurrentLimit);
-    m_leftMotor2.configSupplyCurrentLimit(supplyCurrentLimit);
-    m_rightMotor1.configSupplyCurrentLimit(supplyCurrentLimit);
-    m_rightMotor2.configSupplyCurrentLimit(supplyCurrentLimit);
-
-    m_leftMotor2.follow(m_leftMotor1);
-    m_rightMotor2.follow(m_rightMotor1);
-  }
-
-  /**
-   * Drives the robot using tank drive controls Tank drive is slightly easier to code but less
-   * intuitive to control, so this is here as an example for now
-   *
-   * @param leftPower the commanded power to the left motors
-   * @param rightPower the commanded power to the right motors
-   */
-  public void tankDrive(double leftPower, double rightPower) {
-    m_leftMotor1.set(ControlMode.PercentOutput, leftPower);
-    m_rightMotor1.set(ControlMode.PercentOutput, rightPower);
-  }
-
-  /**
-   * Drives the robot using arcade controls.
-   *
-   * @param forward the commanded forward movement
-   * @param turn the commanded turn rotation
-   */
-  public void arcadeDrive(double throttle, double turn) {
-    m_leftMotor1.set(ControlMode.PercentOutput, throttle + turn);
-    m_rightMotor1.set(ControlMode.PercentOutput, throttle - turn);
-  }
-}

diff --git a/src/main/java/frc/robot/subsystems/LED/LED.java b/src/main/java/frc/robot/subsystems/LED/LED.java
new file mode 100644 (file)
index 0000000..a521639
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/LED/LED.java
@@ -0,0 +1,90 @@
+package frc.robot.subsystems.LED;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.IdConstants;
+
+import com.ctre.phoenix.led.CANdle;
+import com.ctre.phoenix.led.CANdle.LEDStripType;
+import com.ctre.phoenix.led.CANdle.VBatOutputMode;
+import com.ctre.phoenix.led.Animation;
+
+public class LED extends SubsystemBase {
+
+    private CANdle candle;
+    public static final int stripLength = 67;
+
+    // Constructor
+    public LED() {
+        this.candle = new CANdle(IdConstants.CANDLE_ID, "rio");
+
+        candle.configStatusLedState(false);
+        candle.configLOSBehavior(false);
+
+        System.out.println("Strip type ec: " + candle.configLEDType(LEDStripType.GRB));
+
+        candle.configBrightnessScalar(1);
+        candle.configVBatOutput(VBatOutputMode.On);
+        candle.configV5Enabled(true); // Turns off LEDs
+    }
+
+    @Override
+    public void periodic() {
+    }
+
+    /**
+     * Sets the color of all the LEDs.
+     *
+     * @param red   Red value (0-255)
+     * @param green Green value (0-255)
+     * @param blue  Blue value (0-255)
+     */
+    public void setLEDs(int red, int green, int blue) {
+        candle.setLEDs(red, green, blue);
+    }
+
+   /**
+     * Sets an animation for the LEDs.
+     *
+     * @param animation The animation object (e.g., RainbowAnimation, StrobeAnimation, etc.)
+     */
+    public void animate(Animation animation) {
+        candle.animate(animation);
+    }
+
+    /**
+     * Sets the color of a specific section of LEDs.
+     *
+     * @param r     Red value (0-255)
+     * @param g     Green value (0-255)
+     * @param b     Blue value (0-255)
+     * @param start Start index of the section
+     * @param end   End index of the section
+     */
+    public void setSection(int r, int g, int b, int start, int end) {
+        candle.setLEDs(r, g, b, 0, start, end);
+    }
+
+    /**
+     * Creates an alternating pattern of two colors across the LEDs.
+     *
+     * @param r1     Red value of the first color (0-255)
+     * @param g1     Green value of the first color (0-255)
+     * @param b1     Blue value of the first color (0-255)
+     * @param r2     Red value of the second color (0-255)
+     * @param g2     Green value of the second color (0-255)
+     * @param b2     Blue value of the second color (0-255)
+     * @param size   Size of each color block
+     * @param offset Offset for the starting position of the pattern
+     * @param total  Total number of LEDs
+     */
+    public void alternate(int r1, int g1, int b1, int r2, int g2, int b2, int size, int offset, int total) {
+        for (int i = -offset; i < total; i += size) {
+            boolean color2 = ((i - offset) / size) % 2 == 0;
+            if (color2) {
+                setSection(r2, g2, b2, i, i + size - 1);
+            } else {
+                setSection(r1, g1, b1, i, i + size - 1);
+            }
+        }
+    }
+}

diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java
new file mode 100644 (file)
index 0000000..0a075db
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/arm/Arm.java
@@ -0,0 +1,168 @@
+package frc.robot.subsystems.arm;
+
+import java.util.function.BooleanSupplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.sim.TalonFXSimState;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.ArmConstants;
+import frc.robot.constants.Constants;
+import frc.robot.constants.IdConstants;
+import frc.robot.util.PhoenixUtil;
+
+public class Arm extends SubsystemBase implements ArmIO {
+    //motor
+    private TalonFX motor = new TalonFX(IdConstants.ARM_MOTOR);
+    private TalonFXSimState encoderSim;
+
+    // Mechism2d display
+    private Mechanism2d simulationMechanism;
+    private MechanismLigament2d simLigament;
+    private SingleJointedArmSim armSim;
+
+    private final DutyCycleEncoder absoluteEncoder = new DutyCycleEncoder(IdConstants.ARM_ABSOLUTE_ENCODER);
+
+    private double setpoint = ArmConstants.START_ANGLE;
+
+    private MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0);
+
+    private final ArmFeedforward feedforward = new ArmFeedforward(0, ArmConstants.MASS*ArmConstants.CENTER_OF_MASS_LENGTH/ArmConstants.GEAR_RATIO/ArmConstants.MOTOR.KtNMPerAmp*ArmConstants.MOTOR.rOhms, 0);
+
+    private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged();
+
+    private BooleanSupplier elevatorStowed;
+
+    public Arm() {
+        if (RobotBase.isSimulation()) {
+            encoderSim = motor.getSimState();
+            encoderSim.setRawRotorPosition(Units.degreesToRotations(ArmConstants.START_ANGLE)*ArmConstants.GEAR_RATIO);
+            armSim = new SingleJointedArmSim(
+                ArmConstants.MOTOR, 
+                ArmConstants.GEAR_RATIO,
+                ArmConstants.MOI, 
+                ArmConstants.LENGTH, 
+                Units.degreesToRadians(ArmConstants.MIN_ANGLE), //min angle
+                Units.degreesToRadians(ArmConstants.MAX_ANGLE), //max angle
+                true, 
+                Units.degreesToRadians(ArmConstants.START_ANGLE));
+            simulationMechanism = new Mechanism2d(3, 3);
+            MechanismRoot2d root = simulationMechanism.getRoot("Arm", 1.5, 1.5);
+            simLigament = root.append(
+                new MechanismLigament2d("angle", 1, ArmConstants.START_ANGLE, 4, new Color8Bit(Color.kAliceBlue))
+            );
+            SmartDashboard.putData("Arm Display", simulationMechanism);
+        }
+
+        // resetAbsolute();
+        motor.setPosition(Units.degreesToRotations(ArmConstants.START_ANGLE)*ArmConstants.GEAR_RATIO);
+
+        var talonFXConfigs = new TalonFXConfiguration();
+        
+        var slot0Configs = talonFXConfigs.Slot0;
+        slot0Configs.kS = 0.1;  // Static friction compensation (should be >0 if friction exists)
+        slot0Configs.kG = ArmConstants.MASS * 9.81 * ArmConstants.CENTER_OF_MASS_LENGTH / ArmConstants.GEAR_RATIO; // Gravity compensation
+        slot0Configs.kV = 0.12; // Velocity gain: 1 rps -> 0.12V
+        slot0Configs.kA = 0;  // Acceleration gain: 1 rps² -> 0V (should be tuned if acceleration matters)
+        slot0Configs.kP = 0.85; // If position error is 2.5 rotations, apply 12V (0.5 * 2.5 * 12V)
+        slot0Configs.kI = 0.;   // Integral term (usually left at 0 for MotionMagic)
+        slot0Configs.kD = 0;   // Derivative term (used to dampen oscillations)
+
+        // set Motion Magic settings
+        var motionMagicConfigs = talonFXConfigs.MotionMagic;
+        motionMagicConfigs.MotionMagicCruiseVelocity = ArmConstants.MAX_VELOCITY * ArmConstants.GEAR_RATIO/Math.PI/2;
+        motionMagicConfigs.MotionMagicAcceleration = ArmConstants.MAX_ACCELERATION * ArmConstants.GEAR_RATIO/Math.PI/2;
+        talonFXConfigs.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+        motor.getConfigurator().apply(talonFXConfigs);
+        updateInputs();
+        PhoenixUtil.tryUntilOk(100, ()->motor.setNeutralMode(NeutralModeValue.Brake));
+
+    }
+
+    public void setElevatorStowed(BooleanSupplier elevatorStowed){
+        this.elevatorStowed = elevatorStowed;
+    }
+
+    @Override
+    public void periodic() {
+        double setpoint2 = setpoint;
+        if(elevatorStowed == null || elevatorStowed.getAsBoolean() && Math.abs(setpoint2-ArmConstants.L1_SETPOINT) > 0.0001){
+            setpoint2 = ArmConstants.START_ANGLE;
+        }
+        double setpointRotations = Units.degreesToRotations(setpoint2) * ArmConstants.GEAR_RATIO;
+        motor.setControl(voltageRequest.withPosition(setpointRotations).withFeedForward(feedforward.calculate(Units.degreesToRadians(getAngle()), 0)));
+        updateInputs();
+        Logger.recordOutput("Arm/Atsetpoint",atSetpoint());
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        armSim.setInputVoltage(getAppliedVoltage());
+        armSim.update(Constants.LOOP_TIME);
+
+        double armRotations = Units.radiansToRotations(armSim.getAngleRads());
+        encoderSim.setRawRotorPosition(armRotations * ArmConstants.GEAR_RATIO);
+        simLigament.setAngle(getAngle());
+    }
+
+    public void setSetpoint(double setpoint) {
+        
+        this.setpoint = MathUtil.clamp(setpoint, ArmConstants.MIN_ANGLE, ArmConstants.MAX_ANGLE);
+    }
+
+    public double getAppliedVoltage() {
+        return motor.getMotorVoltage().getValueAsDouble();
+    }
+
+    /**
+     * Gets the angle of the arm
+     * @return The angle in degrees
+     */
+    public double getAngle() {
+        return inputs.measuredAngle;
+    }
+
+    public void resetAbsolute(){
+        if(RobotBase.isSimulation()){
+            motor.setPosition(Units.degreesToRotations(ArmConstants.START_ANGLE)*ArmConstants.GEAR_RATIO);
+        }else{
+            double absolutePosition = absoluteEncoder.get() / ArmConstants.ENCODER_GEAR_RATIO;
+            motor.setPosition(MathUtil.inputModulus(absolutePosition - Units.degreesToRotations(ArmConstants.OFFSET), -0.5, 0.5)*ArmConstants.GEAR_RATIO);
+        }
+    }
+
+    public boolean atSetpoint() {
+        return Math.abs(getAngle() - setpoint) < ArmConstants.TOLERANCE;
+    }
+
+    public boolean canMoveElevator() {
+        return Math.abs(getAngle() - ArmConstants.START_ANGLE) < 5 || Math.abs(getAngle() - ArmConstants.L1_SETPOINT) < 5;
+    }
+
+    @Override
+    public void updateInputs(){
+        inputs.measuredAngle = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / ArmConstants.GEAR_RATIO;
+        inputs.currentAmps = motor.getStatorCurrent().getValueAsDouble();
+
+        Logger.processInputs("Arm", inputs);
+        Logger.recordOutput("Arm/setpointDeg", setpoint);
+    }
+}

diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java
new file mode 100644 (file)
index 0000000..8eab615
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java
@@ -0,0 +1,13 @@
+package frc.robot.subsystems.arm;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ArmIO {
+    @AutoLog
+    public static class ArmIOInputs{
+        public double measuredAngle = 0.0;
+        public double currentAmps = 0.0;
+    }
+
+    public void updateInputs();
+}

diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java
new file mode 100644 (file)
index 0000000..9dceaeb
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/climb/Climb.java
@@ -0,0 +1,185 @@
+package frc.robot.subsystems.climb;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.sim.TalonFXSimState;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.Constants;
+import frc.robot.constants.IdConstants;
+import frc.robot.util.ClimbArmSim;
+
+public class Climb extends SubsystemBase {
+    
+    private static final double startingPosition = 0;
+    private static final double extendPosition = 2;
+    private static final double climbPosition = -0.83;
+
+    //Motors
+    private final PIDController pid = new PIDController(2.5, 0, 0.0);
+
+    private TalonFX motor = new TalonFX(IdConstants.CLIMB_MOTOR, Constants.CANIVORE_CAN);
+    private final DCMotor climbGearBox = DCMotor.getKrakenX60(1);
+    private TalonFXSimState encoderSim;
+
+    //Mechism2d display
+    private final Mechanism2d simulationMechanism = new Mechanism2d(3, 3);
+    private final MechanismRoot2d mechanismRoot = simulationMechanism.getRoot("Climb", 1.5, 1.5);
+    private final MechanismLigament2d simLigament = mechanismRoot.append(
+        new MechanismLigament2d("angle", 1, startingPosition, 4, new Color8Bit(Color.kAntiqueWhite))
+    );
+
+    private final double versaPlanetaryGearRatio = 1.0;
+    private final double climbGearRatio = 60.0/1.0;
+    private final double totalGearRatio = versaPlanetaryGearRatio * climbGearRatio;
+
+    private ClimbArmSim climbSim;
+
+    private double power;
+
+    private boolean resetting = false;
+
+    private final ClimbIOInputsAutoLogged inputs = new ClimbIOInputsAutoLogged();
+
+    public Climb() {
+        if (RobotBase.isSimulation()) {
+            encoderSim = motor.getSimState();
+            encoderSim.setRawRotorPosition(Units.degreesToRotations(startingPosition)*totalGearRatio);
+
+            climbSim = new ClimbArmSim(
+                climbGearBox, 
+                totalGearRatio, 
+                0.1, 
+                0.127, 
+                0, //min angle 
+                Units.degreesToRadians(90), //max angle
+                true, 
+                Units.degreesToRadians(startingPosition),
+                60
+                );
+
+            climbSim.setIsClimbing(true);
+            SmartDashboard.putData("Climb Display", simulationMechanism);
+        }
+
+        pid.setIZone(1);
+
+        pid.setSetpoint(Units.degreesToRadians(startingPosition));
+
+        motor.setPosition(Units.degreesToRotations(startingPosition)*totalGearRatio);
+        motor.setNeutralMode(NeutralModeValue.Brake);
+        //SmartDashboard.putData("Climb PID", pid);
+    }
+
+    @Override
+    public void periodic() { 
+
+        inputs.measuredPositionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble() / totalGearRatio);
+        inputs.currentAmps = motor.getStatorCurrent().getValueAsDouble();
+        Logger.processInputs("Climb", inputs);
+
+        double motorPosition = motor.getPosition().getValueAsDouble();
+        double currentPosition = Units.rotationsToRadians(motorPosition/totalGearRatio);
+        power = pid.calculate(currentPosition);
+
+        if(resetting){
+            power = -0.1;
+        }
+
+        Logger.recordOutput("Climb/Motor Power", power);
+        Logger.recordOutput("Climb/setpointDeg", motorPosition/totalGearRatio);
+
+        motor.set(MathUtil.clamp(power, -1, 1));
+    }
+
+
+    @Override
+    public void simulationPeriodic() {
+        climbSim.setInput(power * Constants.ROBOT_VOLTAGE);
+        climbSim.update(Constants.LOOP_TIME);
+
+        double climbRotations = Units.radiansToRotations(climbSim.getAngleRads());
+        encoderSim.setRawRotorPosition(climbRotations * totalGearRatio);
+
+        simLigament.setAngle(Units.radiansToDegrees(getAngle()));
+    }
+
+    /**
+     * Sets the motor to an angle from 0-90 deg
+     * @param angle in degrees
+     */
+    public void setAngle(double angle) {
+        pid.reset();
+        pid.setSetpoint(Units.degreesToRadians(angle));
+    }
+
+    /**
+     * Gets the current position of the motor in degrees
+     * @return The angle in degrees
+     */
+    public double getAngle() {
+        return inputs.measuredPositionDeg;
+    }
+
+    /**
+     * Turns the motor to 90 degrees (extended positiion)
+     */
+    public void extend(){
+        double extendAngle = Units.rotationsToDegrees(extendPosition);
+        setAngle(extendAngle);
+    }
+
+    /**
+     * Turns the motor to 0 degrees (climb position)
+     */
+    public void climb(){
+        setAngle(Units.rotationsToDegrees(climbPosition));
+    }
+
+    /**
+     * Turns the motor to 0 degrees (climb position)
+     */
+    public void stow(){
+        setAngle(startingPosition);
+    }
+
+    public void reset(boolean resetting){
+        this.resetting = resetting;
+        if(!resetting){
+            motor.setPosition(climbPosition*totalGearRatio);
+            pid.setSetpoint(Units.degreesToRadians(startingPosition));
+            pid.reset();
+        }
+    }
+
+    public double getCurrent(){
+        return motor.getStatorCurrent().getValueAsDouble();
+    }
+
+    //not working
+    public Mechanism2d getMech2d() {
+        return simulationMechanism;
+    }
+
+    /**
+     * Gets the estimated angle of the climb
+     * This is slightly inaccurate since it assumes the climb rotates exactly 90 degrees and the motor position is proportional to the climb position
+     * Used only for 3D robot display
+     */
+    public double getEstimatedClimbAngle(){
+        return Units.degreesToRotations(getAngle())/(extendPosition-climbPosition)*Math.PI/2;
+    }
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java
new file mode 100644 (file)
index 0000000..6de5982
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java
@@ -0,0 +1,11 @@
+package frc.robot.subsystems.climb;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ClimbIO {
+    @AutoLog
+    public static class ClimbIOInputs{
+        public double measuredPositionDeg = 0.0;
+        public double currentAmps = 0.0;
+    }
+}

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
new file mode 100644 (file)
index 0000000..0175034
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -0,0 +1,693 @@
+package frc.robot.subsystems.drivetrain;
+
+import java.util.Arrays;
+import java.util.Optional;
+import java.util.concurrent.locks.Lock;
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.function.Supplier;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+import com.pathplanner.lib.util.PathPlannerLogging;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+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.util.Units;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.Constants;
+import frc.robot.constants.FieldConstants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.constants.swerve.ModuleConstants;
+import frc.robot.util.EqualsUtil;
+import frc.robot.util.PhoenixOdometryThread;
+import frc.robot.util.SwerveModulePose;
+import frc.robot.util.SwerveStuff.SwerveSetpoint;
+import frc.robot.util.SwerveStuff.SwerveSetpointGenerator;
+import frc.robot.util.Vision.Vision;
+
+/**
+ * Represents a swerve drive style drivetrain.
+ * <p>
+ * Module IDs are:
+ * 1: Front left
+ * 2: Front right
+ * 3: Back left
+ * 4: Back right
+ */
+public class Drivetrain extends SubsystemBase {
+
+    protected final Module[] modules;
+
+    private final GyroIO gyroIO;
+    private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
+
+    public static Lock odometryLock = new ReentrantLock();
+
+    private SwerveSetpoint currentSetpoint =
+    new SwerveSetpoint(
+        new ChassisSpeeds(),
+        new SwerveModuleState[] {
+          new SwerveModuleState(),
+          new SwerveModuleState(),
+          new SwerveModuleState(),
+          new SwerveModuleState()
+        });
+    // Odometry
+    private final SwerveDrivePoseEstimator poseEstimator;
+
+    // Vision
+    private final Vision vision;
+
+
+    // PID Controllers for chassis movement
+    private final PIDController xController;
+    private final PIDController yController;
+    private final PIDController rotationController;
+
+    // If vision is enabled for drivetrain odometry updating
+    // DO NOT CHANGE THIS HERE TO DISABLE VISION, change VisionConstants.ENABLED instead
+    private boolean visionEnabled = true;
+
+    // Disables vision for the first few seconds after deploying
+    private Timer visionEnableTimer = new Timer();
+
+    // If the robot should algin to the angle
+    private boolean isAlign = false;
+    // Angle to align to, can be null
+    private Double alignAngle = null;
+    // used for drift control
+    private double currentHeading = 0;
+    // used for drift control
+    private boolean drive_turning = false;
+
+    private SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator();
+
+    // The pose supplier to drive to
+    private Supplier<Pose2d> desiredPoSupplier = ()->null;
+
+    private SwerveModulePose modulePoses;
+
+    // The previous pose to reset to if the current pose gets too far off the field
+    private Pose2d prevPose = new Pose2d();
+
+    private SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];;
+
+    private boolean slipped = false;
+
+    private double previousAngularVelocity = 0;
+
+    private double centerOfMassHeight = 0;
+
+    private Rotation2d rawGyroRotation = new Rotation2d();
+
+
+    /**
+     * Creates a new Swerve Style Drivetrain.
+     */
+    public Drivetrain(Vision vision, GyroIO gyroIO) {
+        this.vision = vision;
+
+        modules = new Module[4];
+        this.gyroIO = gyroIO;
+        ModuleConstants[] constants = Arrays.copyOfRange(ModuleConstants.values(), 0, 4);
+
+        if(RobotBase.isReal()){
+            Arrays.stream(constants).forEach(moduleConstants -> {
+                modules[moduleConstants.ordinal()] = new Module(moduleConstants);
+            });
+        }else{
+            Arrays.stream(constants).forEach(moduleConstants -> {
+                modules[moduleConstants.ordinal()] = new ModuleSim(moduleConstants);
+            });
+        }
+
+        /*
+         * By pausing init for a second before setting module offsets, we avoid a bug
+         * with inverting motors.
+         * See https://github.com/Team364/BaseFalconSwerve/issues/8 for more info.
+         */
+        Timer.delay(1.0);
+        resetModulesToAbsolute();
+        gyroIO.updateInputs(gyroInputs);
+        poseEstimator = new SwerveDrivePoseEstimator(
+                DriveConstants.KINEMATICS,
+                gyroInputs.yawPosition,
+                updateModulePositions(),
+                new Pose2d(),
+                // Defaults, except trust pigeon more
+                VecBuilder.fill(0.1, 0.1, 0),
+                VisionConstants.VISION_STD_DEVS
+        );
+       poseEstimator.setVisionMeasurementStdDevs(VisionConstants.VISION_STD_DEVS);
+        
+        // initialize PID controllers
+        xController = new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D);
+        yController = new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D);
+        rotationController = new PIDController(DriveConstants.HEADING_P, 0, DriveConstants.HEADING_D);
+        rotationController.enableContinuousInput(-Math.PI, Math.PI);
+        rotationController.setTolerance(Units.degreesToRadians(0.25), Units.degreesToRadians(0.25));
+
+        PhoenixOdometryThread.getInstance().start();
+
+        modulePoses = new SwerveModulePose(this, DriveConstants.MODULE_LOCATIONS);
+        
+
+        PathPlannerLogging.setLogActivePathCallback(
+            (activePath) -> {
+            Logger.recordOutput(
+                "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
+            });
+        PathPlannerLogging.setLogTargetPoseCallback(
+            (targetPose) -> {
+            Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
+            });
+
+        //PPLibTelemetry.enableCompetitionMode();
+    }
+
+    public void close() {
+        // close each of the modules
+        for (int i = 0; i < modules.length; i++) {
+            modules[i].close();
+        }
+    }
+
+    @Override
+    public void periodic() {
+        odometryLock.lock(); // Prevents odometry updates while reading data
+        gyroIO.updateInputs(gyroInputs);
+        Logger.processInputs("Drive/Gyro", gyroInputs);
+        for (var module : modules) {
+            module.periodic();
+        }
+        odometryLock.unlock();
+            // Update odometry
+        double[] sampleTimestamps =
+            gyroInputs.odometryYawTimestamps; // 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];
+            for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) {
+                modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i];
+            } 
+            // Use the real gyro angle
+            rawGyroRotation = gyroInputs.odometryYawPositions[i];
+            // Apply update
+            poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions);
+        }
+        Logger.recordOutput("Odometry/module poses", modulePoses.getModulePoses());
+        updateOdometryVision();
+    }
+
+    // DRIVE
+    /**
+     * Method to drive the robot using joystick info.
+     *
+     * @param xSpeed        speed of the robot in the x direction (forward) in m/s
+     * @param ySpeed        speed of the robot in the y direction (sideways) in m/s
+     * @param rot           angular rate of the robot in rad/s
+     * @param fieldRelative whether the provided x and y speeds are relative to the field
+     * @param isOpenLoop    whether to use velocity control for the drive motors
+     */
+    public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean isOpenLoop) {
+        // rot = headingControl(rot, xSpeed, ySpeed);
+        ChassisSpeeds speeds = ChassisSpeeds.discretize(xSpeed, ySpeed, rot, Constants.LOOP_TIME);
+        if(fieldRelative){
+            speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw());
+        }
+        setChassisSpeeds(speeds, isOpenLoop);
+    }
+
+    /**
+     * Drives the robot using the provided x speed, y speed, and positional heading.
+     *
+     * @param xSpeed        speed of the robot in the x direction (forward)
+     * @param ySpeed        speed of the robot in the y direction (sideways)
+     * @param heading       target heading of the robot in radians
+     * @param fieldRelative whether the provided x and y speeds are relative to the field
+     */
+    public void driveHeading(double xSpeed, double ySpeed, double heading, boolean fieldRelative) {
+        double rot = rotationController.calculate(getYaw().getRadians(), heading);
+        ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
+        if(fieldRelative){
+            speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw());
+        }
+        setChassisSpeeds(speeds, false);
+    }
+
+    /**
+     * Runs the PID controllers with the provided x, y, and rot values. Then, calls {@link #drive(double, double, double, boolean, boolean)} using the PID outputs.
+     * This is based on the odometry of the chassis.
+     *
+     * @param x   the position to move to in the x, in meters
+     * @param y   the position to move to in the y, in meters
+     * @param rot the angle to move to, in radians
+     */
+    public void driveWithPID(double x, double y, double rot) {
+        Pose2d pose = getPose();
+        double xSpeed = xController.calculate(pose.getX(), x);
+        double ySpeed = yController.calculate(pose.getY(), y);
+        double rotRadians = rotationController.calculate(pose.getRotation().getRadians(), rot);
+        drive(xSpeed, ySpeed, rotRadians, true, false);
+    }
+
+    /**
+     * Updates odometry using vision
+     */
+    public void updateOdometryVision() {
+        // Start the timer if it hasn't started yet
+        visionEnableTimer.start();
+
+        // Update the swerve module poses
+        modulePoses.update();
+
+        if(modulePoses.slipped()){
+            slipped = true;
+        }
+
+        Pose2d pose2 = getPose();
+
+        // Even if vision is disabled, it should still update inputs
+        // This prevents it from storing a lot of unread results, and it could be useful for replays
+        vision.updateInputs();
+
+        if(VisionConstants.ENABLED){
+            if(vision != null && visionEnabled && visionEnableTimer.hasElapsed(5)){
+                vision.updateOdometry(poseEstimator, time->getPoseAt(time).getRotation().getRadians(), slipped);
+
+                if(vision.canSeeTag()){
+                    slipped = false;
+                    modulePoses.reset();
+                }
+            }
+        }
+
+        Pose2d pose3 = getPose();
+        
+        // Reset the pose to a position on the field if it is too far off the field
+        // This uses nearField() instead of onField() so we don't reset the odometry when the wheels slip near the edge of the field
+        // This is meant for poses that are caused by errors
+        if(!Vision.nearField(prevPose)){
+            // If the pose at the beginning of the method is off the field, reset to a position in the middle of the field
+            // Use the rotation of the pose after updating odometry so the yaw is right
+            prevPose = new Pose2d(FieldConstants.FIELD_LENGTH/2, FieldConstants.FIELD_WIDTH/2, pose2.getRotation());
+            resetOdometry(prevPose);
+        }else if(!Vision.nearField(pose2)){
+            // if the drivetrain pose is off the field, reset our odometry to the pose before(this is the right pose)
+            // Keep the rotation from pose2 so yaw is correct for driver
+            prevPose = new Pose2d(prevPose.getTranslation(), pose2.getRotation());
+            resetOdometry(prevPose);
+        }else if(!Vision.nearField(pose3)){
+            //if our vision+drivetrain odometry isn't near the field, reset our odometry to the pose before(this is the right pose)
+            resetOdometry(pose2);
+            prevPose = pose2;
+        }else{
+            // Set the previous pose to the current pose if we need to return to that
+            prevPose = pose3;
+        }
+
+        // if (Robot.isSimulation()) {
+        //     pigeon.getSimState().addYaw(
+        //             +Units.radiansToDegrees(currentSetpoint.chassisSpeeds().omegaRadiansPerSecond * Constants.LOOP_TIME));
+        // }
+    }
+
+    /**
+     * Stops all swerve modules.
+     */
+    public void stop() {
+        Arrays.stream(modules).forEach(Module::stop);
+    }
+
+
+    // GETTERS AND SETTERS
+
+    /**
+     * Sets the desired states for all swerve modules.
+     *
+     * @param swerveModuleStates an array of module states to set swerve modules to. Order of the array matters here!
+     */
+    public void setModuleStates(SwerveModuleState[] swerveModuleStates, boolean isOpenLoop) {
+        // makes sure speeds of modules don't exceed maximum allowed
+        SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, DriveConstants.MAX_SPEED);
+
+        for (int i = 0; i < 4; i++) {
+            modules[i].setDesiredState(swerveModuleStates[i], isOpenLoop);
+        }
+    }
+
+    /**
+     * Sets the chassis speeds of the robot.
+     *
+     * @param chassisSpeeds the target chassis speeds
+     * @param isOpenLoop    if open loop control should be used for the drive velocity
+     */
+    public void setChassisSpeeds(ChassisSpeeds chassisSpeeds, boolean isOpenLoop) {
+
+        if(DriveConstants.USE_ACTUAL_SPEED){
+            SwerveSetpoint currentState = new SwerveSetpoint(getChassisSpeeds(), getModuleStates());
+            currentSetpoint = setpointGenerator.generateSetpoint(
+                DriveConstants.MODULE_LIMITS,
+                centerOfMassHeight,
+                currentState, chassisSpeeds,
+                Constants.LOOP_TIME);
+        }else{
+            currentSetpoint = setpointGenerator.generateSetpoint(
+                DriveConstants.MODULE_LIMITS,
+                centerOfMassHeight,
+                currentSetpoint, chassisSpeeds,
+                Constants.LOOP_TIME);
+        }
+
+        SwerveModuleState[] swerveModuleStates = currentSetpoint.moduleStates();
+        setModuleStates(swerveModuleStates, isOpenLoop);
+    }
+
+    public void setDriveVoltages(Voltage voltage){
+        for (int i = 0; i<modules.length;i++){
+            modules[i].setDriveVoltage(voltage);
+        }
+    }
+
+    public void setAngleMotors(Rotation2d[] angles){
+        for (int i = 0;i<modules.length;i++){
+            modules[i].setAngle(angles[i]);
+        }
+    }
+
+    /**
+     * Returns the angular rate from the pigeon.
+     *
+     * @param id 0 for x, 1 for y, 2 for z
+     * @return the rate in rads/s from the pigeon
+     */
+    public double getAngularRate(int id) {
+        //double speed = 0;
+        // switch(id){
+        //     case 0:
+        //         speed = gyroInputs..getAngularVelocityXWorld().getValueAsDouble();
+        //         break;
+        //     case 1:
+        //         speed = pigeon.getAngularVelocityYWorld().getValueAsDouble();
+        //         break;
+        //     case 2:
+        //         speed = pigeon.getAngularVelocityZWorld().getValueAsDouble();
+        //         break;
+        // }
+        // outputs in deg/s, so convert to rad/s
+        return gyroInputs.yawVelocityRadPerSec;
+    }
+
+
+    /**
+     * Updates and returns the array of SwerveModulePositions, which store the distance travleled by the drive and the steer angle.
+     *
+     * @return An array of all swerve module positions
+     */
+    private SwerveModulePosition[] updateModulePositions() {
+        return modulePositions = Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new);
+    }
+
+    /**
+     * Gets an array of SwerveModulePositions, which store the distance travleled by the drive and the steer angle.
+     *
+     * @return An array of all swerve module positions
+     */
+    public SwerveModulePosition[] getModulePositions() {
+        return modulePositions;
+    }
+
+    /**
+     * Enables or disables the state deadband for all swerve modules.
+     * The state deadband determines if the robot will stop drive and steer motors when inputted drive velocity is low.
+     * It should be enabled for all regular driving, to prevent releasing the controls from setting the angles.
+     */
+    public void setStateDeadband(boolean stateDeadBand) {
+        Arrays.stream(modules).forEach(module -> module.setStateDeadband(stateDeadBand));
+    }
+    public void setOptimized(boolean optimized) {
+        Arrays.stream(modules).forEach(module -> module.setOptimize(optimized));
+    }
+
+    public void setVisionEnabled(boolean enabled){
+        visionEnabled = enabled;
+    }
+
+    public void setIsAlign(boolean isAlign){
+        this.isAlign = isAlign;
+    }
+    public boolean getIsAlign(){
+        return isAlign;
+    }
+
+    /**
+     * Calculates chassis speed of drivetrain using the current SwerveModuleStates
+     * @return ChassisSpeeds object
+     * This is often used as an input for other methods
+     */
+    public ChassisSpeeds getChassisSpeeds() {
+        return DriveConstants.KINEMATICS.toChassisSpeeds(getModuleStates());
+    }
+
+    /**
+     * Gets the state of each module
+     * @return An array of 4 SwerveModuleStates
+     */
+    public SwerveModuleState[] getModuleStates(){
+        return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new);
+    }
+
+    public SwerveSetpoint getCurrSetpoint(){
+        return currentSetpoint;
+    }
+
+    /**
+     * @return the yaw of the robot, aka heading, the direction it is facing
+     */
+    public Rotation2d getYaw() {
+        return getPose().getRotation();
+    }
+
+    /**
+     * @return an array of modules
+     */
+    public Module[] getModules(){
+        return modules;
+    }
+
+    /**
+     * Resets the yaw of the robot.
+     *
+     * @param rotation the new yaw angle as Rotation2d
+     */
+    public void setYaw(Rotation2d rotation) {
+        resetOdometry(new Pose2d(getPose().getTranslation(), rotation));
+    }
+
+    /**
+     * Resets the odometry to the given pose.
+     *
+     * @param pose the pose to reset to.
+     */
+    public void resetOdometry(Pose2d pose) {
+        // NOTE: must use pigeon yaw for odometer!
+        currentHeading = pose.getRotation().getRadians();
+        poseEstimator.resetPosition(gyroInputs.yawPosition, getModulePositions(), pose);
+        modulePoses.reset();
+    }
+
+    /**
+     * @return the pose of the robot as estimated by the odometry
+     */
+    @AutoLogOutput(key = "Odometry/Robot")
+    public Pose2d getPose() {
+        return poseEstimator.getEstimatedPosition();
+    }
+
+    /**
+     * Sets the angle to align to
+     * @param newAngle The new angle in radians, can be set to null
+     */
+    public void setAlignAngle(Double newAngle){
+        alignAngle = newAngle;
+    }
+
+    /**
+     * Returns whether or not the robot is at the input align angle
+     * @return true if it within tolerance the align angle, false otherwise
+     */
+    public boolean atAlignAngle(){
+        if(alignAngle == null){
+            return false;
+        }
+        double diff = Math.abs(alignAngle - getYaw().getRadians());
+        return diff < DriveConstants.HEADING_TOLERANCE || diff > 2*Math.PI - DriveConstants.HEADING_TOLERANCE;
+    }
+
+    /**
+     * Gets the angle to align to
+     * @return The angle in radians
+     */
+    public double getAlignAngle(){
+        if(alignAngle != null){
+            return alignAngle;
+        }
+        return 0;
+    }
+
+    /**
+     * Sets vision to only use certain April tags
+     * @param ids An array of the tags to only use
+     */
+    public void onlyUseTags(int[] ids){
+        if(vision != null){
+            vision.onlyUse(ids);
+        }
+    }
+    /**
+     * Returns if vision has seen an April tag in the last frame
+     * @return true if vision saw a tag last frame or if vision is disabled
+     */
+    public boolean canSeeTag(){
+        // if no vision system, then return true
+        if (vision == null) return true;
+
+        return vision.canSeeTag() || !visionEnabled || !VisionConstants.ENABLED;
+    }
+
+    /**
+     * Gets the pose at a previous time
+     * @param timestamp The timestamp of the pose to get
+     * @return The pose, null if there are no poses yet, or the current pose if timestamp < 0
+     */
+    public Pose2d getPoseAt(double timestamp){
+        if(timestamp < 0){
+            return getPose();
+        }
+        Optional<Pose2d> pose = poseEstimator.sampleAt(timestamp);
+        if(pose.isPresent()){
+            return pose.get();
+        }else{
+            return null;
+        }
+    }
+
+    /**
+     * Uses pigeon and rotational input to return a rotation that accounts for drift
+     * @return A rotation
+     */
+    public double headingControl(double rot, double xSpeed, double ySpeed){
+        if((!EqualsUtil.epsilonEquals(getAngularRate(0), 0, 0.0004)&&EqualsUtil.epsilonEquals(Math.hypot(xSpeed, ySpeed),0,0.1))||!EqualsUtil.epsilonEquals(rot, 0, 0.0004)){
+             drive_turning = true;
+             currentHeading = getYaw().getRadians();
+        }
+        else{
+            drive_turning = false;
+        }
+        if (!drive_turning){
+            rotationController.setSetpoint(currentHeading);
+            double output = rotationController.calculate(getYaw().getRadians());
+            rot = Math.abs(output) > Math.abs(rot) ? output : rot;
+        }
+        return rot;
+    }
+
+    /**
+     * Resets the swerve modules from the absolute encoders
+     */
+    public void resetModulesToAbsolute() {
+        Arrays.stream(modules).forEach(Module::resetToAbsolute);
+    }
+
+    // getters for the PID Controllers
+    public PIDController getXController() {
+        return xController;
+    }
+    public PIDController getYController() {
+        return yController;
+    }
+    public PIDController getRotationController() {
+        return rotationController;
+    }
+
+    /**
+     * Set the desired pose to drive to
+     * This will enable driver assist to go to the pose
+     * @param supplier The supplier for the desired pose, use ()->null to not use a desired pose
+     */
+    public void setDesiredPose(Supplier<Pose2d> supplier){
+        desiredPoSupplier = supplier;
+    }
+
+    /**
+     * Set the desired pose to drive to
+     * This will enable driver assist to go to the pose
+     * @param pose The Pose2d to drive to
+     */
+    public void setDesiredPose(Pose2d pose){
+        setDesiredPose(()->pose);
+    }
+
+    /**
+     * Gets the current desired pose, or null if there is no desired pose
+     * @return The Pose2d if it exists, null otherwise
+     */
+    public Pose2d getDesiredPose(){
+        return desiredPoSupplier.get();
+    }
+
+    public boolean atSetpoint(){
+        Pose2d pose = getDesiredPose();
+        return pose != null && getPose().getTranslation().getDistance(pose.getTranslation()) < 0.025;
+    }
+
+    public SwerveModulePose getSwerveModulePose(){
+        return modulePoses;
+    }
+
+    public double getAcceleration() {
+        double accelX = gyroInputs.accelerationX;
+        double accelY = gyroInputs.accelerationY;
+
+        double angularVelocity = getAngularRate(3);
+        double angularAccel = (angularVelocity - previousAngularVelocity) / Constants.LOOP_TIME;
+        previousAngularVelocity = angularVelocity;
+        
+        double pigeonOffsetX = 0.082677;
+        double pigeonOffsetY = 0.030603444;
+
+        double totalX = accelX + Math.pow(angularVelocity, 2) * pigeonOffsetX + angularAccel * pigeonOffsetY;
+        double totalY = accelY + Math.pow(angularVelocity, 2) * pigeonOffsetY - angularAccel * pigeonOffsetX;
+
+        return Math.hypot(totalX, totalY);
+    }
+   
+    @AutoLogOutput(key = "Drivetrain/AccelerationFaults")
+    public boolean accelerationOverMax() {
+        return getAcceleration() > DriveConstants.MAX_LINEAR_ACCEL;
+    }
+
+    public void setCenterOfMass(double height){
+        centerOfMassHeight = height;
+    }
+
+    public void alignWheels(){
+        SwerveModuleState state = new SwerveModuleState(0, new Rotation2d(1.465));
+        setModuleStates(new SwerveModuleState[]{
+            state, state, state, state
+        }, false);
+    }
+}

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java b/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java
new file mode 100644 (file)
index 0000000..ca66aa9
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java
@@ -0,0 +1,32 @@
+// Copyright 2021-2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drivetrain;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface GyroIO {
+  @AutoLog
+  public static class GyroIOInputs {
+    public boolean connected = false;
+    public Rotation2d yawPosition = new Rotation2d();
+    public double yawVelocityRadPerSec = 0.0;
+    public double accelerationX = 0.0;
+    public double accelerationY = 0.0;
+    public double[] odometryYawTimestamps = new double[] {};
+    public Rotation2d[] odometryYawPositions = new Rotation2d[] {};
+  }
+
+  public default void updateInputs(GyroIOInputs inputs) {}
+}

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java
new file mode 100644 (file)
index 0000000..1520fdf
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java
@@ -0,0 +1,72 @@
+// Copyright 2021-2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drivetrain;
+
+import java.util.Queue;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.Pigeon2Configuration;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.LinearAcceleration;
+import frc.robot.constants.IdConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.util.PhoenixOdometryThread;
+
+/** IO implementation for Pigeon 2. */
+public class GyroIOPigeon2 implements GyroIO {
+  private final Pigeon2 pigeon =
+      new Pigeon2(
+          IdConstants.PIGEON,
+          DriveConstants.PIGEON_CAN);
+  private final StatusSignal<Angle> yaw = pigeon.getYaw();
+  private final StatusSignal<LinearAcceleration> accelrationx = pigeon.getAccelerationX();
+  private final StatusSignal<LinearAcceleration> accelrationy = pigeon.getAccelerationY();
+  private final Queue<Double> yawPositionQueue;
+  private final Queue<Double> yawTimestampQueue;
+  private final StatusSignal<AngularVelocity> yawVelocity = pigeon.getAngularVelocityZWorld();
+
+  public GyroIOPigeon2() {
+    pigeon.getConfigurator().apply(new Pigeon2Configuration());
+    pigeon.getConfigurator().setYaw(0.0);
+    yaw.setUpdateFrequency(250);
+    yawVelocity.setUpdateFrequency(50.0);
+    pigeon.optimizeBusUtilization();
+    yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
+    yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(pigeon.getYaw());
+  }
+
+  @Override
+  public void updateInputs(GyroIOInputs inputs) {
+    inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity, accelrationx, accelrationy).equals(StatusCode.OK);
+    inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
+    inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());
+    inputs.accelerationX = accelrationx.getValueAsDouble();
+    inputs.accelerationY = accelrationy.getValueAsDouble();
+
+    inputs.odometryYawTimestamps =
+        yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray();
+    inputs.odometryYawPositions =
+        yawPositionQueue.stream()
+            .map((Double value) -> Rotation2d.fromDegrees(value))
+            .toArray(Rotation2d[]::new);
+    yawTimestampQueue.clear();
+    yawPositionQueue.clear();
+  }
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Module.java b/src/main/java/frc/robot/subsystems/drivetrain/Module.java
new file mode 100644 (file)
index 0000000..6fa9d46
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Module.java
@@ -0,0 +1,422 @@
+package frc.robot.subsystems.drivetrain;
+
+import java.util.Queue;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MagnetSensorConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
+import com.ctre.phoenix6.controls.PositionDutyCycle;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.ParentDevice;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.constants.swerve.ModuleConstants;
+import frc.robot.constants.swerve.ModuleType;
+import frc.robot.util.PhoenixOdometryThread;
+import lib.CTREModuleState;
+
+
+public class Module implements ModuleIO{
+    private final ModuleType type;
+    
+    // Degrees
+    private final double angleOffset;
+
+    private final TalonFX angleMotor;
+    private final TalonFX driveMotor;
+    private final CANcoder CANcoder;
+    private SwerveModuleState desiredState;
+
+    protected boolean stateDeadband = true;
+    
+    private boolean optimizeStates = true;
+
+    // Inputs from drive motor
+    private final StatusSignal<Angle> drivePosition;
+    private final StatusSignal<AngularVelocity> driveVelocity;
+    private final StatusSignal<Voltage> driveAppliedVolts;
+    private final StatusSignal<Current> driveCurrent;
+
+    // Inputs from turn motor
+    private final StatusSignal<Angle> turnAbsolutePosition;
+    private final StatusSignal<Angle> turnPosition;
+    private final StatusSignal<AngularVelocity> turnVelocity;
+    private final StatusSignal<Voltage> turnAppliedVolts;
+    private final StatusSignal<Current> turnCurrent;
+
+    // Timestamp inputs from Phoenix thread
+    protected final Queue<Double> timestampQueue;
+    protected final Queue<Double> drivePositionQueue;
+    protected final Queue<Double> turnPositionQueue;
+
+    private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
+
+    // Connection debouncers
+    private final Debouncer driveConnectedDebounce = new Debouncer(0.5);
+    private final Debouncer turnConnectedDebounce = new Debouncer(0.5);
+    private final Debouncer turnEncoderConnectedDebounce = new Debouncer(0.5);
+
+    private final Alert driveDisconnectedAlert;
+    private final Alert turnDisconnectedAlert;
+    private final Alert turnEncoderDisconnectedAlert;
+
+    protected final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
+
+    private ModuleConstants moduleConstants;
+      private final MotionMagicVelocityVoltage velocityRequest =
+      new MotionMagicVelocityVoltage(0.0).withUpdateFreqHz(0);
+
+
+    public Module(ModuleConstants moduleConstants) {
+        this.moduleConstants = moduleConstants;
+        
+        type = moduleConstants.getType();
+        angleOffset = moduleConstants.getSteerOffset();
+
+        /* Angle Encoder Config */
+        CANcoder = new CANcoder(moduleConstants.getEncoderPort(), DriveConstants.STEER_ENCODER_CAN);
+        /* Angle Motor Config */
+        angleMotor = new TalonFX(moduleConstants.getSteerPort(), DriveConstants.STEER_ENCODER_CAN);
+        driveMotor = new TalonFX(moduleConstants.getDrivePort(), DriveConstants.DRIVE_MOTOR_CAN);
+        // Create drive status signals
+        drivePosition = driveMotor.getPosition();
+        driveVelocity = driveMotor.getVelocity();
+        driveAppliedVolts = driveMotor.getMotorVoltage();
+        driveCurrent = driveMotor.getStatorCurrent();
+      
+        // Create turn status signals
+        turnAbsolutePosition = CANcoder.getAbsolutePosition();
+        turnPosition = angleMotor.getPosition();
+        turnVelocity = angleMotor.getVelocity();
+        turnAppliedVolts = angleMotor.getMotorVoltage();
+        turnCurrent = angleMotor.getStatorCurrent();
+      
+        // Create timestamp queue
+        timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
+        drivePositionQueue =
+        PhoenixOdometryThread.getInstance().registerSignal(driveMotor.getPosition());
+        turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(angleMotor.getPosition());
+        updateInputs();
+        
+        configCANcoder();
+        configAngleMotor();
+        configDriveMotor();
+
+        driveDisconnectedAlert =
+        new Alert(
+            "Disconnected drive motor on module " + Integer.toString(moduleConstants.ordinal()) + ".",
+            AlertType.kError);
+    turnDisconnectedAlert =
+        new Alert(
+            "Disconnected turn motor on module " + Integer.toString(moduleConstants.ordinal()) + ".", AlertType.kError);
+    turnEncoderDisconnectedAlert =
+        new Alert(
+            "Disconnected turn encoder on module " + Integer.toString(moduleConstants.ordinal()) + ".",
+            AlertType.kError);        
+   
+
+        // Configure periodic frames
+        BaseStatusSignal.setUpdateFrequencyForAll(
+            250, drivePosition, turnPosition);
+        BaseStatusSignal.setUpdateFrequencyForAll(
+            50.0,
+            driveVelocity,
+            driveAppliedVolts,
+            driveCurrent,
+            turnAbsolutePosition,
+            turnVelocity,
+            turnAppliedVolts,
+            turnCurrent);
+        ParentDevice.optimizeBusUtilizationForAll(driveMotor, angleMotor);
+        
+        setDesiredState(new SwerveModuleState(0, getAngle()), false);
+    }
+
+    public void close() {
+        angleMotor.close();
+        driveMotor.close();
+        CANcoder.close();
+    }
+
+    @Override
+    public void updateInputs() {
+      // Refresh all signals
+      var driveStatus =
+          BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);
+      var turnStatus =
+          BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent);
+      var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition);
+  
+      // Update drive inputs
+      inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK());
+      inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()/DriveConstants.DRIVE_GEAR_RATIO);
+      inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()/DriveConstants.DRIVE_GEAR_RATIO);
+      inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
+      inputs.driveCurrentAmps = driveCurrent.getValueAsDouble();
+  
+      // Update turn inputs
+      inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK());
+      inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK());
+      inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble());
+      inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()/DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+      inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()/DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+      inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble();
+      inputs.turnCurrentAmps = turnCurrent.getValueAsDouble();
+
+    // Update odometry inputs
+    inputs.odometryTimestamps =
+        timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
+    inputs.odometryDrivePositionsRad =
+        drivePositionQueue.stream()
+            .mapToDouble((Double value) -> Units.rotationsToRadians(value))
+            .toArray();
+    inputs.odometryTurnPositions =
+        turnPositionQueue.stream()
+            .map((Double value) -> Rotation2d.fromRotations(value))
+            .toArray(Rotation2d[]::new);
+    timestampQueue.clear();
+    drivePositionQueue.clear();
+    turnPositionQueue.clear();
+
+    }
+    
+    public void periodic() {
+        updateInputs();
+        Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs);
+
+         // 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.odometryDrivePositionsRad[i]/DriveConstants.DRIVE_GEAR_RATIO * DriveConstants.WHEEL_RADIUS;
+        Rotation2d angle = inputs.odometryTurnPositions[i].div(DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+        odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
+        }
+        // Update alerts
+        driveDisconnectedAlert.set(!inputs.driveConnected);
+        turnDisconnectedAlert.set(!inputs.turnConnected);
+        turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected);
+        Logger.recordOutput("Angle "+ moduleConstants.ordinal(), MathUtil.inputModulus(getAngle().getDegrees(), 0, 360));
+    }
+
+    public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) {
+        // Separate if here and in setAngle() to avoid warning
+        if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){
+            /*
+            * This is a custom optimize function, since default WPILib optimize assumes
+            * continuous controller which CTRE and Rev onboard is not
+            */
+            desiredState = optimizeStates ? CTREModuleState.optimize(wantedState, getState().angle) : wantedState;
+        }else{
+            desiredState = wantedState;
+        }
+        setAngle();
+        setSpeed(isOpenLoop);
+    }
+
+    public void setSpeed(boolean isOpenLoop) {
+        if(desiredState == null){
+            return;
+        }
+        if (isOpenLoop) {
+            double percentOutput = desiredState.speedMetersPerSecond / DriveConstants.MAX_SPEED;
+            driveMotor.set(percentOutput);
+        } else {
+            double velocity = desiredState.speedMetersPerSecond/DriveConstants.WHEEL_RADIUS/2/Math.PI*DriveConstants.DRIVE_GEAR_RATIO;
+            Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity);
+            
+            driveMotor.setControl(
+                velocityRequest
+                    .withVelocity(velocity));
+                    ///.withFeedForward(feedforward));
+        }     
+    }
+
+    private void setAngle() {
+        if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){
+            // Prevent rotating module if desired speed < 1%. Prevents jittering and unnecessary movement.
+            if (stateDeadband && (Math.abs(desiredState.speedMetersPerSecond) <= (DriveConstants.MAX_SPEED * 0.01))) {
+                stop();
+                return;
+            }
+        }
+        if(desiredState == null){
+            return;
+        }
+        angleMotor.setControl(new PositionDutyCycle(desiredState.angle.getRotations()*DriveConstants.MODULE_CONSTANTS.angleGearRatio));
+    }
+
+    public void setDriveVoltage(Voltage voltage){
+        driveMotor.setVoltage(voltage.baseUnitMagnitude());
+    }
+    public void setAngle(Rotation2d angle){
+        angleMotor.setControl(new PositionDutyCycle(angle.getRotations()*DriveConstants.MODULE_CONSTANTS.angleGearRatio));
+    }
+
+    public void setOptimize(boolean enable) {
+        optimizeStates = enable;
+    }
+
+    public byte getModuleIndex() {
+        return type.id;
+    }
+
+    public Rotation2d getAngle() {
+        return inputs.turnPosition;
+    }
+
+    public Rotation2d getCANcoder() {
+        return inputs.turnAbsolutePosition;
+    }
+
+    public void resetToAbsolute() {
+        // Sensor ticks
+        double absolutePosition = getCANcoder().getRotations() - Units.degreesToRotations(angleOffset);
+        angleMotor.setPosition(absolutePosition*DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+    }
+
+    private void configCANcoder() {
+        CANcoder.getConfigurator().apply(new CANcoderConfiguration());
+        CANcoder.getConfigurator().apply(new MagnetSensorConfigs().withAbsoluteSensorDiscontinuityPoint(1).
+        withSensorDirection(DriveConstants.MODULE_CONSTANTS.canCoderInvert?SensorDirectionValue.Clockwise_Positive:SensorDirectionValue.CounterClockwise_Positive));
+    }
+
+    private void configAngleMotor() {
+        angleMotor.getConfigurator().apply(new TalonFXConfiguration());
+        CurrentLimitsConfigs config = new CurrentLimitsConfigs();
+        config.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT;
+        config.SupplyCurrentLimit = DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT;
+        config.SupplyCurrentLowerLimit = DriveConstants.STEER_PEAK_CURRENT_LIMIT;
+        config.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
+        angleMotor.getConfigurator().apply(config);
+        angleMotor.getConfigurator().apply(new Slot0Configs()
+            .withKP(DriveConstants.MODULE_CONSTANTS.angleKP)
+            .withKI(DriveConstants.MODULE_CONSTANTS.angleKI)
+            .withKD(DriveConstants.MODULE_CONSTANTS.angleKD));
+        angleMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_STEER_MOTOR));
+        angleMotor.setNeutralMode(DriveConstants.STEER_NEUTRAL_MODE);
+        angleMotor.setPosition(0);
+        
+        resetToAbsolute();
+    }
+
+    /**
+     * @return Speed in RPM
+     */
+    public double getDriveVelocity() {
+        return inputs.driveVelocityRadPerSec*60/DriveConstants.MODULE_CONSTANTS.driveGearRatio/2/Math.PI;
+    }
+
+    public double getDriveVoltage(){
+        return inputs.driveAppliedVolts;
+    }
+
+    public double getDriveStatorCurrent(){
+        return inputs.driveCurrentAmps;
+    }
+    private void configDriveMotor() {
+        var talonFXConfigs = new TalonFXConfiguration();
+        // set Motion Magic settings
+        var motionMagicConfigs = talonFXConfigs.MotionMagic;
+        motionMagicConfigs.MotionMagicCruiseVelocity = DriveConstants.MAX_SPEED/DriveConstants.WHEEL_CIRCUMFERENCE * DriveConstants.DRIVE_GEAR_RATIO;
+        motionMagicConfigs.MotionMagicAcceleration = DriveConstants.MAX_DRIVE_ACCEL/DriveConstants.WHEEL_CIRCUMFERENCE * DriveConstants.DRIVE_GEAR_RATIO;
+        var slot0Configs = talonFXConfigs.Slot0;
+        slot0Configs.kS = 0; // Add 0.25 V output to overcome static friction
+        slot0Configs.kV = 0.11; // A velocity target of 1 rps results in 0.12 V output
+        slot0Configs.kA = 0.006; // An acceleration of 1 rps/s requires 0.01 V output
+        slot0Configs.kP = moduleConstants.getDriveP(); // A position error of 2.5 rotations results in 12 V output
+        slot0Configs.kI = moduleConstants.getDriveI(); // no output for integrated error
+        slot0Configs.kD = moduleConstants.getDriveD(); // A velocity error of 1 rps results in 0.1 V output
+        driveMotor.getConfigurator().apply(talonFXConfigs);
+        CurrentLimitsConfigs config = new CurrentLimitsConfigs();
+        config.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+        config.SupplyCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
+        config.SupplyCurrentLowerLimit = DriveConstants.DRIVE_PEAK_CURRENT_LIMIT;
+        config.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
+        config.StatorCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
+        config.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+        driveMotor.getConfigurator().apply(config);
+        driveMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_DRIVE_MOTOR));
+        driveMotor.getConfigurator().apply(new OpenLoopRampsConfigs().withDutyCycleOpenLoopRampPeriod(DriveConstants.OPEN_LOOP_RAMP));
+        driveMotor.getConfigurator().apply(new ClosedLoopRampsConfigs().withDutyCycleClosedLoopRampPeriod(DriveConstants.OPEN_LOOP_RAMP));
+        driveMotor.setNeutralMode(DriveConstants.DRIVE_NEUTRAL_MODE);
+        
+    }
+
+    public SwerveModuleState getState() {
+        return new SwerveModuleState(
+                inputs.driveVelocityRadPerSec*DriveConstants.WHEEL_RADIUS,
+                getAngle());
+    }
+
+    public SwerveModulePosition getPosition() {
+        return new SwerveModulePosition(
+                inputs.drivePositionRad*DriveConstants.WHEEL_RADIUS,
+                getAngle());
+    }
+
+    public SwerveModuleState getDesiredState() {
+        return desiredState;
+    }
+
+
+    public double getDriveVelocityError() {
+        return getDesiredState().speedMetersPerSecond - getState().speedMetersPerSecond;
+    }
+
+    public void stop() {
+        driveMotor.set(0);
+        angleMotor.set(0);
+    }
+
+    public ModuleType getModuleType(){
+        return type;
+    }
+
+    public void setStateDeadband(boolean enabled) {
+        stateDeadband = enabled;
+    }
+
+    public double getDesiredVelocity() {
+        return getDesiredState().speedMetersPerSecond;
+      }
+    
+    public Rotation2d getDesiredAngle() {
+        return getDesiredState().angle;
+    }
+
+        /** Returns the module positions received this cycle. */
+    public SwerveModulePosition[] getOdometryPositions() {
+        return odometryPositions;
+    }
+
+    /** Returns the timestamps of the samples received this cycle. */
+    public double[] getOdometryTimestamps() {
+        return inputs.odometryTimestamps;
+    }
+
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/ModuleIO.java b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIO.java
new file mode 100644 (file)
index 0000000..fece5cf
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIO.java
@@ -0,0 +1,43 @@
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drivetrain;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ModuleIO {
+  @AutoLog
+  public static class ModuleIOInputs {
+    public boolean driveConnected = false;
+    public double drivePositionRad = 0.0;
+    public double driveVelocityRadPerSec = 0.0;
+    public double driveAppliedVolts = 0.0;
+    public double driveCurrentAmps = 0.0;
+
+    public boolean turnConnected = false;
+    public boolean turnEncoderConnected = false;
+    public Rotation2d turnAbsolutePosition = new Rotation2d();
+    public Rotation2d turnPosition = new Rotation2d();
+    public double turnVelocityRadPerSec = 0.0;
+    public double turnAppliedVolts = 0.0;
+    public double turnCurrentAmps = 0.0;
+
+    public double[] odometryTimestamps = new double[] {};
+    public double[] odometryDrivePositionsRad = new double[] {};
+    public Rotation2d[] odometryTurnPositions = new Rotation2d[] {};
+  }
+
+  /** Updates the set of loggable inputs. */
+  public default void updateInputs() {}
+
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/ModuleSim.java b/src/main/java/frc/robot/subsystems/drivetrain/ModuleSim.java
new file mode 100644 (file)
index 0000000..24a2b77
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drivetrain/ModuleSim.java
@@ -0,0 +1,165 @@
+package frc.robot.subsystems.drivetrain;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.constants.Constants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.constants.swerve.ModuleConstants;
+import lib.CTREModuleState;
+
+
+/**
+ * Swerve module for drivetrain to be used inside of simulation.
+ * TODO: improve this simulation to be more realistic
+ */
+public class ModuleSim extends Module {
+
+    private double currentSteerPositionRad = 0;
+    private double currentDrivePositionMeters = 0;
+    private double currentSpeed = 0;
+
+    private SwerveModuleState desiredState;
+
+
+    protected boolean stateDeadband = true;
+
+    public ModuleSim(ModuleConstants ignored) {
+        super(ignored);
+    }
+
+    /**
+     * Updates the simulation
+     */
+    @Override
+    public void periodic() {
+        currentDrivePositionMeters += currentSpeed * Constants.LOOP_TIME;
+        super.periodic();
+    }
+
+    @Override
+    public void updateInputs(){
+        // Update drive inputs
+        inputs.driveConnected = true;
+        inputs.drivePositionRad = currentDrivePositionMeters / DriveConstants.WHEEL_RADIUS;
+        inputs.driveVelocityRadPerSec = currentSpeed / DriveConstants.WHEEL_RADIUS;
+        inputs.driveAppliedVolts = currentSpeed / DriveConstants.MAX_SPEED * Constants.ROBOT_VOLTAGE;
+        inputs.driveCurrentAmps = 0; // This simulation currently isn't good enough to calculate this
+    
+        // Update turn inputs
+        inputs.turnConnected = true;
+        inputs.turnEncoderConnected = true;
+        inputs.turnAbsolutePosition = new Rotation2d(currentSteerPositionRad);
+        inputs.turnPosition = new Rotation2d(currentSteerPositionRad);
+        inputs.turnVelocityRadPerSec = 0; // Simulated modules currently teleport
+        inputs.turnAppliedVolts = 0;
+        inputs.turnCurrentAmps = 0;
+
+        // Update odometry inputs
+        // Simulate as only getting one value per frame
+        inputs.odometryTimestamps =
+            new double[]{Timer.getFPGATimestamp()};
+        inputs.odometryDrivePositionsRad =
+            new double[]{inputs.drivePositionRad*DriveConstants.DRIVE_GEAR_RATIO};
+        inputs.odometryTurnPositions =
+            new Rotation2d[]{inputs.turnPosition};
+        timestampQueue.clear();
+        drivePositionQueue.clear();
+        turnPositionQueue.clear();
+    }
+
+    /**
+     * Sets the desired state for the module.
+     *
+     * @param desiredState Desired state with speed and angle.
+     * @param isOpenLoop   whether to use closed/open loop control for drive velocity
+     */
+    public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) {
+        if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){
+            // If the module isn't moving, don't rotate it
+            if (Math.abs(desiredState.speedMetersPerSecond) < 0.001) {
+                currentSpeed = 0;
+                return;
+            }
+            // Optimize the reference state to avoid spinning further than 90 degrees
+            desiredState = CTREModuleState.optimize(desiredState, new Rotation2d(currentSteerPositionRad));
+        }
+
+        currentSpeed = desiredState.speedMetersPerSecond;
+        currentSteerPositionRad = desiredState.angle.getRadians();
+    }
+
+    public void resetToAbsolute() {
+        // does nothing when robot does not have a swerve drivetrain
+    }
+
+    public SwerveModuleState getDesiredState() {
+        return desiredState;
+      }
+
+    public double getDesiredVelocity() {
+        return getDesiredState().speedMetersPerSecond;
+      }
+    
+      public Rotation2d getDesiredAngle() {
+        return getDesiredState().angle;
+      }
+
+    /**
+     * Sets current speed to zero
+     */
+    public void stop() {
+        currentSpeed = 0;
+    }
+
+    public SwerveModuleState getState() {
+        return new SwerveModuleState(
+                currentSpeed,
+                getAngle()
+        );
+    }
+
+    public SwerveModulePosition getPosition() {
+        return new SwerveModulePosition(
+                currentDrivePositionMeters,
+                new Rotation2d(currentSteerPositionRad)
+        );
+    }
+
+    /**
+     * Gets the simulated angle of the module.
+     */
+    public Rotation2d getAngle() {
+        return new Rotation2d(currentSteerPositionRad);
+    }
+
+     /**
+     * Sets state deadband
+     */
+    public void setStateDeadband(boolean enabled) {
+        stateDeadband = enabled;
+    }
+
+    public TalonFX getDriveMotor(){
+        return null;
+    }
+
+    public double getDriveVoltage(){
+        return 0;
+    }
+
+    public double getDriveStatorCurrent(){
+        return 0;
+    }
+    
+    public double getSteerVelocity() {
+        return 0;
+    }
+    public double getDriveVelocity() {
+        return 0;
+    }
+
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java
new file mode 100644 (file)
index 0000000..9fc85ae
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java
@@ -0,0 +1,183 @@
+// 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 frc.robot.subsystems.elevator;
+
+import java.util.function.BooleanSupplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.Constants;
+import frc.robot.constants.ElevatorConstants;
+import frc.robot.constants.IdConstants;
+import frc.robot.util.AngledElevatorSim;
+import frc.robot.util.PhoenixUtil;
+
+public class Elevator extends SubsystemBase {
+  private TalonFX rightMotor = new TalonFX(IdConstants.ELEVATOR_RIGHT_MOTOR, Constants.CANIVORE_CAN);
+
+  private double setpoint = ElevatorConstants.INTAKE_SETPOINT;
+  
+  private MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0);
+
+  private double maxVelocity = 3.6; // m/s 3.68
+  private double maxAcceleration = 14; // m/s 8
+        
+  // Sim variables
+  private AngledElevatorSim sim;
+  private Mechanism2d mechanism;
+  private MechanismLigament2d ligament;
+  private double voltage;
+
+  private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged();
+
+  private BooleanSupplier armStowed;
+
+  public Elevator() {
+    // This increases both the time and memory efficiency of the code when running
+    // on a real robot; do not remove this if statement
+    if (RobotBase.isSimulation()) {
+      sim = new AngledElevatorSim(ElevatorConstants.MOTOR, ElevatorConstants.GEARING, ElevatorConstants.CARRIAGE_MASS,
+        ElevatorConstants.DRUM_RADIUS, ElevatorConstants.MIN_HEIGHT, ElevatorConstants.MAX_HEIGHT, true,
+        ElevatorConstants.START_HEIGHT, ElevatorConstants.ANGLE, ElevatorConstants.SPRING_FORCE);
+      double width = ElevatorConstants.MAX_HEIGHT * Math.sin(ElevatorConstants.ANGLE);
+      double height = ElevatorConstants.MAX_HEIGHT * Math.cos(ElevatorConstants.ANGLE);
+      double size = Math.max(width, height);
+      mechanism = new Mechanism2d(size, size);
+      ligament = mechanism.getRoot("base", size / 2 - width / 2, size / 2 - height / 2).append(new MechanismLigament2d(
+        "elevator", ElevatorConstants.START_HEIGHT, 90 - Units.radiansToDegrees(Math.abs(ElevatorConstants.ANGLE))));
+      SmartDashboard.putData("elevator", mechanism);
+    }
+    Timer.delay(1.0);
+
+    //m_lastProfiledReference = new ExponentialProfile.State(getPosition(),0);
+    resetEncoder(ElevatorConstants.START_HEIGHT);  
+
+    var talonFXConfigs = new TalonFXConfiguration();
+
+    // set slot 0 gains
+    var slot0Configs = talonFXConfigs.Slot0;
+    slot0Configs.kS = 0.15; // Add 0.25 V output to overcome static friction
+    slot0Configs.kV = 0.12; // A velocity target of 1 rps results in 0.12 V output
+    slot0Configs.kA = 0; // An acceleration of 1 rps/s requires 0.01 V output
+    slot0Configs.kP = 0.75; // A position error of 2.5 rotations results in 12 V output
+    slot0Configs.kI = 0; // no output for integrated error
+    slot0Configs.kD = 0; // A velocity error of 1 rps results in 0.1 V output
+
+    // set Motion Magic settings
+    var motionMagicConfigs = talonFXConfigs.MotionMagic;
+    motionMagicConfigs.MotionMagicCruiseVelocity = ElevatorConstants.GEARING * maxVelocity/ElevatorConstants.DRUM_RADIUS/Math.PI/2; // Target cruise velocity 
+    motionMagicConfigs.MotionMagicAcceleration = ElevatorConstants.GEARING * maxAcceleration/ElevatorConstants.DRUM_RADIUS/Math.PI/2; // Target acceleration 
+    rightMotor.getConfigurator().apply(talonFXConfigs);
+    rightMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive));
+    updateInputs();
+    PhoenixUtil.tryUntilOk(100, ()-> rightMotor.setNeutralMode(NeutralModeValue.Brake));
+  }
+
+  public void setArmStowed(BooleanSupplier armStowed){
+    this.armStowed = armStowed;
+  }
+
+  @Override
+  public void periodic() {
+    double setpoint2 = setpoint;
+    if(setpoint2 < ElevatorConstants.SAFE_SETPOINT && (armStowed == null || !armStowed.getAsBoolean())){
+      setpoint2 = ElevatorConstants.SAFE_SETPOINT;
+    }
+    double setpointRotations = ElevatorConstants.GEARING * setpoint2 / ElevatorConstants.DRUM_RADIUS/Math.PI/2;
+    rightMotor.setControl(voltageRequest.withPosition(setpointRotations).withFeedForward(0.4));
+    updateInputs();
+    Logger.processInputs("Elevator", inputs);
+    Logger.recordOutput("Elevator/Setpoint", getSetpoint());
+    Logger.recordOutput("Elevator/AtSetpoint", atSetpoint());
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    sim.setInputVoltage(0);
+    sim.update(Constants.LOOP_TIME);
+    ligament.setLength(sim.getPositionMeters());
+    rightMotor.getSimState().setRawRotorPosition(
+        sim.getPositionMeters() / (2 * Math.PI * ElevatorConstants.DRUM_RADIUS) * ElevatorConstants.GEARING);
+  }
+
+  public void resetEncoder(double height) {
+    // Without the if statement, this causes loop overruns in simulation, and this
+    // code does nothing anyway on sim (it sets the position to itself)
+    if (RobotBase.isReal()) {
+      rightMotor.setPosition(height / (2 * Math.PI * ElevatorConstants.DRUM_RADIUS) * ElevatorConstants.GEARING);
+    }
+  }
+
+  public void updateInputs(){
+    inputs.measuredPosition = rightMotor.getPosition().getValueAsDouble() / ElevatorConstants.GEARING
+    * (2 * Math.PI * ElevatorConstants.DRUM_RADIUS);
+    inputs.velocity = rightMotor.getVelocity().getValueAsDouble()/ ElevatorConstants.GEARING
+    * (2 * Math.PI * ElevatorConstants.DRUM_RADIUS);
+    inputs.currentAmps = rightMotor.getStatorCurrent().getValueAsDouble();
+  }
+  
+  /**
+   * Get the position of the elevator in  meters. 
+  */
+  public double getPosition() {
+    return inputs.measuredPosition;
+  }
+  
+  /**
+   * Get the velocity of the elevator in m/s. 
+  */
+  public double getVelocity(){
+    return inputs.velocity;
+  }
+
+  public double getVoltage(){
+    return voltage;
+  }
+
+  /**
+   * Method to set the setpoint of the elevator. Clamped between min and max height.
+   * @param setpoint The setpoint in meters.
+  */
+  public void setSetpoint(double setpoint) {
+    this.setpoint = MathUtil.clamp(setpoint, ElevatorConstants.MIN_HEIGHT, ElevatorConstants.MAX_HEIGHT);
+  }
+
+  /**
+   * Get the velocity of the elevator in meters. 
+  */
+  public double getSetpoint() {
+    return setpoint;
+  }
+
+  public Mechanism2d getMechanism2d() {
+    return mechanism;
+  }
+
+  public boolean atSetpoint(){
+    return Math.abs(getPosition() - setpoint) < (0.025+0.0125);
+  }
+
+  /**
+   * Get the COM at the current elevater height. 
+  */
+  public double getCenterOfMassHeight(){
+    return (getPosition()-ElevatorConstants.MIN_HEIGHT)/(ElevatorConstants.MAX_HEIGHT-ElevatorConstants.MIN_HEIGHT)*(ElevatorConstants.CENTER_OF_MASS_HEIGHT_EXTENDED-ElevatorConstants.CENTER_OF_MASS_HEIGHT_STOWED)+ElevatorConstants.CENTER_OF_MASS_HEIGHT_STOWED;
+  }
+}

diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java
new file mode 100644 (file)
index 0000000..c0f96a9
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java
@@ -0,0 +1,12 @@
+package frc.robot.subsystems.elevator;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ElevatorIO {
+    @AutoLog
+    public static class ElevatorIOInputs {
+        public double measuredPosition = 0.0;
+        public double velocity = 0.0;
+        public double currentAmps = 0.0;
+    }
+}

diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java
new file mode 100644 (file)
index 0000000..4a6eaf8
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java
@@ -0,0 +1,142 @@
+package frc.robot.subsystems.indexer;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import au.grapplerobotics.ConfigurationFailedException;
+import au.grapplerobotics.LaserCan;
+import au.grapplerobotics.interfaces.LaserCanInterface;
+import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode;
+import au.grapplerobotics.interfaces.LaserCanInterface.RegionOfInterest;
+import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget;
+import au.grapplerobotics.simulation.MockLaserCan;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Robot;
+import frc.robot.constants.Constants;
+import frc.robot.constants.IdConstants;
+import frc.robot.constants.IndexerConstants;
+
+public class Indexer extends SubsystemBase {
+       private TalonFX motor;
+       private MockLaserCan simSensor;
+       private LaserCanInterface sensor;
+
+       private FlywheelSim flywheelSim;
+
+       // where the coral is for simulation
+       // in meters
+       private double simCoralPos;
+
+       private final IndexerIOInputsAutoLogged inputs = new IndexerIOInputsAutoLogged();
+
+       public Indexer() {
+               motor = new TalonFX(IdConstants.INDEXER_MOTOR);
+               if (Robot.isSimulation()) {
+                       flywheelSim = new FlywheelSim(LinearSystemId.createFlywheelSystem(DCMotor.getNEO(1),
+                                       IndexerConstants.MOMENT_OF_INERTIA, IndexerConstants.GEAR_RATIO), DCMotor.getNEO(1));
+
+                       // have both interfaces availible
+                       simSensor = new MockLaserCan();
+                       sensor = simSensor;
+               } else {
+                       sensor = new LaserCan(IdConstants.INDEXER_SENSOR);
+            try {
+                sensor.setRangingMode(RangingMode.SHORT);
+                sensor.setTimingBudget(TimingBudget.TIMING_BUDGET_20MS);
+                sensor.setRegionOfInterest(new RegionOfInterest(-4, -4, 8, 8));
+            } catch (ConfigurationFailedException e) {
+                DriverStation.reportError("Indexer LaserCan configuration error", true);
+            }
+               }
+               simCoralPos = IndexerConstants.START_SIM_POS_AT; // initialize it anyway, it's easier
+       }
+
+       /** Runs the indexer. */
+       public void run() {
+               motor.set(IndexerConstants.SPEED);
+               simCoralPos = IndexerConstants.START_SIM_POS_AT;
+       }
+
+       public void slow(){
+               motor.set(0.6);
+       }
+
+       /** Reverses the indexer. */
+       public void reverse() {
+               motor.set(-IndexerConstants.SPEED);
+               simCoralPos = IndexerConstants.END_SIM_SENSOR_POS_AT;
+       }
+
+       /** Stops the indexer */
+       public void stop() {
+               motor.stopMotor();
+       }
+
+       /**
+        * @return the motor velocity in rotations per minute
+        */
+       public double getMotor() {
+               return inputs.velocity;
+       }
+
+       /**
+        * Gets the LaserCAN's distance reading.
+        * If the distance is null, return 314,159
+        * 
+        * @return the distance, in millimeters
+        */
+       public int getSensorValue() {
+               return inputs.sensorDistance;
+       }
+
+       /**
+       * Checks whether a coral is in the indexer.
+       * True means nothing is there, false means something is there
+       *
+       * @return the sensor's state
+       */
+       @AutoLogOutput(key = "Intake/isIndexerClear")
+       public boolean isIndexerClear() {
+               return true;
+       }
+
+       @Override
+       public void periodic() {
+               //SmartDashboard.putBoolean("Indexer has coral ", isIndexerClear());
+
+               if (Robot.isReal()) {
+                       inputs.velocity =  motor.getVelocity().getValueAsDouble() / IndexerConstants.GEAR_RATIO;
+               } else {
+                       inputs.velocity = flywheelSim.getAngularVelocityRPM();
+               }
+               var measurement = sensor.getMeasurement();
+               inputs.sensorDistance = (measurement == null || measurement.status > 0) ? 314159 : measurement.distance_mm;
+               Logger.processInputs("Indexer", inputs);
+               Logger.recordOutput("Indexer/indexer coral",isIndexerClear());
+       }
+
+       @Override
+       public void simulationPeriodic() {
+               flywheelSim.setInput(motor.get() * Constants.ROBOT_VOLTAGE);
+               flywheelSim.update(Constants.LOOP_TIME);
+
+               // pretend we have a fake coral
+               simCoralPos += flywheelSim.getAngularVelocityRPM() / 60. * Constants.LOOP_TIME
+                               * IndexerConstants.WHEEL_CIRCUMFERENCE;
+
+               // toggle the sensor (values are backwards because that's how the sensor works)
+               simSensor.setMeasurementPartialSim(0, // 0 == valid measurement
+                               (simCoralPos < IndexerConstants.START_SIM_SENSOR_POS_AT
+                                               || simCoralPos > IndexerConstants.END_SIM_SENSOR_POS_AT)
+                                                               ? IndexerConstants.MEASUREMENT_THRESHOLD * 2
+                                                               : 0,
+                               1000 // IDK what this is exactly, but 1000 seems good
+               );
+       }
+}

diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java
new file mode 100644 (file)
index 0000000..3678c55
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java
@@ -0,0 +1,11 @@
+package frc.robot.subsystems.indexer;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IndexerIO {
+    @AutoLog
+    public class IndexerIOInputs{
+        public double velocity = 0.0;
+        public int sensorDistance = 0;
+    }
+}

diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
new file mode 100644 (file)
index 0000000..27e0309
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -0,0 +1,242 @@
+package frc.robot.subsystems.intake;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import au.grapplerobotics.LaserCan;
+import au.grapplerobotics.interfaces.LaserCanInterface.Measurement;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.Constants;
+import frc.robot.constants.IdConstants;
+import frc.robot.constants.IntakeConstants;
+
+
+public class Intake extends SubsystemBase {
+    private final TalonFX rollerMotor = new TalonFX(IdConstants.INTAKE_ROLLER);
+    private final TalonFX pivotMotor = new TalonFX(IdConstants.INTAKE_PIVOT, Constants.CANIVORE_CAN);
+    
+    private SingleJointedArmSim stowArmSim;
+    private Mechanism2d stowMechanism2d;
+    private MechanismLigament2d stowWheelLigament;
+
+    private final double positionTolerance = 5;
+
+    private final PIDController stowPID = new PIDController(0.015, 0, 0);
+    private double power;
+
+    private LaserCan laserCan;
+    private boolean hasCoral = false;
+    private boolean isMoving = false;
+    private Timer laserCanSimTimer;
+    private DCMotor dcMotor = DCMotor.getKrakenX60(1);
+    private ArmFeedforward feedforward = new ArmFeedforward(0,
+            Constants.GRAVITY_ACCELERATION * IntakeConstants.CENTER_OF_MASS_DIST * IntakeConstants.MASS
+                    / IntakeConstants.PIVOT_GEAR_RATIO * dcMotor.rOhms / dcMotor.KtNMPerAmp / Constants.ROBOT_VOLTAGE,
+            0);
+    private double startPosition = 90;
+
+    private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged();
+
+    public Intake() {
+        if (RobotBase.isSimulation()) {
+            stowMechanism2d = new Mechanism2d(10, 10);
+            stowWheelLigament = stowMechanism2d.getRoot("Root", 5, 5)
+                    .append(new MechanismLigament2d("Intake", 4, startPosition));
+            SmartDashboard.putData("Intake pivot", stowMechanism2d);
+            stowArmSim = new SingleJointedArmSim(
+                    dcMotor,
+                    IntakeConstants.PIVOT_GEAR_RATIO,
+                    IntakeConstants.MOMENT_OFiNERTIA,
+                    IntakeConstants.ARM_LENGTH,
+                    Math.toRadians(0),
+                    Math.toRadians(90),
+                    true,
+                    Units.degreesToRadians(startPosition));
+            laserCanSimTimer = new Timer();
+        } else {
+            // laserCan = new LaserCan(IdConstants.INTAKE_LASER_CAN);
+            // try {
+            //     laserCan.setRangingMode(RangingMode.SHORT);
+            //     laserCan.setTimingBudget(TimingBudget.TIMING_BUDGET_20MS);
+            //     laserCan.setRegionOfInterest(new RegionOfInterest(-4, -4, 8, 8));
+            // } catch (ConfigurationFailedException e) {
+            //     DriverStation.reportError("Intake LaserCan configuration error", true);
+            // }
+        }
+        rollerMotor.getConfigurator().apply(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake).withInverted(InvertedValue.CounterClockwise_Positive));
+        pivotMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive));
+        pivotMotor.setPosition(Units.degreesToRotations(startPosition) * IntakeConstants.PIVOT_GEAR_RATIO);
+        pivotMotor.setNeutralMode(NeutralModeValue.Coast);
+        stowPID.setTolerance(positionTolerance);
+        
+        setAngle(startPosition);
+
+       rollerMotor.getConfigurator().apply(new CurrentLimitsConfigs().withStatorCurrentLimit(40).withStatorCurrentLimitEnable(true));
+    }
+
+    /**
+     * publishes stuff to smartdashboard
+     */
+    @SuppressWarnings("unused")
+    private void publish() {
+        SmartDashboard.putNumber("Stow Motor Position", getPivotAngle());
+        SmartDashboard.putNumber("Target Angle", stowPID.getSetpoint());
+        SmartDashboard.putNumber("Roller Motor Power", power);
+
+        SmartDashboard.putBoolean("Has Coral", hasCoral());
+        SmartDashboard.putBoolean("Stow Arm Sim - Is Stowed", isAtSetpoint(90));
+        SmartDashboard.putBoolean("Stow Arm Sim - Is Unstowed", isAtSetpoint(0));
+        SmartDashboard.putBoolean("Is Roller Active", rollerMotor.get() > 0);
+    }
+
+    @Override
+    public void periodic() {
+        //publish();
+        // SmartDashboard.putNumber("angle", getPivotAngle());
+        // SmartDashboard.putBoolean("Intake has coral", hasCoral());
+
+        double position = getPivotAngle();
+        power = stowPID.calculate(position) + feedforward.calculate(Units.degreesToRadians(position), 0);
+        power = MathUtil.clamp(power, -1, 1);
+        pivotMotor.set(power);
+        if (laserCan != null) {
+            Measurement measurement = laserCan.getMeasurement();
+            hasCoral = measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT
+                    && measurement.distance_mm <= 1000 * IntakeConstants.DETECT_CORAL_DIST;
+        }
+        if(RobotBase.isSimulation()) {
+            inputs.measuredPivotPosition = Units.radiansToDegrees(stowArmSim.getAngleRads());
+        } else {
+            inputs.measuredPivotPosition = Units.rotationsToDegrees(pivotMotor.getPosition().getValueAsDouble()) / IntakeConstants.PIVOT_GEAR_RATIO;
+        }
+        inputs.rollerVelocity = rollerMotor.getVelocity().getValueAsDouble();
+        Logger.processInputs("Intake", inputs);
+
+        Logger.recordOutput("Intake/PivotSetpoint", stowPID.getSetpoint());        
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        stowArmSim.setInputVoltage(power * Constants.ROBOT_VOLTAGE);
+        stowArmSim.update(Constants.LOOP_TIME);
+        stowWheelLigament.setAngle(Units.radiansToDegrees(stowArmSim.getAngleRads()));
+        if (!isMoving) {
+            laserCanSimTimer.reset();
+            laserCanSimTimer.start();
+            hasCoral = false;
+        } else {
+            if (isAtSetpoint()) {
+                laserCanSimTimer.start();
+            }
+            hasCoral = laserCanSimTimer.hasElapsed(0.5) && !laserCanSimTimer.hasElapsed(1);
+        }
+    }
+
+    /**
+     * Gets the rotation of the intake.
+     * 
+     * @return the rotation of the intake (in degrees).
+     */
+    public double getPivotAngle() {
+        return inputs.measuredPivotPosition;
+    }
+
+    public PIDController getPID() {
+        return stowPID;
+    }
+
+    /**
+     * Checks if intake has coral.
+     * 
+     * @return Boolean (True if has Coral, False otherwise)
+     */
+    @AutoLogOutput(key = "Intake/HasCoral")
+    public boolean hasCoral() {
+        return hasCoral;
+    }
+
+    /**
+     * Checks if motor is at current setpoint.
+     * 
+     * @return Boolean (True if at setpoint, False otherwise)
+     */
+    public boolean isAtSetpoint(double setpoint) {
+        return Math.abs(getPivotAngle() - setpoint) < positionTolerance;
+    }
+
+    /**
+     * Returns whether or not the intake is at its setpoint
+     * 
+     * @return True if it is at the setpoint, false otherwise
+     */
+    @AutoLogOutput(key = "Intake/AtSetPoint")
+    public boolean isAtSetpoint() {
+        return stowPID.atSetpoint();
+    }
+
+    /**
+     * Sets the desired angle of the intake, mostly to stow or unstow.
+     * 
+     * @param angle desired angle of the intake in degrees
+     */
+    public void setAngle(double angle) {
+        stowPID.setSetpoint(angle);
+    }
+
+    /**
+     * Sets the speed of the roller motor.
+     * 
+     * @param power The desired speed of the roller, between 0 and 1.
+     */
+    public void setSpeed(double power) {
+        rollerMotor.set(power);
+        isMoving = Math.abs(power) < 0.01;
+
+    }
+
+    /**
+     * Moves the intake up, doesn't stop it.
+     */
+    public void stow() {
+        stowPID.setSetpoint(IntakeConstants.STOW_SETPOINT);
+    }
+
+    /**
+     * Moves the intake down.
+     */
+    public void unstow() {
+        stowPID.setSetpoint(IntakeConstants.INTAKE_SETPOINT);
+    }
+
+    /**
+     * Stops the motor.
+     */
+    public void deactivate() {
+        rollerMotor.set(0);
+    }
+
+    /**
+     * Starts the motor.
+     */
+    public void activate(){
+        rollerMotor.set(IntakeConstants.INTAKE_MOTOR_POWER);
+    }
+}
+

diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
new file mode 100644 (file)
index 0000000..e0e9305
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IntakeIO {
+    @AutoLog
+    public static class IntakeIOInputs {
+        public double rollerVelocity = 0.0;
+        public double measuredPivotPosition = 0.0;
+    }
+}

diff --git a/src/main/java/frc/robot/subsystems/outtake/Outtake.java b/src/main/java/frc/robot/subsystems/outtake/Outtake.java
new file mode 100644 (file)
index 0000000..09bb2e2
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/outtake/Outtake.java
@@ -0,0 +1,51 @@
+package frc.robot.subsystems.outtake;
+
+import edu.wpi.first.wpilibj.simulation.DIOSim;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/**
+ * Abstract class for the outtake. All commands should use this subsystem
+ */
+public abstract class Outtake extends SubsystemBase {
+    /** Coral detected before the rollers */
+    protected DIOSim dioInputLoaded;
+    /** Coral detected after the rollers */
+    protected DIOSim dioInputEjecting;
+
+    protected abstract double getMotorSpeed();
+
+    public void simulationPeriodic(){}
+
+    /** Set the motor power to move the coral */
+    public abstract void setMotor(double power);
+
+    /** stop the coral motor */
+    public void stop() {
+        setMotor(0);
+    }
+
+    /** start spinning the rollers to eject the coral */
+    public abstract void outtake();
+
+    public abstract boolean coralLoaded();
+
+    /**
+     *  Coral is at the ejecting beam break sensor.
+     * @return coral is interrupting the beam breaker.
+     */
+    public abstract boolean coralEjecting();
+
+    public abstract void reverse();
+
+    public void removeAlgae(){
+        setMotor(-0.6);
+    }
+
+    public void intakeAlgaeReef() {
+        setMotor(-0.6);
+    }
+
+    public void outtakeAlgae() {
+        setMotor(0.9);
+    }
+}

diff --git a/src/main/java/frc/robot/subsystems/outtake/OuttakeAlpha.java b/src/main/java/frc/robot/subsystems/outtake/OuttakeAlpha.java
new file mode 100644 (file)
index 0000000..edd057c
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/outtake/OuttakeAlpha.java
@@ -0,0 +1,107 @@
+package frc.robot.subsystems.outtake;
+
+
+import com.revrobotics.ColorSensorV3;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkFlexConfig;
+
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.simulation.DIOSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.constants.IdConstants;
+
+
+public class OuttakeAlpha extends Outtake {
+
+    private SparkFlex  motor = new SparkFlex(IdConstants.OUTTAKE_MOTOR_ALPHA, MotorType.kBrushless);
+    private double power;
+
+
+    /** Coral detected before the rollers */
+    private final ColorSensorV3 colorSensor = new ColorSensorV3(IdConstants.i2cPort);
+    /** Coral detected after the rollers */
+    private DigitalInput digitalInputEjecting = new DigitalInput(IdConstants.OUTTAKE_DIO_EJECTING);
+
+
+
+    public OuttakeAlpha(){
+        motor.configure(new SparkFlexConfig()
+            .inverted(true)
+            .idleMode(IdleMode.kBrake),
+            ResetMode.kResetSafeParameters,
+            PersistMode.kNoPersistParameters
+        );
+        if (RobotBase.isSimulation()){
+            // object that will control the ejecting sensor
+            dioInputEjecting = new DIOSim(digitalInputEjecting);
+            // assume coral is loaded
+            dioInputLoaded.setValue(false);
+            // we are not ejecting
+            dioInputEjecting.setValue(true);
+        }
+    }
+
+    @Override
+    protected double getMotorSpeed(){
+        return motor.get();
+    }
+
+    @Override
+    public void periodic(){
+        motor.set(power);
+        // SmartDashboard.putBoolean("Coral loaded", coralLoaded());
+        // SmartDashboard.putBoolean("Coral ejected", coralEjecting());
+
+    }
+
+    /** Set the motor power to move the coral */
+    public void setMotor(double power){
+        this.power = power;
+    }
+
+
+    /** start spinning the rollers to eject the coral */
+    public void outtake(){
+        // assumes the coral is present
+        // if the coral is not present, we should not bother to spin the rollers
+        setMotor(SmartDashboard.getNumber("wheel speed", 0.2));
+        // this starts the motor... what needs to be done later?
+    }
+
+    /**
+     *  Coral is at the ejecting beam break sensor.
+     * @return coral is interrupting the beam breaker.
+     */
+    public boolean coralEjecting() {
+        return !digitalInputEjecting.get();
+    }
+
+
+    public void reverse(){
+        setMotor(-0.2);
+    }
+
+
+    public boolean isSimulation(){
+        return RobotBase.isSimulation();
+    }
+    
+    public int getProximity() {
+        return colorSensor.getProximity();  // Returns 0 (far) to ~2047 (very close)
+    }
+
+    // coral detection from color sensor
+    public boolean coralLoaded() {
+        //this is about 1/2inch away -- might have to change based on placement
+        if (getProximity() > 800) {
+            return true;
+        }
+        return false;
+    }
+}

diff --git a/src/main/java/frc/robot/subsystems/outtake/OuttakeComp.java b/src/main/java/frc/robot/subsystems/outtake/OuttakeComp.java
new file mode 100644 (file)
index 0000000..b2daa19
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/outtake/OuttakeComp.java
@@ -0,0 +1,87 @@
+package frc.robot.subsystems.outtake;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.revrobotics.ColorSensorV3;
+
+import edu.wpi.first.hal.I2CJNI;
+import frc.robot.constants.IdConstants;
+
+public class OuttakeComp extends Outtake {
+
+    private TalonFX  motor = new TalonFX(IdConstants.OUTTAKE_MOTOR_COMP);
+    private double power;
+
+    /** Coral detected before the rollers */
+    private ColorSensorV3 colorSensor = new ColorSensorV3(IdConstants.i2cPort);
+
+    OuttakeIOIntakesAutoLogged inputs = new OuttakeIOIntakesAutoLogged();
+
+    public OuttakeComp(){
+        motor.getConfigurator().apply(new MotorOutputConfigs()
+            .withInverted(InvertedValue.Clockwise_Positive)
+            .withNeutralMode(NeutralModeValue.Brake)
+        );
+    }
+
+    @Override
+    protected double getMotorSpeed() {
+        return power;
+    }
+
+    @Override
+    public void periodic(){
+        motor.set(power);
+        //  SmartDashboard.putBoolean("Coral loaded", coralLoaded());
+        //  SmartDashboard.putBoolean("Coral ejected", coralEjecting());
+
+        inputs.motorVelocity = motor.getVelocity().getValueAsDouble();
+        Logger.processInputs("Outtake", inputs);
+        //Logger.recordOutput("Outtake/Sensor", getProximity());
+        //Logger.recordOutput("Outtake/SensorConnected", colorSensor.isConnected());
+    }
+
+    /** Set the motor power to move the coral */
+    public void setMotor(double power){
+        this.power = power;
+    }
+
+    /** start spinning the rollers to eject the coral */
+    public void outtake(){
+        setMotor(-0.3);
+    }
+
+    /**
+     *  Coral is in the outtake.
+     * @return The same thing as coralLoaded(), for compatibility with previous code
+     */
+    public boolean coralEjecting() {
+        return coralLoaded();
+    }
+
+
+    public void reverse(){
+        setMotor(0.2);
+    }
+
+    public int getProximity() {
+        inputs.proximity = colorSensor.getProximity();
+        if (inputs.proximity > 0){
+            return inputs.proximity;
+        }
+        else{
+            I2CJNI.i2CClose(1);
+            colorSensor = new ColorSensorV3(IdConstants.i2cPort);
+            return inputs.proximity = colorSensor.getProximity();  // Returns 0 (far) to ~2047 (very close)
+        }
+    }
+
+    // coral detection from color sensor
+    public boolean coralLoaded() {
+        return getProximity() > 2000;
+    }
+}

diff --git a/src/main/java/frc/robot/subsystems/outtake/OuttakeIO.java b/src/main/java/frc/robot/subsystems/outtake/OuttakeIO.java
new file mode 100644 (file)
index 0000000..006d7c8
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/outtake/OuttakeIO.java
@@ -0,0 +1,11 @@
+package frc.robot.subsystems.outtake;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface OuttakeIO {
+    @AutoLog
+    public static class OuttakeIOIntakes {
+        public double motorVelocity = 0.0;
+        public int proximity = 0;
+    }
+}

diff --git a/src/main/java/frc/robot/util/AngledElevatorSim.java b/src/main/java/frc/robot/util/AngledElevatorSim.java
new file mode 100644 (file)
index 0000000..0334792
--- /dev/null
+++ b/src/main/java/frc/robot/util/AngledElevatorSim.java
@@ -0,0 +1,95 @@
+package frc.robot.util;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+
+public class AngledElevatorSim extends ElevatorSim {
+    private double angle;
+    private boolean simulateGravity;
+    private double minHeight;
+    private double maxHeight;
+    private double springAccel;
+
+    /**
+     * Creates a simulated angled elevator mechanism.
+     *
+     * @param gearbox The type of and number of motors in the elevator gearbox.
+     * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
+     * @param carriageMassKg The mass of the elevator carriage.
+     * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
+     * @param minHeightMeters The min allowable height of the elevator.
+     * @param maxHeightMeters The max allowable height of the elevator.
+     * @param simulateGravity Whether gravity should be simulated or not.
+     * @param startingHeightMeters The starting height of the elevator.
+     * @param angleRads The angle of the elevator from vertical in radians.
+     * @param springForceNewtons The force of the constant force spring in Newtons. Up is positive.
+     * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+     *     noise is desired. If present must have 1 element for position.
+     */
+    public AngledElevatorSim(
+      DCMotor gearbox,
+      double gearing,
+      double carriageMassKg,
+      double drumRadiusMeters,
+      double minHeightMeters,
+      double maxHeightMeters,
+      boolean simulateGravity,
+      double startingHeightMeters,
+      double angleRads,
+      double springForceNewtons,
+      double...   measurementStdDevs) {
+        super(gearbox, gearing, carriageMassKg, drumRadiusMeters, minHeightMeters, maxHeightMeters, simulateGravity, startingHeightMeters, measurementStdDevs);
+        angle = angleRads;
+        this.simulateGravity = simulateGravity;
+        minHeight = minHeightMeters;
+        maxHeight = maxHeightMeters;
+        springAccel = springForceNewtons/carriageMassKg;
+    }
+
+    // Copied from ElevatorSim with one difference
+    /**
+     * Creates a simulated elevator mechanism.
+     *
+     * @param gearbox The type of and number of motors in the elevator gearbox.
+     * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
+     * @param carriageMassKg The mass of the elevator carriage.
+     * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
+     * @param minHeightMeters The min allowable height of the elevator.
+     * @param maxHeightMeters The max allowable height of the elevator.
+     * @param simulateGravity Whether gravity should be simulated or not.
+     * @param startingHeightMeters The starting height of the elevator.
+     * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+     *     noise is desired. If present must have 1 element for position.
+     */
+    @Override
+    protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
+        // Calculate updated x-hat from Runge-Kutta.
+        var updatedXhat =
+            NumericalIntegration.rkdp(
+                (x, _u) -> {
+                Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
+                if (simulateGravity) {
+                    // This is the only line that is different
+                    xdot = xdot.plus(VecBuilder.fill(0, springAccel-9.8*Math.cos(angle)));
+                }
+                return xdot;
+                },
+                currentXhat,
+                u,
+                dtSeconds);
+
+        // We check for collisions after updating x-hat.
+        if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
+        return VecBuilder.fill(minHeight, 0);
+        }
+        if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
+        return VecBuilder.fill(maxHeight, 0);
+        }
+        return updatedXhat;
+    }
+}

diff --git a/src/main/java/frc/robot/util/ClimbArmSim.java b/src/main/java/frc/robot/util/ClimbArmSim.java
new file mode 100644 (file)
index 0000000..30cde9d
--- /dev/null
+++ b/src/main/java/frc/robot/util/ClimbArmSim.java
@@ -0,0 +1,166 @@
+package frc.robot.util;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+
+public class ClimbArmSim extends SingleJointedArmSim {
+  private boolean m_simulateGravity;
+  private double m_armLenMeters;
+  private double m_minAngle;
+  private double m_maxAngle;
+  private double mass;
+  private double momentOfInertia;
+  private boolean isClimbing;
+
+  /**
+   * Creates a simulated arm mechanism.
+   *
+   * @param plant The linear system that represents the arm. This system can be created with {@link
+   *     edu.wpi.first.math.system.plant.LinearSystemId#createSingleJointedArmSystem(DCMotor,
+   *     double, double)}.
+   * @param gearbox The type of and number of motors in the arm gearbox.
+   * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
+   * @param armLengthMeters The length of the arm.
+   * @param minAngleRads The minimum angle that the arm is capable of.
+   * @param maxAngleRads The maximum angle that the arm is capable of.
+   * @param simulateGravity Whether gravity should be simulated or not.
+   * @param startingAngleRads The initial position of the Arm simulation in radians.
+   * @param robotMassKilograms The mass of the robot in kilograms, including battery and bumpers
+   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+   *     noise is desired. If present must have 1 element for position.
+   */
+  public ClimbArmSim(
+      LinearSystem<N2, N1, N2> plant,
+      DCMotor gearbox,
+      double gearing,
+      double armLengthMeters,
+      double minAngleRads,
+      double maxAngleRads,
+      boolean simulateGravity,
+      double startingAngleRads,
+      double robotMasKilograms,
+      double armMassKilograms,
+      double... measurementStdDevs) {
+    super(plant, gearbox, gearing, armLengthMeters, minAngleRads, maxAngleRads, simulateGravity, startingAngleRads, measurementStdDevs);
+    m_armLenMeters = armLengthMeters;
+    m_minAngle = minAngleRads;
+    m_maxAngle = maxAngleRads;
+    m_simulateGravity = simulateGravity;
+    mass = robotMasKilograms;
+    momentOfInertia = 1.0/3.0 * armMassKilograms * armLengthMeters * armLengthMeters;
+    isClimbing = false;
+  }
+
+  /**
+   * Creates a simulated arm mechanism.
+   *
+   * @param gearbox The type of and number of motors in the arm gearbox.
+   * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
+   * @param jKgMetersSquared The moment of inertia of the arm; can be calculated from CAD software.
+   * @param armLengthMeters The length of the arm.
+   * @param minAngleRads The minimum angle that the arm is capable of.
+   * @param maxAngleRads The maximum angle that the arm is capable of.
+   * @param simulateGravity Whether gravity should be simulated or not.
+   * @param startingAngleRads The initial position of the Arm simulation in radians.
+   * @param robotMassKilograms The mass of the robot in kilograms, including battery and bumpers
+   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+   *     noise is desired. If present must have 1 element for position.
+   */
+  public ClimbArmSim(
+      DCMotor gearbox,
+      double gearing,
+      double jKgMetersSquared,
+      double armLengthMeters,
+      double minAngleRads,
+      double maxAngleRads,
+      boolean simulateGravity,
+      double startingAngleRads,
+      double robotMassKilograms,
+      double... measurementStdDevs) {
+    this(
+        LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing),
+        gearbox,
+        gearing,
+        armLengthMeters,
+        minAngleRads,
+        maxAngleRads,
+        simulateGravity,
+        startingAngleRads,
+        robotMassKilograms,
+        1,
+        measurementStdDevs);
+    momentOfInertia = jKgMetersSquared;
+  }
+
+  public void setIsClimbing(boolean climbing){
+    isClimbing = climbing;
+  }
+  
+  /**
+   * Updates the state of the arm.
+   *
+   * @param currentXhat The current state estimate.
+   * @param u The system inputs (voltage).
+   * @param dtSeconds The time difference between controller updates.
+   */
+  @Override
+  protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
+    // The torque on the arm is given by τ = F⋅r, where F is the force applied by
+    // gravity and r the distance from pivot to center of mass. Recall from
+    // dynamics that the sum of torques for a rigid body is τ = J⋅α, were τ is
+    // torque on the arm, J is the mass-moment of inertia about the pivot axis,
+    // and α is the angular acceleration in rad/s². Rearranging yields: α = F⋅r/J
+    //
+    // We substitute in F = m⋅g⋅cos(θ), where θ is the angle from horizontal:
+    //
+    //   α = (m⋅g⋅cos(θ))⋅r/J
+    //
+    // Multiply RHS by cos(θ) to account for the arm angle. Further, we know the
+    // arm mass-moment of inertia J of our arm is given by J=1/3 mL², modeled as a
+    // rod rotating about it's end, where L is the overall rod length. The mass
+    // distribution is assumed to be uniform. Substitute r=L/2 to find:
+    //
+    //   α = (m⋅g⋅cos(θ))⋅r/(1/3 mL²)
+    //   α = (m⋅g⋅cos(θ))⋅(L/2)/(1/3 mL²)
+    //   α = 3/2⋅g⋅cos(θ)/L
+    //
+    // Adding the torque from the robot weight, which is in the opposite direction as the arm's mass
+    //   α = 3/2⋅g⋅cos(θ)/L - m⋅g⋅cos(θ)⋅L/J
+    //
+    // This acceleration is next added to the linear system dynamics ẋ=Ax+Bu
+    //
+    //   f(x, u) = Ax + Bu + [0  α]ᵀ
+    //   f(x, u) = Ax + Bu + [0  3/2⋅g⋅cos(θ)/L - m⋅g⋅cos(θ)⋅L/J]ᵀ
+
+    Matrix<N2, N1> updatedXhat =
+        NumericalIntegration.rkdp(
+            (Matrix<N2, N1> x, Matrix<N1, N1> _u) -> {
+              Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
+              if (m_simulateGravity) {
+                double alphaGrav = 3.0 / 2.0 * -9.8 * Math.cos(x.get(0, 0)) / m_armLenMeters + (isClimbing ? mass * 9.8 * Math.cos(x.get(0, 0)) * m_armLenMeters / momentOfInertia : 0);
+                xdot = xdot.plus(VecBuilder.fill(0, alphaGrav));
+              }
+              return xdot;
+            },
+            currentXhat,
+            u,
+            dtSeconds);
+
+    // We check for collision after updating xhat
+    if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
+      return VecBuilder.fill(m_minAngle, 0);
+    }
+    if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
+      return VecBuilder.fill(m_maxAngle, 0);
+    }
+    return updatedXhat;
+  }
+
+}

diff --git a/src/main/java/frc/robot/util/ConversionUtils.java b/src/main/java/frc/robot/util/ConversionUtils.java
new file mode 100644 (file)
index 0000000..7c6da16
--- /dev/null
+++ b/src/main/java/frc/robot/util/ConversionUtils.java
@@ -0,0 +1,146 @@
+package frc.robot.util;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc.robot.constants.FieldConstants;
+
+public class ConversionUtils {
+
+    /**
+     * @param positionCounts CANCoder Position Counts
+     * @param gearRatio      Gear Ratio between CANCoder and Mechanism
+     * @return Degrees of Rotation of Mechanism
+     */
+    public static double CANcoderToDegrees(double positionCounts, double gearRatio) {
+        return positionCounts * (360.0 / (gearRatio * 4096.0));
+    }
+
+    /**
+     * @param degrees   Degrees of rotation of Mechanism
+     * @param gearRatio Gear Ratio between CANCoder and Mechanism
+     * @return CANCoder Position Counts
+     */
+    public static double degreesToCANcoder(double degrees, double gearRatio) {
+        return degrees / (360.0 / (gearRatio * 4096.0));
+    }
+
+    /**
+     * @param positionCounts CANCoder Position Counts
+     * @param gearRatio      Gear Ratio between CANCoder and Mechanism
+     * @return Radians of Rotation of Mechanism
+     */
+    public static double CANcoderToRadians(double positionCounts, double gearRatio) {
+        return Math.toRadians(CANcoderToDegrees(positionCounts, gearRatio));
+    }
+
+    /**
+     * @param radians   Radians of rotation of Mechanism
+     * @param gearRatio Gear Ratio between CANCoder and Mechanism
+     * @return CANCoder Position Counts
+     */
+    public static double radiansToCANcoder(double radians, double gearRatio) {
+        return degreesToCANcoder(Math.toDegrees(radians), gearRatio);
+    }
+
+    /**
+     * @param positionCounts Falcon Position Counts
+     * @param gearRatio      Gear Ratio between Falcon and Mechanism
+     * @return Degrees of Rotation of Mechanism
+     */
+    public static double falconToDegrees(double positionCounts, double gearRatio) {
+        return positionCounts * (360.0 / (gearRatio * 2048.0));
+    }
+
+    /**
+     * @param degrees   Degrees of rotation of Mechanism
+     * @param gearRatio Gear Ratio between Falcon and Mechanism
+     * @return Falcon Position Counts
+     */
+    public static double degreesToFalcon(double degrees, double gearRatio) {
+        return degrees / (360.0 / (gearRatio * 2048.0));
+    }
+
+    /**
+     * @param velocityCounts Falcon Velocity Counts
+     * @param gearRatio      Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM)
+     * @return RPM of Mechanism
+     */
+    public static double falconToRPM(double velocityCounts, double gearRatio) {
+        double motorRPM = velocityCounts * (600.0 / 2048.0);
+        return motorRPM / gearRatio;
+    }
+
+    /**
+     * @param RPM       RPM of mechanism
+     * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM)
+     * @return RPM of Mechanism
+     */
+    public static double RPMToFalcon(double RPM, double gearRatio) {
+        double motorRPM = RPM * gearRatio;
+        return motorRPM * (2048.0 / 600.0);
+    }
+
+    /**
+     * @param velocitycounts Falcon Velocity Counts
+     * @param circumference  Circumference of Wheel
+     * @param gearRatio      Gear Ratio between Falcon and Mechanism (set to 1 for Falcon MPS)
+     * @return Falcon Velocity Counts
+     */
+    public static double falconToMPS(double velocitycounts, double circumference, double gearRatio) {
+        double wheelRPM = falconToRPM(velocitycounts, gearRatio);
+        return (wheelRPM * circumference) / 60;
+    }
+
+    /**
+     * @param velocity      Velocity MPS
+     * @param circumference Circumference of Wheel
+     * @param gearRatio     Gear Ratio between Falcon and Mechanism (set to 1 for Falcon MPS)
+     * @return Falcon Velocity Counts
+     */
+    public static double MPSToFalcon(double velocity, double circumference, double gearRatio) {
+        double wheelRPM = ((velocity * 60) / circumference);
+        return RPMToFalcon(wheelRPM, gearRatio);
+    }
+
+    /**
+     * @param positionCounts Falcon Position Counts
+     * @param circumference  Circumference of Wheel
+     * @param gearRatio      Gear Ratio between Falcon and Wheel
+     * @return Meters
+     */
+    public static double falconToMeters(double positionCounts, double circumference, double gearRatio) {
+        return positionCounts * (circumference / (gearRatio * 2048.0));
+    }
+
+    /**
+     * @param meters        Meters
+     * @param circumference Circumference of Wheel
+     * @param gearRatio     Gear Ratio between Falcon and Wheel
+     * @return Falcon Position Counts
+     */
+    public static double MetersToFalcon(double meters, double circumference, double gearRatio) {
+        return meters / (circumference / (gearRatio * 2048.0));
+    }
+
+
+    /**
+     * Converts between an absolute coordinate system and the pathplanner coordinate system.
+     * <p>
+     * Absolute coordinate system always has the origin right of the blue driver station from blue driver perspective,
+     * bottom left if looking down at the field. Positive X goes toward red alliance (forward from blue driver perspective)
+     * and positive Y toward red loading zone (left from blue driver perspective). The Pathplanner coordinate system has the coordinate
+     * system rotated such that the origin starts right of the current driver station.
+     * <p> The transformation is self-inverse, so there is no second function to convert back.
+     *
+     * @param pose     pose to convert
+     * @param alliance alliance PathPlanner is using for their origin
+     * @return converted pose
+     */
+    public static Pose2d absolutePoseToPathPlannerPose(Pose2d pose, Alliance alliance) {
+        if (alliance == Alliance.Red) {
+            return pose.relativeTo(new Pose2d(FieldConstants.FIELD_LENGTH, FieldConstants.FIELD_WIDTH, new Rotation2d(Math.PI)));
+        }
+        return new Pose2d(pose.getX(), pose.getY(), pose.getRotation());
+    }
+}

diff --git a/src/main/java/frc/robot/util/DynamicSlewRateLimiter.java b/src/main/java/frc/robot/util/DynamicSlewRateLimiter.java
new file mode 100644 (file)
index 0000000..6d5df8c
--- /dev/null
+++ b/src/main/java/frc/robot/util/DynamicSlewRateLimiter.java
@@ -0,0 +1,197 @@
+// 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 frc.robot.util;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.util.WPIUtilJNI;
+
+/**
+ * A class that limits the rate of change of an input value. Useful for implementing voltage,
+ * setpoint, and/or output ramps. A slew-rate limit is most appropriate when the quantity being
+ * controlled is a velocity or a voltage; when controlling a position, consider using a {@link
+ * edu.wpi.first.math.trajectory.TrapezoidProfile} instead.
+ * Edited by 972 to be "dynamic", that is, the slew rate can be modified on the fly.
+ * Additionally, it can be set to be continuous on a range, useful for angles.
+ */
+public class DynamicSlewRateLimiter {
+    private double positiveRateLimit;
+    private double negativeRateLimit;
+    private double prevVal;
+    private double prevTime;
+
+    private boolean continuous = false;
+    private double lowerContinuousLimit = -1;
+    private double upperContinuousLimit = 1;
+
+    /**
+     * Creates a new DynamicSlewRateLimiter with the given positive and negative rate limits and initial
+     * value.
+     *
+     * @param positiveRateLimit The rate-of-change limit in the positive direction, in units per
+     *                          second. This is expected to be positive.
+     * @param negativeRateLimit The rate-of-change limit in the negative direction, in units per
+     *                          second. This is expected to be negative.
+     * @param initialValue      The initial value of the input.
+     */
+    public DynamicSlewRateLimiter(double positiveRateLimit, double negativeRateLimit, double initialValue) {
+        this.positiveRateLimit = positiveRateLimit;
+        this.negativeRateLimit = negativeRateLimit;
+        prevVal = initialValue;
+        prevTime = WPIUtilJNI.now() * 1e-6;
+    }
+
+    /**
+     * Creates a new DynamicSlewRateLimiter with the given positive rate limit and negative rate limit of
+     * -rateLimit and initial value.
+     *
+     * @param rateLimit    The rate-of-change limit, in units per second.
+     * @param initialValue The initial value of the input.
+     */
+    @Deprecated(since = "2023", forRemoval = true)
+    public DynamicSlewRateLimiter(double rateLimit, double initialValue) {
+        this(rateLimit, -rateLimit, initialValue);
+    }
+
+    /**
+     * Creates a new SlewRateLimiter with the given positive rate limit and negative rate limit of
+     * -rateLimit.
+     *
+     * @param rateLimit The rate-of-change limit, in units per second.
+     */
+    public DynamicSlewRateLimiter(double rateLimit) {
+        this(rateLimit, -rateLimit, 0);
+    }
+
+    /**
+     * Filters the input to limit its slew rate.
+     *
+     * @param input The input value whose slew rate is to be limited.
+     * @return The filtered value, which will not change faster than the slew rate.
+     */
+    public double calculate(double input) {
+        double currentTime = WPIUtilJNI.now() * 1e-6;
+        double elapsedTime = currentTime - prevTime;
+        prevTime = currentTime;
+
+        double change = MathUtil.clamp(
+                input - prevVal,
+                negativeRateLimit * elapsedTime,
+                positiveRateLimit * elapsedTime);
+
+        if (continuous) {
+            change = MathUtil.clamp(
+                MathUtil.inputModulus(input - prevVal, lowerContinuousLimit, upperContinuousLimit),
+                negativeRateLimit * elapsedTime,
+                positiveRateLimit * elapsedTime);
+
+            prevVal += change;
+
+            //Extra check to make sure it is within the limits, probably unnecessary
+            prevVal = MathUtil.inputModulus(prevVal, lowerContinuousLimit, upperContinuousLimit);
+        } else {
+            prevVal += change;
+        }
+
+        return prevVal;
+    }
+
+    /**
+     * Sets a new slewrate and filters the input to limit its slew rate.
+     *
+     * @param input     The input value whose slew rate is to be limited.
+     * @param rateLimit The new rate-of-change limit, in units per second.
+     * @return The filtered value, which will not change faster than the slew rate.
+     */
+    public double calculate(double input, double rateLimit) {
+        setRateLimit(rateLimit);
+        return calculate(input);
+    }
+
+    /**
+     * Sets new slew rates and filters the input to limit its slew rate.
+     *
+     * @param input             The input value whose slew rate is to be limited.
+     * @param positiveRateLimit The rate-of-change limit in the positive direction, in units per
+     *                          second. This is expected to be positive.
+     * @param negativeRateLimit The rate-of-change limit in the negative direction, in units per
+     *                          second. This is expected to be negative.
+     * @return The filtered value, which will not change faster than the slew rate.
+     */
+    public double calculate(double input, double positiveRateLimit, double negativeRateLimit) {
+        setRateLimit(positiveRateLimit, negativeRateLimit);
+        return calculate(input);
+    }
+
+
+    /**
+     * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so.
+     *
+     * @param value The value to reset to.
+     */
+    public void reset(double value) {
+        prevVal = value;
+        prevTime = WPIUtilJNI.now() * 1e-6;
+    }
+
+    /**
+     * set positive rate limit
+     *
+     * @param positiveRateLimit new positive rate limit
+     */
+    public void setPositiveRateLimit(double positiveRateLimit) {
+        this.positiveRateLimit = positiveRateLimit;
+    }
+
+    /**
+     * set negative rate limit
+     *
+     * @param negativeRateLimit new negative rate limit
+     */
+    public void setNegativeRateLimit(double negativeRateLimit) {
+        this.negativeRateLimit = negativeRateLimit;
+    }
+
+    /**
+     * Sets positive and negative rate limits
+     *
+     * @param rateLimit new rate limits
+     */
+    public void setRateLimit(double rateLimit) {
+        positiveRateLimit = rateLimit;
+        negativeRateLimit = -rateLimit;
+    }
+
+    /**
+     * Sets positive and negative rate limits
+     *
+     * @param positiveRateLimit new positive rate limit
+     * @param negativeRateLimit new negative rate limit
+     */
+    public void setRateLimit(double positiveRateLimit, double negativeRateLimit) {
+        this.positiveRateLimit = positiveRateLimit;
+        this.negativeRateLimit = negativeRateLimit;
+    }
+
+    /**
+     * Sets Continuous Limits
+     *
+     * @param lowerContinuousLimit Lower Continuous Limit
+     * @param upperContinuousLimit Upper Continuous Limit
+     */
+    public void setContinuousLimits(double lowerContinuousLimit, double upperContinuousLimit) {
+        this.lowerContinuousLimit = lowerContinuousLimit;
+        this.upperContinuousLimit = upperContinuousLimit;
+    }
+
+    /**
+     * Enables or disables continuous
+     * WARNING: Continuous doesn't work properly with non-symmetrical rate limits
+     *
+     * @param continuous is continuous enabled
+     */
+    public void enableContinuous(boolean continuous) {
+        this.continuous = continuous;
+    }
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/util/EqualsUtil.java b/src/main/java/frc/robot/util/EqualsUtil.java
new file mode 100644 (file)
index 0000000..93c3616
--- /dev/null
+++ b/src/main/java/frc/robot/util/EqualsUtil.java
@@ -0,0 +1,29 @@
+// 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 frc.robot.util;
+
+import edu.wpi.first.math.geometry.Twist2d;
+
+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/src/main/java/frc/robot/util/FeedForwardCharacterizationData.java b/src/main/java/frc/robot/util/FeedForwardCharacterizationData.java
new file mode 100644 (file)
index 0000000..a0d92e7
--- /dev/null
+++ b/src/main/java/frc/robot/util/FeedForwardCharacterizationData.java
@@ -0,0 +1,73 @@
+package frc.robot.util;
+
+import lib.PolynomialRegression;
+
+import java.util.LinkedList;
+import java.util.List;
+
+/**
+ * A class for storing and processing feedforward characterization data. Used in automatic feedforward characterization.
+ */
+public class FeedForwardCharacterizationData {
+    private PolynomialRegression regression;
+    private final List<Double> velocityData = new LinkedList<>();
+    private final List<Double> voltageData = new LinkedList<>();
+
+    /**
+     * Adds a data point to the data set.
+     *
+     * @param velocity the velocity of the motor
+     * @param voltage  the voltage applied to the motor
+     */
+    public void add(double velocity, double voltage) {
+        if (Math.abs(velocity) > 1E-4) {
+            velocityData.add(Math.abs(velocity));
+            voltageData.add(Math.abs(voltage));
+        }
+    }
+
+    /**
+     * Processes the data set using {@link PolynomialRegression}
+     *
+     * @see PolynomialRegression
+     */
+    public void process() {
+        // creates a new process polynomial regression to get calculated values
+        regression = new PolynomialRegression(
+                velocityData.stream().mapToDouble(Double::doubleValue).toArray(),
+                voltageData.stream().mapToDouble(Double::doubleValue).toArray(),
+                1
+        );
+    }
+
+    /**
+     * Gets the static voltage of the motor.
+     *
+     * @return the static voltage of the motor
+     */
+    public double getStatic() {
+        // gets y-intercept
+        return regression.beta(0);
+    }
+
+    /**
+     * Gets the velocity of the motor.
+     *
+     * @return the velocity of the motor
+     */
+    public double getVelocity() {
+        // gets a slope of regression line
+        return regression.beta(1);
+    }
+
+    /**
+     * Gets the variance of the data set.
+     *
+     * @return the variance of the data set
+     */
+    public double getVariance() {
+        // gets variance of data set
+        return regression.R2();
+    }
+}
+

diff --git a/src/main/java/frc/robot/util/Functions.java b/src/main/java/frc/robot/util/Functions.java
deleted file mode 100644 (file)
index bd18946..0000000
--- a/src/main/java/frc/robot/util/Functions.java
+++ /dev/null
@@ -1,50 +0,0 @@
-package frc.robot.util;
-
-import frc.robot.constants.Constants;
-
-public class Functions {
-
-  /**
-   * Deadbands an input to [-1, -deadband], [deadband, 1], rescaling inputs to be linear from
-   * (deadband, 0) to (1,1)
-   *
-   * @param input The input value to rescale
-   * @param deadband The deadband
-   * @return the input rescaled and to fit [-1, -deadband], [deadband, 1]
-   */
-  public static double deadband(double input, double deadband) {
-    if (Math.abs(input) <= deadband) {
-      return 0;
-    } else if (Math.abs(input) == 1) {
-      return input;
-    } else {
-      return (1 / (1 - deadband) * (input + Math.signum(-input) * deadband));
-    }
-  }
-
-  /**
-   * Deadbands an input to [-1, -Constants.oi.kDeadband], [Constants.oi.kDeadband, 1], rescaling inputs to be linear from
-   * (Constants.oi.kDeadband, 0) to (1,1)
-   *
-   * @param input The input value to rescale
-   * @return the input rescaled and to fit [-1, -kDeadband], [kDeadband, 1]
-   */
-  public static double deadband(double input) {
-    return deadband(input, Constants.oi.kDeadband);
-  }
-
-  /**
-   * An exponential function that maintains positive or negative.
-   * @param exponent the power to raise the base to
-   * @param base the base which will be raised to the power
-   * @return base to the power of exponent, maintaining sign of base
-   */
-  public static double expoMS(double exponent, double base) {
-    // weird stuff will happen if you don't put a number > 0
-    double finVal = Math.pow(Math.abs(base), exponent);
-    if (base < 0) {
-      finVal *= -1;
-    }
-    return finVal;
-  }
-}

diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java
new file mode 100644 (file)
index 0000000..9cbc39e
--- /dev/null
+++ b/src/main/java/frc/robot/util/GeomUtil.java
@@ -0,0 +1,163 @@
+// 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 frc.robot.util;
+
+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.Transform2d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+
+/** Geometry utilities for working with translations, rotations, transforms, and poses. */
+public class GeomUtil {
+  /**
+   * 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());
+  }
+
+  public static Pose2d inverse(Pose2d pose) {
+    Rotation2d rotationInverse = pose.getRotation().unaryMinus();
+    return new Pose2d(
+        pose.getTranslation().unaryMinus().rotateBy(rotationInverse), rotationInverse);
+  }
+
+  /**
+   * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic
+   * chain
+   *
+   * @param transform The transform that will represent the pose
+   * @return The resulting pose
+   */
+  public static Pose2d toPose2d(Transform2d transform) {
+    return new Pose2d(transform.getTranslation(), transform.getRotation());
+  }
+
+  /**
+   * Creates a pure translated pose
+   *
+   * @param translation The translation to create the pose with
+   * @return The resulting pose
+   */
+  public static Pose2d toPose2d(Translation2d translation) {
+    return new Pose2d(translation, new Rotation2d());
+  }
+
+  /**
+   * Creates a pure rotated pose
+   *
+   * @param rotation The rotation to create the pose with
+   * @return The resulting pose
+   */
+  public static Pose2d toPose2d(Rotation2d rotation) {
+    return new Pose2d(new Translation2d(), rotation);
+  }
+
+  /**
+   * Multiplies a twist by a scaling factor
+   *
+   * @param twist The twist to multiply
+   * @param factor The scaling factor for the twist components
+   * @return The new twist
+   */
+  public static Twist2d multiply(Twist2d twist, double factor) {
+    return new Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor);
+  }
+
+  /**
+   * Converts a Pose3d to a Transform3d to be used in a kinematic chain
+   *
+   * @param pose The pose that will represent the transform
+   * @return The resulting transform
+   */
+  public static Transform3d toTransform3d(Pose3d pose) {
+    return new Transform3d(pose.getTranslation(), pose.getRotation());
+  }
+
+  /**
+   * Converts a Transform3d to a Pose3d to be used as a position or as the start of a kinematic
+   * chain
+   *
+   * @param transform The transform that will represent the pose
+   * @return The resulting pose
+   */
+  public static Pose3d toPose3d(Transform3d transform) {
+    return new Pose3d(transform.getTranslation(), transform.getRotation());
+  }
+
+  /**
+   * Converts a ChassisSpeeds to a Twist2d by extracting two dimensions (Y and Z). chain
+   *
+   * @param speeds The original translation
+   * @return The resulting translation
+   */
+  public static Twist2d toTwist2d(ChassisSpeeds speeds) {
+    return new Twist2d(
+        speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond);
+  }
+
+  /**
+   * Creates a new pose from an existing one using a different translation value.
+   *
+   * @param pose The original pose
+   * @param translation The new translation to use
+   * @return The new pose with the new translation and original rotation
+   */
+  public static Pose2d withTranslation(Pose2d pose, Translation2d translation) {
+    return new Pose2d(translation, pose.getRotation());
+  }
+
+  /**
+   * Creates a new pose from an existing one using a different rotation value.
+   *
+   * @param pose The original pose
+   * @param rotation The new rotation to use
+   * @return The new pose with the original translation and new rotation
+   */
+  public static Pose2d withRotation(Pose2d pose, Rotation2d rotation) {
+    return new Pose2d(pose.getTranslation(), rotation);
+  }
+}

diff --git a/src/main/java/frc/robot/util/MathUtils.java b/src/main/java/frc/robot/util/MathUtils.java
new file mode 100644 (file)
index 0000000..fe52234
--- /dev/null
+++ b/src/main/java/frc/robot/util/MathUtils.java
@@ -0,0 +1,156 @@
+package frc.robot.util;
+
+import java.util.List;
+
+import edu.wpi.first.math.MathUtil;
+import frc.robot.constants.Constants;
+
+/**
+ * Utility class for useful functions.
+ */
+public class MathUtils {
+
+    /**
+     * Deadbands an input to [-1, -deadband], [deadband, 1], rescaling inputs to be linear from
+     * (deadband, 0) to (1,1)
+     *
+     * @param input    The input value to rescale
+     * @param deadband The deadband
+     * @return the input rescaled and to fit [-1, -deadband], [deadband, 1]
+     */
+    public static double deadband(double input, double deadband) {
+        if (Math.abs(input) <= deadband) {
+            return 0;
+        } else if (Math.abs(input) == 1) {
+            return input;
+        } else {
+            return (1 / (1 - deadband) * (input + Math.signum(-input) * deadband));
+        }
+    }
+
+    /**
+     * Deadbands an input to [-1, -OIConstants.DEADBAND], [OIConstants.DEADBAND, 1], rescaling inputs to be linear from
+     * (OIConstants.DEADBAND, 0) to (1,1)
+     *
+     * @param input The input value to rescale
+     * @return the input rescaled and to fit [-1, -DEADBAND], [DEADBAND, 1]
+     */
+    public static double deadband(double input) {
+        return deadband(input, Constants.DEFAULT_DEADBAND);
+    }
+
+    /**
+     * An exponential function that maintains positive or negative sign.
+     *
+     * @param exponent the power to raise the base to
+     * @param base     the base which will be raised to the power
+     * @return base to the power of exponent, maintaining sign of base
+     */
+    public static double expoMS(double base, double exponent) {
+        // weird stuff will happen if you don't put a number > 0 for controller inputs
+        double finVal = Math.pow(Math.abs(base), exponent);
+        if (base < 0) {
+            finVal *= -1;
+        }
+        return finVal;
+    }
+
+    /**
+     * Calculates Midpoint of two numbers on modulus number line
+     *
+     * @param num1       first number
+     * @param num2       second number
+     * @param lowerBound lower bound of modulus number line
+     * @param upperBound upper bound of modulus number line
+     * @return midpoint of 2 numbers on modulus number line
+     */
+    public static double modulusMidpoint(double num1, double num2, double lowerBound, double upperBound) {
+        num1 = MathUtil.inputModulus(num1, lowerBound, upperBound);
+        num2 = MathUtil.inputModulus(num2, lowerBound, upperBound);
+        if (Math.abs(num1 - num2) > (upperBound - lowerBound) / 2) {
+            return MathUtil.inputModulus((num1 + num2) / 2 + (upperBound - lowerBound) / 2, lowerBound, upperBound);
+        }
+        return (num1 + num2) / 2;
+    }
+
+    /**
+     * Interpolates between two numbers on modulus number line
+     *
+     * @param num1       first number
+     * @param num2       second number
+     * @param amount     the amount to interpolate, 0 = first number, 1 = second number
+     * @param lowerBound lower bound of modulus number line
+     * @param upperBound upper bound of modulus number line
+     * @return interpolated value between 2 numbers on modulus number line
+     */
+    public static double modulusInterpolate(double num1, double num2, double amount, double lowerBound, double upperBound) {
+        num1 = MathUtil.inputModulus(num1, lowerBound, upperBound);
+        num2 = MathUtil.inputModulus(num2, lowerBound, upperBound);
+        if (Math.abs(num1 - num2) > (upperBound - lowerBound) / 2) {
+            if(num1 < num2){
+                num1 += upperBound-lowerBound;
+            }else{
+                num2 += upperBound-lowerBound;
+            }
+        }
+        return MathUtil.inputModulus((1-amount)*num1 + amount*num2, lowerBound, upperBound);
+    }
+
+    /**
+     * Calls {@link #mean(double...)}.
+     *
+     * @param data the list of data to find the mean of
+     * @return the mean of the data
+     */
+    public static double mean(List<Double> data) {
+        return mean(doubleListToArray(data));
+    }
+
+    /**
+     * Finds the mean of the provided array of doubles
+     *
+     * @param data an array of doubles
+     * @return the mean of the data
+     */
+    public static double mean(double... data) {
+        double mean = 0;
+        for (double datum : data) {
+            mean += datum;
+        }
+        mean /= data.length;
+        return mean;
+    }
+
+    /**
+     * Calls {@link #stdDev(double...)}.
+     *
+     * @param data the list of data to find the standard deviation of
+     * @return the standard deviation of the data
+     */
+    public static double stdDev(List<Double> data) {
+        return stdDev(doubleListToArray(data));
+    }
+
+    /**
+     * Finds the standard deviation of the provided array of doubles
+     *
+     * @param data an array of doubles
+     * @return the standard deviation of the data
+     */
+    public static double stdDev(double... data) {
+        if (data.length == 0 || data.length == 1) return 0;
+
+        double mean = mean(data);
+
+        double total = 0;
+        for (double datum : data) {
+            total += Math.pow(datum - mean, 2);
+        }
+        return Math.sqrt(total / (data.length - 1));
+    }
+
+    private static double[] doubleListToArray(List<Double> arrayList) {
+        return arrayList.stream().mapToDouble(Double::doubleValue).toArray();
+    }
+
+}

diff --git a/src/main/java/frc/robot/util/MotorFactory.java b/src/main/java/frc/robot/util/MotorFactory.java
index 9df88b521e301cf829877c09200fa19167d5f334..e545d4da3091ec256c95dd72b5181098d9670f86 100644 (file)
--- a/src/main/java/frc/robot/util/MotorFactory.java
+++ b/src/main/java/frc/robot/util/MotorFactory.java
@@ -1,201 +1,163 @@
 package frc.robot.util;
 
-import java.io.IOException;
-
-import java.io.IOError;
-
-import com.ctre.phoenix.motorcontrol.FeedbackDevice;
-import com.ctre.phoenix.motorcontrol.NeutralMode;
-import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration;
-import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
-import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
-import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkMax.IdleMode;
-import com.revrobotics.CANSparkMaxLowLevel.MotorType;
-
-import edu.wpi.first.wpilibj.DriverStation;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.VoltageConfigs;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
 import frc.robot.constants.Constants;
 
+/**
+ * Utility class for easy creation of motor controllers.
+ */
 public class MotorFactory {
 
-  private static int talonSRXDefaultContinuousLimit = 38;
-  private static int kTalonSRXDefaultPeakLimit = 45;
-  private static int kTalonSRXDefaultPeakDuration = 125;
-
-  private static int kSparkMaxDefaultCurrentLimit = 60;
-
-  ///////////////////////////////////////////////////////////////////////////////////////////////
-  // TALON SRX
-  ///////////////////////////////////////////////////////////////////////////////////////////////
-
-  /**
-   * Create a TalonSRX with current limiting enabled, using parameters. It will
-   * have current
-   * limiting and voltage compensation enabled and be set to Brake Mode.
-   *
-   * @param id                     the ID of the TalonSRX
-   * @param continuousCurrentLimit the continuous current limit to set in amps (A)
-   * @param peakCurrentLimit       the peak current limit to set in amps (A)
-   * @param peakCurrentDuration    the peak current limit duration to set in
-   *                               milliseconds (ms)
-   * @return a fully configured TalonSRX object
-   */
-  public static WPI_TalonSRX createTalonSRX(
-      int id, int continuousCurrentLimit, int peakCurrentLimit, int peakCurrentDuration) {
-
-    TalonSRXConfiguration config = new TalonSRXConfiguration();
-    config.continuousCurrentLimit = continuousCurrentLimit;
-    config.peakCurrentLimit = peakCurrentLimit;
-    config.peakCurrentDuration = peakCurrentDuration;
-    config.voltageCompSaturation = Constants.kMaxVoltage;
-
-    WPI_TalonSRX talon = new WPI_TalonSRX(id);
-    talon.configFactoryDefault();
-    talon.configAllSettings(config);
-    talon.enableCurrentLimit(true);
-    talon.enableVoltageCompensation(true);
-    talon.setNeutralMode(NeutralMode.Brake);
-
-    return talon;
-  }
-
-  /**
-   * Create a TalonSRX using default current limits.
-   *
-   * @param id the ID of the TalonSRX
-   * @return a fully configured TalonSRX object
-   */
-  public static WPI_TalonSRX createTalonSRXDefault(int id) {
-    return createTalonSRX(id, talonSRXDefaultContinuousLimit, kTalonSRXDefaultPeakLimit, kTalonSRXDefaultPeakDuration);
-  }
-
-  ///////////////////////////////////////////////////////////////////////////////////////////////
-  // SPARK MAX
-  ///////////////////////////////////////////////////////////////////////////////////////////////
-
-  /**
-   * Create a CANSparkMax with current limiting enabled
-   *
-   * @param id         the ID of the Spark MAX
-   * @param motortype  the type of motor the Spark MAX is connected to
-   * @param stallLimit the current limit to set at stall
-   * @return a fully configured CANSparkMAX
-   */
-  public static CANSparkMax createSparkMAX(int id, MotorType motortype, int stallLimit) {
-    CANSparkMax sparkMAX = new CANSparkMax(id, motortype);
-    sparkMAX.restoreFactoryDefaults();
-    sparkMAX.enableVoltageCompensation(Constants.kMaxVoltage);
-    sparkMAX.setSmartCurrentLimit(stallLimit);
-    sparkMAX.setIdleMode(IdleMode.kBrake);
-
-    sparkMAX.burnFlash();
-    return sparkMAX;
-  }
-
-  /**
-   * Create a CANSparkMax with default current limiting enabled
-   *
-   * @param id        the ID of the Spark MAX
-   * @param motortype the type of motor the Spark MAX is connected to
-   * @return a fully configured CANSparkMAX
-   */
-  public static CANSparkMax createSparkMAXDefault(int id, MotorType motortype) {
-    return createSparkMAX(id, motortype, kSparkMaxDefaultCurrentLimit);
-  }
-
-  ///////////////////////////////////////////////////////////////////////////////////////////////
-  // TALON FX (Falcon 500)
-  ///////////////////////////////////////////////////////////////////////////////////////////////
-
-  /**
-   * 
-   * Creates a TalonFX with all current limit options. If you would like to use
-   * defaults it is recommended to use the other createTalonFX.. methods.
-   * 
-   * @param id                     the id of the the motor
-   * @param CANBus                 the CAN Bus the motor is on
-   * @param StatorLimitEnable      whether or not to enable stator limiting
-   * @param StatorCurrentLimit     the current, in amps, to return to after the
-   *                               stator limit is triggered
-   * @param StatorTriggerThreshold the threshold current to trigger the stator
-   *                               limit
-   * @param StatorTriggerDuration  the duration, in seconds, the current is above
-   *                               the threshold before triggering
-   * @param SupplyLimitEnable      whether or not to enable supply limiting
-   * @param SupplyCurrentLimit     the current, in amps, to return to after the
-   *                               supply limit is triggered
-   * @param SupplyTriggerThreshold the threshold current to trigger the supply
-   *                               limit
-   * @param SupplyTriggerDuration  the duration, in seconds, the current is above
-   *                               the threshold before triggering
-   * @return A fully configured TalonFX
-   */
-  public static WPI_TalonFX createTalonFXFull(int id, String CANBus, boolean StatorLimitEnable,
-      double StatorCurrentLimit,
-      double StatorTriggerThreshold, double StatorTriggerDuration, boolean SupplyLimitEnable, double SupplyCurrentLimit,
-      double SupplyTriggerThreshold, double SupplyTriggerDuration) {
-
-    if (id == -1) {
-      return null;
+    private static final int SPARK_MAX_DEFAULT_CURRENT_LIMIT = 60;
+
+    ///////////////////////////////////////////////////////////////////////////////////////////////
+    // SPARK MAX
+    ///////////////////////////////////////////////////////////////////////////////////////////////
+
+    /**
+     * Create a SparkMax with current limiting enabled
+     *
+     * @param id         the ID of the Spark MAX
+     * @param motortype  the type of motor the Spark MAX is connected to
+     * @param stallLimit the current limit to set at stall
+     * @return a fully configured CANSparkMAX
+     */
+    public static SparkMax createSparkMAX(int id, MotorType motortype, int stallLimit) {
+        SparkMax sparkMAX = new SparkMax(id, motortype);
+
+        sparkMAX.configure(new SparkMaxConfig()
+            .voltageCompensation(Constants.ROBOT_VOLTAGE)
+            .smartCurrentLimit(stallLimit)
+            .idleMode(IdleMode.kBrake),
+            ResetMode.kResetSafeParameters,
+            PersistMode.kNoPersistParameters
+        );
+        return sparkMAX;
+    }
+
+    /**
+     * Create a SparkMax with default current limiting enabled
+     *
+     * @param id        the ID of the Spark MAX
+     * @param motortype the type of motor the Spark MAX is connected to
+     * @return a fully configured CANSparkMAX
+     */
+    public static SparkMax createSparkMAXDefault(int id, MotorType motortype) {
+        return createSparkMAX(id, motortype, SPARK_MAX_DEFAULT_CURRENT_LIMIT);
+    }
+
+    ///////////////////////////////////////////////////////////////////////////////////////////////
+    // TALON FX (Falcon 500 and Kraken X60)
+    ///////////////////////////////////////////////////////////////////////////////////////////////
+
+    /**
+     * Creates a TalonFX with all current limit options. If you would like to use
+     * defaults it is recommended to use the other createTalonFX.. methods.
+     *
+     * @param id                     the CAN ID of the TalonFX
+     * @param CANBus                 the CAN bus the TalonFX is on. If connected to the rio it is "rio".
+     * @param StatorLimitEnable      whether to enable stator limiting
+     * @param StatorCurrentLimit     the current, in amps, to return to after the
+     *                               stator limit is triggered
+     * @param StatorTriggerThreshold the threshold current to trigger the stator
+     *                               limit
+     * @param StatorTriggerDuration  the duration, in seconds, the current is above
+     *                               the threshold before triggering
+     * @param SupplyLimitEnable      whether to enable supply limiting
+     * @param SupplyCurrentLimit     the current, in amps, to return to after the
+     *                               supply limit is triggered
+     * @param SupplyTriggerThreshold the threshold current to trigger the supply
+     *                               limit
+     * @param SupplyTriggerDuration  the duration, in seconds, the current is above
+     *                               the threshold before triggering
+     * @return A fully configured TalonFX
+     */
+    public static TalonFX createTalonFXFull(int id, String CANBus, boolean StatorLimitEnable,
+                                                double StatorCurrentLimit,
+                                                double StatorTriggerThreshold, double StatorTriggerDuration, boolean SupplyLimitEnable, double SupplyCurrentLimit,
+                                                double SupplyTriggerThreshold, double SupplyTriggerDuration) {
+
+        if (id == -1) {
+            return null;
+        }
+
+        TalonFX talon = new TalonFX(id, CANBus);
+
+        TalonFXConfiguration config = new TalonFXConfiguration();
+
+        // See explanations for Supply and Stator limiting in FalconConstants.java
+        config.CurrentLimits = new CurrentLimitsConfigs().withStatorCurrentLimitEnable(StatorLimitEnable).withStatorCurrentLimit(StatorCurrentLimit).
+            withSupplyCurrentLimitEnable(SupplyLimitEnable).withSupplyCurrentLimit(SupplyCurrentLimit).
+            withSupplyCurrentLowerLimit(SupplyTriggerThreshold).withSupplyCurrentLowerTime(SupplyTriggerDuration);
+
+        config.Voltage = new VoltageConfigs().withPeakForwardVoltage(Constants.ROBOT_VOLTAGE);
+
+        talon.getConfigurator().apply(config);
+        talon.setNeutralMode(NeutralModeValue.Brake);
+        
+
+        return talon;
+    }
+
+    /**
+     * Creates a TalonFX with all the default settings.
+     *
+     * @param id     the id of the motor
+     * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio".
+     */
+    public static TalonFX createTalonFX(int id, String CANBus) {
+        return createTalonFXFull(id, CANBus, Constants.TALONFX_STATOR_LIMIT_ENABLE, Constants.TALONFX_STATOR_CURRENT_LIMIT,
+                                 Constants.TALONFX_STATOR_TRIGGER_THRESHOLD, Constants.TALONFX_STATOR_TRIGGER_DURATION,
+                                 Constants.TALONFX_SUPPLY_LIMIT_ENABLE, Constants.TALONFX_SUPPLY_CURRENT_LIMIT,
+                                 Constants.TALONFX_SUPPLY_TRIGGER_THRESHOLD, Constants.TALONFX_SUPPLY_TRIGGER_DURATION);
     }
 
-    WPI_TalonFX talon = new WPI_TalonFX(id, CANBus);
-
-    if (talon.getFirmwareVersion() != Constants.falcon.kFirmwareVersion) {
-      String errorMessage = "TalonFX " + id + " firmware incorrect. Has " + talon.getFirmwareVersion()
-          + ", currently FalconConstants.java requires: " + Constants.falcon.kFirmwareVersion;
-      if (Constants.falcon.kBreakOnWrongFirmware) {
-        DriverStation.reportError(errorMessage, true);
-        throw new IOError(new IOException(errorMessage));
-      } else {
-        DriverStation.reportWarning(errorMessage + ", ignoring due to user specification.", false);
-      }
+    /**
+     * Creates a TalonFX with supply current limit options.
+     * <p>
+     * Supply current is current that's being drawn at the input bus voltage.
+     * Supply limiting is useful for preventing breakers from tripping in the PDP.
+     *
+     * @param id               the CAN ID of the TalonFX
+     * @param CANBus           the CAN bus the TalonFX is on. If connected to the rio it is "rio".
+     * @param currentLimit     the current, in amps, to return to after the supply limit is triggered
+     * @param triggerThreshold the threshold current to trigger the supply limit
+     * @param triggerDuration  the duration, in seconds, the current is above the threshold before triggering
+     */
+    public static TalonFX createTalonFXSupplyLimit(int id, String CANBus, double currentLimit,
+                                                       double triggerThreshold, double triggerDuration) {
+        return createTalonFXFull(id, CANBus, Constants.TALONFX_STATOR_LIMIT_ENABLE, Constants.TALONFX_STATOR_CURRENT_LIMIT,
+                                 Constants.TALONFX_STATOR_TRIGGER_THRESHOLD, Constants.TALONFX_STATOR_TRIGGER_DURATION, true, currentLimit,
+                                 triggerThreshold, triggerDuration);
     }
 
-    TalonFXConfiguration config = new TalonFXConfiguration();
-
-    // See explanations for Supply and Stator limiting in FalconConstants.java
-    config.statorCurrLimit = new StatorCurrentLimitConfiguration(StatorLimitEnable, StatorCurrentLimit,
-        StatorTriggerThreshold, StatorTriggerDuration);
-    config.supplyCurrLimit = new SupplyCurrentLimitConfiguration(SupplyLimitEnable, SupplyCurrentLimit,
-        SupplyTriggerThreshold, SupplyTriggerDuration);
-
-    config.voltageCompSaturation = Constants.kMaxVoltage;
-
-    talon.configFactoryDefault();
-    talon.configAllSettings(config);
-    talon.enableVoltageCompensation(true);
-    talon.setNeutralMode(NeutralMode.Brake);
-    talon.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor);
-
-    return talon;
-  }
-
-  /**
-   * @param id
-   * @param CANBus
-   */
-  public static WPI_TalonFX createTalonFX(int id, String CANBus) {
-    return createTalonFXFull(id, CANBus, Constants.falcon.kStatorLimitEnable, Constants.falcon.kStatorCurrentLimit,
-        Constants.falcon.kStatorTriggerThreshold, Constants.falcon.kStatorTriggerDuration,
-        Constants.falcon.kSupplyLimitEnable, Constants.falcon.kSupplyCurrentLimit,
-        Constants.falcon.kSupplyTriggerThreshold, Constants.falcon.kSupplyTriggerDuration);
-  }
-
-  public static WPI_TalonFX createTalonFXSupplyLimit(int id, String CANBus, double currentLimit,
-      double triggerThreshold, double triggerDuration) {
-    return createTalonFXFull(id, CANBus, Constants.falcon.kStatorLimitEnable, Constants.falcon.kStatorCurrentLimit,
-        Constants.falcon.kStatorTriggerThreshold, Constants.falcon.kStatorTriggerDuration, true, currentLimit,
-        triggerThreshold, triggerDuration);
-  }
-
-  public static WPI_TalonFX createTalonFXStatorLimit(int id, String CANBus, double currentLimit,
-      double triggerThreshold, double triggerDuration) {
-    return createTalonFXFull(id, CANBus, true, currentLimit, triggerThreshold, triggerDuration,
-        Constants.falcon.kSupplyLimitEnable, Constants.falcon.kSupplyCurrentLimit,
-        Constants.falcon.kSupplyTriggerThreshold, Constants.falcon.kSupplyTriggerDuration);
-  }
-}
+    /**
+     * Creates a TalonFX with stator current limit options.
+     * <p>
+     * Stator current is current that’s being drawn by the motor.
+     * Stator limiting is useful for limiting acceleration/heat.
+     *
+     * @param id               the CAN ID of the TalonFX
+     * @param CANBus           the CAN bus the TalonFX is on. If connected to the rio it is "rio".
+     * @param currentLimit     the current, in amps, to return to after the stator limit is triggered
+     * @param triggerThreshold the threshold current to trigger the stator limit
+     * @param triggerDuration  the duration, in seconds, the current is above the threshold before triggering
+     */
+    public static TalonFX createTalonFXStatorLimit(int id, String CANBus, double currentLimit,
+                                                       double triggerThreshold, double triggerDuration) {
+        return createTalonFXFull(id, CANBus, true, currentLimit, triggerThreshold, triggerDuration,
+                                 Constants.TALONFX_SUPPLY_LIMIT_ENABLE, Constants.TALONFX_SUPPLY_CURRENT_LIMIT,
+                                 Constants.TALONFX_SUPPLY_TRIGGER_THRESHOLD, Constants.TALONFX_SUPPLY_TRIGGER_DURATION);
+    }
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/util/PathGroupLoader.java b/src/main/java/frc/robot/util/PathGroupLoader.java
index 084bd85abc7c05637672151b1cf137c2132b4c8f..b1db28c200738e7861b072c581425dafd38792f3 100644 (file)
--- a/src/main/java/frc/robot/util/PathGroupLoader.java
+++ b/src/main/java/frc/robot/util/PathGroupLoader.java
@@ -1,49 +1,70 @@
 package frc.robot.util;
 
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Filesystem;
+import frc.robot.constants.AutoConstants;
+
 import java.io.File;
-import java.util.ArrayList;
 import java.util.HashMap;
+import com.pathplanner.lib.path.PathPlannerPath;
 
-import com.pathplanner.lib.PathConstraints;
-import com.pathplanner.lib.PathPlanner;
-import com.pathplanner.lib.PathPlannerTrajectory;
+/**
+ * Utility class for loading paths using pathplanner.
+ */
+public class PathGroupLoader {
 
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.Filesystem;
-import frc.robot.constants.Constants;
-
-public class PathGroupLoader{
-
-  private static HashMap<String, ArrayList<PathPlannerTrajectory>> pathGroups = new HashMap<String, ArrayList<PathPlannerTrajectory>>();
-
-  public static void loadPathGroups() {
-    double totalTime = 0;
-    File[] directoryListing = Filesystem.getDeployDirectory().toPath().resolve(Constants.auto.kTrajectoryDirectory).toFile().listFiles();
-    if (directoryListing != null) {
-      for (File file : directoryListing) {
-        if (file.isFile() && file.getName().indexOf(".") != -1) {
-          long startTime = System.nanoTime();
-          String name = file.getName().substring(0, file.getName().indexOf("."));
-          pathGroups.put(name, PathPlanner.loadPathGroup(name, new PathConstraints(Constants.auto.kMaxAutoSpeed, Constants.auto.kMaxAutoAccel)));
-          double time = (System.nanoTime() - startTime) / 1000000.0;
-          totalTime += time;
-          System.out.println("Processed file: " + file.getName() + ", took " + time + " milliseconds.");
+    // private static final HashMap<String, List<PathPlannerTrajectory>> pathGroups = new HashMap<>();
+    private static final HashMap<String, PathPlannerPath> pathGroups = new HashMap<>();
+
+
+    /**
+     * Loads all the paths in the trajectory directory (specified in the constants).
+     * These paths are loaded and stored so that they do not take time while the robot is running
+     * and can be accessed with {@link #getPathGroup(String)}
+     */
+    public static void loadPathGroups() {
+        double totalTime = 0;
+        File[] directoryListing = Filesystem.getDeployDirectory().toPath().resolve(AutoConstants.TRAJECTORY_DIRECTORY).toFile().listFiles();
+        
+        if (directoryListing != null) {
+            for (File file : directoryListing) {
+                if (file.isFile() && file.getName().contains(".")) {
+                    try {
+                        long startTime = System.nanoTime();
+                        String name = file.getName().substring(0, file.getName().indexOf("."));
+                        // pathGroups.put(name, PathPlannerAuto.getPathGroupFromAutoFile(name));
+                        pathGroups.put(name, PathPlannerPath.fromPathFile(name));
+                        double time = (System.nanoTime() - startTime) / 1000000.0;
+                        totalTime += time;
+                        System.out.println("Processed file: " + file.getName() + ", took " + time + " milliseconds.");
+                    } catch (Exception e) {
+                        DriverStation.reportError(e.getMessage(), true);
+                    }
+                }
+            }
+        } else {
+            System.out.println("Error processing file");
+            DriverStation.reportWarning(
+                    "Issue with finding path files. Paths will not be loaded.",
+                    true
+                                       );
         }
-      }
-    } else {
-      System.out.println("Error processing file");
-      DriverStation.reportWarning(
-        "Issue with finding path files. Paths will not be loaded.",
-        true
-      );
+        System.out.println("File processing took a total of " + totalTime + " milliseconds");
     }
-    System.out.println("File processing took a total of " + totalTime + " milliseconds");
-  }
 
-  public static ArrayList<PathPlannerTrajectory> getPathGroup(String pathGroupName) {
-    if (pathGroups.get(pathGroupName) == null) {
-      System.out.println("Error retrieving " + pathGroupName + " path!");
+    /**
+     * Gets a path that has already been loaded with {@link #loadPathGroups()}. The path group is a list
+     * of trajectories that path planner can run.
+     *
+     * @param pathGroupName the name of the file, without any extensions. This should be the same exact name that is displayed in pathplanner
+     * @return a list of trajectories that path planner can run.
+     */
+    public static PathPlannerPath getPathGroup(String pathGroupName) {
+        if (pathGroups.get(pathGroupName) == null) {
+            System.out.println("Error retrieving " + pathGroupName + " path!");
+        }
+        return pathGroups.get(pathGroupName);
     }
-    return pathGroups.get(pathGroupName);
-  }
+
+
 }
\ No newline at end of file

diff --git a/src/main/java/frc/robot/util/PhoenixOdometryThread.java b/src/main/java/frc/robot/util/PhoenixOdometryThread.java
new file mode 100644 (file)
index 0000000..6ae5d60
--- /dev/null
+++ b/src/main/java/frc/robot/util/PhoenixOdometryThread.java
@@ -0,0 +1,154 @@
+// Copyright 2021-2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.util;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj.RobotController;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+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.
+ *
+ * <p>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 Lock signalsLock =
+      new ReentrantLock(); // Prevents conflicts when registering signals
+  private BaseStatusSignal[] phoenixSignals = new BaseStatusSignal[0];
+  private final List<DoubleSupplier> genericSignals = new ArrayList<>();
+  private final List<Queue<Double>> phoenixQueues = new ArrayList<>();
+  private final List<Queue<Double>> genericQueues = new ArrayList<>();
+  private final List<Queue<Double>> timestampQueues = new ArrayList<>();
+
+  private static PhoenixOdometryThread instance = null;
+
+  public static PhoenixOdometryThread getInstance() {
+    if (instance == null) {
+      instance = new PhoenixOdometryThread();
+    }
+    return instance;
+  }
+
+  private PhoenixOdometryThread() {
+    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<Double> registerSignal(StatusSignal<Angle> signal) {
+    Queue<Double> queue = new ArrayBlockingQueue<>(20);
+    signalsLock.lock();
+    Drivetrain.odometryLock.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();
+      Drivetrain.odometryLock.unlock();
+    }
+    return queue;
+  }
+
+  /** Registers a generic signal to be read from the thread. */
+  public Queue<Double> registerSignal(DoubleSupplier signal) {
+    Queue<Double> queue = new ArrayBlockingQueue<>(20);
+    signalsLock.lock();
+    Drivetrain.odometryLock.lock();
+    try {
+      genericSignals.add(signal);
+      genericQueues.add(queue);
+    } finally {
+      signalsLock.unlock();
+      Drivetrain.odometryLock.unlock();
+    }
+    return queue;
+  }
+
+  /** Returns a new queue that returns timestamp values for each sample. */
+  public Queue<Double> makeTimestampQueue() {
+    Queue<Double> queue = new ArrayBlockingQueue<>(20);
+    Drivetrain.odometryLock.lock();
+    try {
+      timestampQueues.add(queue);
+    } finally {
+      Drivetrain.odometryLock.unlock();
+    }
+    return queue;
+  }
+
+  @Override
+  public void run() {
+    while (true) {
+      // Wait for updates from all signals
+      signalsLock.lock();
+      try { 
+          BaseStatusSignal.waitForAll(2.0 / 250, phoenixSignals);}
+      finally {
+        signalsLock.unlock();
+      }
+
+      // Save new data to queues
+      Drivetrain.odometryLock.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 {
+        Drivetrain.odometryLock.unlock();
+      }
+    }
+  }
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/util/PhoenixUtil.java b/src/main/java/frc/robot/util/PhoenixUtil.java
new file mode 100644 (file)
index 0000000..8f4f6dd
--- /dev/null
+++ b/src/main/java/frc/robot/util/PhoenixUtil.java
@@ -0,0 +1,44 @@
+package frc.robot.util;
+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<StatusCode> 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/src/main/java/frc/robot/util/ShuffleboardManager.java b/src/main/java/frc/robot/util/ShuffleboardManager.java
deleted file mode 100644 (file)
index e8dc64f..0000000
--- a/src/main/java/frc/robot/util/ShuffleboardManager.java
+++ /dev/null
@@ -1,70 +0,0 @@
-package frc.robot.util;
-
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj.livewindow.LiveWindow;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.PrintCommand;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.Robot;
-
-public class ShuffleboardManager {
-
-  SendableChooser<Command> m_autoCommand = new SendableChooser<>();
-  SendableChooser<TestType> m_testType = new SendableChooser<>();
-
-  ShuffleboardTab m_mainTab = Shuffleboard.getTab("Main");
-  ShuffleboardTab m_autoTab = Shuffleboard.getTab("Auto");
-  ShuffleboardTab m_testTab = Shuffleboard.getTab("Test");
-
-  NetworkTableEntry m_commandScheduler = m_mainTab.add("Command Scheduler", "NULL").getEntry();
-
-  public void setup() {
-    LiveWindow.disableAllTelemetry(); // LiveWindow is causing periodic loop overruns
-
-    autoChooserUpdate();
-
-    m_autoTab.add("Auto Chooser", m_autoCommand);
-    m_testTab.add("Run Test", m_testType);
-  }
-
-  public Command getAutonomousCommand() {
-    return m_autoCommand.getSelected();
-  }
-
-  public void autoChooserUpdate() {
-    m_autoCommand.addOption("Do Nothing", new PrintCommand("This will do nothing!"));
-  }
-
-
-  public TestType getTestType() {
-    if (Robot.isTestMode()) {
-      return m_testType.getSelected();
-    }
-    return TestType.NONE;
-  }
-
-  public Trigger isTestTypeTrigger(TestType testType) {
-    return new Trigger(() -> m_testType.getSelected() == testType);
-  }
-
-  public void testChooserUpdate() {
-    m_testType.addOption("No Test", TestType.NONE);
-  }
-
-  public void loadCommandSchedulerShuffleboard() {
-    CommandScheduler.getInstance()
-        .onCommandInitialize(
-            command -> m_commandScheduler.setString(command.getName() + " initialized."));
-    CommandScheduler.getInstance()
-        .onCommandInterrupt(
-            command -> m_commandScheduler.setString(command.getName() + " interrupted."));
-    CommandScheduler.getInstance()
-        .onCommandFinish(command -> m_commandScheduler.setString(command.getName() + " finished."));
-  }
-}

diff --git a/src/main/java/frc/robot/util/SwerveModulePose.java b/src/main/java/frc/robot/util/SwerveModulePose.java
new file mode 100644 (file)
index 0000000..7f138da
--- /dev/null
+++ b/src/main/java/frc/robot/util/SwerveModulePose.java
@@ -0,0 +1,130 @@
+// 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 frc.robot.util;
+
+import java.util.Arrays;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/** 
+ * Stores and updates the position of each module
+ */
+public class SwerveModulePose {
+
+    private double[] dist = {0,0,0,0};
+    private Translation2d[] moduleTranslations;
+    private Pose2d[] modulePositions;
+    private double[] angles;
+    private Drivetrain drive;
+    private double prevRotation;
+    private Pose2d[] displayPoses;
+
+    /**
+     * Creates a new SwerveModulePose object to store and update the positions of each module
+     * @param drive The drivetrain
+     * @param modulePositions The translations of the modules relative to the center of the robot
+     */
+    public SwerveModulePose(Drivetrain drive, Translation2d... modulePositions){
+        this.drive = drive;
+        this.moduleTranslations = modulePositions;
+        this.modulePositions = new Pose2d[4];
+        angles = new double[4];
+        reset();
+        update();
+        reset();
+    }
+
+    /**
+     * Updates the module positions
+     */
+    public void update(){
+        SwerveModuleState[] states = drive.getModuleStates();
+        double currentRotation = drive.getYaw().getRadians();
+        double chassisRotation = currentRotation - prevRotation;
+
+        for(int i = 0; i<4; i++){
+            double position = drive.getModules()[i].getPosition().distanceMeters;
+            double distance = position - dist[i];
+            dist[i] = position;
+            
+            Twist2d twist = new Twist2d(distance, 0, MathUtil.angleModulus(states[i].angle.getRadians()-angles[i] + chassisRotation));
+            angles[i] = states[i].angle.getRadians();
+            modulePositions[i] = modulePositions[i].exp(twist);
+
+            displayPoses[i] = new Pose2d(
+                modulePositions[i].getTranslation(),
+                EqualsUtil.epsilonEquals(states[i].speedMetersPerSecond, 0, 0.01) ? displayPoses[i].getRotation() :
+                    states[i].speedMetersPerSecond < 0 ? modulePositions[i].getRotation().plus(new Rotation2d(Math.PI)) :
+                    modulePositions[i].getRotation()
+            );
+        }
+        prevRotation = currentRotation;
+    }
+
+    /**
+     * Gets the positions of the modules
+     * @return The module poses as an array of Pose2ds
+     */
+    public Pose2d[] getModulePoses(){
+        return displayPoses;
+    }
+
+    /**
+     * Resets the modules to the correct positions relative to the robot
+     */
+    public void reset(){
+        Pose2d chassisPose2d = drive.getPose();
+        SwerveModuleState[] states = drive.getModuleStates();
+        for (int i = 0; i<4; i++){
+            angles[i] = states[i].angle.getRadians();
+            this.modulePositions[i] = new Pose2d(moduleTranslations[i].rotateBy(chassisPose2d.getRotation()).plus(chassisPose2d.getTranslation()), new Rotation2d(angles[i]).plus(chassisPose2d.getRotation()));
+        }
+        prevRotation = drive.getYaw().getRadians();
+        displayPoses = Arrays.copyOf(modulePositions, 4);
+    }
+
+    /**
+     * Gets whehter or not the modules have slipped
+     * A module has slipped if it has moved 0.3m (about 1ft) from its correct position relative to the other modules
+     * @return True if any of the modules have slipped, false otherwise
+     */
+    public boolean slipped(){
+        Translation2d total = new Translation2d();
+        for(Pose2d pose : modulePositions){
+            total = total.plus(pose.getTranslation());
+        }
+        Pose2d drivePose = new Pose2d(total.div(4), drive.getYaw());
+        for(int i = 0; i < 4; i++){
+            double dist = modulePositions[i].relativeTo(drivePose).getTranslation().getDistance(moduleTranslations[i]);
+            if(dist > 0.3){
+                return true;
+            }
+        }
+        return false;
+    }
+
+    /**
+     * Gets the average slip distance
+     * @return The average distance between each module and its correct position
+     */
+    public double getAverageSlip(){
+        Translation2d total = new Translation2d();
+        for(Pose2d pose : modulePositions){
+            total = total.plus(pose.getTranslation());
+        }
+        Pose2d drivePose = new Pose2d(total.div(4), drive.getYaw());
+        double slip = 0;
+        for(int i = 0; i < 4; i++){
+            slip += modulePositions[i].relativeTo(drivePose).getTranslation().getDistance(moduleTranslations[i]);
+        }
+        return slip/4;
+    }
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/util/SwerveStuff/ModuleLimits.java b/src/main/java/frc/robot/util/SwerveStuff/ModuleLimits.java
new file mode 100644 (file)
index 0000000..e8ee61f
--- /dev/null
+++ b/src/main/java/frc/robot/util/SwerveStuff/ModuleLimits.java
@@ -0,0 +1,11 @@
+// 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 frc.robot.util.SwerveStuff;
+
+public record ModuleLimits(
+    double maxDriveVelocity, double maxDriveAcceleration, double staticFriction, double maxSteeringVelocity) {}

diff --git a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java
new file mode 100644 (file)
index 0000000..374c7de
--- /dev/null
+++ b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java
@@ -0,0 +1,13 @@
+// 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 frc.robot.util.SwerveStuff;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+
+public record SwerveSetpoint(ChassisSpeeds chassisSpeeds, SwerveModuleState[] moduleStates) {}

diff --git a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java
new file mode 100644 (file)
index 0000000..2c7dd20
--- /dev/null
+++ b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java
@@ -0,0 +1,489 @@
+// 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 frc.robot.util.SwerveStuff;
+
+import static frc.robot.util.EqualsUtil.*;
+
+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.SwerveModuleState;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Optional;
+
+import frc.robot.constants.Constants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.util.EqualsUtil;
+import frc.robot.util.GeomUtil;
+
+/**
+ * "Inspired" by FRC team 254. See the license file in the root directory of this project.
+ *
+ * <p>Takes a prior setpoint (ChassisSpeeds), a desired setpoint (from a driver, or from a path
+ * follower), and outputs a new setpoint that respects all of the kinematic constraints on module
+ * rotation speed and wheel velocity/acceleration. By generating a new setpoint every iteration, the
+ * robot will converge to the desired setpoint quickly while avoiding any intermediate state that is
+ * kinematically infeasible (and can result in wheel slip or robot heading drift as a result).
+ */
+
+public class SwerveSetpointGenerator {
+  private final SwerveDriveKinematics kinematics= DriveConstants.KINEMATICS;
+  private final Translation2d[] moduleLocations = DriveConstants.MODULE_LOCATIONS;
+
+  /**
+   * Check if it would be faster to go to the opposite of the goal heading (and reverse drive
+   * direction).
+   *
+   * @param prevToGoal The rotation from the previous state to the goal state (i.e.
+   *     prev.inverse().rotateBy(goal)).
+   * @return True if the shortest path to achieve this rotation involves flipping the drive
+   *     direction.
+   */
+  private boolean flipHeading(Rotation2d prevToGoal) {
+    return Math.abs(prevToGoal.getRadians()) > Math.PI / 2.0;
+  }
+
+  private double unwrapAngle(double ref, double angle) {
+    double diff = angle - ref;
+    if (diff > Math.PI) {
+      return angle - 2.0 * Math.PI;
+    } else if (diff < -Math.PI) {
+      return angle + 2.0 * Math.PI;
+    } else {
+      return angle;
+    }
+  }
+
+  @FunctionalInterface
+  private interface Function2d {
+    double f(double x, double y);
+  }
+
+  /**
+   * Find the root of the generic 2D parametric function 'func' using the regula falsi technique.
+   * This is a pretty naive way to do root finding, but it's usually faster than simple bisection
+   * while being robust in ways that e.g. the Newton-Raphson method isn't.
+   *
+   * @param func The Function2d to take the root of.
+   * @param x_0 x value of the lower bracket.
+   * @param y_0 y value of the lower bracket.
+   * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during
+   *     recursion)
+   * @param x_1 x value of the upper bracket.
+   * @param y_1 y value of the upper bracket.
+   * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during
+   *     recursion)
+   * @param iterations_left Number of iterations of root finding left.
+   * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the
+   *     (approximate) root.
+   */
+  private double findRoot(
+      Function2d func,
+      double x_0,
+      double y_0,
+      double f_0,
+      double x_1,
+      double y_1,
+      double f_1,
+      int iterations_left) {
+    if (iterations_left < 0 || epsilonEquals(f_0, f_1)) {
+      return 1.0;
+    }
+    var s_guess = Math.max(0.0, Math.min(1.0, -f_0 / (f_1 - f_0)));
+    var x_guess = (x_1 - x_0) * s_guess + x_0;
+    var y_guess = (y_1 - y_0) * s_guess + y_0;
+    var f_guess = func.f(x_guess, y_guess);
+    if (Math.signum(f_0) == Math.signum(f_guess)) {
+      // 0 and guess on same side of root, so use upper bracket.
+      return s_guess
+          + (1.0 - s_guess)
+              * findRoot(func, x_guess, y_guess, f_guess, x_1, y_1, f_1, iterations_left - 1);
+    } else {
+      // Use lower bracket.
+      return s_guess
+          * findRoot(func, x_0, y_0, f_0, x_guess, y_guess, f_guess, iterations_left - 1);
+    }
+  }
+
+  protected double findSteeringMaxS(
+      double x_0,
+      double y_0,
+      double f_0,
+      double x_1,
+      double y_1,
+      double f_1,
+      double max_deviation,
+      int max_iterations) {
+    f_1 = unwrapAngle(f_0, f_1);
+    double diff = f_1 - f_0;
+    if (Math.abs(diff) <= max_deviation) {
+      // Can go all the way to s=1.
+      return 1.0;
+    }
+    double offset = f_0 + Math.signum(diff) * max_deviation;
+    Function2d func =
+        (x, y) -> {
+          return unwrapAngle(f_0, Math.atan2(y, x)) - offset;
+        };
+    return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations);
+  }
+
+  protected double findDriveMaxS_254version(
+      double x_0,
+      double y_0,
+      double f_0,
+      double x_1,
+      double y_1,
+      double f_1,
+      double max_vel_step,
+      int max_iterations) {
+    double diff = f_1 - f_0;
+    if (Math.abs(diff) <= max_vel_step) {
+      // Can go all the way to s=1.
+      return 1.0;
+    }
+    double offset = f_0 + Math.signum(diff) * max_vel_step;
+    Function2d func =
+        (x, y) -> {
+          return Math.hypot(x, y) - offset;
+        };
+    return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations);
+  }
+
+  /**
+   * Limits the acceleration in all directions.
+   * This is different from findDriveMaxS because it includes the acceleration perpendicular to the wheel as it rotates.
+   * Given the same velocity step, this will return a lower S value than findDriveMaxS.
+   * @param x_0 The initial x velocity
+   * @param y_0 The initial y velocity
+   * @param x_1 The final x velocity
+   * @param y_1 The final y velocity
+   * @param max_vel_step The maxiumum amount the velocity can change this frame
+   * @param max_iterations The maximum number of iterations to use in findRoot
+   * @return The maximum interpolation value
+   */
+  protected double findAccelerationMaxS(
+      double x_0,
+      double y_0,
+      double x_1,
+      double y_1,
+      double max_vel_step,
+      int max_iterations) {
+    double dist = Math.hypot(x_1-x_0, y_1-y_0);
+    if(dist <= max_vel_step){
+      return 1;
+    }
+    return Math.max(0.0, Math.min(1.0, max_vel_step / dist));
+  }
+
+  protected double findDriveMaxS(
+      double x_0, double y_0, double x_1, double y_1, double max_vel_step) {
+    // Derivation:
+    // Want to find point P(s) between (x_0, y_0) and (x_1, y_1) where the
+    // length of P(s) is the target T. P(s) is linearly interpolated between the
+    // points, so P(s) = (x_0 + (x_1 - x_0) * s, y_0 + (y_1 - y_0) * s).
+    // Then,
+    //     T = sqrt(P(s).x^2 + P(s).y^2)
+    //   T^2 = (x_0 + (x_1 - x_0) * s)^2 + (y_0 + (y_1 - y_0) * s)^2
+    //   T^2 = x_0^2 + 2x_0(x_1-x_0)s + (x_1-x_0)^2*s^2
+    //       + y_0^2 + 2y_0(y_1-y_0)s + (y_1-y_0)^2*s^2
+    //   T^2 = x_0^2 + 2x_0x_1s - 2x_0^2*s + x_1^2*s^2 - 2x_0x_1s^2 + x_0^2*s^2
+    //       + y_0^2 + 2y_0y_1s - 2y_0^2*s + y_1^2*s^2 - 2y_0y_1s^2 + y_0^2*s^2
+    //     0 = (x_0^2 + y_0^2 + x_1^2 + y_1^2 - 2x_0x_1 - 2y_0y_1)s^2
+    //       + (2x_0x_1 + 2y_0y_1 - 2x_0^2 - 2y_0^2)s
+    //       + (x_0^2 + y_0^2 - T^2).
+    //
+    // To simplify, we can factor out some common parts:
+    // Let l_0 = x_0^2 + y_0^2, l_1 = x_1^2 + y_1^2, and
+    // p = x_0 * x_1 + y_0 * y_1.
+    // Then we have
+    //   0 = (l_0 + l_1 - 2p)s^2 + 2(p - l_0)s + (l_0 - T^2),
+    // with which we can solve for s using the quadratic formula.
+
+    double l_0 = x_0 * x_0 + y_0 * y_0;
+    double l_1 = x_1 * x_1 + y_1 * y_1;
+    double sqrt_l_0 = Math.sqrt(l_0);
+    double diff = Math.sqrt(l_1) - sqrt_l_0;
+    if (Math.abs(diff) <= max_vel_step) {
+      // Can go all the way to s=1.
+      return 1.0;
+    }
+
+    double target = sqrt_l_0 + Math.copySign(max_vel_step, diff);
+    double p = x_0 * x_1 + y_0 * y_1;
+
+    // Quadratic of s
+    double a = l_0 + l_1 - 2 * p;
+    double b = 2 * (p - l_0);
+    double c = l_0 - target * target;
+    double root = Math.sqrt(b * b - 4 * a * c);
+
+    // Check if either of the solutions are valid
+    // Won't divide by zero because it is only possible for a to be zero if the
+    // target velocity is exactly the same or the reverse of the current
+    // velocity, which would be caught by the difference check.
+    double s_1 = (-b + root) / (2 * a);
+    if (isValidS(s_1)) {
+      return s_1;
+    }
+    double s_2 = (-b - root) / (2 * a);
+    if (isValidS(s_2)) {
+      return s_2;
+    }
+
+    // Since we passed the initial max_vel_step check, a solution should exist,
+    // but if no solution was found anyway, just don't limit movement
+    return 1.0;
+  }
+
+  protected static boolean isValidS(double s) {
+    return Double.isFinite(s) && s >= 0 && s <= 1;
+  }
+
+  /**
+   * Generate a new setpoint.
+   *
+   * @param limits The kinematic limits to respect for this setpoint.
+   * @param centerOfMassHeight The height of the robot's center of mass, in meters, off the ground.
+   *     This assumes that the center of mass is in the center of the robot in the x and y directions.
+   *     If tipping is not a potential problem this year, set this to 0.
+   * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous
+   *     iteration setpoint instead of the actual measured/estimated kinematic state.
+   * @param desiredState The desired state of motion, such as from the driver sticks or a path
+   *     following algorithm.
+   * @param dt The loop time.
+   * @return A Setpoint object that satisfies all of the KinematicLimits while converging to
+   *     desiredState quickly.
+   */
+  public SwerveSetpoint generateSetpoint(
+      final ModuleLimits limits,
+      double centerOfMassHeight,
+      final SwerveSetpoint prevSetpoint,
+      ChassisSpeeds desiredState,
+      double dt) {
+    final Translation2d[] modules = moduleLocations;
+
+    SwerveModuleState[] desiredModuleState = kinematics.toSwerveModuleStates(desiredState);
+    // Make sure desiredState respects velocity limits.
+    if (limits.maxDriveVelocity() > 0.0) {
+      SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleState, limits.maxDriveVelocity());
+      desiredState = kinematics.toChassisSpeeds(desiredModuleState);
+    }
+
+    // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so
+    // just use the previous angle.
+    boolean need_to_steer = true;
+    if (EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(desiredState),new Twist2d())) {
+      need_to_steer = false;
+      for (int i = 0; i < modules.length; ++i) {
+        desiredModuleState[i].angle = prevSetpoint.moduleStates()[i].angle;
+        desiredModuleState[i].speedMetersPerSecond = 0.0;
+      }
+    }
+
+    // For each module, compute local Vx and Vy vectors.
+    double[] prev_vx = new double[modules.length];
+    double[] prev_vy = new double[modules.length];
+    Rotation2d[] prev_heading = new Rotation2d[modules.length];
+    double[] desired_vx = new double[modules.length];
+    double[] desired_vy = new double[modules.length];
+    Rotation2d[] desired_heading = new Rotation2d[modules.length];
+    boolean all_modules_should_flip = true;
+    for (int i = 0; i < modules.length; ++i) {
+      prev_vx[i] =
+          prevSetpoint.moduleStates()[i].angle.getCos()
+              * prevSetpoint.moduleStates()[i].speedMetersPerSecond;
+      prev_vy[i] =
+          prevSetpoint.moduleStates()[i].angle.getSin()
+              * prevSetpoint.moduleStates()[i].speedMetersPerSecond;
+      prev_heading[i] = prevSetpoint.moduleStates()[i].angle;
+      if (prevSetpoint.moduleStates()[i].speedMetersPerSecond < 0.0) {
+        prev_heading[i] = prev_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI));
+      }
+      desired_vx[i] =
+          desiredModuleState[i].angle.getCos() * desiredModuleState[i].speedMetersPerSecond;
+      desired_vy[i] =
+          desiredModuleState[i].angle.getSin() * desiredModuleState[i].speedMetersPerSecond;
+      desired_heading[i] = desiredModuleState[i].angle;
+      if (desiredModuleState[i].speedMetersPerSecond < 0.0) {
+        desired_heading[i] = desired_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI));
+      }
+      if (all_modules_should_flip) {
+        double required_rotation_rad =
+            Math.abs(prev_heading[i].unaryMinus().rotateBy(desired_heading[i]).getRadians());
+        if (required_rotation_rad < Math.PI / 2.0) {
+          all_modules_should_flip = false;
+        }
+      }
+    }
+    if (all_modules_should_flip
+        && !EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(prevSetpoint.chassisSpeeds()),new Twist2d())
+        && !EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(desiredState),new Twist2d())) {
+      // It will (likely) be faster to stop the robot, rotate the modules in place to the complement
+      // of the desired
+      // angle, and accelerate again.
+      return generateSetpoint(limits, centerOfMassHeight, prevSetpoint, new ChassisSpeeds(), dt);
+    }
+
+    // Compute the deltas between start and goal. We can then interpolate from the start state to
+    // the goal state; then
+    // find the amount we can move from start towards goal in this cycle such that no kinematic
+    // limit is exceeded.
+    double dx = desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond;
+    double dy = desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond;
+    double dtheta =
+        desiredState.omegaRadiansPerSecond - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond;
+
+    // 's' interpolates between start and goal. At 0, we are at prevState and at 1, we are at
+    // desiredState.
+    double min_s = 1.0;
+
+    // In cases where an individual module is stopped, we want to remember the right steering angle
+    // to command (since
+    // inverse kinematics doesn't care about angle, we can be opportunistically lazy).
+    List<Optional<Rotation2d>> overrideSteering = new ArrayList<>(modules.length);
+    // Enforce steering velocity limits. We do this by taking the derivative of steering angle at
+    // the current angle,
+    // and then backing out the maximum interpolant between start and goal states. We remember the
+    // minimum across all modules, since
+    // that is the active constraint.
+    final double max_theta_step = dt * limits.maxSteeringVelocity();
+    for (int i = 0; i < modules.length; ++i) {
+      if (!need_to_steer) {
+        overrideSteering.add(Optional.of(prevSetpoint.moduleStates()[i].angle));
+        continue;
+      }
+      overrideSteering.add(Optional.empty());
+      if (epsilonEquals(prevSetpoint.moduleStates()[i].speedMetersPerSecond, 0.0)) {
+        // If module is stopped, we know that we will need to move straight to the final steering
+        // angle, so limit based
+        // purely on rotation in place.
+        if (epsilonEquals(desiredModuleState[i].speedMetersPerSecond, 0.0)) {
+          // Goal angle doesn't matter. Just leave module at its current angle.
+          overrideSteering.set(i, Optional.of(prevSetpoint.moduleStates()[i].angle));
+          continue;
+        }
+
+        var necessaryRotation =
+            prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(desiredModuleState[i].angle);
+        if (flipHeading(necessaryRotation)) {
+          necessaryRotation = necessaryRotation.rotateBy(Rotation2d.fromRadians(Math.PI));
+        }
+        // getRadians() bounds to +/- Pi.
+        final double numStepsNeeded = Math.abs(necessaryRotation.getRadians()) / max_theta_step;
+
+        if (numStepsNeeded <= 1.0) {
+          // Steer directly to goal angle.
+          overrideSteering.set(i, Optional.of(desiredModuleState[i].angle));
+          // Don't limit the global min_s;
+          continue;
+        } else {
+          // Adjust steering by max_theta_step.
+          overrideSteering.set(
+              i,
+              Optional.of(
+                  prevSetpoint.moduleStates()[i].angle.rotateBy(
+                      Rotation2d.fromRadians(
+                          Math.signum(necessaryRotation.getRadians()) * max_theta_step))));
+          min_s = 0.0;
+          continue;
+        }
+      }
+      if (min_s == 0.0) {
+        // s can't get any lower. Save some CPU.
+        continue;
+      }
+
+      final int kMaxIterations = 8;
+      double s =
+          findSteeringMaxS(
+              prev_vx[i],
+              prev_vy[i],
+              prev_heading[i].getRadians(),
+              desired_vx[i],
+              desired_vy[i],
+              desired_heading[i].getRadians(),
+              max_theta_step,
+              kMaxIterations);
+      min_s = Math.min(min_s, s);
+    }
+
+    // Enforce drive wheel acceleration limits.
+    final double max_vel_step = dt * limits.maxDriveAcceleration();
+    final double max_vel_step_2 = dt * limits.staticFriction() * Constants.GRAVITY_ACCELERATION;
+    for (int i = 0; i < modules.length; ++i) {
+      if (min_s == 0.0) {
+        // No need to carry on.
+        break;
+      }
+      double vx_min_s =
+          min_s == 1.0 ? desired_vx[i] : (desired_vx[i] - prev_vx[i]) * min_s + prev_vx[i];
+      double vy_min_s =
+          min_s == 1.0 ? desired_vy[i] : (desired_vy[i] - prev_vy[i]) * min_s + prev_vy[i];
+      // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we
+      // already know we can't go faster
+      // than that.
+      final int kMaxIterations = 10;
+      double s = min_s*findDriveMaxS(prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step);
+      
+      // Limit the overall acceleration of this wheel
+      double s2 = min_s*findAccelerationMaxS(prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step_2, kMaxIterations);
+
+      min_s = Math.min(Math.min(min_s, s), s2);
+    }
+
+    if(centerOfMassHeight > 0.02){
+      // Limit the acceleration in the x and y directions separately based on the center of mass.
+      // To make the torque on the robot 0, we can assume all of the mass is on the back wheel, where the front is the direction the robot is accelerating toward
+      // Torque is equal to the force times the component of the radius perpendicular to the force
+      // T = torque, m = mass, a = acceleration, g = gravity acceleration, x = distance from center to wheel
+      // T = mgx - mah = 0
+      // a = gx/h
+      double maxAccel = Constants.GRAVITY_ACCELERATION*(DriveConstants.TRACK_WIDTH/2)/centerOfMassHeight;
+      // Limit based on this calculated value
+      // x and y are limited separately because, when tipping in a diagonal direction, the distance is longer
+      double xAccel = Math.abs(desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond) / dt;
+      double yAccel = Math.abs(desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond) / dt;
+      if(!epsilonEquals(xAccel, 0)){
+        double s = maxAccel / xAccel;
+        min_s = Math.min(min_s, s);
+      }
+      if(!epsilonEquals(yAccel, 0)){
+        double s = maxAccel / yAccel;
+        min_s = Math.min(min_s, s);
+      }
+    }
+
+    ChassisSpeeds retSpeeds =
+        new ChassisSpeeds(
+            prevSetpoint.chassisSpeeds().vxMetersPerSecond + min_s * dx,
+            prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy,
+            prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta);
+    var retStates = kinematics.toSwerveModuleStates(retSpeeds);
+    for (int i = 0; i < modules.length; ++i) {
+      final var maybeOverride = overrideSteering.get(i);
+      if (maybeOverride.isPresent()) {
+        var override = maybeOverride.get();
+        if (flipHeading(retStates[i].angle.unaryMinus().rotateBy(override))) {
+          retStates[i].speedMetersPerSecond *= -1.0;
+        }
+        retStates[i].angle = override;
+      } 
+      final var deltaRotation =
+          prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(retStates[i].angle);
+      if (flipHeading(deltaRotation)) {
+        retStates[i].angle = retStates[i].angle.rotateBy(Rotation2d.fromRadians(Math.PI));
+        retStates[i].speedMetersPerSecond *= -1.0;
+      }
+    }
+    return new SwerveSetpoint(retSpeeds, retStates);
+  }
+}

diff --git a/src/main/java/frc/robot/util/SysId.java b/src/main/java/frc/robot/util/SysId.java
new file mode 100644 (file)
index 0000000..6e5a655
--- /dev/null
+++ b/src/main/java/frc/robot/util/SysId.java
@@ -0,0 +1,46 @@
+// 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 frc.robot.util;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism;
+
+import java.util.function.Consumer;
+
+
+/**
+ * Util class for creating SysId routines
+*/
+public class SysId {
+
+    private SysIdRoutine sysIdRoutine;
+
+    public SysId(String name, Consumer<Voltage> driveConsumer, Consumer<SysIdRoutineLog> logConsumer, Subsystem subsystem, Config config){
+        sysIdRoutine = new SysIdRoutine(
+            config,
+            new Mechanism(
+                driveConsumer,
+                logConsumer,
+                subsystem,
+                name
+            )
+        );
+    }
+    public SysId(String name, Consumer<Voltage> driveConsumer, Subsystem subsystem, Config config){
+        this(name,driveConsumer,null,subsystem,config);
+    }
+
+    public Command runQuasisStatic(Direction direction){
+        return sysIdRoutine.quasistatic(direction);
+    }
+    public Command runDynamic(Direction direction){
+        return sysIdRoutine.dynamic(direction);
+    }
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/util/TestType.java b/src/main/java/frc/robot/util/TestType.java
deleted file mode 100644 (file)
index 364ac58..0000000
--- a/src/main/java/frc/robot/util/TestType.java
+++ /dev/null
@@ -1,5 +0,0 @@
-package frc.robot.util;
-
-public enum TestType {
-  NONE, TUNE_EXAMPLE_PID, TEST_DRIVE
-}

diff --git a/src/main/java/frc/robot/util/TimeAccuracyTest.java b/src/main/java/frc/robot/util/TimeAccuracyTest.java
new file mode 100644 (file)
index 0000000..8248fcc
--- /dev/null
+++ b/src/main/java/frc/robot/util/TimeAccuracyTest.java
@@ -0,0 +1,51 @@
+package frc.robot.util;
+
+import edu.wpi.first.util.WPIUtilJNI;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+
+/**
+ * Class for testing if a value is within a certain margin of error for a certain amount of time.
+ */
+public class TimeAccuracyTest {
+
+    private final BooleanSupplier accuracyTest;
+    private final double setpointUpdateTime;
+    private final double errorMargin;
+    private final double timeMargin;
+    private boolean lastUseableResult = false;
+
+    /**
+     * @param actual      DoubleSupplier that returns the actual value
+     * @param setpoint    DoubleSupplier that returns the setpoint
+     * @param errorMargin margin of error for the test to be accurate
+     * @param timeMargin  time in seconds that the setpoint must be held for the test to be accurate
+     */
+    public TimeAccuracyTest(DoubleSupplier actual, DoubleSupplier setpoint, double errorMargin, double timeMargin) {
+        this.errorMargin = errorMargin;
+        this.timeMargin = timeMargin;
+        setpointUpdateTime = WPIUtilJNI.now() * 1e-6;
+        accuracyTest = () -> getDoubleAccuracyTest(actual, setpoint);
+    }
+
+    /**
+     * Determines if the test is successful.
+     *
+     * @return true if the test is successful, false if not
+     */
+    public boolean calculate() {
+        if (setpointUpdateTime + timeMargin <= WPIUtilJNI.now() * 1e-6)
+            lastUseableResult = accuracyTest.getAsBoolean();
+        return lastUseableResult;
+    }
+
+    /**
+     * Determines if the actual value is within the error margin of the setpoint.
+     *
+     * @return true if the actual value is within the error margin of the setpoint, false if not
+     */
+    private boolean getDoubleAccuracyTest(DoubleSupplier actual, DoubleSupplier setpoint) {
+        return Math.abs(actual.getAsDouble() - setpoint.getAsDouble()) <= errorMargin;
+    }
+}

diff --git a/src/main/java/frc/robot/util/Vision/DetectedObject.java b/src/main/java/frc/robot/util/Vision/DetectedObject.java
new file mode 100644 (file)
index 0000000..babcc1d
--- /dev/null
+++ b/src/main/java/frc/robot/util/Vision/DetectedObject.java
@@ -0,0 +1,334 @@
+package frc.robot.util.Vision;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc.robot.Robot;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/**
+ * Stores information about an object detected by vision
+ */
+public class DetectedObject {
+    private static Drivetrain drive;
+    public final Pose3d pose;
+    public final ObjectType type;
+
+    public enum ObjectType{
+        CORAL(Units.inchesToMeters(4.5/2)),
+        ALGAE(Units.inchesToMeters(16.25/2)),
+        RED_ROBOT(0),
+        BLUE_ROBOT(0),
+        NONE(0);
+
+        public final double height;
+
+        private ObjectType(double h){
+            height = h;
+        }
+    };
+
+    /**
+     * Sets the drivetrain to use for pose calculations
+     * @param drive The drivetrain
+     */
+    public static void setDrive(Drivetrain drive){
+        DetectedObject.drive = drive;
+    }
+
+    /**
+     * Creates a default DetectedObject with default attributes
+     */
+    public DetectedObject(){
+        pose = new Pose3d();
+        type = ObjectType.NONE;
+    }
+
+    /**
+     * Creates a new DetectedObject
+     * @param xOffset The x offset from the camera to the object in radians
+     * @param yOffset The y offset form the camera to the object in radians
+     * @param distance The distance from the camera to the object in meters
+     * @param type What type of object it is
+     * @param robotToCamera The transformation form the robot to the camera
+     */
+    public DetectedObject(double xOffset, double yOffset, double distance, ObjectType type, Transform3d robotToCamera){
+        this(xOffset, yOffset, distance, type, robotToCamera, -1);
+    }
+
+    /**
+     * Creates a new DetectedObject
+     * @param xOffset The x offset from the camera to the object in radians
+     * @param yOffset The y offset form the camera to the object in radians
+     * @param distance The distance from the camera to the object in meters
+     * @param type What type of object it is
+     * @param robotToCamera The transformation form the robot to the camera
+     * @param timestamp The timestamp of the picture in seconds
+     */
+    public DetectedObject(double xOffset, double yOffset, double distance, ObjectType type, Transform3d robotToCamera, double timestamp){
+        this.type = type;
+        // Get the position relative to the camera
+        Translation3d translation = new Translation3d(distance, new Rotation3d(0, -yOffset, -xOffset))
+        // Rotate and translate it to get the position relative to the robot
+            .rotateBy(robotToCamera.getRotation())
+            .plus(robotToCamera.getTranslation());
+        // If the drivetrain exists, rotate and translate it to get the field relative position
+        if(drive != null){
+            Pose2d drivePose = drive.getPoseAt(timestamp);
+            translation = translation.rotateBy(new Rotation3d(
+                0,
+                0,
+                drivePose.getRotation().getRadians()
+            )).plus(new Translation3d(
+                drivePose.getX(),
+                drivePose.getY(),
+                0
+            ));
+        }
+        pose = new Pose3d(translation, new Rotation3d());
+    }
+    
+    /**
+     * Creates a new DetectedObject
+     * @param xOffset The x offset from the camera to the object in radians
+     * @param yOffset The y offset form the camera to the object in radians
+     * @param distance The distance from the camera to the object in meters
+     * @param type What type of object it is
+     * @param robotToCamera The transformation form the robot to the camera
+     * @param timestamp The timestamp of the picture in seconds
+     */
+    public DetectedObject(double xOffset, double yOffset, double distance, int type, Transform3d robotToCamera, double timestamp){
+        this(xOffset, yOffset, distance, getType(type), robotToCamera, timestamp);
+    }
+
+    /**
+     * Creates a new DetectedObject
+     * @param xOffset The x offset from the camera to the object in radians
+     * @param yOffset The y offset form the camera to the object in radians
+     * @param distance The distance from the camera to the object in meters
+     * @param type What type of object it is
+     * @param robotToCamera The transformation form the robot to the camera
+     */
+    public DetectedObject(double xOffset, double yOffset, double distance, int type, Transform3d robotToCamera){
+        this(xOffset, yOffset, distance, getType(type), robotToCamera, -1);
+    }
+
+    /**
+     * Creates a new DetectedObject
+     * @param xOffset The x offset from the camera to the object in radians
+     * @param yOffset The y offset form the camera to the object in radians
+     * @param distance The distance from the camera to the object in meters
+     * @param type What type of object it is
+     * @param robotToCamera The transformation form the robot to the camera
+     * @param timestamp The timestamp of the picture in seconds
+     */
+    public DetectedObject(double xOffset, double yOffset, double distance, String type, Transform3d robotToCamera, double timestamp){
+        this(xOffset, yOffset, distance, getType(type), robotToCamera, timestamp);
+    }
+    
+    /**
+     * Creates a new DetectedObject
+     * @param xOffset The x offset from the camera to the object in radians
+     * @param yOffset The y offset form the camera to the object in radians
+     * @param distance The distance from the camera to the object in meters
+     * @param type What type of object it is
+     * @param robotToCamera The transformation form the robot to the camera
+     */
+    public DetectedObject(double xOffset, double yOffset, double distance, String type, Transform3d robotToCamera){
+        this(xOffset, yOffset, distance, getType(type), robotToCamera, -1);
+    }
+
+    /**
+     * Creates a new DetectedObject, assuming the object is on the ground
+     * @param xOffset The x offset from the camera to the object in radians
+     * @param yOffset The y offset form the camera to the object in radians
+     * @param type What type of object it is
+     * @param robotToCamera The transformation form the robot to the camera
+     */
+    public DetectedObject(double xOffset, double yOffset, ObjectType type, Transform3d robotToCamera){
+        this(xOffset, yOffset, type, robotToCamera, -1);
+    }
+    
+    /**
+     * Creates a new DetectedObject, assuming the object is on the ground
+     * @param xOffset The x offset from the camera to the object in radians
+     * @param yOffset The y offset form the camera to the object in radians
+     * @param type What type of object it is
+     * @param robotToCamera The transformation form the robot to the camera
+     * @param timestamp The timestamp of the picture in seconds
+     */
+    public DetectedObject(double xOffset, double yOffset, ObjectType type, Transform3d robotToCamera, double timestamp){
+        this.type = type;
+        // Get the position relative to the camera
+        Translation3d translation = new Translation3d(1, new Rotation3d(0, -yOffset, -xOffset))
+        // Rotate it to get the position relative to the rotated camera
+            .rotateBy(robotToCamera.getRotation());
+        // Scale it so that the object will be on the ground (- because translation's z will be negative)
+        if(!isRobot()){
+            translation = translation.times(-(robotToCamera.getZ()-type.height)/translation.getZ());
+        }else{
+            // Assume all robots are ~3m from the camera
+            translation = translation.times(3);
+        }
+        // Translate it to make it relative to the robot
+        translation = translation.plus(robotToCamera.getTranslation());
+        // If the drivetrain exists, rotate and translate it to be field relative
+        if(drive != null){
+            Pose2d drivePose = drive.getPoseAt(timestamp);
+            translation = translation.rotateBy(new Rotation3d(
+                0,
+                0,
+                drivePose.getRotation().getRadians()
+            )).plus(new Translation3d(
+                drivePose.getX(),
+                drivePose.getY(),
+                0
+            ));
+        }
+        pose = new Pose3d(translation, new Rotation3d());
+    }
+    
+    /**
+     * Creates a new DetectedObject, assuming the object is on the ground
+     * @param xOffset The x offset from the camera to the object in radians
+     * @param yOffset The y offset form the camera to the object in radians
+     * @param type What type of object it is
+     * @param robotToCamera The transformation form the robot to the camera
+     * @param timestamp The timestamp of the picture in seconds
+     */
+    public DetectedObject(double xOffset, double yOffset, int type, Transform3d robotToCamera, double timestamp){
+        this(xOffset, yOffset, getType(type), robotToCamera, timestamp);
+    }
+
+    /**
+     * Creates a new DetectedObject, assuming the object is on the ground
+     * @param xOffset The x offset from the camera to the object in radians
+     * @param yOffset The y offset form the camera to the object in radians
+     * @param type What type of object it is
+     * @param robotToCamera The transformation form the robot to the camera
+     */
+    public DetectedObject(double xOffset, double yOffset, int type, Transform3d robotToCamera){
+        this(xOffset, yOffset, getType(type), robotToCamera, -1);
+    }
+
+    /**
+     * Creates a new DetectedObject, assuming the object is on the ground
+     * @param xOffset The x offset from the camera to the object in radians
+     * @param yOffset The y offset form the camera to the object in radians
+     * @param type What type of object it is
+     * @param robotToCamera The transformation form the robot to the camera
+     * @param timestamp The timestamp of the picture in seconds
+     */
+    public DetectedObject(double xOffset, double yOffset, String type, Transform3d robotToCamera, double timestamp){
+        this(xOffset, yOffset, getType(type), robotToCamera, timestamp);
+    }
+
+    /**
+     * Creates a new DetectedObject, assuming the object is on the ground
+     * @param xOffset The x offset from the camera to the object in radians
+     * @param yOffset The y offset form the camera to the object in radians
+     * @param type What type of object it is
+     * @param robotToCamera The transformation form the robot to the camera
+     */
+    public DetectedObject(double xOffset, double yOffset, String type, Transform3d robotToCamera){
+        this(xOffset, yOffset, getType(type), robotToCamera, -1);
+    }
+
+    /**
+     * Converts an int to an ObjectType
+     * @param type The type as an int, between 0 and the number of object types - 1
+     * @return The type as an ObjectType
+     */
+    public static ObjectType getType(int type){
+        ObjectType[] values = ObjectType.values();
+        if(type < 0 || type >= values.length){
+            return ObjectType.NONE;
+        }
+        return values[type];
+    }
+
+    /**
+     * Converts a String to an ObjectType
+     * @param type The type as a String
+     * @return The type as an ObjectType
+     */
+    public static ObjectType getType(String type){
+        ObjectType result = ObjectType.valueOf(type.toUpperCase());
+        return result==null ? ObjectType.NONE : result;
+    }
+
+    /**
+     * Returns if the object is a game piece
+     * @return True if the object is a game piece, false otherwise
+     */
+    public boolean isGamePiece(){
+        return type==ObjectType.CORAL || type==ObjectType.ALGAE;
+    }
+    /**
+     * Returns if the object is a robot
+     * @return True if the object is a red or blue robot, false otherwise
+     */
+    public boolean isRobot(){
+        return type==ObjectType.RED_ROBOT || type==ObjectType.BLUE_ROBOT;
+    }
+    /**
+     * Returns if the object is a robot on the same alliance
+     * @return If the object is a robot on the same alliance
+     */
+    public boolean isSameAllianceRobot(){
+        return type == (Robot.getAlliance()==Alliance.Red?ObjectType.RED_ROBOT:ObjectType.BLUE_ROBOT);
+    }
+    /**
+     * Returns if the object is a robot on the other alliance
+     * @return If the object is a robot on the other alliance
+     */
+    public boolean isOtherAllianceRobot(){
+        return type == (Robot.getAlliance()==Alliance.Red?ObjectType.BLUE_ROBOT:ObjectType.RED_ROBOT);
+    }
+
+    /**
+     * Gets the distance from the center of the robot to the object
+     * @return The distance in meters
+     */
+    public double getDistance(){
+        return drive.getPose().getTranslation().getDistance(pose.getTranslation().toTranslation2d());
+    }
+
+    /**
+     * Gets the field relative angle from the robot to the object
+     * @return The angle in radians
+     */
+    public double getAngle(){
+        Pose2d drivePose = drive.getPose();
+        return Math.atan2(pose.getY()-drivePose.getY(), pose.getX()-drivePose.getX());
+    }
+
+    /**
+     * Gets the angle relative to the front of the robot (0 is in front, positive counterclockwise)
+     * @return The relative angle in radians
+     */
+    public double getRelativeAngle(){
+        double angle = getAngle()-drive.getYaw().getRadians();
+        return MathUtil.angleModulus(angle);
+    }
+
+    /**
+     * Gets the angle of the object relative to the robot's velocity (0 is in front, positive counterclockwise)
+     * @return The relative angle in radians
+     */
+    public double getVelocityRelativeAngle(){
+        ChassisSpeeds speeds = drive.getChassisSpeeds();
+        double angle = getRelativeAngle() - Math.atan2(speeds.vyMetersPerSecond, speeds.vxMetersPerSecond);
+        return MathUtil.angleModulus(angle);
+    }
+
+    public String toString(){
+        return type+" at ("+pose.getX()+", "+pose.getY()+", "+pose.getZ()+")";
+    }
+}

diff --git a/src/main/java/frc/robot/util/Vision/DriverAssist.java b/src/main/java/frc/robot/util/Vision/DriverAssist.java
new file mode 100644 (file)
index 0000000..b2e9048
--- /dev/null
+++ b/src/main/java/frc/robot/util/Vision/DriverAssist.java
@@ -0,0 +1,185 @@
+// 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 frc.robot.util.Vision;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
+import frc.robot.constants.Constants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.SwerveStuff.SwerveSetpoint;
+import frc.robot.util.SwerveStuff.SwerveSetpointGenerator;
+
+/** 
+ * A util class to assist the driver drive to a pose
+ */
+public class DriverAssist {
+    // The amount to correct the driver's inpus by
+    // 0 = return unchanged driver inputs, 1 = return a value much closer to the calculated speed, sometimes equal to it
+    // This can be greater than 1 to fully correct more of the time, like from farther away
+    private static final double CORRECTION_FACTOR = 1;
+
+    
+    // Variables used for first method
+    // The setpoint generator, which limits the acceleration
+    private static final SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator();
+    private static final TrapezoidProfile xProfile = new TrapezoidProfile(new Constraints(DriveConstants.MAX_SPEED, DriveConstants.MAX_LINEAR_ACCEL));
+    private static final TrapezoidProfile yProfile = new TrapezoidProfile(new Constraints(DriveConstants.MAX_SPEED, DriveConstants.MAX_LINEAR_ACCEL));
+    private static final TrapezoidProfile angleProfile = new TrapezoidProfile(new Constraints(DriveConstants.MAX_ANGULAR_SPEED, DriveConstants.MAX_ANGULAR_ACCEL));
+
+    /**
+     * Combines the driver input with a speed calculated using a trapezoidal profile <p>
+     * Called when VisionConstants.DRIVER_ASSIST_MODE is 2
+     * @param drive The drivetrain
+     * @param driverInput The driver input speed
+     * @param desiredPose The pose to drive to
+     * @param keepAngle True to use the angle in the pose, false to point hte robot toward the pose
+     * @return The new speed
+     */
+    private static ChassisSpeeds calculate2(Drivetrain drive, ChassisSpeeds driverInput, Pose2d desiredPose, boolean keepAngle) {
+        // Do nothing if there is no pose
+        if(desiredPose == null){
+            return driverInput;
+        }
+
+        // Store current states
+        Pose2d currentPose = drive.getPose();
+        Rotation2d yaw = drive.getYaw();
+        ChassisSpeeds driveSpeeds = drive.getChassisSpeeds();
+        driveSpeeds = ChassisSpeeds. fromFieldRelativeSpeeds(driveSpeeds,yaw); // Changing this does not cause problems because getChassisSpeeds() creates a new object
+        State xState = new State(currentPose.getX(), driveSpeeds.vxMetersPerSecond);
+        State yState = new State(currentPose.getY(), driveSpeeds.vyMetersPerSecond);
+        State angleState = new State(currentPose.getRotation().getRadians(), driveSpeeds.omegaRadiansPerSecond);
+
+        // Store goal states
+        State xGoal = new State(desiredPose.getX(), 0);
+        State yGoal = new State(desiredPose.getY(), 0);
+        Translation2d difference = desiredPose.getTranslation().minus(currentPose.getTranslation());
+        double rotation = keepAngle ? desiredPose.getRotation().getRadians() : difference.getAngle().getRadians();
+        if(rotation - currentPose.getRotation().getRadians() > Math.PI){
+            rotation -= 2*Math.PI;
+        }else if(rotation - currentPose.getRotation().getRadians() < -Math.PI){
+            rotation += 2*Math.PI;
+        }
+        State angleGoal = new State(rotation, 0);
+
+        // Calculate ideal speeds for next frame
+        ChassisSpeeds goal = new ChassisSpeeds(
+            xProfile.calculate(Constants.LOOP_TIME, xState, xGoal).velocity,
+            yProfile.calculate(Constants.LOOP_TIME, yState, yGoal).velocity,
+            angleProfile.calculate(Constants.LOOP_TIME, angleState, angleGoal).velocity
+        );
+        // Robot-relataive goal
+        ChassisSpeeds goalRobot = goal.times(1);
+        goalRobot = ChassisSpeeds. fromRobotRelativeSpeeds(goalRobot, yaw);
+
+        // This calculates the actual acceleration we can get
+        // This is the only thing that needs to be robot relative
+        SwerveSetpoint nextSetpoint = setpointGenerator.generateSetpoint(
+                DriveConstants.MODULE_LIMITS,
+                0,
+                drive.getCurrSetpoint(), goalRobot,
+                Constants.LOOP_TIME);
+            ChassisSpeeds nextChassisSpeed = nextSetpoint.chassisSpeeds();
+            nextChassisSpeed = ChassisSpeeds.fromRobotRelativeSpeeds(nextChassisSpeed, yaw);
+
+        // Robot relative driver inputs
+        ChassisSpeeds driverInputRobot = driverInput.times(1); // Copy so original doesn't change
+        driverInputRobot = ChassisSpeeds.fromFieldRelativeSpeeds(driverInputRobot, yaw);
+        // This is the speed the driver will be able to get next frame
+        // Both speeds need to be obtainable in 1 frame or the driver speed will always be farther away
+        SwerveSetpoint driverSetpoint = setpointGenerator.generateSetpoint(
+                DriveConstants.MODULE_LIMITS,
+                0,
+                drive.getCurrSetpoint(), driverInputRobot,
+                Constants.LOOP_TIME);
+        ChassisSpeeds driverSpeeds = driverSetpoint.chassisSpeeds();
+        driverSpeeds= ChassisSpeeds.fromRobotRelativeSpeeds(driverSpeeds,yaw);
+
+        // The difference between the 2 speeds
+        ChassisSpeeds error = nextChassisSpeed.minus(driverSpeeds);
+        
+        // 1.2*1.2^-distance decreases the amount it correct by as distance increases
+        double distanceFactor = 1.2*Math.pow(1.2, -currentPose.getTranslation().getDistance(desiredPose.getTranslation()));
+
+        // Driver input speed
+        double driverInputSpeed = Math.hypot(driverInput.vxMetersPerSecond, driverInput.vyMetersPerSecond);
+
+        // The amount to correct by
+        ChassisSpeeds correction = error.times(Math.min(CORRECTION_FACTOR * distanceFactor * driverInputSpeed / DriveConstants.MAX_SPEED, 1));
+        
+        return driverSpeeds.plus(correction);
+        // return nextChassisSpeed.times(CORRECTION_FACTOR).plus(driverInput.times(1-CORRECTION_FACTOR));
+    }
+
+    // Constants used for second method
+    public static final double MAX_VELOCITY_ANGLE_ERROR = Math.PI/4;
+    public static final double MAX_DISTANCE_ERROR = 2;
+    public static final double ROTATION_CORRECTION_FACTOR = 0.1;
+    public static final double MAX_ROTATION_ERROR = Math.PI/3;
+
+    /**
+     * Combines the driver input with a calculated correction speed
+     * @param drive The drivetrain
+     * @param driverInput The driver input speed
+     * @param desiredPose The pose to drive to
+     * @param keepAngle True to use the angle in the pose, false to point hte robot toward the pose
+     * @return The new speed
+     */
+    @SuppressWarnings("unused") // Needed because some code might not run for some values of DRIVER_ASSIST_MODE
+    public static ChassisSpeeds calculate(Drivetrain drive, ChassisSpeeds driverInput, Pose2d desiredPose, boolean keepAngle) {
+        if(VisionConstants.DRIVER_ASSIST_MODE < 2 || desiredPose == null){
+            return driverInput;
+        }else if(VisionConstants.DRIVER_ASSIST_MODE == 2){
+            return calculate2(drive, driverInput, desiredPose, keepAngle);
+        }
+        // Combines the driver input with a speed perpendicular to the input
+        Pose2d drivePose = drive.getPose();
+        Translation2d difference = desiredPose.getTranslation().minus(drivePose.getTranslation());
+        double distance = difference.getNorm();
+        double velocityAngle = difference.getAngle().getRadians();
+        double targetAngle = keepAngle ? desiredPose.getRotation().getRadians() : MathUtil.angleModulus(velocityAngle + Math.PI/2);
+        double inputSpeed = Math.hypot(driverInput.vxMetersPerSecond, driverInput.vyMetersPerSecond);
+        double driverAngle = Math.atan2(driverInput.vyMetersPerSecond, driverInput.vxMetersPerSecond);
+        double velocityAngleError = MathUtil.angleModulus(velocityAngle - driverAngle);
+        if(Math.abs(velocityAngleError) > MAX_VELOCITY_ANGLE_ERROR){
+            return driverInput;
+        }
+        double perpendicularDist = Math.abs(distance * Math.sin(velocityAngleError));
+        if(perpendicularDist > MAX_DISTANCE_ERROR){
+            return driverInput;
+        }
+        double perpendicularAngle = MathUtil.angleModulus(driverAngle + Math.PI/2*Math.signum(velocityAngleError));
+        // Different options for calculation.
+        double correctionSpeed = 0;
+        switch(VisionConstants.DRIVER_ASSIST_MODE){
+            case 3:
+                correctionSpeed = Math.min(CORRECTION_FACTOR * inputSpeed * Math.pow(2, -perpendicularDist), Math.abs(Math.tan(velocityAngleError)*inputSpeed));
+                break;
+            case 4:
+                correctionSpeed = Math.min(CORRECTION_FACTOR * Math.pow(1.5, -perpendicularDist), 1) * Math.abs(Math.tan(velocityAngleError)*inputSpeed);
+                break;
+            case 5:
+                correctionSpeed = Math.min(CORRECTION_FACTOR * inputSpeed * Math.pow(2, -perpendicularDist) * Math.pow(1.2, -distance+1), Math.abs(Math.tan(velocityAngleError)*inputSpeed));
+                break;
+        }
+        double rotationError = MathUtil.angleModulus(targetAngle-drivePose.getRotation().getRadians());
+        if(Math.abs(rotationError) > MAX_ROTATION_ERROR){
+            return driverInput;
+        }
+        // We want to set the current angular velocity so that we can decelerate to 0rad/s at the setpoint
+        // Since 0=v0^2+2ax, v0=√(2ax)
+        // High correction factors will also ignore the driver's input more
+        double rotationalSpeed = ROTATION_CORRECTION_FACTOR * Math.signum(rotationError)*Math.sqrt(2*DriveConstants.MAX_ANGULAR_ACCEL*Math.abs(rotationError)) - ROTATION_CORRECTION_FACTOR * driverInput.omegaRadiansPerSecond;
+        return driverInput.plus(new ChassisSpeeds(correctionSpeed*Math.cos(perpendicularAngle), correctionSpeed*Math.sin(perpendicularAngle), rotationalSpeed));
+    }
+}

diff --git a/src/main/java/frc/robot/util/Vision/Vision.java b/src/main/java/frc/robot/util/Vision/Vision.java
new file mode 100644 (file)
index 0000000..c55b1db
--- /dev/null
+++ b/src/main/java/frc/robot/util/Vision/Vision.java
@@ -0,0 +1,677 @@
+package frc.robot.util.Vision;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Optional;
+import java.util.function.DoubleUnaryOperator;
+
+import org.littletonrobotics.junction.Logger;
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.simulation.PhotonCameraSim;
+import org.photonvision.simulation.VisionSystemSim;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+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.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.constants.FieldConstants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.util.MathUtils;
+
+// Vision and it's commands are adapted from Iron Claw's FRC2023
+public class Vision {
+  private NetworkTable m_objectDetectionTable;
+
+  private NetworkTableEntry xOffset;
+  private NetworkTableEntry yOffset;
+  private NetworkTableEntry objectDistance;
+  private NetworkTableEntry objectClass;
+  private NetworkTableEntry cameraIndex;
+  
+  // The field layout. Instance variable
+  private AprilTagFieldLayout aprilTagFieldLayout;
+  // A list of the cameras on the robot.
+  private ArrayList<VisionCamera> cameras = new ArrayList<>();
+
+  private VisionSystemSim visionSim;
+
+  private boolean sawTag = false;
+
+  // Array of tags to use, null or empty array to use all tags
+  private int[] onlyUse = null;
+
+  /**
+   * Creates a new instance of Vision and sets up the cameras and field layout
+   */
+  public Vision(ArrayList<Pair<String, Transform3d>> camList) {
+    // Initialize object_detection NetworkTable
+    m_objectDetectionTable = NetworkTableInstance.getDefault().getTable("object_detection");
+
+    // From the object detection NetworkTable, get the entries
+    objectDistance = m_objectDetectionTable.getEntry("distance");
+    xOffset = m_objectDetectionTable.getEntry("x_offset");
+    yOffset = m_objectDetectionTable.getEntry("y_offset");
+    objectClass = m_objectDetectionTable.getEntry("class");
+    cameraIndex = m_objectDetectionTable.getEntry("index");
+
+    // Start NetworkTables server
+    NetworkTableInstance.getDefault().startServer();
+
+    // Load field layout
+    aprilTagFieldLayout = new AprilTagFieldLayout(FieldConstants.APRIL_TAGS, FieldConstants.FIELD_LENGTH, FieldConstants.FIELD_WIDTH);
+
+    // Sets the origin to the right side of the blue alliance wall
+    aprilTagFieldLayout.setOrigin(OriginPosition.kBlueAllianceWallRightSide);
+
+    if(VisionConstants.ENABLED){
+      // Puts the cameras in an array list
+      for (int i = 0; i < camList.size(); i++) {
+        cameras.add(new VisionCamera(camList.get(i).getFirst(), camList.get(i).getSecond()));
+      }
+
+      if(RobotBase.isSimulation()){
+        visionSim = new VisionSystemSim("Vision");
+        visionSim.addAprilTags(aprilTagFieldLayout);
+        for(VisionCamera c : cameras){
+          PhotonCameraSim cameraSim = new PhotonCameraSim(c.camera);
+          cameraSim.enableDrawWireframe(true);
+          cameraSim.prop.setAvgLatencyMs(30);
+          cameraSim.prop.setCalibration(1280, 720, Rotation2d.fromDegrees(78));
+          visionSim.addCamera(cameraSim, c.photonPoseEstimator.getRobotToCameraTransform());
+        }
+      }
+    }
+  }
+
+
+  /**
+   * Get the horizontal offsets from the crosshair to the targets
+   * @return An array of offsets in degrees
+   */
+  public double[] getHorizontalOffset(){
+    if(!VisionConstants.OBJECT_DETECTION_ENABLED){
+      return new double[0];
+    }
+    return xOffset.getDoubleArray(new double[0]);
+  }
+
+  /**
+   * Get the vertical offsets from the crosshair to the targets
+   * @return An array of offsets in degrees
+   */
+  public double[] getVerticalOffset(){
+    if(!VisionConstants.OBJECT_DETECTION_ENABLED){
+      return new double[0];
+    }
+    return yOffset.getDoubleArray(new double[0]);
+  }
+
+  /**
+   * Get the target distances
+   * @return Distance in meters
+   */
+  @SuppressWarnings("unused")
+  public double[] getDistance(){
+    if(!VisionConstants.OBJECT_DETECTION_ENABLED || true){
+      return new double[0];
+    }
+    return objectDistance.getDoubleArray(new double[0]);
+  }
+
+  /**
+   * Returns whether or not a valid object is detected
+   * @return true or false
+   */
+  public boolean validObjectDetected(){
+    return getHorizontalOffset().length > 0;
+  }
+
+  /**
+   * Returns what types of object are detected
+   * @return The object types as a String array
+   */
+  @SuppressWarnings("unused")
+  public String[] getDetectedObjectClass(){
+    if(!VisionConstants.OBJECT_DETECTION_ENABLED || true){
+      return new String[0];
+    }
+    return objectClass.getStringArray(new String[0]);
+  }
+
+  /**
+   * Gets the camera indices (which camera sees the object)
+   * @return The indices as a long array (method returns long array instead of int array)
+   */
+  @SuppressWarnings("unused")
+  public long[] getCameraIndex(){
+    if(!VisionConstants.OBJECT_DETECTION_ENABLED || true){
+      return new long[0];
+    }
+    return cameraIndex.getIntegerArray(new long[0]);
+  }
+
+  /**
+   * Stores all of the detected objects in an array
+   * @return The array of DetectedObjects
+   */
+  public DetectedObject[] getDetectedObjects(){
+    if(!VisionConstants.OBJECT_DETECTION_ENABLED){
+      return new DetectedObject[0];
+    }
+    double[] xOffset = getHorizontalOffset();
+    double[] yOffset = getVerticalOffset();
+    // double[] distance = getDistance();
+    String[] objectClass = getDetectedObjectClass();
+    // long[] cameraIndex = getCameraIndex();
+    DetectedObject[] objects = new DetectedObject[Math.min(xOffset.length, yOffset.length)];
+    for(int i = 0; i < objects.length; i++){
+      objects[i] = new DetectedObject(
+        Units.degreesToRadians(xOffset[i]),
+        -Units.degreesToRadians(yOffset[i]),
+        // distance[i],
+        objectClass[i],
+        // VisionConstants.OBJECT_DETECTION_CAMERAS.get((int)cameraIndex[i]).getSecond()
+        VisionConstants.OBJECT_DETECTION_CAMERAS.get(0)
+      );
+    }
+    return objects;
+  }
+
+  /**
+   * Returns the closest game piece in front of the robot
+   * @param maxAngle The maximum angle between the angle to the object and the robot's heading or rotation to use, in radians
+   * @param relativeToVelocity Whether to compare the angle to the robot's heading or rotation, true for heading
+   * @return The best DetectedObject
+   */
+  public DetectedObject getBestGamePiece(double maxAngle, boolean relativeToVelocity){
+    DetectedObject[] objects = getDetectedObjects();
+    DetectedObject best = null;
+    double closest = Double.POSITIVE_INFINITY;
+    for(DetectedObject object : objects){
+      double dist = object.getDistance();
+      if(object.isGamePiece() && Math.abs(relativeToVelocity ? object.getVelocityRelativeAngle() : object.getAngle()) < maxAngle && dist < closest){
+        closest = dist;
+        best = object;
+      }
+    }
+    return best;
+  }
+
+  /**
+   * Gets the pose as a Pose2d using PhotonVision
+   * @param referencePoses The reference poses in order of preference, null poses will be skipped
+   * @return The pose of the robot, or null if it can't see april tags
+   */
+  public Pose2d getPose2d(Pose2d... referencePoses){
+    Pose2d referencePose = new Pose2d();
+    for (Pose2d checkReferencePose:referencePoses){
+      if (checkReferencePose != null) {
+        referencePose = checkReferencePose;
+        break;
+      }
+    }
+    ArrayList<EstimatedRobotPose> estimatedPoses = getEstimatedPoses(referencePose);
+    
+    if (estimatedPoses.size() == 0) return null;
+
+    if (estimatedPoses.size() == 1) return estimatedPoses.get(0).estimatedPose.toPose2d();
+    
+    if (estimatedPoses.size() == 2) {
+      return new Pose2d(
+        estimatedPoses.get(0).estimatedPose.toPose2d().getTranslation()
+          .plus(estimatedPoses.get(1).estimatedPose.toPose2d().getTranslation())
+          .div(2),
+        
+        new Rotation2d(MathUtils.modulusMidpoint(
+          estimatedPoses.get(0).estimatedPose.toPose2d().getRotation().getRadians(),
+          estimatedPoses.get(1).estimatedPose.toPose2d().getRotation().getRadians(),
+          -Math.PI, Math.PI)
+        )
+      );
+    }
+    
+    // The average translation is just the average of all of the translations (sum divided by total)
+    // Average angle is similar, except every step needs to use a modulus, since -π is the same angle as π
+    // This calculation is essentially newAverage = (oldAverage * valuesInOldAverage + nextValue) / newNumberOfValues
+    Translation2d translation = new Translation2d();
+    double angle = 0;
+    for(int i = 0; i < estimatedPoses.size(); i ++){
+      translation = translation.plus(estimatedPoses.get(i).estimatedPose.toPose2d().getTranslation());
+      angle = MathUtils.modulusInterpolate(angle, estimatedPoses.get(i).estimatedPose.toPose2d().getRotation().getRadians(), 1.0/(i+1), -Math.PI, Math.PI);
+    }
+
+    return new Pose2d(translation.div(estimatedPoses.size()), new Rotation2d(angle));
+  }
+
+  public AprilTagFieldLayout getAprilTagFieldLayout(){
+    return aprilTagFieldLayout;
+  }
+
+  /**
+   * Gets the pose of an april tag
+   * @param id AprilTag id (1-8)
+   * @return Pose3d of the AprilTag
+   */
+  public Pose3d getTagPose(int id){
+    if(id < 1 || id > getAprilTagFieldLayout().getTags().size()){
+      System.out.println("Tried to find the pose of april tag "+id);
+      return null;
+    }
+    return getAprilTagFieldLayout().getTags().get(id-1).pose;
+  }
+
+  /**
+   * Returns where it thinks the robot is
+   * @param referencePose The pose to use as a reference, usually the previous robot pose
+   * @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time
+   * @return An array list of estimated poses, one for each camera that can see an april tag
+   */
+  public ArrayList<EstimatedRobotPose> getEstimatedPoses(Pose2d referencePose) {
+    return getEstimatedPoses(referencePose, ignoree->referencePose.getRotation().getRadians());
+  }
+
+  /**
+   * Returns where it thinks the robot is
+   * @param referencePose The pose to use as a reference, usually the previous robot pose
+   * @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time
+   * @return An array list of estimated poses, one for each camera that can see an april tag
+   */
+  public ArrayList<EstimatedRobotPose> getEstimatedPoses(Pose2d referencePose, DoubleUnaryOperator yawFunction) {
+    ArrayList<EstimatedRobotPose> estimatedPoses = new ArrayList<>();
+    for (int i = 0; i < cameras.size(); i++) {
+      if(VisionConstants.USE_MANUAL_CALCULATIONS){
+        for(EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(yawFunction)){
+          if(pose != null){
+            estimatedPoses.add(pose);
+          }
+        }
+      }else{
+        for(EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(referencePose)){
+          // If the camera can see an april tag that exists, add it to the array list
+          // April tags that don't exist might return a result that is present but doesn't have a pose
+          if (pose.estimatedPose != null) {
+            estimatedPoses.add(pose);
+
+          }
+        }
+      }
+    }
+    if(estimatedPoses.size() > 1){
+      Translation2d average = new Translation2d();
+      for(EstimatedRobotPose pose : estimatedPoses){
+        average = average.plus(pose.estimatedPose.getTranslation().toTranslation2d());
+      }
+      average = average.div(estimatedPoses.size());
+      for(int i = estimatedPoses.size()-1; i>=0; i--){
+        if(estimatedPoses.get(i).estimatedPose.getTranslation().toTranslation2d().getDistance(average) > VisionConstants.MAX_POSE_DIFFERENCE/2){
+          estimatedPoses.remove(i);
+        }
+      }
+    }
+    return estimatedPoses; 
+  }
+
+  /**
+   * Updates the robot's odometry with vision
+   * @param poseEstimator The pose estimator to update
+   * @param yawFunction A function that returns the yaw as a double given the timestamp
+   * @param slipped True if the wheels have slipped, false otherwise
+   */
+  public void updateOdometry(SwerveDrivePoseEstimator poseEstimator, DoubleUnaryOperator yawFunction, boolean slipped){
+    // Simulate vision
+    // 2 ifs to avoid warning
+    if(VisionConstants.ENABLED_SIM){
+      if(RobotBase.isSimulation()){
+        visionSim.update(poseEstimator.getEstimatedPosition());
+      }
+    }
+
+    sawTag = false;
+
+    // An array list of poses returned by different cameras
+    ArrayList<EstimatedRobotPose> estimatedPoses = getEstimatedPoses(poseEstimator.getEstimatedPosition(), yawFunction);
+    for (EstimatedRobotPose estimatedPose : estimatedPoses) {
+      // Continue if this pose doesn't exist
+      if(estimatedPose.timestampSeconds < 0 || !onField(estimatedPose.estimatedPose.toPose2d()) || Timer.getFPGATimestamp() < estimatedPose.timestampSeconds || Timer.getFPGATimestamp() > estimatedPose.timestampSeconds + 1){
+        continue;
+      }
+
+      poseEstimator.addVisionMeasurement(
+        estimatedPose.estimatedPose.toPose2d(),
+        estimatedPose.timestampSeconds,
+        slipped ? VisionConstants.VISION_STD_DEVS_2 : VisionConstants.VISION_STD_DEVS
+      );
+      sawTag = true;
+    }
+  }
+
+  /**
+   * Updates each camera's inputs for logging
+   */
+  public void updateInputs(){
+    for(VisionCamera c : cameras){
+      c.updateInputs();
+    }
+  }
+
+  /**
+   * If vision saw any April tags last frame
+   * @return If vision saw an April tag last frame
+   */
+  public boolean canSeeTag(){
+    return sawTag;
+  }
+
+  /**
+   * Enable or disable a single camera
+   * @param index The camera index
+   * @param enabled If it should be enabled or disabled
+   */
+  public void enableCamera(int index, boolean enabled){
+    try{
+      cameras.get(index).enable(enabled);
+    }catch(IndexOutOfBoundsException e){
+      DriverStation.reportWarning("Camera index "+index+" is out of bounds", false);
+    }
+  }
+  /**
+   * Sets the cameras to only use April tag in the specified array
+   * @param ids The ids of the tags to use, null or empty array to use all
+   */
+  public void onlyUse(int[] ids){
+    onlyUse = ids;
+  }
+
+  /**
+   * Checks if a pose is on the field
+   * @param pose The pose to check
+   * @return If the pose is on the field
+   */
+  public static boolean onField(Pose2d pose){
+    return pose!=null && pose.getX()>0 && pose.getX()<FieldConstants.FIELD_LENGTH && pose.getY()>0 && pose.getY()<FieldConstants.FIELD_WIDTH;
+  }
+
+  /**
+   * Checks if a pose is on or near the field
+   * @param pose The pose to check
+   * @return If the pose is within an area with twice the length and width of the field
+   */
+  public static boolean nearField(Pose2d pose){
+    return pose!=null && pose.getX()>-FieldConstants.FIELD_LENGTH/2 && pose.getX()<FieldConstants.FIELD_LENGTH*1.5 && pose.getY()>-FieldConstants.FIELD_WIDTH/2 && pose.getY()<FieldConstants.FIELD_WIDTH*1.5;
+  }
+  
+  private class VisionCamera implements VisionIO {
+    private PhotonCamera camera;
+    private PhotonPoseEstimator photonPoseEstimator;
+    private Pose2d lastPose;
+    private double lastTimestamp = 0;
+    private boolean enabled = true;
+    private final VisionIOInputs inputs = new VisionIOInputs();
+  
+    /**
+     * Stores information about a camera
+     * @param cameraName The name of the camera on PhotonVision
+     * @param robotToCam The transformation from the robot to the camera
+     */
+    public VisionCamera(String cameraName, Transform3d robotToCam) {
+      camera = new PhotonCamera(cameraName);
+      photonPoseEstimator = new PhotonPoseEstimator(
+        aprilTagFieldLayout, 
+        VisionConstants.POSE_STRATEGY, 
+        robotToCam
+      );
+      photonPoseEstimator.setMultiTagFallbackStrategy(VisionConstants.MULTITAG_FALLBACK_STRATEGY);
+      photonPoseEstimator.setReferencePose(new Pose2d());
+      lastPose = null;
+    }
+  
+    /**
+     * Gets the estimated poses from the camera
+     * @param referencePose Pose to use for reference, usually the previous estimated robot pose
+     * @return estimated robot poses
+     */
+    public ArrayList<EstimatedRobotPose> getEstimatedPose(Pose2d referencePose) {
+      photonPoseEstimator.setReferencePose(referencePose);
+
+      ArrayList<EstimatedRobotPose> list = new ArrayList<>();
+
+      if(!enabled){
+        return list;
+      }
+
+      for(PhotonPipelineResult cameraResult : inputs.results){
+        if(!cameraResult.hasTargets() || cameraResult.getTimestampSeconds()<0){
+            continue;
+        }
+        
+        // if there is a target detected and the timestamp exists, 
+        // check the ambiguity isn't too high
+        List<PhotonTrackedTarget> targetsUsed = cameraResult.targets;
+        for (int i = targetsUsed.size()-1; i >= 0; i--) {
+          // Remove it from the list if it should not be used or if it has too high of an ambiguity
+          if(!useTag(targetsUsed.get(i).getFiducialId()) || targetsUsed.get(i).getPoseAmbiguity() > VisionConstants.HIGHEST_AMBIGUITY || targetsUsed.get(i).bestCameraToTarget.getTranslation().getNorm() > VisionConstants.MAX_DISTANCE){
+            targetsUsed.remove(i);
+          }
+        }
+
+        // If there are no targets, the timestamp doesn't exist, or there there is only 1 tag and the constant is set to only use 2 tags, continue
+        if(targetsUsed.size() == 0 || cameraResult.getTimestampSeconds()<0 || targetsUsed.size()==1 && VisionConstants.ONLY_USE_2_TAGS){
+          continue;
+        }
+
+        // Set strategy to single tag if there is only 1 good tag and update
+        photonPoseEstimator.setPrimaryStrategy(targetsUsed.size() > 1  ? VisionConstants.POSE_STRATEGY : VisionConstants.MULTITAG_FALLBACK_STRATEGY);
+        Optional<EstimatedRobotPose> pose = photonPoseEstimator.update(cameraResult);
+        
+        if(pose.isPresent() && pose.get()!=null && onField(pose.get().estimatedPose.toPose2d())){
+          double timestamp = cameraResult.getTimestampSeconds();
+
+          // If the pose moved too much, don't use it
+          if(lastPose==null || lastPose.getTranslation().getDistance(pose.get().estimatedPose.toPose2d().getTranslation()) > DriveConstants.MAX_SPEED*1.25*(timestamp-lastTimestamp) || timestamp < lastTimestamp){
+            lastPose = pose.get().estimatedPose.toPose2d();
+            lastTimestamp = timestamp;
+            continue;
+          }
+
+          // Otherwise, add the pose to the list
+          lastPose = pose.get().estimatedPose.toPose2d();
+          lastTimestamp = timestamp;
+          list.add(pose.get());
+        }
+      }
+      return list;
+    }
+
+  /**
+   * Updates the VisionIOInputs object with the results from PhotonVision for logging
+   */
+  @Override
+    public void updateInputs() {
+      inputs.connected = camera.isConnected();
+      inputs.results = camera.getAllUnreadResults();
+
+      Logger.processInputs("Vision/"+camera.getName(), inputs);
+
+      // Mechanical Advantage's vision logging
+      // // Read new camera observations
+      // Set<Short> tagIds = new HashSet<>();
+      // List<PoseObservation> poseObservations = new LinkedList<>();
+      // for (var result : camera.getAllUnreadResults()) {
+      //   // Update latest target observation
+      //   if (result.hasTargets()) {
+      //     inputs.latestTargetObservation =
+      //         new TargetObservation(
+      //             Rotation2d.fromDegrees(result.getBestTarget().getYaw()),
+      //             Rotation2d.fromDegrees(result.getBestTarget().getPitch()));
+      //   } else {
+      //     inputs.latestTargetObservation = new TargetObservation(new Rotation2d(), new Rotation2d());
+      //   }
+      // }
+      //   // Add pose observation
+      //   if (result.multitagResult.isPresent()) { // Multitag result
+      //     var multitagResult = result.multitagResult.get();
+
+      //     // Calculate robot pose
+      //     Transform3d fieldToCamera = multitagResult.estimatedPose.best;
+      //     Transform3d fieldToRobot = fieldToCamera.plus(VisionConstants.APRIL_TAG_CAMERAS.get(0).getSecond().inverse());
+      //     Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation());
+
+      //     // Calculate average tag distance
+      //     double totalTagDistance = 0.0;
+      //     for (var target : result.targets) {
+      //       totalTagDistance += target.bestCameraToTarget.getTranslation().getNorm();
+      //     }
+
+      //     // Add tag IDs
+      //     tagIds.addAll(multitagResult.fiducialIDsUsed);
+
+      //     // Add observation
+      //     poseObservations.add(
+      //         new PoseObservation(
+      //             result.getTimestampSeconds(), // Timestamp
+      //             robotPose, // 3D pose estimate
+      //             multitagResult.estimatedPose.ambiguity, // Ambiguity
+      //             multitagResult.fiducialIDsUsed.size(), // Tag count
+      //             totalTagDistance / result.targets.size(), // Average tag distance
+      //             PoseObservationType.PHOTONVISION)); // Observation type
+
+      //   } else if (!result.targets.isEmpty()) { // Single tag result
+      //     var target = result.targets.get(0);
+
+      //     // Calculate robot pose
+      //     var tagPose = aprilTagLayout.getTagPose(target.fiducialId);
+      //     if (tagPose.isPresent()) {
+      //       Transform3d fieldToTarget =
+      //           new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation());
+      //       Transform3d cameraToTarget = target.bestCameraToTarget;
+      //       Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse());
+      //       Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse());
+      //       Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation());
+
+      //       // Add tag ID
+      //       tagIds.add((short) target.fiducialId);
+
+      //       // Add observation
+      //       poseObservations.add(
+      //           new PoseObservation(
+      //               result.getTimestampSeconds(), // Timestamp
+      //               robotPose, // 3D pose estimate
+      //               target.poseAmbiguity, // Ambiguity
+      //               1, // Tag count
+      //               cameraToTarget.getTranslation().getNorm(), // Average tag distance
+      //               PoseObservationType.PHOTONVISION)); // Observation type
+      //     }
+      //   }
+      // }
+    }
+    
+    /**
+     * Gets the pose using manual calculations
+     * @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time
+     * @return A list of estimated poses as EstimatedRobotPoses
+     */
+    public ArrayList<EstimatedRobotPose> getEstimatedPose(DoubleUnaryOperator yawFunction){
+      ArrayList<EstimatedRobotPose> list = new ArrayList<>();
+
+      // Do nothing if this camera is disabled
+      if(!enabled){
+        return list;
+      }
+
+      // The latest camera results
+      for(PhotonPipelineResult result : inputs.results){
+        // TODO: This could be improved by averaging all targets instead of only using 1
+        // Gets the best target to use for the calculations
+        PhotonTrackedTarget target = result.getBestTarget();
+        // Continue if the target doesn't exist or it should be ignored
+        if(target==null){
+          continue;
+        }
+        // Continue if the id is too high or too low
+        int id = target.getFiducialId();
+        if(!useTag(id) || target.bestCameraToTarget.getTranslation().getNorm() > VisionConstants.MAX_DISTANCE || target.poseAmbiguity > VisionConstants.HIGHEST_AMBIGUITY){
+          continue;
+        }
+        // Stores target pose and robot to camera transformation for easy access later
+        Pose3d targetPose = FieldConstants.APRIL_TAGS.get(id-1).pose;
+        Transform3d robotToCamera = photonPoseEstimator.getRobotToCameraTransform();
+
+        double timestamp = result.getTimestampSeconds();
+        double yaw = yawFunction.applyAsDouble(timestamp);
+
+        // Get the tag position relative to the robot, assuming the robot is on the ground
+        Translation3d translation = target.getBestCameraToTarget().getTranslation()
+          .rotateBy(robotToCamera.getRotation());
+        translation = translation//.times((targetPose.getZ()-robotToCamera.getZ())/translation.getZ())
+          .plus(robotToCamera.getTranslation())
+          .rotateBy(new Rotation3d(0, 0, yaw))
+  
+        // Invert it to get the robot position relative to the April tag
+        // Multiply by a constant. I don't know why this works, but it was consistently 10% off in 2023 Fall Semester
+          .times(-VisionConstants.DISTANCE_SCALE)
+        // Get the field relative robot pose
+          .plus(targetPose.getTranslation());
+        try{
+          // Adds an EstimatedRobotPose
+          list.add(new EstimatedRobotPose(
+            new Pose3d(translation.getX(), translation.getY(), 0, new Rotation3d(0, 0, yaw)), 
+            timestamp, 
+            List.of(target),
+            VisionConstants.POSE_STRATEGY
+          ));
+        }catch(Exception e){
+          DriverStation.reportError("Error creating EstimatedRobotPose", true);
+        }
+      }
+      return list;
+    }
+
+    public boolean useTag(int id){
+      // Never use tags that don't exist
+      if(id <= 0 || id > FieldConstants.APRIL_TAGS.size()){
+        return false;
+      }
+      // Return false if it is in the list of tags to ignore
+      for(int id2 : VisionConstants.TAGS_TO_IGNORE){
+        if(id == id2){
+          return false;
+        }
+      }
+      // If it's in the array to only use and not in the array to ignore, return true
+      for(int j = 0; onlyUse != null && j < onlyUse.length; j++){
+        if(id == onlyUse[j]){
+          return true;
+        }
+      }
+      // If it isn't in the array to only use, only reutrn true if the array is empty/null
+      return onlyUse == null || onlyUse.length == 0;
+    }
+
+    /**
+     * Enables or disables this camera
+     * @param enable If it should be enabled or disabled
+     */
+    public void enable(boolean enable){
+      enabled = enable;
+    }
+  }
+}

diff --git a/src/main/java/frc/robot/util/Vision/VisionIO.java b/src/main/java/frc/robot/util/Vision/VisionIO.java
new file mode 100644 (file)
index 0000000..02bb492
--- /dev/null
+++ b/src/main/java/frc/robot/util/Vision/VisionIO.java
@@ -0,0 +1,82 @@
+// Copyright 2021-2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.util.Vision;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+import org.photonvision.targeting.PhotonPipelineResult;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public interface VisionIO {
+  public static class VisionIOInputs implements LoggableInputs {
+    public boolean connected = false;
+    public List<PhotonPipelineResult> results = new ArrayList<>();
+    
+    // PhotonVision should never return more than 5 results, except possibly for very long loop overruns
+    private static final int maxLength = 5;
+
+    private boolean intitalized = false;
+
+    @Override
+    public void toLog(LogTable table) {
+      // LogTable does not easily allow removal of logs, especially ProtobufSerializables, so extra values will need to be ignored
+      // This is not very efficient, since unused values are still taking up memory, but there is no easy way to remove them
+      if (!intitalized){
+        for(int i = 0; i < maxLength; i++){
+          table.put("Results"+i, new PhotonPipelineResult());
+          intitalized = true;
+        }
+      }
+      table.put("Connected", connected);
+      double length = Math.min(results.size(), maxLength);
+      table.put("Length", length);
+      for(int i = 0; i < length; i++){
+        table.put("Results"+i, results.get(i));
+      }
+    }
+
+    @Override
+    public void fromLog(LogTable table) {
+      connected = table.get("Connected", false);
+      int length = table.get("Length", 0);
+      results = new ArrayList<>(length);
+      // Java gets confused when null is used for a generic type argument
+      PhotonPipelineResult nullResult = null;
+      for(int i = 0; i < length; i++){
+        PhotonPipelineResult result = table.get("Results"+i, nullResult);
+        if(result != null){
+          results.add(result);
+        }
+      }
+    }
+  }
+
+  /** Represents the angle to a simple target, not used for pose estimation. */
+  public static record TargetObservation(Rotation2d tx, Rotation2d ty) {}
+
+  /** Represents a robot pose sample used for pose estimation. */
+  public static record PoseObservation(
+      double timestamp,
+      Pose3d pose,
+      double ambiguity,
+      int tagCount,
+      double averageTagDistance) {}
+
+  public default void updateInputs() {}
+}
\ No newline at end of file

diff --git a/src/main/java/lib/COTSFalconSwerveConstants.java b/src/main/java/lib/COTSFalconSwerveConstants.java
new file mode 100644 (file)
index 0000000..d4a9009
--- /dev/null
+++ b/src/main/java/lib/COTSFalconSwerveConstants.java
@@ -0,0 +1,139 @@
+package lib;
+
+import edu.wpi.first.math.util.Units;
+
+/* Contains values and required settings for common COTS swerve modules. */
+public class COTSFalconSwerveConstants {
+    public final double wheelDiameter;
+    public final double wheelCircumference;
+    public final double angleGearRatio;
+    public final double driveGearRatio;
+    public final double angleKP;
+    public final double angleKI;
+    public final double angleKD;
+    public final double angleKF;
+    public final boolean driveMotorInvert;
+    public final boolean angleMotorInvert;
+    public final boolean canCoderInvert;
+
+    public COTSFalconSwerveConstants(double wheelDiameter, double angleGearRatio, double driveGearRatio, double angleKP, double angleKI, double angleKD, double angleKF, boolean driveMotorInvert, boolean angleMotorInvert, boolean canCoderInvert) {
+        this.wheelDiameter = wheelDiameter;
+        this.wheelCircumference = wheelDiameter * Math.PI;
+        this.angleGearRatio = angleGearRatio;
+        this.driveGearRatio = driveGearRatio;
+        this.angleKP = angleKP;
+        this.angleKI = angleKI;
+        this.angleKD = angleKD;
+        this.angleKF = angleKF;
+        this.driveMotorInvert = driveMotorInvert;
+        this.angleMotorInvert = angleMotorInvert;
+        this.canCoderInvert = canCoderInvert;
+    }
+
+    /**
+     * Swerve Drive Specialties - MK3 Module
+     */
+    public static COTSFalconSwerveConstants SDSMK3(double driveGearRatio) {
+        double wheelDiameter = Units.inchesToMeters(4.0);
+
+        /** 12.8 : 1 */
+        double angleGearRatio = (12.8);
+
+        double angleKP = 0.2;
+        double angleKI = 0.0;
+        double angleKD = 0.0;
+        double angleKF = 0.0;
+
+        boolean driveMotorInvert = false;
+        boolean angleMotorInvert = false;
+        boolean canCoderInvert = false;
+        return new COTSFalconSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, angleKF, driveMotorInvert, angleMotorInvert, canCoderInvert);
+    }
+
+    /**
+     * Swerve Drive Specialties - MK4 Module
+     */
+    public static COTSFalconSwerveConstants SDSMK4(double driveGearRatio) {
+        double wheelDiameter = Units.inchesToMeters(4.0);
+
+        /** 12.8 : 1 */
+        double angleGearRatio = (12.8);
+
+        double angleKP = 0.2;
+        double angleKI = 0.0;
+        double angleKD = 0.0;
+        double angleKF = 0.0;
+
+        boolean driveMotorInvert = false;
+        boolean angleMotorInvert = false;
+        boolean canCoderInvert = false;
+        return new COTSFalconSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, angleKF, driveMotorInvert, angleMotorInvert, canCoderInvert);
+    }
+
+    /**
+     * Swerve Drive Specialties - MK4i Module
+     */
+    public static COTSFalconSwerveConstants SDSMK4i(double driveGearRatio) {
+        double wheelDiameter = Units.inchesToMeters(4.0);
+
+        /** (150 / 7) : 1 */
+        double angleGearRatio = ((150.0 / 7.0));
+
+        double angleKP = 0.3;
+        double angleKI = 0.0;
+        double angleKD = 0.0;
+        double angleKF = 0.0;
+
+        boolean driveMotorInvert = false;
+        boolean angleMotorInvert = true;
+        boolean canCoderInvert = false;
+        return new COTSFalconSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, angleKF, driveMotorInvert, angleMotorInvert, canCoderInvert);
+    }
+
+    /* Drive Gear Ratios for all supported modules */
+    public static class DriveGearRatios {
+        /* SDS MK3 */
+        /**
+         * SDS MK3 - 8.16 : 1
+         */
+        public static final double SDSMK3_Standard = (8.16);
+        /**
+         * SDS MK3 - 6.86 : 1
+         */
+        public static final double SDSMK3_Fast = (6.86);
+
+        /* SDS MK4 */
+        /**
+         * SDS MK4 - 8.14 : 1
+         */
+        public static final double SDSMK4_L1 = (8.14);
+        /**
+         * SDS MK4 - 6.75 : 1
+         */
+        public static final double SDSMK4_L2 = (6.75);
+        /**
+         * SDS MK4 - 6.12 : 1
+         */
+        public static final double SDSMK4_L3 = (6.12);
+        /**
+         * SDS MK4 - 5.14 : 1
+         */
+        public static final double SDSMK4_L4 = (5.14);
+
+        /* SDS MK4i */
+        /**
+         * SDS MK4i - 8.14 : 1
+         */
+        public static final double SDSMK4i_L1 = (8.14);
+        /**
+         * SDS MK4i - 6.75 : 1
+         */
+        public static final double SDSMK4i_L2 = (6.75);
+        /**
+         * SDS MK4i - 6.12 : 1
+         */
+        public static final double SDSMK4i_L3 = (6.12);
+    }
+}
+
+  
\ No newline at end of file

diff --git a/src/main/java/lib/CTREModuleState.java b/src/main/java/lib/CTREModuleState.java
new file mode 100644 (file)
index 0000000..8e8005d
--- /dev/null
+++ b/src/main/java/lib/CTREModuleState.java
@@ -0,0 +1,60 @@
+package lib;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+
+public class CTREModuleState {
+
+    /**
+     * Minimize the change in heading the desired swerve module state would require by potentially
+     * reversing the direction the wheel spins. Customized from WPILib's version to include placing
+     * in appropriate scope for CTRE onboard control.
+     *
+     * @param desiredState The desired state.
+     * @param currentAngle The current module angle.
+     */
+    public static SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle) {
+        double targetAngle = placeInAppropriate0To360Scope(currentAngle.getDegrees(), desiredState.angle.getDegrees());
+        double targetSpeed = desiredState.speedMetersPerSecond;
+        double delta = targetAngle - currentAngle.getDegrees();
+        if (Math.abs(delta) > 90) {
+            targetSpeed = -targetSpeed;
+            if (delta > 90) {
+                targetAngle -= 180;
+            } else {
+                targetAngle += 180;
+            }
+        }
+        return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle));
+    }
+
+    /**
+     * @param scopeReference Current Angle
+     * @param newAngle       Target Angle
+     * @return Closest angle within scope
+     */
+    private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) {
+        double lowerBound;
+        double upperBound;
+        double lowerOffset = scopeReference % 360;
+        if (lowerOffset >= 0) {
+            lowerBound = scopeReference - lowerOffset;
+            upperBound = scopeReference + (360 - lowerOffset);
+        } else {
+            upperBound = scopeReference - lowerOffset;
+            lowerBound = scopeReference - (360 + lowerOffset);
+        }
+        while (newAngle < lowerBound) {
+            newAngle += 360;
+        }
+        while (newAngle > upperBound) {
+            newAngle -= 360;
+        }
+        if (newAngle - scopeReference > 180) {
+            newAngle -= 360;
+        } else if (newAngle - scopeReference < -180) {
+            newAngle += 360;
+        }
+        return newAngle;
+    }
+}

diff --git a/src/main/java/lib/PolynomialRegression.java b/src/main/java/lib/PolynomialRegression.java
new file mode 100644 (file)
index 0000000..04d4f65
--- /dev/null
+++ b/src/main/java/lib/PolynomialRegression.java
@@ -0,0 +1,195 @@
+package lib;
+
+import Jama.Matrix;
+import Jama.QRDecomposition;
+
+
+// NOTE: This file is available at
+// http://algs4.cs.princeton.edu/14analysis/PolynomialRegression.java.html
+
+/**
+ * The {@code PolynomialRegression} class performs a polynomial regression on an set of <em>N</em>
+ * data points (<em>y<sub>i</sub></em>, <em>x<sub>i</sub></em>). That is, it fits a polynomial
+ * <em>y</em> = &beta;<sub>0</sub> + &beta;<sub>1</sub> <em>x</em> + &beta;<sub>2</sub>
+ * <em>x</em><sup>2</sup> + ... + &beta;<sub><em>d</em></sub> <em>x</em><sup><em>d</em></sup> (where
+ * <em>y</em> is the response variable, <em>x</em> is the predictor variable, and the
+ * &beta;<sub><em>i</em></sub> are the regression coefficients) that minimizes the sum of squared
+ * residuals of the multiple regression model. It also computes associated the coefficient of
+ * determination <em>R</em><sup>2</sup>.
+ *
+ * <p>This implementation performs a QR-decomposition of the underlying Vandermonde matrix, so it is
+ * neither the fastest nor the most numerically stable way to perform the polynomial regression.
+ *
+ * @author Robert Sedgewick
+ * @author Kevin Wayne
+ */
+public class PolynomialRegression implements Comparable<PolynomialRegression> {
+    private final String variableName; // name of the predictor variable
+    private int degree; // degree of the polynomial regression
+    private final Matrix beta; // the polynomial regression coefficients
+    private final double sse; // sum of squares due to error
+    private double sst; // total sum of squares
+
+    /**
+     * Performs a polynomial regression on the data points {@code (y[i], x[i])}. Uses n as the name
+     * of the predictor variable.
+     *
+     * @param x      the values of the predictor variable
+     * @param y      the corresponding values of the response variable
+     * @param degree the degree of the polynomial to fit
+     * @throws IllegalArgumentException if the lengths of the two arrays are not equal
+     */
+    public PolynomialRegression(double[] x, double[] y, int degree) {
+        this(x, y, degree, "n");
+    }
+
+    /**
+     * Performs a polynomial regression on the data points {@code (y[i], x[i])}.
+     *
+     * @param x            the values of the predictor variable
+     * @param y            the corresponding values of the response variable
+     * @param degree       the degree of the polynomial to fit
+     * @param variableName the name of the predictor variable
+     * @throws IllegalArgumentException if the lengths of the two arrays are not equal
+     */
+    public PolynomialRegression(double[] x, double[] y, int degree, String variableName) {
+        this.degree = degree;
+        this.variableName = variableName;
+
+        int n = x.length;
+        QRDecomposition qr = null;
+        Matrix matrixX = null;
+
+        // in case Vandermonde matrix does not have full rank, reduce degree until it
+        // does
+        while (true) {
+
+            // build Vandermonde matrix
+            double[][] vandermonde = new double[n][this.degree + 1];
+            for (int i = 0; i < n; i++) {
+                for (int j = 0; j <= this.degree; j++) {
+                    vandermonde[i][j] = Math.pow(x[i], j);
+                }
+            }
+            matrixX = new Matrix(vandermonde);
+
+            // find least squares solution
+            qr = new QRDecomposition(matrixX);
+            if (qr.isFullRank()) break;
+
+            // decrease degree and try again
+            this.degree--;
+        }
+
+        // create matrix from vector
+        Matrix matrixY = new Matrix(y, n);
+
+        // linear regression coefficients
+        beta = qr.solve(matrixY);
+
+        // mean of y[] values
+        double sum = 0.0;
+        for (int i = 0; i < n; i++) sum += y[i];
+        double mean = sum / n;
+
+        // total variation to be accounted for
+        for (int i = 0; i < n; i++) {
+            double dev = y[i] - mean;
+            sst += dev * dev;
+        }
+
+        // variation not accounted for
+        Matrix residuals = matrixX.times(beta).minus(matrixY);
+        sse = residuals.norm2() * residuals.norm2();
+    }
+
+    /**
+     * Returns the {@code j}th regression coefficient.
+     *
+     * @param j the index
+     * @return the {@code j}th regression coefficient
+     */
+    public double beta(int j) {
+        // to make -0.0 print as 0.0
+        if (Math.abs(beta.get(j, 0)) < 1E-4) return 0.0;
+        return beta.get(j, 0);
+    }
+
+    /**
+     * Returns the degree of the polynomial to fit.
+     *
+     * @return the degree of the polynomial to fit
+     */
+    public int degree() {
+        return degree;
+    }
+
+    /**
+     * Returns the coefficient of determination <em>R</em><sup>2</sup>.
+     *
+     * @return the coefficient of determination <em>R</em><sup>2</sup>, which is a real number between
+     * 0 and 1
+     */
+    public double R2() {
+        if (sst == 0.0) return 1.0; // constant function
+        return 1.0 - sse / sst;
+    }
+
+    /**
+     * Returns the expected response {@code y} given the value of the predictor variable {@code x}.
+     *
+     * @param x the value of the predictor variable
+     * @return the expected response {@code y} given the value of the predictor variable {@code x}
+     */
+    public double predict(double x) {
+        // horner's method
+        double y = 0.0;
+        for (int j = degree; j >= 0; j--) y = beta(j) + (x * y);
+        return y;
+    }
+
+    /**
+     * Returns a string representation of the polynomial regression model.
+     *
+     * @return a string representation of the polynomial regression model, including the best-fit
+     * polynomial and the coefficient of determination <em>R</em><sup>2</sup>
+     */
+    public String toString() {
+        StringBuilder s = new StringBuilder();
+        int j = degree;
+
+        // ignoring leading zero coefficients
+        while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--;
+
+        // create remaining terms
+        while (j >= 0) {
+            if (j == 0) s.append(String.format("%.4f ", beta(j)));
+            else if (j == 1) s.append(String.format("%.4f %s + ", beta(j), variableName));
+            else s.append(String.format("%.4f %s^%d + ", beta(j), variableName, j));
+            j--;
+        }
+        s = s.append("  (R^2 = " + String.format("%.3f", R2()) + ")");
+
+        // replace "+ -2n" with "- 2n"
+        return s.toString().replace("+ -", "- ");
+    }
+
+    /**
+     * Compare lexicographically.
+     */
+    public int compareTo(PolynomialRegression that) {
+        double EPSILON = 1E-5;
+        int maxDegree = Math.max(this.degree(), that.degree());
+        for (int j = maxDegree; j >= 0; j--) {
+            double term1 = 0.0;
+            double term2 = 0.0;
+            if (this.degree() >= j) term1 = this.beta(j);
+            if (that.degree() >= j) term2 = that.beta(j);
+            if (Math.abs(term1) < EPSILON) term1 = 0.0;
+            if (Math.abs(term2) < EPSILON) term2 = 0.0;
+            if (term1 < term2) return -1;
+            else if (term1 > term2) return +1;
+        }
+        return 0;
+    }
+}

diff --git a/src/main/java/lib/controllers/Controller.java b/src/main/java/lib/controllers/Controller.java
index 8fba25cc0c2e853c76c840f8740b80d588e0b5e2..2b1dfb70d1fe6159c381c73f9f97d4850b13ca5a 100644 (file)
--- a/src/main/java/lib/controllers/Controller.java
+++ b/src/main/java/lib/controllers/Controller.java
@@ -1,11 +1,18 @@
 package lib.controllers;
 
 import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
+import java.util.function.BooleanSupplier;
 
 public class Controller {
-  protected final Joystick m_controller;
+    protected final Joystick controller;
+
+    public Controller(int port) {
+        this.controller = new Joystick(port);
+    }
 
-  public Controller(int port) {
-    this.m_controller = new Joystick(port);
-  }
+    public Trigger get(BooleanSupplier sup) {
+        return new Trigger(sup);
+    }
 }

diff --git a/src/main/java/lib/controllers/Ex3DProController.java b/src/main/java/lib/controllers/Ex3DProController.java
index 7d09c52d4f0cb439c000eea09a4dc81c04779581..29d13779442967e154be33b65988f4a3377d3335 100644 (file)
--- a/src/main/java/lib/controllers/Ex3DProController.java
+++ b/src/main/java/lib/controllers/Ex3DProController.java
@@ -1,55 +1,77 @@
 package lib.controllers;
 
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj2.command.button.*;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 
 public class Ex3DProController extends Controller {
-  public Ex3DProController(int port) {
-    super(port);
-  }
-
-  public enum Button {
-    B1(1),
-    B2(2),
-    B3(3),
-    B4(4),
-    B6(6),
-    B7(7),
-    B8(8),
-    B9(9),
-    B10(10),
-    B11(11),
-    B12(12);
-
-    public final int id;
-
-    Button(final int id) {
-      this.id = id;
-    }
-  }
-
-  public enum Axis {
-    X(0),
-    Y(1),
-    Z(2),
-    SLIDER(3);
-
-    public final int id;
-
-    Axis(final int id) {
-      this.id = id;
-    }
-  }
-
-  public JoystickButton get(Button button) {
-    return new JoystickButton(m_controller, button.id);
-  }
-
-  public double get(Axis axis) {
-    return m_controller.getRawAxis(axis.id);
-  }
-
-  public Joystick get() {
-    return m_controller;
-  }
+    public Ex3DProController(int port) {
+        super(port);
+    }
+
+    public enum Ex3DProButton {
+        B1(1),
+        B2(2),
+        B3(3),
+        B4(4),
+        B6(6),
+        B7(7),
+        B8(8),
+        B9(9),
+        B10(10),
+        B11(11),
+        B12(12);
+
+        public final int id;
+
+        Ex3DProButton(final int id) {
+            this.id = id;
+        }
+    }
+
+    public enum Ex3DProAxis {
+        X(0),
+        Y(1),
+        Z(2),
+        SLIDER(3);
+
+        public final int id;
+
+        Ex3DProAxis(final int id) {
+            this.id = id;
+        }
+    }
+
+    public enum Ex3DProHatSwitch {
+        UNPRESSED(-1),
+        UP(0),
+        UP_RIGHT(45),
+        RIGHT(90),
+        DOWN_RIGHT(135),
+        DOWN(180),
+        DOWN_LEFT(235),
+        LEFT(270),
+        UP_LEFT(315);
+
+        public final int angle;
+
+        Ex3DProHatSwitch(final int angle) {
+            this.angle = angle;
+        }
+    }
+
+    public Trigger get(Ex3DProButton button) {
+        return new Trigger(() -> controller.getRawButton(button.id));
+    }
+
+    public double get(Ex3DProAxis axis) {
+        return controller.getRawAxis(axis.id);
+    }
+
+    public Trigger get(Ex3DProHatSwitch hatSwitch) {
+        return new Trigger(() -> controller.getPOV() == hatSwitch.angle);
+    }
+
+    public Joystick get() {
+        return controller;
+    }
 }

diff --git a/src/main/java/lib/controllers/GameController.java b/src/main/java/lib/controllers/GameController.java
index 01e81196e52f7ba6e3a0766d3c0fa6939a8f9365..37f93475ab33874f6f38152bf6f9fef0d6b65ac0 100644 (file)
--- a/src/main/java/lib/controllers/GameController.java
+++ b/src/main/java/lib/controllers/GameController.java
@@ -2,111 +2,115 @@ package lib.controllers;
 
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj2.command.button.*;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
 import java.util.function.BooleanSupplier;
 
 public class GameController extends Controller {
-  // These are the different controller triggers
-  public final BooleanSupplier LEFT_TRIGGER_BUTTON = () -> get(Axis.LEFT_TRIGGER) > 0.5,
-      RIGHT_TRIGGER_BUTTON = () -> get(Axis.RIGHT_TRIGGER) > 0.5;
-  public final Trigger ALL_UP = get(DPad.UP).or(get(DPad.UP_LEFT)).or(get(DPad.UP_RIGHT)),
-      ALL_DOWN = get(DPad.DOWN).or(get(DPad.DOWN_LEFT)).or(get(DPad.DOWN_RIGHT)),
-      ALL_LEFT = get(DPad.LEFT).or(get(DPad.UP_LEFT)).or(get(DPad.DOWN_LEFT)),
-      ALL_RIGHT = get(DPad.RIGHT).or(get(DPad.UP_RIGHT)).or(get(DPad.DOWN_RIGHT));
-
-  public GameController(int port) {
-    super(port);
-  }
-
-  public enum Button {
-    A(1),
-    B(2),
-    X(3),
-    Y(4),
-    LB(5),
-    RB(6),
-    BACK(7),
-    START(8),
-    LEFT_JOY(9),
-    RIGHT_JOY(10);
-
-    public final int id;
-
-    Button(final int id) {
-      this.id = id;
+    // These are the different controller triggers
+    public final BooleanSupplier LEFT_TRIGGER_BUTTON = () -> get(Axis.LEFT_TRIGGER) > 0.5,
+            RIGHT_TRIGGER_BUTTON = () -> get(Axis.RIGHT_TRIGGER) > 0.5;
+    public final Trigger ALL_UP = get(DPad.UP).or(get(DPad.UP_LEFT)).or(get(DPad.UP_RIGHT)),
+            ALL_DOWN = get(DPad.DOWN).or(get(DPad.DOWN_LEFT)).or(get(DPad.DOWN_RIGHT)),
+            ALL_LEFT = get(DPad.LEFT).or(get(DPad.UP_LEFT)).or(get(DPad.DOWN_LEFT)),
+            ALL_RIGHT = get(DPad.RIGHT).or(get(DPad.UP_RIGHT)).or(get(DPad.DOWN_RIGHT));
+
+    public final BooleanSupplier
+            LEFT_STICK_LEFT = () -> get(Axis.LEFT_X) < -0.75,
+            LEFT_STICK_RIGHT = () -> get(Axis.LEFT_X) > 0.75,
+            LEFT_STICK_UP = () -> get(Axis.LEFT_Y) < -0.75,
+            LEFT_STICK_DOWN = () -> get(Axis.LEFT_Y) > 0.75;
+    public final BooleanSupplier
+            RIGHT_STICK_LEFT = () -> get(Axis.RIGHT_X) < -0.75,
+            RIGHT_STICK_RIGHT = () -> get(Axis.RIGHT_X) > 0.75,
+            RIGHT_STICK_UP = () -> get(Axis.RIGHT_Y) < -0.75,
+            RIGHT_STICK_DOWN = () -> get(Axis.RIGHT_Y) > 0.75;
+
+    public GameController(int port) {
+        super(port);
+    }
+
+    public enum Button {
+        A(1),
+        B(2),
+        X(3),
+        Y(4),
+        LB(5),
+        RB(6),
+        BACK(7),
+        START(8),
+        LEFT_JOY(9),
+        RIGHT_JOY(10);
+
+        public final int id;
+
+        Button(final int id) {
+            this.id = id;
+        }
     }
-  }
 
-  public enum Axis {
-    LEFT_X(0),
-    LEFT_Y(1),
-    LEFT_TRIGGER(2),
-    RIGHT_TRIGGER(3),
-    RIGHT_X(4),
-    RIGHT_Y(5);
+    public enum Axis {
+        LEFT_X(0),
+        LEFT_Y(1),
+        LEFT_TRIGGER(2),
+        RIGHT_TRIGGER(3),
+        RIGHT_X(4),
+        RIGHT_Y(5);
 
-    public final int id;
+        public final int id;
 
-    Axis(final int id) {
-      this.id = id;
+        Axis(final int id) {
+            this.id = id;
+        }
     }
-  }
-
-  public enum DPad {
-    UNPRESSED(-1),
-    UP(0),
-    UP_RIGHT(45),
-    RIGHT(90),
-    DOWN_RIGHT(135),
-    DOWN(180),
-    DOWN_LEFT(235),
-    LEFT(270),
-    UP_LEFT(315);
-
-    public final int angle;
-
-    DPad(final int angle) {
-      this.angle = angle;
+
+    public enum DPad {
+        UNPRESSED(-1),
+        UP(0),
+        UP_RIGHT(45),
+        RIGHT(90),
+        DOWN_RIGHT(135),
+        DOWN(180),
+        DOWN_LEFT(235),
+        LEFT(270),
+        UP_LEFT(315);
+
+        public final int angle;
+
+        DPad(final int angle) {
+            this.angle = angle;
+        }
     }
-  }
 
-  public enum RumbleStatus {
-    RUMBLE_ON(0.7),
-    RUMBLE_OFF(0);
+    public enum RumbleStatus {
+        RUMBLE_ON(0.7),
+        RUMBLE_OFF(0);
 
-    public final double rumbleValue;
+        public final double rumbleValue;
 
-    RumbleStatus(final double rumbleValue) {
-      this.rumbleValue = rumbleValue;
+        RumbleStatus(final double rumbleValue) {
+            this.rumbleValue = rumbleValue;
+        }
     }
-  }
-
-  public JoystickButton get(Button button) {
-    return new JoystickButton(m_controller, button.id);
-  }
-
-  public double get(Axis axis) {
-    return m_controller.getRawAxis(axis.id);
-  }
 
-  public POVButton get(DPad dPad) {
-    return new POVButton(m_controller, dPad.angle);
-  }
+    public Trigger get(Button button) {
+        return new Trigger(() -> controller.getRawButton(button.id));
+    }
 
-  public Trigger get(BooleanSupplier condition) {
-    return new Trigger(condition);
-  }
+    public double get(Axis axis) {
+        return controller.getRawAxis(axis.id);
+    }
 
-  public Trigger get(Trigger trigger) {
-    return trigger;
-  }
+    public Trigger get(DPad dPad) {
+        return new Trigger(() -> controller.getPOV() == dPad.angle);
+    }
 
-  public Joystick get() {
-    return m_controller;
-  }
+    public Joystick get() {
+        return controller;
+    }
 
-  public void setRumble(RumbleStatus rumbleStatus) {
-    m_controller.setRumble(RumbleType.kLeftRumble, rumbleStatus.rumbleValue);
-    m_controller.setRumble(RumbleType.kRightRumble, rumbleStatus.rumbleValue);
-  }
-}
+    public void setRumble(RumbleStatus rumbleStatus) {
+        controller.setRumble(RumbleType.kLeftRumble, rumbleStatus.rumbleValue);
+        controller.setRumble(RumbleType.kRightRumble, rumbleStatus.rumbleValue);
+    }
+}
\ No newline at end of file

diff --git a/src/main/java/lib/controllers/MadCatzController.java b/src/main/java/lib/controllers/MadCatzController.java
index 0fbd04c43f864e88ea29cb5be37ffbf4e9a9ebed..c94ae6976ed80d6986f7523d2572e5d399e52e80 100644 (file)
--- a/src/main/java/lib/controllers/MadCatzController.java
+++ b/src/main/java/lib/controllers/MadCatzController.java
@@ -1,82 +1,78 @@
 package lib.controllers;
 
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj2.command.button.*;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 
 public class MadCatzController extends Controller {
-  public final Trigger
-      ALL_UP = get(HatSwitch.UP).or(get(HatSwitch.UP_LEFT)).or(get(HatSwitch.UP_RIGHT)),
-      ALL_DOWN = get(HatSwitch.DOWN).or(get(HatSwitch.DOWN_LEFT)).or(get(HatSwitch.DOWN_RIGHT)),
-      ALL_LEFT = get(HatSwitch.LEFT).or(get(HatSwitch.UP_LEFT)).or(get(HatSwitch.DOWN_LEFT)),
-      ALL_RIGHT = get(HatSwitch.RIGHT).or(get(HatSwitch.UP_RIGHT)).or(get(HatSwitch.DOWN_RIGHT));
-
-  public MadCatzController(int port) {
-    super(port);
-  }
-
-  public enum Button {
-    B1(1),
-    B2(2),
-    B3(3),
-    B4(4),
-    B6(6),
-    B7(7);
-
-    public final int id;
-
-    Button(final int id) {
-      this.id = id;
+    public final Trigger
+            ALL_UP = get(MadCatzHatSwitch.UP).or(get(MadCatzHatSwitch.UP_LEFT)).or(get(MadCatzHatSwitch.UP_RIGHT)),
+            ALL_DOWN = get(MadCatzHatSwitch.DOWN).or(get(MadCatzHatSwitch.DOWN_LEFT)).or(get(MadCatzHatSwitch.DOWN_RIGHT)),
+            ALL_LEFT = get(MadCatzHatSwitch.LEFT).or(get(MadCatzHatSwitch.UP_LEFT)).or(get(MadCatzHatSwitch.DOWN_LEFT)),
+            ALL_RIGHT = get(MadCatzHatSwitch.RIGHT).or(get(MadCatzHatSwitch.UP_RIGHT)).or(get(MadCatzHatSwitch.DOWN_RIGHT));
+
+    public MadCatzController(int port) {
+        super(port);
     }
-  }
 
-  public enum Axis {
-    X(0),
-    Y(1),
-    ZAXIS(2),
-    ZROTATE(3);
+    public enum MadCatzButton {
+        B1(1),
+        B2(2),
+        B3(3),
+        B4(4),
+        B6(6),
+        B7(7);
 
-    public final int id;
+        public final int id;
 
-    Axis(final int id) {
-      this.id = id;
+        MadCatzButton(final int id) {
+            this.id = id;
+        }
     }
-  }
-
-  public enum HatSwitch {
-    UNPRESSED(-1),
-    UP(0),
-    UP_RIGHT(45),
-    RIGHT(90),
-    DOWN_RIGHT(135),
-    DOWN(180),
-    DOWN_LEFT(235),
-    LEFT(270),
-    UP_LEFT(315);
-
-    public final int angle;
-
-    HatSwitch(final int angle) {
-      this.angle = angle;
+
+    public enum MadCatzAxis {
+        X(0),
+        Y(1),
+        SLIDER(2),
+        ZROTATE(3);
+
+        public final int id;
+
+        MadCatzAxis(final int id) {
+            this.id = id;
+        }
     }
-  }
 
-  public JoystickButton get(Button button) {
-    return new JoystickButton(m_controller, button.id);
-  }
+    public enum MadCatzHatSwitch {
+        UNPRESSED(-1),
+        UP(0),
+        UP_RIGHT(45),
+        RIGHT(90),
+        DOWN_RIGHT(135),
+        DOWN(180),
+        DOWN_LEFT(235),
+        LEFT(270),
+        UP_LEFT(315);
+
+        public final int angle;
+
+        MadCatzHatSwitch(final int angle) {
+            this.angle = angle;
+        }
+    }
 
-  public double get(Axis axis) {
-    return m_controller.getRawAxis(axis.id);
-  }
+    public Trigger get(MadCatzButton button) {
+        return new Trigger(() -> controller.getRawButton(button.id));
+    }
 
-  public POVButton get(HatSwitch hatSwitch) {
-    return new POVButton(m_controller, hatSwitch.angle);
-  }
+    public double get(MadCatzAxis axis) {
+        return controller.getRawAxis(axis.id);
+    }
 
-  public Trigger get(Trigger trigger) {
-    return trigger;
-  }
+    public Trigger get(MadCatzHatSwitch hatSwitch) {
+        return new Trigger(() -> controller.getPOV() == hatSwitch.angle);
+    }
 
-  public Joystick get() {
-    return m_controller;
-  }
+    public Joystick get() {
+        return controller;
+    }
 }

diff --git a/src/main/java/lib/controllers/PS5Controller.java b/src/main/java/lib/controllers/PS5Controller.java
new file mode 100644 (file)
index 0000000..8f45a52
--- /dev/null
+++ b/src/main/java/lib/controllers/PS5Controller.java
@@ -0,0 +1,115 @@
+package lib.controllers;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
+import java.util.function.BooleanSupplier;
+
+public class PS5Controller extends Controller {
+    // These are the different controller triggers
+    public final Trigger ALL_UP = get(DPad.UP).or(get(DPad.UP_LEFT)).or(get(DPad.UP_RIGHT)),
+            ALL_DOWN = get(DPad.DOWN).or(get(DPad.DOWN_LEFT)).or(get(DPad.DOWN_RIGHT)),
+            ALL_LEFT = get(DPad.LEFT).or(get(DPad.UP_LEFT)).or(get(DPad.DOWN_LEFT)),
+            ALL_RIGHT = get(DPad.RIGHT).or(get(DPad.UP_RIGHT)).or(get(DPad.DOWN_RIGHT));
+
+    public final BooleanSupplier
+            LEFT_STICK_LEFT = () -> get(PS5Axis.LEFT_X) < -0.75,
+            LEFT_STICK_RIGHT = () -> get(PS5Axis.LEFT_X) > 0.75,
+            LEFT_STICK_UP = () -> get(PS5Axis.LEFT_Y) < -0.75,
+            LEFT_STICK_DOWN = () -> get(PS5Axis.LEFT_Y) > 0.75;
+    public final BooleanSupplier
+            RIGHT_STICK_LEFT = () -> get(PS5Axis.RIGHT_X) < -0.75,
+            RIGHT_STICK_RIGHT = () -> get(PS5Axis.RIGHT_X) > 0.75,
+            RIGHT_STICK_UP = () -> get(PS5Axis.RIGHT_Y) < -0.75,
+            RIGHT_STICK_DOWN = () -> get(PS5Axis.RIGHT_Y) > 0.75;
+
+    public PS5Controller(int port) {
+        super(port);
+    }
+
+    public enum PS5Button {
+        SQUARE(1),
+        CROSS(2),
+        CIRCLE(3),
+        TRIANGLE(4),
+        LB(5),
+        RB(6),
+        LEFT_TRIGGER(7),
+        RIGHT_TRIGGER(8),
+        CREATE(9),
+        OPTIONS(10),
+        LEFT_JOY(11),
+        RIGHT_JOY(12),
+        PS(13),
+        TOUCHPAD(14),
+        MUTE(15);
+
+        public final int id;
+
+        PS5Button(final int id) {
+            this.id = id;
+        }
+    }
+
+    public enum PS5Axis {
+        LEFT_X(0),
+        LEFT_Y(1),
+        RIGHT_X(2),
+        /**
+         * note: ps5 controller trigger goes from -1 when unpressed, to 1 when fully pressed
+         */
+        LEFT_TRIGGER(3),
+        /**
+         * note: ps5 controller trigger goes from -1 when unpressed, to 1 when fully pressed
+         */
+        RIGHT_TRIGGER(4),
+        RIGHT_Y(5);
+
+        public final int id;
+
+        PS5Axis(final int id) {
+            this.id = id;
+        }
+    }
+
+    public enum DPad {
+        UNPRESSED(-1),
+        UP(0),
+        UP_RIGHT(45),
+        RIGHT(90),
+        DOWN_RIGHT(135),
+        DOWN(180),
+        DOWN_LEFT(235),
+        LEFT(270),
+        UP_LEFT(315);
+
+        public final int angle;
+
+        DPad(final int angle) {
+            this.angle = angle;
+        }
+    }
+
+    public Trigger get(PS5Button button) {
+        return new Trigger(() -> controller.getRawButton(button.id));
+    }
+
+    public double get(PS5Axis axis) {
+        return controller.getRawAxis(axis.id);
+    }
+
+    public Trigger get(DPad dPad) {
+        return new Trigger(() -> controller.getPOV() == dPad.angle);
+    }
+
+    public Joystick get() {
+        return controller;
+    }
+    public void rumbleOn(){
+        controller.setRumble(RumbleType.kBothRumble,0.1);
+    }
+    public void rumbleOff(){
+        controller.setRumble(RumbleType.kBothRumble,0);
+    }
+}
\ No newline at end of file

diff --git a/src/main/java/lib/controllers/PistolController.java b/src/main/java/lib/controllers/PistolController.java
index 0acb6afdd9bfaae3e9ccda82a822f761b9d8fe74..b7d1123026cc53b85966c2927201ff367480a87c 100644 (file)
--- a/src/main/java/lib/controllers/PistolController.java
+++ b/src/main/java/lib/controllers/PistolController.java
@@ -1,57 +1,52 @@
 package lib.controllers;
 
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 
 public class PistolController extends Controller {
-  public final Trigger TOP_BACK_ONLY = get(Button.TOP_BACK).and(get(Button.TOP_FRONT).negate()),
-      TOP_FRONT_ONLY = get(Button.TOP_FRONT).and(get(Button.TOP_BACK).negate()),
-      BOTTOM_BACK_ONLY = get(Button.BOTTOM_BACK).and(get(Button.BOTTOM_FRONT).negate()),
-      BOTTOM_FRONT_ONLY = get(Button.BOTTOM_FRONT).and(get(Button.BOTTOM_BACK).negate());
-
-  public PistolController(int port) {
-    super(port);
-  }
-
-  public enum Button {
-    TOP_BACK(1),
-    TOP_FRONT(2),
-    BOTTOM_FRONT(3),
-    BOTTOM_BACK(4),
-    BOTTOM(5);
-
-    public final int id;
-
-    Button(final int id) {
-      this.id = id;
+    public final Trigger TOP_BACK_ONLY = get(Button.TOP_BACK).and(get(Button.TOP_FRONT).negate()),
+            TOP_FRONT_ONLY = get(Button.TOP_FRONT).and(get(Button.TOP_BACK).negate()),
+            BOTTOM_BACK_ONLY = get(Button.BOTTOM_BACK).and(get(Button.BOTTOM_FRONT).negate()),
+            BOTTOM_FRONT_ONLY = get(Button.BOTTOM_FRONT).and(get(Button.BOTTOM_BACK).negate());
+
+    public PistolController(int port) {
+        super(port);
     }
-  }
 
-  public enum Axis {
-    WHEEL(0),
-    TRIGGER(1);
+    public enum Button {
+        TOP_BACK(1),
+        TOP_FRONT(2),
+        BOTTOM_FRONT(3),
+        BOTTOM_BACK(4),
+        BOTTOM(5);
 
-    public final int id;
+        public final int id;
 
-    Axis(final int id) {
-      this.id = id;
+        Button(final int id) {
+            this.id = id;
+        }
     }
-  }
 
-  public JoystickButton get(Button button) {
-    return new JoystickButton(m_controller, button.id);
-  }
+    public enum Axis {
+        WHEEL(0),
+        TRIGGER(1);
+
+        public final int id;
 
-  public double get(Axis axis) {
-    return m_controller.getRawAxis(axis.id);
-  }
+        Axis(final int id) {
+            this.id = id;
+        }
+    }
 
-  public Trigger get(Trigger trigger) {
-    return trigger;
-  }
+    public Trigger get(Button button) {
+        return new Trigger(() -> controller.getRawButton(button.id));
+    }
 
-  public Joystick get() {
-    return m_controller;
-  }
+    public double get(Axis axis) {
+        return controller.getRawAxis(axis.id);
+    }
+
+    public Joystick get() {
+        return controller;
+    }
 }

diff --git a/src/main/java/lib/ctre_shims/PhoenixMotorControllerGroup.java b/src/main/java/lib/ctre_shims/PhoenixMotorControllerGroup.java
deleted file mode 100644 (file)
index 28b6371..0000000
--- a/src/main/java/lib/ctre_shims/PhoenixMotorControllerGroup.java
+++ /dev/null
@@ -1,110 +0,0 @@
-package lib.ctre_shims;
-
-import com.ctre.phoenix.motorcontrol.InvertType;
-import com.ctre.phoenix.motorcontrol.can.BaseMotorController;
-import edu.wpi.first.util.sendable.Sendable;
-import edu.wpi.first.util.sendable.SendableBuilder;
-import edu.wpi.first.util.sendable.SendableRegistry;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-import java.util.Arrays;
-
-/**
- * Allows multiple {@link MotorController} objects to be linked together.
- *
- * <p>Reimplementation with supports CTRE CAN follower functionality.
- */
-public class PhoenixMotorControllerGroup implements MotorController, Sendable, AutoCloseable {
-  private final MotorController[] m_motorControllers;
-  private final MotorController m_leadMotorController;
-  private static int instances;
-
-  /**
-   * Create a new PhoenixMotorControllerGroup with the provided MotorControllers.
-   *
-   * @param leadMotorController The lead MotorController to add
-   * @param motorControllers The MotorControllers to add
-   */
-  public PhoenixMotorControllerGroup(
-      MotorController leadMotorController, MotorController... motorControllers) {
-    boolean validArgumentTypes = leadMotorController instanceof BaseMotorController;
-    for (MotorController motorController : motorControllers) {
-      validArgumentTypes = validArgumentTypes && (motorController instanceof BaseMotorController);
-    }
-
-    if (!validArgumentTypes) {
-      throw new IllegalArgumentException(
-          "One or more MotorControllers do not inherit from BaseMotorController, i.e. are not CTRE classes");
-    }
-    m_leadMotorController = leadMotorController;
-    m_motorControllers = Arrays.copyOf(motorControllers, motorControllers.length);
-    init();
-  }
-
-  private void init() {
-    SendableRegistry.addChild(this, m_leadMotorController);
-    for (MotorController controller : m_motorControllers) {
-      SendableRegistry.addChild(this, controller);
-      ((BaseMotorController) controller).follow((BaseMotorController) m_leadMotorController);
-      ((BaseMotorController) controller).setInverted(InvertType.FollowMaster);
-    }
-    instances++;
-    SendableRegistry.addLW(this, "PhoenixMotorControllerGroup", instances);
-  }
-
-  @Override
-  public void close() {
-    SendableRegistry.remove(this);
-  }
-
-  @Override
-  public void set(double speed) {
-    m_leadMotorController.set(speed);
-  }
-
-  @Override
-  public double get() {
-    return m_leadMotorController.get();
-  }
-
-  @Override
-  public void setInverted(boolean isInverted) {
-    m_leadMotorController.setInverted(isInverted);
-  }
-
-  @Override
-  public boolean getInverted() {
-    return m_leadMotorController.getInverted();
-  }
-
-  @Override
-  public void disable() {
-    m_leadMotorController.disable();
-    for (MotorController motorController : m_motorControllers) {
-      motorController.disable();
-    }
-  }
-
-  @Override
-  public void stopMotor() {
-    m_leadMotorController.stopMotor();
-    for (MotorController motorController : m_motorControllers) {
-      motorController.stopMotor();
-    }
-  }
-
-  @Override
-  public void initSendable(SendableBuilder builder) {
-    builder.setSmartDashboardType("Phoenix Motor Controller");
-    builder.setActuator(true);
-    builder.setSafeState(this::stopMotor);
-    builder.addDoubleProperty("Value", this::get, this::set);
-  }
-
-  @Override
-  public void setVoltage(double outputVolts) {
-    m_leadMotorController.setVoltage(outputVolts);
-    for (MotorController motorController : m_motorControllers) {
-      motorController.setVoltage(outputVolts);
-    }
-  }
-}

diff --git a/src/main/java/lib/ctre_shims/TalonEncoder.java b/src/main/java/lib/ctre_shims/TalonEncoder.java
deleted file mode 100644 (file)
index 1c7de84..0000000
--- a/src/main/java/lib/ctre_shims/TalonEncoder.java
+++ /dev/null
@@ -1,181 +0,0 @@
-package lib.ctre_shims;
-
-import com.ctre.phoenix.ParamEnum;
-import com.ctre.phoenix.motorcontrol.FeedbackDevice;
-import com.ctre.phoenix.motorcontrol.can.BaseTalon;
-import edu.wpi.first.util.sendable.Sendable;
-import edu.wpi.first.util.sendable.SendableBuilder;
-import edu.wpi.first.util.sendable.SendableRegistry;
-
-/** A class to read encoder data from CTRE motors, frc::Encoder compatible. */
-public class TalonEncoder implements Sendable, AutoCloseable {
-  private final BaseTalon m_motor;
-  private double m_distancePerPulse = 1;
-
-  public TalonEncoder(BaseTalon motor) {
-    this(motor, false);
-  }
-
-  public TalonEncoder(BaseTalon motor, boolean reverseDirection) {
-    m_motor = motor;
-    setReverseDirection(reverseDirection);
-    SendableRegistry.addLW(this, "Talon Encoder", motor.getDeviceID());
-  }
-
-  @Override
-  public void close() {
-    SendableRegistry.remove(this);
-  }
-
-  /**
-   * Gets the current count. Returns the current count on the Encoder. This method compensates for
-   * the decoding type.
-   *
-   * @return Current count from the Encoder adjusted for the 1x, 2x, or 4x scale factor.
-   */
-  public int get() {
-    return (int) m_motor.getSelectedSensorPosition();
-  }
-
-  /** Reset the Encoder distance to zero. Resets the current count to zero on the encoder. */
-  public void reset() {
-    m_motor.setSelectedSensorPosition(0);
-  }
-
-  /**
-   * Returns the period of the most recent pulse. Returns the period of the most recent Encoder
-   * pulse in seconds. This method compensates for the decoding type.
-   *
-   * <p><b>Warning:</b> This returns unscaled periods. Use getRate() for rates that are scaled using
-   * the value from setDistancePerPulse().
-   *
-   * @return Period in seconds of the most recent pulse.
-   * @deprecated Use getRate() in favor of this method.
-   */
-  @Deprecated
-  public double getPeriod() {
-    // distance / (distance / second) = seconds
-    return m_distancePerPulse / getRate();
-  }
-
-  /**
-   * The last direction the encoder value changed.
-   *
-   * @return The last direction the encoder value changed.
-   */
-  public boolean getDirection() {
-    if (getRate() >= 0) {
-      return true;
-    } else {
-      return false;
-    }
-  }
-
-  /**
-   * Get the distance the robot has driven since the last reset as scaled by the value from {@link
-   * #setDistancePerPulse(double)}.
-   *
-   * @return The distance driven since the last reset
-   */
-  public double getDistance() {
-    return get() * m_distancePerPulse;
-  }
-
-  /**
-   * Get the current rate of the encoder. Units are distance per second as scaled by the value from
-   * setDistancePerPulse().
-   *
-   * @return The current rate of the encoder.
-   */
-  public double getRate() {
-    return m_motor.getSelectedSensorVelocity() * 10 * m_distancePerPulse;
-  }
-
-  /**
-   * Set the distance per pulse for this encoder. This sets the multiplier used to determine the
-   * distance driven based on the count value from the encoder. Do not include the decoding type in
-   * this scale. The library already compensates for the decoding type. Set this value based on the
-   * encoder's rated Pulses per Revolution and factor in gearing reductions following the encoder
-   * shaft. This distance can be in any units you like, linear or angular.
-   *
-   * @param distancePerPulse The scale factor that will be used to convert pulses to useful units.
-   */
-  public void setDistancePerPulse(double distancePerPulse) {
-    m_distancePerPulse = distancePerPulse;
-  }
-
-  /**
-   * Get the distance per pulse for this encoder.
-   *
-   * @return The scale factor that will be used to convert pulses to useful units.
-   */
-  public double getDistancePerPulse() {
-    return m_distancePerPulse;
-  }
-
-  /**
-   * Set the direction sensing for this encoder. This sets the direction sensing on the encoder so
-   * that it could count in the correct software direction regardless of the mounting.
-   *
-   * @param reverseDirection true if the encoder direction should be reversed
-   */
-  public void setReverseDirection(boolean reverseDirection) {
-    m_motor.setSensorPhase(reverseDirection);
-  }
-
-  /**
-   * Set the Samples to Average which specifies the number of samples of the timer to average when
-   * calculating the period. Perform averaging to account for mechanical imperfections or as
-   * oversampling to increase resolution.
-   *
-   * <p>Defaults to 64. See also, CTRE's ConfigVelocityMeasurementPeriod().
-   * https://docs.ctre-phoenix.com/en/stable/ch14_MCSensor.html#changing-velocity-measurement-parameters
-   * See also, the note in Measurement-Delays.md in the root of this repository.
-   *
-   * @param samplesToAverage The number of samples to average (one of 1, 2, 4, 8, 16, 32, or 64).
-   */
-  public void setSamplesToAverage(int samplesToAverage) {
-    // (n & (n-1)) checks if it is a power of two. See:
-    // http://
-    // www.graphics.stanford.edu/~seander/bithacks.html#DetermineIfPowerOf2
-    if (samplesToAverage < 1
-        || samplesToAverage > 64
-        || (samplesToAverage & (samplesToAverage - 1)) != 0) {
-      throw new IllegalArgumentException(
-          "Samples to average must be a power of 2 between 1 and 64, got " + samplesToAverage);
-    }
-    m_motor.configVelocityMeasurementWindow(samplesToAverage);
-  }
-
-  /**
-   * Get the Samples to Average which specifies the number of samples of the timer to average when
-   * calculating the period. Perform averaging to account for mechanical imperfections or as
-   * oversampling to increase resolution.
-   *
-   * <p>Defaults to 64. See also, CTRE's ConfigVelocityMeasurementPeriod().
-   * https://docs.ctre-phoenix.com/en/stable/ch14_MCSensor.html#changing-velocity-measurement-parameters
-   * See also, the note in Measurement-Delays.md in the root of this repository.
-   *
-   * @return samplesToAverage The number of samples to average (one of 1, 2, 4, 8, 16, 32, or 64).
-   */
-  public int getSamplesToAverage() {
-    return (int) m_motor.configGetParameter(ParamEnum.eSampleVelocityWindow, 0);
-  }
-
-  public BaseTalon getMotor() {
-    return m_motor;
-  }
-
-  public FeedbackDevice getSelectedFeedbackSensor() {
-    return FeedbackDevice.valueOf(m_motor.configGetParameter(ParamEnum.eFeedbackSensorType, 0));
-  }
-
-  @Override
-  public void initSendable(SendableBuilder builder) {
-    builder.setSmartDashboardType(
-        String.format("Encoder (%s)", getSelectedFeedbackSensor().name()));
-    builder.addDoubleProperty("Speed", this::getRate, null);
-    builder.addDoubleProperty("Distance", this::getDistance, null);
-    builder.addDoubleProperty("Distance per Tick", this::getDistancePerPulse, null);
-  }
-}

diff --git a/src/main/java/lib/ctre_shims/TalonEncoderSim.java b/src/main/java/lib/ctre_shims/TalonEncoderSim.java
deleted file mode 100644 (file)
index 996c35d..0000000
--- a/src/main/java/lib/ctre_shims/TalonEncoderSim.java
+++ /dev/null
@@ -1,194 +0,0 @@
-package lib.ctre_shims;
-
-import com.ctre.phoenix.motorcontrol.FeedbackDevice;
-import com.ctre.phoenix.motorcontrol.TalonFXSimCollection;
-import com.ctre.phoenix.motorcontrol.TalonSRXSimCollection;
-import com.ctre.phoenix.motorcontrol.can.TalonFX;
-import com.ctre.phoenix.motorcontrol.can.TalonSRX;
-
-/** Class to control a simulated encoder. */
-public class TalonEncoderSim {
-  private TalonEncoder m_encoder;
-
-  private TalonSRXSimCollection m_simCollectionSRX = null;
-  private TalonFXSimCollection m_simCollectionFX = null;
-
-  /**
-   * Constructs from an Encoder object.
-   *
-   * @param encoder Encoder to simulate
-   */
-  public TalonEncoderSim(TalonEncoder encoder) {
-    m_encoder = encoder;
-
-    if (m_encoder.getMotor() instanceof TalonSRX) {
-      m_simCollectionSRX = new TalonSRXSimCollection(m_encoder.getMotor());
-    } else if (m_encoder.getMotor() instanceof TalonFX) {
-      m_simCollectionFX = new TalonFXSimCollection(m_encoder.getMotor());
-    } else {
-      throw new IllegalStateException(
-          "Motor type " + m_encoder.getMotor().getClass().getName() + " is unsupported.");
-    }
-  }
-
-  /**
-   * Read the count of the encoder.
-   *
-   * @return the count
-   */
-  public int getCount() {
-    return m_encoder.get();
-  }
-
-  /**
-   * Change the count of the encoder.
-   *
-   * @param count the new count
-   */
-  public void setCount(int count) {
-    if (m_simCollectionSRX != null) {
-      FeedbackDevice selected = m_encoder.getSelectedFeedbackSensor();
-
-      switch (selected) {
-        case QuadEncoder:
-        case CTRE_MagEncoder_Relative:
-          m_simCollectionSRX.setQuadratureRawPosition(count);
-          break;
-
-        case Analog:
-          m_simCollectionSRX.setAnalogPosition(count);
-          break;
-
-        default:
-          throw new IllegalStateException(
-              "Selected feedback sensor is not supported: " + selected.name());
-      }
-    } else if (m_simCollectionFX != null) {
-      m_simCollectionFX.setIntegratedSensorRawPosition(count);
-    } else {
-      // This should have errored already in the constructor.
-      assert false;
-    }
-  }
-
-  /**
-   * Read the period of the encoder.
-   *
-   * @return the encoder period
-   */
-  public double getPeriod() {
-    return m_encoder.getRate();
-  }
-
-  /**
-   * Change the encoder period.
-   *
-   * @param period the new period
-   */
-  public void setPeriod(double period) {
-    // seconds -> distance/second
-    // m_distancePerPulse (distance) / period (seconds) = distance / second
-    setRate(m_encoder.getDistancePerPulse() / period);
-  }
-
-  // These are no-op in WPILib
-  // boolean getReset();
-  // void setReset();
-
-  /**
-   * Get the direction of the encoder.
-   *
-   * @return the direction of the encoder
-   */
-  public boolean getDirection() {
-    return m_encoder.getDirection();
-  }
-
-  /**
-   * Get the samples-to-average value.
-   *
-   * <p>See {@link TalonEncoder#getSamplesToAverage()}.
-   *
-   * @return the samples-to-average value
-   */
-  public int getSamplesToAverage() {
-    return m_encoder.getSamplesToAverage();
-  }
-
-  /**
-   * Set the samples-to-average value.
-   *
-   * <p>See {@link TalonEncoder#setSamplesToAverage()}.
-   *
-   * @param samplesToAverage the new value
-   */
-  public void setSamplesToAverage(int samplesToAverage) {
-    m_encoder.setSamplesToAverage(samplesToAverage);
-  }
-
-  /**
-   * Change the encoder distance.
-   *
-   * @param distance the new distance
-   */
-  public void setDistance(double distance) {
-    setCount((int) (distance / m_encoder.getDistancePerPulse()));
-  }
-
-  /**
-   * Read the distance of the encoder.
-   *
-   * @return the encoder distance
-   */
-  public double getDistance() {
-    return m_encoder.getDistance();
-  }
-
-  /**
-   * Change the rate of the encoder.
-   *
-   * @param rate the new rate
-   */
-  public void setRate(double rate) {
-    int rateInNativeUnits = (int) (rate / (10 * m_encoder.getDistancePerPulse()));
-
-    if (m_simCollectionSRX != null) {
-      FeedbackDevice selected = m_encoder.getSelectedFeedbackSensor();
-
-      switch (selected) {
-        case QuadEncoder:
-        case CTRE_MagEncoder_Relative:
-          m_simCollectionSRX.setQuadratureVelocity(rateInNativeUnits);
-          break;
-
-        case Analog:
-          m_simCollectionSRX.setAnalogVelocity(rateInNativeUnits);
-          break;
-
-        default:
-          throw new IllegalStateException(
-              "Selected feedback sensor is not supported: " + selected.name());
-      }
-    } else if (m_simCollectionFX != null) {
-      m_simCollectionFX.setIntegratedSensorVelocity(rateInNativeUnits);
-    } else {
-      // This should have errored already in the constructor.
-      assert false;
-    }
-  }
-
-  /**
-   * Get the rate of the encoder.
-   *
-   * @return the rate of change
-   */
-  public double getRate() {
-    return m_encoder.getRate();
-  }
-
-  /** Resets all simulation data for this encoder. */
-  public void resetData() {
-    setDistance(0);
-    setRate(0);
-  }
-}

diff --git a/src/test/java/frc/robot/constants/AprilTagPoseTest.java b/src/test/java/frc/robot/constants/AprilTagPoseTest.java
new file mode 100644 (file)
index 0000000..b5ce24d
--- /dev/null
+++ b/src/test/java/frc/robot/constants/AprilTagPoseTest.java
@@ -0,0 +1,125 @@
+package frc.robot.constants;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import frc.robot.util.Vision.Vision;
+
+/**
+ * Tests if all of the AprilTags are in the right spot
+ */
+public class AprilTagPoseTest {
+  /**
+   * Tests if there are the right number of AprilTags, that the tags in Vision match the ones in FieldConstants, and that they are on the right side of the field
+   */
+  @Test
+  public void testTagPoses() {
+    // Construct the vision instance
+    //   makes the field layout
+    Vision vision = new Vision(new ArrayList<Pair<String, Transform3d>>());
+
+    // we should have 22 tags
+    assertEquals(22, FieldConstants.APRIL_TAGS.size());
+    assertEquals(22, vision.getAprilTagFieldLayout().getTags().size());
+
+    // Check each tag in the field layout
+    for (int i = 0; i < vision.getAprilTagFieldLayout().getTags().size(); i++) {
+      // The expected tagId. The ArrayList is zero-based and our tags start at 1, so the tagId is i+1.
+      int tagId = i + 1;
+
+      // Get the poses from the two sources
+      // From the ArrayList<AprilTag> source
+      AprilTag apriltag1 = FieldConstants.APRIL_TAGS.get(i);
+      Pose3d p1 = apriltag1.pose;
+      // From the vision source
+      Pose3d p2 = vision.getTagPose(tagId);
+
+      // Check the tag id in the ArrayList
+      assertEquals(tagId, apriltag1.ID);
+
+      // Make sure the points match
+      assertEquals(p1.getX(), p2.getX(), 0.0001);
+      assertEquals(p1.getY(), p2.getY(), 0.0001);
+      assertEquals(p1.getZ(), p2.getZ(), 0.0001);
+
+      // Make sure the rotations match
+      assertEquals(p1.getRotation().getX(), p2.getRotation().getX(), 0.0001);
+      assertEquals(p1.getRotation().getY(), p2.getRotation().getY(), 0.0001);
+      assertEquals(p1.getRotation().getZ(), p2.getRotation().getZ(), 0.0001);
+
+      // 1-11 should be on the right, and 12-22 should be on the left
+      if(tagId > 11){
+        assertTrue(p1.getX() < FieldConstants.FIELD_LENGTH/2);
+      }else{
+        assertTrue(p1.getX() > FieldConstants.FIELD_LENGTH/2);
+      }
+    }
+  }
+
+  @Test
+  public void testReefTags(){
+    List<Pose3d> redPoses = FieldConstants.APRIL_TAGS.subList(5, 11).stream().map(tag->tag.pose).toList();
+    List<Pose3d> bluePoses = FieldConstants.APRIL_TAGS.subList(16, 22).stream().map(tag->tag.pose).toList();
+    Pose3d redCenter = findCenter(redPoses);
+    Pose3d blueCenter = findCenter(bluePoses);
+    
+    // The tags should be symmetrical, so the total rotation should be 0
+    assertEquals(redCenter.getRotation().getX(), 0, 0.0001);
+    assertEquals(blueCenter.getRotation().getX(), 0, 0.0001);
+    assertEquals(redCenter.getRotation().getY(), 0, 0.0001);
+    assertEquals(blueCenter.getRotation().getY(), 0, 0.0001);
+    assertEquals(MathUtil.angleModulus(redCenter.getRotation().getZ()), Math.PI, 0.0001);
+    assertEquals(MathUtil.angleModulus(blueCenter.getRotation().getZ()), Math.PI, 0.0001);
+
+    // Y and Z should be equal
+    assertEquals(redCenter.getY(), blueCenter.getY(), 0.0001);
+    assertEquals(redCenter.getZ(), blueCenter.getZ(), 0.0001);
+
+    // X should be mirrored
+    assertEquals(redCenter.getX(), FieldConstants.FIELD_LENGTH-blueCenter.getX(), 0.01);
+
+    // Compare each matching pair of tags
+    for(int i = 5; i < 11; i++){
+      Pose3d red = FieldConstants.APRIL_TAGS.get(i).pose;
+      Pose3d blue = FieldConstants.APRIL_TAGS.get(i+11).pose;
+      assertEquals(red.getY(), blue.getY(), 0.0001);
+      assertEquals(red.getZ(), blue.getZ(), 0.0001);
+      assertEquals(red.getZ(), redCenter.getZ(), 0.0001);
+      assertEquals(red.getX(), FieldConstants.FIELD_LENGTH-blue.getX(), 0.01);
+      assertEquals(MathUtil.angleModulus(red.getRotation().getZ()), MathUtil.angleModulus(Math.PI-blue.getRotation().getZ()), 0.0001);
+    }
+  }
+
+  /**
+   * Gets the center pose with the sum of the rotations, used for checking the reef
+   * @param poses The poses to find the center of
+   * @return A pose with the translation at the center and the sum of the rotations
+   */
+  private Pose3d findCenter(List<Pose3d> poses){
+    double x = 0;
+    double y = 0;
+    double z = 0;
+    Rotation3d rot = new Rotation3d();
+    for(Pose3d pose : poses){
+      x += pose.getX();
+      y += pose.getY();
+      z += pose.getZ();
+      rot = rot.plus(pose.getRotation());
+    }
+    x /= poses.size();
+    y /= poses.size();
+    z /= poses.size();
+    return new Pose3d(x, y, z, rot);
+  }
+}

diff --git a/src/test/java/frc/robot/constants/ConstantsTest.java b/src/test/java/frc/robot/constants/ConstantsTest.java
new file mode 100644 (file)
index 0000000..b368426
--- /dev/null
+++ b/src/test/java/frc/robot/constants/ConstantsTest.java
@@ -0,0 +1,92 @@
+package frc.robot.constants;
+
+import edu.wpi.first.math.util.Units;
+import frc.robot.constants.swerve.DriveConstants;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+/**
+ * Check some robot constants/parameters.
+ */
+public class ConstantsTest {
+
+    @Test
+    public void testRobotSize() {
+        // The competition robot frame width and length is 26 inches.
+        // It has 3/16 inch plates on all sides,
+        // so frame width is 26.375!
+        double widthFrame = Units.inchesToMeters(26.0);
+        double widthFrameAndPlates = widthFrame + 2.0 * Units.inchesToMeters(3.0 / 16.0);
+        
+        // At Port Hueneme, the frame perimeter was 105 inches
+        // frame perimeter is 105.5...
+        assertEquals(105.0, 4 * Units.metersToInches(widthFrameAndPlates), 0.501);
+        
+        // Bumpers.
+        // The backing board is 0.75 inches.
+        // The noodles are 2.5 inches.
+        // Board + noodles = 3.25 inches
+        // Measure the red bumpers, and they are 3.5 inches.
+        //
+        // The latch studs are centered on the 1x2 frame rails. That's 0.5 inches
+        // The bumper latch bracket holes for the studs are 3/4 in from the inside surface of the bumpers.
+        // That puts the bumper inside surface at 3/4 - (0.5) = 1/4 inch out from widthFrame
+        double thickBumpers = Units.inchesToMeters(3.5 + 0.25);
+        
+        // so width with bumpers is
+        @SuppressWarnings("unused")
+        double widthFrameWithBumpers = widthFrame + 2 * thickBumpers;
+
+        // check with values in DriveConstants
+        // System.out.printf("widthFrameWithBumpers  = %8f %8f\n", widthFrameWithBumpers, Units.metersToInches(widthFrameWithBumpers));
+        // System.out.printf("kRobotWidthWithBumpers = %8f %8f\n", DriveConstants.kRobotWidthWithBumpers, Units.metersToInches(DriveConstants.kRobotWidthWithBumpers));
+        // assertEquals(widthFrameWithBumpers, DriveConstants.kRobotWidthWithBumpers, 0.001);
+    }
+
+    /**
+     * MK4i module
+     * https://www.swervedrivespecialties.com/products/mk4i-swerve-module
+     */
+    enum SwerveDriveSpecialties {
+        // Gearbox ratios from the SDS webpage
+        //  https://www.swervedrivespecialties.com/products/mk4i-swerve-module
+        L1((50.0 / 14.0) * (19.0 / 25.0) * (45.0 / 15.0)),
+        L2((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)),
+        L3((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)),
+        // 16-tooth pinion
+        //    https://www.swervedrivespecialties.com/products/kit-adapter-16t-drive-pinion-gear-mk4i
+        L1P((50.0 / 16.0) * (19.0 / 25.0) * (45.0 / 15.0)),
+        L2P((50.0 / 16.0) * (17.0 / 27.0) * (45.0 / 15.0)),
+        L3P((50.0 / 16.0) * (16.0 / 28.0) * (45.0 / 15.0));
+
+        /**
+         * Drive gear ratio varies for each module
+         */
+        final double driveRatio;
+        /**
+         * Steering Gear ratio (same for all MK4i modules)
+         */
+        final double steerRatio = 150.0 / 7.0;
+
+        SwerveDriveSpecialties(double drive) {
+            this.driveRatio = drive;
+        }
+    }
+
+    @Test
+    public void testSwerveRatios() {
+        // check the mroe exact ratios against the published-to-2-digits ratios
+        assertEquals(8.14, SwerveDriveSpecialties.L1.driveRatio, 0.01);
+        assertEquals(6.75, SwerveDriveSpecialties.L2.driveRatio, 0.01);
+        assertEquals(6.12, SwerveDriveSpecialties.L3.driveRatio, 0.01);
+
+        // The drive ratio could be more accurate, but does not hurt
+        assertEquals(SwerveDriveSpecialties.L2P.driveRatio, DriveConstants.DRIVE_GEAR_RATIO, 0.01);
+
+        // The steer ratio
+        // print the relative error: 0.6e-4. After 100 rotations, error would be 0.6e-2 rotations (about 1.5 degrees)
+        // System.out.println((DriveConstants.kSteerGearRatio - SwerveDriveSpecialties.L2.steerRatio) / SwerveDriveSpecialties.L2.steerRatio);
+        assertEquals(SwerveDriveSpecialties.L2P.steerRatio, DriveConstants.STEER_GEAR_RATIO, 0.01);
+    }
+}

diff --git a/src/test/java/frc/robot/util/ArithTest.java b/src/test/java/frc/robot/util/ArithTest.java
new file mode 100644 (file)
index 0000000..59e3a4f
--- /dev/null
+++ b/src/test/java/frc/robot/util/ArithTest.java
@@ -0,0 +1,44 @@
+package frc.robot.util;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Disabled;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+/**
+ * Example of a JUnit test class.
+ * This test should run everytime someone builds the robot code.
+ * See https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-simulation/unit-testing.html
+ * <p>
+ * To disable a test, annotate with Disabled
+ */
+public class ArithTest {
+
+    @BeforeEach
+    public void prepare() {
+    }
+
+    @AfterEach
+    public void cleanup() {
+    }
+
+    /**
+     * Test if floating point addition works.
+     */
+    @Test
+    public void testSimpleArith() {
+        assertEquals(5.0, 2.0 + 3.0, 0.0001);
+    }
+
+    /**
+     * Here is a disabled test
+     */
+    @Disabled
+    @Test
+    public void testNaught() {
+        assertEquals(0, 0);
+    }
+
+}

diff --git a/src/test/java/frc/robot/util/DetectedObjectTest.java b/src/test/java/frc/robot/util/DetectedObjectTest.java
new file mode 100644 (file)
index 0000000..014533b
--- /dev/null
+++ b/src/test/java/frc/robot/util/DetectedObjectTest.java
@@ -0,0 +1,86 @@
+package frc.robot.util;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import java.util.Random;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import frc.robot.util.Vision.DetectedObject;
+import frc.robot.util.Vision.DetectedObject.ObjectType;
+
+/**
+ * Tests DetectedObject
+ */
+public class DetectedObjectTest {
+
+  @BeforeEach
+  public void prepare() {
+    DetectedObject.setDrive(null);
+  }
+
+  @AfterEach
+  public void cleanup() {}
+
+  /**
+   * Tests if the objec pose is correct
+   */
+  @Test
+  public void testObjectPose() {
+    DetectedObject object = new DetectedObject(
+      Units.degreesToRadians(45),
+      0,
+      1,
+      ObjectType.NONE,
+      new Transform3d(new Translation3d(0, 0, 1), new Rotation3d(0, -Math.PI/2, Math.PI/2))
+    );
+    Translation3d expected =  new Translation3d(Math.sqrt(2)/2, 0, Math.sqrt(2)/2+1);
+    assertEquals(expected.getX(), object.pose.getX(),  0.001);
+    assertEquals(expected.getY(), object.pose.getY(),  0.001);
+    assertEquals(expected.getZ(), object.pose.getZ(),  0.001);
+  }
+  
+  /**
+   * Tests the position of an object when distance is not specified
+   */
+  @Test
+  public void testObjectPoseWithoutDistance(){
+    DetectedObject object = new DetectedObject(
+      0,
+      -Units.degreesToRadians(20),
+      ObjectType.NONE,
+      new Transform3d(new Translation3d(0, 0, 1), new Rotation3d(0, Units.degreesToRadians(25), 0))
+    );
+    Translation3d expected =  new Translation3d(1, 0, 0);
+    assertEquals(expected.getX(), object.pose.getX(),  0.001);
+    assertEquals(expected.getY(), object.pose.getY(),  0.001);
+    assertEquals(expected.getZ(), object.pose.getZ(),  0.001);
+  }
+
+  /**
+   * Tests if the object is on the ground using random offsets
+   */
+  @Test
+  public void testObjectOnGround(){
+    Random random = new Random();
+    DetectedObject object = new DetectedObject(
+      random.nextDouble(-Math.PI, Math.PI),
+      random.nextDouble(0.001, Math.PI/4),
+      ObjectType.NONE,
+      new Transform3d(new Translation3d(
+        random.nextDouble(0, 100),
+        random.nextDouble(0, 100),
+        random.nextDouble(0.1, 100)
+      ), new Rotation3d(
+        0, random.nextDouble(0.001, Math.PI/4), random.nextDouble(-Math.PI, Math.PI)
+      ))
+    );
+    assertEquals(object.pose.getZ(), 0, 0.001);
+  }
+}

diff --git a/src/test/java/frc/robot/util/PathCheck.java b/src/test/java/frc/robot/util/PathCheck.java
new file mode 100644 (file)
index 0000000..e601bb7
--- /dev/null
+++ b/src/test/java/frc/robot/util/PathCheck.java
@@ -0,0 +1,21 @@
+package frc.robot.util;
+
+import org.junit.jupiter.api.Test;
+
+/**
+ * Simple check on PathPlanner path
+ */
+public class PathCheck {
+
+    /**
+     * Load the path groups.
+     * <p>
+     * We have had problems with syntax errors in a path.
+     */
+    @Test
+    public void pathGroupLoaderTest() {
+        // load the paths
+        //   may throw a ParseException; that error will fail this test
+        PathGroupLoader.loadPathGroups();
+    }
+}

diff --git a/src/test/java/frc/robot/util/PolynomialRegressionTest.java b/src/test/java/frc/robot/util/PolynomialRegressionTest.java
new file mode 100644 (file)
index 0000000..7ac3687
--- /dev/null
+++ b/src/test/java/frc/robot/util/PolynomialRegressionTest.java
@@ -0,0 +1,25 @@
+package frc.robot.util;
+
+import lib.PolynomialRegression;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class PolynomialRegressionTest {
+
+    /**
+     * Unit tests the {@code PolynomialRegression} data type.
+     */
+    @Test
+    public void testRegression() {
+        double[] x = {10, 20, 40, 80, 160, 200};
+        double[] y = {100, 350, 1500, 6700, 20160, 40000};
+        PolynomialRegression regression = new PolynomialRegression(x, y, 3);
+
+        assertEquals(regression.beta(3), 0.0092, 0.0001);
+        assertEquals(regression.beta(2), -1.6395, 0.0001);
+        assertEquals(regression.beta(1), 168.9232, 0.0001);
+        assertEquals(regression.beta(0), -2113.7306, 0.0001);
+    }
+
+}

diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
new file mode 100644 (file)
index 0000000..bef4a15
--- /dev/null
+++ b/vendordeps/AdvantageKit.json
@@ -0,0 +1,35 @@
+{
+    "fileName": "AdvantageKit.json",
+    "name": "AdvantageKit",
+    "version": "4.1.2",
+    "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
+    "frcYear": "2025",
+    "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": "4.1.2"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "org.littletonrobotics.akit",
+            "artifactId": "akit-wpilibio",
+            "version": "4.1.2",
+            "skipInvalidPlatforms": false,
+            "isJar": false,
+            "validPlatforms": [
+                "linuxathena",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal",
+                "windowsx86-64"
+            ]
+        }
+    ],
+    "cppDependencies": []
+}
\ No newline at end of file

diff --git a/vendordeps/ChoreoLib-2025.0.3.json b/vendordeps/ChoreoLib-2025.0.3.json
new file mode 100644 (file)
index 0000000..5a8cd54
--- /dev/null
+++ b/vendordeps/ChoreoLib-2025.0.3.json
@@ -0,0 +1,44 @@
+{
+    "fileName": "ChoreoLib-2025.0.3.json",
+    "name": "ChoreoLib",
+    "version": "2025.0.3",
+    "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca",
+    "frcYear": "2025",
+    "mavenUrls": [
+        "https://lib.choreo.autos/dep",
+        "https://repo1.maven.org/maven2"
+    ],
+    "jsonUrl": "https://lib.choreo.autos/dep/ChoreoLib2025.json",
+    "javaDependencies": [
+        {
+            "groupId": "choreo",
+            "artifactId": "ChoreoLib-java",
+            "version": "2025.0.3"
+        },
+        {
+            "groupId": "com.google.code.gson",
+            "artifactId": "gson",
+            "version": "2.11.0"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "choreo",
+            "artifactId": "ChoreoLib-cpp",
+            "version": "2025.0.3",
+            "libName": "ChoreoLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal",
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64"
+            ]
+        }
+    ]
+}
\ No newline at end of file

diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
index b0508c92011750b841d1483de6cdf9b588b91a2a..10f6cd22d4f190d352feacaee1ae5f2ea444f443 100644 (file)
--- a/vendordeps/PathplannerLib.json
+++ b/vendordeps/PathplannerLib.json
@@ -1,8 +1,9 @@
 {
     "fileName": "PathplannerLib.json",
     "name": "PathplannerLib",
-    "version": "2023.0.1",
+    "version": "2025.2.6",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
+    "frcYear": "2025",
     "mavenUrls": [
         "https://3015rangerrobotics.github.io/pathplannerlib/repo"
     ],
@@ -11,7 +12,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2023.0.1"
+            "version": "2025.2.6"
         }
     ],
     "jniDependencies": [],
@@ -19,7 +20,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2023.0.1",
+            "version": "2025.2.6",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -27,8 +28,10 @@
             "binaryPlatforms": [
                 "windowsx86-64",
                 "linuxx86-64",
-                "osxx86-64",
-                "linuxathena"
+                "osxuniversal",
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64"
             ]
         }
     ]

diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json
deleted file mode 100644 (file)
index dd0b3f4..0000000
--- a/vendordeps/Phoenix.json
+++ /dev/null
@@ -1,257 +0,0 @@
-{
-    "fileName": "Phoenix.json",
-    "name": "CTRE-Phoenix",
-    "version": "5.21.2",
-    "frcYear": 2022,
-    "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
-    "mavenUrls": [
-        "https://maven.ctr-electronics.com/release/"
-    ],
-    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix-frc2022-latest.json",
-    "javaDependencies": [
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "api-java",
-            "version": "5.21.2"
-        },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "wpiapi-java",
-            "version": "5.21.2"
-        }
-    ],
-    "jniDependencies": [
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "cci",
-            "version": "5.21.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "linuxathena"
-            ]
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "cci-sim",
-            "version": "5.21.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxx86-64"
-            ]
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "simTalonSRX",
-            "version": "5.21.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxx86-64"
-            ]
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "simTalonFX",
-            "version": "5.21.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxx86-64"
-            ]
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "simVictorSPX",
-            "version": "5.21.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxx86-64"
-            ]
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "simPigeonIMU",
-            "version": "5.21.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxx86-64"
-            ]
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "simCANCoder",
-            "version": "5.21.2",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxx86-64"
-            ]
-        }
-    ],
-    "cppDependencies": [
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "wpiapi-cpp",
-            "version": "5.21.2",
-            "libName": "CTRE_Phoenix_WPI",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "linuxathena"
-            ]
-        },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "api-cpp",
-            "version": "5.21.2",
-            "libName": "CTRE_Phoenix",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "linuxathena"
-            ]
-        },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "cci",
-            "version": "5.21.2",
-            "libName": "CTRE_PhoenixCCI",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "linuxathena"
-            ]
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "wpiapi-cpp-sim",
-            "version": "5.21.2",
-            "libName": "CTRE_Phoenix_WPISim",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxx86-64"
-            ]
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "api-cpp-sim",
-            "version": "5.21.2",
-            "libName": "CTRE_PhoenixSim",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxx86-64"
-            ]
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "cci-sim",
-            "version": "5.21.2",
-            "libName": "CTRE_PhoenixCCISim",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxx86-64"
-            ]
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "simTalonSRX",
-            "version": "5.21.2",
-            "libName": "CTRE_SimTalonSRX",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxx86-64"
-            ]
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "simTalonFX",
-            "version": "5.21.2",
-            "libName": "CTRE_SimTalonFX",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxx86-64"
-            ]
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "simVictorSPX",
-            "version": "5.21.2",
-            "libName": "CTRE_SimVictorSPX",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxx86-64"
-            ]
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "simPigeonIMU",
-            "version": "5.21.2",
-            "libName": "CTRE_SimPigeonIMU",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxx86-64"
-            ]
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "simCANCoder",
-            "version": "5.21.2",
-            "libName": "CTRE_SimCANCoder",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxx86-64"
-            ]
-        }
-    ]
-}
\ No newline at end of file

diff --git a/vendordeps/Phoenix5-frc2025-latest.json b/vendordeps/Phoenix5-frc2025-latest.json
new file mode 100644 (file)
index 0000000..c1098dc
--- /dev/null
+++ b/vendordeps/Phoenix5-frc2025-latest.json
@@ -0,0 +1,171 @@
+{
+    "fileName": "Phoenix5-frc2025-latest.json",
+    "name": "CTRE-Phoenix (v5)",
+    "version": "5.35.1",
+    "frcYear": "2025",
+    "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
+    "mavenUrls": [
+        "https://maven.ctr-electronics.com/release/"
+    ],
+    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json",
+    "requires": [
+        {
+            "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
+            "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6.  Please add the Phoenix 6 vendordep before adding Phoenix 5.",
+            "offlineFileName": "Phoenix6-frc2025-latest.json",
+            "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json"
+        }
+    ],
+    "conflictsWith": [
+        {
+            "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af",
+            "errorMessage": "Users must use the Phoenix 5 replay vendordep when using the Phoenix 6 replay vendordep.",
+            "offlineFileName": "Phoenix6-replay-frc2025-latest.json"
+        },
+        {
+            "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df",
+            "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.",
+            "offlineFileName": "Phoenix5-replay-frc2025-latest.json"
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "api-java",
+            "version": "5.35.1"
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "wpiapi-java",
+            "version": "5.35.1"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "cci",
+            "version": "5.35.1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "cci-sim",
+            "version": "5.35.1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "wpiapi-cpp",
+            "version": "5.35.1",
+            "libName": "CTRE_Phoenix_WPI",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "api-cpp",
+            "version": "5.35.1",
+            "libName": "CTRE_Phoenix",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "cci",
+            "version": "5.35.1",
+            "libName": "CTRE_PhoenixCCI",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "wpiapi-cpp-sim",
+            "version": "5.35.1",
+            "libName": "CTRE_Phoenix_WPISim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "api-cpp-sim",
+            "version": "5.35.1",
+            "libName": "CTRE_PhoenixSim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "cci-sim",
+            "version": "5.35.1",
+            "libName": "CTRE_PhoenixCCISim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ]
+}
\ No newline at end of file

diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-frc2025-latest.json
new file mode 100644 (file)
index 0000000..d7bea57
--- /dev/null
+++ b/vendordeps/Phoenix6-frc2025-latest.json
@@ -0,0 +1,449 @@
+{
+    "fileName": "Phoenix6-frc2025-latest.json",
+    "name": "CTRE-Phoenix (v6)",
+    "version": "25.3.2",
+    "frcYear": "2025",
+    "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-frc2025-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-frc2025-latest.json"
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "wpiapi-java",
+            "version": "25.3.2"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "api-cpp",
+            "version": "25.3.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "tools",
+            "version": "25.3.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "api-cpp-sim",
+            "version": "25.3.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "tools-sim",
+            "version": "25.3.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simTalonSRX",
+            "version": "25.3.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simVictorSPX",
+            "version": "25.3.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simPigeonIMU",
+            "version": "25.3.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simCANCoder",
+            "version": "25.3.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFX",
+            "version": "25.3.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFXS",
+            "version": "25.3.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANcoder",
+            "version": "25.3.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProPigeon2",
+            "version": "25.3.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANrange",
+            "version": "25.3.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdi",
+            "version": "25.3.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "wpiapi-cpp",
+            "version": "25.3.2",
+            "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": "25.3.2",
+            "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": "25.3.2",
+            "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": "25.3.2",
+            "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": "25.3.2",
+            "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": "25.3.2",
+            "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": "25.3.2",
+            "libName": "CTRE_SimPigeonIMU",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simCANCoder",
+            "version": "25.3.2",
+            "libName": "CTRE_SimCANCoder",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFX",
+            "version": "25.3.2",
+            "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": "25.3.2",
+            "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": "25.3.2",
+            "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": "25.3.2",
+            "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": "25.3.2",
+            "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": "25.3.2",
+            "libName": "CTRE_SimProCANdi",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ]
+}
\ No newline at end of file

diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json
index 997e2a46911e873e3da9987e453d92be2c7edea3..ac62be885c796f2b617f901147e54d74e47a15c1 100644 (file)
--- a/vendordeps/REVLib.json
+++ b/vendordeps/REVLib.json
@@ -1,34 +1,34 @@
 {
     "fileName": "REVLib.json",
     "name": "REVLib",
-    "version": "2022.1.1",
+    "version": "2025.0.3",
+    "frcYear": "2025",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
         "https://maven.revrobotics.com/"
     ],
-    "jsonUrl": "https://software-metadata.revrobotics.com/REVLib.json",
+    "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json",
     "javaDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2022.1.1"
+            "version": "2025.0.3"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2022.1.1",
+            "version": "2025.0.3",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
-                "linuxaarch64bionic",
+                "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
-                "linuxraspbian",
-                "osxx86-64"
+                "linuxarm32",
+                "osxuniversal"
             ]
         }
     ],
@@ -36,37 +36,35 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2022.1.1",
+            "version": "2025.0.3",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
-                "linuxaarch64bionic",
+                "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
-                "linuxraspbian",
-                "osxx86-64"
+                "linuxarm32",
+                "osxuniversal"
             ]
         },
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2022.1.1",
+            "version": "2025.0.3",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
-                "linuxaarch64bionic",
+                "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
-                "linuxraspbian",
-                "osxx86-64"
+                "linuxarm32",
+                "osxuniversal"
             ]
         }
     ]

diff --git a/vendordeps/Studica-2025.0.1.json b/vendordeps/Studica-2025.0.1.json
new file mode 100644 (file)
index 0000000..5010be0
--- /dev/null
+++ b/vendordeps/Studica-2025.0.1.json
@@ -0,0 +1,71 @@
+{
+    "fileName": "Studica-2025.0.1.json",
+    "name": "Studica",
+    "version": "2025.0.1",
+    "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+    "frcYear": "2025",
+    "mavenUrls": [
+        "https://dev.studica.com/maven/release/2025/"
+    ],
+    "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json",
+    "cppDependencies": [
+        {
+            "artifactId": "Studica-cpp",
+            "binaryPlatforms": [
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64",
+                "linuxx86-64",
+                "osxuniversal",
+                "windowsx86-64"
+            ],
+            "groupId": "com.studica.frc",
+            "headerClassifier": "headers",
+            "libName": "Studica",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "version": "2025.0.1"
+        },
+        {
+            "artifactId": "Studica-driver",
+            "binaryPlatforms": [
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64",
+                "linuxx86-64",
+                "osxuniversal",
+                "windowsx86-64"
+            ],
+            "groupId": "com.studica.frc",
+            "headerClassifier": "headers",
+            "libName": "StudicaDriver",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "version": "2025.0.1"
+        }
+    ],
+    "javaDependencies": [
+        {
+            "artifactId": "Studica-java",
+            "groupId": "com.studica.frc",
+            "version": "2025.0.1"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "artifactId": "Studica-driver",
+            "groupId": "com.studica.frc",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64",
+                "linuxx86-64",
+                "osxuniversal",
+                "windowsx86-64"
+            ],
+            "version": "2025.0.1"
+        }
+    ]
+}
\ No newline at end of file

diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json
index d7bd9b06dc6e6ed554f6a770a61e20781b5feb7a..3718e0acd7c6c4f50a2c6ae682eaa370ecc5e2bc 100644 (file)
--- a/vendordeps/WPILibNewCommands.json
+++ b/vendordeps/WPILibNewCommands.json
@@ -1,37 +1,38 @@
 {
   "fileName": "WPILibNewCommands.json",
   "name": "WPILib-New-Commands",
-  "version": "2020.0.0",
+  "version": "1.0.0",
   "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
+  "frcYear": "2025",
   "mavenUrls": [],
   "jsonUrl": "",
   "javaDependencies": [
-      {
-          "groupId": "edu.wpi.first.wpilibNewCommands",
-          "artifactId": "wpilibNewCommands-java",
-          "version": "wpilib"
-      }
+    {
+      "groupId": "edu.wpi.first.wpilibNewCommands",
+      "artifactId": "wpilibNewCommands-java",
+      "version": "wpilib"
+    }
   ],
   "jniDependencies": [],
   "cppDependencies": [
-      {
-          "groupId": "edu.wpi.first.wpilibNewCommands",
-          "artifactId": "wpilibNewCommands-cpp",
-          "version": "wpilib",
-          "libName": "wpilibNewCommands",
-          "headerClassifier": "headers",
-          "sourcesClassifier": "sources",
-          "sharedLibrary": true,
-          "skipInvalidPlatforms": true,
-          "binaryPlatforms": [
-              "linuxathena",
-              "linuxraspbian",
-              "linuxaarch64bionic",
-              "windowsx86-64",
-              "windowsx86",
-              "linuxx86-64",
-              "osxx86-64"
-          ]
-      }
+    {
+      "groupId": "edu.wpi.first.wpilibNewCommands",
+      "artifactId": "wpilibNewCommands-cpp",
+      "version": "wpilib",
+      "libName": "wpilibNewCommands",
+      "headerClassifier": "headers",
+      "sourcesClassifier": "sources",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "linuxathena",
+        "linuxarm32",
+        "linuxarm64",
+        "windowsx86-64",
+        "windowsx86",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    }
   ]
 }

diff --git a/vendordeps/libgrapplefrc2025.json b/vendordeps/libgrapplefrc2025.json
new file mode 100644 (file)
index 0000000..a49af31
--- /dev/null
+++ b/vendordeps/libgrapplefrc2025.json
@@ -0,0 +1,74 @@
+{
+    "fileName": "libgrapplefrc2025.json",
+    "name": "libgrapplefrc",
+    "version": "2025.1.3",
+    "frcYear": "2025",
+    "uuid": "8ef3423d-9532-4665-8339-206dae1d7168",
+    "mavenUrls": [
+        "https://storage.googleapis.com/grapple-frc-maven"
+    ],
+    "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2025.json",
+    "javaDependencies": [
+        {
+            "groupId": "au.grapplerobotics",
+            "artifactId": "libgrapplefrcjava",
+            "version": "2025.1.3"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "au.grapplerobotics",
+            "artifactId": "libgrapplefrcdriver",
+            "version": "2025.1.3",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "au.grapplerobotics",
+            "artifactId": "libgrapplefrccpp",
+            "version": "2025.1.3",
+            "libName": "grapplefrc",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "au.grapplerobotics",
+            "artifactId": "libgrapplefrcdriver",
+            "version": "2025.1.3",
+            "libName": "grapplefrcdriver",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ]
+}
\ No newline at end of file

diff --git a/vendordeps/navx_frc.json b/vendordeps/navx_frc.json
deleted file mode 100644 (file)
index 1718692..0000000
--- a/vendordeps/navx_frc.json
+++ /dev/null
@@ -1,35 +0,0 @@
-{
-    "fileName": "navx_frc.json",
-    "name": "KauaiLabs_navX_FRC",
-    "version": "4.0.447",
-    "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
-    "mavenUrls": [
-        "https://repo1.maven.org/maven2/"
-    ],
-    "jsonUrl": "https://www.kauailabs.com/dist/frc/2022/navx_frc.json",
-    "javaDependencies": [
-        {
-            "groupId": "com.kauailabs.navx.frc",
-            "artifactId": "navx-java",
-            "version": "4.0.447"
-        }
-    ],
-    "jniDependencies": [],
-    "cppDependencies": [
-        {
-            "groupId": "com.kauailabs.navx.frc",
-            "artifactId": "navx-cpp",
-            "version": "4.0.447",
-            "headerClassifier": "headers",
-            "sourcesClassifier": "sources",
-            "sharedLibrary": false,
-            "libName": "navx_frc",
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "linuxathena",
-                "linuxraspbian",
-                "windowsx86-64"
-            ]
-        }
-    ]
-}
\ No newline at end of file

diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
new file mode 100644 (file)
index 0000000..1219919
--- /dev/null
+++ b/vendordeps/photonlib.json
@@ -0,0 +1,71 @@
+{
+    "fileName": "photonlib.json",
+    "name": "photonlib",
+    "version": "v2025.2.1",
+    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+    "frcYear": "2025",
+    "mavenUrls": [
+        "https://maven.photonvision.org/repository/internal",
+        "https://maven.photonvision.org/repository/snapshots"
+    ],
+    "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
+    "jniDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-cpp",
+            "version": "v2025.2.1",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photonlib-cpp",
+            "version": "v2025.2.1",
+            "libName": "photonlib",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-cpp",
+            "version": "v2025.2.1",
+            "libName": "photontargeting",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photonlib-java",
+            "version": "v2025.2.1"
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-java",
+            "version": "v2025.2.1"
+        }
+    ]
+}
\ No newline at end of file