From 46375e9f6e04c5257c01e030f72833add40c82ac Mon Sep 17 00:00:00 2001 From: Sid Senthil Date: Sun, 9 Aug 2026 15:38:20 -0700 Subject: [PATCH 01/12] shooter hw --- .github/workflows/main.yml | 31 + .gitignore | 375 ++++---- .vscode/launch.json | 21 - .vscode/settings.json | 62 +- build.gradle | 68 +- simgui-ds.json | 109 +++ .../deploy/Vision/k2026RebuiltWelded.json | 584 ++++++++++++ .../Vision/k2026RebuiltWeldedBlueSide.json | 296 ++++++ .../Vision/k2026RebuiltWeldedRedSide.json | 296 ++++++ src/main/deploy/choreo/CristianoRonaldo.chor | 84 ++ src/main/deploy/choreo/DepotStart.traj | 269 ++++++ src/main/deploy/choreo/DepotSweep.traj | 269 ++++++ src/main/deploy/choreo/HubLeft.traj | 143 +++ src/main/deploy/choreo/HubRight.traj | 135 +++ .../deploy/choreo/LeftIntakeSweepShort.traj | 207 ++++ src/main/deploy/choreo/LeftSecondDip.traj | 263 ++++++ src/main/deploy/choreo/LeftSecondDipLong.traj | 223 +++++ .../deploy/choreo/LeftSecondDipShort.traj | 205 ++++ src/main/deploy/choreo/LeftShoot.traj | 96 ++ src/main/deploy/choreo/LeftShootToBump.traj | 95 ++ src/main/deploy/choreo/Nike.traj | 78 ++ src/main/deploy/choreo/OutpostStart.traj | 350 +++++++ src/main/deploy/choreo/OutpostStartPush.traj | 360 +++++++ src/main/deploy/choreo/OutpostSweep.traj | 324 +++++++ .../deploy/choreo/RightIntakeSweepShort.traj | 197 ++++ src/main/deploy/choreo/RightSecondDip.traj | 273 ++++++ .../deploy/choreo/RightSecondDipLong.traj | 218 +++++ .../deploy/choreo/RightSecondDipShort.traj | 207 ++++ src/main/deploy/choreo/RightShoot.traj | 95 ++ src/main/deploy/choreo/RightShootToBump.traj | 97 ++ src/main/deploy/choreo/anthony.traj | 99 ++ src/main/java/frc/robot/AutoRoutines.java | 787 ++++++++++++++++ src/main/java/frc/robot/Constants.java | 885 ++++++++++++++++++ src/main/java/frc/robot/Robot.java | 130 ++- src/main/java/frc/robot/RobotContainer.java | 230 +++++ src/main/java/frc/robot/Telemetry.java | 142 +++ src/main/java/frc/robot/UnusedAutos.txt | 386 ++++++++ .../robot/commandGroups/IntakeToBumpDTP.java | 24 + .../commandGroups/ReverseIntakeAndHopper.java | 16 + .../ShootCommandGroups/DriveToAndShoot.java | 154 +++ .../ShootCommandGroups/ShootBasic.java | 21 + .../ShootCommandGroups/ShootBasicRetract.java | 24 + .../ShootCommandGroups/ShootPassing.java | 53 ++ .../ShootCommandGroups/ShootWithAim.java | 78 ++ .../robot/commandGroups/ShootToBumpDTP.java | 24 + .../java/frc/robot/commands/DriveToPose.java | 132 +++ .../SwerveCommands/SwerveJoystickCommand.java | 171 ++++ .../SwerveJoystickCommandInArc.java | 123 +++ .../SwerveJoystickCommandWithCorrection.java | 246 +++++ .../SwerveJoystickCommandWithPointing.java | 114 +++ .../SwerveJoystickDefaultCommand.java | 52 + .../frc/robot/generated/TunerConstants.java | 349 +++++++ .../subsystems/CommandSwerveDrivetrain.java | 429 +++++++++ .../robot/subsystems/FuelGaugeDetection.java | 218 +++++ .../frc/robot/subsystems/HopperSubsystem.java | 164 ++++ .../frc/robot/subsystems/IntakeSubsystem.java | 340 +++++++ .../subsystems/IntakeVisionDetection.java | 87 ++ .../frc/robot/subsystems/LEDSubsystem.java | 169 ++++ .../robot/subsystems/ShooterSubsystem.java | 88 ++ .../frc/robot/subsystems/VisionSubsystem.java | 291 ++++++ .../java/frc/robot/util/CustomController.java | 33 + src/main/java/frc/robot/util/Elastic.java | 390 ++++++++ .../java/frc/robot/util/LoggedTalonFX.java | 298 ++++++ .../frc/robot/util/MathUtils/MiscMath.java | 23 + .../frc/robot/util/MathUtils/Vector2.java | 65 ++ .../frc/robot/util/MathUtils/Vector3.java | 78 ++ src/main/java/frc/robot/util/MiscUtils.java | 211 +++++ src/main/java/frc/robot/util/Targeting.java | 276 ++++++ .../frc/robot/util/VisionCameraConstants.java | 242 +++++ src/main/java/frc/robot/util/VisionUtils.java | 285 ++++++ vendordeps/ChoreoLib2026.json | 44 + vendordeps/DogLog.json | 20 + vendordeps/Phoenix6-26.1.3.json | 449 +++++++++ vendordeps/photonlib.json | 71 ++ 74 files changed, 14226 insertions(+), 315 deletions(-) create mode 100644 .github/workflows/main.yml delete mode 100644 .vscode/launch.json create mode 100644 simgui-ds.json create mode 100644 src/main/deploy/Vision/k2026RebuiltWelded.json create mode 100644 src/main/deploy/Vision/k2026RebuiltWeldedBlueSide.json create mode 100644 src/main/deploy/Vision/k2026RebuiltWeldedRedSide.json create mode 100644 src/main/deploy/choreo/CristianoRonaldo.chor create mode 100644 src/main/deploy/choreo/DepotStart.traj create mode 100644 src/main/deploy/choreo/DepotSweep.traj create mode 100644 src/main/deploy/choreo/HubLeft.traj create mode 100644 src/main/deploy/choreo/HubRight.traj create mode 100644 src/main/deploy/choreo/LeftIntakeSweepShort.traj create mode 100644 src/main/deploy/choreo/LeftSecondDip.traj create mode 100644 src/main/deploy/choreo/LeftSecondDipLong.traj create mode 100644 src/main/deploy/choreo/LeftSecondDipShort.traj create mode 100644 src/main/deploy/choreo/LeftShoot.traj create mode 100644 src/main/deploy/choreo/LeftShootToBump.traj create mode 100644 src/main/deploy/choreo/Nike.traj create mode 100644 src/main/deploy/choreo/OutpostStart.traj create mode 100644 src/main/deploy/choreo/OutpostStartPush.traj create mode 100644 src/main/deploy/choreo/OutpostSweep.traj create mode 100644 src/main/deploy/choreo/RightIntakeSweepShort.traj create mode 100644 src/main/deploy/choreo/RightSecondDip.traj create mode 100644 src/main/deploy/choreo/RightSecondDipLong.traj create mode 100644 src/main/deploy/choreo/RightSecondDipShort.traj create mode 100644 src/main/deploy/choreo/RightShoot.traj create mode 100644 src/main/deploy/choreo/RightShootToBump.traj create mode 100644 src/main/deploy/choreo/anthony.traj create mode 100644 src/main/java/frc/robot/AutoRoutines.java create mode 100644 src/main/java/frc/robot/Constants.java create mode 100644 src/main/java/frc/robot/RobotContainer.java create mode 100644 src/main/java/frc/robot/Telemetry.java create mode 100644 src/main/java/frc/robot/UnusedAutos.txt create mode 100644 src/main/java/frc/robot/commandGroups/IntakeToBumpDTP.java create mode 100644 src/main/java/frc/robot/commandGroups/ReverseIntakeAndHopper.java create mode 100644 src/main/java/frc/robot/commandGroups/ShootCommandGroups/DriveToAndShoot.java create mode 100644 src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootBasic.java create mode 100644 src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootBasicRetract.java create mode 100644 src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootPassing.java create mode 100644 src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootWithAim.java create mode 100644 src/main/java/frc/robot/commandGroups/ShootToBumpDTP.java create mode 100644 src/main/java/frc/robot/commands/DriveToPose.java create mode 100644 src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java create mode 100644 src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommandInArc.java create mode 100644 src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommandWithCorrection.java create mode 100644 src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommandWithPointing.java create mode 100644 src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickDefaultCommand.java create mode 100644 src/main/java/frc/robot/generated/TunerConstants.java create mode 100644 src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java create mode 100644 src/main/java/frc/robot/subsystems/FuelGaugeDetection.java create mode 100644 src/main/java/frc/robot/subsystems/HopperSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/IntakeSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/IntakeVisionDetection.java create mode 100644 src/main/java/frc/robot/subsystems/LEDSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/ShooterSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/VisionSubsystem.java create mode 100644 src/main/java/frc/robot/util/CustomController.java create mode 100644 src/main/java/frc/robot/util/Elastic.java create mode 100644 src/main/java/frc/robot/util/LoggedTalonFX.java create mode 100644 src/main/java/frc/robot/util/MathUtils/MiscMath.java create mode 100644 src/main/java/frc/robot/util/MathUtils/Vector2.java create mode 100644 src/main/java/frc/robot/util/MathUtils/Vector3.java create mode 100644 src/main/java/frc/robot/util/MiscUtils.java create mode 100644 src/main/java/frc/robot/util/Targeting.java create mode 100644 src/main/java/frc/robot/util/VisionCameraConstants.java create mode 100644 src/main/java/frc/robot/util/VisionUtils.java create mode 100644 vendordeps/ChoreoLib2026.json create mode 100644 vendordeps/DogLog.json create mode 100644 vendordeps/Phoenix6-26.1.3.json create mode 100644 vendordeps/photonlib.json diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml new file mode 100644 index 0000000..9e64ed4 --- /dev/null +++ b/.github/workflows/main.yml @@ -0,0 +1,31 @@ +on: [push] +# A workflow run is made up of one or more jobs that can run sequentially or in parallel +jobs: + spotless: + # The type of runner that the job will run on + runs-on: ubuntu-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@v4 + with: + fetch-depth: 0 + - uses: actions/setup-java@v4 + with: + distribution: 'zulu' + java-version: 17 + - run: ./gradlew spotlessCheck + build: + # The type of runner that the job will run on + runs-on: ubuntu-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@v4 + with: + fetch-depth: 0 + - uses: actions/setup-java@v4 + with: + distribution: 'zulu' + java-version: 17 + - run: ./gradlew build \ No newline at end of file diff --git a/.gitignore b/.gitignore index 9a9ca7b..49fd843 100644 --- a/.gitignore +++ b/.gitignore @@ -1,187 +1,188 @@ -# This gitignore has been specially created by the WPILib team. -# If you remove items from this file, intellisense might break. - -### C++ ### -# Prerequisites -*.d - -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app - -### Java ### -# Compiled class file -*.class - -# Log file -*.log - -# BlueJ files -*.ctxt - -# Mobile Tools for Java (J2ME) -.mtj.tmp/ - -# Package Files # -*.jar -*.war -*.nar -*.ear -*.zip -*.tar.gz -*.rar - -# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml -hs_err_pid* - -### Linux ### -*~ - -# temporary files which can be created if a process still has a handle open of a deleted file -.fuse_hidden* - -# KDE directory preferences -.directory - -# Linux trash folder which might appear on any partition or disk -.Trash-* - -# .nfs files are created when an open file is removed but is still being accessed -.nfs* - -### macOS ### -# General -.DS_Store -.AppleDouble -.LSOverride - -# Icon must end with two \r -Icon - -# Thumbnails -._* - -# Files that might appear in the root of a volume -.DocumentRevisions-V100 -.fseventsd -.Spotlight-V100 -.TemporaryItems -.Trashes -.VolumeIcon.icns -.com.apple.timemachine.donotpresent - -# Directories potentially created on remote AFP share -.AppleDB -.AppleDesktop -Network Trash Folder -Temporary Items -.apdisk - -### VisualStudioCode ### -.vscode/* -!.vscode/settings.json -!.vscode/tasks.json -!.vscode/launch.json -!.vscode/extensions.json - -### Windows ### -# Windows thumbnail cache files -Thumbs.db -ehthumbs.db -ehthumbs_vista.db - -# Dump file -*.stackdump - -# Folder config file -[Dd]esktop.ini - -# Recycle Bin used on file shares -$RECYCLE.BIN/ - -# Windows Installer files -*.cab -*.msi -*.msix -*.msm -*.msp - -# Windows shortcuts -*.lnk - -### Gradle ### -.gradle -/build/ - -# Ignore Gradle GUI config -gradle-app.setting - -# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) -!gradle-wrapper.jar - -# Cache of project -.gradletasknamecache - -# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 -# gradle/wrapper/gradle-wrapper.properties - -# # VS Code Specific Java Settings -# DO NOT REMOVE .classpath and .project -.classpath -.project -.settings/ -bin/ - -# IntelliJ -*.iml -*.ipr -*.iws -.idea/ -out/ - -# Fleet -.fleet - -# Simulation GUI and other tools window save file -networktables.json -simgui.json -*-window.json - -# Simulation data log directory -logs/ - -# Folder that has CTRE Phoenix Sim device config storage -ctre_sim/ - -# clangd -/.cache -compile_commands.json - -# Eclipse generated file for annotation processors -.factorypath +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +# DO NOT REMOVE .classpath and .project +.classpath +.project +.settings/ +bin/ + +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + +# Simulation GUI and other tools window save file +networktables.json +simgui.json +*-window.json + +# Simulation data log directory +logs/ + +# Folder that has CTRE Phoenix Sim device config storage +ctre_sim/ + +# clangd +/.cache +compile_commands.json + +# Eclipse generated file for annotation processors +.factorypath +.gradle/* \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json deleted file mode 100644 index c9c9713..0000000 --- a/.vscode/launch.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - // Use IntelliSense to learn about possible attributes. - // Hover to view descriptions of existing attributes. - // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 - "version": "0.2.0", - "configurations": [ - - { - "type": "wpilib", - "name": "WPILib Desktop Debug", - "request": "launch", - "desktop": true, - }, - { - "type": "wpilib", - "name": "WPILib roboRIO Debug", - "request": "launch", - "desktop": false, - } - ] -} diff --git a/.vscode/settings.json b/.vscode/settings.json index 5e6ede8..e0f15db 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,61 +1,3 @@ { - "java.configuration.updateBuildConfiguration": "automatic", - "java.server.launchMode": "Standard", - "files.exclude": { - "**/.git": true, - "**/.svn": true, - "**/.hg": true, - "**/CVS": true, - "**/.DS_Store": true, - "bin/": true, - "**/.classpath": true, - "**/.project": true, - "**/.settings": true, - "**/.factorypath": true, - "**/*~": true - }, - "java.test.config": [ - { - "name": "WPIlibUnitTests", - "workingDirectory": "${workspaceFolder}/build/jni/release", - "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], - "env": { - "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , - "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" - } - }, - ], - "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.*", - ], - "java.dependency.enableDependencyCheckup": false -} + "java.configuration.updateBuildConfiguration": "automatic" +} \ No newline at end of file diff --git a/build.gradle b/build.gradle index 21805c5..e99bf18 100644 --- a/build.gradle +++ b/build.gradle @@ -1,8 +1,10 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2026.2.1" + id 'com.diffplug.spotless' version '6.20.0' } + java { sourceCompatibility = JavaVersion.VERSION_17 targetCompatibility = JavaVersion.VERSION_17 @@ -26,7 +28,16 @@ deploy { // First part is artifact name, 2nd is artifact type // getTargetTypeClass is a shortcut to get the class type using a string + // frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + // } frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + // Enable VisualVM connection + jvmArgs.add("-Dcom.sun.management.jmxremote=true") + jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") + jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") + jvmArgs.add("-Djava.rmi.server.hostname=10.35.1.2") // Replace TE.AM with team number } // Static files artifact @@ -34,13 +45,15 @@ deploy { 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 + // longer exist in deploy directory of this project } } } } } + + def deployArtifact = deploy.targets.roborio.artifacts.frcJava // Set to true to use debug for all targets including JNI, which will drastically impact @@ -88,7 +101,11 @@ wpi.sim.addDriverstation() // in order to make them all available at runtime. Also adding the manifest so WPILib // knows where to look for our Robot Class. jar { - from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from { + configurations.runtimeClasspath.collect { + it.isDirectory() ? it : zipTree(it) + } + } from('src') { into 'backup/src' } from('vendordeps') { into 'backup/vendordeps' } from('build.gradle') { into 'backup' } @@ -105,3 +122,50 @@ wpi.java.configureTestTasks(test) tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' } + +tasks.named("build") { + dependsOn("spotlessApply") +} + +spotless { + java { + target fileTree('.') { + include '**/*.java' + exclude '**/build/**', '**/build-*/**', '**/bin/**' + } + toggleOffOn() + googleJavaFormat() + removeUnusedImports() + trimTrailingWhitespace() + endWithNewline() + } + groovyGradle { + target fileTree('.') { + include '**/*.gradle' + exclude '**/build/**', '**/build-*/**' + } + greclipse() + indentWithSpaces(4) + trimTrailingWhitespace() + endWithNewline() + } + format 'xml', { + target fileTree('.') { + include '**/*.xml' + exclude '**/build/**', '**/build-*/**' + } + eclipseWtp('xml') + trimTrailingWhitespace() + indentWithSpaces(2) + endWithNewline() + } + format 'misc', { + target fileTree('.') { + include '**/*.md', '**/.gitignore' + exclude '**/build/**', '**/build-*/**' + } + trimTrailingWhitespace() + indentWithSpaces(2) + endWithNewline() + } +} diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..5a2c369 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,109 @@ +{ + "FMS": { + "window": { + "visible": false + } + }, + "Keyboard 0 Settings": { + "window": { + "visible": true + } + }, + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 68, + "incKey": 65 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + {}, + {}, + { + "guid": "Keyboard0" + } + ] +} diff --git a/src/main/deploy/Vision/k2026RebuiltWelded.json b/src/main/deploy/Vision/k2026RebuiltWelded.json new file mode 100644 index 0000000..8c5a52d --- /dev/null +++ b/src/main/deploy/Vision/k2026RebuiltWelded.json @@ -0,0 +1,584 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 11.8779798, + "y": 7.4247756, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 11.9154194, + "y": 4.638039999999999, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 11.3118646, + "y": 4.3902376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 11.3118646, + "y": 4.0346376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 11.9154194, + "y": 3.4312351999999997, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 11.8779798, + "y": 0.6444996, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": 11.9528844, + "y": 0.6444996, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 12.2710194, + "y": 3.4312351999999997, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 12.519177399999998, + "y": 3.6790375999999996, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 12.519177399999998, + "y": 4.0346376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 12.2710194, + "y": 4.638039999999999, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.9528844, + "y": 7.4247756, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 16.5333172, + "y": 7.4033126, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 16.5333172, + "y": 6.9715126, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 16.5329616, + "y": 4.3235626, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 16.5329616, + "y": 3.8917626, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 17, + "pose": { + "translation": { + "x": 4.6630844, + "y": 0.6444996, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 4.6256194, + "y": 3.4312351999999997, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 5.229174199999999, + "y": 3.6790375999999996, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 5.229174199999999, + "y": 4.0346376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 4.6256194, + "y": 4.638039999999999, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 22, + "pose": { + "translation": { + "x": 4.6630844, + "y": 7.4247756, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 23, + "pose": { + "translation": { + "x": 4.5881798, + "y": 7.4247756, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 24, + "pose": { + "translation": { + "x": 4.2700194, + "y": 4.638039999999999, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 25, + "pose": { + "translation": { + "x": 4.0218614, + "y": 4.3902376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 26, + "pose": { + "translation": { + "x": 4.0218614, + "y": 4.0346376, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 27, + "pose": { + "translation": { + "x": 4.2700194, + "y": 3.4312351999999997, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 28, + "pose": { + "translation": { + "x": 4.5881798, + "y": 0.6444996, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 29, + "pose": { + "translation": { + "x": 0.0077469999999999995, + "y": 0.6659626, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 30, + "pose": { + "translation": { + "x": 0.0077469999999999995, + "y": 1.0977626, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 31, + "pose": { + "translation": { + "x": 0.0080772, + "y": 3.7457125999999996, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 32, + "pose": { + "translation": { + "x": 0.0080772, + "y": 4.1775126, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.069 + } +} diff --git a/src/main/deploy/Vision/k2026RebuiltWeldedBlueSide.json b/src/main/deploy/Vision/k2026RebuiltWeldedBlueSide.json new file mode 100644 index 0000000..277da8f --- /dev/null +++ b/src/main/deploy/Vision/k2026RebuiltWeldedBlueSide.json @@ -0,0 +1,296 @@ +{ + "tags": [ + { + "ID": 17, + "pose": { + "translation": { + "x": 4.6490636, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 4.6115986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 5.2151534, + "y": 3.6657534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 5.2151534, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 4.6115986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 22, + "pose": { + "translation": { + "x": 4.6490636, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 23, + "pose": { + "translation": { + "x": 4.574159, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 24, + "pose": { + "translation": { + "x": 4.2559986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 25, + "pose": { + "translation": { + "x": 4.007866, + "y": 4.3769534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 26, + "pose": { + "translation": { + "x": 4.007866, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 27, + "pose": { + "translation": { + "x": 4.2559986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 28, + "pose": { + "translation": { + "x": 4.574159, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 29, + "pose": { + "translation": { + "x": 0.0136906, + "y": 0.6507734, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 30, + "pose": { + "translation": { + "x": 0.0136906, + "y": 1.0825734, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 31, + "pose": { + "translation": { + "x": 0.0140462, + "y": 3.7301932, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 32, + "pose": { + "translation": { + "x": 0.0140462, + "y": 4.1619931999999995, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + } + ], + "field": { + "length": 16.518, + "width": 8.043 + } +} diff --git a/src/main/deploy/Vision/k2026RebuiltWeldedRedSide.json b/src/main/deploy/Vision/k2026RebuiltWeldedRedSide.json new file mode 100644 index 0000000..cdffa5e --- /dev/null +++ b/src/main/deploy/Vision/k2026RebuiltWeldedRedSide.json @@ -0,0 +1,296 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 11.863959, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 11.9013986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 11.2978438, + "y": 4.3769534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 11.2978438, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 11.9013986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 11.863959, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": 11.9388636, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 12.2569986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 12.5051566, + "y": 3.6657534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 12.5051566, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 12.2569986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.9388636, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 16.499332, + "y": 7.391907999999999, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 16.499332, + "y": 6.960107999999999, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 16.4989764, + "y": 4.3124882, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 16.4989764, + "y": 3.8806881999999994, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + } + ], + "field": { + "length": 16.518, + "width": 8.043 + } +} diff --git a/src/main/deploy/choreo/CristianoRonaldo.chor b/src/main/deploy/choreo/CristianoRonaldo.chor new file mode 100644 index 0000000..70fd3cb --- /dev/null +++ b/src/main/deploy/choreo/CristianoRonaldo.chor @@ -0,0 +1,84 @@ +{ + "name":"CristianoRonaldo", + "version":2, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"10.375 in", + "val":0.263525 + }, + "y":{ + "exp":"11.875 in", + "val":0.301625 + } + }, + "backLeft":{ + "x":{ + "exp":"-10.375 in", + "val":-0.263525 + }, + "y":{ + "exp":"11.875 in", + "val":0.301625 + } + }, + "mass":{ + "exp":"140 lbs", + "val":63.502931800000006 + }, + "inertia":{ + "exp":"20174 in ^ 2 * lbs", + "val":5.903712368280681 + }, + "gearing":{ + "exp":"5.27", + "val":5.27 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"5300 RPM", + "val":555.0147021341968 + }, + "tmax":{ + "exp":"1.3 N * m", + "val":1.3 + }, + "cof":{ + "exp":"1", + "val":1.0 + }, + "bumper":{ + "front":{ + "exp":"17.5625 in", + "val":0.4460875 + }, + "side":{ + "exp":"16.0625 in", + "val":0.4079875 + }, + "back":{ + "exp":"17.5625 in", + "val":0.4460875 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/src/main/deploy/choreo/DepotStart.traj b/src/main/deploy/choreo/DepotStart.traj new file mode 100644 index 0000000..090b5ba --- /dev/null +++ b/src/main/deploy/choreo/DepotStart.traj @@ -0,0 +1,269 @@ +{ + "name":"DepotStart", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.5281453132629395, "y":4.020892143249512, "heading":-3.1361187805263304, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.6894803047180176, "y":4.556199550628662, "heading":-1.5707963267948966, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.5171908140182495, "y":7.144200325012207, "heading":-1.586837712220261, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.37151533365249634, "y":7.197152614593506, "heading":-1.57, "intervals":75, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.3741641044616699, "y":5.363866329193115, "heading":-1.57, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.561471939086914, "y":4.870401382446289, "heading":2.487401236737339, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":"first", "to":0, "data":{"type":"MaxVelocity", "props":{"max":5.0}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":6.7}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxVelocity", "props":{"max":0.5}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"MaxVelocity", "props":{"max":5.0}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.5281453132629395 m", "val":3.5281453132629395}, "y":{"exp":"4.020892143249512 m", "val":4.020892143249512}, "heading":{"exp":"-3.1361187805263304 rad", "val":-3.1361187805263304}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.6894803047180176 m", "val":1.6894803047180176}, "y":{"exp":"4.556199550628662 m", "val":4.556199550628662}, "heading":{"exp":"-1.5707963267948966 rad", "val":-1.5707963267948966}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.5171908140182495 m", "val":1.5171908140182495}, "y":{"exp":"7.144200325012207 m", "val":7.144200325012207}, "heading":{"exp":"-1.586837712220261 rad", "val":-1.586837712220261}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.37151533365249634 m", "val":0.37151533365249634}, "y":{"exp":"7.197152614593506 m", "val":7.197152614593506}, "heading":{"exp":"-3.14 / 2 rad", "val":-1.57}, "intervals":75, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.3741641044616699 m", "val":0.3741641044616699}, "y":{"exp":"5.363866329193115 m", "val":5.363866329193115}, "heading":{"exp":"-3.14 / 2 rad", "val":-1.57}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.561471939086914 m", "val":1.561471939086914}, "y":{"exp":"4.870401382446289 m", "val":4.870401382446289}, "heading":{"exp":"2.487401236737339 rad", "val":2.487401236737339}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":"first", "to":0, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"5 m / s", "val":5.0}}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6.7 m / s ^ 2", "val":6.7}}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.5 m / s", "val":0.5}}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"5 m / s", "val":5.0}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.02534,2.04412,2.87834,6.62866,7.50558], + "samples":[ + {"t":0.0, "x":3.52815, "y":4.02089, "heading":-3.13612, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.42693, "ay":1.87763, "alpha":6.5074, "fx":[-100.77541,-84.76082,-107.44797,-115.14472], "fy":[5.07656,65.89637,45.40124,2.86111]}, + {"t":0.03107, "x":3.52504, "y":4.0218, "heading":-3.13612, "vx":-0.19969, "vy":0.05834, "omega":0.20219, "ax":-6.42898, "ay":1.87799, "alpha":6.12202, "fx":[-100.67019,-85.968,-107.11707,-114.50361], "fy":[6.9661,63.52798,44.64973,4.11431]}, + {"t":0.06214, "x":3.51574, "y":4.02452, "heading":-3.12984, "vx":-0.39944, "vy":0.11669, "omega":0.39241, "ax":-6.4289, "ay":1.8777, "alpha":5.77558, "fx":[-100.53472,-87.00857,-106.85698,-113.85375], "fy":[8.77587,61.55032,43.76371,5.14925]}, + {"t":0.09321, "x":3.50022, "y":4.02905, "heading":-3.11764, "vx":-0.59919, "vy":0.17503, "omega":0.57186, "ax":-6.42882, "ay":1.87734, "alpha":5.42197, "fx":[-100.35762,-88.07073,-106.65003,-113.17032], "fy":[10.75541,59.59953,42.72529,6.13636]}, + {"t":0.12428, "x":3.4785, "y":4.03539, "heading":-3.09988, "vx":-0.79894, "vy":0.23336, "omega":0.74032, "ax":-6.42873, "ay":1.87691, "alpha":5.0644, "fx":[-100.15618,-89.14504,-106.48327,-112.45841], "fy":[12.84135,57.68091,41.56121,7.10596]}, + {"t":0.15535, "x":3.45057, "y":4.04355, "heading":-3.07687, "vx":-0.99869, "vy":0.29168, "omega":0.89768, "ax":-6.42863, "ay":1.87639, "alpha":4.70377, "fx":[-99.94519,-90.23039,-106.33971,-111.72125], "fy":[14.9823,55.77273,40.30641,8.09476]}, + {"t":0.18642, "x":3.41644, "y":4.05352, "heading":-3.04898, "vx":-1.19843, "vy":0.34998, "omega":1.04383, "ax":-6.42852, "ay":1.87572, "alpha":4.34595, "fx":[-99.74702,-91.30493,-106.21148,-110.96659], "fy":[17.10519,53.91029,38.97984,9.11813]}, + {"t":0.2175, "x":3.3761, "y":4.0653, "heading":-3.01655, "vx":-1.39817, "vy":0.40826, "omega":1.17886, "ax":-6.42841, "ay":1.87487, "alpha":3.98465, "fx":[-99.56562,-92.39248,-106.07274,-110.19204], "fy":[19.19417,51.99631,37.64009,10.22904]}, + {"t":0.24857, "x":3.32956, "y":4.07889, "heading":-2.97992, "vx":-1.59791, "vy":0.46651, "omega":1.30267, "ax":-6.42831, "ay":1.87371, "alpha":3.62713, "fx":[-99.42449,-93.4628,-105.92135,-109.40774], "fy":[21.17182,50.0882,36.30129,11.42461]}, + {"t":0.27964, "x":3.2768, "y":4.09429, "heading":-2.93945, "vx":-1.79764, "vy":0.52473, "omega":1.41537, "ax":-6.42822, "ay":1.87212, "alpha":3.27344, "fx":[-99.33406,-94.51061,-105.74658,-108.61939], "fy":[23.00139,48.16744,34.99838,12.71806]}, + {"t":0.31071, "x":3.21785, "y":4.1115, "heading":-2.89547, "vx":-1.99737, "vy":0.5829, "omega":1.51707, "ax":-6.4281, "ay":1.8701, "alpha":2.92502, "fx":[-99.30273,-95.52434,-105.54124,-107.83457], "fy":[24.64687,46.2355,33.76481,14.10936]}, + {"t":0.34178, "x":3.15268, "y":4.13051, "heading":-2.84833, "vx":-2.1971, "vy":0.64101, "omega":1.60796, "ax":-6.42812, "ay":1.86674, "alpha":2.58631, "fx":[-99.34045,-96.48852,-105.30861,-107.06669], "fy":[26.05685,44.31438,32.61015,15.56202]}, + {"t":0.37285, "x":3.08132, "y":4.15133, "heading":-2.79837, "vx":-2.39682, "vy":0.69901, "omega":1.68832, "ax":-6.42816, "ay":1.86196, "alpha":2.25962, "fx":[-99.44226,-97.39054,-105.04695,-106.32723], "fy":[27.21902,42.40991,31.56377,17.04739]}, + {"t":0.40392, "x":3.00374, "y":4.17395, "heading":-2.74592, "vx":-2.59655, "vy":0.75686, "omega":1.75852, "ax":-6.42829, "ay":1.85448, "alpha":1.95284, "fx":[-99.60108,-98.2103,-104.7673,-105.63675], "fy":[28.10157,40.55751,30.6222,18.48337]}, + {"t":0.43499, "x":2.91996, "y":4.19836, "heading":-2.69128, "vx":-2.79628, "vy":0.81448, "omega":1.8192, "ax":-6.42906, "ay":1.83988, "alpha":1.6784, "fx":[-99.80236,-98.93735,-104.49609,-105.02808], "fy":[28.64233,38.76037,29.73563,19.69954]}, + {"t":0.46606, "x":2.82998, "y":4.22455, "heading":-2.63475, "vx":-2.99604, "vy":0.87165, "omega":1.87135, "ax":-6.43004, "ay":1.81164, "alpha":1.47616, "fx":[-99.9834,-99.50549,-104.28051,-104.55705], "fy":[28.74294,37.16247,28.80422,20.33467]}, + {"t":0.49713, "x":2.73378, "y":4.25251, "heading":-2.57661, "vx":-3.19583, "vy":0.92794, "omega":1.91721, "ax":-6.4443, "ay":1.67551, "alpha":1.51191, "fx":[-99.9824,-100.07583,-104.60703,-104.56685], "fy":[26.86075,35.13335,26.4782,17.92739]}, + {"t":0.5282, "x":2.63138, "y":4.28215, "heading":-2.51704, "vx":-3.39606, "vy":0.97999, "omega":1.96419, "ax":3.39322, "ay":-1.16432, "alpha":2.91422, "fx":[63.4799,56.80533,43.84158,51.35237], "fy":[-19.19598,-7.1183,-17.87291,-29.75038]}, + {"t":0.55927, "x":2.52749, "y":4.31204, "heading":-2.45601, "vx":-3.29063, "vy":0.94382, "omega":2.05474, "ax":6.34507, "ay":-2.02792, "alpha":0.91068, "fx":[102.28323,102.12642,99.16263,99.3581], "fy":[-31.38646,-27.07601,-33.19777,-37.11864]}, + {"t":0.59035, "x":2.42831, "y":4.34038, "heading":-2.39217, "vx":-3.09348, "vy":0.88081, "omega":2.08303, "ax":6.3846, "ay":-1.96789, "alpha":0.53001, "fx":[102.33789,102.09324,100.3777,100.63215], "fy":[-30.54558,-28.37654,-31.98018,-34.06431]}, + {"t":0.62142, "x":2.33528, "y":4.3668, "heading":-2.32745, "vx":-2.89511, "vy":0.81967, "omega":2.0995, "ax":6.39897, "ay":-1.94286, "alpha":0.19723, "fx":[102.006,101.81425,101.177,101.35611], "fy":[-30.50095,-29.86233,-31.14808,-31.86597]}, + {"t":0.65249, "x":2.24842, "y":4.39133, "heading":-2.26221, "vx":-2.69628, "vy":0.7593, "omega":2.10563, "ax":6.40723, "ay":-1.9265, "alpha":-0.14715, "fx":[101.5013,101.51677,101.94844,101.91158], "fy":[-30.87565,-31.42924,-30.24107,-29.79243]}, + {"t":0.68356, "x":2.16773, "y":4.41399, "heading":-2.19679, "vx":-2.49721, "vy":0.69944, "omega":2.10106, "ax":6.41256, "ay":-1.91533, "alpha":-0.51422, "fx":[100.87186,101.24387,102.74423,102.35643], "fy":[-31.62255,-33.08115,-29.17387,-27.75157]}, + {"t":0.71463, "x":2.09324, "y":4.4348, "heading":-2.13151, "vx":-2.29796, "vy":0.63993, "omega":2.08508, "ax":6.41697, "ay":-1.90496, "alpha":-0.9002, "fx":[100.15572,101.04753,103.5788,102.71405], "fy":[-32.68691,-34.68765,-27.88988,-25.70582]}, + {"t":0.7457, "x":2.02494, "y":4.45376, "heading":-2.06672, "vx":-2.09858, "vy":0.58074, "omega":2.05711, "ax":6.42108, "ay":-1.8942, "alpha":-1.30197, "fx":[99.37703,100.95243,104.44122,102.98703], "fy":[-34.04003,-36.19427,-26.38756,-23.66536]}, + {"t":0.77677, "x":1.96283, "y":4.47089, "heading":-2.00281, "vx":-1.89907, "vy":0.52189, "omega":2.01666, "ax":6.42532, "ay":-1.88219, "alpha":-1.71454, "fx":[98.55777,100.97889,105.31427,103.17548], "fy":[-35.65152,-37.5399,-24.68458,-21.64855]}, + {"t":0.80784, "x":1.90693, "y":4.4862, "heading":-1.94015, "vx":-1.69943, "vy":0.46341, "omega":1.96338, "ax":6.42999, "ay":-1.86802, "alpha":-2.13759, "fx":[97.72961,101.12442,106.18439,103.28503], "fy":[-37.45858,-38.73495,-22.78595,-19.6453]}, + {"t":0.83891, "x":1.85723, "y":4.4997, "heading":-1.87914, "vx":-1.49965, "vy":0.40537, "omega":1.89697, "ax":6.43551, "ay":-1.85045, "alpha":-2.56626, "fx":[96.9272,101.39389,107.03387,103.31852], "fy":[-39.39933,-39.74688,-20.7181,-17.64491]}, + {"t":0.86998, "x":1.81374, "y":4.5114, "heading":-1.8202, "vx":-1.29969, "vy":0.34787, "omega":1.81723, "ax":6.44251, "ay":-1.82716, "alpha":-2.99733, "fx":[96.20387,101.78343,107.84874,103.28233], "fy":[-41.37587,-40.56874,-18.49931,-15.58629]}, + {"t":0.90105, "x":1.77647, "y":4.52133, "heading":-1.76374, "vx":-1.09952, "vy":0.2911, "omega":1.7241, "ax":6.45279, "ay":-1.79158, "alpha":-3.4223, "fx":[95.66798,102.31781,108.61137,103.174], "fy":[-43.20138,-41.11565,-16.15536,-13.29849]}, + {"t":0.93212, "x":1.74542, "y":4.52951, "heading":-1.71017, "vx":-0.89902, "vy":0.23543, "omega":1.61777, "ax":6.69325, "ay":-0.23039, "alpha":-3.85239, "fx":[102.14094,108.6046,110.48025,103.81533], "fy":[-19.41054,-19.59258,9.02638,15.34654]}, + {"t":0.9632, "x":1.72071, "y":4.53671, "heading":-1.6599, "vx":-0.69106, "vy":0.22827, "omega":1.49807, "ax":6.26253, "ay":2.37533, "alpha":-4.12718, "fx":[98.78536,108.04159,101.48946,89.37269], "fy":[25.85657,18.37902,47.51593,59.08889]}, + {"t":0.99427, "x":1.70227, "y":4.54495, "heading":-1.61336, "vx":-0.49648, "vy":0.30208, "omega":1.36984, "ax":5.47117, "ay":3.86466, "alpha":-4.34777, "fx":[85.888,100.99631,89.35438,71.19647], "fy":[53.37428,41.18784,68.88287,81.97222]}, + {"t":1.02534, "x":1.68948, "y":4.5562, "heading":-1.5708, "vx":-0.32648, "vy":0.42216, "omega":1.23475, "ax":4.60477, "ay":4.86333, "alpha":-4.86061, "fx":[69.84341,93.27089,77.17494,52.12725], "fy":[71.82135,56.2041,83.75202,97.05821]}, + {"t":1.06452, "x":1.68022, "y":4.57647, "heading":-1.52241, "vx":-0.14605, "vy":0.61272, "omega":1.04429, "ax":2.20352, "ay":6.32532, "alpha":-4.38646, "fx":[23.02756,58.9644,43.78839,14.1497], "fy":[98.70894,88.02284,104.38912,110.5555]}, + {"t":1.1037, "x":1.67619, "y":4.60534, "heading":-1.48149, "vx":-0.05971, "vy":0.86057, "omega":0.87241, "ax":-0.14448, "ay":6.69627, "alpha":-4.0609, "fx":[-20.03477,19.05276,10.79949,-18.99225], "fy":[101.243,101.9658,111.68778,110.33648]}, + {"t":1.14289, "x":1.67374, "y":4.6442, "heading":-1.44731, "vx":-0.06537, "vy":1.12296, "omega":0.71329, "ax":-0.28645, "ay":6.69152, "alpha":-3.77019, "fx":[-20.83865,15.14337,7.71327,-20.20847], "fy":[100.99704,102.83028,111.51665,109.58717]}, + {"t":1.18207, "x":1.67096, "y":4.69334, "heading":-1.41936, "vx":-0.07659, "vy":1.38516, "omega":0.56556, "ax":-0.34167, "ay":6.68871, "alpha":-3.47733, "fx":[-20.05921,12.77257,5.78983,-20.20043], "fy":[101.12686,103.35641,111.21252,109.05695]}, + {"t":1.22126, "x":1.6677, "y":4.75275, "heading":-1.3972, "vx":-0.08998, "vy":1.64724, "omega":0.42931, "ax":-0.37565, "ay":6.68663, "alpha":-3.18645, "fx":[-18.99738,10.75159,4.20833,-19.81734], "fy":[101.39543,103.74229,110.85874,108.624]}, + {"t":1.26044, "x":1.66388, "y":4.82243, "heading":-1.38038, "vx":-0.1047, "vy":1.90925, "omega":0.30445, "ax":-0.39933, "ay":6.68494, "alpha":-2.89871, "fx":[-17.87695,8.88312,2.83817,-19.2028], "fy":[101.73217,104.04352,110.47488,108.26244]}, + {"t":1.29962, "x":1.65947, "y":4.90237, "heading":-1.36845, "vx":-0.12035, "vy":2.17119, "omega":0.19087, "ax":-0.4167, "ay":6.68347, "alpha":-2.61369, "fx":[-16.76526,7.10424,1.62428,-18.42467], "fy":[102.11123,104.28992,110.06746,107.95163]}, + {"t":1.33881, "x":1.65444, "y":4.99258, "heading":-1.36097, "vx":-0.13668, "vy":2.43308, "omega":0.08845, "ax":-0.42967, "ay":6.68214, "alpha":-2.33385, "fx":[-15.69495,5.39862,0.53351,-17.52273], "fy":[102.50622,104.49123,109.6508,107.68693]}, + {"t":1.37799, "x":1.64875, "y":5.09305, "heading":-1.3575, "vx":-0.15351, "vy":2.69491, "omega":-0.003, "ax":-0.43933, "ay":6.68081, "alpha":-2.05835, "fx":[-14.66967,3.76375,-0.45579,-16.53729], "fy":[102.91754,104.66719,109.22085,107.44563]}, + {"t":1.41718, "x":1.6424, "y":5.20377, "heading":-1.35762, "vx":-0.17073, "vy":2.95669, "omega":-0.08365, "ax":-0.44635, "ay":6.67937, "alpha":-1.79354, "fx":[-13.71747,2.21745,-1.35091,-15.49339], "fy":[103.2568,104.88314,108.79188,107.22772]}, + {"t":1.45636, "x":1.63537, "y":5.32475, "heading":-1.3609, "vx":-0.18822, "vy":3.21841, "omega":-0.15393, "ax":-0.45116, "ay":6.67756, "alpha":-1.54188, "fx":[-12.8217,0.77942,-2.1681,-14.43962], "fy":[103.62865,105.00765,108.37716,107.03132]}, + {"t":1.49554, "x":1.62765, "y":5.45599, "heading":-1.36693, "vx":-0.2059, "vy":3.48007, "omega":-0.21435, "ax":-0.45421, "ay":6.67484, "alpha":-1.31076, "fx":[-12.01876,-0.49703,-2.88756,-13.44011], "fy":[103.95774,105.12438,107.96419,106.82554]}, + {"t":1.53473, "x":1.61923, "y":5.59748, "heading":-1.37533, "vx":-0.22369, "vy":3.74161, "omega":-0.26571, "ax":-0.4563, "ay":6.6695, "alpha":-1.14496, "fx":[-11.45513,-1.46638,-3.42867,-12.62601], "fy":[104.1203,105.08161,107.64932,106.68134]}, + {"t":1.57391, "x":1.61011, "y":5.74921, "heading":-1.38574, "vx":-0.24157, "vy":4.00295, "omega":-0.31057, "ax":-0.46315, "ay":6.65195, "alpha":-1.29636, "fx":[-12.12649,-1.01017,-3.17928,-13.09552], "fy":[103.73368,104.17218,107.78411,106.72866]}, + {"t":1.61309, "x":1.60029, "y":5.91116, "heading":-1.39791, "vx":-0.25972, "vy":4.2636, "omega":-0.36137, "ax":0.58524, "ay":-5.87733, "alpha":-1.68006, "fx":[4.59756,15.51912,14.38088,2.66694], "fy":[-97.78857,-95.00238,-89.26756,-91.16928]}, + {"t":1.65228, "x":1.59056, "y":6.07372, "heading":-1.41207, "vx":-0.23679, "vy":4.0333, "omega":-0.4272, "ax":0.42855, "ay":-6.65879, "alpha":-0.6872, "fx":[4.33319,9.90986,9.40932,3.56162], "fy":[-106.78232,-106.28978,-104.6161,-105.1647]}, + {"t":1.69146, "x":1.58162, "y":6.22664, "heading":-1.42881, "vx":-0.22, "vy":3.77238, "omega":-0.45413, "ax":0.42774, "ay":-6.67246, "alpha":-0.4682, "fx":[5.05928,8.89558,8.57291,4.63507], "fy":[-106.69023,-106.30306,-105.18766,-105.53961]}, + {"t":1.73065, "x":1.57332, "y":6.36934, "heading":-1.4466, "vx":-0.20324, "vy":3.51093, "omega":-0.47247, "ax":0.41807, "ay":-6.67767, "alpha":-0.26319, "fx":[5.64562,7.80608,7.63522,5.46153], "fy":[-106.47464,-106.20671,-105.59325,-105.77685]}, + {"t":1.76983, "x":1.56568, "y":6.50178, "heading":-1.46512, "vx":-0.18685, "vy":3.24927, "omega":-0.48279, "ax":0.40304, "ay":-6.6809, "alpha":-0.05329, "fx":[6.23538,6.59663,6.55966,6.2028], "fy":[-106.19527,-106.136,-105.94756,-105.97792]}, + {"t":1.80901, "x":1.55867, "y":6.62397, "heading":-1.48403, "vx":-0.17106, "vy":2.98749, "omega":-0.48487, "ax":0.38188, "ay":-6.68353, "alpha":0.16872, "fx":[6.81384,5.26799,5.32293,6.84594], "fy":[-105.91252,-105.99328,-106.30954,-106.20853]}, + {"t":1.8482, "x":1.55226, "y":6.7359, "heading":-1.50303, "vx":-0.1561, "vy":2.7256, "omega":-0.47826, "ax":0.35187, "ay":-6.68611, "alpha":0.39973, "fx":[7.32826,3.76264,3.90103,7.35304], "fy":[-105.63101,-105.82444,-106.66549,-106.46641]}, + {"t":1.88738, "x":1.54641, "y":6.83757, "heading":-1.52177, "vx":-0.14231, "vy":2.46362, "omega":-0.4626, "ax":0.3063, "ay":-6.68901, "alpha":0.63894, "fx":[7.64283,1.95694,2.21857,7.63254], "fy":[-105.37651,-105.6144,-107.01378,-106.76717]}, + {"t":1.92657, "x":1.54107, "y":6.92897, "heading":-1.5399, "vx":-0.13031, "vy":2.20152, "omega":-0.43756, "ax":0.22135, "ay":-6.69286, "alpha":0.88388, "fx":[7.29555,-0.59245,-0.01584,7.36885], "fy":[-105.19306,-105.34738,-107.33763,-107.13814]}, + {"t":1.96575, "x":1.53614, "y":7.0101, "heading":-1.55705, "vx":-0.12163, "vy":1.93926, "omega":-0.40293, "ax":-2.46234, "ay":-6.22812, "alpha":1.16246, "fx":[-36.27202,-45.14902,-41.6741,-33.27056], "fy":[-98.35092,-95.58594,-99.59968,-101.9671]}, + {"t":2.00493, "x":1.52948, "y":7.0813, "heading":-1.57283, "vx":-0.21812, "vy":1.69522, "omega":-0.35738, "ax":-4.87314, "ay":-4.59552, "alpha":1.50777, "fx":[-76.49916,-83.42813,-78.36181,-71.16969], "fy":[-70.97692,-66.3479,-74.99504,-79.50893]}, + {"t":2.04412, "x":1.51719, "y":7.1442, "heading":-1.58684, "vx":-0.40907, "vy":1.51515, "omega":-0.2983, "ax":-5.33307, "ay":-4.05139, "alpha":1.63349, "fx":[-84.1142,-90.47397,-85.4491,-78.62827], "fy":[-61.59585,-56.79517,-66.99059,-71.89378]}, + {"t":2.09046, "x":1.4925, "y":7.21007, "heading":-1.60066, "vx":-0.65623, "vy":1.32739, "omega":-0.22259, "ax":-5.29344, "ay":-4.10489, "alpha":1.45488, "fx":[-83.4021,-89.29237,-84.83361,-78.62076], "fy":[-62.9383,-58.42006,-67.41511,-71.89916]}, + {"t":2.13681, "x":1.45641, "y":7.26718, "heading":-1.61098, "vx":-0.90156, "vy":1.13715, "omega":-0.15517, "ax":-5.24092, "ay":-4.17142, "alpha":1.28668, "fx":[-82.54248,-87.91831,-83.98527,-78.36784], "fy":[-64.34884,-60.27959,-68.10248,-72.16618]}, + {"t":2.18315, "x":1.40899, "y":7.3154, "heading":-1.61817, "vx":-1.14445, "vy":0.94382, "omega":-0.09554, "ax":-5.17053, "ay":-4.25794, "alpha":1.12964, "fx":[-81.39558,-86.32392,-82.85037,-77.77401], "fy":[-66.11116,-62.37205,-69.13885,-72.76985]}, + {"t":2.2295, "x":1.3504, "y":7.35457, "heading":-1.6226, "vx":-1.38408, "vy":0.74648, "omega":-0.04318, "ax":-5.0715, "ay":-4.37488, "alpha":0.97239, "fx":[-79.84621,-84.24394,-81.24132,-76.72348], "fy":[-68.2287,-65.03145,-70.68942,-73.86834]}, + {"t":2.27584, "x":1.28081, "y":7.38447, "heading":-1.6246, "vx":-1.61912, "vy":0.54373, "omega":0.00188, "ax":-4.92234, "ay":-4.54129, "alpha":0.82415, "fx":[-77.46001,-81.43241,-78.84027,-74.8502], "fy":[-71.20199,-68.38161,-73.04919,-75.75242]}, + {"t":2.32219, "x":1.20048, "y":7.40479, "heading":-1.62451, "vx":-1.84725, "vy":0.33326, "omega":0.04008, "ax":-4.67407, "ay":-4.79529, "alpha":0.67498, "fx":[-73.53491,-77.01928,-74.89976,-71.36326], "fy":[-75.46996,-73.22928,-76.78395,-79.0316]}, + {"t":2.36854, "x":1.10985, "y":7.41509, "heading":-1.62265, "vx":-2.06387, "vy":0.11102, "omega":0.07136, "ax":-4.18949, "ay":-5.22209, "alpha":0.53721, "fx":[-65.75127,-68.93592,-67.24976,-64.10769], "fy":[-82.56958,-80.73324,-83.28949,-85.0255]}, + {"t":2.41488, "x":1.0097, "y":7.41462, "heading":-1.61935, "vx":-2.25804, "vy":-0.131, "omega":0.09626, "ax":-2.9397, "ay":-6.01143, "alpha":0.39802, "fx":[-45.70338,-48.62542,-47.60997,-44.74068], "fy":[-95.29829,-94.20102,-95.59363,-96.65018]}, + {"t":2.46123, "x":0.90189, "y":7.4021, "heading":-1.61489, "vx":-2.39428, "vy":-0.40961, "omega":0.11471, "ax":1.65755, "ay":-6.47595, "alpha":0.27523, "fx":[27.73661,25.47952,24.91048,27.13284], "fy":[-102.26531,-102.62751,-103.37136,-102.9775]}, + {"t":2.50757, "x":0.79271, "y":7.37616, "heading":-1.60957, "vx":-2.31746, "vy":-0.70974, "omega":0.12746, "ax":6.51504, "ay":-1.5138, "alpha":0.16056, "fx":[103.7209,103.39345,103.14365,103.46621], "fy":[-23.26189,-23.51722,-24.7666,-24.58478]}, + {"t":2.55392, "x":0.6923, "y":7.34164, "heading":-1.60366, "vx":-2.01552, "vy":-0.77989, "omega":0.1349, "ax":6.6131, "ay":1.0354, "alpha":0.0115, "fx":[105.00199,104.97027,104.9813,104.99777], "fy":[16.46209,16.50424,16.39659,16.3878]}, + {"t":2.60026, "x":0.606, "y":7.30661, "heading":-1.59741, "vx":-1.70903, "vy":-0.73191, "omega":0.13544, "ax":6.40459, "ay":1.95311, "alpha":-0.12665, "fx":[101.6547,101.92579,101.70885,101.42105], "fy":[30.59043,30.42664,31.38731,31.624]}, + {"t":2.64661, "x":0.53367, "y":7.27478, "heading":-1.59113, "vx":-1.41221, "vy":-0.64139, "omega":0.12957, "ax":6.25323, "ay":2.39688, "alpha":-0.27234, "fx":[99.23447,99.90433,99.32361,98.636], "fy":[37.30163,36.72824,38.80149,39.3775]}, + {"t":2.69295, "x":0.47493, "y":7.24763, "heading":-1.58513, "vx":-1.1224, "vy":-0.53031, "omega":0.11694, "ax":6.14898, "ay":2.65455, "alpha":-0.41216, "fx":[97.57417,98.64272,97.68923,96.57193], "fy":[41.02368,40.18025,43.21465,44.15322]}, + {"t":2.7393, "x":0.42952, "y":7.2259, "heading":-1.57971, "vx":-0.83742, "vy":-0.40728, "omega":0.09784, "ax":6.07436, "ay":2.82213, "alpha":-0.55467, "fx":[96.37407,97.88847,96.51574,94.96154], "fy":[43.40048,42.11587,46.20566,47.49141]}, + {"t":2.78565, "x":0.39723, "y":7.21006, "heading":-1.57517, "vx":-0.5559, "vy":-0.27649, "omega":0.07214, "ax":6.01877, "ay":2.93952, "alpha":-0.70408, "fx":[95.49402,97.44902,95.66506,93.60142], "fy":[44.88193,43.3152,48.38883,50.08198]}, + {"t":2.83199, "x":0.37793, "y":7.2004, "heading":-1.57183, "vx":-0.27696, "vy":-0.14025, "omega":0.03951, "ax":5.97589, "ay":3.02624, "alpha":-0.85241, "fx":[94.81022,97.21978,95.01141,92.44521], "fy":[45.91905,43.99928,50.08594,52.17056]}, + {"t":2.87834, "x":0.37152, "y":7.19715, "heading":-1.57, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00967, "ay":-6.696, "alpha":0.00033, "fx":[0.1541,0.15309,0.15308,0.1541], "fy":[-106.30496,-106.30048,-106.30508,-106.30507]}, + {"t":2.92834, "x":0.37153, "y":7.18878, "heading":-1.57, "vx":0.00048, "vy":-0.33483, "omega":0.00002, "ax":0.00477, "ay":-3.30113, "alpha":-0.00083, "fx":[0.07763,0.0738,0.07383,0.07763], "fy":[-52.39935,-52.42791,-52.40212,-52.40212]}, + {"t":2.97834, "x":0.37156, "y":7.16791, "heading":-1.57, "vx":0.00072, "vy":-0.4999, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00344, "fx":[0.00218,-0.00219,-0.00218,0.00219], "fy":[0.02522,-0.0628,0.01871,0.01871]}, + {"t":3.02835, "x":0.37159, "y":7.14291, "heading":-1.57, "vx":0.00072, "vy":-0.4999, "omega":-0.0002, "ax":0.0, "ay":0.0, "alpha":0.00108, "fx":[0.00192,-0.00193,-0.00192,0.00193], "fy":[0.00214,0.00507,-0.00361,-0.0036]}, + {"t":3.07835, "x":0.37163, "y":7.11792, "heading":-1.57001, "vx":0.00072, "vy":-0.4999, "omega":-0.00014, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[0.00152,-0.00152,-0.00152,0.00152], "fy":[0.00169,0.00441,-0.00305,-0.00305]}, + {"t":3.12836, "x":0.37167, "y":7.09292, "heading":-1.57002, "vx":0.00072, "vy":-0.4999, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":0.00066, "fx":[0.00118,-0.00119,-0.00118,0.00119], "fy":[0.00174,0.00264,-0.00219,-0.00219]}, + {"t":3.17836, "x":0.3717, "y":7.06792, "heading":-1.57002, "vx":0.00072, "vy":-0.4999, "omega":-0.00007, "ax":0.0, "ay":0.0, "alpha":0.00052, "fx":[0.00091,-0.00091,-0.00091,0.00091], "fy":[0.00152,0.00193,-0.00173,-0.00173]}, + {"t":3.22837, "x":0.37174, "y":7.04293, "heading":-1.57003, "vx":0.00072, "vy":-0.4999, "omega":-0.00004, "ax":0.0, "ay":0.0, "alpha":0.00041, "fx":[0.00069,-0.00069,-0.00069,0.00069], "fy":[0.00127,0.00157,-0.00142,-0.00142]}, + {"t":3.27837, "x":0.37177, "y":7.01793, "heading":-1.57003, "vx":0.00072, "vy":-0.4999, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":0.00033, "fx":[0.0005,-0.0005,-0.0005,0.0005], "fy":[0.00104,0.00131,-0.00117,-0.00117]}, + {"t":3.32838, "x":0.37181, "y":6.99293, "heading":-1.57003, "vx":0.00072, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00026, "fx":[0.00035,-0.00035,-0.00035,0.00035], "fy":[0.00085,0.00109,-0.00097,-0.00097]}, + {"t":3.37838, "x":0.37185, "y":6.96793, "heading":-1.57003, "vx":0.00072, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.0002, "fx":[0.00022,-0.00023,-0.00022,0.00023], "fy":[0.00069,0.00092,-0.0008,-0.0008]}, + {"t":3.42838, "x":0.37188, "y":6.94294, "heading":-1.57003, "vx":0.00072, "vy":-0.4999, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":0.00016, "fx":[0.00012,-0.00013,-0.00012,0.00013], "fy":[0.00055,0.00077,-0.00066,-0.00066]}, + {"t":3.47839, "x":0.37192, "y":6.91794, "heading":-1.57003, "vx":0.00072, "vy":-0.4999, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":0.00012, "fx":[0.00004,-0.00005,-0.00004,0.00005], "fy":[0.00044,0.00064,-0.00054,-0.00054]}, + {"t":3.52839, "x":0.37195, "y":6.89294, "heading":-1.57003, "vx":0.00072, "vy":-0.4999, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":0.00009, "fx":[-0.00002,0.00002,0.00002,-0.00002], "fy":[0.00036,0.00054,-0.00045,-0.00045]}, + {"t":3.5784, "x":0.37199, "y":6.86795, "heading":-1.57002, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[-0.00007,0.00006,0.00007,-0.00006], "fy":[0.00028,0.00045,-0.00037,-0.00037]}, + {"t":3.6284, "x":0.37203, "y":6.84295, "heading":-1.57002, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":0.00004, "fx":[-0.0001,0.0001,0.0001,-0.0001], "fy":[0.00023,0.00038,-0.0003,-0.0003]}, + {"t":3.67841, "x":0.37206, "y":6.81795, "heading":-1.57002, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":0.00003, "fx":[-0.00013,0.00013,0.00013,-0.00013], "fy":[0.00018,0.00032,-0.00025,-0.00025]}, + {"t":3.72841, "x":0.3721, "y":6.79295, "heading":-1.57002, "vx":0.00072, "vy":-0.4999, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":0.00002, "fx":[-0.00014,0.00014,0.00014,-0.00014], "fy":[0.00014,0.00027,-0.00021,-0.00021]}, + {"t":3.77841, "x":0.37214, "y":6.76796, "heading":-1.57002, "vx":0.00072, "vy":-0.4999, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[-0.00016,0.00015,0.00016,-0.00015], "fy":[0.00012,0.00023,-0.00017,-0.00017]}, + {"t":3.82842, "x":0.37217, "y":6.74296, "heading":-1.57001, "vx":0.00072, "vy":-0.4999, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00016,0.00016,0.00016,-0.00016], "fy":[0.00009,0.00019,-0.00014,-0.00014]}, + {"t":3.87842, "x":0.37221, "y":6.71796, "heading":-1.57001, "vx":0.00072, "vy":-0.4999, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00016,0.00016,0.00016,-0.00016], "fy":[0.00008,0.00016,-0.00012,-0.00012]}, + {"t":3.92843, "x":0.37224, "y":6.69297, "heading":-1.57001, "vx":0.00072, "vy":-0.4999, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00016,0.00016,0.00016,-0.00016], "fy":[0.00006,0.00014,-0.0001,-0.0001]}, + {"t":3.97843, "x":0.37228, "y":6.66797, "heading":-1.57001, "vx":0.00072, "vy":-0.4999, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00016,0.00015,0.00016,-0.00015], "fy":[0.00005,0.00012,-0.00009,-0.00009]}, + {"t":4.02844, "x":0.37232, "y":6.64297, "heading":-1.57, "vx":0.00072, "vy":-0.4999, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00015,0.00015,0.00015,-0.00015], "fy":[0.00005,0.0001,-0.00007,-0.00007]}, + {"t":4.07844, "x":0.37235, "y":6.61797, "heading":-1.57, "vx":0.00072, "vy":-0.4999, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00014,0.00014,0.00014,-0.00014], "fy":[0.00004,0.00009,-0.00006,-0.00006]}, + {"t":4.12844, "x":0.37239, "y":6.59298, "heading":-1.57, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00013,0.00013,0.00013,-0.00013], "fy":[0.00003,0.00007,-0.00005,-0.00005]}, + {"t":4.17845, "x":0.37242, "y":6.56798, "heading":-1.57, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00012,0.00012,0.00012,-0.00012], "fy":[0.00003,0.00006,-0.00005,-0.00004]}, + {"t":4.22845, "x":0.37246, "y":6.54298, "heading":-1.56999, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00011,0.00011,0.00011,-0.00011], "fy":[0.00003,0.00005,-0.00004,-0.00004]}, + {"t":4.27846, "x":0.3725, "y":6.51798, "heading":-1.56999, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.0001,0.0001,0.0001,-0.0001], "fy":[0.00002,0.00004,-0.00003,-0.00003]}, + {"t":4.32846, "x":0.37253, "y":6.49299, "heading":-1.56999, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00009,0.00009,0.00009,-0.00009], "fy":[0.00002,0.00003,-0.00002,-0.00002]}, + {"t":4.37847, "x":0.37257, "y":6.46799, "heading":-1.56999, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00008,0.00008,0.00008,-0.00008], "fy":[0.00002,0.00002,-0.00002,-0.00002]}, + {"t":4.42847, "x":0.3726, "y":6.44299, "heading":-1.56999, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00007,0.00007,0.00007,-0.00007], "fy":[0.00001,0.00001,-0.00001,-0.00001]}, + {"t":4.47847, "x":0.37264, "y":6.418, "heading":-1.56998, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00006,0.00006,0.00006,-0.00006], "fy":[0.00001,0.0,0.0,0.0]}, + {"t":4.52848, "x":0.37268, "y":6.393, "heading":-1.56998, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00005,0.00005,0.00005,-0.00005], "fy":[0.0,-0.00001,0.0,0.0]}, + {"t":4.57848, "x":0.37271, "y":6.368, "heading":-1.56998, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00004,0.00004,0.00004,-0.00004], "fy":[0.0,-0.00002,0.00001,0.00001]}, + {"t":4.62849, "x":0.37275, "y":6.343, "heading":-1.56998, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00004,0.00004,0.00004,-0.00004], "fy":[-0.00001,-0.00003,0.00002,0.00002]}, + {"t":4.67849, "x":0.37279, "y":6.31801, "heading":-1.56998, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00003,0.00003,0.00003,-0.00003], "fy":[-0.00001,-0.00004,0.00003,0.00003]}, + {"t":4.7285, "x":0.37282, "y":6.29301, "heading":-1.56997, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00002,0.00002,0.00002,-0.00002], "fy":[-0.00002,-0.00006,0.00004,0.00004]}, + {"t":4.7785, "x":0.37286, "y":6.26801, "heading":-1.56997, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00001,0.00001,0.00001,-0.00001], "fy":[-0.00003,-0.00007,0.00005,0.00005]}, + {"t":4.82851, "x":0.37289, "y":6.24302, "heading":-1.56997, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.00003,-0.00008,0.00006,0.00006]}, + {"t":4.87851, "x":0.37293, "y":6.21802, "heading":-1.56997, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.00004,-0.00009,0.00007,0.00007]}, + {"t":4.92851, "x":0.37297, "y":6.19302, "heading":-1.56997, "vx":0.00072, "vy":-0.4999, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[0.00001,-0.00001,-0.00001,0.00001], "fy":[-0.00005,-0.0001,0.00008,0.00008]}, + {"t":4.97852, "x":0.373, "y":6.16802, "heading":-1.56996, "vx":0.00072, "vy":-0.4999, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[0.00001,-0.00001,-0.00001,0.00001], "fy":[-0.00006,-0.00012,0.00009,0.00009]}, + {"t":5.02852, "x":0.37304, "y":6.14303, "heading":-1.56996, "vx":0.00072, "vy":-0.4999, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[0.00002,-0.00002,-0.00002,0.00002], "fy":[-0.00006,-0.00013,0.0001,0.0001]}, + {"t":5.07853, "x":0.37307, "y":6.11803, "heading":-1.56996, "vx":0.00072, "vy":-0.4999, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[0.00003,-0.00003,-0.00003,0.00003], "fy":[-0.00007,-0.00015,0.00011,0.00011]}, + {"t":5.12853, "x":0.37311, "y":6.09303, "heading":-1.56996, "vx":0.00072, "vy":-0.4999, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[0.00003,-0.00003,-0.00003,0.00003], "fy":[-0.00008,-0.00016,0.00012,0.00012]}, + {"t":5.17854, "x":0.37315, "y":6.06804, "heading":-1.56996, "vx":0.00072, "vy":-0.4999, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[0.00004,-0.00004,-0.00004,0.00004], "fy":[-0.00009,-0.00018,0.00013,0.00013]}, + {"t":5.22854, "x":0.37318, "y":6.04304, "heading":-1.56996, "vx":0.00072, "vy":-0.4999, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[0.00004,-0.00004,-0.00004,0.00004], "fy":[-0.0001,-0.00019,0.00015,0.00015]}, + {"t":5.27854, "x":0.37322, "y":6.01804, "heading":-1.56996, "vx":0.00072, "vy":-0.4999, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[0.00004,-0.00004,-0.00004,0.00004], "fy":[-0.00011,-0.00021,0.00016,0.00016]}, + {"t":5.32855, "x":0.37326, "y":5.99304, "heading":-1.56995, "vx":0.00072, "vy":-0.4999, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[0.00005,-0.00005,-0.00005,0.00005], "fy":[-0.00012,-0.00023,0.00018,0.00018]}, + {"t":5.37855, "x":0.37329, "y":5.96805, "heading":-1.56995, "vx":0.00072, "vy":-0.4999, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[0.00005,-0.00005,-0.00005,0.00005], "fy":[-0.00014,-0.00025,0.00019,0.00019]}, + {"t":5.42856, "x":0.37333, "y":5.94305, "heading":-1.56995, "vx":0.00072, "vy":-0.4999, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[0.00005,-0.00005,-0.00005,0.00005], "fy":[-0.00015,-0.00027,0.00021,0.00021]}, + {"t":5.47856, "x":0.37336, "y":5.91805, "heading":-1.56995, "vx":0.00072, "vy":-0.4999, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00004, "fx":[0.00005,-0.00005,-0.00005,0.00005], "fy":[-0.00017,-0.00029,0.00023,0.00023]}, + {"t":5.52857, "x":0.3734, "y":5.89306, "heading":-1.56995, "vx":0.00072, "vy":-0.4999, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00004, "fx":[0.00004,-0.00004,-0.00004,0.00004], "fy":[-0.00019,-0.00032,0.00025,0.00025]}, + {"t":5.57857, "x":0.37344, "y":5.86806, "heading":-1.56995, "vx":0.00072, "vy":-0.4999, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00005, "fx":[0.00004,-0.00004,-0.00004,0.00004], "fy":[-0.00021,-0.00035,0.00028,0.00028]}, + {"t":5.62857, "x":0.37347, "y":5.84306, "heading":-1.56995, "vx":0.00072, "vy":-0.4999, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00006, "fx":[0.00003,-0.00003,-0.00003,0.00003], "fy":[-0.00023,-0.00038,0.00031,0.00031]}, + {"t":5.67858, "x":0.37351, "y":5.81806, "heading":-1.56995, "vx":0.00072, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[0.00002,-0.00002,-0.00002,0.00002], "fy":[-0.00026,-0.00042,0.00034,0.00034]}, + {"t":5.72858, "x":0.37354, "y":5.79307, "heading":-1.56994, "vx":0.00072, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00008, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.0003,-0.00046,0.00038,0.00038]}, + {"t":5.77859, "x":0.37358, "y":5.76807, "heading":-1.56994, "vx":0.00072, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00009, "fx":[-0.00002,0.00002,0.00002,-0.00002], "fy":[-0.00034,-0.00051,0.00042,0.00042]}, + {"t":5.82859, "x":0.37362, "y":5.74307, "heading":-1.56994, "vx":0.00072, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.0001, "fx":[-0.00004,0.00004,0.00004,-0.00004], "fy":[-0.00038,-0.00056,0.00047,0.00047]}, + {"t":5.8786, "x":0.37365, "y":5.71808, "heading":-1.56994, "vx":0.00072, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00012, "fx":[-0.00008,0.00008,0.00008,-0.00008], "fy":[-0.00044,-0.00063,0.00053,0.00053]}, + {"t":5.9286, "x":0.37369, "y":5.69308, "heading":-1.56994, "vx":0.00072, "vy":-0.4999, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00014, "fx":[-0.00012,0.00012,0.00012,-0.00012], "fy":[-0.0005,-0.0007,0.0006,0.0006]}, + {"t":5.9786, "x":0.37372, "y":5.66808, "heading":-1.56994, "vx":0.00072, "vy":-0.4999, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00017, "fx":[-0.00017,0.00017,0.00017,-0.00017], "fy":[-0.00058,-0.00079,0.00068,0.00068]}, + {"t":6.02861, "x":0.37376, "y":5.64308, "heading":-1.56995, "vx":0.00072, "vy":-0.4999, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":-0.0002, "fx":[-0.00024,0.00023,0.00024,-0.00023], "fy":[-0.00067,-0.00089,0.00078,0.00078]}, + {"t":6.07861, "x":0.3738, "y":5.61809, "heading":-1.56995, "vx":0.00072, "vy":-0.4999, "omega":-0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00024, "fx":[-0.00031,0.00031,0.00031,-0.00031], "fy":[-0.00078,-0.001,0.00089,0.00089]}, + {"t":6.12862, "x":0.37383, "y":5.59309, "heading":-1.56995, "vx":0.00072, "vy":-0.4999, "omega":-0.00005, "ax":0.0, "ay":0.0, "alpha":-0.00028, "fx":[-0.00041,0.00041,0.00041,-0.00041], "fy":[-0.0009,-0.00114,0.00102,0.00102]}, + {"t":6.17862, "x":0.37387, "y":5.56809, "heading":-1.56995, "vx":0.00072, "vy":-0.4999, "omega":-0.00006, "ax":0.0, "ay":0.0, "alpha":-0.00033, "fx":[-0.00052,0.00052,0.00052,-0.00052], "fy":[-0.00105,-0.00129,0.00117,0.00117]}, + {"t":6.22863, "x":0.37391, "y":5.5431, "heading":-1.56995, "vx":0.00072, "vy":-0.4999, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":-0.00039, "fx":[-0.00065,0.00065,0.00065,-0.00065], "fy":[-0.00122,-0.00148,0.00135,0.00135]}, + {"t":6.27863, "x":0.37394, "y":5.5181, "heading":-1.56996, "vx":0.00072, "vy":-0.4999, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":-0.00047, "fx":[-0.00081,0.00081,0.00081,-0.00081], "fy":[-0.0014,-0.00174,0.00157,0.00157]}, + {"t":6.32864, "x":0.37398, "y":5.4931, "heading":-1.56996, "vx":0.00072, "vy":-0.4999, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":-0.00057, "fx":[-0.001,0.001,0.001,-0.001], "fy":[-0.00156,-0.00224,0.0019,0.0019]}, + {"t":6.37864, "x":0.37401, "y":5.4681, "heading":-1.56997, "vx":0.00072, "vy":-0.4999, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":-0.00073, "fx":[-0.00123,0.00123,0.00123,-0.00123], "fy":[-0.00154,-0.00343,0.00249,0.00249]}, + {"t":6.42864, "x":0.37405, "y":5.44311, "heading":-1.56998, "vx":0.00072, "vy":-0.4999, "omega":-0.00019, "ax":0.0, "ay":0.0, "alpha":-0.00079, "fx":[-0.0015,0.00151,0.0015,-0.00151], "fy":[-0.00215,-0.00299,0.00257,0.00257]}, + {"t":6.47865, "x":0.37409, "y":5.41811, "heading":-1.56999, "vx":0.00072, "vy":-0.4999, "omega":-0.00022, "ax":0.0, "ay":0.0, "alpha":0.00292, "fx":[-0.00166,0.00166,0.00166,-0.00166], "fy":[-0.02097,0.05257,-0.01572,-0.01572]}, + {"t":6.52865, "x":0.37412, "y":5.39311, "heading":-1.57, "vx":0.00072, "vy":-0.4999, "omega":-0.00008, "ax":-0.00477, "ay":3.29913, "alpha":0.00175, "fx":[-0.07732,-0.07403,-0.07403,-0.07732], "fy":[52.36329,52.40906,52.36618,52.36618]}, + {"t":6.57866, "x":0.37415, "y":5.37224, "heading":-1.57, "vx":0.00048, "vy":-0.33493, "omega":0.00001, "ax":-0.00968, "ay":6.698, "alpha":-0.00017, "fx":[-0.15429,-0.15298,-0.15298,-0.15429], "fy":[106.33558,106.33507,106.33586,106.33586]}, + {"t":6.62866, "x":0.37416, "y":5.36387, "heading":-1.57, "vx":0.0, "vy":0.0, "omega":0.0, "ax":6.1842, "ay":-2.56497, "alpha":-13.85279, "fx":[52.3012,104.12915,126.33043,109.95413], "fy":[-112.78973,-74.4501,6.48137,17.87565]}, + {"t":6.65695, "x":0.37664, "y":5.36284, "heading":-1.57, "vx":0.17494, "vy":-0.07256, "omega":-0.39186, "ax":6.18617, "ay":-2.56625, "alpha":-13.54898, "fx":[53.78266,104.12385,125.77102,109.1622], "fy":[-111.25653,-73.59724,6.14684,15.74252]}, + {"t":6.68524, "x":0.38406, "y":5.35976, "heading":-1.58108, "vx":0.34993, "vy":-0.14515, "omega":-0.77513, "ax":6.1858, "ay":-2.56661, "alpha":-13.28681, "fx":[55.02903,103.77271,125.23041,108.78408], "fy":[-109.65254,-73.13581,5.12122,14.68014]}, + {"t":6.71352, "x":0.39644, "y":5.35463, "heading":-1.60301, "vx":0.52491, "vy":-0.21775, "omega":-1.15098, "ax":6.18538, "ay":-2.56699, "alpha":-13.01553, "fx":[56.49282,103.16184,124.67878,108.45611], "fy":[-107.71277,-72.91873,3.5136,14.10638]}, + {"t":6.74181, "x":0.41376, "y":5.34744, "heading":-1.63557, "vx":0.69988, "vy":-0.29037, "omega":-1.51916, "ax":6.18487, "ay":-2.56747, "alpha":-12.73055, "fx":[57.92219,102.29879,124.07377,108.4626], "fy":[-105.49992,-72.89248,1.47119,13.87913]}, + {"t":6.7701, "x":0.43603, "y":5.3382, "heading":-1.67854, "vx":0.87483, "vy":-0.36299, "omega":-1.87928, "ax":6.18428, "ay":-2.568, "alpha":-12.3832, "fx":[59.77405,101.18495,123.37045,108.39049], "fy":[-102.67484,-73.00226,-1.05295,13.65435]}, + {"t":6.79839, "x":0.46325, "y":5.3269, "heading":-1.7317, "vx":1.04977, "vy":-0.43564, "omega":-2.22957, "ax":6.18358, "ay":-2.5686, "alpha":-11.95659, "fx":[61.77527,99.88487,122.50621,108.50913], "fy":[-99.27537,-73.07675,-3.95541,13.19384]}, + {"t":6.82667, "x":0.49542, "y":5.31355, "heading":-1.79477, "vx":1.22469, "vy":-0.5083, "omega":-2.56779, "ax":6.18278, "ay":-2.56918, "alpha":-11.37872, "fx":[64.45677,98.32635,121.40264,108.43869], "fy":[-94.77572,-73.13227,-7.34274,12.10053]}, + {"t":6.85496, "x":0.53254, "y":5.29815, "heading":-1.86741, "vx":1.39959, "vy":-0.58097, "omega":-2.88967, "ax":6.18183, "ay":-2.56971, "alpha":-10.64256, "fx":[67.41778,96.66657,119.98666,108.49323], "fy":[-89.37349,-72.85656,-11.10483,10.15106]}, + {"t":6.88325, "x":0.5746, "y":5.28068, "heading":-1.94915, "vx":1.57446, "vy":-0.65366, "omega":-3.19072, "ax":6.18076, "ay":-2.56995, "alpha":-9.66266, "fx":[71.24086,94.87385,118.13186,108.24954], "fy":[-82.44079,-72.20827,-15.41597,6.86585]}, + {"t":6.91154, "x":0.62161, "y":5.26117, "heading":-2.03941, "vx":1.7493, "vy":-0.72636, "omega":-3.46406, "ax":6.17941, "ay":-2.5699, "alpha":-8.44497, "fx":[75.48173,93.25361,115.73232,107.94293], "fy":[-74.39438,-70.67657,-20.16485,2.03956]}, + {"t":6.93982, "x":0.67357, "y":5.23959, "heading":-2.1374, "vx":1.9241, "vy":-0.79906, "omega":-3.70294, "ax":6.17765, "ay":-2.56919, "alpha":-6.957, "fx":[80.61958,91.91356,112.68956,107.07588], "fy":[-65.08832,-68.1614,-25.30747,-4.59389]}, + {"t":6.96811, "x":0.73047, "y":5.21596, "heading":-2.24215, "vx":2.09885, "vy":-0.87173, "omega":-3.89974, "ax":6.17476, "ay":-2.56781, "alpha":-5.29523, "fx":[85.58504,91.60969,109.04842,105.87196], "fy":[-56.17444,-63.7634,-30.40877,-12.71658]}, + {"t":6.9964, "x":0.79231, "y":5.19027, "heading":-2.35246, "vx":2.27352, "vy":-0.94437, "omega":-4.04953, "ax":6.16862, "ay":-2.56562, "alpha":-3.48154, "fx":[90.40882,92.52902,104.96927,103.81811], "fy":[-48.47994,-57.29773,-35.04223,-22.10424]}, + {"t":7.02469, "x":0.85909, "y":5.16253, "heading":-2.46701, "vx":2.44801, "vy":-1.01695, "omega":-4.14802, "ax":6.16467, "ay":-2.52143, "alpha":-1.53363, "fx":[94.94828,95.14728,100.66656,100.71271], "fy":[-42.16347,-47.96291,-38.35916,-31.63274]}, + {"t":7.05298, "x":0.93081, "y":5.13276, "heading":-2.58435, "vx":2.6224, "vy":-1.08827, "omega":-4.1914, "ax":-0.01902, "ay":-0.05174, "alpha":2.53947, "fx":[8.91959,2.40491,-9.57376,-2.95866], "fy":[-2.27844,8.12494,0.66332,-9.79537]}, + {"t":7.08126, "x":1.00498, "y":5.10195, "heading":-2.70292, "vx":2.62186, "vy":-1.08974, "omega":-4.11956, "ax":-6.16418, "ay":2.52236, "alpha":3.19126, "fx":[-93.12232,-90.78022,-102.73989,-104.80118], "fy":[39.81366,57.37851,40.51597,22.46928]}, + {"t":7.10955, "x":1.07668, "y":5.07213, "heading":-2.81945, "vx":2.44749, "vy":-1.01838, "omega":-4.02929, "ax":-6.16741, "ay":2.56843, "alpha":4.52534, "fx":[-92.61038,-86.08913,-104.42496,-108.52408], "fy":[37.73133,65.50966,43.36701,16.49512]}, + {"t":7.13784, "x":1.14345, "y":5.04435, "heading":-2.93343, "vx":2.27303, "vy":-0.94573, "omega":-3.90128, "ax":-6.17402, "ay":2.56952, "alpha":5.77836, "fx":[-93.51739,-81.33114,-105.40039,-111.8197], "fy":[31.86751,72.69361,47.20819,11.40295]}, + {"t":7.16613, "x":1.20528, "y":5.01863, "heading":-3.04379, "vx":2.09838, "vy":-0.87304, "omega":-3.73782, "ax":-6.17723, "ay":2.57015, "alpha":6.94152, "fx":[-95.68289,-76.33979,-105.48188,-114.76793], "fy":[23.45332,79.16639,52.01276,8.57966]}, + {"t":7.19441, "x":1.26216, "y":4.99496, "heading":3.13367, "vx":1.92364, "vy":-0.80034, "omega":-3.54146, "ax":-6.17924, "ay":2.57028, "alpha":8.02579, "fx":[-98.67227,-71.49994,-104.93242,-117.29549], "fy":[13.57388,84.651,57.06542,7.93008]}, + {"t":7.2227, "x":1.3141, "y":4.97335, "heading":3.03349, "vx":1.74884, "vy":-0.72763, "omega":-3.31443, "ax":-6.18058, "ay":2.57036, "alpha":9.02515, "fx":[-101.97643,-67.40303,-103.75077,-119.35477], "fy":[3.2514,88.90359,62.23699,8.83312]}, + {"t":7.25099, "x":1.3611, "y":4.9538, "heading":2.93973, "vx":1.57401, "vy":-0.65492, "omega":-3.05913, "ax":-6.18153, "ay":2.57042, "alpha":9.93003, "fx":[-105.10358,-64.18346,-102.2682,-120.98978], "fy":[-6.59343,91.98947,67.05698,10.77639]}, + {"t":7.27928, "x":1.40315, "y":4.9363, "heading":2.85319, "vx":1.39915, "vy":-0.58221, "omega":-2.77824, "ax":-6.18212, "ay":2.57075, "alpha":10.71427, "fx":[-107.69287,-61.91651,-100.70023,-122.27297], "fy":[-15.07694,94.12898,71.27786,12.92039]}, + {"t":7.30756, "x":1.44026, "y":4.92086, "heading":2.7746, "vx":1.22427, "vy":-0.50949, "omega":-2.47515, "ax":-6.18249, "ay":2.57121, "alpha":11.36658, "fx":[-109.73727,-60.55843,-99.07717,-123.2334], "fy":[-22.38471,95.31988,75.0323,15.31219]}, + {"t":7.33585, "x":1.47242, "y":4.90747, "heading":2.70459, "vx":1.04938, "vy":-0.43676, "omega":-2.15362, "ax":-6.18267, "ay":2.57187, "alpha":11.88894, "fx":[-111.20576,-59.86147,-97.57037,-123.98016], "fy":[-28.2065,95.98028,78.1903,17.35695]}, + {"t":7.36414, "x":1.49963, "y":4.89615, "heading":2.64367, "vx":0.87449, "vy":-0.36401, "omega":-1.81731, "ax":-6.18279, "ay":2.57248, "alpha":12.29708, "fx":[-112.24052,-59.72186,-96.13319,-124.52957], "fy":[-33.00086,96.06943,80.94855,19.34259]}, + {"t":7.39243, "x":1.52189, "y":4.88688, "heading":2.59226, "vx":0.69959, "vy":-0.29124, "omega":-1.46946, "ax":-6.18281, "ay":2.57316, "alpha":12.62145, "fx":[-112.98818,-59.73793,-94.90882,-124.99167], "fy":[-36.74118,96.06871,83.21738,20.85824]}, + {"t":7.42071, "x":1.53921, "y":4.87967, "heading":2.55069, "vx":0.5247, "vy":-0.21845, "omega":-1.11243, "ax":-6.18286, "ay":2.57367, "alpha":12.88453, "fx":[-113.47173,-60.00443,-93.79987,-125.3535], "fy":[-39.90682,95.89763,85.20019,22.24485]}, + {"t":7.449, "x":1.55158, "y":4.87452, "heading":2.51922, "vx":0.3498, "vy":-0.14565, "omega":-0.74795, "ax":-6.18288, "ay":2.57416, "alpha":13.11294, "fx":[-113.82159,-60.13729,-92.96791,-125.704], "fy":[-42.4646,95.97054,86.77001,23.19087]}, + {"t":7.47729, "x":1.559, "y":4.87143, "heading":2.49807, "vx":0.1749, "vy":-0.07283, "omega":-0.37702, "ax":-6.1829, "ay":2.57457, "alpha":13.32804, "fx":[-114.06339,-60.01986,-92.48101,-126.06807], "fy":[-44.62752,96.47986,87.91252,23.7277]}, + {"t":7.50558, "x":1.56147, "y":4.8704, "heading":2.4874, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[ + {"name":"IntakeDown", "from":{"target":3, "targetTimestamp":2.87834, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeDown"}}}, + {"name":"IntakeUp", "from":{"target":4, "targetTimestamp":6.62866, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeUp"}}}] +} diff --git a/src/main/deploy/choreo/DepotSweep.traj b/src/main/deploy/choreo/DepotSweep.traj new file mode 100644 index 0000000..1026ca1 --- /dev/null +++ b/src/main/deploy/choreo/DepotSweep.traj @@ -0,0 +1,269 @@ +{ + "name":"DepotSweep", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.8245291709899905, "y":2.779820442199707, "heading":-2.57025541936111, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.819257140159607, "y":3.781932830810547, "heading":-1.5800127441632663, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.8013427257537842, "y":5.886612892150879, "heading":-1.57, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.5171908140182495, "y":7.144200325012207, "heading":-1.586837712220261, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.37151533365249634, "y":7.15724515914917, "heading":-1.57, "intervals":77, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.3652238249778747, "y":5.286482334136963, "heading":-1.57, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.5026499032974243, "y":4.99053430557251, "heading":2.611376810175422, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":"first", "to":4, "data":{"type":"MaxVelocity", "props":{"max":4.0}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":8.0}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":5, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"MaxVelocity", "props":{"max":0.5}}, "enabled":true}, + {"from":5, "to":6, "data":{"type":"MaxVelocity", "props":{"max":5.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.8245291709899902 m", "val":1.8245291709899905}, "y":{"exp":"2.779820442199707 m", "val":2.779820442199707}, "heading":{"exp":"-2.5702554193611094 rad", "val":-2.57025541936111}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.819257140159607 m", "val":1.819257140159607}, "y":{"exp":"3.781932830810547 m", "val":3.781932830810547}, "heading":{"exp":"-1.5800127441632665 rad", "val":-1.5800127441632663}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.8013427257537842 m", "val":1.8013427257537842}, "y":{"exp":"5.886612892150879 m", "val":5.886612892150879}, "heading":{"exp":"-3.14 / 2 rad", "val":-1.57}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.5171908140182495 m", "val":1.5171908140182495}, "y":{"exp":"7.144200325012207 m", "val":7.144200325012207}, "heading":{"exp":"-1.586837712220261 rad", "val":-1.586837712220261}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.37151533365249634 m", "val":0.37151533365249634}, "y":{"exp":"7.15724515914917 m", "val":7.15724515914917}, "heading":{"exp":"-3.14 / 2 rad", "val":-1.57}, "intervals":77, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.36522382497787476 m", "val":0.3652238249778747}, "y":{"exp":"5.286482334136963 m", "val":5.286482334136963}, "heading":{"exp":"-3.14 / 2 rad", "val":-1.57}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.5026499032974243 m", "val":1.5026499032974243}, "y":{"exp":"4.99053430557251 m", "val":4.99053430557251}, "heading":{"exp":"2.611376810175422 rad", "val":2.611376810175422}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":"first", "to":4, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"4 m / s", "val":4.0}}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"8 m / s ^ 2", "val":8.0}}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":5, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.5 m / s", "val":0.5}}}, "enabled":true}, + {"from":5, "to":6, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"5 m / s", "val":5.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.50096,1.02758,1.45269,2.1029,5.91531,6.71034], + "samples":[ + {"t":0.0, "x":1.82453, "y":2.77982, "heading":-2.57026, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.05309, "ay":7.98597, "alpha":8.74748, "fx":[62.77193,13.89599,-52.4794,-27.56011], "fy":[118.74194,133.74813,123.69251,130.94984]}, + {"t":0.02178, "x":1.82452, "y":2.78171, "heading":-2.57026, "vx":-0.00116, "vy":0.17394, "omega":0.19053, "ax":-0.05238, "ay":7.99252, "alpha":8.67642, "fx":[62.24952,13.77382,-52.13818,-27.21167], "fy":[118.99365,133.74552,123.81663,130.99266]}, + {"t":0.04356, "x":1.82448, "y":2.7874, "heading":-2.56611, "vx":-0.0023, "vy":0.34803, "omega":0.37951, "ax":-0.05279, "ay":7.99216, "alpha":8.65586, "fx":[62.14375,13.57955,-52.2501,-26.82568], "fy":[119.01941,133.74584,123.74191,131.01838]}, + {"t":0.06534, "x":1.82442, "y":2.79688, "heading":-2.55784, "vx":-0.00345, "vy":0.5221, "omega":0.56804, "ax":-0.05291, "ay":7.99178, "alpha":8.62553, "fx":[62.03097,13.22048,-52.52352,-26.08788], "fy":[119.04561,133.75963,123.59371,131.10234]}, + {"t":0.08712, "x":1.82433, "y":2.81014, "heading":-2.54547, "vx":-0.0046, "vy":0.69617, "omega":0.75591, "ax":-0.05283, "ay":7.99137, "alpha":8.58531, "fx":[61.89611,12.69064,-52.95629,-24.98548], "fy":[119.07955,133.78568,123.37081,131.23933]}, + {"t":0.1089, "x":1.82422, "y":2.8272, "heading":-2.529, "vx":-0.00575, "vy":0.87023, "omega":0.94291, "ax":-0.05223, "ay":7.99093, "alpha":8.535, "fx":[61.72369,11.98581,-53.54145,-23.48485], "fy":[119.12856,133.8218,123.07313,131.42411]}, + {"t":0.13069, "x":1.82408, "y":2.84805, "heading":-2.50847, "vx":-0.00689, "vy":1.04428, "omega":1.12881, "ax":-0.05104, "ay":7.99046, "alpha":8.47462, "fx":[61.4866,11.09635,-54.27087,-21.55317], "fy":[119.20571,133.86537,122.70029,131.64614]}, + {"t":0.15247, "x":1.82392, "y":2.87269, "heading":-2.48388, "vx":-0.008, "vy":1.21832, "omega":1.31339, "ax":-0.04892, "ay":7.98994, "alpha":8.40436, "fx":[61.15245,10.01336,-55.12824,-19.14407], "fy":[119.3263,133.91259,122.25484,131.89088]}, + {"t":0.17425, "x":1.82373, "y":2.90112, "heading":-2.45527, "vx":-0.00907, "vy":1.39234, "omega":1.49644, "ax":-0.04567, "ay":7.98936, "alpha":8.32478, "fx":[60.67675,8.72545,-56.08857,-16.21359], "fy":[119.51097,133.95853,121.74288,132.13561]}, + {"t":0.19603, "x":1.82352, "y":2.93334, "heading":-2.42268, "vx":-0.01006, "vy":1.56636, "omega":1.67776, "ax":-0.0411, "ay":7.98871, "alpha":8.23691, "fx":[60.00523,7.22245,-57.11126,-12.72621], "fy":[119.78375,133.9967,121.17796,132.348]}, + {"t":0.21781, "x":1.82329, "y":2.96936, "heading":-2.38614, "vx":-0.01095, "vy":1.74036, "omega":1.85717, "ax":-0.03503, "ay":7.98795, "alpha":8.14228, "fx":[59.07715,5.49955,-58.13424,-8.66706], "fy":[120.16921,134.01863,120.58483,132.4854]}, + {"t":0.23959, "x":1.82305, "y":3.00916, "heading":-2.34568, "vx":-0.01172, "vy":1.91434, "omega":2.03452, "ax":-0.02783, "ay":7.98704, "alpha":8.04286, "fx":[57.82771,3.55875,-59.07372,-4.08009], "fy":[120.68962,134.01394,120.0007,132.49609]}, + {"t":0.26137, "x":1.82278, "y":3.05275, "heading":-2.30137, "vx":-0.01232, "vy":2.08831, "omega":2.2097, "ax":-0.01971, "ay":7.98593, "alpha":7.94039, "fx":[56.20853,1.4213,-59.81968,0.93818], "fy":[121.35307,133.97034,119.47783,132.32869]}, + {"t":0.28315, "x":1.82251, "y":3.10013, "heading":-2.25324, "vx":-0.01275, "vy":2.26225, "omega":2.38265, "ax":-0.01201, "ay":7.98455, "alpha":7.83553, "fx":[54.1898,-0.88091,-60.25671,6.18487], "fy":[122.1516,133.87462,119.07177,131.9444]}, + {"t":0.30493, "x":1.82223, "y":3.15129, "heading":-2.20135, "vx":-0.01302, "vy":2.43616, "omega":2.55331, "ax":-0.00532, "ay":7.98282, "alpha":7.72599, "fx":[51.78074,-3.29159,-60.26199,11.4347], "fy":[123.052,133.7131,118.83779,131.32942]}, + {"t":0.32671, "x":1.82195, "y":3.20625, "heading":-2.14573, "vx":-0.01313, "vy":2.61003, "omega":2.72159, "ax":-0.00172, "ay":7.98061, "alpha":7.60465, "fx":[49.00299,-5.76035,-59.73467,16.38289], "fy":[124.00878,133.47098,118.80846,130.5041]}, + {"t":0.34849, "x":1.82166, "y":3.26499, "heading":-2.08645, "vx":-0.01317, "vy":2.78386, "omega":2.88723, "ax":-0.00129, "ay":7.97777, "alpha":7.45616, "fx":[45.88465,-8.21617,-58.54302,20.79232], "fy":[124.96499,133.13007,119.00916,129.50731]}, + {"t":0.37027, "x":1.82137, "y":3.32752, "heading":-2.02357, "vx":-0.0132, "vy":2.95762, "omega":3.04963, "ax":-0.00659, "ay":7.97399, "alpha":7.25069, "fx":[42.3914,-10.60267,-56.52143,24.31448], "fy":[125.86918,132.65992,119.43968,128.40326]}, + {"t":0.39206, "x":1.82108, "y":3.39383, "heading":-1.95714, "vx":-0.01334, "vy":3.1313, "omega":3.20755, "ax":-0.01699, "ay":7.96878, "alpha":6.92779, "fx":[38.39164,-12.7864,-53.258,26.574], "fy":[126.65696,132.00556,120.1312,127.24747]}, + {"t":0.41384, "x":1.82079, "y":3.46392, "heading":-1.88728, "vx":-0.01371, "vy":3.30487, "omega":3.35845, "ax":-0.0374, "ay":7.96106, "alpha":6.34543, "fx":[33.39613,-14.51437,-47.83784,26.58127], "fy":[127.24361,131.0398,121.12665,126.1409]}, + {"t":0.43562, "x":1.82048, "y":3.53779, "heading":-1.81413, "vx":-0.01452, "vy":3.47826, "omega":3.49665, "ax":-0.06967, "ay":7.94801, "alpha":5.0895, "fx":[25.80857,-14.75977,-37.3586,21.88582], "fy":[127.40098,129.44745,122.62876,125.24452]}, + {"t":0.4574, "x":1.82015, "y":3.61544, "heading":-1.73797, "vx":-0.01604, "vy":3.65138, "omega":3.60751, "ax":-0.13812, "ay":7.91577, "alpha":1.70616, "fx":[7.50296,-8.32446,-13.0325,5.08317], "fy":[126.3635,126.49024,124.76441,125.0564]}, + {"t":0.47918, "x":1.81977, "y":3.69685, "heading":-1.6594, "vx":-0.01905, "vy":3.82379, "omega":3.64467, "ax":-0.39755, "ay":7.59339, "alpha":-5.29269, "fx":[-40.55512,21.32969,16.60577,-22.62619], "fy":[114.83949,118.8393,124.58176,123.94192]}, + {"t":0.50096, "x":1.81926, "y":3.78193, "heading":-1.58001, "vx":-0.02771, "vy":3.98918, "omega":3.52939, "ax":-0.81398, "ay":0.36915, "alpha":-25.37188, "fx":[-81.53116,57.1299,46.46058,-73.74953], "fy":[-58.06397,-65.46316,78.17056,68.79855]}, + {"t":0.5249, "x":1.81836, "y":3.87753, "heading":-1.49553, "vx":-0.04719, "vy":3.99802, "omega":2.92205, "ax":-0.56356, "ay":-0.00669, "alpha":-24.14169, "fx":[-66.87259,58.18337,44.1321,-71.2309], "fy":[-65.904,-65.04702,72.05492,58.47158]}, + {"t":0.54883, "x":1.81707, "y":3.97323, "heading":-1.42558, "vx":-0.06068, "vy":3.99786, "omega":2.34416, "ax":-0.41807, "ay":-0.00683, "alpha":-22.8157, "fx":[-54.73853,58.46375,38.55481,-68.82846], "fy":[-68.15222,-56.96321,71.73753,52.94443]}, + {"t":0.57277, "x":1.8155, "y":4.06893, "heading":-1.36947, "vx":-0.07069, "vy":3.99769, "omega":1.79801, "ax":-0.28186, "ay":-0.0052, "alpha":-21.17332, "fx":[-45.5675,57.25777,34.95344,-64.54237], "fy":[-65.41177,-50.4562,68.1632,47.37474]}, + {"t":0.59671, "x":1.81372, "y":4.16462, "heading":-1.32643, "vx":-0.07744, "vy":3.99757, "omega":1.29117, "ax":-0.15467, "ay":-0.00306, "alpha":-19.38682, "fx":[-36.91763,55.44492,31.33655,-59.68558], "fy":[-62.39011,-42.74665,63.50994,41.43269]}, + {"t":0.62065, "x":1.81183, "y":4.26031, "heading":-1.29552, "vx":-0.08114, "vy":3.9975, "omega":0.8271, "ax":-0.12725, "ay":-0.00261, "alpha":-17.38918, "fx":[-31.94411,50.95697,26.97577,-54.0691], "fy":[-56.21143,-37.64626,57.55287,36.13883]}, + {"t":0.64458, "x":1.80985, "y":4.356, "heading":-1.27572, "vx":-0.08419, "vy":3.99743, "omega":0.41085, "ax":-0.04282, "ay":-0.00091, "alpha":-15.43743, "fx":[-25.77254,46.67882,24.1918,-47.81713], "fy":[-51.28719,-31.48327,51.36562,31.34701]}, + {"t":0.66852, "x":1.80782, "y":4.45169, "heading":-1.26589, "vx":-0.08521, "vy":3.99741, "omega":0.04131, "ax":-0.054, "ay":-0.00115, "alpha":-13.31686, "fx":[-22.63485,40.38691,20.2954,-41.47692], "fy":[-43.71753,-27.67285,44.48155,26.83559]}, + {"t":0.69246, "x":1.80576, "y":4.54738, "heading":-1.2649, "vx":-0.0865, "vy":3.99739, "omega":-0.27746, "ax":-0.0293, "ay":-0.00065, "alpha":-11.3491, "fx":[-18.52061,34.47769,17.55455,-35.3722], "fy":[-38.04396,-22.664,37.80059,22.86614]}, + {"t":0.7164, "x":1.80369, "y":4.64306, "heading":-1.27154, "vx":-0.08721, "vy":3.99737, "omega":-0.54913, "ax":-0.01863, "ay":-0.0004, "alpha":-9.29597, "fx":[-15.61679,28.36732,14.66887,-28.60275], "fy":[-30.47567,-19.40722,30.90641,18.95078]}, + {"t":0.74033, "x":1.80159, "y":4.73875, "heading":-1.28468, "vx":-0.08765, "vy":3.99736, "omega":-0.77165, "ax":-0.05778, "ay":-0.00129, "alpha":-7.41269, "fx":[-13.12072,21.60817,11.31049,-23.46695], "fy":[-24.70652,-15.16615,24.4119,15.37872]}, + {"t":0.76427, "x":1.79948, "y":4.83444, "heading":-1.30316, "vx":-0.08904, "vy":3.99733, "omega":-0.94909, "ax":0.02166, "ay":0.00048, "alpha":-5.52965, "fx":[-9.21421,17.04194,9.78262,-16.23464], "fy":[-17.89309,-11.94411,18.08721,11.7806]}, + {"t":0.78821, "x":1.79735, "y":4.93012, "heading":-1.32588, "vx":-0.08852, "vy":3.99734, "omega":-1.08146, "ax":-0.05488, "ay":-0.00123, "alpha":-3.75955, "fx":[-7.51702,10.24389,5.80091,-12.01256], "fy":[-12.27775,-8.15661,12.11117,8.24486]}, + {"t":0.81215, "x":1.79522, "y":5.02581, "heading":-1.35176, "vx":-0.08983, "vy":3.99731, "omega":-1.17145, "ax":0.13409, "ay":0.00296, "alpha":-2.04585, "fx":[-1.5985,8.05662,5.94416,-3.88721], "fy":[-6.51258,-4.53293,6.5292,4.70405]}, + {"t":0.83608, "x":1.79311, "y":5.1215, "heading":-1.3798, "vx":-0.08662, "vy":3.99738, "omega":-1.22042, "ax":0.13371, "ay":0.00285, "alpha":-0.35386, "fx":[1.38548,3.09069,2.86378,1.15079], "fy":[-0.89964,-0.99319,1.11933,0.95448]}, + {"t":0.86002, "x":1.79107, "y":5.21718, "heading":-1.40902, "vx":-0.08342, "vy":3.99745, "omega":-1.2289, "ax":0.45534, "ay":0.00887, "alpha":1.2742, "fx":[9.95699,3.5603,4.79272,10.60571], "fy":[3.89606,3.42262,-3.91815,-2.83695]}, + {"t":0.88396, "x":1.7892, "y":5.31288, "heading":-1.43843, "vx":-0.07252, "vy":3.99766, "omega":-1.19839, "ax":0.85465, "ay":0.01333, "alpha":2.91767, "fx":[19.44215,5.59596,7.70923,21.52531], "fy":[9.55117,7.05389,-8.87038,-6.88822]}, + {"t":0.9079, "x":1.78771, "y":5.40857, "heading":-1.46712, "vx":-0.05206, "vy":3.99798, "omega":-1.12855, "ax":1.273, "ay":0.01171, "alpha":4.54449, "fx":[30.22366,8.03786,10.72347,31.85407], "fy":[13.42523,12.34809,-13.80613,-11.22347]}, + {"t":0.93183, "x":1.78683, "y":5.50428, "heading":-1.49414, "vx":-0.02159, "vy":3.99826, "omega":-1.01977, "ax":2.73649, "ay":-0.00772, "alpha":5.79098, "fx":[55.44262,29.12791,31.58604,57.6187], "fy":[17.80904,15.5498,-18.63823,-15.21116]}, + {"t":0.95577, "x":1.7871, "y":5.59998, "heading":-1.51855, "vx":0.04392, "vy":3.99808, "omega":-0.88115, "ax":3.10474, "ay":-0.06317, "alpha":7.42852, "fx":[65.69617,31.34219,33.60514,66.51672], "fy":[20.16007,21.09829,-24.62767,-20.64195]}, + {"t":0.97971, "x":1.78904, "y":5.69567, "heading":-1.53964, "vx":0.11823, "vy":3.99656, "omega":-0.70333, "ax":5.67682, "ay":-0.26523, "alpha":5.77861, "fx":[98.60022,82.05495,81.67411,98.1652], "fy":[15.65135,18.02051,-28.92444,-21.59032]}, + {"t":1.00365, "x":1.7935, "y":5.79126, "heading":-1.55648, "vx":0.25412, "vy":3.99022, "omega":-0.565, "ax":6.15308, "ay":-0.57758, "alpha":6.67546, "fx":[107.5998,89.91048,87.58973,105.63861], "fy":[13.13904,18.24,-39.52488,-28.53215]}, + {"t":1.02758, "x":1.80134, "y":5.88661, "heading":-1.57, "vx":0.40141, "vy":3.97639, "omega":-0.40521, "ax":-0.32593, "ay":0.08792, "alpha":10.78611, "fx":[21.13147,-31.36311,-31.21452,20.7484], "fy":[32.05921,30.65876,-29.02278,-28.1118]}, + {"t":1.0512, "x":1.81073, "y":5.98055, "heading":-1.57957, "vx":0.39372, "vy":3.97847, "omega":-0.15047, "ax":-7.60726, "ay":-1.40468, "alpha":1.67395, "fx":[-121.40293,-122.68219,-120.50184,-118.49635], "fy":[-15.65133,-14.00033,-27.84533,-31.70439]}, + {"t":1.07482, "x":1.81791, "y":6.07411, "heading":-1.58312, "vx":0.21406, "vy":3.94529, "omega":-0.11094, "ax":-6.29397, "ay":-4.83579, "alpha":0.949, "fx":[-100.38968,-103.76167,-99.58179,-95.9524], "fy":[-75.59872,-71.70347,-77.98725,-81.79747]}, + {"t":1.09844, "x":1.82121, "y":6.16594, "heading":-1.58574, "vx":0.06541, "vy":3.83109, "omega":-0.08853, "ax":-5.72921, "ay":-5.53725, "alpha":0.80029, "fx":[-91.01624,-94.75036,-90.97049,-87.08485], "fy":[-87.42181,-83.8776,-88.4585,-91.87386]}, + {"t":1.12205, "x":1.82116, "y":6.25487, "heading":-1.58783, "vx":-0.06989, "vy":3.70031, "omega":-0.06963, "ax":-5.45743, "ay":-5.8201, "alpha":0.71592, "fx":[-86.51528,-90.24598,-86.79818,-83.00357], "fy":[-92.15287,-88.91147,-92.72621,-95.80264]}, + {"t":1.14567, "x":1.81798, "y":6.34064, "heading":-1.58948, "vx":-0.19878, "vy":3.56286, "omega":-0.05272, "ax":-5.30304, "ay":-5.96837, "alpha":0.64363, "fx":[-83.98568,-87.51981,-84.42095,-80.83234], "fy":[-94.60875,-91.69528,-94.95996,-97.74488]}, + {"t":1.16929, "x":1.81181, "y":6.42312, "heading":-1.59072, "vx":-0.32402, "vy":3.42191, "omega":-0.03752, "ax":-5.20216, "ay":-6.06078, "alpha":0.58229, "fx":[-82.34167,-85.65727,-82.84543,-79.50809], "fy":[-96.13927,-93.49467,-96.36236,-98.88108]}, + {"t":1.1929, "x":1.80271, "y":6.50224, "heading":-1.59161, "vx":-0.44688, "vy":3.27877, "omega":-0.02377, "ax":-5.13192, "ay":-6.12319, "alpha":0.52183, "fx":[-81.21753,-84.25123,-81.74052,-78.68266], "fy":[-97.16111,-94.80171,-97.30599,-99.57159]}, + {"t":1.21652, "x":1.79072, "y":6.57797, "heading":-1.59217, "vx":-0.56808, "vy":3.13416, "omega":-0.01144, "ax":-5.07946, "ay":-6.16877, "alpha":0.46607, "fx":[-80.38441,-83.14506,-80.89755,-78.13328], "fy":[-97.90928,-95.79996,-98.00251,-100.02328]}, + {"t":1.24014, "x":1.77589, "y":6.65027, "heading":-1.59244, "vx":-0.68804, "vy":2.98848, "omega":-0.00044, "ax":-5.03949, "ay":-6.20295, "alpha":0.40932, "fx":[-79.76868,-82.21516,-80.24882,-77.78968], "fy":[-98.45988,-96.6205,-98.52206,-100.30275]}, + {"t":1.26375, "x":1.75823, "y":6.71912, "heading":-1.59245, "vx":-0.80706, "vy":2.84198, "omega":0.00923, "ax":-5.00757, "ay":-6.22988, "alpha":0.35537, "fx":[-79.27938,-81.42828,-79.71878,-77.56922], "fy":[-98.89845,-97.30126,-98.93554,-100.48057]}, + {"t":1.28737, "x":1.73778, "y":6.7845, "heading":-1.59223, "vx":-0.92532, "vy":2.69485, "omega":0.01762, "ax":-4.98172, "ay":-6.25149, "alpha":0.30047, "fx":[-78.89737,-80.72442,-79.28101,-77.45119], "fy":[-99.24303,-97.90088,-99.26789,-100.57586]}, + {"t":1.31099, "x":1.71454, "y":6.8464, "heading":-1.59182, "vx":-1.04297, "vy":2.54721, "omega":0.02472, "ax":-4.96025, "ay":-6.26928, "alpha":0.24759, "fx":[-78.58424,-80.10023,-78.91051,-77.39523], "fy":[-99.52946,-98.42502,-99.54245,-100.62082]}, + {"t":1.3346, "x":1.68852, "y":6.90481, "heading":-1.59123, "vx":-1.16012, "vy":2.39915, "omega":0.03057, "ax":-4.94223, "ay":-6.28411, "alpha":0.19389, "fx":[-78.33136,-79.52272,-78.59165,-77.40025], "fy":[-99.76444,-98.90451,-99.7728,-100.61775]}, + {"t":1.35822, "x":1.65974, "y":6.95972, "heading":-1.59051, "vx":-1.27684, "vy":2.25074, "omega":0.03514, "ax":-4.92676, "ay":-6.29676, "alpha":0.14118, "fx":[-78.11919,-78.99012,-78.31246,-77.44204], "fy":[-99.96591,-99.34171,-99.96993,-100.58545]}, + {"t":1.38184, "x":1.62822, "y":7.01111, "heading":-1.58968, "vx":-1.39319, "vy":2.10203, "omega":0.03848, "ax":-4.91346, "ay":-6.30759, "alpha":0.08807, "fx":[-77.94332,-78.48848,-78.0652,-77.52216], "fy":[-100.13715,-99.74984,-100.1404,-100.52324]}, + {"t":1.40545, "x":1.59394, "y":7.059, "heading":-1.58877, "vx":-1.50923, "vy":1.95307, "omega":0.04056, "ax":-4.90181, "ay":-6.31704, "alpha":0.03545, "fx":[-77.79452,-78.01365,-77.84527,-77.6256], "fy":[-100.28672,-100.13259,-100.28858,-100.44256]}, + {"t":1.42907, "x":1.55693, "y":7.10336, "heading":-1.58782, "vx":-1.625, "vy":1.80388, "omega":0.0414, "ax":-4.8916, "ay":-6.32528, "alpha":-0.0173, "fx":[-77.66811,-77.56199,-77.64543,-77.75551], "fy":[-100.41799,-100.49403,-100.42032,-100.34138]}, + {"t":1.45269, "x":1.51719, "y":7.1442, "heading":-1.58684, "vx":-1.74052, "vy":1.6545, "omega":0.04099, "ax":-4.80327, "ay":-6.39133, "alpha":-0.0312, "fx":[-76.27876,-76.07901,-76.23328,-76.43066], "fy":[-101.46394,-101.60272,-101.46954,-101.33224]}, + {"t":1.49093, "x":1.44711, "y":7.20281, "heading":-1.58527, "vx":-1.92423, "vy":1.41004, "omega":0.03979, "ax":-4.59887, "ay":-6.54254, "alpha":-0.03394, "fx":[-73.04087,-72.81648,-72.9776,-73.20685], "fy":[-103.86201,-104.00779,-103.87503,-103.72564]}, + {"t":1.52918, "x":1.37014, "y":7.25195, "heading":-1.58375, "vx":-2.10013, "vy":1.1598, "omega":0.0385, "ax":-4.31745, "ay":-6.731, "alpha":-0.03699, "fx":[-68.58734,-68.32433,-68.49903,-68.76003], "fy":[-106.84818,-107.00456,-106.87048,-106.715]}, + {"t":1.56743, "x":1.28666, "y":7.29139, "heading":-1.58228, "vx":-2.26527, "vy":0.90235, "omega":0.03708, "ax":-3.91305, "ay":-6.9731, "alpha":-0.03962, "fx":[-62.18497,-61.8839,-62.05843,-62.36298], "fy":[-110.68641,-110.84328,-110.72088,-110.56159]}, + {"t":1.60568, "x":1.19716, "y":7.3208, "heading":-1.58086, "vx":-2.41493, "vy":0.63565, "omega":0.03557, "ax":-3.29296, "ay":-7.2854, "alpha":-0.04241, "fx":[-52.37271,-52.01514,-52.18409,-52.54067], "fy":[-115.63729,-115.78827,-115.68472,-115.53384]}, + {"t":1.64393, "x":1.10238, "y":7.33978, "heading":-1.5795, "vx":-2.54088, "vy":0.357, "omega":0.03394, "ax":-2.26072, "ay":-7.66721, "alpha":-0.0442, "fx":[-36.03744,-35.61734,-35.74544,-36.16201], "fy":[-121.69818,-121.81429,-121.74679,-121.63115]}, + {"t":1.68217, "x":1.00354, "y":7.34783, "heading":-1.5782, "vx":-2.62735, "vy":0.06374, "omega":0.03225, "ax":-0.39355, "ay":-7.98157, "alpha":-0.04719, "fx":[-6.48133,-5.99259,-6.01335,-6.50454], "fy":[-126.7187,-126.74157,-126.70784,-126.68479]}, + {"t":1.72042, "x":0.90277, "y":7.34443, "heading":-1.57697, "vx":-2.6424, "vy":-0.24154, "omega":0.03045, "ax":2.89358, "ay":-7.44595, "alpha":-0.05209, "fx":[45.61014,46.06766,46.26248,45.81055], "fy":[-118.3485,-118.18361,-118.07191,-118.23581]}, + {"t":1.75867, "x":0.80382, "y":7.32975, "heading":-1.5758, "vx":-2.53173, "vy":-0.52633, "omega":0.02846, "ax":6.48427, "ay":-4.66512, "alpha":-0.06347, "fx":[102.6915,102.90882,103.19051,102.97927], "fy":[-74.40632,-74.15161,-73.72151,-73.96921]}, + {"t":1.79692, "x":0.71173, "y":7.3062, "heading":-1.57471, "vx":-2.28372, "vy":-0.70476, "omega":0.02603, "ax":7.88212, "ay":-1.31312, "alpha":-0.07163, "fx":[125.05806,125.10795,125.21085,125.16105], "fy":[-21.21747,-21.13348,-20.47474,-20.56158]}, + {"t":1.83517, "x":0.63014, "y":7.27829, "heading":-1.57372, "vx":-1.98224, "vy":-0.75499, "omega":0.02329, "ax":7.95432, "ay":0.78762, "alpha":-0.07606, "fx":[126.29187,126.33491,126.26976,126.2259], "fy":[12.17843,12.1231,12.8268,12.88763]}, + {"t":1.87341, "x":0.56015, "y":7.24999, "heading":-1.57283, "vx":-1.67801, "vy":-0.72486, "omega":0.02038, "ax":7.74678, "ay":1.97598, "alpha":-0.07954, "fx":[123.03193,123.10916,122.9405,122.86169], "fy":[31.08885,30.94101,31.64987,31.80071]}, + {"t":1.91166, "x":0.50163, "y":7.22371, "heading":-1.57205, "vx":-1.38171, "vy":-0.64928, "omega":0.01734, "ax":7.52858, "ay":2.69337, "alpha":-0.08304, "fx":[119.58217,119.69802,119.46206,119.34441], "fy":[42.50762,42.30025,43.01016,43.21912]}, + {"t":1.94991, "x":0.45429, "y":7.20084, "heading":-1.57138, "vx":-1.09376, "vy":-0.54627, "omega":0.01416, "ax":7.34502, "ay":3.16161, "alpha":-0.08664, "fx":[116.67524,116.82572,116.54129,116.38808], "fy":[49.95884,49.71146,50.42531,50.67564]}, + {"t":1.98816, "x":0.41783, "y":7.18226, "heading":-1.57084, "vx":-0.81282, "vy":-0.42534, "omega":0.01085, "ax":7.19658, "ay":3.48748, "alpha":-0.09048, "fx":[114.32159,114.5049,114.1817,113.99552], "fy":[55.14563,54.86288,55.58597,55.87068]}, + {"t":2.02641, "x":0.392, "y":7.16854, "heading":-1.57043, "vx":-0.53757, "vy":-0.29195, "omega":0.00739, "ax":7.07651, "ay":3.7259, "alpha":-0.0945, "fx":[112.41822,112.62995,112.27296,112.05789], "fy":[58.937,58.62755,59.36461,59.67664]}, + {"t":2.06465, "x":0.37662, "y":7.1601, "heading":-1.57014, "vx":-0.26691, "vy":-0.14945, "omega":0.00377, "ax":6.97834, "ay":3.90728, "alpha":-0.09862, "fx":[110.86029,111.10045,110.71402,110.47006], "fy":[61.82302,61.48537,62.2379,62.57769]}, + {"t":2.1029, "x":0.37152, "y":7.15725, "heading":-1.57, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.02689, "ay":-7.9958, "alpha":0.0, "fx":[-0.42691,-0.4269,-0.4269,-0.42691], "fy":[-126.9392,-126.9392,-126.9392,-126.9392]}, + {"t":2.15241, "x":0.37148, "y":7.14744, "heading":-1.57, "vx":-0.00133, "vy":-0.39589, "omega":0.0, "ax":-0.00706, "ay":-2.1007, "alpha":0.0005, "fx":[-0.11146,-0.11286,-0.11287,-0.11145], "fy":[-33.34994,-33.34664,-33.35198,-33.35198]}, + {"t":2.20193, "x":0.37141, "y":7.12527, "heading":-1.57, "vx":-0.00168, "vy":-0.4999, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.0016, "fx":[0.00069,-0.00069,-0.00069,0.00069], "fy":[0.01159,-0.02854,0.00843,0.00843]}, + {"t":2.25144, "x":0.37132, "y":7.10052, "heading":-1.57, "vx":-0.00168, "vy":-0.4999, "omega":-0.00005, "ax":0.0, "ay":0.0, "alpha":0.00047, "fx":[0.00058,-0.00058,-0.00058,0.00058], "fy":[0.00095,0.00266,-0.00181,-0.00181]}, + {"t":2.30095, "x":0.37124, "y":7.07577, "heading":-1.57, "vx":-0.00168, "vy":-0.4999, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00038, "fx":[0.00036,-0.00036,-0.00036,0.00036], "fy":[0.00067,0.00243,-0.00155,-0.00155]}, + {"t":2.35046, "x":0.37116, "y":7.05102, "heading":-1.57, "vx":-0.00168, "vy":-0.4999, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":0.00024, "fx":[0.00019,-0.00019,-0.00019,0.00019], "fy":[0.00078,0.00125,-0.00102,-0.00101]}, + {"t":2.39997, "x":0.37107, "y":7.02627, "heading":-1.57, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00016, "fx":[0.00006,-0.00007,-0.00006,0.00007], "fy":[0.00072,0.00077,-0.00075,-0.00074]}, + {"t":2.44948, "x":0.37099, "y":7.00151, "heading":-1.57, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.00011, "fx":[-0.00004,0.00003,0.00004,-0.00003], "fy":[0.0006,0.00057,-0.00059,-0.00058]}, + {"t":2.499, "x":0.37091, "y":6.97676, "heading":-1.57, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.00008, "fx":[-0.00012,0.00011,0.00012,-0.00011], "fy":[0.0005,0.00045,-0.00047,-0.00047]}, + {"t":2.54851, "x":0.37083, "y":6.95201, "heading":-1.57, "vx":-0.00168, "vy":-0.4999, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":0.00005, "fx":[-0.00018,0.00017,0.00018,-0.00017], "fy":[0.00041,0.00036,-0.00039,-0.00038]}, + {"t":2.59802, "x":0.37074, "y":6.92726, "heading":-1.57, "vx":-0.00168, "vy":-0.4999, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":0.00002, "fx":[-0.00022,0.00022,0.00022,-0.00022], "fy":[0.00034,0.00028,-0.00031,-0.00031]}, + {"t":2.64753, "x":0.37066, "y":6.90251, "heading":-1.57, "vx":-0.00168, "vy":-0.4999, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[-0.00025,0.00025,0.00025,-0.00025], "fy":[0.00029,0.00022,-0.00026,-0.00025]}, + {"t":2.69704, "x":0.37058, "y":6.87776, "heading":-1.57, "vx":-0.00168, "vy":-0.4999, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00027,0.00027,0.00027,-0.00027], "fy":[0.00024,0.00017,-0.00021,-0.00021]}, + {"t":2.74656, "x":0.37049, "y":6.85301, "heading":-1.57, "vx":-0.00168, "vy":-0.4999, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.00028,0.00028,0.00028,-0.00028], "fy":[0.00021,0.00013,-0.00017,-0.00017]}, + {"t":2.79607, "x":0.37041, "y":6.82826, "heading":-1.57, "vx":-0.00168, "vy":-0.4999, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.00029,0.00029,0.00029,-0.00029], "fy":[0.00019,0.0001,-0.00015,-0.00014]}, + {"t":2.84558, "x":0.37033, "y":6.80351, "heading":-1.57, "vx":-0.00168, "vy":-0.4999, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.00029,0.00029,0.00029,-0.00029], "fy":[0.00017,0.00008,-0.00012,-0.00012]}, + {"t":2.89509, "x":0.37024, "y":6.77876, "heading":-1.57, "vx":-0.00168, "vy":-0.4999, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.00028,0.00028,0.00028,-0.00028], "fy":[0.00015,0.00006,-0.00011,-0.0001]}, + {"t":2.9446, "x":0.37016, "y":6.75401, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.00028,0.00027,0.00028,-0.00027], "fy":[0.00014,0.00005,-0.0001,-0.00009]}, + {"t":2.99411, "x":0.37008, "y":6.72926, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.00027,0.00026,0.00027,-0.00026], "fy":[0.00013,0.00004,-0.00009,-0.00008]}, + {"t":3.04363, "x":0.36999, "y":6.70451, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.00025,0.00025,0.00025,-0.00025], "fy":[0.00013,0.00003,-0.00008,-0.00008]}, + {"t":3.09314, "x":0.36991, "y":6.67976, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.00024,0.00024,0.00024,-0.00024], "fy":[0.00012,0.00002,-0.00007,-0.00007]}, + {"t":3.14265, "x":0.36983, "y":6.655, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.00022,0.00022,0.00022,-0.00022], "fy":[0.00012,0.00002,-0.00007,-0.00007]}, + {"t":3.19216, "x":0.36974, "y":6.63025, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.00021,0.00021,0.00021,-0.00021], "fy":[0.00012,0.00002,-0.00007,-0.00007]}, + {"t":3.24167, "x":0.36966, "y":6.6055, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.00019,0.00019,0.00019,-0.00019], "fy":[0.00011,0.00002,-0.00007,-0.00006]}, + {"t":3.29119, "x":0.36958, "y":6.58075, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.00018,0.00018,0.00018,-0.00018], "fy":[0.00011,0.00002,-0.00006,-0.00006]}, + {"t":3.3407, "x":0.36949, "y":6.556, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.00016,0.00016,0.00016,-0.00016], "fy":[0.00011,0.00002,-0.00006,-0.00006]}, + {"t":3.39021, "x":0.36941, "y":6.53125, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00015,0.00014,0.00015,-0.00014], "fy":[0.0001,0.00002,-0.00006,-0.00006]}, + {"t":3.43972, "x":0.36933, "y":6.5065, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00013,0.00013,0.00013,-0.00013], "fy":[0.0001,0.00002,-0.00006,-0.00006]}, + {"t":3.48923, "x":0.36924, "y":6.48175, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00012,0.00011,0.00012,-0.00011], "fy":[0.00009,0.00002,-0.00006,-0.00005]}, + {"t":3.53874, "x":0.36916, "y":6.457, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.0001,0.0001,0.0001,-0.0001], "fy":[0.00009,0.00002,-0.00005,-0.00005]}, + {"t":3.58826, "x":0.36908, "y":6.43225, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00009,0.00009,0.00009,-0.00009], "fy":[0.00008,0.00001,-0.00005,-0.00005]}, + {"t":3.63777, "x":0.36899, "y":6.4075, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00007,0.00007,0.00007,-0.00007], "fy":[0.00008,0.00001,-0.00004,-0.00004]}, + {"t":3.68728, "x":0.36891, "y":6.38275, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00006,0.00006,0.00006,-0.00006], "fy":[0.00007,0.00001,-0.00004,-0.00004]}, + {"t":3.73679, "x":0.36883, "y":6.358, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00005,0.00005,0.00005,-0.00005], "fy":[0.00006,0.00001,-0.00004,-0.00004]}, + {"t":3.7863, "x":0.36874, "y":6.33324, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00004,0.00004,0.00004,-0.00004], "fy":[0.00005,0.00001,-0.00003,-0.00003]}, + {"t":3.83582, "x":0.36866, "y":6.30849, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00002,0.00002,0.00002,-0.00002], "fy":[0.00005,0.0,-0.00002,-0.00002]}, + {"t":3.88533, "x":0.36858, "y":6.28374, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,0.00001,0.00001,-0.00001], "fy":[0.00004,0.0,-0.00002,-0.00002]}, + {"t":3.93484, "x":0.36849, "y":6.25899, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.00003,0.0,-0.00001,-0.00001]}, + {"t":3.98435, "x":0.36841, "y":6.23424, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,-0.00001,-0.00001,0.00001], "fy":[0.00002,-0.00001,-0.00001,-0.00001]}, + {"t":4.03386, "x":0.36833, "y":6.20949, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,-0.00002,-0.00002,0.00002], "fy":[0.00001,-0.00001,0.0,0.0]}, + {"t":4.08337, "x":0.36824, "y":6.18474, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00003,-0.00003,-0.00003,0.00003], "fy":[0.0,-0.00001,0.00001,0.00001]}, + {"t":4.13289, "x":0.36816, "y":6.15999, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00004,-0.00004,-0.00004,0.00004], "fy":[-0.00001,-0.00002,0.00001,0.00001]}, + {"t":4.1824, "x":0.36808, "y":6.13524, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00005,-0.00005,-0.00005,0.00005], "fy":[-0.00002,-0.00002,0.00002,0.00002]}, + {"t":4.23191, "x":0.368, "y":6.11049, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00006,-0.00006,-0.00006,0.00006], "fy":[-0.00003,-0.00003,0.00003,0.00003]}, + {"t":4.28142, "x":0.36791, "y":6.08574, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00007,-0.00007,-0.00007,0.00007], "fy":[-0.00004,-0.00003,0.00004,0.00004]}, + {"t":4.33093, "x":0.36783, "y":6.06099, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00008,-0.00009,-0.00008,0.00009], "fy":[-0.00005,-0.00004,0.00004,0.00004]}, + {"t":4.38045, "x":0.36775, "y":6.03624, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.0001,-0.0001,-0.0001,0.0001], "fy":[-0.00006,-0.00004,0.00005,0.00005]}, + {"t":4.42996, "x":0.36766, "y":6.01148, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00011,-0.00011,-0.00011,0.00011], "fy":[-0.00007,-0.00005,0.00006,0.00006]}, + {"t":4.47947, "x":0.36758, "y":5.98673, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00012,-0.00012,-0.00012,0.00012], "fy":[-0.00008,-0.00005,0.00006,0.00007]}, + {"t":4.52898, "x":0.3675, "y":5.96198, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00013,-0.00013,-0.00013,0.00013], "fy":[-0.00009,-0.00006,0.00007,0.00007]}, + {"t":4.57849, "x":0.36741, "y":5.93723, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00014,-0.00014,-0.00014,0.00014], "fy":[-0.0001,-0.00006,0.00008,0.00008]}, + {"t":4.628, "x":0.36733, "y":5.91248, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00015,-0.00015,-0.00015,0.00015], "fy":[-0.00011,-0.00007,0.00009,0.00009]}, + {"t":4.67752, "x":0.36725, "y":5.88773, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00016,-0.00016,-0.00016,0.00016], "fy":[-0.00012,-0.00008,0.0001,0.0001]}, + {"t":4.72703, "x":0.36716, "y":5.86298, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00017,-0.00017,-0.00017,0.00017], "fy":[-0.00013,-0.00008,0.00011,0.00011]}, + {"t":4.77654, "x":0.36708, "y":5.83823, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00017,-0.00018,-0.00017,0.00018], "fy":[-0.00014,-0.00009,0.00012,0.00012]}, + {"t":4.82605, "x":0.367, "y":5.81348, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00018,-0.00018,-0.00018,0.00018], "fy":[-0.00015,-0.0001,0.00013,0.00013]}, + {"t":4.87556, "x":0.36691, "y":5.78873, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00019,-0.00019,-0.00019,0.00019], "fy":[-0.00017,-0.00012,0.00014,0.00014]}, + {"t":4.92507, "x":0.36683, "y":5.76398, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00019,-0.00019,-0.00019,0.00019], "fy":[-0.00018,-0.00013,0.00016,0.00016]}, + {"t":4.97459, "x":0.36675, "y":5.73923, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00019,-0.0002,-0.00019,0.0002], "fy":[-0.0002,-0.00015,0.00017,0.00018]}, + {"t":5.0241, "x":0.36666, "y":5.71448, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[0.00019,-0.0002,-0.00019,0.0002], "fy":[-0.00022,-0.00017,0.0002,0.0002]}, + {"t":5.07361, "x":0.36658, "y":5.68973, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[0.00019,-0.0002,-0.00019,0.0002], "fy":[-0.00025,-0.0002,0.00022,0.00022]}, + {"t":5.12312, "x":0.3665, "y":5.66497, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[0.00018,-0.00019,-0.00018,0.00019], "fy":[-0.00028,-0.00023,0.00025,0.00025]}, + {"t":5.17263, "x":0.36641, "y":5.64022, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[0.00017,-0.00018,-0.00017,0.00018], "fy":[-0.00031,-0.00026,0.00029,0.00029]}, + {"t":5.22215, "x":0.36633, "y":5.61547, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00004, "fx":[0.00015,-0.00016,-0.00015,0.00016], "fy":[-0.00035,-0.00031,0.00033,0.00033]}, + {"t":5.27166, "x":0.36625, "y":5.59072, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00005, "fx":[0.00013,-0.00013,-0.00013,0.00013], "fy":[-0.0004,-0.00036,0.00038,0.00038]}, + {"t":5.32117, "x":0.36616, "y":5.56597, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[0.00009,-0.0001,-0.00009,0.0001], "fy":[-0.00047,-0.00042,0.00044,0.00044]}, + {"t":5.37068, "x":0.36608, "y":5.54122, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.0001, "fx":[0.00005,-0.00005,-0.00005,0.00005], "fy":[-0.00054,-0.0005,0.00052,0.00052]}, + {"t":5.42019, "x":0.366, "y":5.51647, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00012, "fx":[-0.00001,0.00001,0.00001,-0.00001], "fy":[-0.00063,-0.00058,0.0006,0.00061]}, + {"t":5.4697, "x":0.36591, "y":5.49172, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00016, "fx":[-0.00008,0.00008,0.00008,-0.00008], "fy":[-0.00073,-0.00069,0.00071,0.00071]}, + {"t":5.51922, "x":0.36583, "y":5.46697, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":-0.0002, "fx":[-0.00017,0.00017,0.00017,-0.00017], "fy":[-0.00086,-0.00082,0.00084,0.00084]}, + {"t":5.56873, "x":0.36575, "y":5.44222, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00026, "fx":[-0.00028,0.00028,0.00028,-0.00028], "fy":[-0.00099,-0.00102,0.001,0.001]}, + {"t":5.61824, "x":0.36566, "y":5.41747, "heading":-1.56998, "vx":-0.00168, "vy":-0.4999, "omega":-0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00033, "fx":[-0.00042,0.00042,0.00042,-0.00042], "fy":[-0.00109,-0.00143,0.00126,0.00126]}, + {"t":5.66775, "x":0.36558, "y":5.39272, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":-0.00006, "ax":0.0, "ay":0.0, "alpha":-0.00046, "fx":[-0.0006,0.0006,0.0006,-0.0006], "fy":[-0.00108,-0.00235,0.00171,0.00172]}, + {"t":5.71726, "x":0.3655, "y":5.36797, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":-0.0005, "fx":[-0.00081,0.00081,0.00081,-0.00081], "fy":[-0.00159,-0.00193,0.00176,0.00176]}, + {"t":5.76678, "x":0.36541, "y":5.34321, "heading":-1.56999, "vx":-0.00168, "vy":-0.4999, "omega":-0.00011, "ax":0.0, "ay":0.0, "alpha":0.00155, "fx":[-0.00094,0.00094,0.00094,-0.00094], "fy":[-0.01216,0.02902,-0.00839,-0.00839]}, + {"t":5.81629, "x":0.36533, "y":5.31846, "heading":-1.57, "vx":-0.00168, "vy":-0.4999, "omega":-0.00003, "ax":0.00706, "ay":2.09862, "alpha":0.00065, "fx":[0.11104,0.11305,0.11306,0.11104], "fy":[33.3103,33.33212,33.31312,33.31312]}, + {"t":5.8658, "x":0.36526, "y":5.29629, "heading":-1.57, "vx":-0.00133, "vy":-0.39599, "omega":0.0, "ax":0.0269, "ay":7.99788, "alpha":-0.00001, "fx":[0.42696,0.42707,0.42707,0.42696], "fy":[126.97216,126.97216,126.97217,126.97217]}, + {"t":5.91531, "x":0.36522, "y":5.28648, "heading":-1.57, "vx":0.0, "vy":0.0, "omega":0.0, "ax":7.2523, "ay":-1.61937, "alpha":-14.04238, "fx":[82.09859,119.12337,133.19274,126.12744], "fy":[-106.68427,-62.91597,20.09621,46.669]}, + {"t":5.9437, "x":0.36815, "y":5.28583, "heading":-1.57, "vx":0.20592, "vy":-0.04598, "omega":-0.39872, "ax":7.27216, "ay":-1.63241, "alpha":-13.87689, "fx":[82.81469,119.2101,133.23608,126.54257], "fy":[-106.10505,-62.7264,19.71827,45.45066]}, + {"t":5.9721, "x":0.37693, "y":5.28387, "heading":-1.58132, "vx":0.41241, "vy":-0.09233, "omega":-0.79274, "ax":7.2698, "ay":-1.64704, "alpha":-13.88067, "fx":[82.89098,118.96267,133.31575,126.48451], "fy":[-106.0123,-63.16324,19.06372,45.51975]}, + {"t":6.00049, "x":0.39157, "y":5.28058, "heading":-1.60383, "vx":0.61882, "vy":-0.1391, "omega":-1.18686, "ax":7.25827, "ay":-1.67056, "alpha":-13.94983, "fx":[82.79229,118.44345,133.45468,126.23112], "fy":[-106.04665,-64.09428,17.93278,46.12244]}, + {"t":6.02889, "x":0.41206, "y":5.27596, "heading":-1.63753, "vx":0.82491, "vy":-0.18653, "omega":-1.58295, "ax":7.23878, "ay":-1.70463, "alpha":-14.0668, "fx":[82.56989,117.64332,133.64115,125.82924], "fy":[-106.16417,-65.50611,16.31382,47.10728]}, + {"t":6.05728, "x":0.4384, "y":5.26997, "heading":-1.68248, "vx":1.03045, "vy":-0.23493, "omega":-1.98236, "ax":7.21323, "ay":-1.75159, "alpha":-14.2043, "fx":[82.30563,116.54801,133.8584,125.34941], "fy":[-106.29499,-67.37986,14.18384,48.25967]}, + {"t":6.08567, "x":0.47057, "y":5.2626, "heading":-1.73876, "vx":1.23526, "vy":-0.28467, "omega":-2.38568, "ax":7.1843, "ay":-1.81362, "alpha":-14.32565, "fx":[82.12518,115.13839,134.08231,124.87821], "fy":[-106.33289,-69.69086,11.51419,49.33921]}, + {"t":6.11407, "x":0.50854, "y":5.25378, "heading":-1.8065, "vx":1.43925, "vy":-0.33616, "omega":-2.79244, "ax":7.15577, "ay":-1.89215, "alpha":-14.38243, "fx":[82.22107,113.39548,134.27934,124.51631], "fy":[-106.11423,-72.40027,8.26947,50.08794]}, + {"t":6.14246, "x":0.55229, "y":5.24348, "heading":-1.88579, "vx":1.64243, "vy":-0.38989, "omega":-3.20081, "ax":7.1328, "ay":-1.98669, "alpha":-14.31433, "fx":[82.8855,111.30289,134.4028,124.36244], "fy":[-105.37929,-75.45184,4.41518,50.25504]}, + {"t":6.17086, "x":0.6018, "y":5.2316, "heading":-1.97667, "vx":1.84496, "vy":-0.4463, "omega":-3.60725, "ax":7.12216, "ay":-2.0933, "alpha":-14.04963, "fx":[84.53954,108.85662,134.38784,124.49392], "fy":[-103.70713,-78.75572,-0.0737,49.60601]}, + {"t":6.19925, "x":0.65706, "y":5.21809, "heading":-2.0791, "vx":2.04719, "vy":-0.50573, "omega":-4.00617, "ax":7.13218, "ay":-2.20186, "alpha":-13.50666, "fx":[87.76265,106.07285,134.14074,124.93826], "fy":[-100.36347,-82.16698,-5.21122,47.9173]}, + {"t":6.22764, "x":0.71806, "y":5.20284, "heading":-2.19285, "vx":2.2497, "vy":-0.56825, "omega":-4.38968, "ax":7.17006, "ay":-2.29156, "alpha":-12.5992, "fx":[93.18422,103.00223,133.49728,125.63615], "fy":[-93.98404,-85.401,-10.99678,44.86129]}, + {"t":6.25604, "x":0.78483, "y":5.18578, "heading":-2.31749, "vx":2.45328, "vy":-0.63332, "omega":-4.74742, "ax":7.22884, "ay":-2.31643, "alpha":-11.16714, "fx":[100.98488,99.85479,131.91553,126.29723], "fy":[-81.21204,-87.37074,-17.54957,39.03228]}, + {"t":6.28443, "x":0.8574, "y":5.16687, "heading":-2.45228, "vx":2.65854, "vy":-0.69909, "omega":-5.0645, "ax":6.71691, "ay":-2.16533, "alpha":2.19047, "fx":[110.71401,109.98944,102.0144,103.82564], "fy":[-32.68338,-22.49499,-36.8178,-45.50882]}, + {"t":6.31283, "x":0.93559, "y":5.14614, "heading":-2.59609, "vx":2.84926, "vy":-0.76057, "omega":-5.0023, "ax":-6.74953, "ay":2.19883, "alpha":12.861, "fx":[-101.5323,-76.44723,-129.87376,-120.76152], "fy":[57.85937,107.47536,27.77174,-53.47432]}, + {"t":6.34122, "x":1.01377, "y":5.12543, "heading":-2.73812, "vx":2.65761, "vy":-0.69814, "omega":-4.63713, "ax":-7.28299, "ay":1.86468, "alpha":11.03177, "fx":[-128.30262,-77.98623,-128.66476,-127.53773], "fy":[13.03367,108.1781,37.02606,-39.82528]}, + {"t":6.36961, "x":1.0863, "y":5.10636, "heading":-2.86979, "vx":2.45082, "vy":-0.6452, "omega":-4.32389, "ax":-7.33407, "ay":1.68127, "alpha":11.39789, "fx":[-130.92648,-77.56344,-126.77407,-130.47112], "fy":[-15.44091,109.04101,44.10028,-30.93475]}, + {"t":6.39801, "x":1.15293, "y":5.08872, "heading":-2.99256, "vx":2.24258, "vy":-0.59746, "omega":-4.00026, "ax":-7.28719, "ay":1.62983, "alpha":12.11364, "fx":[-128.94257,-76.96294,-124.66439,-132.18821], "fy":[-32.623,109.72571,50.18857,-23.79205]}, + {"t":6.4264, "x":1.21367, "y":5.07241, "heading":-3.10614, "vx":2.03566, "vy":-0.55118, "omega":-3.65631, "ax":-7.227, "ay":1.65591, "alpha":12.72841, "fx":[-126.52625,-76.66493,-122.46292,-133.28149], "fy":[-42.88911,110.07912,55.57148,-17.60665]}, + {"t":6.45479, "x":1.26855, "y":5.05743, "heading":3.07323, "vx":1.83046, "vy":-0.50416, "omega":-3.2949, "ax":-7.17924, "ay":1.72146, "alpha":13.12047, "fx":[-124.70682,-76.95623,-120.26496,-133.97474], "fy":[-48.86245,109.96621,60.31665,-12.10257]}, + {"t":6.48319, "x":1.31763, "y":5.04381, "heading":2.97967, "vx":1.62661, "vy":-0.45528, "omega":-2.92236, "ax":-7.14979, "ay":1.80184, "alpha":13.29176, "fx":[-123.58378,-77.91846,-118.14731,-134.38326], "fy":[-52.18489,109.34821,64.45529,-7.19617]}, + {"t":6.51158, "x":1.36094, "y":5.03161, "heading":2.89669, "vx":1.4236, "vy":-0.40412, "omega":-2.54496, "ax":-7.13717, "ay":1.88175, "alpha":13.28984, "fx":[-122.99373,-79.48156,-116.1706,-134.58542], "fy":[-53.91219,108.26289,68.01627,-2.8704]}, + {"t":6.53998, "x":1.39848, "y":5.02089, "heading":2.82443, "vx":1.22095, "vy":-0.35069, "omega":-2.16761, "ax":-7.13777, "ay":1.95289, "alpha":13.16997, "fx":[-122.75434,-81.49354,-114.37805,-134.64351], "fy":[-54.69582,106.7931,71.03605,0.88069]}, + {"t":6.56837, "x":1.43027, "y":5.01172, "heading":2.76289, "vx":1.01828, "vy":-0.29524, "omega":-1.79366, "ax":-7.14713, "ay":2.01153, "alpha":12.98566, "fx":[-122.7172,-83.73871,-112.79881,-134.60878], "fy":[-54.95197,105.07363,73.55427,4.062]}, + {"t":6.59676, "x":1.4563, "y":5.00415, "heading":2.71196, "vx":0.81535, "vy":-0.23813, "omega":-1.42495, "ax":-7.16089, "ay":2.05689, "alpha":12.78197, "fx":[-122.77729,-85.98414,-111.45287,-134.5231], "fy":[-54.94611,103.27395,75.60751,6.68297]}, + {"t":6.62516, "x":1.47657, "y":4.99822, "heading":2.6715, "vx":0.61202, "vy":-0.17972, "omega":-1.06202, "ax":-7.17513, "ay":2.08998, "alpha":12.59433, "fx":[-122.86387,-88.00633,-110.35168,-134.41974], "fy":[-54.85023,101.58557,77.23032,8.75409]}, + {"t":6.65355, "x":1.49105, "y":4.99396, "heading":2.64134, "vx":0.40829, "vy":-0.12038, "omega":-0.70442, "ax":-7.18665, "ay":2.11268, "alpha":12.44815, "fx":[-122.93162,-89.61405,-109.50354,-134.32412], "fy":[-54.77424,100.20002,78.44923,10.28624]}, + {"t":6.68195, "x":1.49975, "y":4.99139, "heading":2.62134, "vx":0.20424, "vy":-0.06039, "omega":-0.35097, "ax":-7.19301, "ay":2.12702, "alpha":12.36061, "fx":[-122.95155,-90.6576,-108.91341,-134.25453], "fy":[-54.78913,99.28757,79.28489,11.28884]}, + {"t":6.71034, "x":1.50265, "y":4.99053, "heading":2.61138, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[ + {"name":"IntakeDown", "from":{"target":4, "targetTimestamp":2.1029, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeDown"}}}, + {"name":"IntakeUp", "from":{"target":5, "targetTimestamp":5.91531, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeUp"}}}] +} diff --git a/src/main/deploy/choreo/HubLeft.traj b/src/main/deploy/choreo/HubLeft.traj new file mode 100644 index 0000000..4521550 --- /dev/null +++ b/src/main/deploy/choreo/HubLeft.traj @@ -0,0 +1,143 @@ +{ + "name":"HubLeft", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":5.630887031555176, "y":5.732077598571777, "heading":-1.5707963267948966, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.842833042144775, "y":4.093028545379639, "heading":-1.56622998352472, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.623462200164795, "y":5.721696853637695, "heading":-1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":8.0}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":0, "to":0, "data":{"type":"MaxAngularVelocity", "props":{"max":5.0}}, "enabled":true}, + {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"5.630887031555176 m", "val":5.630887031555176}, "y":{"exp":"5.732077598571777 m", "val":5.732077598571777}, "heading":{"exp":"-1.5707963267948966 rad", "val":-1.5707963267948966}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.842833042144775 m", "val":5.842833042144775}, "y":{"exp":"4.093028545379639 m", "val":4.093028545379639}, "heading":{"exp":"-1.56622998352472 rad", "val":-1.56622998352472}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.623462200164795 m", "val":5.623462200164795}, "y":{"exp":"5.721696853637695 m", "val":5.721696853637695}, "heading":{"exp":"-1.5707963267948966 rad", "val":-1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"8 m / s ^ 2", "val":8.0}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, + {"from":0, "to":0, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"5 rad / s", "val":5.0}}}, "enabled":true}, + {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.78254,3.5561], + "samples":[ + {"t":0.0, "x":5.63089, "y":5.73208, "heading":-1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":1.02865, "ay":-7.92923, "alpha":0.02157, "fx":[16.47387,15.95161,16.42832,16.46829], "fy":[-125.86115,-125.93791,-125.86762,-125.86247]}, + {"t":0.04818, "x":5.63208, "y":5.72288, "heading":-1.5708, "vx":0.04956, "vy":-0.382, "omega":0.00104, "ax":1.03651, "ay":-7.92814, "alpha":-0.00707, "fx":[16.40512,16.66917,16.34983,16.39729], "fy":[-125.87314,-125.83142,-125.8808,-125.87455]}, + {"t":0.09635, "x":5.63567, "y":5.69527, "heading":-1.57075, "vx":0.09949, "vy":-0.76396, "omega":0.0007, "ax":0.62187, "ay":-4.72057, "alpha":0.06078, "fx":[10.00784,9.84018,9.83276,9.80955], "fy":[-74.22557,-75.12769,-75.2093,-75.20736]}, + {"t":0.14453, "x":5.64119, "y":5.65299, "heading":-1.57071, "vx":0.12945, "vy":-0.99138, "omega":0.00363, "ax":-0.07354, "ay":-0.00949, "alpha":-0.07043, "fx":[-1.08216,-1.2546,-1.24911,-1.08402], "fy":[0.45105,-1.58907,0.26678,0.26875]}, + {"t":0.19271, "x":5.64734, "y":5.60522, "heading":-1.57054, "vx":0.12591, "vy":-0.99183, "omega":0.00023, "ax":0.0392, "ay":0.00501, "alpha":0.00333, "fx":[0.69278,0.53166,0.53195,0.73314], "fy":[-0.29223,0.32583,0.14211,0.14274]}, + {"t":0.24088, "x":5.65345, "y":5.55744, "heading":-1.57053, "vx":0.1278, "vy":-0.99159, "omega":0.00039, "ax":0.01093, "ay":0.00141, "alpha":0.00675, "fx":[0.24317,0.10357,0.10362,0.24362], "fy":[0.21912,-0.23045,0.05028,0.05068]}, + {"t":0.28906, "x":5.65962, "y":5.50967, "heading":-1.57051, "vx":0.12832, "vy":-0.99153, "omega":0.00072, "ax":0.00986, "ay":0.00128, "alpha":0.01109, "fx":[0.21083,0.07882,0.07946,0.25696], "fy":[-0.17028,0.18425,0.03332,0.03388]}, + {"t":0.33724, "x":5.66581, "y":5.4619, "heading":-1.57047, "vx":0.1288, "vy":-0.99146, "omega":0.00125, "ax":-0.01172, "ay":-0.00152, "alpha":0.00978, "fx":[-0.13203,-0.24046,-0.23994,-0.13152], "fy":[0.10831,-0.15556,-0.02488,-0.0243]}, + {"t":0.38541, "x":5.672, "y":5.41413, "heading":-1.57041, "vx":0.12823, "vy":-0.99154, "omega":0.00172, "ax":0.00339, "ay":0.00044, "alpha":0.01281, "fx":[0.09781,-0.00214,-0.00146,0.12098], "fy":[-0.06471,0.10689,-0.00746,-0.00687]}, + {"t":0.43359, "x":5.67819, "y":5.36637, "heading":-1.57033, "vx":0.1284, "vy":-0.99152, "omega":0.00234, "ax":-0.00536, "ay":-0.00069, "alpha":0.01152, "fx":[-0.04646,-0.12408,-0.12359,-0.04598], "fy":[0.06395,-0.04109,-0.03371,-0.03316]}, + {"t":0.48177, "x":5.68436, "y":5.3186, "heading":-1.57022, "vx":0.12814, "vy":-0.99155, "omega":0.0029, "ax":0.00121, "ay":0.00016, "alpha":0.01176, "fx":[0.05326,-0.01759,-0.01701,0.05827], "fy":[-0.00147,0.05769,-0.02339,-0.02289]}, + {"t":0.52994, "x":5.69054, "y":5.27083, "heading":-1.57008, "vx":0.1282, "vy":-0.99154, "omega":0.00346, "ax":-0.00092, "ay":-0.00012, "alpha":0.0099, "fx":[0.01101,-0.04056,-0.04019,0.01138], "fy":[0.03921,0.00882,-0.028,-0.02757]}, + {"t":0.57812, "x":5.69671, "y":5.22306, "heading":-1.56991, "vx":0.12815, "vy":-0.99155, "omega":0.00394, "ax":0.00046, "ay":0.00006, "alpha":0.00905, "fx":[0.0313,-0.01578,-0.01537,0.02895], "fy":[0.02015,0.03034,-0.02354,-0.02319]}, + {"t":0.6263, "x":5.70289, "y":5.17529, "heading":-1.56972, "vx":0.12818, "vy":-0.99154, "omega":0.00438, "ax":0.00023, "ay":0.00003, "alpha":0.00714, "fx":[0.01896,-0.01205,-0.01181,0.0192], "fy":[0.02179,0.02192,-0.02107,-0.02079]}, + {"t":0.67447, "x":5.70906, "y":5.12752, "heading":-1.56951, "vx":0.12819, "vy":-0.99154, "omega":0.00472, "ax":0.00016, "ay":0.00002, "alpha":0.00597, "fx":[0.0175,-0.01082,-0.01058,0.01385], "fy":[0.02101,0.01496,-0.01745,-0.01724]}, + {"t":0.72265, "x":5.71524, "y":5.07975, "heading":-1.56928, "vx":0.12819, "vy":-0.99154, "omega":0.00501, "ax":0.00012, "ay":0.00002, "alpha":0.00409, "fx":[0.0093,-0.00558,-0.00546,0.00942], "fy":[0.0102,0.01734,-0.01334,-0.0132]}, + {"t":0.77083, "x":5.72142, "y":5.03198, "heading":-1.56904, "vx":0.1282, "vy":-0.99154, "omega":0.0052, "ax":0.00018, "ay":0.00002, "alpha":0.00292, "fx":[0.00988,-0.00328,-0.00318,0.00783], "fy":[0.01232,0.00639,-0.00867,-0.00859]}, + {"t":0.819, "x":5.72759, "y":4.98421, "heading":-1.56879, "vx":0.12821, "vy":-0.99154, "omega":0.00534, "ax":-0.00022, "ay":-0.00003, "alpha":0.00105, "fx":[-0.0028,-0.00428,-0.00425,-0.00277], "fy":[0.00238,0.00571,-0.00497,-0.00494]}, + {"t":0.86718, "x":5.73377, "y":4.93644, "heading":-1.56853, "vx":0.1282, "vy":-0.99154, "omega":0.0054, "ax":0.0004, "ay":0.00005, "alpha":-0.00004, "fx":[0.00602,0.00625,0.00626,0.0067], "fy":[0.00035,0.00079,0.00106,0.00106]}, + {"t":0.91536, "x":5.73995, "y":4.88867, "heading":-1.56827, "vx":0.12822, "vy":-0.99154, "omega":0.00539, "ax":-0.00059, "ay":-0.00008, "alpha":-0.00198, "fx":[-0.0149,-0.00372,-0.00375,-0.01493], "fy":[-0.00427,-0.00774,0.0036,0.00358]}, + {"t":0.96353, "x":5.74612, "y":4.8409, "heading":-1.56801, "vx":0.12819, "vy":-0.99154, "omega":0.0053, "ax":0.00067, "ay":0.00009, "alpha":-0.00302, "fx":[0.0028,0.0166,0.01657,0.00645], "fy":[-0.01149,-0.00486,0.01094,0.0109]}, + {"t":1.01171, "x":5.7523, "y":4.79313, "heading":-1.56776, "vx":0.12822, "vy":-0.99154, "omega":0.00515, "ax":-0.00099, "ay":-0.00013, "alpha":-0.00509, "fx":[-0.02819,-0.0031,-0.00316,-0.02825], "fy":[-0.01242,-0.01956,0.01196,0.01192]}, + {"t":1.05989, "x":5.75848, "y":4.74537, "heading":-1.56751, "vx":0.12817, "vy":-0.99154, "omega":0.00491, "ax":0.00084, "ay":0.00011, "alpha":-0.00614, "fx":[-0.00289,0.02661,0.02658,0.00302], "fy":[-0.01982,-0.0136,0.02017,0.02014]}, + {"t":1.10806, "x":5.76465, "y":4.6976, "heading":-1.56727, "vx":0.12821, "vy":-0.99154, "omega":0.00461, "ax":-0.00127, "ay":-0.00016, "alpha":-0.00829, "fx":[-0.04124,0.00098,0.00091,-0.04131], "fy":[-0.02483,-0.02465,0.01954,0.01952]}, + {"t":1.15624, "x":5.77083, "y":4.64983, "heading":-1.56705, "vx":0.12815, "vy":-0.99155, "omega":0.00421, "ax":0.00078, "ay":0.0001, "alpha":-0.00933, "fx":[-0.01368,0.0355,0.03549,-0.00787], "fy":[-0.01852,-0.02922,0.02706,0.02706]}, + {"t":1.20442, "x":5.777, "y":4.60206, "heading":-1.56685, "vx":0.12819, "vy":-0.99154, "omega":0.00376, "ax":-0.00061, "ay":-0.00008, "alpha":-0.0113, "fx":[-0.04182,0.0225,0.02242,-0.0419], "fy":[-0.04392,-0.01288,0.02588,0.0259]}, + {"t":1.2526, "x":5.78318, "y":4.55429, "heading":-1.56667, "vx":0.12816, "vy":-0.99155, "omega":0.00322, "ax":0.00035, "ay":0.00004, "alpha":-0.01222, "fx":[-0.03172,0.04254,0.04257,-0.0314], "fy":[0.00326,-0.0568,0.02818,0.02821]}, + {"t":1.30077, "x":5.78935, "y":4.50652, "heading":-1.56651, "vx":0.12818, "vy":-0.99154, "omega":0.00263, "ax":0.00285, "ay":0.00037, "alpha":-0.01329, "fx":[-0.0009,0.09158,0.09145,-0.00101], "fy":[-0.07143,0.03402,0.0304,0.03043]}, + {"t":1.34895, "x":5.79553, "y":4.45875, "heading":-1.56638, "vx":0.12832, "vy":-0.99153, "omega":0.00199, "ax":-0.00126, "ay":-0.00016, "alpha":-0.01358, "fx":[-0.06873,0.03635,0.03639,-0.0839], "fy":[0.06559,-0.10548,0.01476,0.01479]}, + {"t":1.39713, "x":5.80171, "y":4.41098, "heading":-1.56629, "vx":0.12825, "vy":-0.99153, "omega":0.00134, "ax":0.00858, "ay":0.00111, "alpha":-0.01222, "fx":[0.0733,0.19931,0.19912,0.07311], "fy":[-0.11744,0.1437,0.02215,0.02217]}, + {"t":1.4453, "x":5.8079, "y":4.36321, "heading":-1.56622, "vx":0.12867, "vy":-0.99148, "omega":0.00075, "ax":-0.00619, "ay":-0.0008, "alpha":-0.01248, "fx":[-0.15906,-0.01966,-0.01971,-0.19436], "fy":[0.16831,-0.17933,-0.01992,-0.01997]}, + {"t":1.49348, "x":5.81409, "y":4.31545, "heading":-1.56619, "vx":0.12837, "vy":-0.99152, "omega":0.00015, "ax":-0.01048, "ay":-0.00135, "alpha":-0.01071, "fx":[-0.24675,-0.08594,-0.08599,-0.24712], "fy":[-0.21832,0.21199,-0.03982,-0.03989]}, + {"t":1.54166, "x":5.82026, "y":4.26768, "heading":-1.56618, "vx":0.12786, "vy":-0.99158, "omega":-0.00037, "ax":-0.03287, "ay":-0.00421, "alpha":-0.00624, "fx":[-0.60031,-0.42876,-0.42893,-0.6291], "fy":[0.27898,-0.31252,-0.11668,-0.11722]}, + {"t":1.58983, "x":5.82639, "y":4.2199, "heading":-1.5662, "vx":0.12628, "vy":-0.99179, "omega":-0.00067, "ax":0.06454, "ay":0.00834, "alpha":0.05749, "fx":[0.92667,1.12487,1.11879,0.92802], "fy":[-0.43521,1.43634,-0.23464,-0.23695]}, + {"t":1.63801, "x":5.83254, "y":4.17213, "heading":-1.56623, "vx":0.12939, "vy":-0.99139, "omega":0.0021, "ax":-0.62396, "ay":4.71811, "alpha":-0.0684, "fx":[-10.05918,-9.87685,-9.86891,-9.8183], "fy":[74.08116,75.11543,75.20981,75.2076]}, + {"t":1.68619, "x":5.83805, "y":4.12984, "heading":-1.56613, "vx":0.09933, "vy":-0.76408, "omega":-0.0012, "ax":-1.03338, "ay":7.92855, "alpha":0.007, "fx":[-16.35934,-16.61906,-16.29458,-16.34953], "fy":[125.87767,125.84346,125.88601,125.87873]}, + {"t":1.73436, "x":5.84164, "y":4.10223, "heading":-1.56619, "vx":0.04955, "vy":-0.38211, "omega":-0.00086, "ax":-1.02841, "ay":7.93144, "alpha":0.01785, "fx":[-16.11371,-16.41825,-16.41294,-16.3624], "fy":[125.95147,125.90343,125.90387,125.91088]}, + {"t":1.78254, "x":5.84283, "y":4.09303, "heading":-1.56623, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-1.06509, "ay":7.92459, "alpha":-0.02155, "fx":[-17.05105,-16.51744,-17.02072,-17.0472], "fy":[125.78766,125.86487,125.79294,125.78953]}, + {"t":1.8318, "x":5.84154, "y":4.10265, "heading":-1.56623, "vx":-0.05247, "vy":0.39041, "omega":-0.00106, "ax":-1.05704, "ay":7.92561, "alpha":-0.01013, "fx":[-17.00989,-16.73667,-16.73434,-16.64407], "fy":[125.80198,125.82772,125.8293,125.8405]}, + {"t":1.88107, "x":5.83767, "y":4.1315, "heading":-1.56628, "vx":-0.10455, "vy":0.78087, "omega":-0.00156, "ax":-0.60103, "ay":4.2603, "alpha":-0.04919, "fx":[-9.57335,-9.53983,-9.48727,-9.56651], "fy":[67.78376,67.05325,67.85528,67.84907]}, + {"t":1.93034, "x":5.83179, "y":4.17514, "heading":-1.56636, "vx":-0.13416, "vy":0.99076, "omega":-0.00398, "ax":0.01299, "ay":0.00177, "alpha":0.0475, "fx":[0.20455,0.3418,0.34285,-0.06425], "fy":[1.22785,-0.4717,-0.32025,-0.32357]}, + {"t":1.9796, "x":5.8252, "y":4.22395, "heading":-1.56656, "vx":-0.13352, "vy":0.99084, "omega":-0.00164, "ax":0.00854, "ay":0.00115, "alpha":0.00021, "fx":[0.08039,0.19074,0.19092,0.08006], "fy":[-0.18408,0.32014,-0.03145,-0.03167]}, + {"t":2.02887, "x":5.81863, "y":4.27277, "heading":-1.56664, "vx":-0.1331, "vy":0.9909, "omega":-0.00163, "ax":-0.01039, "ay":-0.0014, "alpha":-0.00643, "fx":[-0.20901,-0.08712,-0.0872,-0.27624], "fy":[0.22549,-0.19756,-0.05829,-0.05841]}, + {"t":2.07813, "x":5.81206, "y":4.32158, "heading":-1.56672, "vx":-0.13361, "vy":0.99083, "omega":-0.00195, "ax":0.00595, "ay":0.0008, "alpha":-0.0026, "fx":[0.04986,0.13888,0.13901,0.04986], "fy":[-0.13686,0.21535,-0.01386,-0.01377]}, + {"t":2.1274, "x":5.80549, "y":4.3704, "heading":-1.56681, "vx":-0.13332, "vy":0.99087, "omega":-0.00208, "ax":-0.00647, "ay":-0.00087, "alpha":-0.00833, "fx":[-0.13838,-0.04573,-0.04552,-0.18115], "fy":[0.11902,-0.12884,-0.02285,-0.02266]}, + {"t":2.17666, "x":5.79891, "y":4.41921, "heading":-1.56692, "vx":-0.13363, "vy":0.99083, "omega":-0.00249, "ax":0.00456, "ay":0.00061, "alpha":-0.00698, "fx":[0.04018,0.10431,0.10446,0.04035], "fy":[-0.0732,0.08066,0.01563,0.0159]}, + {"t":2.22593, "x":5.79233, "y":4.46803, "heading":-1.56704, "vx":-0.13341, "vy":0.99086, "omega":-0.00283, "ax":-0.00216, "ay":-0.00029, "alpha":-0.00922, "fx":[-0.06231,0.00143,0.00178,-0.0782], "fy":[0.02718,-0.06396,0.00899,0.0093]}, + {"t":2.2752, "x":5.78576, "y":4.51684, "heading":-1.56718, "vx":-0.13352, "vy":0.99084, "omega":-0.00329, "ax":0.00155, "ay":0.00021, "alpha":-0.00725, "fx":[0.00341,0.04549,0.04573,0.00365], "fy":[-0.03778,0.01026,0.02022,0.02054]}, + {"t":2.32446, "x":5.77918, "y":4.56565, "heading":-1.56734, "vx":-0.13344, "vy":0.99085, "omega":-0.00364, "ax":-0.00048, "ay":-0.00006, "alpha":-0.00714, "fx":[-0.02663,0.0128,0.01317,-0.02974], "fy":[-0.00659,-0.02935,0.01577,0.01609]}, + {"t":2.37373, "x":5.77261, "y":4.61447, "heading":-1.56752, "vx":-0.13346, "vy":0.99085, "omega":-0.004, "ax":0.00019, "ay":0.00003, "alpha":-0.00543, "fx":[-0.00979,0.01564,0.01588,-0.00955], "fy":[-0.01758,-0.01249,0.01571,0.01601]}, + {"t":2.42299, "x":5.76603, "y":4.66328, "heading":-1.56772, "vx":-0.13345, "vy":0.99085, "omega":-0.00426, "ax":-0.00003, "ay":0.0, "alpha":-0.00429, "fx":[-0.01111,0.00932,0.00963,-0.00969], "fy":[-0.01352,-0.01126,0.01213,0.0124]}, + {"t":2.47226, "x":5.75946, "y":4.7121, "heading":-1.56793, "vx":-0.13346, "vy":0.99085, "omega":-0.00447, "ax":-0.00007, "ay":-0.00001, "alpha":-0.00305, "fx":[-0.00812,0.00569,0.00587,-0.00794], "fy":[-0.00566,-0.01242,0.00861,0.00885]}, + {"t":2.52152, "x":5.75288, "y":4.76091, "heading":-1.56815, "vx":-0.13346, "vy":0.99085, "omega":-0.00462, "ax":-0.00004, "ay":-0.00001, "alpha":-0.0014, "fx":[-0.00397,0.00181,0.00204,-0.00225], "fy":[-0.0073,-0.00215,0.00446,0.00466]}, + {"t":2.57079, "x":5.74631, "y":4.80973, "heading":-1.56837, "vx":-0.13346, "vy":0.99085, "omega":-0.00469, "ax":0.00008, "ay":0.00001, "alpha":-0.00064, "fx":[-0.00165,0.00421,0.00433,-0.00153], "fy":[0.00131,-0.00206,0.00066,0.00082]}, + {"t":2.62005, "x":5.73973, "y":4.85854, "heading":-1.56861, "vx":-0.13346, "vy":0.99085, "omega":-0.00472, "ax":-0.00025, "ay":-0.00003, "alpha":0.00136, "fx":[-0.00095,-0.00711,-0.00697,-0.00085], "fy":[0.00432,0.00251,-0.00455,-0.00442]}, + {"t":2.66932, "x":5.73316, "y":4.90736, "heading":-1.56884, "vx":-0.13347, "vy":0.99085, "omega":-0.00466, "ax":0.00039, "ay":0.00005, "alpha":0.00168, "fx":[0.00619,0.00605,0.00611,0.00625], "fy":[0.0061,0.01191,-0.0074,-0.00731]}, + {"t":2.71859, "x":5.72658, "y":4.95617, "heading":-1.56907, "vx":-0.13345, "vy":0.99085, "omega":-0.00457, "ax":-0.00052, "ay":-0.00007, "alpha":0.00403, "fx":[0.00079,-0.01642,-0.01634,-0.00137], "fy":[0.01726,0.00586,-0.01383,-0.01377]}, + {"t":2.76785, "x":5.72001, "y":5.00499, "heading":-1.56929, "vx":-0.13348, "vy":0.99085, "omega":-0.00438, "ax":0.00078, "ay":0.00011, "alpha":0.00398, "fx":[0.01542,0.0094,0.00943,0.01544], "fy":[0.01122,0.0258,-0.01519,-0.01514]}, + {"t":2.81712, "x":5.71343, "y":5.0538, "heading":-1.56951, "vx":-0.13344, "vy":0.99085, "omega":-0.00418, "ax":-0.00075, "ay":-0.0001, "alpha":0.00674, "fx":[0.00375,-0.02544,-0.02539,-0.00031], "fy":[0.02831,0.01069,-0.02272,-0.02268]}, + {"t":2.86638, "x":5.70686, "y":5.10262, "heading":-1.56971, "vx":-0.13347, "vy":0.99085, "omega":-0.00385, "ax":0.00115, "ay":0.00015, "alpha":0.00626, "fx":[0.025,0.01144,0.01145,0.02501], "fy":[0.0192,0.03512,-0.02227,-0.02223]}, + {"t":2.91565, "x":5.70028, "y":5.15143, "heading":-1.5699, "vx":-0.13342, "vy":0.99086, "omega":-0.00354, "ax":-0.0008, "ay":-0.00011, "alpha":0.00945, "fx":[0.01028,-0.03365,-0.03359,0.00623], "fy":[0.03193,0.02058,-0.02969,-0.02965]}, + {"t":2.96491, "x":5.69371, "y":5.20025, "heading":-1.57008, "vx":-0.13346, "vy":0.99085, "omega":-0.00307, "ax":0.00084, "ay":0.00011, "alpha":0.00825, "fx":[0.02553,0.00109,0.00112,0.02557], "fy":[0.03274,0.0302,-0.02791,-0.02785]}, + {"t":3.01418, "x":5.68714, "y":5.24906, "heading":-1.57023, "vx":-0.13342, "vy":0.99086, "omega":-0.00267, "ax":-0.0006, "ay":-0.00008, "alpha":0.01177, "fx":[0.02172,-0.04117,-0.04107,0.02265], "fy":[0.01667,0.04071,-0.03128,-0.03119]}, + {"t":3.06344, "x":5.68056, "y":5.29788, "heading":-1.57036, "vx":-0.13345, "vy":0.99085, "omega":-0.00209, "ax":-0.00177, "ay":-0.00024, "alpha":0.0091, "fx":[-0.00822,-0.04799,-0.0479,-0.00813], "fy":[0.0542,-0.00742,-0.03101,-0.03089]}, + {"t":3.11271, "x":5.67399, "y":5.34669, "heading":-1.57046, "vx":-0.13353, "vy":0.99084, "omega":-0.00164, "ax":0.00045, "ay":0.00006, "alpha":0.01259, "fx":[0.04654,-0.04013,-0.03996,0.062], "fy":[-0.03686,0.07964,-0.01955,-0.01939]}, + {"t":3.16198, "x":5.66741, "y":5.39551, "heading":-1.57055, "vx":-0.13351, "vy":0.99084, "omega":-0.00102, "ax":-0.00702, "ay":-0.00095, "alpha":0.00683, "fx":[-0.08192,-0.14109,-0.14092,-0.08176], "fy":[0.09203,-0.10697,-0.02269,-0.0225]}, + {"t":3.21124, "x":5.66082, "y":5.44432, "heading":-1.5706, "vx":-0.13386, "vy":0.9908, "omega":-0.00068, "ax":0.00536, "ay":0.00072, "alpha":0.00974, "fx":[0.13174,0.01827,0.01852,0.17196], "fy":[-0.15008,0.15181,0.022,0.02222]}, + {"t":3.26051, "x":5.65423, "y":5.49313, "heading":-1.57063, "vx":-0.13359, "vy":0.99083, "omega":-0.0002, "ax":0.00631, "ay":0.00085, "alpha":0.0003, "fx":[0.13943,0.06067,0.06069,0.13964], "fy":[0.19123,-0.23028,0.04639,0.04659]}, + {"t":3.30977, "x":5.64766, "y":5.54195, "heading":-1.57064, "vx":-0.13328, "vy":0.99087, "omega":-0.00019, "ax":0.03016, "ay":0.00403, "alpha":0.00074, "fx":[0.53603,0.39847,0.39865,0.58226], "fy":[-0.29259,0.28764,0.13035,0.13078]}, + {"t":3.35904, "x":5.64113, "y":5.59077, "heading":-1.57065, "vx":-0.1318, "vy":0.99107, "omega":-0.00015, "ax":-0.05183, "ay":-0.00697, "alpha":-0.04963, "fx":[-0.77445,-0.87326,-0.86821,-0.77558], "fy":[0.33275,-1.12366,0.17333,0.17464]}, + {"t":3.4083, "x":5.63457, "y":5.63959, "heading":-1.57066, "vx":-0.13435, "vy":0.99073, "omega":-0.0026, "ax":0.58207, "ay":-4.26075, "alpha":0.05376, "fx":[9.35739,9.21881,9.21407,9.1729], "fy":[-66.98937,-67.81217,-67.88472,-67.88393]}, + {"t":3.45757, "x":5.62866, "y":5.68323, "heading":-1.57078, "vx":-0.10567, "vy":0.78082, "omega":0.00005, "ax":1.07507, "ay":-7.92318, "alpha":-0.00726, "fx":[17.01873,17.2416,16.9911,17.01868], "fy":[-125.79111,-125.76735,-125.79535,-125.79164]}, + {"t":3.50684, "x":5.62476, "y":5.71208, "heading":-1.57078, "vx":-0.05271, "vy":0.39048, "omega":-0.00031, "ax":1.06989, "ay":-7.92604, "alpha":0.00622, "fx":[17.02411,16.91285,16.984,17.02045], "fy":[-125.82615,-125.84097,-125.83228,-125.82745]}, + {"t":3.5561, "x":5.62346, "y":5.7217, "heading":-1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[ + {"name":"IntakeDown", "from":{"target":0, "targetTimestamp":0.0, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeDown"}}}, + {"name":"IntakeUp", "from":{"target":2, "targetTimestamp":3.5561, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeUp"}}}] +} diff --git a/src/main/deploy/choreo/HubRight.traj b/src/main/deploy/choreo/HubRight.traj new file mode 100644 index 0000000..8c517b7 --- /dev/null +++ b/src/main/deploy/choreo/HubRight.traj @@ -0,0 +1,135 @@ +{ + "name":"HubRight", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":5.616757392883301, "y":2.326812267303467, "heading":1.5609444415374634, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.790645122528076, "y":3.856259822845459, "heading":1.5774184900038868, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.630136966705322, "y":2.3510687351226807, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":8.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"5.616757392883301 m", "val":5.616757392883301}, "y":{"exp":"2.326812267303467 m", "val":2.326812267303467}, "heading":{"exp":"1.5609444415374634 rad", "val":1.5609444415374634}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.790645122528076 m", "val":5.790645122528076}, "y":{"exp":"3.856259822845459 m", "val":3.856259822845459}, "heading":{"exp":"1.5774184900038868 rad", "val":1.5774184900038868}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.630136966705322 m", "val":5.630136966705322}, "y":{"exp":"2.3510687351226807 m", "val":2.3510687351226807}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"8 m / s ^ 2", "val":8.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.67338,3.31248], + "samples":[ + {"t":0.0, "x":5.61676, "y":2.32681, "heading":1.56094, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.88173, "ay":7.94686, "alpha":0.00384, "fx":[13.98069,14.01904,14.01657,13.9762], "fy":[126.16262,126.15851,126.16166,126.16589]}, + {"t":0.04781, "x":5.61777, "y":2.3359, "heading":1.56094, "vx":0.04216, "vy":0.37995, "omega":0.00018, "ax":0.88152, "ay":7.94681, "alpha":0.00336, "fx":[13.97954,14.01198,14.01208,13.97554], "fy":[126.16184,126.15838,126.16093,126.16476]}, + {"t":0.09562, "x":5.62079, "y":2.36314, "heading":1.56095, "vx":0.0843, "vy":0.75989, "omega":0.00034, "ax":0.54485, "ay":4.88998, "alpha":0.02539, "fx":[8.57853,8.73948,8.73252,8.54925], "fy":[77.58313,77.58269,77.67494,77.68739]}, + {"t":0.14343, "x":5.62544, "y":2.40506, "heading":1.56097, "vx":0.11035, "vy":0.99368, "omega":0.00156, "ax":-0.00281, "ay":0.00033, "alpha":0.03706, "fx":[-0.13796,0.04868,0.04837,-0.13724], "fy":[-0.1001,-0.08955,0.10412,0.10656]}, + {"t":0.19124, "x":5.63071, "y":2.45257, "heading":1.56104, "vx":0.11022, "vy":0.9937, "omega":0.00333, "ax":0.00235, "ay":-0.00026, "alpha":0.0553, "fx":[-0.03279,0.12942,0.1309,-0.07798], "fy":[-0.38984,0.00052,0.18561,0.18711]}, + {"t":0.23905, "x":5.63599, "y":2.50008, "heading":1.5612, "vx":0.11033, "vy":0.99369, "omega":0.00597, "ax":-0.00137, "ay":0.00015, "alpha":0.03984, "fx":[-0.09016,0.04538,0.04665,-0.08889], "fy":[-0.01844,-0.24724,0.13711,0.13823]}, + {"t":0.28687, "x":5.64126, "y":2.54759, "heading":1.56149, "vx":0.11027, "vy":0.99369, "omega":0.00788, "ax":0.0004, "ay":-0.00004, "alpha":0.03075, "fx":[-0.0488,0.06705,0.06808,-0.06079], "fy":[-0.15951,-0.03647,0.09612,0.09702]}, + {"t":0.33468, "x":5.64653, "y":2.5951, "heading":1.56187, "vx":0.11028, "vy":0.99369, "omega":0.00935, "ax":-0.00019, "ay":0.00002, "alpha":0.02402, "fx":[-0.05264,0.0458,0.04662,-0.05182], "fy":[-0.03699,-0.11107,0.07434,0.07505]}, + {"t":0.38249, "x":5.65181, "y":2.64261, "heading":1.56231, "vx":0.11028, "vy":0.99369, "omega":0.0105, "ax":0.00012, "ay":-0.00001, "alpha":0.01952, "fx":[-0.03848,0.04427,0.04491,-0.04291], "fy":[-0.08032,-0.03684,0.05787,0.05843]}, + {"t":0.4303, "x":5.65708, "y":2.69012, "heading":1.56281, "vx":0.11028, "vy":0.99369, "omega":0.01143, "ax":-0.00008, "ay":0.00001, "alpha":0.01591, "fx":[-0.03703,0.03391,0.03442,-0.03652], "fy":[-0.03263,-0.06067,0.04672,0.04716]}, + {"t":0.47811, "x":5.66235, "y":2.73763, "heading":1.56336, "vx":0.11028, "vy":0.99369, "omega":0.01219, "ax":0.00004, "ay":0.0, "alpha":0.01313, "fx":[-0.02811,0.03038,0.03076,-0.03079], "fy":[-0.04713,-0.02911,0.03783,0.03816]}, + {"t":0.52592, "x":5.66762, "y":2.78514, "heading":1.56394, "vx":0.11028, "vy":0.99369, "omega":0.01282, "ax":0.0, "ay":0.0, "alpha":0.01078, "fx":[-0.02507,0.02485,0.02515,-0.02477], "fy":[-0.02459,-0.03725,0.03079,0.03104]}, + {"t":0.57373, "x":5.6729, "y":2.83265, "heading":1.56456, "vx":0.11028, "vy":0.99369, "omega":0.01333, "ax":-0.00004, "ay":0.0, "alpha":0.00876, "fx":[-0.01999,0.01986,0.02007,-0.02224], "fy":[-0.029,-0.02072,0.02489,0.02508]}, + {"t":0.62154, "x":5.67817, "y":2.88016, "heading":1.56519, "vx":0.11028, "vy":0.99369, "omega":0.01375, "ax":0.00007, "ay":-0.00001, "alpha":0.00696, "fx":[-0.01544,0.01752,0.01768,-0.01528], "fy":[-0.01694,-0.02264,0.01947,0.01961]}, + {"t":0.66935, "x":5.68344, "y":2.92767, "heading":1.56585, "vx":0.11028, "vy":0.99369, "omega":0.01409, "ax":-0.00011, "ay":0.00001, "alpha":0.0054, "fx":[-0.01345,0.01123,0.01133,-0.01591], "fy":[-0.01677,-0.01302,0.01522,0.01531]}, + {"t":0.71716, "x":5.68871, "y":2.97517, "heading":1.56653, "vx":0.11028, "vy":0.99369, "omega":0.01434, "ax":0.00014, "ay":-0.00002, "alpha":0.00386, "fx":[-0.00698,0.0115,0.01157,-0.00691], "fy":[-0.00994,-0.01218,0.01052,0.01058]}, + {"t":0.76497, "x":5.69399, "y":3.02268, "heading":1.56721, "vx":0.11028, "vy":0.99369, "omega":0.01453, "ax":-0.00019, "ay":0.00002, "alpha":0.0026, "fx":[-0.00793,0.00359,0.00362,-0.0111], "fy":[-0.00743,-0.00589,0.0073,0.00733]}, + {"t":0.81278, "x":5.69926, "y":3.07019, "heading":1.56791, "vx":0.11027, "vy":0.99369, "omega":0.01465, "ax":0.00023, "ay":-0.00003, "alpha":0.00111, "fx":[0.00096,0.00631,0.00633,0.00098], "fy":[-0.00342,-0.00362,0.00271,0.00272]}, + {"t":0.8606, "x":5.70453, "y":3.1177, "heading":1.56861, "vx":0.11028, "vy":0.99369, "omega":0.01471, "ax":-0.00028, "ay":0.00003, "alpha":0.00004, "fx":[-0.00302,-0.00372,-0.00372,-0.00722], "fy":[0.00093,0.00088,0.00008,0.00007]}, + {"t":0.90841, "x":5.7098, "y":3.16521, "heading":1.56931, "vx":0.11027, "vy":0.99369, "omega":0.01471, "ax":0.00033, "ay":-0.00004, "alpha":-0.00154, "fx":[0.0089,0.00158,0.00156,0.00889], "fy":[0.00278,0.00474,-0.00491,-0.00492]}, + {"t":0.95622, "x":5.71508, "y":3.21272, "heading":1.57001, "vx":0.11029, "vy":0.99369, "omega":0.01463, "ax":-0.00039, "ay":0.00004, "alpha":-0.00255, "fx":[0.00165,-0.01127,-0.01128,-0.00372], "fy":[0.00991,0.00747,-0.00732,-0.00733]}, + {"t":1.00403, "x":5.72035, "y":3.26023, "heading":1.57071, "vx":0.11027, "vy":0.99369, "omega":0.01451, "ax":0.00045, "ay":-0.00005, "alpha":-0.00437, "fx":[0.01733,-0.00308,-0.00309,0.01732], "fy":[0.00881,0.01458,-0.01327,-0.01328]}, + {"t":1.05184, "x":5.72562, "y":3.30774, "heading":1.57141, "vx":0.11029, "vy":0.99369, "omega":0.0143, "ax":-0.00052, "ay":0.00006, "alpha":-0.00548, "fx":[0.00647,-0.01969,-0.01969,-0.00007], "fy":[0.02141,0.01404,-0.0159,-0.0159]}, + {"t":1.09965, "x":5.73089, "y":3.35525, "heading":1.57209, "vx":0.11027, "vy":0.99369, "omega":0.01404, "ax":0.00059, "ay":-0.00007, "alpha":-0.00772, "fx":[0.02685,-0.00805,-0.00804,0.02687], "fy":[0.01478,0.02818,-0.02358,-0.02356]}, + {"t":1.14746, "x":5.73617, "y":3.40276, "heading":1.57276, "vx":0.11029, "vy":0.99369, "omega":0.01367, "ax":-0.00068, "ay":0.00008, "alpha":-0.00918, "fx":[0.01191,-0.02983,-0.02978,0.00439], "fy":[0.03847,0.02061,-0.02716,-0.02712]}, + {"t":1.19527, "x":5.74144, "y":3.45027, "heading":1.57342, "vx":0.11026, "vy":0.9937, "omega":0.01323, "ax":0.00077, "ay":-0.00009, "alpha":-0.01215, "fx":[0.03836,-0.01393,-0.01384,0.03844], "fy":[0.0203,0.05026,-0.03804,-0.03796]}, + {"t":1.24308, "x":5.74671, "y":3.49778, "heading":1.57405, "vx":0.1103, "vy":0.99369, "omega":0.01265, "ax":-0.00089, "ay":0.0001, "alpha":-0.01465, "fx":[0.01853,-0.04309,-0.04295,0.01114], "fy":[0.07032,0.02562,-0.0449,-0.04479]}, + {"t":1.29089, "x":5.75198, "y":3.54529, "heading":1.57465, "vx":0.11026, "vy":0.9937, "omega":0.01195, "ax":0.00101, "ay":-0.00011, "alpha":-0.0193, "fx":[0.05331,-0.02141,-0.0212,0.05352], "fy":[0.02108,0.09909,-0.06374,-0.06356]}, + {"t":1.3387, "x":5.75726, "y":3.5928, "heading":1.57522, "vx":0.1103, "vy":0.99369, "omega":0.01103, "ax":-0.00111, "ay":0.00012, "alpha":-0.02476, "fx":[0.02689,-0.06186,-0.06156,0.02584], "fy":[0.14816,0.02085,-0.08072,-0.08046]}, + {"t":1.38651, "x":5.76253, "y":3.64031, "heading":1.57575, "vx":0.11025, "vy":0.9937, "omega":0.00985, "ax":0.00176, "ay":-0.0002, "alpha":-0.03367, "fx":[0.08024,-0.02483,-0.02441,0.08067], "fy":[-0.0006,0.23263,-0.1224,-0.12203]}, + {"t":1.43433, "x":5.7678, "y":3.68781, "heading":1.57622, "vx":0.11033, "vy":0.99369, "omega":0.00824, "ax":-0.00466, "ay":0.00052, "alpha":-0.04964, "fx":[-0.02028,-0.1475,-0.14681,0.01876], "fy":[0.39842,-0.02522,-0.17041,-0.16997]}, + {"t":1.48214, "x":5.77307, "y":3.73532, "heading":1.57662, "vx":0.11011, "vy":0.99371, "omega":0.00586, "ax":0.06145, "ay":-0.00693, "alpha":-0.0324, "fx":[1.04862,0.90054,0.9031,1.05026], "fy":[-0.03946,0.00752,-0.2043,-0.20411]}, + {"t":1.52995, "x":5.77841, "y":3.78283, "heading":1.5769, "vx":0.11305, "vy":0.99338, "omega":0.00431, "ax":-0.00519, "ay":-5.72334, "alpha":-0.01956, "fx":[0.06859,-0.05411,-0.0486,-0.29578], "fy":[-90.55754,-90.92105,-90.98414,-90.98623]}, + {"t":1.57776, "x":5.7838, "y":3.82378, "heading":1.5771, "vx":0.1128, "vy":0.71974, "omega":0.00338, "ax":-0.86035, "ay":-7.94915, "alpha":-0.00294, "fx":[-13.64562,-13.67399,-13.6736,-13.64169], "fy":[-126.1987,-126.19588,-126.19814,-126.20137]}, + {"t":1.62557, "x":5.78821, "y":3.84911, "heading":1.57726, "vx":0.07167, "vy":0.33969, "omega":0.00324, "ax":-0.87145, "ay":-7.95019, "alpha":-0.00243, "fx":[-13.81149,-13.84979,-13.84562,-13.833], "fy":[-126.21632,-126.21241,-126.21535,-126.2166]}, + {"t":1.67338, "x":5.79065, "y":3.85626, "heading":1.57742, "vx":0.03, "vy":-0.04042, "omega":0.00312, "ax":-0.87413, "ay":-7.94774, "alpha":-0.00309, "fx":[-13.86386,-13.89448,-13.89196,-13.8597], "fy":[-126.17642,-126.17313,-126.17587,-126.17918]}, + {"t":1.72159, "x":5.79108, "y":3.84508, "heading":1.57757, "vx":-0.01214, "vy":-0.42357, "omega":0.00297, "ax":-0.8791, "ay":-7.94712, "alpha":-0.00171, "fx":[-13.93471,-13.96729,-13.96353,-13.95981], "fy":[-126.16751,-126.16415,-126.16665,-126.16697]}, + {"t":1.7698, "x":5.78947, "y":3.81542, "heading":1.57771, "vx":-0.05452, "vy":-0.80669, "omega":0.00289, "ax":-1.09708, "ay":-3.88557, "alpha":0.01238, "fx":[-17.36629,-17.46048,-17.48249,-17.35845], "fy":[-61.49555,-62.09535,-61.57362,-61.5808]}, + {"t":1.81801, "x":5.78557, "y":3.77202, "heading":1.57785, "vx":-0.10741, "vy":-0.99401, "omega":0.00349, "ax":-0.03221, "ay":0.0035, "alpha":-0.03999, "fx":[-0.44125,-0.57874,-0.57673,-0.44844], "fy":[0.31623,0.06931,-0.08171,-0.08182]}, + {"t":1.86621, "x":5.78035, "y":3.7241, "heading":1.57802, "vx":-0.10896, "vy":-0.99384, "omega":0.00156, "ax":0.00581, "ay":-0.00064, "alpha":-0.0491, "fx":[0.14886,0.0348,0.03559,0.14961], "fy":[-0.06887,0.43112,-0.20162,-0.20103]}, + {"t":1.91442, "x":5.7751, "y":3.67619, "heading":1.57809, "vx":-0.10868, "vy":-0.99387, "omega":-0.00081, "ax":-0.00179, "ay":0.0002, "alpha":-0.03076, "fx":[0.02329,-0.07399,-0.07343,0.01042], "fy":[0.22389,0.00357,-0.10775,-0.10728]}, + {"t":1.96263, "x":5.76986, "y":3.62828, "heading":1.57806, "vx":-0.10877, "vy":-0.99386, "omega":-0.00229, "ax":0.00143, "ay":-0.00016, "alpha":-0.0234, "fx":[0.06405,-0.01921,-0.01878,0.06448], "fy":[0.01289,0.13876,-0.08096,-0.0806]}, + {"t":2.01084, "x":5.76462, "y":3.58036, "heading":1.57795, "vx":-0.1087, "vy":-0.99387, "omega":-0.00342, "ax":-0.00122, "ay":0.00013, "alpha":-0.01726, "fx":[0.01888,-0.05018,-0.04987,0.00369], "fy":[0.09602,0.02342,-0.05562,-0.05535]}, + {"t":2.05905, "x":5.75938, "y":3.53245, "heading":1.57778, "vx":-0.10876, "vy":-0.99386, "omega":-0.00425, "ax":0.00102, "ay":-0.00011, "alpha":-0.01463, "fx":[0.04676,-0.01451,-0.01425,0.04702], "fy":[0.02015,0.06613,-0.0468,-0.0466]}, + {"t":2.10726, "x":5.75414, "y":3.48454, "heading":1.57758, "vx":-0.10871, "vy":-0.99387, "omega":-0.00496, "ax":-0.00086, "ay":0.00009, "alpha":-0.01108, "fx":[0.01402,-0.03548,-0.03532,0.00225], "fy":[0.05038,0.02292,-0.03374,-0.03359]}, + {"t":2.15547, "x":5.7489, "y":3.43662, "heading":1.57734, "vx":-0.10875, "vy":-0.99386, "omega":-0.00549, "ax":0.00074, "ay":-0.00008, "alpha":-0.00997, "fx":[0.03434,-0.01108,-0.01093,0.03448], "fy":[0.01771,0.03765,-0.03029,-0.03019]}, + {"t":2.20368, "x":5.74365, "y":3.38871, "heading":1.57707, "vx":-0.10871, "vy":-0.99387, "omega":-0.00597, "ax":-0.00064, "ay":0.00007, "alpha":-0.00752, "fx":[0.00968,-0.02569,-0.02561,0.00101], "fy":[0.03037,0.01832,-0.02216,-0.02209]}, + {"t":2.25188, "x":5.73841, "y":3.3408, "heading":1.57678, "vx":-0.10874, "vy":-0.99386, "omega":-0.00633, "ax":0.00055, "ay":-0.00006, "alpha":-0.00691, "fx":[0.02535,-0.00795,-0.00788,0.02542], "fy":[0.01363,0.02301,-0.02025,-0.02021]}, + {"t":2.30009, "x":5.73317, "y":3.29288, "heading":1.57648, "vx":-0.10872, "vy":-0.99387, "omega":-0.00667, "ax":-0.00048, "ay":0.00005, "alpha":-0.00502, "fx":[0.00629,-0.01816,-0.01814,-0.00019], "fy":[0.01876,0.0135,-0.01448,-0.01447]}, + {"t":2.3483, "x":5.72793, "y":3.24497, "heading":1.57616, "vx":-0.10874, "vy":-0.99386, "omega":-0.00691, "ax":0.00041, "ay":-0.00004, "alpha":-0.00461, "fx":[0.01813,-0.00521,-0.00518,0.01816], "fy":[0.00947,0.01387,-0.01309,-0.01309]}, + {"t":2.39651, "x":5.72269, "y":3.19706, "heading":1.57582, "vx":-0.10872, "vy":-0.99387, "omega":-0.00713, "ax":-0.00035, "ay":0.00004, "alpha":-0.00307, "fx":[0.00336,-0.012,-0.01201,-0.00149], "fy":[0.01107,0.00884,-0.00874,-0.00875]}, + {"t":2.44472, "x":5.71745, "y":3.14915, "heading":1.57548, "vx":-0.10874, "vy":-0.99386, "omega":-0.00728, "ax":0.00029, "ay":-0.00003, "alpha":-0.00271, "fx":[0.01196,-0.00261,-0.00262,0.01195], "fy":[0.00539,0.00741,-0.00741,-0.00743]}, + {"t":2.49293, "x":5.7122, "y":3.10123, "heading":1.57513, "vx":-0.10872, "vy":-0.99387, "omega":-0.00741, "ax":-0.00025, "ay":0.00003, "alpha":-0.00136, "fx":[0.00063,-0.00659,-0.00662,-0.00304], "fy":[0.0051,0.00439,-0.00388,-0.0039]}, + {"t":2.54114, "x":5.70696, "y":3.05332, "heading":1.57477, "vx":-0.10873, "vy":-0.99386, "omega":-0.00748, "ax":0.0002, "ay":-0.00002, "alpha":-0.00096, "fx":[0.00636,0.00005,0.00002,0.00634], "fy":[0.00143,0.00179,-0.00229,-0.00232]}, + {"t":2.58935, "x":5.70172, "y":3.00541, "heading":1.57441, "vx":-0.10872, "vy":-0.99386, "omega":-0.00752, "ax":-0.00016, "ay":0.00002, "alpha":0.00031, "fx":[-0.00214,-0.00148,-0.00152,-0.00504], "fy":[-0.00077,0.00014,0.00089,0.00086]}, + {"t":2.63755, "x":5.69648, "y":2.95749, "heading":1.57405, "vx":-0.10873, "vy":-0.99386, "omega":-0.00751, "ax":0.00012, "ay":-0.00001, "alpha":0.00084, "fx":[0.00094,0.00296,0.00293,0.00091], "fy":[-0.00237,-0.00455,0.00305,0.00302]}, + {"t":2.68576, "x":5.69124, "y":2.90958, "heading":1.57369, "vx":-0.10873, "vy":-0.99386, "omega":-0.00747, "ax":-0.00009, "ay":0.00001, "alpha":0.00217, "fx":[-0.00517,0.00376,0.00372,-0.00774], "fy":[-0.00819,-0.00387,0.00635,0.00631]}, + {"t":2.73397, "x":5.686, "y":2.86167, "heading":1.57333, "vx":-0.10873, "vy":-0.99386, "omega":-0.00736, "ax":0.00005, "ay":-0.00001, "alpha":0.00295, "fx":[-0.00473,0.00634,0.0063,-0.00477], "fy":[-0.0059,-0.0135,0.00955,0.00951]}, + {"t":2.78218, "x":5.68075, "y":2.81376, "heading":1.57297, "vx":-0.10873, "vy":-0.99386, "omega":-0.00722, "ax":-0.00001, "ay":0.0, "alpha":0.00453, "fx":[-0.00873,0.0097,0.00965,-0.01127], "fy":[-0.01963,-0.00748,0.01361,0.01357]}, + {"t":2.83039, "x":5.67551, "y":2.76584, "heading":1.57262, "vx":-0.10873, "vy":-0.99386, "omega":-0.007, "ax":-0.00004, "ay":0.0, "alpha":0.00578, "fx":[-0.01145,0.01029,0.01024,-0.01149], "fy":[-0.00861,-0.02884,0.01888,0.01883]}, + {"t":2.8786, "x":5.67027, "y":2.71793, "heading":1.57229, "vx":-0.10873, "vy":-0.99386, "omega":-0.00672, "ax":0.00008, "ay":-0.00001, "alpha":0.00811, "fx":[-0.01314,0.01724,0.01719,-0.01602], "fy":[-0.04274,-0.00911,0.02566,0.02562]}, + {"t":2.92681, "x":5.66503, "y":2.67002, "heading":1.57196, "vx":-0.10873, "vy":-0.99386, "omega":-0.00633, "ax":-0.00016, "ay":0.00002, "alpha":0.01067, "fx":[-0.02021,0.01522,0.01517,-0.02026], "fy":[-0.00629,-0.06662,0.03703,0.03698]}, + {"t":2.97502, "x":5.65979, "y":2.6221, "heading":1.57166, "vx":-0.10873, "vy":-0.99386, "omega":-0.00582, "ax":0.00036, "ay":-0.00004, "alpha":0.01539, "fx":[-0.01596,0.03071,0.03065,-0.02272], "fy":[-0.10754,-0.00057,0.05284,0.05279]}, + {"t":3.02322, "x":5.65455, "y":2.57419, "heading":1.57138, "vx":-0.10872, "vy":-0.99387, "omega":-0.00508, "ax":-0.00116, "ay":0.00013, "alpha":0.02181, "fx":[-0.04538,0.00848,0.00843,-0.04544], "fy":[0.01985,-0.18227,0.08527,0.08523]}, + {"t":3.07143, "x":5.6493, "y":2.52628, "heading":1.57113, "vx":-0.10877, "vy":-0.99386, "omega":-0.00402, "ax":0.00192, "ay":-0.00021, "alpha":0.04168, "fx":[0.00688,0.07662,0.07652,-0.03802], "fy":[-0.41466,0.08062,0.16035,0.16034]}, + {"t":3.11964, "x":5.64406, "y":2.47837, "heading":1.57094, "vx":-0.10868, "vy":-0.99387, "omega":-0.00201, "ax":-0.00116, "ay":0.00015, "alpha":0.02843, "fx":[-0.0585,0.02241,0.02137,-0.059], "fy":[0.01191,-0.21509,0.10609,0.10633]}, + {"t":3.16785, "x":5.63882, "y":2.43045, "heading":1.57084, "vx":-0.10874, "vy":-0.99386, "omega":-0.00064, "ax":0.51644, "ay":4.71723, "alpha":0.00943, "fx":[8.15668,8.24555,8.24044,8.15269], "fy":[74.91612,74.84777,74.89444,74.89963]}, + {"t":3.21606, "x":5.63418, "y":2.38802, "heading":1.57081, "vx":-0.08384, "vy":-0.76645, "omega":-0.00019, "ax":0.86942, "ay":7.94819, "alpha":0.00173, "fx":[13.79491,13.81131,13.81175,13.79271], "fy":[126.18348,126.18187,126.18313,126.18508]}, + {"t":3.26427, "x":5.63115, "y":2.36031, "heading":1.5708, "vx":-0.04193, "vy":-0.38328, "omega":-0.00011, "ax":0.86968, "ay":7.95041, "alpha":0.00219, "fx":[13.79691,13.82072,13.81533,13.79448], "fy":[126.21882,126.21636,126.21847,126.2206]}, + {"t":3.31248, "x":5.63014, "y":2.35107, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[ + {"name":"IntakeDown", "from":{"target":0, "targetTimestamp":0.0, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeDown"}}}, + {"name":"IntakeUp", "from":{"target":2, "targetTimestamp":3.31248, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeUp"}}}] +} diff --git a/src/main/deploy/choreo/LeftIntakeSweepShort.traj b/src/main/deploy/choreo/LeftIntakeSweepShort.traj new file mode 100644 index 0000000..f1df33f --- /dev/null +++ b/src/main/deploy/choreo/LeftIntakeSweepShort.traj @@ -0,0 +1,207 @@ +{ + "name":"LeftIntakeSweepShort", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":5.631003379821777, "y":5.925289154052734, "heading":-1.57, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.97802209854126, "y":7.121528148651123, "heading":-1.677531398412399, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.731671810150147, "y":6.831313133239746, "heading":-1.57, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.731671810150147, "y":4.8194780349731445, "heading":-1.57, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.640606880187988, "y":5.06390380859375, "heading":4.71238898038469, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.625009536743164, "y":5.591437816619873, "heading":-1.57, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":2, "to":3, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":5.0}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":7.0}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxAngularVelocity", "props":{"max":10.0}}, "enabled":true}, + {"from":3, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":5.0}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":5, "to":5, "data":{"type":"MaxAngularVelocity", "props":{"max":10.0}}, "enabled":false}, + {"from":3, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":3.0}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"5.631003379821777 m", "val":5.631003379821777}, "y":{"exp":"5.925289154052734 m", "val":5.925289154052734}, "heading":{"exp":"-3.14 / 2 rad", "val":-1.57}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.97802209854126 m", "val":6.97802209854126}, "y":{"exp":"7.121528148651123 m", "val":7.121528148651123}, "heading":{"exp":"-1.677531398412399 rad", "val":-1.677531398412399}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.7316718101501465 m", "val":7.731671810150147}, "y":{"exp":"6.831313133239746 m", "val":6.831313133239746}, "heading":{"exp":"-3.14 / 2 rad", "val":-1.57}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.7316718101501465 m", "val":7.731671810150147}, "y":{"exp":"4.8194780349731445 m", "val":4.8194780349731445}, "heading":{"exp":"-3.14 / 2 rad", "val":-1.57}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.640606880187988 m", "val":6.640606880187988}, "y":{"exp":"5.06390380859375 m", "val":5.06390380859375}, "heading":{"exp":"270 deg", "val":4.71238898038469}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.625009536743164 m", "val":5.625009536743164}, "y":{"exp":"5.591437816619873 m", "val":5.591437816619873}, "heading":{"exp":"-3.14 / 2 rad", "val":-1.57}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":2, "to":3, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"5 m / s", "val":5.0}}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"7 m / s ^ 2", "val":7.0}}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"10 rad / s", "val":10.0}}}, "enabled":true}, + {"from":3, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"5 m / s", "val":5.0}}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, + {"from":5, "to":5, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"10 rad / s", "val":10.0}}}, "enabled":false}, + {"from":3, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"3 rad / s", "val":3.0}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.51607,0.96582,3.00277,3.55146,4.12415], + "samples":[ + {"t":0.0, "x":5.631, "y":5.92529, "heading":-1.57, "vx":2.76089, "vy":4.09666, "omega":-0.22962, "ax":-0.18924, "ay":-6.24216, "alpha":-0.23526, "fx":[-4.01254,-1.97515,-1.95259,-4.07725], "fy":[-100.09082,-98.59072,-98.87693,-98.83727]}, + {"t":0.0215, "x":5.69033, "y":6.01194, "heading":-1.57494, "vx":2.75682, "vy":3.96244, "omega":-0.23468, "ax":-0.45915, "ay":-6.90574, "alpha":-0.27446, "fx":[-8.38245,-6.09989,-6.14307,-8.53169], "fy":[-109.90124,-110.01159,-109.37424,-109.24778]}, + {"t":0.04301, "x":5.7495, "y":6.09554, "heading":-1.57998, "vx":2.74694, "vy":3.81395, "omega":-0.24058, "ax":-0.54444, "ay":-6.93525, "alpha":-0.27966, "fx":[-9.88023,-7.3993,-7.50933,-9.78468], "fy":[-110.35222,-110.51044,-109.84327,-109.70308]}, + {"t":0.06451, "x":5.80844, "y":6.17595, "heading":-1.58516, "vx":2.73524, "vy":3.66482, "omega":-0.2466, "ax":-0.57964, "ay":-6.946, "alpha":-0.27112, "fx":[-10.29168,-7.99319,-8.0883,-10.43591], "fy":[-110.5085,-110.66098,-110.04083,-109.8811]}, + {"t":0.08601, "x":5.86712, "y":6.25315, "heading":-1.59046, "vx":2.72277, "vy":3.51546, "omega":-0.25243, "ax":-0.60374, "ay":-6.9511, "alpha":-0.22179, "fx":[-10.55776,-8.60307,-8.69171,-10.48665], "fy":[-110.54925,-110.68911,-110.1514,-110.02528]}, + {"t":0.10751, "x":5.92553, "y":6.32713, "heading":-1.59589, "vx":2.70979, "vy":3.36599, "omega":-0.2572, "ax":-0.61879, "ay":-6.95418, "alpha":-0.17635, "fx":[-10.52993,-9.05345,-9.10285,-10.60892], "fy":[-110.56102,-110.66704,-110.24668,-110.13585]}, + {"t":0.12902, "x":5.98365, "y":6.3979, "heading":-1.60142, "vx":2.69649, "vy":3.21646, "omega":-0.26099, "ax":-0.63057, "ay":-6.9561, "alpha":-0.10503, "fx":[-10.49127,-9.56622,-9.60481,-10.38054], "fy":[-110.53978,-110.60923,-110.32014,-110.26371]}, + {"t":0.15052, "x":6.04149, "y":6.46546, "heading":-1.60703, "vx":2.68293, "vy":3.06688, "omega":-0.26325, "ax":-0.63922, "ay":-6.95747, "alpha":-0.03957, "fx":[-10.28398,-10.01145,-9.99911,-10.29785], "fy":[-110.51546,-110.53165,-110.39645,-110.37592]}, + {"t":0.17202, "x":6.09903, "y":6.5298, "heading":-1.61269, "vx":2.66918, "vy":2.91728, "omega":-0.2641, "ax":-0.64636, "ay":-6.95843, "alpha":0.04296, "fx":[-10.09823,-10.4945,-10.48106,-9.97213], "fy":[-110.47486,-110.43883,-110.461,-110.50627]}, + {"t":0.19353, "x":6.15628, "y":6.59092, "heading":-1.61837, "vx":2.65528, "vy":2.76765, "omega":-0.26317, "ax":-0.65207, "ay":-6.95917, "alpha":0.1205, "fx":[-9.79863,-10.94196,-10.88921,-9.77858], "fy":[-110.43598,-110.33445,-110.53051,-110.62707]}, + {"t":0.21503, "x":6.21322, "y":6.64882, "heading":-1.62403, "vx":2.64126, "vy":2.61801, "omega":-0.26058, "ax":-0.65693, "ay":-6.95974, "alpha":0.21054, "fx":[-9.51644,-11.42347,-11.36585,-9.41095], "fy":[-110.38848,-110.22074,-110.5923,-110.76247]}, + {"t":0.23653, "x":6.26987, "y":6.70351, "heading":-1.62963, "vx":2.62714, "vy":2.46836, "omega":-0.25605, "ax":-0.66101, "ay":-6.96019, "alpha":0.29666, "fx":[-9.15506,-11.87894,-11.79325,-9.1488], "fy":[-110.34316,-110.09958,-110.65819,-110.89186]}, + {"t":0.25803, "x":6.32621, "y":6.75497, "heading":-1.63514, "vx":2.61292, "vy":2.31869, "omega":-0.24968, "ax":-0.66455, "ay":-6.96056, "alpha":0.39187, "fx":[-8.80044,-12.36206,-12.27136,-8.76728], "fy":[-110.29348,-109.97193,-110.71865,-111.03188]}, + {"t":0.27954, "x":6.38224, "y":6.80322, "heading":-1.64051, "vx":2.59863, "vy":2.16902, "omega":-0.24125, "ax":-0.66764, "ay":-6.96086, "alpha":0.4846, "fx":[-8.39394,-12.82996,-12.71527,-8.45766], "fy":[-110.24517,-109.83844,-110.78241,-111.16901]}, + {"t":0.30104, "x":6.43796, "y":6.84825, "heading":-1.64569, "vx":2.58428, "vy":2.01934, "omega":-0.23083, "ax":-0.67035, "ay":-6.96111, "alpha":0.58317, "fx":[-7.98773,-13.31063,-13.19654,-8.07433], "fy":[-110.19442,-109.70123,-110.84213,-111.31316]}, + {"t":0.32254, "x":6.49337, "y":6.89007, "heading":-1.65066, "vx":2.56986, "vy":1.86966, "omega":-0.21829, "ax":-0.67277, "ay":-6.96132, "alpha":0.68159, "fx":[-7.53939,-13.79472,-13.65651,-7.73194], "fy":[-110.14545,-109.55775,-110.90431,-111.45683]}, + {"t":0.34404, "x":6.54848, "y":6.92866, "heading":-1.65535, "vx":2.5554, "vy":1.71997, "omega":-0.20363, "ax":-0.67492, "ay":-6.9615, "alpha":0.78258, "fx":[-7.09008,-14.27293,-14.14012,-7.3561], "fy":[-110.09425,-109.41338,-110.96379,-111.60439]}, + {"t":0.36555, "x":6.60327, "y":6.96403, "heading":-1.65973, "vx":2.54088, "vy":1.57028, "omega":-0.18681, "ax":-0.67686, "ay":-6.96166, "alpha":0.88592, "fx":[-6.60802,-14.77221,-14.61674,-6.98577], "fy":[-110.04511,-109.26203,-111.02464,-111.75386]}, + {"t":0.38705, "x":6.65775, "y":6.99619, "heading":-1.66375, "vx":2.52633, "vy":1.42058, "omega":-0.16776, "ax":-0.67861, "ay":-6.96179, "alpha":0.98946, "fx":[-6.11942,-15.25684,-15.10085,-6.61686], "fy":[-109.99381,-109.11101,-111.08469,-111.90472]}, + {"t":0.40855, "x":6.71192, "y":7.02513, "heading":-1.66735, "vx":2.51174, "vy":1.27089, "omega":-0.14648, "ax":-0.68021, "ay":-6.96191, "alpha":1.09698, "fx":[-5.61034,-15.77507,-15.58681,-6.22337], "fy":[-109.94431,-108.95265,-111.14561,-112.05914]}, + {"t":0.43006, "x":6.76577, "y":7.05085, "heading":-1.6705, "vx":2.49711, "vy":1.12119, "omega":-0.12289, "ax":-0.68165, "ay":-6.96202, "alpha":1.20331, "fx":[-5.09338,-16.27032,-16.07382,-5.84946], "fy":[-109.89215,-108.79601,-111.20661,-112.21367]}, + {"t":0.45156, "x":6.81931, "y":7.07335, "heading":-1.67315, "vx":2.48245, "vy":0.97148, "omega":-0.09702, "ax":-0.68301, "ay":-6.96211, "alpha":1.31405, "fx":[-4.55786,-16.80592,-16.56376,-5.44534], "fy":[-109.84186,-108.63202,-111.2688,-112.37153]}, + {"t":0.47306, "x":6.87253, "y":7.09263, "heading":-1.67523, "vx":2.46777, "vy":0.82178, "omega":-0.06876, "ax":-0.68422, "ay":-6.96219, "alpha":1.42237, "fx":[-4.01513,-17.31485,-17.0441,-5.07566], "fy":[-109.78787,-108.47033,-111.33277,-112.52859]}, + {"t":0.49456, "x":6.92543, "y":7.10869, "heading":-1.67671, "vx":2.45305, "vy":0.67207, "omega":-0.03818, "ax":-0.68537, "ay":-6.96226, "alpha":1.53701, "fx":[-3.46018,-17.87825,-17.53751,-4.64677], "fy":[-109.73612,-108.30058,-111.39737,-112.6901]}, + {"t":0.51607, "x":6.97802, "y":7.12153, "heading":-1.67753, "vx":2.43832, "vy":0.52236, "omega":-0.00513, "ax":-0.83356, "ay":-6.9434, "alpha":1.75633, "fx":[-4.85568,-21.42466,-20.61205,-6.04106], "fy":[-109.46463,-107.50648,-111.12621,-112.82923]}, + {"t":0.54252, "x":7.04224, "y":7.13292, "heading":-1.67767, "vx":2.41626, "vy":0.33867, "omega":0.04134, "ax":-1.16588, "ay":-6.89852, "alpha":1.61265, "fx":[-11.21808,-26.22231,-24.97024,-11.6264], "fy":[-109.05257,-106.65977,-110.15273,-112.21125]}, + {"t":0.56898, "x":7.10576, "y":7.13946, "heading":-1.67657, "vx":2.38542, "vy":0.15616, "omega":0.084, "ax":-1.5532, "ay":-6.82143, "alpha":1.49155, "fx":[-18.41005,-32.0035,-30.23515,-17.98387], "fy":[-108.08292,-105.24646,-108.70986,-111.14183]}, + {"t":0.59544, "x":7.16832, "y":7.14121, "heading":-1.67435, "vx":2.34433, "vy":-0.02431, "omega":0.12346, "ax":-2.00601, "ay":-6.70188, "alpha":1.36636, "fx":[-26.68051,-38.71821,-36.49329,-25.49566], "fy":[-106.36026,-103.14776,-106.65346,-109.42757]}, + {"t":0.62189, "x":7.22964, "y":7.13822, "heading":-1.67108, "vx":2.29126, "vy":-0.20161, "omega":0.15961, "ax":-2.53443, "ay":-6.51999, "alpha":1.25548, "fx":[-36.13322,-46.64671,-43.95623,-34.20761], "fy":[-103.56899,-100.0129,-103.67837,-106.77792]}, + {"t":0.64835, "x":7.28937, "y":7.1306, "heading":-1.66686, "vx":2.22421, "vy":-0.3741, "omega":0.19283, "ax":-3.14512, "ay":-6.24783, "alpha":1.14009, "fx":[-46.92635,-55.70211,-52.71766,-44.37837], "fy":[-99.20027,-95.47557,-99.37347,-102.70651]}, + {"t":0.6748, "x":7.34711, "y":7.11852, "heading":-1.66176, "vx":2.141, "vy":-0.5394, "omega":0.22299, "ax":-3.83598, "ay":-5.84858, "alpha":1.03893, "fx":[-58.87303,-65.99214,-62.79909,-55.93171], "fy":[-92.70345,-88.90641,-93.17273,-96.61907]}, + {"t":0.70126, "x":7.40241, "y":7.1022, "heading":-1.65586, "vx":2.03951, "vy":-0.69413, "omega":0.25048, "ax":-4.58607, "ay":-5.28034, "alpha":0.93139, "fx":[-71.69817,-76.99257,-73.92184,-68.6161], "fy":[-83.31981,-79.81403,-84.4219,-87.76164]}, + {"t":0.72772, "x":7.45477, "y":7.08199, "heading":-1.64923, "vx":1.91818, "vy":-0.83382, "omega":0.27512, "ax":-5.34596, "ay":-4.50879, "alpha":0.83161, "fx":[-84.38268,-88.11005,-85.39071,-81.60073], "fy":[-70.6655,-67.60624,-72.54075,-75.50895]}, + {"t":0.75417, "x":7.50364, "y":7.05835, "heading":-1.64196, "vx":1.77675, "vy":-0.95311, "omega":0.29712, "ax":-6.03622, "ay":-3.53115, "alpha":0.72278, "fx":[-95.73026,-98.04013,-96.00173,-93.54538], "fy":[-54.65652,-52.4441,-57.40522,-59.73248]}, + {"t":0.78063, "x":7.54854, "y":7.0319, "heading":-1.6341, "vx":1.61706, "vy":-1.04653, "omega":0.31624, "ax":-6.5691, "ay":-2.39815, "alpha":0.60305, "fx":[-104.24088,-105.61051,-104.37348,-102.93236], "fy":[-36.43158,-35.02713,-39.66749,-41.16353]}, + {"t":0.80708, "x":7.58902, "y":7.00338, "heading":-1.62573, "vx":1.44327, "vy":-1.10997, "omega":0.33219, "ax":-6.88789, "ay":-1.20976, "alpha":0.47408, "fx":[-109.20991,-110.00011,-109.5063,-108.68509], "fy":[-17.5526,-16.92104,-20.80935,-21.54026]}, + {"t":0.83354, "x":7.62479, "y":6.97359, "heading":-1.61694, "vx":1.26104, "vy":-1.14198, "omega":0.34474, "ax":-6.99326, "ay":-0.07532, "alpha":0.33384, "fx":[-110.7779,-111.26717,-111.26946,-110.7781], "fy":[0.18342,0.26232,-2.53356,-2.69526]}, + {"t":0.86, "x":7.6557, "y":6.94335, "heading":-1.60782, "vx":1.07603, "vy":-1.14397, "omega":0.35357, "ax":-6.93214, "ay":0.92899, "alpha":0.19042, "fx":[-109.81467,-110.10804,-110.29414,-109.99443], "fy":[15.57223,15.52584,13.89928,13.99614]}, + {"t":0.88645, "x":7.68175, "y":6.91341, "heading":-1.59847, "vx":0.89263, "vy":-1.11939, "omega":0.35861, "ax":-6.76594, "ay":1.77379, "alpha":0.04966, "fx":[-107.31534,-107.43165,-107.50441,-107.40559], "fy":[28.42303,28.28983,27.93191,27.9963]}, + {"t":0.91291, "x":7.70299, "y":6.88442, "heading":-1.58898, "vx":0.71363, "vy":-1.07247, "omega":0.35992, "ax":-6.54666, "ay":2.46416, "alpha":-0.09293, "fx":[-104.15357,-103.93879,-103.72941,-103.91019], "fy":[38.62902,38.87564,39.56452,39.4119]}, + {"t":0.93936, "x":7.71958, "y":6.8569, "heading":-1.57946, "vx":0.54043, "vy":-1.00728, "omega":0.35746, "ax":-6.3094, "ay":3.02131, "alpha":-0.22578, "fx":[-100.75997,-100.2166,-99.55102,-100.13773], "fy":[46.88367,47.33301,49.08786,48.55718]}, + {"t":0.96582, "x":7.73167, "y":6.83131, "heading":-1.57, "vx":0.37351, "vy":-0.92734, "omega":0.35149, "ax":-6.81519, "ay":-1.52777, "alpha":-0.34496, "fx":[-108.23715,-107.64053,-108.16061,-108.7462], "fy":[-25.46302,-25.90541,-23.03662,-22.61299]}, + {"t":1.01211, "x":7.74166, "y":6.78675, "heading":-1.55373, "vx":0.05801, "vy":-0.99807, "omega":0.33552, "ax":-2.60199, "ay":0.0051, "alpha":-1.11405, "fx":[-43.57167,-38.86339,-38.946,-43.8531], "fy":[-3.26879,-3.26861,3.52237,3.33887]}, + {"t":1.05841, "x":7.74156, "y":6.74055, "heading":-1.5382, "vx":-0.06245, "vy":-0.99783, "omega":0.28394, "ax":0.89934, "ay":-0.03748, "alpha":-1.14977, "fx":[11.60497,17.14409,16.953,11.40894], "fy":[-3.92069,-3.67398,2.68666,2.52766]}, + {"t":1.1047, "x":7.73963, "y":6.69431, "heading":-1.52505, "vx":-0.02082, "vy":-0.99957, "omega":0.23072, "ax":0.08876, "ay":-0.00167, "alpha":-0.98857, "fx":[-0.94361,3.89217,3.64963,-0.9617], "fy":[-2.8985,-2.69813,2.84959,2.64126]}, + {"t":1.151, "x":7.73876, "y":6.64804, "heading":-1.51437, "vx":-0.01671, "vy":-0.99965, "omega":0.18495, "ax":0.05349, "ay":-0.00083, "alpha":-0.86143, "fx":[-1.11188,3.07106,2.81065,-1.37288], "fy":[-2.51586,-2.27321,2.48044,2.25607]}, + {"t":1.19729, "x":7.73805, "y":6.60176, "heading":-1.50581, "vx":-0.01423, "vy":-0.99969, "omega":0.14507, "ax":0.04147, "ay":-0.00055, "alpha":-0.73387, "fx":[-1.01841,2.55494,2.29874,-1.202], "fy":[-2.15055,-1.94747,2.14299,1.92007]}, + {"t":1.24358, "x":7.73743, "y":6.55548, "heading":-1.49909, "vx":-0.01231, "vy":-0.99971, "omega":0.1111, "ax":0.03367, "ay":-0.00039, "alpha":-0.63433, "fx":[-0.88366,2.19699,1.95273,-1.12808], "fy":[-1.87007,-1.64322,1.85009,1.63854]}, + {"t":1.28988, "x":7.7369, "y":6.5092, "heading":-1.49395, "vx":-0.01075, "vy":-0.99973, "omega":0.08173, "ax":0.02853, "ay":-0.00029, "alpha":-0.5408, "fx":[-0.75634,1.87021,1.64713,-0.94917], "fy":[-1.5928,-1.41443,1.5918,1.39714]}, + {"t":1.33617, "x":7.73643, "y":6.46291, "heading":-1.49016, "vx":-0.00943, "vy":-0.99974, "omega":0.0567, "ax":0.02453, "ay":-0.00022, "alpha":-0.46619, "fx":[-0.64117,1.62217,1.42011,-0.84328], "fy":[-1.38156,-1.19602,1.36947,1.1943]}, + {"t":1.38247, "x":7.73602, "y":6.41663, "heading":-1.48754, "vx":-0.0083, "vy":-0.99975, "omega":0.03511, "ax":0.02159, "ay":-0.00017, "alpha":-0.39711, "fx":[-0.53685,1.39005,1.21255,-0.69477], "fy":[-1.17467,-1.03064,1.17482,1.01981]}, + {"t":1.42876, "x":7.73566, "y":6.37035, "heading":-1.48591, "vx":-0.0073, "vy":-0.99976, "omega":0.01673, "ax":0.01915, "ay":-0.00013, "alpha":-0.34191, "fx":[-0.44785,1.21182,1.05574,-0.60395], "fy":[-1.01509,-0.87364,1.00786,0.87254]}, + {"t":1.47506, "x":7.73534, "y":6.32406, "heading":-1.48514, "vx":-0.00641, "vy":-0.99977, "omega":0.0009, "ax":0.01724, "ay":-0.0001, "alpha":-0.2909, "fx":[-0.36745,1.04283,0.90909,-0.48952], "fy":[-0.86217,-0.75197,0.86219,0.74537]}, + {"t":1.52135, "x":7.73506, "y":6.27778, "heading":-1.4851, "vx":-0.00561, "vy":-0.99977, "omega":-0.01256, "ax":0.01562, "ay":-0.00008, "alpha":-0.24996, "fx":[-0.30103,0.91237,0.79712,-0.41629], "fy":[-0.74163,-0.63859,0.73745,0.63756]}, + {"t":1.56764, "x":7.73482, "y":6.2315, "heading":-1.48568, "vx":-0.00489, "vy":-0.99977, "omega":-0.02414, "ax":0.01432, "ay":-0.00007, "alpha":-0.21236, "fx":[-0.23925,0.78891,0.692,-0.33214], "fy":[-0.62912,-0.54799,0.62881,0.54415]}, + {"t":1.61394, "x":7.73461, "y":6.18521, "heading":-1.4868, "vx":-0.00423, "vy":-0.99978, "omega":-0.03397, "ax":0.01322, "ay":-0.00005, "alpha":-0.18178, "fx":[-0.19034,0.69224,0.61008,-0.2725], "fy":[-0.53799,-0.46561,0.53575,0.46457]}, + {"t":1.66023, "x":7.73443, "y":6.13893, "heading":-1.48837, "vx":-0.00361, "vy":-0.99978, "omega":-0.04238, "ax":0.01232, "ay":-0.00004, "alpha":-0.15403, "fx":[-0.14224,0.60223,0.53429,-0.21207], "fy":[-0.4553,-0.39739,0.45472,0.39536]}, + {"t":1.70653, "x":7.73427, "y":6.09264, "heading":-1.49033, "vx":-0.00304, "vy":-0.99978, "omega":-0.04951, "ax":0.01158, "ay":-0.00003, "alpha":-0.13097, "fx":[-0.10588,0.53014,0.47344,-0.16258], "fy":[-0.38612,-0.33705,0.38511,0.33601]}, + {"t":1.75282, "x":7.73414, "y":6.04636, "heading":-1.49262, "vx":-0.00251, "vy":-0.99978, "omega":-0.05558, "ax":0.01097, "ay":-0.00002, "alpha":-0.11038, "fx":[-0.06755,0.46482,0.41877,-0.11933], "fy":[-0.32512,-0.28497,0.32438,0.28414]}, + {"t":1.79912, "x":7.73404, "y":6.00008, "heading":-1.4952, "vx":-0.002, "vy":-0.99978, "omega":-0.06069, "ax":0.01051, "ay":-0.00002, "alpha":-0.09274, "fx":[-0.03951,0.41089,0.37319,-0.07721], "fy":[-0.27205,-0.2402,0.27186,0.23922]}, + {"t":1.84541, "x":7.73396, "y":5.95379, "heading":-1.49801, "vx":-0.00151, "vy":-0.99979, "omega":-0.06498, "ax":0.01015, "ay":-0.00001, "alpha":-0.07727, "fx":[-0.00781,0.36388,0.33396,-0.04574], "fy":[-0.22657,-0.19965,0.22577,0.19963]}, + {"t":1.8917, "x":7.7339, "y":5.90751, "heading":-1.50101, "vx":-0.00104, "vy":-0.99979, "omega":-0.06856, "ax":0.00992, "ay":-0.00001, "alpha":-0.06346, "fx":[0.01531,0.32341,0.29962,-0.00849], "fy":[-0.18507,-0.16573,0.18545,0.16484]}, + {"t":1.938, "x":7.73386, "y":5.86122, "heading":-1.50419, "vx":-0.00059, "vy":-0.99979, "omega":-0.07149, "ax":0.00977, "ay":0.0, "alpha":-0.05158, "fx":[0.04298,0.29004,0.27186,0.01548], "fy":[-0.15039,-0.13317,0.14961,0.13372]}, + {"t":1.98429, "x":7.73384, "y":5.81494, "heading":-1.5075, "vx":-0.00013, "vy":-0.99979, "omega":-0.07388, "ax":0.00976, "ay":0.0, "alpha":-0.04036, "fx":[0.06392,0.25953,0.24583,0.05022], "fy":[-0.11683,-0.1065,0.11763,0.10575]}, + {"t":2.03059, "x":7.73385, "y":5.76865, "heading":-1.51092, "vx":0.00032, "vy":-0.99979, "omega":-0.07575, "ax":0.00981, "ay":0.00001, "alpha":-0.03084, "fx":[0.0898,0.23634,0.22665,0.07016], "fy":[-0.08927,-0.07913,0.0886,0.08014]}, + {"t":2.07688, "x":7.73387, "y":5.72237, "heading":-1.51443, "vx":0.00077, "vy":-0.99979, "omega":-0.07718, "ax":0.01001, "ay":0.00001, "alpha":-0.0212, "fx":[0.11098,0.21315,0.20678,0.1046], "fy":[-0.0607,-0.05689,0.06189,0.05633]}, + {"t":2.12318, "x":7.73392, "y":5.67609, "heading":-1.518, "vx":0.00124, "vy":-0.99979, "omega":-0.07816, "ax":0.01027, "ay":0.00002, "alpha":-0.01305, "fx":[0.13713,0.19756,0.19404,0.12358], "fy":[-0.03732,-0.03232,0.03684,0.03376]}, + {"t":2.16947, "x":7.73399, "y":5.6298, "heading":-1.52162, "vx":0.00171, "vy":-0.99979, "omega":-0.07876, "ax":0.01069, "ay":0.00002, "alpha":-0.00414, "fx":[0.16088,0.17973,0.17869,0.15984], "fy":[-0.01119,-0.01216,0.01282,0.01186]}, + {"t":2.21576, "x":7.73408, "y":5.58352, "heading":-1.52526, "vx":0.00221, "vy":-0.99978, "omega":-0.07896, "ax":0.01119, "ay":0.00003, "alpha":0.0035, "fx":[0.18938,0.16975,0.17074,0.18096], "fy":[0.01049,0.01178,-0.01069,-0.00983]}, + {"t":2.26206, "x":7.73419, "y":5.53723, "heading":-1.52892, "vx":0.00273, "vy":-0.99978, "omega":-0.07879, "ax":0.01187, "ay":0.00004, "alpha":0.01247, "fx":[0.21824,0.1559,0.15877,0.22111], "fy":[0.03654,0.03197,-0.03434,-0.03192]}, + {"t":2.30835, "x":7.73433, "y":5.49095, "heading":-1.53257, "vx":0.00327, "vy":-0.99978, "omega":-0.07822, "ax":0.01266, "ay":0.00005, "alpha":0.02043, "fx":[0.25145,0.15006,0.15434,0.24792], "fy":[0.05874,0.0575,-0.05856,-0.05481]}, + {"t":2.35465, "x":7.7345, "y":5.44466, "heading":-1.53619, "vx":0.00386, "vy":-0.99978, "omega":-0.07727, "ax":0.01366, "ay":0.00006, "alpha":0.03021, "fx":[0.2886,0.13941,0.1451,0.29428], "fy":[0.08717,0.07973,-0.08407,-0.07921]}, + {"t":2.40094, "x":7.73469, "y":5.39838, "heading":-1.53976, "vx":0.00449, "vy":-0.99978, "omega":-0.07587, "ax":0.01482, "ay":0.00007, "alpha":0.03936, "fx":[0.32968,0.13692,0.14353,0.33092], "fy":[0.112,0.10935,-0.11129,-0.10551]}, + {"t":2.44724, "x":7.73492, "y":5.3521, "heading":-1.54328, "vx":0.00518, "vy":-0.99977, "omega":-0.07405, "ax":0.01626, "ay":0.00009, "alpha":0.05078, "fx":[0.37934,0.1294,0.13699,0.38693], "fy":[0.14564,0.13564,-0.14103,-0.13452]}, + {"t":2.49353, "x":7.73517, "y":5.30581, "heading":-1.5467, "vx":0.00593, "vy":-0.99977, "omega":-0.0717, "ax":0.01797, "ay":0.00011, "alpha":0.06201, "fx":[0.43343,0.13092,0.13899,0.43795], "fy":[0.17518,0.1725,-0.17374,-0.1667]}, + {"t":2.53982, "x":7.73547, "y":5.25953, "heading":-1.55002, "vx":0.00676, "vy":-0.99976, "omega":-0.06883, "ax":0.02009, "ay":0.00015, "alpha":0.07613, "fx":[0.50172,0.12758,0.13618,0.51032], "fy":[0.21764,0.20496,-0.21038,-0.20299]}, + {"t":2.58612, "x":7.7358, "y":5.21325, "heading":-1.55321, "vx":0.00769, "vy":-0.99976, "omega":-0.0653, "ax":0.02272, "ay":0.00019, "alpha":0.09044, "fx":[0.57773,0.13674,0.14533,0.58278], "fy":[0.25392,0.25332,-0.25145,-0.24394]}, + {"t":2.63241, "x":7.73618, "y":5.16696, "heading":-1.55623, "vx":0.00875, "vy":-0.99975, "omega":-0.06112, "ax":0.02604, "ay":0.00024, "alpha":0.10858, "fx":[0.67568,0.14241,0.15103,0.6843], "fy":[0.31026,0.294,-0.29812,-0.29069]}, + {"t":2.67871, "x":7.73661, "y":5.12068, "heading":-1.55906, "vx":0.00995, "vy":-0.99974, "omega":-0.05609, "ax":0.03046, "ay":0.00032, "alpha":0.12776, "fx":[0.78827,0.16728,0.17538,0.80356], "fy":[0.35461,0.36022,-0.35066,-0.34356]}, + {"t":2.725, "x":7.73711, "y":5.0744, "heading":-1.56166, "vx":0.01136, "vy":-0.99972, "omega":-0.05018, "ax":0.0365, "ay":0.00045, "alpha":0.15099, "fx":[0.94654,0.20487,0.21243,0.95411], "fy":[0.43323,0.41043,-0.41096,-0.4044]}, + {"t":2.7713, "x":7.73767, "y":5.02812, "heading":-1.56398, "vx":0.01305, "vy":-0.9997, "omega":-0.04319, "ax":0.04592, "ay":0.00065, "alpha":0.18002, "fx":[1.13041,0.27279,0.27937,1.23318], "fy":[0.48556,0.50517,-0.47768,-0.47188]}, + {"t":2.81759, "x":7.73833, "y":4.98184, "heading":-1.56598, "vx":0.01518, "vy":-0.99967, "omega":-0.03485, "ax":0.08291, "ay":0.00142, "alpha":0.20789, "fx":[1.81955,0.8071,0.81283,1.82536], "fy":[0.59981,0.59508,-0.55482,-0.55004]}, + {"t":2.86388, "x":7.73912, "y":4.93556, "heading":-1.56759, "vx":0.01901, "vy":-0.99961, "omega":-0.02523, "ax":1.50646, "ay":0.08144, "alpha":0.22099, "fx":[24.56171,23.45468,23.46216,24.18604], "fy":[2.01718,1.93202,0.6099,0.61247]}, + {"t":2.91018, "x":7.74161, "y":4.88937, "heading":-1.56876, "vx":0.08876, "vy":-0.99584, "omega":-0.015, "ax":-3.78273, "ay":5.53946, "alpha":0.07066, "fx":[-59.67629,-60.26288,-60.34913,-59.9263], "fy":[88.23193,87.90454,87.71514,87.92055]}, + {"t":2.95647, "x":7.74167, "y":4.84921, "heading":-1.56946, "vx":-0.08636, "vy":-0.73939, "omega":-0.01173, "ax":-5.59697, "ay":4.19948, "alpha":0.0631, "fx":[-88.63221,-88.86128,-89.09397,-88.83683], "fy":[66.95817,66.78677,66.36213,66.57231]}, + {"t":3.00277, "x":7.73167, "y":4.81948, "heading":-1.57, "vx":-0.34547, "vy":-0.54498, "omega":-0.00881, "ax":-6.04497, "ay":3.52064, "alpha":0.04351, "fx":[-95.8309,-95.9704,-96.10347,-95.96851], "fy":[56.11035,55.98123,55.67904,55.8006]}, + {"t":3.03504, "x":7.71737, "y":4.80372, "heading":-1.57028, "vx":-0.54058, "vy":-0.43135, "omega":-0.0074, "ax":-6.04269, "ay":3.52876, "alpha":0.03922, "fx":[-95.8126,-95.93217,-96.05902,-95.92503], "fy":[56.20944,56.10683,55.8214,55.94867]}, + {"t":3.06732, "x":7.69678, "y":4.79164, "heading":-1.57052, "vx":-0.73561, "vy":-0.31745, "omega":-0.00613, "ax":-6.03788, "ay":3.53666, "alpha":0.03732, "fx":[-95.73922,-95.85426,-95.97274,-95.85711], "fy":[56.33066,56.22795,55.9627,56.06687]}, + {"t":3.09959, "x":7.66989, "y":4.78323, "heading":-1.57072, "vx":-0.93049, "vy":-0.2033, "omega":-0.00493, "ax":-6.03237, "ay":3.54569, "alpha":0.0333, "fx":[-95.66643,-95.76941,-95.87785,-95.75935], "fy":[56.4497,56.36031,56.11817,56.23357]}, + {"t":3.13187, "x":7.63672, "y":4.77852, "heading":-1.57088, "vx":-1.12519, "vy":-0.08886, "omega":-0.00386, "ax":-6.02598, "ay":3.55611, "alpha":0.03142, "fx":[-95.5682,-95.66524,-95.76587,-95.66803], "fy":[56.61038,56.5238,56.30057,56.38887]}, + {"t":3.16415, "x":7.59726, "y":4.7775, "heading":-1.571, "vx":-1.31969, "vy":0.02591, "omega":-0.00284, "ax":-6.01849, "ay":3.56827, "alpha":0.02744, "fx":[-95.46433,-95.54974,-95.64064,-95.53724], "fy":[56.77885,56.70515,56.50422,56.6076]}, + {"t":3.19642, "x":7.55153, "y":4.7802, "heading":-1.5711, "vx":-1.51394, "vy":0.14108, "omega":-0.00196, "ax":-6.0096, "ay":3.58264, "alpha":0.02604, "fx":[-95.32664,-95.40123,-95.49017,-95.40926], "fy":[57.00153,56.9396,56.74704,56.82028]}, + {"t":3.2287, "x":7.49954, "y":4.78662, "heading":-1.57116, "vx":-1.70791, "vy":0.25672, "omega":-0.00112, "ax":-5.99887, "ay":3.59989, "alpha":0.02119, "fx":[-95.17392,-95.23756,-95.31168,-95.2225], "fy":[57.24625,57.19613,57.03459,57.12658]}, + {"t":3.26097, "x":7.44129, "y":4.79678, "heading":-1.5712, "vx":-1.90152, "vy":0.37291, "omega":-0.00043, "ax":-5.98565, "ay":3.62097, "alpha":0.02068, "fx":[-94.96295,-95.02065,-95.09381,-95.02909], "fy":[57.58318,57.53663,57.38162,57.44048]}, + {"t":3.29325, "x":7.3768, "y":4.8107, "heading":-1.57121, "vx":-2.09472, "vy":0.48978, "omega":0.00024, "ax":-5.96899, "ay":3.64731, "alpha":0.01691, "fx":[-94.71037,-94.76694,-94.82499,-94.74596], "fy":[57.98236,57.93123,57.80731,57.89379]}, + {"t":3.32553, "x":7.30608, "y":4.82841, "heading":-1.5712, "vx":-2.28737, "vy":0.6075, "omega":0.00078, "ax":-5.94732, "ay":3.68117, "alpha":0.01574, "fx":[-94.37146,-94.40806,-94.47103,-94.4218], "fy":[58.51133,58.48767,58.36042,58.40551]}, + {"t":3.3578, "x":7.22915, "y":4.84993, "heading":-1.57118, "vx":-2.47933, "vy":0.72631, "omega":0.00129, "ax":-5.91801, "ay":3.7263, "alpha":0.01021, "fx":[-93.92654,-93.95536,-93.99792,-93.93119], "fy":[59.1953,59.17826,59.09055,59.16665]}, + {"t":3.39008, "x":7.14605, "y":4.87532, "heading":-1.57113, "vx":-2.67034, "vy":0.84658, "omega":0.00162, "ax":-5.87616, "ay":3.78943, "alpha":0.01061, "fx":[-93.25578,-93.28141,-93.32533,-93.29094], "fy":[60.20757,60.19,60.10523,60.13711]}, + {"t":3.42235, "x":7.0568, "y":4.90462, "heading":-1.57108, "vx":-2.86, "vy":0.96889, "omega":0.00196, "ax":-5.8116, "ay":3.88398, "alpha":0.00593, "fx":[-92.25142,-92.27423,-92.302,-92.22618], "fy":[61.67749,61.65935,61.60605,61.70095]}, + {"t":3.45463, "x":6.96146, "y":4.93791, "heading":-1.57102, "vx":-3.04757, "vy":1.09425, "omega":0.00215, "ax":-5.69925, "ay":4.04092, "alpha":0.00589, "fx":[-90.46178,-90.47195,-90.50269,-90.4824], "fy":[64.1769,64.17273,64.12083,64.14005]}, + {"t":3.48691, "x":6.86013, "y":4.97533, "heading":-1.57095, "vx":-3.23152, "vy":1.22467, "omega":0.00234, "ax":-5.45688, "ay":4.35087, "alpha":0.00217, "fx":[-86.63969,-86.65862,-86.67204,-86.55727], "fy":[69.06413,69.04521,69.02458,69.15904]}, + {"t":3.51918, "x":6.75299, "y":5.01713, "heading":-1.57087, "vx":-3.40765, "vy":1.3651, "omega":0.00241, "ax":-4.60052, "ay":5.21639, "alpha":0.00098, "fx":[-73.006,-73.1073,-73.02039,-73.01264], "fy":[82.84228,82.74766,82.82948,82.83647]}, + {"t":3.55146, "x":6.64061, "y":5.0639, "heading":-1.5708, "vx":-3.55613, "vy":1.53347, "omega":0.00245, "ax":5.43229, "ay":3.97301, "alpha":-0.04042, "fx":[86.15297,86.49677,86.15589,86.16102], "fy":[63.20475,62.69655,63.20216,63.19431]}, + {"t":3.58515, "x":6.52389, "y":5.11782, "heading":-1.57071, "vx":-3.37313, "vy":1.66731, "omega":0.00108, "ax":6.7272, "ay":-1.81687, "alpha":0.01014, "fx":[106.82932,106.7905,106.78929,106.7877], "fy":[-28.72612,-28.87924,-28.88313,-28.8882]}, + {"t":3.61883, "x":6.41407, "y":5.17295, "heading":-1.57068, "vx":-3.1465, "vy":1.6061, "omega":0.00143, "ax":6.48391, "ay":-2.59332, "alpha":0.00286, "fx":[102.92744,102.96364,102.92862,102.92753], "fy":[-41.19438,-41.10303,-41.19328,-41.19279]}, + {"t":3.65252, "x":6.31175, "y":5.22559, "heading":-1.57063, "vx":-2.92807, "vy":1.51874, "omega":0.00152, "ax":6.37065, "ay":-2.87348, "alpha":0.00602, "fx":[101.16753,101.13545,101.13729,101.11486], "fy":[-45.55197,-45.63042,-45.62434,-45.66742]}, + {"t":3.68621, "x":6.21673, "y":5.27512, "heading":-1.57058, "vx":-2.71346, "vy":1.42194, "omega":0.00172, "ax":6.30703, "ay":-3.01699, "alpha":-0.00033, "fx":[100.12093,100.14089,100.12842,100.1246], "fy":[-47.91232,-47.87432,-47.89908,-47.90185]}, + {"t":3.7199, "x":6.1289, "y":5.32131, "heading":-1.57052, "vx":-2.50099, "vy":1.3203, "omega":0.00171, "ax":6.2665, "ay":-3.10408, "alpha":0.0019, "fx":[99.49892,99.48271,99.48834,99.47127], "fy":[-49.24969,-49.29034,-49.27554,-49.30244]}, + {"t":3.75359, "x":6.0482, "y":5.36403, "heading":-1.57046, "vx":-2.28988, "vy":1.21573, "omega":0.00178, "ax":6.23848, "ay":-3.16251, "alpha":-0.00181, "fx":[99.02988,99.04987,99.0445,99.0375], "fy":[-50.22674,-50.1936,-50.20105,-50.20746]}, + {"t":3.78727, "x":5.9746, "y":5.40319, "heading":-1.5704, "vx":-2.07972, "vy":1.10919, "omega":0.00172, "ax":6.21797, "ay":-3.20442, "alpha":-0.00064, "fx":[98.71861,98.71413,98.72385,98.70245], "fy":[-50.86289,-50.88111,-50.85727,-50.88907]}, + {"t":3.82096, "x":5.90806, "y":5.43874, "heading":-1.57034, "vx":-1.87025, "vy":1.00124, "omega":0.00169, "ax":6.20231, "ay":-3.23595, "alpha":-0.00337, "fx":[98.45295,98.47164,98.47521,98.46477], "fy":[-51.39649,-51.3698,-51.35777,-51.36803]}, + {"t":3.85465, "x":5.84858, "y":5.47063, "heading":-1.57029, "vx":-1.66131, "vy":0.89223, "omega":0.00158, "ax":6.18996, "ay":-3.26051, "alpha":-0.00177, "fx":[98.27114,98.26774,98.28173,98.26011], "fy":[-51.75852,-51.77692,-51.7438,-51.77291]}, + {"t":3.88834, "x":5.79612, "y":5.49884, "heading":-1.57023, "vx":-1.45278, "vy":0.78239, "omega":0.00152, "ax":6.17998, "ay":-3.2802, "alpha":-0.00465, "fx":[98.0944,98.11738,98.1247,98.11061], "fy":[-52.106,-52.07436,-52.05372,-52.06801]}, + {"t":3.92203, "x":5.75069, "y":5.52334, "heading":-1.57018, "vx":-1.24459, "vy":0.67189, "omega":0.00137, "ax":6.17175, "ay":-3.29632, "alpha":-0.0038, "fx":[97.97447,97.97916,97.99765,97.97304], "fy":[-52.34097,-52.34636,-52.30348,-52.33515]}, + {"t":3.95571, "x":5.71226, "y":5.5441, "heading":-1.57014, "vx":-1.03668, "vy":0.56084, "omega":0.00124, "ax":6.16484, "ay":-3.30977, "alpha":-0.00615, "fx":[97.85041,97.87493,97.88916,97.87121], "fy":[-52.58138,-52.55025,-52.51492,-52.53341]}, + {"t":3.9894, "x":5.68084, "y":5.56111, "heading":-1.5701, "vx":-0.82899, "vy":0.44934, "omega":0.00103, "ax":6.15897, "ay":-3.32116, "alpha":-0.00486, "fx":[97.76905,97.77459,97.79775,97.77108], "fy":[-52.73914,-52.74586,-52.69286,-52.72524]}, + {"t":4.02309, "x":5.65641, "y":5.57437, "heading":-1.57006, "vx":-0.62151, "vy":0.33746, "omega":0.00087, "ax":6.1539, "ay":-3.33092, "alpha":-0.00769, "fx":[97.67277,97.69949,97.72037,97.69837], "fy":[-52.9237,-52.89191,-52.84242,-52.8653]}, + {"t":4.05678, "x":5.63896, "y":5.58385, "heading":-1.57003, "vx":-0.4142, "vy":0.22524, "omega":0.00061, "ax":6.1495, "ay":-3.33939, "alpha":-0.0083, "fx":[97.59995,97.63112,97.65214,97.62803], "fy":[-53.06309,-53.02459,-52.97415,-52.99929]}, + {"t":4.09047, "x":5.6285, "y":5.58954, "heading":-1.57001, "vx":-0.20703, "vy":0.11275, "omega":0.00033, "ax":6.14563, "ay":-3.3468, "alpha":-0.00973, "fx":[97.53841,97.56132,97.59598,97.56988], "fy":[-53.18044,-53.15928,-53.08254,-53.10962]}, + {"t":4.12415, "x":5.62501, "y":5.59144, "heading":-1.57, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[ + {"name":"IntakeDown", "from":{"target":1, "targetTimestamp":0.51607, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeDown"}}}, + {"name":"IntakeUp", "from":{"target":4, "targetTimestamp":3.55146, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeUp"}}}] +} diff --git a/src/main/deploy/choreo/LeftSecondDip.traj b/src/main/deploy/choreo/LeftSecondDip.traj new file mode 100644 index 0000000..26fba0d --- /dev/null +++ b/src/main/deploy/choreo/LeftSecondDip.traj @@ -0,0 +1,263 @@ +{ + "name":"LeftSecondDip", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":5.611854076385498, "y":5.705188751220703, "heading":-1.5657181171509245, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.429483413696289, "y":6.459790229797363, "heading":-0.00631101017301983, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.390566825866699, "y":6.004539966583252, "heading":-0.8915152357196665, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.448474407196045, "y":4.726224899291992, "heading":-2.5646739792250837, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.469950199127197, "y":4.578090190887451, "heading":-3.1254652057435086, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.04764461517334, "y":5.082523822784424, "heading":-2.594308272602356, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.611854076385498, "y":5.705188274383545, "heading":-1.564467367692855, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true}, + {"from":0, "to":6, "data":{"type":"MaxAcceleration", "props":{"max":4.0}}, "enabled":true}, + {"from":0, "to":6, "data":{"type":"MaxAngularVelocity", "props":{"max":4.0}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"first", "to":3, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":4, "to":6, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"5.611854076385498 m", "val":5.611854076385498}, "y":{"exp":"5.705188751220703 m", "val":5.705188751220703}, "heading":{"exp":"-1.5657181171509245 rad", "val":-1.5657181171509245}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.429483413696289 m", "val":6.429483413696289}, "y":{"exp":"6.459790229797363 m", "val":6.459790229797363}, "heading":{"exp":"-6.31101017301983 mrad", "val":-0.00631101017301983}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.390566825866699 m", "val":7.390566825866699}, "y":{"exp":"6.004539966583252 m", "val":6.004539966583252}, "heading":{"exp":"-0.8915152357196665 rad", "val":-0.8915152357196665}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.448474407196045 m", "val":7.448474407196045}, "y":{"exp":"4.726224899291992 m", "val":4.726224899291992}, "heading":{"exp":"-2.5646739792250837 rad", "val":-2.5646739792250837}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.469950199127197 m", "val":6.469950199127197}, "y":{"exp":"4.578090190887451 m", "val":4.578090190887451}, "heading":{"exp":"-3.1254652057435086 rad", "val":-3.1254652057435086}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.04764461517334 m", "val":6.04764461517334}, "y":{"exp":"5.082523822784424 m", "val":5.082523822784424}, "heading":{"exp":"-2.594308272602356 rad", "val":-2.594308272602356}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.611854076385498 m", "val":5.611854076385498}, "y":{"exp":"5.705188274383545 m", "val":5.705188274383545}, "heading":{"exp":"-1.5644673676928553 rad", "val":-1.564467367692855}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true}, + {"from":0, "to":6, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"4 m / s ^ 2", "val":4.0}}}, "enabled":true}, + {"from":0, "to":6, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"4 rad / s", "val":4.0}}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"first", "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, + {"from":4, "to":6, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.59657,1.16412,2.0081,3.0028,3.47503,4.1062], + "samples":[ + {"t":0.0, "x":5.61185, "y":5.70519, "heading":-1.56572, "vx":0.52595, "vy":1.92494, "omega":2.95002, "ax":1.17916, "ay":-0.13416, "alpha":1.74809, "fx":[22.86517,14.47859,14.55218,22.98404], "fy":[2.58051,2.92095,-7.02678,-6.994]}, + {"t":0.01808, "x":5.62155, "y":5.73997, "heading":-1.51239, "vx":0.54727, "vy":1.92251, "omega":2.98162, "ax":3.40067, "ay":-1.02805, "alpha":1.33561, "fx":[56.86992,50.82813,51.11771,57.13685], "fy":[-12.73837,-12.09218,-20.48554,-19.96823]}, + {"t":0.03616, "x":5.632, "y":5.77455, "heading":-1.45849, "vx":0.60874, "vy":1.90393, "omega":3.00576, "ax":3.58426, "ay":-1.21496, "alpha":0.99479, "fx":[58.88043,54.59201,54.92988,59.20888], "fy":[-16.63096,-15.93041,-22.66225,-21.93011]}, + {"t":0.05423, "x":5.64359, "y":5.80877, "heading":-1.40415, "vx":0.67354, "vy":1.88196, "omega":3.02375, "ax":3.61745, "ay":-1.36744, "alpha":0.72467, "fx":[58.75597,55.76057,56.09077,59.11127], "fy":[-19.82368,-19.0905,-24.33997,-23.58261]}, + {"t":0.07231, "x":5.65636, "y":5.84257, "heading":-1.34949, "vx":0.73894, "vy":1.85724, "omega":3.03685, "ax":3.60561, "ay":-1.51013, "alpha":0.49961, "fx":[58.07783,56.10848,56.37529,58.40502], "fy":[-22.74568,-21.9972,-25.91085,-25.2438]}, + {"t":0.09039, "x":5.67031, "y":5.8759, "heading":-1.29459, "vx":0.80412, "vy":1.82994, "omega":3.04588, "ax":3.57184, "ay":-1.64744, "alpha":0.31002, "fx":[57.16584,56.03712,56.19844,57.42114], "fy":[-25.51132,-24.68877,-27.45352,-26.96358]}, + {"t":0.10847, "x":5.68543, "y":5.90871, "heading":-1.23953, "vx":0.86869, "vy":1.80016, "omega":3.05148, "ax":3.52465, "ay":-1.78093, "alpha":0.14639, "fx":[56.12248,55.70477,55.72857,56.27003], "fy":[-28.16725,-27.20504,-28.98147,-28.74071]}, + {"t":0.12654, "x":5.70171, "y":5.94096, "heading":-1.18436, "vx":0.93241, "vy":1.76796, "omega":3.05413, "ax":3.46787, "ay":-1.91119, "alpha":-0.00175, "fx":[54.98317,55.20012,55.05294,54.98344], "fy":[-30.74381,-29.58232,-30.48109,-30.55869]}, + {"t":0.14462, "x":5.71913, "y":5.97261, "heading":-1.12915, "vx":0.9951, "vy":1.73341, "omega":3.0541, "ax":3.40346, "ay":-2.0384, "alpha":-0.1404, "fx":[53.76304,54.58411,54.21723,53.56525], "fy":[-33.28523,-31.77045,-31.96065,-32.42823]}, + {"t":0.1627, "x":5.73768, "y":6.00361, "heading":-1.07394, "vx":1.05662, "vy":1.69656, "omega":3.05156, "ax":3.33257, "ay":-2.16262, "alpha":-0.28022, "fx":[52.47169,53.89934,53.25069,52.00646], "fy":[-35.79468,-33.83464,-33.38083,-34.32235]}, + {"t":0.18078, "x":5.75732, "y":6.03393, "heading":-1.01877, "vx":1.11687, "vy":1.65747, "omega":3.0465, "ax":3.25591, "ay":-2.2838, "alpha":-0.42903, "fx":[51.1205,53.17616,52.1682,50.29528], "fy":[-38.26564,-35.83882,-34.70519,-36.21842]}, + {"t":0.19886, "x":5.77805, "y":6.06352, "heading":-0.9637, "vx":1.17573, "vy":1.61618, "omega":3.03874, "ax":3.17397, "ay":-2.40187, "alpha":-0.59311, "fx":[49.71987,52.44885,50.97617,48.41121], "fy":[-40.71354,-37.77881,-35.91449,-38.11925]}, + {"t":0.21693, "x":5.79982, "y":6.09235, "heading":-0.90877, "vx":1.23311, "vy":1.57276, "omega":3.02802, "ax":3.08707, "ay":-2.51674, "alpha":-0.77686, "fx":[48.28279,51.75395,49.67013,46.33117], "fy":[-43.17335,-39.5663,-37.02166,-40.05913]}, + {"t":0.23501, "x":5.82261, "y":6.12037, "heading":-0.85403, "vx":1.28891, "vy":1.52727, "omega":3.01397, "ax":2.99548, "ay":-2.62831, "alpha":-0.9678, "fx":[46.90877,51.10336,48.21107,43.9985], "fy":[-45.47387,-41.16596,-38.11651,-42.14912]}, + {"t":0.25309, "x":5.8464, "y":6.14755, "heading":-0.79954, "vx":1.34307, "vy":1.47975, "omega":2.99648, "ax":2.89933, "ay":-2.73654, "alpha":-1.21212, "fx":[45.4668,50.56637,46.63663,41.44637], "fy":[-47.9727,-42.6083,-38.99112,-44.20627]}, + {"t":0.27117, "x":5.87116, "y":6.17385, "heading":-0.74537, "vx":1.39548, "vy":1.43028, "omega":2.97457, "ax":2.79849, "ay":-2.84162, "alpha":-1.48803, "fx":[44.08118,50.16427,44.85348,38.61329], "fy":[-50.42435,-43.8759,-39.82729,-46.32334]}, + {"t":0.28924, "x":5.89684, "y":6.19924, "heading":-0.6916, "vx":1.44607, "vy":1.37891, "omega":2.94767, "ax":2.69152, "ay":-2.94477, "alpha":-1.81171, "fx":[42.69984,49.84401,42.85405,35.5217], "fy":[-52.99598,-45.12072,-40.50875,-48.37633]}, + {"t":0.30732, "x":5.92342, "y":6.22369, "heading":-0.63831, "vx":1.49473, "vy":1.32568, "omega":2.91491, "ax":2.57274, "ay":-3.05044, "alpha":-2.19566, "fx":[41.33558,49.5984,40.50988,31.93296], "fy":[-55.59426,-46.34485,-41.03246,-50.74029]}, + {"t":0.3254, "x":5.95087, "y":6.24715, "heading":-0.58561, "vx":1.54124, "vy":1.27053, "omega":2.87522, "ax":2.42806, "ay":-3.16792, "alpha":-2.64632, "fx":[39.77662,49.25585,37.50208,27.65468], "fy":[-58.52664,-47.33568,-41.75898,-53.55113]}, + {"t":0.34348, "x":5.97912, "y":6.26961, "heading":-0.53364, "vx":1.58513, "vy":1.21326, "omega":2.82738, "ax":2.24599, "ay":-3.30044, "alpha":-3.17366, "fx":[37.88588,48.5919,33.6356,22.51336], "fy":[-61.73746,-48.65068,-42.44087,-56.75863]}, + {"t":0.36156, "x":6.00815, "y":6.291, "heading":-0.48252, "vx":1.62573, "vy":1.1536, "omega":2.77001, "ax":2.04141, "ay":-3.43153, "alpha":-3.76925, "fx":[35.95709,47.92191,28.96966,16.78659], "fy":[-64.96536,-49.71924,-43.1505,-60.07729]}, + {"t":0.37963, "x":6.03787, "y":6.31129, "heading":-0.43245, "vx":1.66264, "vy":1.09156, "omega":2.70187, "ax":1.83823, "ay":-3.5452, "alpha":-4.4464, "fx":[34.36815,47.6097,23.85874,10.89626], "fy":[-68.00732,-50.27602,-43.52712,-63.32005]}, + {"t":0.39771, "x":6.06823, "y":6.33045, "heading":-0.38361, "vx":1.69587, "vy":1.02747, "omega":2.62149, "ax":1.64929, "ay":-3.63752, "alpha":-5.1997, "fx":[33.36107,47.79059,18.45835,5.1247], "fy":[-70.76033,-50.38886,-43.45856,-66.38544]}, + {"t":0.41579, "x":6.09915, "y":6.34843, "heading":-0.33621, "vx":1.72568, "vy":0.96172, "omega":2.52749, "ax":1.47869, "ay":-3.71064, "alpha":-6.03217, "fx":[32.97354,48.52459,12.78894,-0.38612], "fy":[-73.31061,-49.894,-43.06459,-69.36764]}, + {"t":0.43387, "x":6.13059, "y":6.36521, "heading":-0.29052, "vx":1.75241, "vy":0.89464, "omega":2.41844, "ax":1.32652, "ay":-3.76816, "alpha":-6.93334, "fx":[33.16582,49.8267,6.80648,-5.56132], "fy":[-75.63634,-48.97924,-42.42886,-72.24454]}, + {"t":0.45194, "x":6.16249, "y":6.38076, "heading":-0.2468, "vx":1.77639, "vy":0.82652, "omega":2.2931, "ax":1.19136, "ay":-3.81342, "alpha":-7.89338, "fx":[33.90022,51.58916,0.49651,-10.33119], "fy":[-77.78302,-47.69556,-41.61919,-75.06565]}, + {"t":0.47002, "x":6.1948, "y":6.39508, "heading":-0.20535, "vx":1.79793, "vy":0.75758, "omega":2.15041, "ax":1.07132, "ay":-3.8492, "alpha":-8.90255, "fx":[35.09263,53.73245,-6.14106,-14.65186], "fy":[-79.79451,-46.02682,-40.7493,-77.86465]}, + {"t":0.4881, "x":6.22747, "y":6.40815, "heading":-0.16647, "vx":1.8173, "vy":0.68799, "omega":1.98947, "ax":0.96452, "ay":-3.87763, "alpha":-9.94064, "fx":[36.65787,56.17337,-13.07857,-18.50311], "fy":[-81.63218,-44.20358,-39.82427,-80.58081]}, + {"t":0.50618, "x":6.26048, "y":6.41995, "heading":-0.13051, "vx":1.83473, "vy":0.61789, "omega":1.80977, "ax":0.8692, "ay":-3.90036, "alpha":-10.99902, "fx":[38.48139,58.8452,-20.24082,-21.88925], "fy":[-83.3512,-42.12798,-38.95277,-83.25231]}, + {"t":0.52425, "x":6.29379, "y":6.43048, "heading":-0.09779, "vx":1.85045, "vy":0.54738, "omega":1.61093, "ax":0.78383, "ay":-3.91864, "alpha":-12.0577, "fx":[40.46922,61.676,-27.52634,-24.84316], "fy":[-84.92928,-39.96357,-38.13553,-85.8165]}, + {"t":0.54233, "x":6.32737, "y":6.43974, "heading":-0.06867, "vx":1.86462, "vy":0.47654, "omega":1.39295, "ax":0.70711, "ay":-3.93341, "alpha":-13.10756, "fx":[42.4623,64.64716,-34.77917,-27.42644], "fy":[-86.4391,-37.52185,-37.53346,-88.28863]}, + {"t":0.56041, "x":6.3612, "y":6.44771, "heading":-0.04349, "vx":1.8774, "vy":0.40544, "omega":1.156, "ax":0.63791, "ay":-3.94541, "alpha":-14.12833, "fx":[44.44972,67.61528,-41.85996,-29.69577], "fy":[-87.85125,-35.22087,-36.85828,-90.61462]}, + {"t":0.57849, "x":6.39524, "y":6.4544, "heading":-0.02259, "vx":1.88893, "vy":0.33411, "omega":0.90059, "ax":0.57528, "ay":-3.95519, "alpha":-15.10838, "fx":[46.32191,70.57684,-48.62185,-31.7452], "fy":[-89.20581,-33.02543,-36.16892,-92.76629]}, + {"t":0.59657, "x":6.42948, "y":6.45979, "heading":-0.00631, "vx":1.89933, "vy":0.26261, "omega":0.62746, "ax":0.51667, "ay":-3.96054, "alpha":-15.06387, "fx":[45.83161,68.94235,-50.21591,-31.74771], "fy":[-89.01639,-32.77914,-36.81056,-92.90006]}, + {"t":0.61614, "x":6.46675, "y":6.46417, "heading":0.00597, "vx":1.90944, "vy":0.1851, "omega":0.33265, "ax":0.44802, "ay":-3.9717, "alpha":-14.16205, "fx":[43.43763,63.43984,-47.7052,-30.72183], "fy":[-87.52069,-34.36835,-39.16844,-91.1569]}, + {"t":0.63571, "x":6.50421, "y":6.46703, "heading":0.01248, "vx":1.91821, "vy":0.10737, "omega":0.05549, "ax":0.37074, "ay":-3.97947, "alpha":-13.29522, "fx":[40.7314,58.00834,-45.33688,-29.85956], "fy":[-86.27445,-36.29901,-40.70253,-89.43198]}, + {"t":0.65528, "x":6.54182, "y":6.46837, "heading":0.01357, "vx":1.92547, "vy":0.02949, "omega":-0.20471, "ax":0.28386, "ay":-3.9864, "alpha":-12.40863, "fx":[37.64721,52.49488,-42.9205,-29.19543], "fy":[-85.11674,-38.47578,-41.98835,-87.56743]}, + {"t":0.67485, "x":6.57956, "y":6.46819, "heading":0.00956, "vx":1.93102, "vy":-0.04853, "omega":-0.44755, "ax":0.18571, "ay":-3.99195, "alpha":-11.5163, "fx":[34.16182,46.94435,-40.56944,-28.74342], "fy":[-84.05243,-40.68683,-43.15912,-85.60215]}, + {"t":0.69442, "x":6.61738, "y":6.46647, "heading":0.0008, "vx":1.93466, "vy":-0.12665, "omega":-0.67293, "ax":0.07427, "ay":-3.99532, "alpha":-10.62198, "fx":[30.3228,41.32125,-38.4,-28.52737], "fy":[-83.00998,-43.01117,-44.16787,-83.52543]}, + {"t":0.71399, "x":6.65526, "y":6.46323, "heading":-0.01237, "vx":1.93611, "vy":-0.20484, "omega":-0.88081, "ax":-0.05286, "ay":-3.99537, "alpha":-9.73872, "fx":[26.15094,35.60453,-36.54071,-28.57152], "fy":[-81.96555,-45.39474,-44.98935,-81.36785]}, + {"t":0.73356, "x":6.69314, "y":6.45845, "heading":-0.02961, "vx":1.93508, "vy":-0.28303, "omega":-1.07141, "ax":-0.19851, "ay":-3.99045, "alpha":-8.87428, "fx":[21.56875,29.80213,-35.068,-28.90869], "fy":[-80.90016,-47.54715,-45.80926,-79.14866]}, + {"t":0.75313, "x":6.73097, "y":6.45215, "heading":-0.05058, "vx":1.93119, "vy":-0.36113, "omega":-1.24508, "ax":-0.36581, "ay":-3.97822, "alpha":-8.03996, "fx":[16.62248,23.84163,-34.10456,-29.58941], "fy":[-79.76315,-49.48278,-46.50407,-76.87868]}, + {"t":0.7727, "x":6.7687, "y":6.44432, "heading":-0.07494, "vx":1.92403, "vy":-0.43899, "omega":-1.40243, "ax":-0.55783, "ay":-3.95543, "alpha":-7.23497, "fx":[11.29041,17.66888,-33.72126,-30.66212], "fy":[-78.45934,-51.17916,-47.03277,-74.50996]}, + {"t":0.79227, "x":6.80625, "y":6.43497, "heading":-0.10239, "vx":1.91312, "vy":-0.5164, "omega":-1.54402, "ax":-0.77644, "ay":-3.91788, "alpha":-6.47137, "fx":[5.59127,11.25059,-33.97999,-32.1681], "fy":[-76.9449,-52.46641,-47.34314,-72.04216]}, + {"t":0.81184, "x":6.84354, "y":6.42412, "heading":-0.13261, "vx":1.89792, "vy":-0.59307, "omega":-1.67067, "ax":-1.01907, "ay":-3.86129, "alpha":-5.74406, "fx":[-0.41247,4.63861,-34.85508,-34.08518], "fy":[-75.07728,-53.43617,-47.31441,-69.3754]}, + {"t":0.83141, "x":6.88049, "y":6.41177, "heading":-0.1653, "vx":1.87798, "vy":-0.66864, "omega":-1.78309, "ax":-1.27111, "ay":-3.78513, "alpha":-5.06638, "fx":[-6.47786,-1.93838,-36.10863,-36.19432], "fy":[-72.91435,-53.77458,-47.06341,-66.6143]}, + {"t":0.85098, "x":6.917, "y":6.39796, "heading":-0.2002, "vx":1.8531, "vy":-0.74272, "omega":-1.88224, "ax":-1.4986, "ay":-3.70016, "alpha":-4.44014, "fx":[-12.0395,-7.91504,-37.22165,-37.98925], "fy":[-70.5978,-53.65096,-46.77487,-63.94757]}, + {"t":0.87056, "x":6.95298, "y":6.38272, "heading":-0.23704, "vx":1.82377, "vy":-0.81513, "omega":-1.96914, "ax":-1.679, "ay":-3.62093, "alpha":-3.85375, "fx":[-16.72649,-12.94239,-37.81426,-39.13809], "fy":[-68.2803,-53.50619,-46.6161,-61.53728]}, + {"t":0.89013, "x":6.98835, "y":6.36607, "heading":-0.27557, "vx":1.79091, "vy":-0.886, "omega":-2.04456, "ax":-1.82996, "ay":-3.5459, "alpha":-3.31463, "fx":[-20.78921,-17.29948,-38.19568,-39.92352], "fy":[-66.00074,-53.23262,-46.56815,-59.37375]}, + {"t":0.9097, "x":7.02305, "y":6.34805, "heading":-0.31559, "vx":1.7551, "vy":-0.95539, "omega":-2.10943, "ax":-1.96962, "ay":-3.4689, "alpha":-2.80716, "fx":[-24.53125,-21.32605,-38.61237,-40.60727], "fy":[-63.60666,-52.92081,-46.47199,-57.2859]}, + {"t":0.92927, "x":7.05702, "y":6.32869, "heading":-0.35687, "vx":1.71655, "vy":-1.02328, "omega":-2.16437, "ax":-2.10406, "ay":-3.38737, "alpha":-2.34703, "fx":[-28.02104,-25.12051,-39.17907,-41.29323], "fy":[-61.20636,-52.17737,-46.38233,-55.34162]}, + {"t":0.94884, "x":7.09021, "y":6.30801, "heading":-0.39923, "vx":1.67537, "vy":-1.08957, "omega":-2.2103, "ax":-2.23445, "ay":-3.30072, "alpha":-1.91267, "fx":[-31.30859,-28.73788,-39.87132,-41.9762], "fy":[-58.67009,-51.31995,-46.19719,-53.41815]}, + {"t":0.96841, "x":7.12257, "y":6.28606, "heading":-0.44249, "vx":1.63164, "vy":-1.15417, "omega":-2.24773, "ax":-2.36081, "ay":-3.20892, "alpha":-1.5088, "fx":[-34.37698,-32.17384,-40.70249,-42.66486], "fy":[-56.03824,-50.31604,-45.90299,-51.51856]}, + {"t":0.98798, "x":7.15405, "y":6.26286, "heading":-0.48648, "vx":1.58544, "vy":-1.21697, "omega":-2.27726, "ax":-2.4829, "ay":-3.11203, "alpha":-1.12622, "fx":[-37.24648,-35.48553,-41.62406,-43.31547], "fy":[-53.35277,-48.9678,-45.59607,-49.70624]}, + {"t":1.00755, "x":7.1846, "y":6.23844, "heading":-0.53104, "vx":1.53685, "vy":-1.27788, "omega":-2.2993, "ax":-2.60041, "ay":-3.01011, "alpha":-0.76211, "fx":[-39.90635,-38.65858,-42.63829,-43.93057], "fy":[-50.58478,-47.42531,-45.21315,-47.92776]}, + {"t":1.02712, "x":7.21418, "y":6.21286, "heading":-0.57604, "vx":1.48596, "vy":-1.33679, "omega":-2.31421, "ax":-2.71294, "ay":-2.90323, "alpha":-0.40443, "fx":[-42.37457,-41.71415,-43.71424,-44.4766], "fy":[-47.66447,-45.82028,-44.73794,-46.14072]}, + {"t":1.04669, "x":7.24274, "y":6.18614, "heading":-0.62133, "vx":1.43286, "vy":-1.39361, "omega":-2.32213, "ax":-2.81996, "ay":-2.79139, "alpha":-0.05232, "fx":[-44.63655,-44.6596,-44.84182,-44.93794], "fy":[-44.63769,-44.02301,-44.21434,-44.38616]}, + {"t":1.06626, "x":7.27025, "y":6.15833, "heading":-0.66678, "vx":1.37767, "vy":-1.44823, "omega":-2.32315, "ax":-2.92078, "ay":-2.67455, "alpha":0.30354, "fx":[-46.69196,-47.51099,-45.99667,-45.27857], "fy":[-41.47672,-42.03562,-43.66281,-42.66672]}, + {"t":1.08583, "x":7.29665, "y":6.12948, "heading":-0.71224, "vx":1.32051, "vy":-1.50058, "omega":-2.31721, "ax":-3.0144, "ay":-2.55258, "alpha":0.66807, "fx":[-48.5217,-50.2658,-47.16372,-45.47205], "fy":[-38.17748,-39.83148,-43.09618,-40.99124]}, + {"t":1.1054, "x":7.32191, "y":6.09962, "heading":-0.75759, "vx":1.26152, "vy":-1.55053, "omega":-2.30414, "ax":-3.09934, "ay":-2.42519, "alpha":1.04852, "fx":[-50.10378,-52.91465,-48.31961,-45.47915], "fy":[-34.6933,-37.45785,-42.50989,-39.34568]}, + {"t":1.12497, "x":7.34601, "y":6.06881, "heading":-0.80269, "vx":1.20086, "vy":-1.598, "omega":-2.28362, "ax":-3.17353, "ay":-2.29167, "alpha":1.45009, "fx":[-51.39759,-55.42736,-49.43954,-45.26395], "fy":[-30.99501,-34.88316,-41.91701,-37.7325]}, + {"t":1.14455, "x":7.3689, "y":6.0371, "heading":-0.84738, "vx":1.13875, "vy":-1.64285, "omega":-2.25524, "ax":-3.25601, "ay":-2.11749, "alpha":1.87763, "fx":[-52.6803,-58.07568,-50.85192,-45.15803], "fy":[-26.4902,-31.5449,-40.82269,-35.60882]}, + {"t":1.16412, "x":7.39057, "y":6.00454, "heading":-0.89152, "vx":1.07503, "vy":-1.68429, "omega":-2.21849, "ax":-3.15428, "ay":-1.96495, "alpha":1.04717, "fx":[-50.29085,-53.79336,-49.87198,-46.34973], "fy":[-27.47676,-29.77393,-35.1529,-32.3763]}, + {"t":1.18823, "x":7.41557, "y":5.96335, "heading":-0.94501, "vx":0.99897, "vy":-1.73167, "omega":-2.19324, "ax":-3.25324, "ay":-1.78161, "alpha":0.89129, "fx":[-51.68774,-54.71028,-51.62374,-48.56839], "fy":[-25.00886,-27.0367,-31.65638,-29.43537]}, + {"t":1.21234, "x":7.43872, "y":5.92108, "heading":-0.9979, "vx":0.92052, "vy":-1.77463, "omega":-2.17175, "ax":-2.75355, "ay":-1.36404, "alpha":0.80635, "fx":[-43.5136,-46.48625,-43.93015,-40.92866], "fy":[-18.63271,-20.67353,-24.62182,-22.69258]}, + {"t":1.23646, "x":7.46011, "y":5.87789, "heading":-1.05027, "vx":0.85412, "vy":-1.80752, "omega":-2.1523, "ax":-1.6198, "ay":-0.74434, "alpha":0.71472, "fx":[-25.33467,-28.21028,-26.10589,-23.21127], "fy":[-8.9568,-11.35765,-14.29381,-12.65934]}, + {"t":1.26057, "x":7.48024, "y":5.83409, "heading":-1.10217, "vx":0.81506, "vy":-1.82547, "omega":-2.13507, "ax":-0.83766, "ay":-0.36856, "alpha":0.62247, "fx":[-12.80643,-15.49486,-13.7957,-11.09676], "fy":[-3.21847,-5.72094,-7.9004,-6.56462]}, + {"t":1.28469, "x":7.49965, "y":5.78996, "heading":-1.15365, "vx":0.79486, "vy":-1.83436, "omega":-2.12006, "ax":-0.46998, "ay":-0.20195, "alpha":0.54026, "fx":[-6.90511,-9.36783,-8.02033,-5.55167], "fy":[-0.85504,-3.18308,-4.92468,-3.86131]}, + {"t":1.3088, "x":7.51868, "y":5.74567, "heading":-1.20478, "vx":0.78353, "vy":-1.83923, "omega":-2.10703, "ax":-0.46662, "ay":-0.19712, "alpha":0.47051, "fx":[-6.82352,-9.04416,-7.9952,-5.76907], "fy":[-1.08711,-3.05322,-4.60037,-3.77709]}, + {"t":1.33291, "x":7.53744, "y":5.70126, "heading":-1.25559, "vx":0.77228, "vy":-1.84398, "omega":-2.09569, "ax":-0.83886, "ay":-0.34599, "alpha":0.41819, "fx":[-12.7302,-14.70966,-13.90727,-11.92315], "fy":[-3.79672,-5.16051,-6.81622,-6.19806]}, + {"t":1.35703, "x":7.55582, "y":5.65669, "heading":-1.30612, "vx":0.75205, "vy":-1.85233, "omega":-2.0856, "ax":-1.69523, "ay":-0.66654, "alpha":0.38663, "fx":[-26.34976,-28.08221,-27.4792,-25.74091], "fy":[-9.22643,-9.81927,-11.85925,-11.42242]}, + {"t":1.38114, "x":7.57346, "y":5.61183, "heading":-1.35641, "vx":0.71117, "vy":-1.8684, "omega":-2.07628, "ax":-3.02196, "ay":-1.0834, "alpha":0.36718, "fx":[-47.46899,-48.9393,-48.48027,-47.01486], "fy":[-16.18571,-15.92177,-18.49788,-18.19348]}, + {"t":1.40526, "x":7.58973, "y":5.56646, "heading":-1.40648, "vx":0.6383, "vy":-1.89452, "omega":-2.06742, "ax":-3.55895, "ay":-1.1104, "alpha":0.32873, "fx":[-56.05101,-57.29269,-56.94143,-55.71841], "fy":[-16.82649,-16.27547,-18.79274,-18.61916]}, + {"t":1.42937, "x":7.60409, "y":5.52045, "heading":-1.45633, "vx":0.55248, "vy":-1.9213, "omega":-2.0595, "ax":-3.73526, "ay":-0.97998, "alpha":0.29528, "fx":[-58.87824,-59.96599,-59.71153,-58.64391], "fy":[-14.85609,-14.29674,-16.58188,-16.49685]}, + {"t":1.45348, "x":7.61632, "y":5.47384, "heading":-1.506, "vx":0.46241, "vy":-1.94493, "omega":-2.05238, "ax":-3.83187, "ay":-0.81517, "alpha":0.26749, "fx":[-60.42491,-61.40215,-61.23468,-60.27301], "fy":[-12.28468,-11.81966,-13.84046,-13.82113]}, + {"t":1.4776, "x":7.62636, "y":5.4267, "heading":-1.55549, "vx":0.37001, "vy":-1.96459, "omega":-2.04593, "ax":-3.89396, "ay":-0.6366, "alpha":0.23791, "fx":[-61.40214,-62.30985,-62.22324,-61.34289], "fy":[-9.58928,-9.07461,-10.94297,-10.81883]}, + {"t":1.50171, "x":7.63415, "y":5.37914, "heading":-1.60482, "vx":0.27611, "vy":-1.97994, "omega":-2.04019, "ax":-3.93957, "ay":-0.41692, "alpha":0.24682, "fx":[-62.10499,-62.95718,-62.97886,-62.13297], "fy":[-6.10724,-5.4663,-7.43278,-7.46909]}, + {"t":1.52583, "x":7.63966, "y":5.33128, "heading":-1.65402, "vx":0.18111, "vy":-1.98999, "omega":-2.03424, "ax":-3.81894, "ay":1.09693, "alpha":0.22629, "fx":[-60.09054,-60.95448,-61.16492,-60.30385], "fy":[18.05646,18.21848,16.68092,16.70224]}, + {"t":1.54994, "x":7.64292, "y":5.28361, "heading":-1.70307, "vx":0.08902, "vy":-1.96354, "omega":-2.02878, "ax":-3.1679, "ay":2.41471, "alpha":0.21935, "fx":[-49.60214,-50.62993,-50.95458,-49.9844], "fy":[39.06644,38.86428,37.68084,37.72929]}, + {"t":1.57405, "x":7.64415, "y":5.23696, "heading":-1.752, "vx":0.01263, "vy":-1.90531, "omega":-2.02349, "ax":-2.73428, "ay":2.90359, "alpha":0.2399, "fx":[-42.56797,-43.77062,-44.20695,-43.08934], "fy":[46.85143,46.64587,45.43224,45.45716]}, + {"t":1.59817, "x":7.64365, "y":5.19186, "heading":-1.80079, "vx":-0.0533, "vy":-1.8353, "omega":-2.01771, "ax":-2.47356, "ay":3.13228, "alpha":0.26888, "fx":[-38.25268,-39.65786,-40.23446,-38.93355], "fy":[50.54845,50.30171,49.04264,49.01598]}, + {"t":1.62228, "x":7.64165, "y":5.14852, "heading":-1.84945, "vx":-0.11295, "vy":-1.75976, "omega":-2.01122, "ax":-2.30517, "ay":3.26034, "alpha":0.32042, "fx":[-35.36638,-36.99374,-37.77392,-36.25076], "fy":[52.62789,52.5382,50.98981,50.8855]}, + {"t":1.64639, "x":7.63826, "y":5.10703, "heading":-1.89794, "vx":-0.16854, "vy":-1.68114, "omega":-2.0035, "ax":-2.18868, "ay":3.34106, "alpha":0.38985, "fx":[-33.25684,-35.13448,-36.19335,-34.40317], "fy":[53.95417,54.12533,52.14973,51.93813]}, + {"t":1.67051, "x":7.63356, "y":5.06746, "heading":-1.94626, "vx":-0.22132, "vy":-1.60058, "omega":-1.9941, "ax":-2.1037, "ay":3.39621, "alpha":0.45856, "fx":[-31.58161,-33.78695,-35.16003,-33.06264], "fy":[54.95099,55.1668,52.95392,52.59755]}, + {"t":1.69462, "x":7.62761, "y":5.02986, "heading":-1.99434, "vx":-0.27205, "vy":-1.51868, "omega":-1.98304, "ax":-2.0391, "ay":3.43612, "alpha":0.54639, "fx":[-30.17047,-32.73109,-34.51663,-32.07071], "fy":[55.69617,56.07176,53.49127,52.94466]}, + {"t":1.71874, "x":7.62045, "y":4.99423, "heading":-2.04216, "vx":-0.32122, "vy":-1.43582, "omega":-1.96986, "ax":-1.9884, "ay":3.46628, "alpha":0.65382, "fx":[-28.91547,-31.85419,-34.1658,-31.33395], "fy":[56.27745,56.91827,53.8578,53.06517]}, + {"t":1.74285, "x":7.61213, "y":4.96062, "heading":-2.08966, "vx":-0.36916, "vy":-1.35224, "omega":-1.9541, "ax":-1.94759, "ay":3.48982, "alpha":0.77841, "fx":[-27.73787,-31.11071,-34.04037,-30.78847], "fy":[56.75637,57.70885,54.12608,53.0228]}, + {"t":1.76696, "x":7.60266, "y":4.92902, "heading":-2.13678, "vx":-0.41613, "vy":-1.26808, "omega":-1.93533, "ax":-1.91403, "ay":3.50871, "alpha":0.92293, "fx":[-26.59735,-30.42816,-34.11338,-30.40785], "fy":[57.15154,58.4976,54.32345,52.84055]}, + {"t":1.79108, "x":7.59207, "y":4.89947, "heading":-2.18345, "vx":-0.46228, "vy":-1.18348, "omega":-1.91307, "ax":-1.88597, "ay":3.52417, "alpha":1.08022, "fx":[-25.43721,-29.83136,-34.33592,-30.16037], "fy":[57.51765,59.19839,54.51787,52.56139]}, + {"t":1.81519, "x":7.58038, "y":4.87195, "heading":-2.22958, "vx":-0.50776, "vy":-1.09849, "omega":-1.88702, "ax":-1.86216, "ay":3.53707, "alpha":1.2687, "fx":[-24.25676,-29.20228,-34.72686,-30.06701], "fy":[57.80479,60.01407,54.66852,52.12676]}, + {"t":1.83931, "x":7.56759, "y":4.84649, "heading":-2.27509, "vx":-0.55267, "vy":-1.0132, "omega":-1.85643, "ax":-1.84171, "ay":3.54798, "alpha":1.47995, "fx":[-23.02384,-28.55484,-35.26542,-30.10988], "fy":[58.04863,60.84302,54.82519,51.59024]}, + {"t":1.86342, "x":7.55373, "y":4.82309, "heading":-2.31985, "vx":-0.59708, "vy":-0.92765, "omega":-1.82074, "ax":-1.82395, "ay":3.55733, "alpha":1.72159, "fx":[-21.71801,-27.86415,-35.94504,-30.29909], "fy":[58.2365,61.7469,54.98966,50.92783]}, + {"t":1.88753, "x":7.5388, "y":4.80176, "heading":-2.36376, "vx":-0.64106, "vy":-0.84186, "omega":-1.77923, "ax":-1.80839, "ay":3.56543, "alpha":2.00858, "fx":[-20.33334,-27.05621,-36.78793,-30.66068], "fy":[58.33326,62.85185,55.13418,50.09598]}, + {"t":1.91165, "x":7.52282, "y":4.78249, "heading":-2.40666, "vx":-0.68467, "vy":-0.75589, "omega":-1.73079, "ax":-1.79465, "ay":3.57251, "alpha":2.31988, "fx":[-18.82614,-26.21948,-37.74426,-31.17539], "fy":[58.41001,63.92186,55.35893,49.17429]}, + {"t":1.93576, "x":7.50578, "y":4.7653, "heading":-2.4484, "vx":-0.72794, "vy":-0.66974, "omega":-1.67485, "ax":-1.78242, "ay":3.57876, "alpha":2.66946, "fx":[-17.19115,-25.29596,-38.83109,-31.87044], "fy":[58.42899,65.07574,55.63574,48.12125]}, + {"t":1.95988, "x":7.48771, "y":4.75019, "heading":-2.48878, "vx":-0.77092, "vy":-0.58344, "omega":-1.61048, "ax":-1.77146, "ay":3.58431, "alpha":3.06212, "fx":[-15.41471,-24.2623,-40.0507,-32.7655], "fy":[58.38228,66.33358,55.9714,46.92678]}, + {"t":1.98399, "x":7.46861, "y":4.73717, "heading":-2.52762, "vx":-0.81364, "vy":-0.49701, "omega":-1.53664, "ax":-1.7616, "ay":3.58927, "alpha":3.49908, "fx":[-13.48162,-23.11232,-41.39446,-33.87849], "fy":[58.27224,67.68123,56.38513,45.59044]}, + {"t":2.0081, "x":7.44847, "y":4.72622, "heading":-2.56467, "vx":-0.85612, "vy":-0.41046, "omega":-1.45226, "ax":-1.80665, "ay":3.56485, "alpha":3.26119, "fx":[-15.46673,-23.69522,-40.96261,-34.60271], "fy":[57.38341,66.69196,56.39289,45.91046]}, + {"t":2.03919, "x":7.42099, "y":4.71519, "heading":-2.60982, "vx":-0.91228, "vy":-0.29965, "omega":-1.35089, "ax":-2.14114, "ay":3.37289, "alpha":2.78613, "fx":[-23.11676,-29.23183,-44.10912,-39.51074], "fy":[53.89673,62.40972,53.64246,44.23941]}, + {"t":2.07027, "x":7.3916, "y":4.7075, "heading":-2.65181, "vx":-0.97883, "vy":-0.1948, "omega":-1.26429, "ax":-0.51865, "ay":3.14249, "alpha":2.66668, "fx":[2.39817,-3.96046,-18.36511,-13.00832], "fy":[48.89319,57.81988,51.39409,41.44993]}, + {"t":2.10136, "x":7.36092, "y":4.70297, "heading":-2.69111, "vx":-0.99496, "vy":-0.09712, "omega":-1.18139, "ax":0.07238, "ay":-0.66891, "alpha":2.92153, "fx":[11.54432,5.33536,-9.21944,-3.06385], "fy":[-13.43759,-0.66717,-7.84887,-20.52423]}, + {"t":2.13244, "x":7.33003, "y":4.69962, "heading":-2.72783, "vx":-0.99271, "vy":-0.11792, "omega":-1.09058, "ax":0.04767, "ay":-0.38183, "alpha":2.46529, "fx":[9.52815,4.65481,-8.0445,-3.11116], "fy":[-8.68612,1.94532,-3.26886,-14.23788]}, + {"t":2.16353, "x":7.29919, "y":4.69577, "heading":-2.76173, "vx":-0.99122, "vy":-0.12978, "omega":-1.01395, "ax":0.01856, "ay":-0.13935, "alpha":2.09769, "fx":[7.71817,3.86858,-7.15675,-3.25159], "fy":[-4.65061,4.37369,0.47126,-9.04361]}, + {"t":2.19461, "x":7.26839, "y":4.69167, "heading":-2.79325, "vx":-0.99065, "vy":-0.13412, "omega":-0.94874, "ax":0.00824, "ay":-0.06044, "alpha":1.80816, "fx":[6.45233,3.39051,-6.20882,-3.11067], "fy":[-3.25724,4.6271,1.55016,-6.758]}, + {"t":2.22569, "x":7.2376, "y":4.68747, "heading":-2.82274, "vx":-0.99039, "vy":-0.13599, "omega":-0.89254, "ax":0.00711, "ay":-0.05146, "alpha":1.57289, "fx":[5.52884,3.08199,-5.3137,-2.84568], "fy":[-2.98795,4.00841,1.49975,-5.78818]}, + {"t":2.25678, "x":7.20682, "y":4.68322, "heading":-2.85048, "vx":-0.99017, "vy":-0.13759, "omega":-0.84365, "ax":0.0079, "ay":-0.05647, "alpha":1.38053, "fx":[4.80404,2.83773,-4.55913,-2.58103], "fy":[-2.94621,3.31925,1.23736,-5.19671]}, + {"t":2.28786, "x":7.17604, "y":4.67892, "heading":-2.87671, "vx":-0.98992, "vy":-0.13935, "omega":-0.80073, "ax":0.00821, "ay":-0.05793, "alpha":1.22363, "fx":[4.21348,2.62289,-3.95611,-2.359], "fy":[-2.85456,2.79566,1.05516,-4.67488]}, + {"t":2.31895, "x":7.14528, "y":4.67456, "heading":-2.9016, "vx":-0.98967, "vy":-0.14115, "omega":-0.7627, "ax":0.00786, "ay":-0.05481, "alpha":1.09936, "fx":[3.73923,2.4399,-3.49138,-2.18831], "fy":[-2.70624,2.44201,0.97701,-4.19317]}, + {"t":2.35003, "x":7.11452, "y":4.67014, "heading":-2.92531, "vx":-0.98942, "vy":-0.14285, "omega":-0.72853, "ax":0.00726, "ay":-0.04998, "alpha":1.0039, "fx":[3.36933,2.29545,-3.14006,-2.06389], "fy":[-2.55234,2.20009,0.95937,-3.78129]}, + {"t":2.38112, "x":7.08377, "y":4.66568, "heading":-2.94795, "vx":-0.9892, "vy":-0.14441, "omega":-0.69732, "ax":0.0067, "ay":-0.04567, "alpha":0.93609, "fx":[3.09949,2.19845,-2.88752,-1.98493], "fy":[-2.43592,2.03265,0.97046,-3.46719]}, + {"t":2.4122, "x":7.05302, "y":4.66117, "heading":-2.96963, "vx":-0.98899, "vy":-0.14583, "omega":-0.66822, "ax":0.00632, "ay":-0.04269, "alpha":0.89363, "fx":[2.92009,2.15171,-2.71987,-1.95031], "fy":[-2.37408,1.91838,0.99819,-3.25366]}, + {"t":2.44328, "x":7.02228, "y":4.65662, "heading":-2.9904, "vx":-0.98879, "vy":-0.14715, "omega":-0.64044, "ax":0.00613, "ay":-0.04102, "alpha":0.87618, "fx":[2.82613,2.15927,-2.63193,-1.96405], "fy":[-2.37229,1.85627,1.04667,-3.13581]}, + {"t":2.47437, "x":6.99155, "y":4.65202, "heading":-3.01031, "vx":-0.9886, "vy":-0.14843, "omega":-0.61321, "ax":0.00608, "ay":-0.04033, "alpha":0.88296, "fx":[2.81128,2.22319,-2.61868,-2.02961], "fy":[-2.42933,1.84698,1.12419,-3.10285]}, + {"t":2.50545, "x":6.96082, "y":4.64739, "heading":-3.02937, "vx":-0.98841, "vy":-0.14968, "omega":-0.58576, "ax":0.00614, "ay":-0.04035, "alpha":0.91435, "fx":[2.8731,2.34773,-2.67878,-2.15237], "fy":[-2.54661,1.89333,1.23955,-3.14854]}, + {"t":2.53654, "x":6.9301, "y":4.64272, "heading":-3.04757, "vx":-0.98822, "vy":-0.15094, "omega":-0.55734, "ax":0.0063, "ay":-0.04105, "alpha":0.97124, "fx":[3.0115,2.53874,-2.81216,-2.33816], "fy":[-2.72959,1.99497,1.39913,-3.27162]}, + {"t":2.56762, "x":6.89939, "y":4.638, "heading":-3.0649, "vx":-0.98803, "vy":-0.15221, "omega":-0.52715, "ax":0.0066, "ay":-0.04266, "alpha":1.05473, "fx":[3.22796,2.80313,-3.01918,-2.59273], "fy":[-2.98746,2.14758,1.60614,-3.47511]}, + {"t":2.5987, "x":6.86868, "y":4.63325, "heading":-3.08129, "vx":-0.98782, "vy":-0.15354, "omega":-0.49437, "ax":0.0071, "ay":-0.04547, "alpha":1.16737, "fx":[3.52853,3.15153,-3.30426,-2.92488], "fy":[-3.33386,2.34874,1.8652,-3.76757]}, + {"t":2.62979, "x":6.83797, "y":4.62846, "heading":-3.09665, "vx":-0.9876, "vy":-0.15495, "omega":-0.45808, "ax":0.00782, "ay":-0.0496, "alpha":1.31118, "fx":[3.91761,3.59303,-3.67123,-3.34275], "fy":[-3.77719,2.59606,2.18394,-4.15231]}, + {"t":2.66087, "x":6.80728, "y":4.62362, "heading":-3.11089, "vx":-0.98736, "vy":-0.15649, "omega":-0.41732, "ax":0.00867, "ay":-0.0544, "alpha":1.49038, "fx":[4.40412,4.14043,-4.13233,-3.86162], "fy":[-4.31875,2.90451,2.58632,-4.62686]}, + {"t":2.69196, "x":6.77659, "y":4.61873, "heading":-3.12386, "vx":-0.98709, "vy":-0.15819, "omega":-0.37099, "ax":0.00936, "ay":-0.05808, "alpha":1.70809, "fx":[4.99143,4.80128,-4.7009,-4.49732], "fy":[-4.93739,3.30235,3.11363,-5.16671]}, + {"t":2.72304, "x":6.74591, "y":4.61378, "heading":-3.1354, "vx":-0.9868, "vy":-0.15999, "omega":-0.3179, "ax":0.00949, "ay":-0.05821, "alpha":1.97006, "fx":[5.68843,5.58809,-5.40021,-5.27352], "fy":[-5.60769,3.83556,3.82091,-5.74535]}, + {"t":2.75413, "x":6.71524, "y":4.60878, "heading":3.13791, "vx":-0.9865, "vy":-0.1618, "omega":-0.25666, "ax":0.0092, "ay":-0.05579, "alpha":2.28107, "fx":[6.50994,6.5194,-6.24336,-6.20175], "fy":[-6.35452,4.49642,4.70462,-6.38915]}, + {"t":2.78521, "x":6.68458, "y":4.60372, "heading":3.12993, "vx":-0.98622, "vy":-0.16353, "omega":-0.18576, "ax":0.01105, "ay":-0.06619, "alpha":2.64969, "fx":[7.51778,7.65886,-7.21419,-7.26095], "fy":[-7.44187,5.07517,5.53144,-7.36823]}, + {"t":2.81629, "x":6.65393, "y":4.59861, "heading":3.12416, "vx":-0.98587, "vy":-0.16559, "omega":-0.10339, "ax":0.02341, "ay":-0.13757, "alpha":3.08817, "fx":[8.87235,9.15976,-8.20692,-8.33834], "fy":[-9.6715,4.88726,5.54158,-9.49365]}, + {"t":2.84738, "x":6.6233, "y":4.59339, "heading":3.12094, "vx":-0.98515, "vy":-0.16987, "omega":-0.0074, "ax":0.06106, "ay":-0.343, "alpha":3.62059, "fx":[10.87045,11.27228,-9.02575,-9.23978], "fy":[-14.29492,2.94199,3.58917,-14.0177]}, + {"t":2.87846, "x":6.59271, "y":4.58795, "heading":3.12071, "vx":-0.98325, "vy":-0.18053, "omega":0.10514, "ax":0.09935, "ay":-0.51727, "alpha":4.25065, "fx":[13.22653,13.51753,-10.00997,-10.425], "fy":[-18.71552,1.94947,2.23454,-18.31666]}, + {"t":2.90955, "x":6.56219, "y":4.58209, "heading":3.12398, "vx":-0.98016, "vy":-0.19661, "omega":0.23727, "ax":-0.42336, "ay":2.74122, "alpha":4.01435, "fx":[5.72353,5.33207,-18.05802,-19.88201], "fy":[35.10283,52.07015,51.89149,35.01103]}, + {"t":2.94063, "x":6.53152, "y":4.5773, "heading":3.13136, "vx":-0.99332, "vy":-0.1114, "omega":0.36206, "ax":0.02952, "ay":3.99461, "alpha":3.99075, "fx":[14.46295,12.53553,-11.96037,-13.16328], "fy":[55.55991,70.46131,71.24302,56.40508]}, + {"t":2.97172, "x":6.50066, "y":4.57577, "heading":-3.14058, "vx":-0.9924, "vy":0.01277, "omega":0.4861, "ax":0.29204, "ay":3.98753, "alpha":4.5748, "fx":[21.03014,18.14661,-9.68858,-10.94265], "fy":[54.32936,71.39995,72.22713,55.26357]}, + {"t":3.0028, "x":6.46995, "y":4.57809, "heading":-3.12547, "vx":-0.98332, "vy":0.13672, "omega":0.62831, "ax":0.31423, "ay":3.98346, "alpha":3.05871, "fx":[15.81544,14.10143,-4.87342,-5.08883], "fy":[57.3507,68.82341,69.09493,57.69257]}, + {"t":3.02333, "x":6.44983, "y":4.58174, "heading":-3.11257, "vx":-0.97687, "vy":0.21851, "omega":0.69111, "ax":0.31695, "ay":3.98523, "alpha":2.79675, "fx":[14.99879,13.34364,-4.16344,-4.05169], "fy":[57.98704,68.3656,68.57105,58.14998]}, + {"t":3.04386, "x":6.42984, "y":4.58706, "heading":-3.09838, "vx":-0.97036, "vy":0.30033, "omega":0.74853, "ax":0.31978, "ay":3.98488, "alpha":2.59834, "fx":[14.39647,12.70862,-3.58536,-3.21274], "fy":[58.42723,68.08742,68.12434,58.41281]}, + {"t":3.06439, "x":6.40998, "y":4.59407, "heading":-3.08301, "vx":-0.9638, "vy":0.38214, "omega":0.80188, "ax":0.32293, "ay":3.9845, "alpha":2.42112, "fx":[13.87587,12.14662,-3.06282,-2.45261], "fy":[58.82806,67.83193,67.72112,58.64619]}, + {"t":3.08493, "x":6.39026, "y":4.60276, "heading":-3.06654, "vx":-0.95717, "vy":0.46395, "omega":0.85159, "ax":0.32646, "ay":3.98406, "alpha":2.26645, "fx":[13.43894,11.65042,-2.59204,-1.76621], "fy":[59.17438,67.62994,67.35204,58.8433]}, + {"t":3.10546, "x":6.37068, "y":4.61312, "heading":-3.04906, "vx":-0.95047, "vy":0.54575, "omega":0.89812, "ax":0.33044, "ay":3.98357, "alpha":2.1327, "fx":[13.0839,11.22047,-2.17228,-1.14836], "fy":[59.48092,67.45951,67.0246,59.00322]}, + {"t":3.12599, "x":6.35123, "y":4.62517, "heading":-3.03062, "vx":-0.94368, "vy":0.62754, "omega":0.94191, "ax":0.33496, "ay":3.983, "alpha":2.01097, "fx":[12.78317,10.87934,-1.79887,-0.59293], "fy":[59.79006,67.21972,66.7583,59.16416]}, + {"t":3.14652, "x":6.33193, "y":4.63889, "heading":-3.01128, "vx":-0.9368, "vy":0.70932, "omega":0.9832, "ax":0.34013, "ay":3.98234, "alpha":1.91603, "fx":[12.58397,10.58096,-1.47423,-0.09116], "fy":[60.01963,67.11055,66.50296,59.25742]}, + {"t":3.16705, "x":6.31277, "y":4.65429, "heading":-2.99109, "vx":-0.92982, "vy":0.79108, "omega":1.02254, "ax":0.34613, "ay":3.98158, "alpha":1.83697, "fx":[12.45837,10.35416,-1.19473,0.36216], "fy":[60.22724,66.99558,66.28865,59.3303]}, + {"t":3.18758, "x":6.29375, "y":4.67137, "heading":-2.9701, "vx":-0.92271, "vy":0.87283, "omega":1.06025, "ax":0.35314, "ay":3.98066, "alpha":1.78207, "fx":[12.42583,10.17457,-0.95465,0.77962], "fy":[60.36767,66.99845,66.07994,59.3378]}, + {"t":3.20812, "x":6.27488, "y":4.69013, "heading":-2.94833, "vx":-0.91546, "vy":0.95456, "omega":1.09684, "ax":0.36146, "ay":3.97957, "alpha":1.73747, "fx":[12.46528,10.08261,-0.75672,1.1625], "fy":[60.52192,66.92647,65.92478,59.34093]}, + {"t":3.22865, "x":6.25616, "y":4.71057, "heading":-2.92581, "vx":-0.90804, "vy":1.03627, "omega":1.13251, "ax":0.37149, "ay":3.97822, "alpha":1.71432, "fx":[12.60651,10.05116,-0.59367,1.52662], "fy":[60.62529,66.93112,65.78492,59.2871]}, + {"t":3.24918, "x":6.23759, "y":4.73268, "heading":-2.90256, "vx":-0.90042, "vy":1.11794, "omega":1.16771, "ax":0.38382, "ay":3.97652, "alpha":1.71981, "fx":[12.87089,10.07133,-0.45748,1.88895], "fy":[60.65158,67.1,65.63517,59.13402]}, + {"t":3.26971, "x":6.21919, "y":4.75648, "heading":-2.87858, "vx":-0.89254, "vy":1.19959, "omega":1.20302, "ax":0.39934, "ay":3.97433, "alpha":1.72963, "fx":[13.23627,10.21646,-0.34683,2.25366], "fy":[60.71642,67.1148,65.54765,59.00266]}, + {"t":3.29024, "x":6.20095, "y":4.78194, "heading":-2.85388, "vx":-0.88434, "vy":1.28119, "omega":1.23854, "ax":0.41949, "ay":3.97139, "alpha":1.76349, "fx":[13.77289,10.44194,-0.23733,2.66131], "fy":[60.73105,67.21346,65.4663,58.78398]}, + {"t":3.31077, "x":6.18288, "y":4.80909, "heading":-2.82845, "vx":-0.87572, "vy":1.36273, "omega":1.27474, "ax":0.44668, "ay":3.96725, "alpha":1.81774, "fx":[14.49366,10.82975,-0.10192,3.14409], "fy":[60.71215,67.34061,65.38386,58.49513]}, + {"t":3.33131, "x":6.16499, "y":4.8379, "heading":-2.80228, "vx":-0.86655, "vy":1.44418, "omega":1.31206, "ax":0.48543, "ay":3.961, "alpha":1.89085, "fx":[15.49995,11.42145,0.1222,3.78288], "fy":[60.66439,67.4305,65.31996,58.12036]}, + {"t":3.35184, "x":6.1473, "y":4.86839, "heading":-2.77534, "vx":-0.85659, "vy":1.52551, "omega":1.35089, "ax":0.54514, "ay":3.95059, "alpha":2.00237, "fx":[16.94232,12.37021,0.57184,4.73358], "fy":[60.50044,67.72418,65.09799,57.55171]}, + {"t":3.37237, "x":6.12983, "y":4.90054, "heading":-2.74761, "vx":-0.84539, "vy":1.60662, "omega":1.392, "ax":0.64903, "ay":3.93019, "alpha":2.13994, "fx":[19.23912,14.03705,1.58111,6.35801], "fy":[60.19571,67.7558,64.91392,56.71345]}, + {"t":3.3929, "x":6.11261, "y":4.93436, "heading":-2.71903, "vx":-0.83207, "vy":1.68731, "omega":1.43593, "ax":0.8743, "ay":3.87578, "alpha":2.36014, "fx":[23.68855,17.66785,4.29822,9.86571], "fy":[59.25491,67.58178,64.35656,54.93037]}, + {"t":3.41343, "x":6.09571, "y":4.96982, "heading":-2.68955, "vx":-0.81412, "vy":1.76689, "omega":1.48439, "ax":1.68772, "ay":3.55234, "alpha":2.75714, "fx":[37.8853,30.77586,15.87618,22.6379], "fy":[53.97405,63.75011,60.01241,47.84739]}, + {"t":3.43396, "x":6.07935, "y":5.00684, "heading":-2.65907, "vx":-0.77947, "vy":1.83982, "omega":1.541, "ax":1.66215, "ay":0.73209, "alpha":3.51487, "fx":[39.24946,31.08184,13.6755,21.54471], "fy":[8.85839,23.05418,15.14634,-0.56892]}, + {"t":3.45449, "x":6.0637, "y":5.04477, "heading":-2.62743, "vx":-0.74534, "vy":1.85485, "omega":1.61317, "ax":-3.55833, "ay":-1.56002, "alpha":3.2989, "fx":[-44.90237,-53.97331,-67.6643,-59.42415], "fy":[-28.90789,-12.14745,-21.46148,-36.54906]}, + {"t":3.47503, "x":6.04764, "y":5.08252, "heading":-2.59431, "vx":-0.8184, "vy":1.82282, "omega":1.6809, "ax":-3.49347, "ay":-1.61698, "alpha":3.81346, "fx":[-42.14459,-52.94607,-68.1816,-58.57318], "fy":[-29.8726,-10.65544,-22.5452,-39.60942]}, + {"t":3.4984, "x":6.02756, "y":5.12469, "heading":-2.55501, "vx":-0.90006, "vy":1.78502, "omega":1.77004, "ax":-3.42322, "ay":-1.82374, "alpha":3.12218, "fx":[-43.35408,-52.91668,-64.90035,-56.21369], "fy":[-32.20518,-16.51775,-26.66399,-40.42588]}, + {"t":3.52178, "x":6.00558, "y":5.16592, "heading":-2.51364, "vx":-0.98009, "vy":1.74239, "omega":1.84303, "ax":-3.25379, "ay":-1.92591, "alpha":2.69209, "fx":[-42.11882,-50.59427,-60.89541,-53.0165], "fy":[-32.7744,-19.84124,-29.20536,-40.4797]}, + {"t":3.54516, "x":5.98178, "y":5.20613, "heading":-2.47055, "vx":-1.05615, "vy":1.69737, "omega":1.90596, "ax":-2.90048, "ay":-1.88673, "alpha":2.2931, "fx":[-37.87434,-45.21145,-54.04513,-47.05824], "fy":[-31.21017,-20.86568,-29.35716,-38.38002]}, + {"t":3.56853, "x":5.9563, "y":5.24529, "heading":-2.426, "vx":-1.12395, "vy":1.65326, "omega":1.95957, "ax":-2.03876, "ay":-1.43043, "alpha":1.83435, "fx":[-25.63851,-31.78705,-39.01168,-33.03018], "fy":[-23.15763,-15.75778,-22.54183,-29.37906]}, + {"t":3.59191, "x":5.92947, "y":5.28355, "heading":-2.38019, "vx":-1.17161, "vy":1.61982, "omega":2.00245, "ax":-0.29557, "ay":-2.27272, "alpha":1.31421, "fx":[0.34238,-4.18173,-9.74119,-5.18912], "fy":[-35.97671,-31.35024,-36.42313,-40.57454]}, + {"t":3.61529, "x":5.902, "y":5.3208, "heading":-2.33338, "vx":-1.17852, "vy":1.5667, "omega":2.03317, "ax":2.33132, "ay":-3.20417, "alpha":0.75385, "fx":[39.83884,37.59434,34.09628,36.51639], "fy":[-50.39471,-48.1492,-51.5338,-53.39629]}, + {"t":3.63866, "x":5.87509, "y":5.35654, "heading":-2.28585, "vx":-1.12402, "vy":1.49179, "omega":2.05079, "ax":2.37358, "ay":-3.19649, "alpha":0.35456, "fx":[39.04323,37.92751,36.27348,37.48499], "fy":[-50.39707,-49.55537,-51.09004,-51.94417]}, + {"t":3.66204, "x":5.84946, "y":5.39054, "heading":-2.23791, "vx":-1.06854, "vy":1.41707, "omega":2.05908, "ax":2.38749, "ay":-3.19391, "alpha":-0.00641, "fx":[37.88318,37.93115,37.88416,37.91434], "fy":[-50.6966,-50.78283,-50.64347,-50.69982]}, + {"t":3.68542, "x":5.82513, "y":5.4228, "heading":-2.18977, "vx":-1.01273, "vy":1.34241, "omega":2.05893, "ax":2.39435, "ay":-3.19267, "alpha":-0.35244, "fx":[36.61715,37.93489,39.36896,38.12699], "fy":[-51.20681,-51.8439,-50.15326,-49.53964]}, + {"t":3.70879, "x":5.80211, "y":5.45331, "heading":-2.14164, "vx":-0.95675, "vy":1.26777, "omega":2.05069, "ax":2.39856, "ay":-3.19183, "alpha":-0.71075, "fx":[35.26993,38.05988,40.81785,38.16784], "fy":[-51.85023,-52.93601,-49.53781,-48.36657]}, + {"t":3.73217, "x":5.7804, "y":5.48207, "heading":-2.0937, "vx":-0.90068, "vy":1.19316, "omega":2.03408, "ax":2.4013, "ay":-3.19132, "alpha":-1.09825, "fx":[33.86644,38.24426,42.30629,38.07282], "fy":[-52.60244,-54.18087,-48.74991,-47.12474]}, + {"t":3.75555, "x":5.76, "y":5.50909, "heading":-2.04615, "vx":-0.84455, "vy":1.11855, "omega":2.0084, "ax":2.40334, "ay":-3.19089, "alpha":-1.48707, "fx":[32.36776,38.63046,43.81899,37.80188], "fy":[-53.59977,-55.18661,-47.90503,-45.93947]}, + {"t":3.77893, "x":5.74092, "y":5.53437, "heading":-1.9992, "vx":-0.78837, "vy":1.04396, "omega":1.97364, "ax":2.40482, "ay":-3.19061, "alpha":-1.92262, "fx":[30.8061,39.10725,45.41893,37.38057], "fy":[-54.7091,-56.42245,-46.83802,-44.64332]}, + {"t":3.8023, "x":5.72314, "y":5.5579, "heading":-1.95306, "vx":-0.73215, "vy":0.96937, "omega":1.9287, "ax":2.40592, "ay":-3.19042, "alpha":-2.38357, "fx":[29.16148,39.7458,47.10022,36.77539], "fy":[-56.04603,-57.57989,-45.63698,-43.33811]}, + {"t":3.82568, "x":5.70669, "y":5.57969, "heading":-1.90798, "vx":-0.67591, "vy":0.89479, "omega":1.87298, "ax":2.40686, "ay":-3.19023, "alpha":-2.8885, "fx":[27.4485,40.53139,48.88511,35.97741], "fy":[-57.5617,-58.83251,-44.23214,-41.96246]}, + {"t":3.84906, "x":5.69154, "y":5.59973, "heading":-1.86419, "vx":-0.61964, "vy":0.82021, "omega":1.80545, "ax":2.40759, "ay":-3.1901, "alpha":-3.43482, "fx":[25.6558,41.50578,50.77128,34.95601], "fy":[-59.3052,-60.08457,-42.64084,-40.54991]}, + {"t":3.87243, "x":5.67772, "y":5.61804, "heading":-1.82199, "vx":-0.56336, "vy":0.74564, "omega":1.72516, "ax":2.40824, "ay":-3.18995, "alpha":-4.02224, "fx":[23.78895,42.70166,52.75569,33.68421], "fy":[-61.28379,-61.29239,-40.86686,-39.12835]}, + {"t":3.89581, "x":5.6652, "y":5.6346, "heading":-1.78166, "vx":-0.50706, "vy":0.67107, "omega":1.63113, "ax":2.40874, "ay":-3.18987, "alpha":-4.66076, "fx":[21.8871,44.04447,54.86418,32.16642], "fy":[-63.4616,-62.58211,-38.87482,-37.64781]}, + {"t":3.91919, "x":5.65401, "y":5.64941, "heading":-1.74353, "vx":-0.45075, "vy":0.5965, "omega":1.52218, "ax":2.40919, "ay":-3.18979, "alpha":-5.34688, "fx":[19.96096,45.55595,57.09148,30.3821], "fy":[-65.84119,-63.89972,-36.68034,-36.13985]}, + {"t":3.94256, "x":5.64413, "y":5.66248, "heading":-1.70794, "vx":-0.39443, "vy":0.52193, "omega":1.39718, "ax":2.40959, "ay":-3.18971, "alpha":-6.07841, "fx":[18.03195,47.22138,59.4369,28.32577], "fy":[-68.40428,-65.23891,-34.29694,-34.6157]}, + {"t":3.96594, "x":5.63557, "y":5.67381, "heading":-1.67528, "vx":-0.33811, "vy":0.44737, "omega":1.25509, "ax":2.40992, "ay":-3.18965, "alpha":-6.85435, "fx":[16.13927,48.96502,61.90886,26.02364], "fy":[-71.11756,-66.65905,-31.72181,-33.05396]}, + {"t":3.98932, "x":5.62832, "y":5.6834, "heading":-1.64594, "vx":-0.28177, "vy":0.3728, "omega":1.09486, "ax":2.41021, "ay":-3.1896, "alpha":-7.66277, "fx":[14.26512,50.86478,64.4789,23.44662], "fy":[-73.95285,-68.03125,-29.03911,-31.52588]}, + {"t":4.01269, "x":5.62239, "y":5.69124, "heading":-1.62035, "vx":-0.22543, "vy":0.29824, "omega":0.91573, "ax":2.41047, "ay":-3.18955, "alpha":-8.49765, "fx":[12.44613,52.83521,67.15126,20.63963], "fy":[-76.86094,-69.39907,-26.27721,-30.00876]}, + {"t":4.03607, "x":5.61778, "y":5.69734, "heading":-1.59894, "vx":-0.16908, "vy":0.22368, "omega":0.71708, "ax":2.41072, "ay":-3.18951, "alpha":-9.35583, "fx":[10.67221,54.81756,69.92887,17.66886], "fy":[-79.79472,-70.82049,-23.47375,-28.45408]}, + {"t":4.05945, "x":5.61449, "y":5.7017, "heading":-1.58218, "vx":-0.11272, "vy":0.14912, "omega":0.49837, "ax":2.41091, "ay":-3.18948, "alpha":-10.22671, "fx":[8.94411,56.75661,72.80263,14.59666], "fy":[-82.7065,-72.2999,-20.6993,-26.83561]}, + {"t":4.08282, "x":5.61251, "y":5.70432, "heading":-1.57053, "vx":-0.05636, "vy":0.07456, "omega":0.2593, "ax":2.41108, "ay":-3.18946, "alpha":-11.09221, "fx":[7.25923,58.52129,75.73002,11.60014], "fy":[-85.58028,-73.83649,-18.04973,-25.07374]}, + {"t":4.1062, "x":5.61185, "y":5.70519, "heading":-1.56447, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[ + {"name":"IntakeDown", "from":{"target":0, "targetTimestamp":0.0, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeDown"}}}, + {"name":"IntakeUp", "from":{"target":5, "targetTimestamp":3.47503, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeUp"}}}] +} diff --git a/src/main/deploy/choreo/LeftSecondDipLong.traj b/src/main/deploy/choreo/LeftSecondDipLong.traj new file mode 100644 index 0000000..19455f5 --- /dev/null +++ b/src/main/deploy/choreo/LeftSecondDipLong.traj @@ -0,0 +1,223 @@ +{ + "name":"LeftSecondDipLong", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":5.631760597229004, "y":5.71936559677124, "heading":-1.5657181171509245, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.984349727630615, "y":4.598323345184326, "heading":-0.6132069164543249, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.522083282470703, "y":4.669139862060547, "heading":0.6682892074576462, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.491733074188232, "y":6.075356960296631, "heading":2.4373184253713447, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.469949722290039, "y":6.510373592376709, "heading":3.14, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.620150089263916, "y":5.711156845092773, "heading":-1.5649821399611368, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":7.0}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxAngularVelocity", "props":{"max":5.0}}, "enabled":true}, + {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":0, "to":1, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"5.631760597229004 m", "val":5.631760597229004}, "y":{"exp":"5.71936559677124 m", "val":5.71936559677124}, "heading":{"exp":"-1.5657181171509245 rad", "val":-1.5657181171509245}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.984349727630615 m", "val":5.984349727630615}, "y":{"exp":"4.598323345184326 m", "val":4.598323345184326}, "heading":{"exp":"-0.6132069164543249 rad", "val":-0.6132069164543249}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.522083282470703 m", "val":7.522083282470703}, "y":{"exp":"4.669139862060547 m", "val":4.669139862060547}, "heading":{"exp":"0.6682892074576462 rad", "val":0.6682892074576462}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.491733074188232 m", "val":7.491733074188232}, "y":{"exp":"6.075356960296631 m", "val":6.075356960296631}, "heading":{"exp":"2.4373184253713447 rad", "val":2.4373184253713447}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.469949722290039 m", "val":6.469949722290039}, "y":{"exp":"6.510373592376709 m", "val":6.510373592376709}, "heading":{"exp":"3.14 rad", "val":3.14}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.620150089263916 m", "val":5.620150089263916}, "y":{"exp":"5.711156845092773 m", "val":5.711156845092773}, "heading":{"exp":"-1.5649821399611368 rad", "val":-1.5649821399611368}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"7 m / s ^ 2", "val":7.0}}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"5 rad / s", "val":5.0}}}, "enabled":true}, + {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":0, "to":1, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.58831,1.52881,2.23931,2.86162,3.59205], + "samples":[ + {"t":0.0, "x":5.63176, "y":5.71937, "heading":-1.56572, "vx":0.59669, "vy":-1.90523, "omega":1.73711, "ax":-0.08594, "ay":-0.14894, "alpha":0.43328, "fx":[-0.24607,-2.48331,-2.48059,-0.24763], "fy":[-0.96414,-1.47846,-3.51873,-3.49658]}, + {"t":0.02263, "x":5.64524, "y":5.67622, "heading":-1.52641, "vx":0.59475, "vy":-1.9086, "omega":1.74691, "ax":0.00273, "ay":0.00084, "alpha":0.34226, "fx":[0.85849,-0.85728,-0.77127,0.9437], "fy":[1.09285,0.77642,-0.94601,-0.8701]}, + {"t":0.04525, "x":5.6587, "y":5.63303, "heading":-1.48688, "vx":0.59481, "vy":-1.90858, "omega":1.75465, "ax":0.00794, "ay":0.00248, "alpha":0.24642, "fx":[0.67158,-0.53401,-0.41943,0.78592], "fy":[0.80044,0.62544,-0.68459,-0.58383]}, + {"t":0.06788, "x":5.67216, "y":5.58984, "heading":-1.44718, "vx":0.59499, "vy":-1.90853, "omega":1.76023, "ax":-0.00586, "ay":-0.00183, "alpha":0.14966, "fx":[0.21564,-0.50253,-0.40168,0.31639], "fy":[0.43309,0.33476,-0.48643,-0.39734]}, + {"t":0.09051, "x":5.68562, "y":5.54666, "heading":-1.40735, "vx":0.59486, "vy":-1.90857, "omega":1.76362, "ax":-0.01551, "ay":-0.00483, "alpha":0.05432, "fx":[-0.14503,-0.39349,-0.34727,-0.09886], "fy":[0.08601,0.07061,-0.25273,-0.21065]}, + {"t":0.11314, "x":5.69908, "y":5.50347, "heading":-1.36744, "vx":0.59451, "vy":-1.90868, "omega":1.76485, "ax":-0.02188, "ay":-0.00681, "alpha":-0.04023, "fx":[-0.42708,-0.21888,-0.26771,-0.47592], "fy":[-0.25101,-0.16387,0.01109,-0.0288]}, + {"t":0.13576, "x":5.71252, "y":5.46028, "heading":-1.32751, "vx":0.59401, "vy":-1.90883, "omega":1.76393, "ax":-0.02697, "ay":-0.00839, "alpha":-0.13497, "fx":[-0.66398,-0.00774,-0.19245,-0.84869], "fy":[-0.59235,-0.37515,0.29601,0.13881]}, + {"t":0.15839, "x":5.72596, "y":5.41709, "heading":-1.2876, "vx":0.5934, "vy":-1.90902, "omega":1.76088, "ax":-0.03208, "ay":-0.00996, "alpha":-0.23101, "fx":[-0.87788,0.22204,-0.14077,-1.24069], "fy":[-0.94873,-0.56773,0.59736,0.2863]}, + {"t":0.18102, "x":5.73938, "y":5.37389, "heading":-1.24775, "vx":0.59267, "vy":-1.90925, "omega":1.75565, "ax":-0.038, "ay":-0.01179, "alpha":-0.32965, "fx":[-1.08223,0.46101,-0.12445,-1.66767], "fy":[-1.3277,-0.74555,0.91411,0.41064]}, + {"t":0.20365, "x":5.75278, "y":5.33068, "heading":-1.20803, "vx":0.59181, "vy":-1.90951, "omega":1.74819, "ax":-0.04519, "ay":-0.01399, "alpha":-0.43226, "fx":[-1.2845,0.70545,-0.15043,-2.14033], "fy":[-1.73474,-0.91231,1.24787,0.51063]}, + {"t":0.22627, "x":5.76616, "y":5.28747, "heading":-1.16847, "vx":0.59079, "vy":-1.90983, "omega":1.73841, "ax":-0.05384, "ay":-0.01664, "alpha":-0.54031, "fx":[-1.48751,0.95604,-0.2222,-2.6656], "fy":[-2.17391,-1.07089,1.60214,0.58616]}, + {"t":0.2489, "x":5.77951, "y":5.24425, "heading":-1.12913, "vx":0.58957, "vy":-1.91021, "omega":1.72619, "ax":-0.06393, "ay":-0.01971, "alpha":-0.6553, "fx":[-1.6899,1.2174,-0.34024,-3.24713], "fy":[-2.64826,-1.22295,1.98197,0.63789]}, + {"t":0.27153, "x":5.79284, "y":5.20103, "heading":-1.09008, "vx":0.58813, "vy":-1.91065, "omega":1.71136, "ax":-0.0752, "ay":-0.02311, "alpha":-0.77871, "fx":[-1.88609,1.498,-0.50208,-3.88521], "fy":[-3.16005,-1.368,2.39361,0.66683]}, + {"t":0.29416, "x":5.80612, "y":5.15779, "heading":-1.05135, "vx":0.58643, "vy":-1.91117, "omega":1.69374, "ax":-0.08721, "ay":-0.02671, "alpha":-0.91199, "fx":[-2.067,1.80941,-0.70308,-4.57733], "fy":[-3.71155,-1.50199,2.84369,0.67368]}, + {"t":0.31678, "x":5.81937, "y":5.11454, "heading":-1.01303, "vx":0.58445, "vy":-1.91178, "omega":1.6731, "ax":-0.09959, "ay":-0.03038, "alpha":-1.05652, "fx":[-2.22401,2.16201,-0.94045,-5.32195], "fy":[-4.30646,-1.61765,3.33757,0.65716]}, + {"t":0.33941, "x":5.83257, "y":5.07127, "heading":-0.97517, "vx":0.5822, "vy":-1.91247, "omega":1.6492, "ax":-0.11285, "ay":-0.03427, "alpha":-1.21366, "fx":[-2.36163,2.55212,-1.22596,-6.13073], "fy":[-4.95299,-1.71016,3.87593,0.61091]}, + {"t":0.36204, "x":5.84572, "y":5.02799, "heading":-0.93785, "vx":0.57964, "vy":-1.91324, "omega":1.62174, "ax":-0.12987, "ay":-0.03924, "alpha":-1.3848, "fx":[-2.5219,2.93739,-1.61016,-7.05253], "fy":[-5.66979,-1.78812,4.44875,0.51755]}, + {"t":0.38467, "x":5.8588, "y":4.98468, "heading":-0.90116, "vx":0.57671, "vy":-1.91413, "omega":1.5904, "ax":-0.15808, "ay":-0.04746, "alpha":-1.57141, "fx":[-2.8186,3.20364,-2.21568,-8.20767], "fy":[-6.49622,-1.88207,5.02551,0.33874]}, + {"t":0.40729, "x":5.87181, "y":4.94136, "heading":-0.86517, "vx":0.57313, "vy":-1.9152, "omega":1.55484, "ax":-0.21157, "ay":-0.06302, "alpha":-1.77532, "fx":[-3.47209,3.13345,-3.27082,-9.82566], "fy":[-7.50996,-2.03054,5.53988,-0.00141]}, + {"t":0.42992, "x":5.88472, "y":4.89801, "heading":-0.82999, "vx":0.56834, "vy":-1.91663, "omega":1.51467, "ax":-0.31183, "ay":-0.09185, "alpha":-2.0, "fx":[-4.82022,2.40027,-5.11998,-12.26235], "fy":[-8.82963,-2.27623,5.89077,-0.61739]}, + {"t":0.45255, "x":5.8975, "y":4.85462, "heading":-0.79571, "vx":0.56129, "vy":-1.91871, "omega":1.46942, "ax":-0.47524, "ay":-0.1376, "alpha":-2.25204, "fx":[-7.11678,0.7701,-8.02533,-15.80724], "fy":[-10.5299,-2.65961,6.02854,-1.57678]}, + {"t":0.47518, "x":5.91008, "y":4.81117, "heading":-0.76246, "vx":0.55053, "vy":-1.92182, "omega":1.41846, "ax":-0.63329, "ay":-0.17891, "alpha":-2.53775, "fx":[-9.25344,-0.65473,-10.89735,-19.41037], "fy":[-12.2805,-2.84199,6.30247,-2.54125]}, + {"t":0.4978, "x":5.92238, "y":4.76763, "heading":-0.73037, "vx":0.5362, "vy":-1.92587, "omega":1.36104, "ax":-0.26783, "ay":-0.07406, "alpha":-2.79879, "fx":[-2.90933,5.9822,-5.50377,-14.57705], "fy":[-11.69644,-0.6758,8.8425,-1.17331]}, + {"t":0.52043, "x":5.93444, "y":4.72404, "heading":-0.69957, "vx":0.53014, "vy":-1.92754, "omega":1.29771, "ax":5.75477, "ay":1.80183, "alpha":-1.27306, "fx":[93.11194,93.95896,89.67108,88.70255], "fy":[21.87167,27.5206,35.05566,29.97327]}, + {"t":0.54306, "x":5.94791, "y":4.68088, "heading":-0.67021, "vx":0.66036, "vy":-1.88677, "omega":1.2689, "ax":6.46576, "ay":2.6119, "alpha":-0.82644, "fx":[104.27803,104.05855,101.0399,101.21807], "fy":[36.87035,40.36522,45.89031,42.73724]}, + {"t":0.56569, "x":5.96451, "y":4.63886, "heading":-0.6415, "vx":0.80666, "vy":-1.82767, "omega":1.2502, "ax":6.21428, "ay":3.20204, "alpha":-0.8947, "fx":[100.76136,100.38351,96.59212,96.88812], "fy":[46.12131,49.54505,55.36651,52.30601]}, + {"t":0.58831, "x":5.98435, "y":4.59832, "heading":-0.61321, "vx":0.94728, "vy":-1.75522, "omega":1.22996, "ax":6.01109, "ay":3.56461, "alpha":-0.98119, "fx":[97.97374,97.47125,92.92873,93.3478], "fy":[51.62105,55.14513,61.3858,58.21146]}, + {"t":0.61865, "x":6.01586, "y":4.54671, "heading":-0.57589, "vx":1.12964, "vy":-1.64707, "omega":1.20019, "ax":5.57211, "ay":4.22501, "alpha":-0.87066, "fx":[91.03939,90.58101,85.92486,86.30008], "fy":[63.06682,65.69825,70.97176,68.56381]}, + {"t":0.64899, "x":6.05269, "y":4.49869, "heading":-0.53948, "vx":1.29869, "vy":-1.51889, "omega":1.17377, "ax":5.07424, "ay":4.80841, "alpha":-0.77025, "fx":[83.07619,82.69208,78.09858,78.36214], "fy":[73.13253,75.12702,79.44526,77.64324]}, + {"t":0.67933, "x":6.09443, "y":4.45482, "heading":-0.50387, "vx":1.45264, "vy":-1.37301, "omega":1.1504, "ax":4.52884, "ay":5.32076, "alpha":-0.67295, "fx":[74.24167,74.01559,69.61204,69.72527], "fy":[81.99327,83.44026,86.88421,85.56591]}, + {"t":0.70967, "x":6.14058, "y":4.41561, "heading":-0.46897, "vx":1.59004, "vy":-1.21159, "omega":1.12999, "ax":3.93458, "ay":5.767, "alpha":-0.57888, "fx":[64.54988,64.51575,60.41747,60.37432], "fy":[89.71097,90.72229,93.36736,92.42052]}, + {"t":0.74001, "x":6.19063, "y":4.38151, "heading":-0.43468, "vx":1.70941, "vy":-1.03662, "omega":1.11243, "ax":3.29621, "ay":6.14141, "alpha":-0.48716, "fx":[54.12413,54.22868,50.56941,50.39699], "fy":[96.1744,96.8916,98.80493,98.12629]}, + {"t":0.77035, "x":6.24401, "y":4.35288, "heading":-0.40093, "vx":1.80941, "vy":-0.8503, "omega":1.09765, "ax":2.61952, "ay":6.43053, "alpha":-0.40453, "fx":[43.06649,43.32084,40.11419,39.8457], "fy":[101.18277,101.67454,102.99541,102.50451]}, + {"t":0.80068, "x":6.30011, "y":4.33005, "heading":-0.36763, "vx":1.88889, "vy":-0.65521, "omega":1.08537, "ax":1.907, "ay":6.582, "alpha":-0.3358, "fx":[31.46731,31.83731,29.0638,28.73194], "fy":[103.89065,104.24826,105.10832,104.72876]}, + {"t":0.83102, "x":6.3583, "y":4.3132, "heading":-0.3347, "vx":1.94674, "vy":-0.45552, "omega":1.07519, "ax":1.10414, "ay":5.92747, "alpha":-0.35414, "fx":[18.61249,19.14673,16.42238,15.93427], "fy":[93.4744,93.88916,94.91163,94.13633]}, + {"t":0.86136, "x":6.41787, "y":4.30211, "heading":-0.30209, "vx":1.98024, "vy":-0.27569, "omega":1.06444, "ax":0.30103, "ay":2.51473, "alpha":-0.30242, "fx":[5.4311,5.79384,4.14254,3.74887], "fy":[38.95468,40.73663,40.57695,39.42416]}, + {"t":0.8917, "x":6.47808, "y":4.2949, "heading":-0.26979, "vx":1.98937, "vy":-0.19939, "omega":1.05527, "ax":0.06712, "ay":0.70814, "alpha":-0.01093, "fx":[1.0081,0.94925,1.12827,1.17654], "fy":[10.96803,12.32006,10.75941,10.92145]}, + {"t":0.92204, "x":6.53847, "y":4.28918, "heading":-0.23778, "vx":1.99141, "vy":-0.17791, "omega":1.05494, "ax":0.00558, "ay":0.06282, "alpha":0.34523, "fx":[-0.77865,-1.23357,0.95901,1.40773], "fy":[1.74475,1.60616,-0.59879,1.23724]}, + {"t":0.95238, "x":6.59889, "y":4.28381, "heading":-0.20577, "vx":1.99158, "vy":-0.176, "omega":1.06541, "ax":-0.00994, "ay":-0.11145, "alpha":0.72283, "fx":[-1.92336,-2.68536,1.61201,2.36529], "fy":[0.05705,-1.91356,-4.42257,-0.79809]}, + {"t":0.98272, "x":6.65931, "y":4.27842, "heading":-0.17345, "vx":1.99128, "vy":-0.17938, "omega":1.08734, "ax":-0.01216, "ay":-0.13347, "alpha":1.11332, "fx":[-2.95459,-3.91995,2.57629,3.52605], "fy":[0.76475,-3.1192,-5.80741,-0.31394]}, + {"t":1.01306, "x":6.71971, "y":4.27291, "heading":-0.14046, "vx":1.99091, "vy":-0.18343, "omega":1.12112, "ax":-0.00761, "ay":-0.08208, "alpha":1.51724, "fx":[-3.98841,-5.04803,3.76377,4.78919], "fy":[2.59605,-3.22421,-6.01253,1.42826]}, + {"t":1.04339, "x":6.78011, "y":4.26731, "heading":-0.10645, "vx":1.99068, "vy":-0.18592, "omega":1.16715, "ax":0.01428, "ay":0.15488, "alpha":1.93498, "fx":[-4.85993,-5.90254,5.35798,6.3114], "fy":[7.32107,-0.49434,-3.20836,6.21693]}, + {"t":1.07373, "x":6.84051, "y":4.26174, "heading":-0.07104, "vx":1.99111, "vy":-0.18122, "omega":1.22585, "ax":0.07924, "ay":0.94623, "alpha":2.34558, "fx":[-5.11217,-6.03275,7.75148,8.42544], "fy":[20.71545,10.84027,8.67943,19.85306]}, + {"t":1.10407, "x":6.90096, "y":4.25668, "heading":-0.03385, "vx":1.99351, "vy":-0.15252, "omega":1.29701, "ax":0.16247, "ay":3.05743, "alpha":2.58202, "fx":[-4.80623,-5.55115,10.3033,10.37123], "fy":[54.33252,43.22362,42.70677,53.89262]}, + {"t":1.13441, "x":6.96151, "y":4.25346, "heading":0.0055, "vx":1.99844, "vy":-0.05976, "omega":1.37535, "ax":-0.12156, "ay":6.42082, "alpha":1.63768, "fx":[-7.95613,-8.96977,4.84641,4.36018], "fy":[103.53313,99.8243,100.65859,103.72472]}, + {"t":1.16475, "x":7.02209, "y":4.2546, "heading":0.04723, "vx":1.99475, "vy":0.13504, "omega":1.42503, "ax":-0.82383, "ay":6.83278, "alpha":1.318, "fx":[-18.05913,-19.33656,-7.47186,-7.44805], "fy":[108.86326,106.86044,108.20853,109.96909]}, + {"t":1.19509, "x":7.08222, "y":4.26184, "heading":0.09046, "vx":1.96976, "vy":0.34234, "omega":1.46502, "ax":-1.55145, "ay":6.77547, "alpha":1.37023, "fx":[-29.54966,-31.16908,-19.13512,-18.66759], "fy":[107.56077,105.22457,107.75847,109.71848]}, + {"t":1.22543, "x":7.14127, "y":4.27535, "heading":0.13491, "vx":1.92269, "vy":0.5479, "omega":1.50659, "ax":-2.26575, "ay":6.5945, "alpha":1.46895, "fx":[-40.93159,-42.89328,-30.4882,-29.56891], "fy":[104.33141,101.43963,105.30798,107.69081]}, + {"t":1.25577, "x":7.19856, "y":4.295, "heading":0.18062, "vx":1.85395, "vy":0.74797, "omega":1.55116, "ax":-2.95732, "ay":6.32555, "alpha":1.60178, "fx":[-51.92502,-54.36381,-41.46283,-40.04697], "fy":[99.73325,96.03326,101.46386,104.4608]}, + {"t":1.2861, "x":7.25345, "y":4.32061, "heading":0.22768, "vx":1.76423, "vy":0.93987, "omega":1.59975, "ax":-3.70236, "ay":5.92658, "alpha":1.74811, "fx":[-63.56506,-66.61249,-53.53026,-51.40271], "fy":[93.14545,88.22878,95.5008,99.48033]}, + {"t":1.31644, "x":7.30527, "y":4.35185, "heading":0.27621, "vx":1.65191, "vy":1.11968, "omega":1.65279, "ax":-5.08361, "ay":4.79951, "alpha":1.86002, "fx":[-84.41184,-87.93948,-76.89264,-73.57993], "fy":[75.35519,67.79903,77.48518,84.14369]}, + {"t":1.34678, "x":7.35304, "y":4.38803, "heading":0.32636, "vx":1.49768, "vy":1.26529, "omega":1.70922, "ax":-5.85538, "ay":3.82406, "alpha":1.99027, "fx":[-96.15563,-99.36154,-89.90155,-86.41502], "fy":[60.23284,50.40916,61.50486,70.69185]}, + {"t":1.37712, "x":7.39579, "y":4.42817, "heading":0.37821, "vx":1.32003, "vy":1.38131, "omega":1.7696, "ax":-6.23316, "ay":3.17427, "alpha":2.15658, "fx":[-102.11059,-104.81136,-96.06383,-92.83822], "fy":[50.10905,38.51211,50.95027,62.00425]}, + {"t":1.40746, "x":7.43297, "y":4.47154, "heading":0.4319, "vx":1.13093, "vy":1.47761, "omega":1.83503, "ax":-6.44075, "ay":2.73095, "alpha":2.34167, "fx":[-105.59664,-107.72556,-99.25716,-96.42689], "fy":[43.00504,30.02872,43.96792,56.42171]}, + {"t":1.4378, "x":7.46431, "y":4.51763, "heading":0.48757, "vx":0.93552, "vy":1.56046, "omega":1.90607, "ax":-6.56662, "ay":2.41454, "alpha":2.54553, "fx":[-107.90622,-109.44907,-101.01055,-98.63364], "fy":[37.68856,23.55439,39.29318,52.79403]}, + {"t":1.46814, "x":7.48967, "y":4.56608, "heading":0.5454, "vx":0.7363, "vy":1.63372, "omega":1.9833, "ax":-6.64892, "ay":2.17923, "alpha":2.76745, "fx":[-109.59936,-110.5361,-101.98275,-100.1079], "fy":[33.44636,18.31883,36.19319,50.42894]}, + {"t":1.49847, "x":7.50895, "y":4.61665, "heading":0.60557, "vx":0.53458, "vy":1.69983, "omega":2.06726, "ax":-6.70594, "ay":1.9982, "alpha":3.009, "fx":[-110.94386,-111.24522,-102.48619,-101.17129], "fy":[29.85983,13.86873,34.25283,48.9102]}, + {"t":1.52881, "x":7.52208, "y":4.66914, "heading":0.66829, "vx":0.33113, "vy":1.76046, "omega":2.15855, "ax":-6.70465, "ay":1.98269, "alpha":2.20129, "fx":[-109.79672,-109.7956,-103.29955,-102.87311], "fy":[29.3458,18.54893,34.10813,43.90389]}, + {"t":1.55102, "x":7.52778, "y":4.70872, "heading":0.71622, "vx":0.18227, "vy":1.80448, "omega":2.20743, "ax":-6.63218, "ay":2.21737, "alpha":1.98042, "fx":[-108.56637,-108.48402,-102.10168,-102.01086], "fy":[32.47535,23.91418,38.44428,45.976]}, + {"t":1.57322, "x":7.53019, "y":4.74933, "heading":0.76523, "vx":0.03501, "vy":1.85371, "omega":2.2514, "ax":-6.19976, "ay":3.20643, "alpha":1.87109, "fx":[-102.45109,-102.38942,-94.17756,-94.68497], "fy":[47.05455,41.37364,55.50213,59.68753]}, + {"t":1.59542, "x":7.52944, "y":4.79128, "heading":0.81521, "vx":-0.10264, "vy":1.9249, "omega":2.29294, "ax":-0.21651, "ay":3.21044, "alpha":5.07726, "fx":[-23.70916,-6.04151,17.39767,-1.39582], "fy":[48.64733,33.74729,54.52643,66.95113]}, + {"t":1.61763, "x":7.52711, "y":4.83481, "heading":0.86612, "vx":-0.10745, "vy":1.99618, "omega":2.40567, "ax":0.5945, "ay":0.03105, "alpha":5.50452, "fx":[-11.28995,9.84068,29.96869,9.23301], "fy":[-2.93679,-18.87298,3.24249,20.53917]}, + {"t":1.63983, "x":7.52487, "y":4.87913, "heading":0.91954, "vx":-0.09425, "vy":1.99687, "omega":2.52789, "ax":0.50062, "ay":0.02223, "alpha":4.74771, "fx":[-9.6218,9.33057,25.27782,6.8045], "fy":[-3.51495,-16.4834,3.62843,17.78155]}, + {"t":1.66203, "x":7.5229, "y":4.92348, "heading":0.97566, "vx":-0.08313, "vy":1.99737, "omega":2.6333, "ax":0.36049, "ay":0.01428, "alpha":4.01563, "fx":[-8.89909,7.71196,20.18024,3.89877], "fy":[-3.87453,-13.94962,3.82094,14.90988]}, + {"t":1.68423, "x":7.52115, "y":4.96783, "heading":1.03413, "vx":-0.07513, "vy":1.99768, "omega":2.72246, "ax":0.26335, "ay":0.00952, "alpha":3.35271, "fx":[-7.79654,6.5406,16.06325,1.91601], "fy":[-3.96832,-11.57449,3.8559,12.29122]}, + {"t":1.70644, "x":7.51954, "y":5.01218, "heading":1.09458, "vx":-0.06928, "vy":1.9979, "omega":2.7969, "ax":0.20775, "ay":0.00696, "alpha":2.74731, "fx":[-6.29162,5.81752,12.83985,0.82726], "fy":[-3.8312,-9.37351,3.7313,9.9156]}, + {"t":1.72864, "x":7.51806, "y":5.05654, "heading":1.15668, "vx":-0.06467, "vy":1.99805, "omega":2.8579, "ax":0.17662, "ay":0.00554, "alpha":2.19017, "fx":[-4.62611,5.28502,10.21259,0.34471], "fy":[-3.49287,-7.35802,3.44377,7.7591]}, + {"t":1.75084, "x":7.51666, "y":5.10091, "heading":1.22013, "vx":-0.06075, "vy":1.99817, "omega":2.90653, "ax":0.15715, "ay":0.00464, "alpha":1.67169, "fx":[-2.98591,4.74976,7.96724,0.24827], "fy":[-2.98459,-5.5058,2.98974,5.79532]}, + {"t":1.77305, "x":7.51535, "y":5.14528, "heading":1.28466, "vx":-0.05726, "vy":1.99828, "omega":2.94364, "ax":0.14288, "ay":0.00398, "alpha":1.18246, "fx":[-1.45912,4.1179,5.99302,0.42127], "fy":[-2.32866,-3.78776,2.36948,3.99973]}, + {"t":1.79525, "x":7.51412, "y":5.18964, "heading":1.35002, "vx":-0.05409, "vy":1.99836, "omega":2.9699, "ax":0.13132, "ay":0.00346, "alpha":0.71567, "fx":[-0.07045,3.35566,4.23942,0.81447], "fy":[-1.52665,-2.2083,1.59381,2.36078]}, + {"t":1.81745, "x":7.51295, "y":5.23401, "heading":1.41596, "vx":-0.05117, "vy":1.99844, "omega":2.98579, "ax":0.12205, "ay":0.00304, "alpha":0.26274, "fx":[1.18861,2.45781,2.68658,1.41747], "fy":[-0.57718,-0.75821,0.66507,0.86354]}, + {"t":1.83966, "x":7.51184, "y":5.27839, "heading":1.48226, "vx":-0.04846, "vy":1.99851, "omega":2.99162, "ax":0.11543, "ay":0.00273, "alpha":-0.18651, "fx":[2.33777,1.42641,1.32744,2.23884], "fy":[0.52207,0.58613,-0.42483,-0.51028]}, + {"t":1.86186, "x":7.5108, "y":5.32276, "heading":1.54868, "vx":-0.0459, "vy":1.99857, "omega":2.98748, "ax":0.11191, "ay":0.0025, "alpha":-0.6415, "fx":[3.3948,0.25926,0.15855,3.29423], "fy":[1.78269,1.83323,-1.68516,-1.77192]}, + {"t":1.88406, "x":7.5098, "y":5.36713, "heading":1.61501, "vx":-0.04341, "vy":1.99862, "omega":2.97324, "ax":0.11169, "ay":0.00236, "alpha":-1.11027, "fx":[4.36929,-1.05445,-0.82294,4.60077], "fy":[3.22779,2.96556,-3.12246,-2.92117]}, + {"t":1.90626, "x":7.50887, "y":5.41151, "heading":1.68102, "vx":-0.04093, "vy":1.99868, "omega":2.94858, "ax":0.1147, "ay":0.00228, "alpha":-1.60119, "fx":[5.26103,-2.53381,-1.61864,6.17518], "fy":[4.88337,3.96068,-4.74671,-3.95275]}, + {"t":1.92847, "x":7.50799, "y":5.45589, "heading":1.74649, "vx":-0.03839, "vy":1.99873, "omega":2.91303, "ax":0.12081, "ay":0.00224, "alpha":-2.12755, "fx":[6.06297,-4.20146,-2.22485,8.03522], "fy":[6.74933,4.87376,-6.59769,-4.88313]}, + {"t":1.95067, "x":7.50716, "y":5.50027, "heading":1.81117, "vx":-0.0357, "vy":1.99878, "omega":2.8658, "ax":0.12956, "ay":0.00222, "alpha":-2.69855, "fx":[6.75699,-6.0864,-2.63661,10.19338], "fy":[8.84249,5.70347,-8.69899,-5.70585]}, + {"t":1.97287, "x":7.5064, "y":5.54465, "heading":1.8748, "vx":-0.03283, "vy":1.99883, "omega":2.80588, "ax":0.14038, "ay":0.0022, "alpha":-3.32058, "fx":[7.31851,-8.21815,-2.84363,12.65804], "fy":[11.19163,6.40146,-11.0591,-6.39444]}, + {"t":1.99508, "x":7.50571, "y":5.58903, "heading":1.9371, "vx":-0.02971, "vy":1.99888, "omega":2.73215, "ax":0.15337, "ay":0.00215, "alpha":-4.00046, "fx":[7.72955,-10.61227,-2.81913,15.441], "fy":[13.81387,6.9375,-13.69033,-6.92446]}, + {"t":2.01728, "x":7.50509, "y":5.63341, "heading":1.99776, "vx":-0.02631, "vy":1.99892, "omega":2.64333, "ax":0.1705, "ay":0.00208, "alpha":-4.74433, "fx":[8.00182,-13.24777,-2.50117,18.57424], "fy":[16.71406,7.29686,-16.60488,-7.27369]}, + {"t":2.03948, "x":7.50455, "y":5.67779, "heading":2.05645, "vx":-0.02252, "vy":1.99897, "omega":2.53799, "ax":0.19811, "ay":0.00202, "alpha":-5.55625, "fx":[8.21917,-16.02919,-1.75797,22.14843], "fy":[19.89353,7.45368,-19.80299,-7.41619]}, + {"t":2.06168, "x":7.50409, "y":5.72217, "heading":2.1128, "vx":-0.01812, "vy":1.99901, "omega":2.41463, "ax":0.2503, "ay":0.00192, "alpha":-6.43944, "fx":[8.59596,-18.738,-0.33892,26.3758], "fy":[23.35254,7.3695,-23.27236,-7.3275]}, + {"t":2.08389, "x":7.50375, "y":5.76656, "heading":2.16641, "vx":-0.01256, "vy":1.99906, "omega":2.27165, "ax":0.35062, "ay":0.00152, "alpha":-7.39999, "fx":[9.50131,-21.01638,2.14419,31.63615], "fy":[27.07871,7.0367,-27.00828,-7.01029]}, + {"t":2.10609, "x":7.50356, "y":5.81094, "heading":2.21685, "vx":-0.00478, "vy":1.99909, "omega":2.10735, "ax":0.52024, "ay":-0.00025, "alpha":-8.44689, "fx":[11.24637,-22.5446,6.01525,38.31945], "fy":[31.05931,6.43029,-31.00929,-6.49643]}, + {"t":2.12829, "x":7.50358, "y":5.85533, "heading":2.26364, "vx":0.00677, "vy":1.99909, "omega":1.91981, "ax":0.71019, "ay":-0.0052, "alpha":-9.58082, "fx":[12.90323,-23.96047,10.37812,45.77797], "fy":[35.28152,5.48645,-35.24184,-5.85622]}, + {"t":2.1505, "x":7.50391, "y":5.89971, "heading":2.30626, "vx":0.02254, "vy":1.99897, "omega":1.70708, "ax":0.4905, "ay":-0.00691, "alpha":-10.60746, "fx":[6.76161,-30.44306,8.00608,46.82354], "fy":[39.59178,4.20269,-38.89855,-5.33485]}, + {"t":2.1727, "x":7.50453, "y":5.94409, "heading":2.34417, "vx":0.03343, "vy":1.99882, "omega":1.47157, "ax":-6.76096, "ay":-0.19561, "alpha":-3.33073, "fx":[-107.56176,-112.01836,-106.3569,-103.40373], "fy":[17.39738,-0.37556,-22.59061,-6.85286]}, + {"t":2.1949, "x":7.50361, "y":5.98843, "heading":2.37684, "vx":-0.11668, "vy":1.99447, "omega":1.39761, "ax":-6.78768, "ay":-1.6453, "alpha":-3.21213, "fx":[-111.42001,-112.65632,-103.72451,-103.23697], "fy":[-6.92559,-22.15486,-43.89324,-31.50757]}, + {"t":2.21711, "x":7.49934, "y":6.0323, "heading":2.40787, "vx":-0.26739, "vy":1.95794, "omega":1.32629, "ax":-6.78462, "ay":-1.69867, "alpha":-3.47154, "fx":[-111.89538,-113.00549,-103.02918,-102.91347], "fy":[-6.15326,-23.19443,-46.32304,-32.20008]}, + {"t":2.23931, "x":7.49173, "y":6.07536, "heading":2.43732, "vx":-0.41803, "vy":1.92023, "omega":1.24922, "ax":-6.74803, "ay":-1.82861, "alpha":-3.46572, "fx":[-111.7514,-112.45014,-101.99864,-102.31928], "fy":[-8.30272,-25.59097,-48.36924,-33.85945]}, + {"t":2.2642, "x":7.47924, "y":6.12259, "heading":2.46841, "vx":-0.586, "vy":1.87471, "omega":1.16295, "ax":-6.65281, "ay":-2.16137, "alpha":-3.1448, "fx":[-110.69144,-110.62546,-100.21627,-100.93964], "fy":[-15.85321,-31.15395,-51.57199,-38.67395]}, + {"t":2.28909, "x":7.46259, "y":6.16859, "heading":2.49736, "vx":-0.75161, "vy":1.82091, "omega":1.08466, "ax":-6.49899, "ay":-2.58527, "alpha":-2.85748, "fx":[-108.73327,-108.00268,-97.47038,-98.49865], "fy":[-24.73289,-37.98826,-56.25664,-45.19414]}, + {"t":2.31399, "x":7.44187, "y":6.21311, "heading":2.52436, "vx":-0.91339, "vy":1.75655, "omega":1.01353, "ax":-6.25459, "ay":-3.12831, "alpha":-2.57599, "fx":[-105.31503,-104.07913,-93.31812,-94.47228], "fy":[-35.6252,-46.60581,-62.7017,-53.72438]}, + {"t":2.33888, "x":7.41719, "y":6.25587, "heading":2.54959, "vx":-1.06908, "vy":1.67868, "omega":0.94941, "ax":-5.87192, "ay":-3.79575, "alpha":-2.2975, "fx":[-99.57573,-98.13707,-87.0818,-88.08959], "fy":[-48.66969,-57.15426,-70.98987,-64.22751]}, + {"t":2.36377, "x":7.38876, "y":6.29648, "heading":2.57323, "vx":-1.21525, "vy":1.58419, "omega":0.89222, "ax":-5.397, "ay":-4.44229, "alpha":-2.01095, "fx":[-91.9667,-90.69647,-79.72147,-80.34095], "fy":[-61.35364,-67.55574,-79.0161,-74.17292]}, + {"t":2.38866, "x":7.35684, "y":6.33454, "heading":2.59544, "vx":-1.34959, "vy":1.47361, "omega":0.84216, "ax":-4.95401, "ay":-4.92785, "alpha":-1.71226, "fx":[-84.3914,-83.46964,-73.24935,-73.4836], "fy":[-71.16058,-75.62552,-84.81108,-81.33557]}, + {"t":2.41356, "x":7.32171, "y":6.36969, "heading":2.6164, "vx":-1.47291, "vy":1.35095, "omega":0.79954, "ax":-4.49847, "ay":-5.34201, "alpha":-1.41037, "fx":[-76.33709,-75.87809,-66.77517,-66.67597], "fy":[-79.60139,-82.62669,-89.71419,-87.29112]}, + {"t":2.43845, "x":7.28365, "y":6.40167, "heading":2.6363, "vx":-1.58489, "vy":1.21797, "omega":0.76443, "ax":-4.0118, "ay":-5.70937, "alpha":-1.10177, "fx":[-67.63113,-67.54754,-59.95784,-59.62439], "fy":[-87.03753,-88.98986,-94.07375,-92.46066]}, + {"t":2.46334, "x":7.24295, "y":6.43022, "heading":2.65533, "vx":-1.68476, "vy":1.07585, "omega":0.737, "ax":-3.49497, "ay":-6.02812, "alpha":-0.79024, "fx":[-58.32119,-58.54374,-52.75373,-52.32198], "fy":[-93.45865,-94.57708,-97.88073,-96.8868]}, + {"t":2.48823, "x":7.19993, "y":6.45513, "heading":2.67368, "vx":-1.77175, "vy":0.92579, "omega":0.71733, "ax":-2.95099, "ay":-6.29143, "alpha":-0.46928, "fx":[-48.49455,-48.86351,-45.20284,-44.83552], "fy":[-98.75897,-99.25836,-101.0092,-100.49764]}, + {"t":2.51313, "x":7.15491, "y":6.47623, "heading":2.69153, "vx":-1.84521, "vy":0.76918, "omega":0.70565, "ax":-2.38216, "ay":-6.48206, "alpha":-0.12248, "fx":[-38.194,-38.4462,-37.37071,-37.26329], "fy":[-102.69183,-102.73239,-103.15512,-103.05036]}, + {"t":2.53802, "x":7.10824, "y":6.49336, "heading":2.7091, "vx":-1.90451, "vy":0.60783, "omega":0.7026, "ax":-1.78523, "ay":-6.52854, "alpha":0.29295, "fx":[-27.29794,-27.04009,-29.30456,-29.72453], "fy":[-104.22003,-103.78353,-103.11057,-103.46702]}, + {"t":2.56291, "x":7.06028, "y":6.50647, "heading":2.72659, "vx":-1.94895, "vy":0.44531, "omega":0.7099, "ax":-1.0773, "ay":-5.63092, "alpha":1.27965, "fx":[-14.03759,-11.6657,-20.33226,-22.37619], "fy":[-92.58361,-88.46077,-86.50722,-90.02806]}, + {"t":2.58781, "x":7.01143, "y":6.51581, "heading":2.74426, "vx":-1.97577, "vy":0.30515, "omega":0.74175, "ax":-0.3917, "ay":-2.88001, "alpha":2.87754, "fx":[-0.77995,4.68143,-12.00211,-16.77358], "fy":[-54.15637,-43.37516,-36.88532,-48.47233]}, + {"t":2.6127, "x":6.96213, "y":6.52252, "heading":2.76272, "vx":-1.98552, "vy":0.23345, "omega":0.81338, "ax":-0.1298, "ay":-1.17895, "alpha":4.28847, "fx":[5.92103,13.7432,-10.52889,-17.37822], "fy":[-31.46958,-15.17132,-4.89888,-23.32696]}, + {"t":2.63759, "x":6.91267, "y":6.52796, "heading":2.78297, "vx":-1.98875, "vy":0.20411, "omega":0.92013, "ax":-0.04279, "ay":-0.42826, "alpha":5.61072, "fx":[9.97841,19.78869,-12.01612,-20.46849], "fy":[-23.38492,-1.65651,11.21848,-13.37258]}, + {"t":2.66248, "x":6.86315, "y":6.53291, "heading":2.80588, "vx":-1.98981, "vy":0.19345, "omega":1.0598, "ax":-0.03446, "ay":-0.36299, "alpha":6.86871, "fx":[12.89036,24.45716,-15.02056,-24.51523], "fy":[-25.72873,1.27755,15.84946,-14.44902]}, + {"t":2.68738, "x":6.8136, "y":6.53761, "heading":2.83226, "vx":-1.99067, "vy":0.18441, "omega":1.23078, "ax":-0.08268, "ay":-0.9547, "alpha":7.99717, "fx":[14.84913,28.0489,-19.24979,-28.89896], "fy":[-37.68666,-6.0931,9.14089,-25.98768]}, + {"t":2.71227, "x":6.76403, "y":6.54191, "heading":2.86289, "vx":-1.99273, "vy":0.16065, "omega":1.42985, "ax":-0.15808, "ay":-2.41649, "alpha":8.68756, "fx":[15.76266,30.51105,-24.02538,-32.28709], "fy":[-61.28354,-27.7117,-13.81589,-50.64337]}, + {"t":2.73716, "x":6.71437, "y":6.54516, "heading":2.89849, "vx":-1.99666, "vy":0.10049, "omega":1.6461, "ax":-0.09554, "ay":-4.98543, "alpha":7.73363, "fx":[16.13253,32.20715,-25.11582,-29.29112], "fy":[-95.226,-70.05253,-62.28441,-89.02665]}, + {"t":2.76205, "x":6.66464, "y":6.54612, "heading":2.93946, "vx":-1.99904, "vy":-0.02361, "omega":1.83861, "ax":0.35826, "ay":-6.67845, "alpha":4.81542, "fx":[18.62766,32.38959,-13.027,-15.23996], "fy":[-110.47805,-101.25456,-102.67121,-109.69713]}, + {"t":2.78695, "x":6.61499, "y":6.54346, "heading":2.98523, "vx":-1.99013, "vy":-0.18985, "omega":1.95848, "ax":0.94878, "ay":-6.82087, "alpha":4.52333, "fx":[27.25733,40.24027,-1.75979,-5.48756], "fy":[-111.28731,-101.97147,-106.80255,-113.08386]}, + {"t":2.81184, "x":6.56575, "y":6.53662, "heading":3.03398, "vx":-1.96651, "vy":-0.35964, "omega":2.07108, "ax":1.5452, "ay":-6.76765, "alpha":4.69404, "fx":[36.83401,50.41455,7.93755,2.93862], "fy":[-109.70908,-98.7082,-107.19998,-114.14853]}, + {"t":2.83673, "x":6.51727, "y":6.52557, "heading":3.08554, "vx":-1.92804, "vy":-0.52811, "omega":2.18793, "ax":2.16785, "ay":-6.61849, "alpha":4.96293, "fx":[46.82922,61.28514,18.01742,11.53288], "fy":[-106.72859,-93.32093,-106.04684,-114.19717]}, + {"t":2.86162, "x":6.46995, "y":6.51037, "heading":3.14, "vx":-1.87408, "vy":-0.69286, "omega":2.31147, "ax":2.64758, "ay":-6.40439, "alpha":4.80634, "fx":[54.01046,67.25493,26.25264,20.61077], "fy":[-102.50426,-88.39882,-103.68391,-112.11078]}, + {"t":2.88681, "x":6.42359, "y":6.49089, "heading":-3.08497, "vx":-1.8074, "vy":-0.85417, "omega":2.43253, "ax":3.23573, "ay":-6.1419, "alpha":4.19698, "fx":[61.39759,72.47604,38.903,32.70187], "fy":[-97.54841,-84.0525,-99.89043,-108.53731]}, + {"t":2.912, "x":6.37909, "y":6.46743, "heading":-3.0237, "vx":-1.7259, "vy":-1.00886, "omega":2.53824, "ax":3.7282, "ay":-5.78901, "alpha":3.86134, "fx":[68.02999,77.63562,48.43348,42.65229], "fy":[-91.47756,-77.86226,-94.67827,-103.60109]}, + {"t":2.93719, "x":6.3368, "y":6.44018, "heading":-2.95977, "vx":-1.63199, "vy":-1.15467, "omega":2.63549, "ax":4.08023, "ay":-5.28264, "alpha":3.85492, "fx":[73.34803,81.80831,54.45183,49.49843], "fy":[-83.33375,-68.45329,-86.801,-96.87533]}, + {"t":2.96237, "x":6.29699, "y":6.40942, "heading":-2.89339, "vx":-1.52922, "vy":-1.28773, "omega":2.73259, "ax":3.45079, "ay":-3.83776, "alpha":5.72526, "fx":[70.09657,73.00055,36.73244,39.30558], "fy":[-64.83434,-41.19572,-57.42585,-80.25326]}, + {"t":2.98756, "x":6.25957, "y":6.37577, "heading":-2.82456, "vx":-1.44231, "vy":-1.38439, "omega":2.87679, "ax":1.68461, "ay":-1.70311, "alpha":6.1398, "fx":[46.59439,40.89142,5.48472,14.00716], "fy":[-33.59931,-8.00535,-19.61874,-46.9293]}, + {"t":3.01275, "x":6.22378, "y":6.34036, "heading":-2.7521, "vx":-1.39988, "vy":-1.42728, "omega":3.03143, "ax":0.60539, "ay":-0.58755, "alpha":4.95942, "fx":[26.97966,18.94133,-8.29625,0.81926], "fy":[-14.09383,5.83842,-3.40937,-25.64638]}, + {"t":3.03793, "x":6.18871, "y":6.30423, "heading":-2.67575, "vx":-1.38463, "vy":-1.44208, "omega":3.15635, "ax":0.13667, "ay":-0.1309, "alpha":3.42574, "fx":[14.80003,7.45872,-10.60617,-2.97375], "fy":[-4.56691,8.41759,1.49447,-13.6576]}, + {"t":3.06312, "x":6.15388, "y":6.26786, "heading":-2.59625, "vx":-1.38119, "vy":-1.44538, "omega":3.24263, "ax":-0.01986, "ay":0.01901, "alpha":1.82483, "fx":[6.73083,1.94659,-7.38074,-2.55795], "fy":[-0.43591,5.76173,1.9028,-6.02171]}, + {"t":3.08831, "x":6.11908, "y":6.23147, "heading":-2.51458, "vx":-1.38169, "vy":-1.4449, "omega":3.28859, "ax":-0.05004, "ay":0.04791, "alpha":0.21096, "fx":[0.27721,-0.54733,-1.8693,-1.03833], "fy":[0.95,0.91043,1.13823,0.04352]}, + {"t":3.1135, "x":6.08427, "y":6.19509, "heading":-2.43175, "vx":-1.38295, "vy":-1.4437, "omega":3.29391, "ax":-0.0369, "ay":0.03537, "alpha":-1.43149, "fx":[-5.72788,-1.3602,4.53975,0.20535], "fy":[0.76217,-4.93226,0.6308,5.78548]}, + {"t":3.13868, "x":6.04942, "y":6.15874, "heading":-2.34878, "vx":-1.38388, "vy":-1.4428, "omega":3.25785, "ax":-0.01403, "ay":0.01346, "alpha":-3.13146, "fx":[-11.76439,-1.01649,11.28967,0.60031], "fy":[-0.55175,-11.28526,0.97943,11.7123]}, + {"t":3.16387, "x":6.01456, "y":6.1224, "heading":-2.26673, "vx":-1.38423, "vy":-1.44247, "omega":3.17898, "ax":0.0018, "ay":-0.00173, "alpha":-4.91261, "fx":[-18.06502,0.24814,18.09395,-0.16297], "fy":[-2.80398,-17.82684,2.47758,18.04367]}, + {"t":3.18906, "x":5.9797, "y":6.08607, "heading":-2.18666, "vx":-1.38419, "vy":-1.44251, "omega":3.05524, "ax":0.01129, "ay":-0.01083, "alpha":-6.79136, "fx":[-24.54002,2.47271,24.89397,-2.10961], "fy":[-6.02776,-24.51701,5.16032,24.69651]}, + {"t":3.21424, "x":5.94484, "y":6.04973, "heading":-2.10971, "vx":-1.3839, "vy":-1.44278, "omega":2.88419, "ax":0.02524, "ay":-0.0242, "alpha":-8.77164, "fx":[-30.89407,5.83231,31.72677,-5.06243], "fy":[-10.34644,-31.46598,8.88329,31.39237]}, + {"t":3.23943, "x":5.90999, "y":6.01339, "heading":-2.03706, "vx":-1.38327, "vy":-1.44339, "omega":2.66326, "ax":0.03173, "ay":-0.03041, "alpha":-10.82609, "fx":[-37.13838,10.07088,38.2198,-9.13729], "fy":[-15.45663,-38.31559,13.7789,38.06213]}, + {"t":3.26462, "x":5.87516, "y":5.97702, "heading":-1.96998, "vx":-1.38247, "vy":-1.44416, "omega":2.39058, "ax":-0.04378, "ay":0.04268, "alpha":-12.88321, "fx":[-44.47951,14.11205,42.80457,-15.21708], "fy":[-19.80956,-43.616,20.83823,45.29792]}, + {"t":3.28981, "x":5.84032, "y":5.94066, "heading":-1.90977, "vx":-1.38357, "vy":-1.44308, "omega":2.06609, "ax":1.6905, "ay":1.80588, "alpha":-12.56388, "fx":[-17.33554,52.27471,63.28084,9.1314], "fy":[10.27235,-14.94162,47.29416,72.05393]}, + {"t":3.31499, "x":5.80601, "y":5.90489, "heading":-1.85773, "vx":-1.34099, "vy":-1.3976, "omega":1.74964, "ax":4.82548, "ay":5.02344, "alpha":-4.22489, "fx":[67.39041,94.62096,83.9974,60.42337], "fy":[83.03894,58.30637,80.42476,97.23328]}, + {"t":3.34018, "x":5.77377, "y":5.87128, "heading":-1.81366, "vx":-1.21945, "vy":-1.27107, "omega":1.64323, "ax":4.83514, "ay":5.03886, "alpha":-4.59435, "fx":[67.69316,96.35541,84.38797,58.60885], "fy":[82.40352,57.09925,81.36693,99.11257]}, + {"t":3.36537, "x":5.74459, "y":5.84086, "heading":-1.77227, "vx":-1.09767, "vy":-1.14416, "omega":1.52751, "ax":4.83861, "ay":5.04336, "alpha":-5.05915, "fx":[67.96369,98.4969,84.51304,56.2926], "fy":[81.70259,54.99425,82.44222,101.12895]}, + {"t":3.39055, "x":5.71847, "y":5.81364, "heading":-1.7338, "vx":-0.9758, "vy":-1.01713, "omega":1.40008, "ax":4.84037, "ay":5.04552, "alpha":-5.52816, "fx":[68.4178,100.61509,84.48858,53.85608], "fy":[80.87741,52.83615,83.59362,103.09821]}, + {"t":3.41574, "x":5.69543, "y":5.78963, "heading":-1.69854, "vx":-0.85388, "vy":-0.89005, "omega":1.26085, "ax":4.84146, "ay":5.04676, "alpha":-5.98575, "fx":[69.04536,102.64476,84.34706,51.40981], "fy":[79.97681,50.73546,84.79669,104.97486]}, + {"t":3.44093, "x":5.67546, "y":5.76881, "heading":-1.66678, "vx":-0.73194, "vy":-0.76293, "omega":1.11008, "ax":4.84222, "ay":5.04754, "alpha":-6.41527, "fx":[69.82325,104.48402,84.16266,49.0253], "fy":[78.97973,48.86445,85.96727,106.72217]}, + {"t":3.46612, "x":5.65856, "y":5.75119, "heading":-1.63882, "vx":-0.60998, "vy":-0.6358, "omega":0.9485, "ax":4.84279, "ay":5.04807, "alpha":-6.82246, "fx":[70.71801,106.18171,83.924,46.70792], "fy":[77.94766,47.13544,87.1254,108.35851]}, + {"t":3.4913, "x":5.64473, "y":5.73678, "heading":-1.61493, "vx":-0.488, "vy":-0.50865, "omega":0.77666, "ax":4.84326, "ay":5.04843, "alpha":-7.20263, "fx":[71.65585,107.71761,83.68904,44.49857], "fy":[76.9184,45.58467,88.21344,109.87336]}, + {"t":3.51649, "x":5.63398, "y":5.72557, "heading":-1.59537, "vx":-0.36601, "vy":-0.3815, "omega":0.59525, "ax":4.84364, "ay":5.04868, "alpha":-7.55761, "fx":[72.55754,109.12214,83.48202,42.42354], "fy":[75.97033,44.14993,89.21475,111.27129]}, + {"t":3.54168, "x":5.6263, "y":5.71756, "heading":-1.58038, "vx":-0.24402, "vy":-0.25434, "omega":0.40489, "ax":4.84396, "ay":5.04888, "alpha":-7.88595, "fx":[73.35782,110.40013,83.34238,40.50519], "fy":[75.14642,42.82039,90.09643,112.55526]}, + {"t":3.56686, "x":5.62169, "y":5.71276, "heading":-1.57018, "vx":-0.12201, "vy":-0.12717, "omega":0.20627, "ax":4.84423, "ay":5.04903, "alpha":-8.18944, "fx":[73.90709,111.60674,83.30038,38.80835], "fy":[74.6108,41.46637,90.83684,113.71397]}, + {"t":3.59205, "x":5.62015, "y":5.71116, "heading":-1.56498, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[ + {"name":"IntakeDown", "from":{"target":0, "targetTimestamp":0.0, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeDown"}}}, + {"name":"IntakeUp", "from":{"target":4, "targetTimestamp":2.86162, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeUp"}}}] +} diff --git a/src/main/deploy/choreo/LeftSecondDipShort.traj b/src/main/deploy/choreo/LeftSecondDipShort.traj new file mode 100644 index 0000000..4783f78 --- /dev/null +++ b/src/main/deploy/choreo/LeftSecondDipShort.traj @@ -0,0 +1,205 @@ +{ + "name":"LeftSecondDipShort", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":5.611854076385498, "y":5.705188751220703, "heading":-1.5657181171509245, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.448474407196045, "y":4.726224899291992, "heading":-2.671099987796757, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.064094543457031, "y":4.601637363433838, "heading":-3.1254652057435086, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.831842422485352, "y":5.160763740539551, "heading":-2.594308272602356, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.611854076385498, "y":5.705188274383545, "heading":-1.564467367692855, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":4.0}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxAngularVelocity", "props":{"max":4.0}}, "enabled":true}, + {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"first", "to":1, "data":{"type":"MaxVelocity", "props":{"max":3.0}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":2, "to":4, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"5.611854076385498 m", "val":5.611854076385498}, "y":{"exp":"5.705188751220703 m", "val":5.705188751220703}, "heading":{"exp":"-1.5657181171509245 rad", "val":-1.5657181171509245}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.448474407196045 m", "val":7.448474407196045}, "y":{"exp":"4.726224899291992 m", "val":4.726224899291992}, "heading":{"exp":"-2.671099987796757 rad", "val":-2.671099987796757}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.064094543457031 m", "val":6.064094543457031}, "y":{"exp":"4.601637363433838 m", "val":4.601637363433838}, "heading":{"exp":"-3.1254652057435086 rad", "val":-3.1254652057435086}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.831842422485352 m", "val":5.831842422485352}, "y":{"exp":"5.160763740539551 m", "val":5.160763740539551}, "heading":{"exp":"-2.594308272602356 rad", "val":-2.594308272602356}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.611854076385498 m", "val":5.611854076385498}, "y":{"exp":"5.705188274383545 m", "val":5.705188274383545}, "heading":{"exp":"-1.5644673676928553 rad", "val":-1.564467367692855}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"4 m / s ^ 2", "val":4.0}}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"4 rad / s", "val":4.0}}}, "enabled":true}, + {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"first", "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3 m / s", "val":3.0}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, + {"from":2, "to":4, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.0688,2.58534,3.07637,3.6212], + "samples":[ + {"t":0.0, "x":5.61185, "y":5.70519, "heading":-1.56572, "vx":2.74477, "vy":-1.19999, "omega":-0.94649, "ax":0.07296, "ay":-0.13189, "alpha":-0.4929, "fx":[-0.02088,2.34224,2.33817,-0.02664], "fy":[-3.45235,-3.49376,-0.70573,-0.72333]}, + {"t":0.02741, "x":5.6871, "y":5.67225, "heading":-1.59166, "vx":2.74677, "vy":-1.2036, "omega":-0.96, "ax":-0.00261, "ay":-0.0061, "alpha":-0.43399, "fx":[-1.11005,0.97633,1.02745,-1.05958], "fy":[-1.25461,-1.36184,1.09249,1.13636]}, + {"t":0.05481, "x":5.76238, "y":5.63927, "heading":-1.61797, "vx":2.7467, "vy":-1.20377, "omega":-0.97189, "ax":0.01203, "ay":0.02747, "alpha":-0.38262, "fx":[-0.78283,1.06431,1.16489,-0.68254], "fy":[-0.57255,-0.68116,1.45504,1.54315]}, + {"t":0.08222, "x":5.83766, "y":5.60629, "heading":-1.6446, "vx":2.74703, "vy":-1.20302, "omega":-0.98238, "ax":0.01063, "ay":0.02429, "alpha":-0.34538, "fx":[-0.73327,0.92911,1.07088,-0.59176], "fy":[-0.49716,-0.64903,1.28231,1.40632]}, + {"t":0.10962, "x":5.91294, "y":5.57333, "heading":-1.67152, "vx":2.74732, "vy":-1.20235, "omega":-0.99184, "ax":-0.00045, "ay":-0.00101, "alpha":-0.31743, "fx":[-0.85903,0.66691,0.84496,-0.68128], "fy":[-0.80687,-0.98182,0.78447,0.93986]}, + {"t":0.13703, "x":5.98823, "y":5.54038, "heading":-1.6987, "vx":2.74731, "vy":-1.20238, "omega":-1.00054, "ax":-0.02304, "ay":-0.0526, "alpha":-0.29525, "fx":[-1.18823,0.24398,0.45682,-0.97576], "fy":[-1.57321,-1.71305,-0.11969,0.06569]}, + {"t":0.16443, "x":6.06352, "y":5.5074, "heading":-1.72612, "vx":2.74668, "vy":-1.20382, "omega":-1.00863, "ax":-0.06497, "ay":-0.14793, "alpha":-0.27824, "fx":[-1.8439,-0.46775,-0.21858,-1.5953], "fy":[-3.0663,-3.12635,-1.70893,-1.49222]}, + {"t":0.19184, "x":6.13876, "y":5.47436, "heading":-1.75377, "vx":2.7449, "vy":-1.20788, "omega":-1.01626, "ax":-0.13974, "ay":-0.31622, "alpha":-0.26533, "fx":[-3.03969,-1.68642,-1.39684,-2.75113], "fy":[-5.74653,-5.68327,-4.45158,-4.19972]}, + {"t":0.21924, "x":6.21394, "y":5.44114, "heading":-1.78162, "vx":2.74107, "vy":-1.21654, "omega":-1.02353, "ax":-0.26426, "ay":-0.59075, "alpha":-0.25407, "fx":[-5.0426,-3.68329,-3.347,-4.70841], "fy":[-10.14449,-9.89151,-8.88582,-8.5928]}, + {"t":0.24665, "x":6.28896, "y":5.40758, "heading":-1.80967, "vx":2.73382, "vy":-1.23273, "omega":-1.03049, "ax":-0.4628, "ay":-1.01268, "alpha":-0.24007, "fx":[-8.23607,-6.84707,-6.45581,-7.8499], "fy":[-16.92269,-16.36217,-15.68267,-15.34039]}, + {"t":0.27405, "x":6.3637, "y":5.37341, "heading":-1.83791, "vx":2.72114, "vy":-1.26048, "omega":-1.03707, "ax":-0.75395, "ay":-1.59099, "alpha":-0.21523, "fx":[-12.9065,-11.48039,-11.02572,-12.46559], "fy":[-26.24128,-25.16501,-25.01361,-24.61272]}, + {"t":0.30146, "x":6.438, "y":5.33827, "heading":-1.86633, "vx":2.70048, "vy":-1.30409, "omega":-1.04297, "ax":-2.64239, "ay":-1.67099, "alpha":-0.18728, "fx":[-42.78369,-41.55152,-41.09202,-42.37219], "fy":[-27.5244,-26.318,-26.36129,-25.90894]}, + {"t":0.32886, "x":6.51101, "y":5.3019, "heading":-1.89491, "vx":2.62806, "vy":-1.34988, "omega":-1.0481, "ax":-3.72972, "ay":1.3291, "alpha":-0.27932, "fx":[-59.95961,-58.98661,-58.45682,-59.44541], "fy":[20.55678,19.93487,21.78183,22.1284]}, + {"t":0.35627, "x":6.58163, "y":5.26541, "heading":-1.92364, "vx":2.52585, "vy":-1.31346, "omega":-1.05576, "ax":-3.69828, "ay":1.47018, "alpha":-0.27326, "fx":[-59.4727,-58.53295,-57.94235,-58.90347], "fy":[22.79834,22.22572,23.98971,24.34693]}, + {"t":0.38367, "x":6.64946, "y":5.22997, "heading":-1.95257, "vx":2.4245, "vy":-1.27317, "omega":-1.06325, "ax":-3.68739, "ay":1.51517, "alpha":-0.26704, "fx":[-59.29182,-58.39753,-57.7751,-58.69565], "fy":[23.54887,22.95176,24.67057,25.0463]}, + {"t":0.41108, "x":6.71452, "y":5.19564, "heading":-1.98171, "vx":2.32344, "vy":-1.23164, "omega":-1.07056, "ax":-3.68164, "ay":1.53787, "alpha":-0.25339, "fx":[-59.17277,-58.3406,-57.71034,-58.57104], "fy":[23.95268,23.36561,24.9765,25.36466]}, + {"t":0.43848, "x":6.77682, "y":5.16247, "heading":-2.01105, "vx":2.22255, "vy":-1.1895, "omega":-1.07751, "ax":-3.67811, "ay":1.55151, "alpha":-0.23292, "fx":[-59.07142,-58.31305,-57.70163,-58.48442], "fy":[24.21741,23.67334,25.1224,25.5123]}, + {"t":0.46589, "x":6.83634, "y":5.13045, "heading":-2.04058, "vx":2.12175, "vy":-1.14698, "omega":-1.08389, "ax":-3.67572, "ay":1.56062, "alpha":-0.20599, "fx":[-58.97137,-58.29853,-57.72969,-58.41912], "fy":[24.41235,23.9457,25.18367,25.56191]}, + {"t":0.49329, "x":6.89311, "y":5.0996, "heading":-2.07028, "vx":2.02101, "vy":-1.10421, "omega":-1.08954, "ax":-3.67404, "ay":1.56701, "alpha":-0.17665, "fx":[-58.86499,-58.29448,-57.78496,-58.36788], "fy":[24.57873,24.16697,25.20642,25.55742]}, + {"t":0.5207, "x":6.94712, "y":5.06993, "heading":-2.10014, "vx":1.92033, "vy":-1.06126, "omega":-1.09438, "ax":-3.67276, "ay":1.57183, "alpha":-0.14729, "fx":[-58.74703,-58.29784,-57.86039,-58.326], "fy":[24.74053,24.33091,25.21908,25.52516]}, + {"t":0.5481, "x":6.99837, "y":5.04144, "heading":-2.13013, "vx":1.81967, "vy":-1.01819, "omega":-1.09841, "ax":-3.67176, "ay":1.57558, "alpha":-0.11417, "fx":[-58.61718,-58.30358,-57.95354,-58.29344], "fy":[24.88797,24.49174,25.21628,25.45818]}, + {"t":0.57551, "x":7.04686, "y":5.01413, "heading":-2.16023, "vx":1.71905, "vy":-0.97501, "omega":-1.10154, "ax":-3.67096, "ay":1.5786, "alpha":-0.07287, "fx":[-58.47858,-58.29636,-58.06701,-58.27448], "fy":[25.00729,24.70644,25.18764,25.34426]}, + {"t":0.60292, "x":7.09259, "y":4.988, "heading":-2.19042, "vx":1.61844, "vy":-0.93175, "omega":-1.10354, "ax":-3.67031, "ay":1.58102, "alpha":-0.028, "fx":[-58.32776,-58.28378,-58.19713,-58.26683], "fy":[25.11732,24.92358,25.15519,25.20348]}, + {"t":0.63032, "x":7.13556, "y":4.96306, "heading":-2.22066, "vx":1.51786, "vy":-0.88842, "omega":-1.10431, "ax":-3.66977, "ay":1.58305, "alpha":0.02124, "fx":[-58.16335,-58.26352,-58.34347,-58.27066], "fy":[25.21791,25.15413,25.12,25.03645]}, + {"t":0.65773, "x":7.17578, "y":4.9393, "heading":-2.25093, "vx":1.41729, "vy":-0.84503, "omega":-1.10373, "ax":-3.66931, "ay":1.58477, "alpha":0.07666, "fx":[-57.9863,-58.22988,-58.50653,-58.28897], "fy":[25.30138,25.41813,25.08257,24.83566]}, + {"t":0.68513, "x":7.21325, "y":4.91674, "heading":-2.28118, "vx":1.31673, "vy":-0.8016, "omega":-1.10162, "ax":-3.66892, "ay":1.58623, "alpha":0.13353, "fx":[-57.79244,-58.18914,-58.68542,-58.32004], "fy":[25.38809,25.66015,25.06069,24.62123]}, + {"t":0.71254, "x":7.24795, "y":4.89537, "heading":-2.31137, "vx":1.21618, "vy":-0.75813, "omega":-1.09796, "ax":-3.66857, "ay":1.58751, "alpha":0.19853, "fx":[-57.58285,-58.13182,-58.88269,-58.36772], "fy":[25.45444,25.95222,25.0355,24.3694]}, + {"t":0.73994, "x":7.27991, "y":4.87519, "heading":-2.34146, "vx":1.11564, "vy":-0.71463, "omega":-1.09252, "ax":-3.66828, "ay":1.58862, "alpha":0.27041, "fx":[-57.35456,-58.05775,-59.10148,-58.4325], "fy":[25.5094,26.27536,25.00872,24.08827]}, + {"t":0.76735, "x":7.3091, "y":4.8562, "heading":-2.3714, "vx":1.01511, "vy":-0.67109, "omega":-1.08511, "ax":-3.66801, "ay":1.58959, "alpha":0.34551, "fx":[-57.10475,-57.97821,-59.33159,-58.5151], "fy":[25.55643,26.58736,25.01891,23.78111]}, + {"t":0.79475, "x":7.33554, "y":4.83841, "heading":-2.40113, "vx":0.91459, "vy":-0.62753, "omega":-1.07564, "ax":-3.66779, "ay":1.59045, "alpha":0.42905, "fx":[-56.8346,-57.8713,-59.58825,-58.62103], "fy":[25.58919,26.9369,25.03186,23.44017]}, + {"t":0.82216, "x":7.35923, "y":4.82181, "heading":-2.43061, "vx":0.81407, "vy":-0.58394, "omega":-1.06389, "ax":-3.66758, "ay":1.59122, "alpha":0.52574, "fx":[-56.54342,-57.73364,-59.87107,-58.75402], "fy":[25.58798,27.37019,25.04338,23.04537]}, + {"t":0.84956, "x":7.38016, "y":4.8064, "heading":-2.45977, "vx":0.71356, "vy":-0.54033, "omega":-1.04948, "ax":-3.6674, "ay":1.59191, "alpha":0.62977, "fx":[-56.22366,-57.57673,-60.17743,-58.91261], "fy":[25.57581,27.81611,25.08071,22.61817]}, + {"t":0.87697, "x":7.39834, "y":4.79219, "heading":-2.48853, "vx":0.61306, "vy":-0.4967, "omega":-1.03222, "ax":-3.66723, "ay":1.59253, "alpha":0.74205, "fx":[-55.87306,-57.39465,-60.51083,-59.10127], "fy":[25.55171,28.27473,25.14834,22.15572]}, + {"t":0.90437, "x":7.41377, "y":4.77918, "heading":-2.51682, "vx":0.51256, "vy":-0.45306, "omega":-1.01188, "ax":-3.66708, "ay":1.5931, "alpha":0.87038, "fx":[-55.49137,-57.17529,-60.877,-59.32649], "fy":[25.48913,28.82148,25.22595,21.63005]}, + {"t":0.93178, "x":7.42644, "y":4.76736, "heading":-2.54455, "vx":0.41206, "vy":-0.4094, "omega":-0.98803, "ax":-3.66694, "ay":1.59361, "alpha":1.01111, "fx":[-55.0723,-56.9226,-61.27642,-59.5902], "fy":[25.40374,29.40494,25.33538,21.05505]}, + {"t":0.95918, "x":7.43635, "y":4.75674, "heading":-2.57163, "vx":0.31156, "vy":-0.36573, "omega":-0.96032, "ax":-3.66682, "ay":1.59408, "alpha":1.16709, "fx":[-54.61252,-56.63007,-61.71324,-59.89778], "fy":[25.28788,30.04338,25.47649,20.42116]}, + {"t":0.98659, "x":7.44351, "y":4.74731, "heading":-2.59794, "vx":0.21107, "vy":-0.32204, "omega":-0.92834, "ax":-3.6667, "ay":1.59451, "alpha":1.34097, "fx":[-54.10756,-56.29308,-62.1912,-60.25448], "fy":[25.13426,30.75223,25.65053,19.7193]}, + {"t":1.01399, "x":7.44792, "y":4.73909, "heading":-2.62339, "vx":0.11059, "vy":-0.27834, "omega":-0.89159, "ax":-3.6666, "ay":1.59491, "alpha":1.53684, "fx":[-53.55392,-55.89862,-62.71968,-60.66753], "fy":[24.93439,31.56023,25.84834,18.93831]}, + {"t":1.0414, "x":7.44957, "y":4.73206, "heading":-2.64782, "vx":0.0101, "vy":-0.23464, "omega":-0.84947, "ax":-3.6665, "ay":1.59527, "alpha":1.75338, "fx":[-52.94301,-55.45503,-63.29502,-61.1406], "fy":[24.69187,32.43607,26.09732,18.07913]}, + {"t":1.0688, "x":7.44847, "y":4.72622, "heading":-2.6711, "vx":-0.09038, "vy":-0.19092, "omega":-0.80142, "ax":-3.70536, "ay":1.50068, "alpha":1.71433, "fx":[-53.69948,-56.09184,-63.77224,-61.73753], "fy":[22.95621,30.7144,24.85137,16.77551]}, + {"t":1.10769, "x":7.44216, "y":4.71994, "heading":-2.70226, "vx":-0.23446, "vy":-0.13256, "omega":-0.73476, "ax":-3.8062, "ay":1.22516, "alpha":1.43445, "fx":[-56.24918,-58.14372,-64.48397,-62.82831], "fy":[18.36613,25.2437,20.64964,13.54159]}, + {"t":1.14658, "x":7.43016, "y":4.71571, "heading":-2.73083, "vx":-0.38247, "vy":-0.08492, "omega":-0.67898, "ax":-3.9325, "ay":0.72048, "alpha":1.2155, "fx":[-58.98824,-60.60278,-65.80357,-64.33049], "fy":[10.1074,16.43055,12.76517,6.44977]}, + {"t":1.18546, "x":7.41232, "y":4.71295, "heading":-2.75724, "vx":-0.53539, "vy":-0.0569, "omega":-0.63171, "ax":-3.99544, "ay":0.1069, "alpha":1.0363, "fx":[-60.53448,-61.99963,-66.28738,-64.90059], "fy":[0.19821,6.02234,3.0557,-2.48791]}, + {"t":1.22435, "x":7.38848, "y":4.71082, "heading":-2.7818, "vx":-0.69075, "vy":-0.05275, "omega":-0.59141, "ax":-3.9889, "ay":-0.19977, "alpha":0.87321, "fx":[-60.86472,-62.13783,-65.76103,-64.54304], "fy":[-4.6193,0.39107,-1.84142,-6.61646]}, + {"t":1.26323, "x":7.3586, "y":4.70862, "heading":-2.8048, "vx":-0.84586, "vy":-0.06052, "omega":-0.55746, "ax":-3.90089, "ay":-0.14069, "alpha":0.76275, "fx":[-59.6989,-60.79739,-64.1364,-63.08524], "fy":[-3.51689,0.6729,-0.96329,-5.12703]}, + {"t":1.30212, "x":7.32276, "y":4.70616, "heading":-2.82648, "vx":-0.99755, "vy":-0.06599, "omega":-0.5278, "ax":0.00557, "ay":-0.08931, "alpha":0.87245, "fx":[3.0872,1.74438,-2.91141,-1.56632], "fy":[-2.64228,1.23711,-0.0934,-4.17277]}, + {"t":1.341, "x":7.28398, "y":4.70352, "heading":-2.847, "vx":-0.99733, "vy":-0.06946, "omega":-0.49387, "ax":0.00616, "ay":-0.08631, "alpha":0.7236, "fx":[2.56138,1.51952,-2.36717,-1.3227], "fy":[-2.43349,0.80828,-0.22968,-3.62632]}, + {"t":1.37989, "x":7.2452, "y":4.70076, "heading":-2.8662, "vx":-0.99709, "vy":-0.07282, "omega":-0.46574, "ax":0.00489, "ay":-0.06583, "alpha":0.6, "fx":[2.09814,1.28977,-1.94359,-1.1336], "fy":[-1.96807,0.7501,-0.06709,-2.89566]}, + {"t":1.41877, "x":7.20643, "y":4.69788, "heading":-2.88431, "vx":-0.9969, "vy":-0.07538, "omega":-0.4424, "ax":0.00406, "ay":-0.05301, "alpha":0.49874, "fx":[1.72414,1.09622,-1.59556,-0.96677], "fy":[-1.64282,0.64696,-0.00601,-2.36432]}, + {"t":1.45766, "x":7.16767, "y":4.6949, "heading":-2.90152, "vx":-0.99674, "vy":-0.07744, "omega":-0.42301, "ax":0.00349, "ay":-0.04444, "alpha":0.41605, "fx":[1.42377,0.93484,-1.31313,-0.82374], "fy":[-1.40099,0.53299,0.00915,-1.96353]}, + {"t":1.49654, "x":7.12891, "y":4.69186, "heading":-2.91797, "vx":-0.99661, "vy":-0.07917, "omega":-0.40683, "ax":0.00308, "ay":-0.03841, "alpha":0.34868, "fx":[1.18275,0.80083,-1.08508,-0.70291], "fy":[-1.21379,0.42472,0.00389,-1.65383]}, + {"t":1.53543, "x":7.09016, "y":4.68875, "heading":-2.93379, "vx":-0.99649, "vy":-0.08066, "omega":-0.39327, "ax":0.00278, "ay":-0.03406, "alpha":0.29456, "fx":[0.9915,0.69135,-0.9033,-0.60301], "fy":[-1.06799,0.3293,-0.01003,-1.41437]}, + {"t":1.57432, "x":7.05141, "y":4.68559, "heading":-2.94908, "vx":-0.99638, "vy":-0.08198, "omega":-0.38182, "ax":0.00256, "ay":-0.03082, "alpha":0.25155, "fx":[0.84098,0.60311,-0.75989,-0.52195], "fy":[-0.95337,0.24988,-0.02564,-1.22834]}, + {"t":1.6132, "x":7.01267, "y":4.68238, "heading":-2.96393, "vx":-0.99628, "vy":-0.08318, "omega":-0.37204, "ax":0.00238, "ay":-0.02833, "alpha":0.2184, "fx":[0.72545,0.53441,-0.64986,-0.45877], "fy":[-0.86405,0.18826,-0.03813,-1.0853]}, + {"t":1.65209, "x":6.97393, "y":4.67912, "heading":-2.97839, "vx":-0.99619, "vy":-0.08428, "omega":-0.36354, "ax":0.00225, "ay":-0.0264, "alpha":0.19373, "fx":[0.63939,0.48324,-0.56806,-0.41188], "fy":[-0.79619,0.14326,-0.04592,-0.97733]}, + {"t":1.69097, "x":6.9352, "y":4.67582, "heading":-2.99253, "vx":-0.9961, "vy":-0.08531, "omega":-0.35601, "ax":0.00215, "ay":-0.02491, "alpha":0.17691, "fx":[0.57983,0.44907,-0.51172,-0.38094], "fy":[-0.74828,0.11426,-0.04747,-0.90019]}, + {"t":1.72986, "x":6.89646, "y":4.67249, "heading":-3.00637, "vx":-0.99602, "vy":-0.08628, "omega":-0.34913, "ax":0.00207, "ay":-0.0238, "alpha":0.1672, "fx":[0.54373,0.43109,-0.47792,-0.36526], "fy":[-0.71896,0.09968,-0.0422,-0.84993]}, + {"t":1.76874, "x":6.85774, "y":4.66912, "heading":-3.01995, "vx":-0.99594, "vy":-0.0872, "omega":-0.34263, "ax":0.00203, "ay":-0.02303, "alpha":0.16449, "fx":[0.53003,0.42979,-0.46569,-0.36543], "fy":[-0.70824,0.09953,-0.02871,-0.82484]}, + {"t":1.80763, "x":6.81901, "y":4.66571, "heading":-3.03327, "vx":-0.99586, "vy":-0.0881, "omega":-0.33623, "ax":0.00201, "ay":-0.02255, "alpha":0.16863, "fx":[0.53769,0.44562,-0.47403,-0.38195], "fy":[-0.71616,0.11311,-0.00583,-0.8232]}, + {"t":1.84651, "x":6.78029, "y":4.66226, "heading":-3.04635, "vx":-0.99578, "vy":-0.08898, "omega":-0.32968, "ax":0.00201, "ay":-0.02235, "alpha":0.17999, "fx":[0.5672,0.48021,-0.50348,-0.41647], "fy":[-0.74408,0.14142,0.02828,-0.84513]}, + {"t":1.8854, "x":6.74157, "y":4.65879, "heading":-3.05917, "vx":-0.9957, "vy":-0.08985, "omega":-0.32268, "ax":0.00203, "ay":-0.02242, "alpha":0.19904, "fx":[0.6194,0.53551,-0.55485,-0.47094], "fy":[-0.79359,0.18477,0.07574,-0.8909]}, + {"t":1.92429, "x":6.70285, "y":4.65528, "heading":-3.07171, "vx":-0.99562, "vy":-0.09072, "omega":-0.31494, "ax":0.00208, "ay":-0.02277, "alpha":0.22671, "fx":[0.69635,0.61462,-0.63018,-0.54842], "fy":[-0.86767,0.24515,0.13919,-0.96235]}, + {"t":1.96317, "x":6.66414, "y":4.65173, "heading":-3.08396, "vx":-0.99554, "vy":-0.0916, "omega":-0.30612, "ax":0.00216, "ay":-0.0234, "alpha":0.26415, "fx":[0.80091,0.72146,-0.73224,-0.65275], "fy":[-0.96996,0.32406,0.22209,-1.06186]}, + {"t":2.00206, "x":6.62543, "y":4.64815, "heading":-3.09586, "vx":-0.99546, "vy":-0.09251, "omega":-0.29585, "ax":0.00227, "ay":-0.02434, "alpha":0.31297, "fx":[0.93699,0.86108,-0.86481,-0.78884], "fy":[-1.1055,0.42443,0.32834,-1.19323]}, + {"t":2.04094, "x":6.58672, "y":4.64454, "heading":-3.10737, "vx":-0.99537, "vy":-0.09346, "omega":-0.28368, "ax":0.00242, "ay":-0.02566, "alpha":0.37526, "fx":[1.10996,1.0401,-1.03311,-0.96314], "fy":[-1.28066,0.54926,0.4634,-1.36138]}, + {"t":2.07983, "x":6.54802, "y":4.64088, "heading":-3.1184, "vx":-0.99527, "vy":-0.09446, "omega":-0.26909, "ax":0.00261, "ay":-0.02739, "alpha":0.45348, "fx":[1.32618,1.26629,-1.24328,-1.18321], "fy":[-1.50288,0.70252,0.63344,-1.57214]}, + {"t":2.11871, "x":6.50932, "y":4.63719, "heading":-3.12886, "vx":-0.99517, "vy":-0.09552, "omega":-0.25145, "ax":0.00286, "ay":-0.02958, "alpha":0.55099, "fx":[1.59446,1.54999,-1.50392,-1.45913], "fy":[-1.78173,0.88934,0.84724,-1.83339]}, + {"t":2.1576, "x":6.47062, "y":4.63345, "heading":-3.13864, "vx":-0.99506, "vy":-0.09667, "omega":-0.23003, "ax":0.00317, "ay":-0.03237, "alpha":0.67147, "fx":[1.92472,1.90287,-1.82453,-1.80206], "fy":[-2.12858,1.1141,1.11363,-2.15456]}, + {"t":2.19648, "x":6.43193, "y":4.62967, "heading":3.1356, "vx":-0.99494, "vy":-0.09793, "omega":-0.20392, "ax":0.00358, "ay":-0.03608, "alpha":0.82007, "fx":[2.33116,2.34084,-2.21825,-2.22659], "fy":[-2.56177,1.38118,1.44145,-2.5522]}, + {"t":2.23537, "x":6.39324, "y":4.62583, "heading":3.12767, "vx":-0.9948, "vy":-0.09933, "omega":-0.17203, "ax":0.00417, "ay":-0.04144, "alpha":1.00232, "fx":[2.82999,2.8819,-2.69907,-2.74789], "fy":[-3.10627,1.68987,1.83448,-3.04972]}, + {"t":2.27425, "x":6.35456, "y":4.62194, "heading":3.12098, "vx":-0.99464, "vy":-0.10095, "omega":-0.13305, "ax":0.00504, "ay":-0.04917, "alpha":1.22595, "fx":[3.44316,3.5496,-3.28693,-3.38593], "fy":[-3.79231,2.04741,2.29955,-3.67698]}, + {"t":2.31314, "x":6.31589, "y":4.61798, "heading":3.11581, "vx":-0.99444, "vy":-0.10286, "omega":-0.08538, "ax":0.00617, "ay":-0.05901, "alpha":1.49925, "fx":[4.19309,4.36777,-4.00584,-4.16308], "fy":[-4.63504,2.4814,2.85754,-4.45115]}, + {"t":2.35203, "x":6.27723, "y":4.61393, "heading":3.11249, "vx":-0.9942, "vy":-0.10515, "omega":-0.02708, "ax":0.00784, "ay":-0.07311, "alpha":1.83416, "fx":[5.11396,5.37101,-4.88371,-5.10356], "fy":[-5.69728,3.00603,3.4913,-5.44254]}, + {"t":2.39091, "x":6.23857, "y":4.60979, "heading":3.11143, "vx":-0.9939, "vy":-0.10799, "omega":0.04424, "ax":0.01312, "ay":-0.11818, "alpha":2.24746, "fx":[6.29131,6.63648,-5.90599,-6.18881], "fy":[-7.45424,3.29963,3.79061,-7.14086]}, + {"t":2.4298, "x":6.19993, "y":4.6055, "heading":3.11315, "vx":-0.99339, "vy":-0.11259, "omega":0.13163, "ax":0.01922, "ay":-0.16371, "alpha":2.75844, "fx":[7.73421,8.08411,-7.12628,-7.47165], "fy":[-9.52357,4.0404,4.24658,-9.15921]}, + {"t":2.46868, "x":6.16132, "y":4.601, "heading":3.11827, "vx":-0.99264, "vy":-0.11896, "omega":0.2389, "ax":3.49973, "ay":0.98163, "alpha":2.448, "fx":[62.23666,61.04682,48.95791,50.00149], "fy":[8.06703,21.35547,23.30028,9.61355]}, + {"t":2.50757, "x":6.12537, "y":4.59711, "heading":3.12756, "vx":-0.85655, "vy":-0.08079, "omega":0.33409, "ax":1.92212, "ay":3.50384, "alpha":2.76181, "fx":[40.24377,37.57129,21.26512,22.98011], "fy":[48.31785,59.94376,63.01872,51.22381]}, + {"t":2.54645, "x":6.09351, "y":4.59662, "heading":3.14055, "vx":-0.78181, "vy":0.05546, "omega":0.44148, "ax":1.30148, "ay":3.78095, "alpha":3.3132, "fx":[32.66584,29.64769,9.56309,10.77122], "fy":[52.22164,65.33507,67.80318,54.74154]}, + {"t":2.58534, "x":6.06409, "y":4.60164, "heading":-3.12547, "vx":-0.7312, "vy":0.20249, "omega":0.57032, "ax":1.20737, "ay":3.80968, "alpha":2.05716, "fx":[26.59215,24.83096,12.10027,13.14797], "fy":[55.81846,63.86969,65.16058,57.07706]}, + {"t":2.60766, "x":6.04808, "y":4.60711, "heading":-3.11274, "vx":-0.70425, "vy":0.28752, "omega":0.61623, "ax":1.20232, "ay":3.81305, "alpha":1.95927, "fx":[26.20369,24.41859,12.28065,13.44818], "fy":[56.14535,63.82674,64.94643,57.22137]}, + {"t":2.62998, "x":6.03266, "y":4.61447, "heading":-3.09898, "vx":-0.67742, "vy":0.37262, "omega":0.65996, "ax":1.19612, "ay":3.81489, "alpha":1.90122, "fx":[25.93952,24.09239,12.32448,13.60073], "fy":[56.35839,63.83587,64.79123,57.27131]}, + {"t":2.6523, "x":6.01783, "y":4.62374, "heading":-3.08425, "vx":-0.65072, "vy":0.45777, "omega":0.7024, "ax":1.18918, "ay":3.81694, "alpha":1.86417, "fx":[25.73502,23.81062,12.28268,13.68838], "fy":[56.53732,63.88755,64.68491,57.27689]}, + {"t":2.67462, "x":6.00361, "y":4.63491, "heading":-3.06857, "vx":-0.62418, "vy":0.54296, "omega":0.744, "ax":1.18138, "ay":3.81922, "alpha":1.84388, "fx":[25.58199,23.56396,12.16525,13.70992], "fy":[56.70197,63.96115,64.61658,57.2521]}, + {"t":2.69694, "x":5.98997, "y":4.64798, "heading":-3.05197, "vx":-0.59781, "vy":0.6282, "omega":0.78516, "ax":1.17253, "ay":3.82179, "alpha":1.84282, "fx":[25.49214,23.33712,11.96508,13.6649], "fy":[56.82432,64.07807,64.59069,57.2019]}, + {"t":2.71926, "x":5.97692, "y":4.66295, "heading":-3.03444, "vx":-0.57164, "vy":0.7135, "omega":0.82629, "ax":1.16242, "ay":3.8247, "alpha":1.86212, "fx":[25.46002,23.13893,11.66447,13.55343], "fy":[56.91757,64.23539,64.6114,57.1155]}, + {"t":2.74157, "x":5.96445, "y":4.67983, "heading":-3.016, "vx":-0.5457, "vy":0.79887, "omega":0.86785, "ax":1.15074, "ay":3.82803, "alpha":1.90182, "fx":[25.48538,22.94432,11.27005,13.37551], "fy":[56.97944,64.46665,64.66066,56.98424]}, + {"t":2.76389, "x":5.95256, "y":4.69861, "heading":-2.99663, "vx":-0.52001, "vy":0.88431, "omega":0.9103, "ax":1.13711, "ay":3.83186, "alpha":1.9616, "fx":[25.56348,22.76047,10.76101,13.12461], "fy":[57.02397,64.7284,64.7673,56.81472]}, + {"t":2.78621, "x":5.94123, "y":4.7193, "heading":-2.97632, "vx":-0.49463, "vy":0.96984, "omega":0.95408, "ax":1.12098, "ay":3.83633, "alpha":2.04139, "fx":[25.68834,22.57925,10.13328,12.78492], "fy":[57.05421,65.05418,64.89242,56.61718]}, + {"t":2.80853, "x":5.93047, "y":4.74191, "heading":-2.95502, "vx":-0.46961, "vy":1.05546, "omega":0.99964, "ax":1.10162, "ay":3.84159, "alpha":2.14763, "fx":[25.87208,22.33843,9.36776,12.37811], "fy":[57.06253,65.51426,65.05057,56.3251]}, + {"t":2.83085, "x":5.92027, "y":4.76642, "heading":-2.93271, "vx":-0.44502, "vy":1.1412, "omega":1.04758, "ax":1.07794, "ay":3.84789, "alpha":2.28034, "fx":[26.09859,22.06113,8.43644,11.85645], "fy":[57.0549,66.08149,65.23418,55.98203]}, + {"t":2.85317, "x":5.9106, "y":4.79285, "heading":-2.90933, "vx":-0.42097, "vy":1.22709, "omega":1.09847, "ax":1.04832, "ay":3.85556, "alpha":2.43112, "fx":[26.32114,21.77251,7.28957,11.18818], "fy":[57.10902,66.61431,65.48793,55.62806]}, + {"t":2.87549, "x":5.90147, "y":4.8212, "heading":-2.88481, "vx":-0.39757, "vy":1.31314, "omega":1.15273, "ax":1.01021, "ay":3.86507, "alpha":2.60919, "fx":[26.53621,21.39592,5.87774,10.34112], "fy":[57.19605,67.22211,65.79686,55.22802]}, + {"t":2.89781, "x":5.89285, "y":4.85147, "heading":-2.85908, "vx":-0.37502, "vy":1.39941, "omega":1.21097, "ax":0.95936, "ay":3.87713, "alpha":2.83381, "fx":[26.73655,20.781,4.14422,9.2605], "fy":[57.26709,68.15953,66.09729,54.68536]}, + {"t":2.92013, "x":5.88471, "y":4.88367, "heading":-2.83205, "vx":-0.35361, "vy":1.48594, "omega":1.27422, "ax":0.8882, "ay":3.89285, "alpha":3.09415, "fx":[26.76865,19.90758,1.9303,7.79664], "fy":[57.4548,69.23309,66.44893,54.0705]}, + {"t":2.94245, "x":5.87704, "y":4.9178, "heading":-2.80361, "vx":-0.33378, "vy":1.57283, "omega":1.34328, "ax":0.78167, "ay":3.91385, "alpha":3.39059, "fx":[26.43954,18.54457,-1.02179,5.67628], "fy":[57.82449,70.399,66.8936,53.42401]}, + {"t":2.96477, "x":5.86979, "y":4.95388, "heading":-2.77363, "vx":-0.31634, "vy":1.66018, "omega":1.41895, "ax":0.60542, "ay":3.94208, "alpha":3.72741, "fx":[25.23712,16.20727,-5.27182,2.27341], "fy":[58.42068,71.76344,67.30773,52.84151]}, + {"t":2.98709, "x":5.86288, "y":4.99192, "heading":-2.74196, "vx":-0.30282, "vy":1.74817, "omega":1.50215, "ax":0.26179, "ay":3.97422, "alpha":4.17026, "fx":[21.61461,11.42557,-12.28847,-4.12717], "fy":[59.51142,73.79085,67.48733,51.58523]}, + {"t":3.00941, "x":5.85618, "y":5.03193, "heading":-2.70843, "vx":-0.29698, "vy":1.83687, "omega":1.59523, "ax":-0.64461, "ay":3.91638, "alpha":4.71957, "fx":[8.96479,-1.78211,-27.81897,-20.29845], "fy":[59.9169,75.29966,65.67337,47.81154]}, + {"t":3.03173, "x":5.8494, "y":5.0739, "heading":-2.67283, "vx":-0.31137, "vy":1.92428, "omega":1.70057, "ax":-3.60202, "ay":1.58478, "alpha":5.24017, "fx":[-39.90676,-49.63325,-72.6241,-66.57472], "fy":[22.07913,45.38599,28.37562,4.79717]}, + {"t":3.05405, "x":5.84155, "y":5.11725, "heading":-2.63487, "vx":-0.39176, "vy":1.95965, "omega":1.81752, "ax":-3.8631, "ay":-0.88222, "alpha":5.60898, "fx":[-42.81927,-57.51082,-78.29124,-66.69708], "fy":[-21.52584,9.41734,-8.52696,-35.38811]}, + {"t":3.07637, "x":5.83184, "y":5.16076, "heading":-2.59431, "vx":-0.47799, "vy":1.93996, "omega":1.94271, "ax":-3.80296, "ay":-1.0107, "alpha":5.42921, "fx":[-41.84545,-57.52599,-77.30658,-64.82134], "fy":[-22.65759,6.36278,-11.30544,-36.58192]}, + {"t":3.09816, "x":5.82052, "y":5.2028, "heading":-2.55197, "vx":-0.56087, "vy":1.91794, "omega":2.06103, "ax":-3.22124, "ay":-2.30936, "alpha":4.43715, "fx":[-34.57155,-50.20529,-66.68439,-53.09725], "fy":[-42.53797,-19.46123,-32.54509,-52.10677]}, + {"t":3.11995, "x":5.80753, "y":5.24405, "heading":-2.50705, "vx":-0.63107, "vy":1.86761, "omega":2.15773, "ax":-1.26096, "ay":-3.7678, "alpha":3.71461, "fx":[-4.6038,-18.52267,-35.15783,-21.79031], "fy":[-63.15271,-47.18957,-58.0035,-70.92027]}, + {"t":3.14175, "x":5.79348, "y":5.28386, "heading":-2.46003, "vx":-0.65855, "vy":1.78549, "omega":2.23869, "ax":-0.08171, "ay":-3.98113, "alpha":2.94995, "fx":[11.17219,0.57984,-13.83839,-3.10225], "fy":[-64.18475,-53.73502,-63.07425,-71.81933]}, + {"t":3.16354, "x":5.77911, "y":5.32183, "heading":-2.41124, "vx":-0.66033, "vy":1.69873, "omega":2.30298, "ax":0.50555, "ay":-3.9547, "alpha":2.2308, "fx":[17.45428,9.49848,-1.51069,6.66201], "fy":[-62.65111,-55.72937,-63.36005,-69.3946]}, + {"t":3.18533, "x":5.76484, "y":5.35791, "heading":-2.36105, "vx":-0.64931, "vy":1.61255, "omega":2.3516, "ax":0.83509, "ay":-3.90142, "alpha":1.57775, "fx":[19.8408,14.15639,6.59823,12.43553], "fy":[-61.43325,-56.85005,-62.76085,-66.70769]}, + {"t":3.20713, "x":5.75089, "y":5.39212, "heading":-2.3098, "vx":-0.63111, "vy":1.52752, "omega":2.38598, "ax":1.04178, "ay":-3.85334, "alpha":0.93237, "fx":[20.45542,16.92012,12.59597,16.18487], "fy":[-60.60063,-58.25361,-61.80729,-64.03689]}, + {"t":3.22892, "x":5.73738, "y":5.4245, "heading":-2.2578, "vx":-0.60841, "vy":1.44354, "omega":2.4063, "ax":1.18236, "ay":-3.81393, "alpha":0.31859, "fx":[20.10364,18.83585,17.42619,18.71771], "fy":[-60.27467,-59.57825,-60.78943,-61.55312]}, + {"t":3.25071, "x":5.7244, "y":5.45505, "heading":-2.20536, "vx":-0.58264, "vy":1.36042, "omega":2.41324, "ax":1.28377, "ay":-3.78201, "alpha":-0.27097, "fx":[19.1723,20.37483,21.55982,20.41604], "fy":[-60.37804,-60.79044,-59.74473,-59.25558]}, + {"t":3.27251, "x":5.71201, "y":5.4838, "heading":-2.15277, "vx":-0.55466, "vy":1.278, "omega":2.40734, "ax":1.36021, "ay":-3.75597, "alpha":-0.87561, "fx":[17.84903,21.75127,25.2845,21.49236], "fy":[-60.81054,-62.1527,-58.54858,-57.00352]}, + {"t":3.2943, "x":5.70024, "y":5.51076, "heading":-2.1003, "vx":-0.52502, "vy":1.19614, "omega":2.38825, "ax":1.41982, "ay":-3.73446, "alpha":-1.50352, "fx":[16.2483,23.06644,28.78784,22.05988], "fy":[-61.55841,-63.62667,-57.18968,-54.77461]}, + {"t":3.31609, "x":5.68914, "y":5.53594, "heading":-2.04825, "vx":-0.49408, "vy":1.11476, "omega":2.35549, "ax":1.46756, "ay":-3.71646, "alpha":-2.16445, "fx":[14.42389,24.43402,32.17798,22.1586], "fy":[-62.61948,-65.19526,-55.648,-52.54322]}, + {"t":3.33789, "x":5.67872, "y":5.55936, "heading":-1.99692, "vx":-0.46209, "vy":1.03376, "omega":2.30832, "ax":1.50665, "ay":-3.7012, "alpha":-2.86879, "fx":[12.41842,25.91443,35.53558,21.80808], "fy":[-63.99874,-66.86324,-53.88729,-50.28759]}, + {"t":3.35968, "x":5.66901, "y":5.58101, "heading":-1.94661, "vx":-0.42926, "vy":0.9531, "omega":2.2458, "ax":1.53922, "ay":-3.68812, "alpha":-3.61183, "fx":[10.27051,27.58123,38.89732,20.99589], "fy":[-65.72463,-68.49879,-51.93546,-48.04729]}, + {"t":3.38148, "x":5.66002, "y":5.6009, "heading":-1.89767, "vx":-0.39571, "vy":0.87272, "omega":2.16708, "ax":1.56677, "ay":-3.67679, "alpha":-4.39709, "fx":[8.01459,29.50024,42.2839,19.69607], "fy":[-67.78161,-70.07828,-49.77511,-45.8519]}, + {"t":3.40327, "x":5.65177, "y":5.61905, "heading":-1.85044, "vx":-0.36157, "vy":0.79259, "omega":2.07125, "ax":1.59038, "ay":-3.66689, "alpha":-5.23235, "fx":[5.73425,31.62466,45.72683,17.90836], "fy":[-70.10033,-71.70381,-47.37974,-43.67456]}, + {"t":3.42506, "x":5.64426, "y":5.63545, "heading":-1.8053, "vx":-0.32691, "vy":0.71268, "omega":1.95722, "ax":1.61084, "ay":-3.65818, "alpha":-6.12225, "fx":[3.47751,33.90649,49.26613,15.64279], "fy":[-72.69503,-73.38273,-44.72254,-41.50456]}, + {"t":3.44686, "x":5.63752, "y":5.65011, "heading":-1.76265, "vx":-0.2918, "vy":0.63296, "omega":1.8238, "ax":1.62872, "ay":-3.65044, "alpha":-7.05657, "fx":[1.30981,36.33138,52.89449,12.8931], "fy":[-75.50357,-75.0781,-41.82074,-39.4113]}, + {"t":3.46865, "x":5.63155, "y":5.66304, "heading":-1.7229, "vx":-0.25631, "vy":0.5534, "omega":1.67001, "ax":1.6445, "ay":-3.64353, "alpha":-8.01922, "fx":[-0.71965,38.92977,56.55413,9.66615], "fy":[-78.46041,-76.70216,-38.77552,-37.43705]}, + {"t":3.49044, "x":5.62635, "y":5.67424, "heading":-1.68651, "vx":-0.22047, "vy":0.474, "omega":1.49524, "ax":1.65851, "ay":-3.63733, "alpha":-9.00048, "fx":[-2.5631,41.6342,60.2517,5.9973], "fy":[-81.49416,-78.25154,-35.60343,-35.63213]}, + {"t":3.51224, "x":5.62194, "y":5.6837, "heading":-1.65392, "vx":-0.18432, "vy":0.39473, "omega":1.29909, "ax":1.67103, "ay":-3.63173, "alpha":-9.99246, "fx":[-4.19706,44.3634,63.97307,1.97613], "fy":[-84.54475,-79.76039,-32.36054,-33.95997]}, + {"t":3.53403, "x":5.61832, "y":5.69144, "heading":-1.62561, "vx":-0.1479, "vy":0.31558, "omega":1.08132, "ax":1.6823, "ay":-3.62665, "alpha":-10.97998, "fx":[-5.61773,47.06386,67.70654,-2.32187], "fy":[-87.53552,-81.20448,-29.09058,-32.47251]}, + {"t":3.55582, "x":5.6155, "y":5.69746, "heading":-1.60204, "vx":-0.11124, "vy":0.23654, "omega":0.84203, "ax":1.69248, "ay":-3.62203, "alpha":-11.95113, "fx":[-6.85019,49.66417,71.41424,-6.75092], "fy":[-90.42785,-82.59564,-25.92561,-31.06014]}, + {"t":3.57762, "x":5.61348, "y":5.70175, "heading":-1.58369, "vx":-0.07436, "vy":0.1576, "omega":0.58157, "ax":1.70172, "ay":-3.61779, "alpha":-12.89664, "fx":[-7.94224,52.10203,75.094,-11.18942], "fy":[-93.17083,-83.96359,-22.91674,-29.68935]}, + {"t":3.59941, "x":5.61226, "y":5.70433, "heading":-1.57102, "vx":-0.03727, "vy":0.07876, "omega":0.30051, "ax":1.71016, "ay":-3.61391, "alpha":-13.78912, "fx":[-9.19603,54.22791,78.6126,-15.04422], "fy":[-95.74336,-85.33114,-20.13289,-28.28626]}, + {"t":3.6212, "x":5.61185, "y":5.70519, "heading":-1.56447, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[ + {"name":"IntakeDown", "from":{"target":0, "targetTimestamp":0.0, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeDown"}}}, + {"name":"IntakeUp", "from":{"target":3, "targetTimestamp":3.07637, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeUp"}}}] +} diff --git a/src/main/deploy/choreo/LeftShoot.traj b/src/main/deploy/choreo/LeftShoot.traj new file mode 100644 index 0000000..a05beba --- /dev/null +++ b/src/main/deploy/choreo/LeftShoot.traj @@ -0,0 +1,96 @@ +{ + "name":"LeftShoot", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.599256992340088, "y":5.614581108093262, "heading":4.71238898038469, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.2884325981140137, "y":5.549228191375732, "heading":2.1236372236423873, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":5.0}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":8.0}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":5.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.599256992340088 m", "val":3.599256992340088}, "y":{"exp":"5.614581108093262 m", "val":5.614581108093262}, "heading":{"exp":"270 deg", "val":4.71238898038469}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.2884325981140137 m", "val":3.2884325981140137}, "y":{"exp":"5.549228191375732 m", "val":5.549228191375732}, "heading":{"exp":"2.1236372236423873 rad", "val":2.1236372236423873}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"5 m / s", "val":5.0}}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"8 m / s ^ 2", "val":8.0}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"5 rad / s", "val":5.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.6561], + "samples":[ + {"t":0.0, "x":3.59926, "y":5.61458, "heading":-1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.34442, "ay":-0.05626, "alpha":-36.4657, "fx":[-92.6474,81.64727,83.75243,-94.62423], "fy":[-97.6298,-106.9668,105.31691,95.70697]}, + {"t":0.0205, "x":3.59918, "y":5.61457, "heading":-1.5708, "vx":-0.00706, "vy":-0.00115, "omega":-0.74767, "ax":-0.38319, "ay":-0.0625, "alpha":-36.43809, "fx":[-93.03322,80.7875,83.13315,-95.22123], "fy":[-97.19476,-107.54652,105.73078,95.04158]}, + {"t":0.04101, "x":3.59896, "y":5.61453, "heading":-1.58613, "vx":-0.01492, "vy":-0.00243, "omega":-1.49477, "ax":-0.43936, "ay":-0.06988, "alpha":-36.39543, "fx":[-94.94664,77.77898,83.95778,-94.69059], "fy":[-95.22777,-109.6399,104.96339,95.46676]}, + {"t":0.06151, "x":3.59856, "y":5.61447, "heading":-1.61677, "vx":-0.02393, "vy":-0.00387, "omega":-2.24099, "ax":-0.52871, "ay":-0.07971, "alpha":-36.32132, "fx":[-98.3589,72.08946,85.99083,-93.29608], "fy":[-91.5393,-113.30337,103.11314,96.66774]}, + {"t":0.08201, "x":3.59796, "y":5.61437, "heading":-1.66272, "vx":-0.03477, "vy":-0.0055, "omega":-2.9857, "ax":-0.69471, "ay":-0.095, "alpha":-36.16399, "fx":[-103.2403,62.29143,88.58434,-91.7519], "fy":[-85.69404,-118.68984,100.5089,97.84203]}, + {"t":0.10252, "x":3.5971, "y":5.61424, "heading":-1.72394, "vx":-0.04901, "vy":-0.00745, "omega":-3.72718, "ax":-1.11376, "ay":-0.12118, "alpha":-35.65624, "fx":[-109.7206,42.83412,89.01005,-92.85037], "fy":[-76.45996,-126.35174,99.02557,96.09074]}, + {"t":0.12302, "x":3.59586, "y":5.61406, "heading":-1.80036, "vx":-0.07185, "vy":-0.00993, "omega":-4.45825, "ax":-4.56757, "ay":0.0485, "alpha":-26.24409, "fx":[-119.40421,-43.48977,-10.94483,-116.21536], "fy":[-55.76173,-122.45099,120.65896,60.63382]}, + {"t":0.14352, "x":3.59343, "y":5.61387, "heading":-1.89177, "vx":-0.1655, "vy":-0.00894, "omega":-4.99634, "ax":-7.56286, "ay":-1.65065, "alpha":-0.05629, "fx":[-120.18493,-119.87298,-120.1634,-120.04251], "fy":[-25.78765,-27.06229,-25.63202,-26.33941]}, + {"t":0.16403, "x":3.58845, "y":5.61334, "heading":-1.99421, "vx":-0.32056, "vy":-0.04278, "omega":-4.99749, "ax":-7.49063, "ay":-1.72193, "alpha":-0.00004, "fx":[-118.9197,-118.91731,-118.92455,-118.91542], "fy":[-27.33692,-27.34613,-27.31454,-27.35001]}, + {"t":0.18453, "x":3.5803, "y":5.6121, "heading":-2.09667, "vx":-0.47414, "vy":-0.07809, "omega":-4.99749, "ax":-7.43269, "ay":-1.62897, "alpha":0.0, "fx":[-117.99499,-118.00329,-117.99619,-118.00302], "fy":[-25.88335,-25.84267,-25.87381,-25.84464]}, + {"t":0.20503, "x":3.56901, "y":5.61015, "heading":-2.19914, "vx":-0.62654, "vy":-0.11149, "omega":-4.99749, "ax":-7.30279, "ay":-1.7264, "alpha":0.0, "fx":[-115.96369,-115.92423,-115.93836,-115.9224], "fy":[-27.29869,-27.46419,-27.40284,-27.46553]}, + {"t":0.22554, "x":3.55463, "y":5.60751, "heading":-2.3016, "vx":-0.77627, "vy":-0.14689, "omega":-4.99749, "ax":-7.18344, "ay":-1.51667, "alpha":0.0, "fx":[-114.03885,-114.04986,-114.03232,-114.04859], "fy":[-24.10677,-24.04202,-24.11693,-24.04722]}, + {"t":0.24604, "x":3.53721, "y":5.60418, "heading":-2.40407, "vx":-0.92355, "vy":-0.17798, "omega":-4.99749, "ax":-6.77043, "ay":-1.6881, "alpha":0.00002, "fx":[-107.63902,-107.53247,-107.50247,-107.26817], "fy":[-26.49795,-26.86924,-26.80058,-27.03157]}, + {"t":0.26654, "x":3.51685, "y":5.60017, "heading":-2.50653, "vx":-1.06237, "vy":-0.21259, "omega":-4.99749, "ax":-6.54015, "ay":-1.24247, "alpha":0.00001, "fx":[-103.84435,-103.84676,-103.79986,-103.82787], "fy":[-19.75194,-19.64963,-19.7956,-19.70304]}, + {"t":0.28705, "x":3.49369, "y":5.59555, "heading":-2.609, "vx":-1.19646, "vy":-0.23807, "omega":-4.99749, "ax":-3.50731, "ay":-1.01821, "alpha":-0.00002, "fx":[-55.87589,-55.92901,-56.03683,-54.88299], "fy":[-15.9895,-16.12626,-16.19683,-16.34643]}, + {"t":0.30755, "x":3.46842, "y":5.59046, "heading":-2.71146, "vx":-1.26837, "vy":-0.25894, "omega":-4.99749, "ax":-1.99064, "ay":-0.4822, "alpha":0.00001, "fx":[-31.62103,-31.61132,-31.58441,-31.59476], "fy":[-7.66823,-7.61479,-7.67508,-7.66299]}, + {"t":0.32805, "x":3.442, "y":5.58505, "heading":-2.81393, "vx":-1.30919, "vy":-0.26883, "omega":-4.99749, "ax":1.43022, "ay":0.07673, "alpha":0.00001, "fx":[22.73855,22.76469,22.85197,22.46766], "fy":[1.10856,1.20839,1.23888,1.31643]}, + {"t":0.34856, "x":3.41546, "y":5.57955, "heading":-2.91639, "vx":-1.27986, "vy":-0.26726, "omega":-4.99749, "ax":5.54892, "ay":0.58548, "alpha":-0.00002, "fx":[88.13301,88.12692,88.0488,88.06427], "fy":[9.32913,9.15875,9.37106,9.32078]}, + {"t":0.36906, "x":3.39038, "y":5.57419, "heading":-3.01886, "vx":-1.16609, "vy":-0.25525, "omega":-4.99749, "ax":5.80813, "ay":1.08324, "alpha":0.00002, "fx":[92.32633,92.33653,92.5593,91.61143], "fy":[16.92427,17.10423,17.11956,17.64079]}, + {"t":0.38956, "x":3.36769, "y":5.56919, "heading":-3.12132, "vx":-1.04701, "vy":-0.23304, "omega":-4.99749, "ax":7.00953, "ay":1.16269, "alpha":-0.00002, "fx":[111.28431,111.30386,111.26311,111.27424], "fy":[18.46403,18.33906,18.55143,18.47956]}, + {"t":0.41007, "x":3.3477, "y":5.56465, "heading":3.0594, "vx":-0.90329, "vy":-0.2092, "omega":-4.99749, "ax":7.20749, "ay":1.4647, "alpha":0.0, "fx":[114.46442,114.43149,114.4364,114.36461], "fy":[23.06791,23.24015,23.19165,23.51312]}, + {"t":0.43057, "x":3.3307, "y":5.56067, "heading":2.95694, "vx":-0.75551, "vy":-0.17917, "omega":-4.99749, "ax":7.37111, "ay":1.41858, "alpha":0.0, "fx":[117.02034,117.03923,117.01143,117.01626], "fy":[22.53051,22.43008,22.57491,22.54846]}, + {"t":0.45107, "x":3.31675, "y":5.5573, "heading":2.85447, "vx":-0.60438, "vy":-0.15009, "omega":-4.99749, "ax":7.45306, "ay":1.55773, "alpha":0.0, "fx":[118.3395,118.32031,118.33434,118.29694], "fy":[24.65061,24.7418,24.67656,24.85176]}, + {"t":0.47158, "x":3.30593, "y":5.55455, "heading":2.75201, "vx":-0.45157, "vy":-0.11815, "omega":-4.99749, "ax":7.53299, "ay":1.53648, "alpha":0.00005, "fx":[119.59028,119.59668,119.58937,119.59082], "fy":[24.40015,24.36842,24.4048,24.39762]}, + {"t":0.49208, "x":3.29825, "y":5.55245, "heading":2.64954, "vx":-0.29712, "vy":-0.08665, "omega":-4.99749, "ax":7.57451, "ay":1.60768, "alpha":0.05123, "fx":[120.37774,120.22862,120.22499,120.17222], "fy":[24.94361,25.77385,25.62793,25.74705]}, + {"t":0.51258, "x":3.29375, "y":5.55101, "heading":2.54708, "vx":-0.14182, "vy":-0.05368, "omega":-4.99644, "ax":3.69354, "ay":1.5511, "alpha":26.20797, "fx":[108.65028,130.12186,65.65659,-69.87805], "fy":[-72.50039,20.63803,112.59392,37.76752]}, + {"t":0.53308, "x":3.29162, "y":5.55023, "heading":2.44463, "vx":-0.06609, "vy":-0.02188, "omega":-4.45909, "ax":0.98596, "ay":0.35249, "alpha":35.68542, "fx":[51.47846,133.52372,10.56388,-132.95467], "fy":[-123.17043,7.56254,133.10572,4.88616]}, + {"t":0.55359, "x":3.29047, "y":5.54986, "heading":2.35321, "vx":-0.04587, "vy":-0.01465, "omega":-3.72742, "ax":0.63465, "ay":0.21596, "alpha":36.17114, "fx":[28.53821,134.0933,10.69039,-133.02001], "fy":[-130.99592,-4.68303,133.67314,15.71969]}, + {"t":0.57409, "x":3.28967, "y":5.5496, "heading":2.27678, "vx":-0.03286, "vy":-0.01023, "omega":-2.9858, "ax":0.4923, "ay":0.16031, "alpha":36.32407, "fx":[13.51653,133.53191,16.0409,-131.82676], "fy":[-133.62255,-14.95956,133.36927,25.39274]}, + {"t":0.59459, "x":3.2891, "y":5.54943, "heading":2.21556, "vx":-0.02276, "vy":-0.00694, "omega":-2.24103, "ax":0.41451, "ay":0.12979, "alpha":36.39674, "fx":[2.55777,132.46011,21.56035,-130.25573], "fy":[-134.40837,-23.19346,132.7141,33.12991]}, + {"t":0.6151, "x":3.28872, "y":5.54931, "heading":2.16961, "vx":-0.01427, "vy":-0.00428, "omega":-1.49478, "ax":0.36513, "ay":0.11073, "alpha":36.43869, "fx":[-5.36302,131.29602,26.01981,-128.76597], "fy":[-134.40636,-29.38765,131.99062,38.83485]}, + {"t":0.6356, "x":3.2885, "y":5.54925, "heading":2.13897, "vx":-0.00678, "vy":-0.00201, "omega":-0.74767, "ax":0.33062, "ay":0.09795, "alpha":36.46587, "fx":[-10.65946,130.33817,28.9794,-127.66282], "fy":[-134.14586,-33.57454,131.42616,42.51439]}, + {"t":0.6561, "x":3.28843, "y":5.54923, "heading":2.12364, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/LeftShootToBump.traj b/src/main/deploy/choreo/LeftShootToBump.traj new file mode 100644 index 0000000..e43bddb --- /dev/null +++ b/src/main/deploy/choreo/LeftShootToBump.traj @@ -0,0 +1,95 @@ +{ + "name":"LeftShootToBump", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.2884325981140137, "y":5.549228191375732, "heading":2.1236372236423873, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.60064959526062, "y":5.671574592590332, "heading":-1.57, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":5.0}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":7.0}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":5.5}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.2884325981140137 m", "val":3.2884325981140137}, "y":{"exp":"5.549228191375732 m", "val":5.549228191375732}, "heading":{"exp":"2.1236372236423873 rad", "val":2.1236372236423873}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.60064959526062 m", "val":3.60064959526062}, "y":{"exp":"5.671574592590332 m", "val":5.671574592590332}, "heading":{"exp":"-3.14 / 2 rad", "val":-1.57}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"5 m / s", "val":5.0}}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"7 m / s ^ 2", "val":7.0}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"5.5 rad / s", "val":5.5}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.63042], + "samples":[ + {"t":0.0, "x":3.28843, "y":5.54923, "heading":2.12364, "vx":0.0, "vy":0.0, "omega":0.0, "ax":1.37256, "ay":0.62734, "alpha":35.51726, "fx":[18.24928,133.08119,51.25657,-115.42541], "fy":[-133.29412,-20.37651,124.47765,69.03089]}, + {"t":0.02034, "x":3.28872, "y":5.54936, "heading":2.12364, "vx":0.02791, "vy":0.01276, "omega":0.72228, "ax":1.46576, "ay":0.67508, "alpha":35.36581, "fx":[21.1208,133.19936,52.75115,-113.99104], "fy":[-132.82166,-19.39086,123.81778,71.26439]}, + {"t":0.04067, "x":3.28959, "y":5.54976, "heading":2.13833, "vx":0.05772, "vy":0.02649, "omega":1.44149, "ax":1.5799, "ay":0.74569, "alpha":35.15785, "fx":[26.95089,133.53274,53.1958,-113.35137], "fy":[-131.70086,-16.65179,123.58245,72.12341]}, + {"t":0.06101, "x":3.29109, "y":5.55045, "heading":2.16764, "vx":0.08985, "vy":0.04165, "omega":2.15646, "ax":1.72947, "ay":0.85325, "alpha":34.84895, "fx":[36.14111,133.97181,52.94654,-113.23318], "fy":[-129.39752,-12.12247,123.62776,72.07599]}, + {"t":0.08134, "x":3.29327, "y":5.55147, "heading":2.21149, "vx":0.12502, "vy":0.059, "omega":2.86515, "ax":1.93895, "ay":1.02439, "alpha":34.34731, "fx":[49.21999,134.32921,52.59929,-113.01918], "fy":[-124.88849,-5.77019,123.68674,72.02368]}, + {"t":0.10168, "x":3.29622, "y":5.55288, "heading":2.26976, "vx":0.16445, "vy":0.07983, "omega":3.56364, "ax":2.26652, "ay":1.32429, "alpha":33.40253, "fx":[67.05304,134.33297,53.42095,-110.87647], "fy":[-116.09757,2.44878,123.19481,74.55033]}, + {"t":0.12202, "x":3.30003, "y":5.55478, "heading":2.34223, "vx":0.21054, "vy":0.10677, "omega":4.24292, "ax":2.91879, "ay":1.96583, "alpha":30.99816, "fx":[90.8474,133.59932,58.39294,-97.48824], "fy":[-98.22819,12.6235,120.6715,89.76904]}, + {"t":0.14235, "x":3.30491, "y":5.55736, "heading":2.42852, "vx":0.2699, "vy":0.14674, "omega":4.8733, "ax":5.45609, "ay":3.18413, "alpha":19.3691, "fx":[118.75714,131.61858,74.73727,21.36491], "fy":[-60.57006,24.40984,110.73174,127.63019]}, + {"t":0.16269, "x":3.31153, "y":5.561, "heading":2.52762, "vx":0.38086, "vy":0.2115, "omega":5.2672, "ax":6.52541, "ay":2.42404, "alpha":11.26794, "fx":[120.06934,124.32743,83.20918,86.77652], "fy":[-33.74954,28.78305,93.67489,65.22533]}, + {"t":0.18303, "x":3.32063, "y":5.5658, "heading":2.63473, "vx":0.51356, "vy":0.26079, "omega":5.49634, "ax":6.54924, "ay":2.35136, "alpha":0.02177, "fx":[104.03938,103.95316,103.92109,103.9823], "fy":[37.14713,37.33288,37.4813,37.35706]}, + {"t":0.20336, "x":3.33242, "y":5.57159, "heading":2.74651, "vx":0.64674, "vy":0.30861, "omega":5.49679, "ax":6.54793, "ay":2.3269, "alpha":0.0, "fx":[103.98833,103.93647,103.99003,103.89818], "fy":[36.84433,36.97136,36.84995,37.09919]}, + {"t":0.2237, "x":3.34693, "y":5.57835, "heading":2.85829, "vx":0.7799, "vy":0.35593, "omega":5.49679, "ax":6.54696, "ay":2.28455, "alpha":-0.00001, "fx":[103.93044,103.97272,103.92428,103.92383], "fy":[36.29239,36.16284,36.30952,36.31058]}, + {"t":0.24403, "x":3.36414, "y":5.58606, "heading":2.97007, "vx":0.91304, "vy":0.40239, "omega":5.49679, "ax":6.5406, "ay":2.21894, "alpha":0.0, "fx":[103.87363,103.84269,103.86511,103.76561], "fy":[35.11864,35.21258,35.16263,35.4154]}, + {"t":0.26437, "x":3.38406, "y":5.5947, "heading":3.08186, "vx":1.04605, "vy":0.44751, "omega":5.49679, "ax":6.54818, "ay":1.98243, "alpha":0.0, "fx":[103.95756,104.02191,103.91551,103.93365], "fy":[31.50383,31.25076,31.60519,31.5301]}, + {"t":0.28471, "x":3.40669, "y":5.60421, "heading":-3.08954, "vx":1.17922, "vy":0.48783, "omega":5.49679, "ax":6.28367, "ay":1.4482, "alpha":-0.00004, "fx":[99.93769,99.93438,100.16327,98.99626], "fy":[22.67953,22.84289,22.78901,23.6534]}, + {"t":0.30504, "x":3.43197, "y":5.61443, "heading":-2.97776, "vx":1.307, "vy":0.51728, "omega":5.49678, "ax":-1.0682, "ay":-1.37688, "alpha":0.00001, "fx":[-17.04833,-17.02911,-16.86625,-16.89033], "fy":[-21.94225,-21.49504,-22.08863,-21.90995]}, + {"t":0.32538, "x":3.45833, "y":5.62467, "heading":-2.86598, "vx":1.28528, "vy":0.48928, "omega":5.49679, "ax":-5.94593, "ay":-3.06467, "alpha":0.00003, "fx":[-94.64979,-94.51098,-94.52301,-93.90027], "fy":[-48.24893,-48.58006,-48.52866,-49.25776]}, + {"t":0.34571, "x":3.48324, "y":5.63399, "heading":-2.75419, "vx":1.16436, "vy":0.42695, "omega":5.49679, "ax":-6.31747, "ay":-2.68724, "alpha":0.0, "fx":[-100.29795,-100.34817,-100.24078,-100.29089], "fy":[-42.67574,-42.5326,-42.77967,-42.65955]}, + {"t":0.36605, "x":3.50561, "y":5.64211, "heading":-2.64241, "vx":1.03589, "vy":0.37231, "omega":5.49679, "ax":-6.38408, "ay":-2.65881, "alpha":0.00001, "fx":[-101.40696,-101.35993,-101.34186,-101.29879], "fy":[-42.10036,-42.20335,-42.21737,-42.32085]}, + {"t":0.38639, "x":3.52536, "y":5.64913, "heading":-2.53063, "vx":0.90606, "vy":0.31824, "omega":5.49679, "ax":-6.43007, "ay":-2.6075, "alpha":0.0, "fx":[-102.08323,-102.09176,-102.0696,-102.08383], "fy":[-41.39218,-41.37219,-41.42764,-41.39206]}, + {"t":0.40672, "x":3.54245, "y":5.65507, "heading":-2.41884, "vx":0.7753, "vy":0.26521, "omega":5.49679, "ax":-6.45445, "ay":-2.58235, "alpha":-0.00001, "fx":[-102.48344,-102.46109,-102.46273,-102.46929], "fy":[-40.93762,-41.00249,-41.02505,-41.02169]}, + {"t":0.42706, "x":3.55688, "y":5.65993, "heading":-2.30706, "vx":0.64404, "vy":0.21269, "omega":5.49679, "ax":-6.47056, "ay":-2.56499, "alpha":-0.03809, "fx":[-102.69932,-102.58305,-102.77188,-102.84515], "fy":[-40.69701,-41.07371,-40.67586,-40.43778]}, + {"t":0.44739, "x":3.56864, "y":5.66372, "heading":-2.19528, "vx":0.51246, "vy":0.16053, "omega":5.49601, "ax":-6.46278, "ay":-2.58214, "alpha":-11.98695, "fx":[-123.98788,-81.41526,-85.50593,-119.49621], "fy":[-38.89772,-99.7597,-68.5286,43.21226]}, + {"t":0.46773, "x":3.57773, "y":5.66645, "heading":-2.08351, "vx":0.38103, "vy":0.10802, "omega":5.25224, "ax":-6.02014, "ay":-2.79774, "alpha":-15.24513, "fx":[-125.58709,-66.30585,-71.49268,-118.91099], "fy":[-46.45771,-115.99355,-75.64639,60.43268]}, + {"t":0.48807, "x":3.58423, "y":5.66807, "heading":-1.9767, "vx":0.2586, "vy":0.05113, "omega":4.94222, "ax":-2.49776, "ay":-0.45106, "alpha":-32.87524, "fx":[-122.75193,-35.96345,107.88955,-107.78956], "fy":[-54.3105,-129.09187,75.22084,79.53772]}, + {"t":0.5084, "x":3.58897, "y":5.66902, "heading":-1.87619, "vx":0.20781, "vy":0.04195, "omega":4.27366, "ax":-2.12078, "ay":-0.35221, "alpha":-34.11077, "fx":[-118.7724,-10.70711,98.88194,-104.0779], "fy":[-62.85798,-133.77471,89.50764,84.75869]}, + {"t":0.52874, "x":3.59276, "y":5.6698, "heading":-1.78928, "vx":0.16468, "vy":0.03479, "omega":3.57998, "ax":-1.88398, "ay":-0.33664, "alpha":-34.70295, "fx":[-114.36521,8.94337,90.28108,-104.49774], "fy":[-70.73368,-134.02751,98.90475,84.47879]}, + {"t":0.54908, "x":3.59572, "y":5.67043, "heading":-1.71648, "vx":0.12637, "vy":0.02795, "omega":2.87426, "ax":-1.71767, "ay":-0.33926, "alpha":-35.05027, "fx":[-110.00414,24.38044,82.75042,-106.20351], "fy":[-77.44925,-132.18083,105.60925,82.47708]}, + {"t":0.56941, "x":3.59794, "y":5.67093, "heading":-1.65803, "vx":0.09143, "vy":0.02105, "omega":2.16147, "ax":-1.59234, "ay":-0.34421, "alpha":-35.27893, "fx":[-106.04107,36.32943,76.62926,-108.03569], "fy":[-82.8651,-129.4726,110.31055,80.16883]}, + {"t":0.58975, "x":3.59947, "y":5.67129, "heading":-1.61407, "vx":0.05905, "vy":0.01405, "omega":1.44403, "ax":-1.49257, "ay":-0.34644, "alpha":-35.44264, "fx":[-102.74504,45.27749,72.17205,-109.48732], "fy":[-86.97086,-126.67241,113.39,78.2531]}, + {"t":0.61008, "x":3.60036, "y":5.6715, "heading":-1.58471, "vx":0.0287, "vy":0.007, "omega":0.72327, "ax":-1.41125, "ay":-0.34426, "alpha":-35.56569, "fx":[-100.32455,51.54127,69.50438,-110.33971], "fy":[-89.79237,-124.29732,115.12206,77.10607]}, + {"t":0.63042, "x":3.60065, "y":5.67157, "heading":-1.57, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/Nike.traj b/src/main/deploy/choreo/Nike.traj new file mode 100644 index 0000000..bcda87f --- /dev/null +++ b/src/main/deploy/choreo/Nike.traj @@ -0,0 +1,78 @@ +{ + "name":"Nike", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.5263495445251465, "y":4.0151591300964355, "heading":3.14, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.4656169414520264, "y":4.010843276977539, "heading":3.14, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":5.0}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":7.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.5263495445251465 m", "val":3.5263495445251465}, "y":{"exp":"4.0151591300964355 m", "val":4.0151591300964355}, "heading":{"exp":"3.14 rad", "val":3.14}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.4656169414520264 m", "val":2.4656169414520264}, "y":{"exp":"4.010843276977539 m", "val":4.010843276977539}, "heading":{"exp":"3.14 rad", "val":3.14}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"5 m / s", "val":5.0}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"7 m / s ^ 2", "val":7.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.77872], + "samples":[ + {"t":0.0, "x":3.52635, "y":4.01516, "heading":3.14, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.99687, "ay":-0.02869, "alpha":-0.00651, "fx":[-111.09201,-111.09191,-111.06883,-111.06887], "fy":[-0.43207,-0.48566,-0.47166,-0.43225]}, + {"t":0.04867, "x":3.51806, "y":4.01513, "heading":3.14, "vx":-0.34054, "vy":-0.0014, "omega":-0.00032, "ax":-6.99817, "ay":-0.02871, "alpha":-0.00623, "fx":[-111.11199,-111.1119,-111.0901,-111.09015], "fy":[-0.43336,-0.48333,-0.47325,-0.43355]}, + {"t":0.09734, "x":3.4932, "y":4.01502, "heading":3.13998, "vx":-0.68114, "vy":-0.00279, "omega":-0.00062, "ax":-6.99784, "ay":-0.02876, "alpha":0.00043, "fx":[-111.07444,-111.07435,-111.05115,-111.18364], "fy":[-0.43105,-0.47872,-0.47888,-0.43762]}, + {"t":0.14601, "x":3.45176, "y":4.01485, "heading":3.13995, "vx":-1.02172, "vy":-0.00419, "omega":-0.0006, "ax":-6.99738, "ay":-0.02878, "alpha":-0.00593, "fx":[-111.09818,-111.09808,-111.07878,-111.07885], "fy":[-0.43464,-0.48186,-0.47613,-0.43481]}, + {"t":0.19468, "x":3.39375, "y":4.01461, "heading":3.13993, "vx":-1.36229, "vy":-0.00559, "omega":-0.00089, "ax":-6.99664, "ay":-0.02884, "alpha":0.0052, "fx":[-111.03165,-111.03155,-111.01047,-111.23344], "fy":[-0.43296,-0.48038,-0.48058,-0.43741]}, + {"t":0.24335, "x":3.31916, "y":4.01431, "heading":3.13988, "vx":-1.70281, "vy":-0.007, "omega":-0.00063, "ax":-6.99531, "ay":-0.02892, "alpha":-0.00566, "fx":[-111.06419,-111.06409,-111.04719,-111.04728], "fy":[-0.43698,-0.4811,-0.48125,-0.43719]}, + {"t":0.29202, "x":3.228, "y":4.01393, "heading":3.13985, "vx":-2.04328, "vy":-0.00841, "omega":-0.00091, "ax":-6.99219, "ay":-0.0294, "alpha":-0.00079, "fx":[-110.98998,-110.98986,-110.97186,-111.07301], "fy":[-0.4416,-0.48964,-0.48993,-0.44566]}, + {"t":0.34069, "x":3.12027, "y":4.01349, "heading":3.13981, "vx":-2.38359, "vy":-0.00984, "omega":-0.00095, "ax":-6.97628, "ay":-0.0216, "alpha":-0.00749, "fx":[-110.76538,-110.76532,-110.74158,-110.74164], "fy":[-0.31436,-0.36653,-0.37593,-0.31471]}, + {"t":0.38936, "x":2.996, "y":4.01299, "heading":3.13976, "vx":-2.72312, "vy":-0.01089, "omega":-0.00131, "ax":6.97496, "ay":0.02652, "alpha":0.07083, "fx":[111.10543,111.10548,111.13419,109.58526], "fy":[0.4343,0.39044,0.38993,0.46932]}, + {"t":0.43803, "x":2.87172, "y":4.01249, "heading":3.1397, "vx":-2.38365, "vy":-0.0096, "omega":0.00213, "ax":6.99207, "ay":0.02736, "alpha":-0.00531, "fx":[110.99811,110.99822,111.01029,111.01016], "fy":[0.45742,0.41633,0.40686,0.45708]}, + {"t":0.4867, "x":2.76399, "y":4.01205, "heading":3.1398, "vx":-2.04335, "vy":-0.00826, "omega":0.00188, "ax":6.99527, "ay":0.0281, "alpha":-0.01651, "fx":[110.99217,110.99227,111.00352,111.23199], "fy":[0.46113,0.4258,0.42548,0.4722]}, + {"t":0.53537, "x":2.67283, "y":4.01168, "heading":3.13989, "vx":-1.70289, "vy":-0.0069, "omega":0.00107, "ax":6.99662, "ay":0.02824, "alpha":-0.00435, "fx":[111.07187,111.07197,111.08102,111.08088], "fy":[0.46763,0.43766,0.42059,0.46734]}, + {"t":0.58404, "x":2.59823, "y":4.01138, "heading":3.13994, "vx":-1.36236, "vy":-0.00552, "omega":0.00086, "ax":6.99736, "ay":0.0283, "alpha":-0.01202, "fx":[111.04592,111.04601,111.05466,111.20649], "fy":[0.46033,0.43054,0.43026,0.47582]}, + {"t":0.63271, "x":2.54021, "y":4.01115, "heading":3.13999, "vx":-1.0218, "vy":-0.00415, "omega":0.00028, "ax":6.99784, "ay":0.02837, "alpha":-0.00381, "fx":[111.09258,111.09268,111.09898,111.09883], "fy":[0.4682,0.44332,0.42192,0.46795]}, + {"t":0.68138, "x":2.49877, "y":4.01098, "heading":3.14, "vx":-0.68121, "vy":-0.00276, "omega":0.00009, "ax":6.99816, "ay":0.0284, "alpha":-0.00356, "fx":[111.09843,111.09852,111.10349,111.10334], "fy":[0.46805,0.44582,0.42173,0.46781]}, + {"t":0.73005, "x":2.47391, "y":4.01088, "heading":3.14, "vx":-0.34061, "vy":-0.00138, "omega":-0.00008, "ax":6.9984, "ay":0.02841, "alpha":0.00169, "fx":[111.07615,111.17891,111.08199,111.08188], "fy":[0.46775,0.43397,0.43462,0.46746]}, + {"t":0.77872, "x":2.46562, "y":4.01084, "heading":3.14, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/OutpostStart.traj b/src/main/deploy/choreo/OutpostStart.traj new file mode 100644 index 0000000..55b189b --- /dev/null +++ b/src/main/deploy/choreo/OutpostStart.traj @@ -0,0 +1,350 @@ +{ + "name":"OutpostStart", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.574246883392334, "y":3.9598469734191895, "heading":-3.1230766652301596, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.3966126441955566, "y":3.047424077987671, "heading":-1.901270611327069, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.145883560180664, "y":0.6102350950241089, "heading":3.141592653589793, "intervals":65, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.5819629430770874, "y":0.3701103627681732, "heading":3.141592653589793, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.2776426076889038, "y":0.5162930488586426, "heading":1.5707963267948966, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.40682798624038696, "y":1.3462082147598269, "heading":1.5874614232311806, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.39594003558158875, "y":1.9668220281600952, "heading":1.5770462432899754, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.7446147203445437, "y":2.606724500656128, "heading":-2.57025541936111, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.0}}, "enabled":true}, + {"from":0, "to":7, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":0.5}}, "enabled":true}, + {"from":0, "to":6, "data":{"type":"MaxAngularVelocity", "props":{"max":2.0}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":7, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":7, "to":7, "data":{"type":"MaxAngularVelocity", "props":{"max":10.0}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.574246883392334 m", "val":3.574246883392334}, "y":{"exp":"3.9598469734191895 m", "val":3.9598469734191895}, "heading":{"exp":"-3.1230766652301596 rad", "val":-3.1230766652301596}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.3966126441955566 m", "val":2.3966126441955566}, "y":{"exp":"3.047424077987671 m", "val":3.047424077987671}, "heading":{"exp":"-1.901270611327069 rad", "val":-1.901270611327069}, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.145883560180664 m", "val":2.145883560180664}, "y":{"exp":"0.6102350950241089 m", "val":0.6102350950241089}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":65, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.5819629430770874 m", "val":0.5819629430770874}, "y":{"exp":"0.3701103627681732 m", "val":0.3701103627681732}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.2776426076889038 m", "val":1.2776426076889038}, "y":{"exp":"0.5162930488586426 m", "val":0.5162930488586426}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.40682798624038696 m", "val":0.40682798624038696}, "y":{"exp":"1.3462082147598267 m", "val":1.3462082147598269}, "heading":{"exp":"1.5874614232311806 rad", "val":1.5874614232311806}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.39594003558158875 m", "val":0.39594003558158875}, "y":{"exp":"1.9668220281600952 m", "val":1.9668220281600952}, "heading":{"exp":"1.5770462432899754 rad", "val":1.5770462432899754}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.7446147203445435 m", "val":1.7446147203445437}, "y":{"exp":"2.606724500656128 m", "val":2.606724500656128}, "heading":{"exp":"-2.5702554193611094 rad", "val":-2.57025541936111}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3 m / s", "val":3.0}}}, "enabled":true}, + {"from":0, "to":7, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.5 m / s", "val":0.5}}}, "enabled":true}, + {"from":0, "to":6, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"2 rad / s", "val":2.0}}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":7, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":7, "to":7, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"10 rad / s", "val":10.0}}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.83392,1.91069,5.16588,5.99572,6.86419,7.28968,8.42028], + "samples":[ + {"t":0.0, "x":3.57425, "y":3.95985, "heading":-3.12308, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.9518, "ay":-0.71533, "alpha":14.00207, "fx":[-64.97105,-81.51352,-118.48181,-112.99062], "fy":[-85.6954,60.1156,25.26085,-45.10657]}, + {"t":0.02316, "x":3.57265, "y":3.95966, "heading":-3.12308, "vx":-0.13787, "vy":-0.01657, "omega":0.32435, "ax":-5.94487, "ay":-0.79033, "alpha":12.89024, "fx":[-66.64394,-82.80764,-116.80661,-111.25856], "fy":[-80.32596,51.24836,22.96496,-44.07572]}, + {"t":0.04633, "x":3.56786, "y":3.95906, "heading":-3.11556, "vx":-0.27558, "vy":-0.03488, "omega":0.62295, "ax":-5.9331, "ay":-0.87321, "alpha":11.58228, "fx":[-68.86164,-84.02721,-114.864,-109.01667], "fy":[-73.62343,41.83062,19.44545,-43.10426]}, + {"t":0.06949, "x":3.55989, "y":3.95802, "heading":-3.10113, "vx":-0.41302, "vy":-0.05511, "omega":0.89125, "ax":-5.91857, "ay":-0.96576, "alpha":10.00047, "fx":[-71.51537,-85.48318,-112.43889,-106.40893], "fy":[-65.64287,30.77651,15.06492,-41.52692]}, + {"t":0.09266, "x":3.54873, "y":3.95648, "heading":-3.08049, "vx":-0.55012, "vy":-0.07748, "omega":1.1229, "ax":-5.90049, "ay":-1.06955, "alpha":8.1915, "fx":[-74.959,-86.87115,-109.55398,-103.31447], "fy":[-56.45642,19.48138,8.93404,-39.87869]}, + {"t":0.11582, "x":3.5344, "y":3.9544, "heading":-3.05448, "vx":-0.6868, "vy":-0.10225, "omega":1.31266, "ax":-5.87781, "ay":-1.18679, "alpha":6.27416, "fx":[-78.56193,-88.38364,-106.15605,-100.15638], "fy":[-47.75241,7.83769,2.03021,-37.47999]}, + {"t":0.13899, "x":3.51692, "y":3.95171, "heading":-3.02407, "vx":-0.82296, "vy":-0.12974, "omega":1.45799, "ax":-5.84906, "ay":-1.32004, "alpha":4.44711, "fx":[-82.16382,-89.5743,-102.51146,-97.18302], "fy":[-40.2103,-2.62392,-5.75616,-35.23632]}, + {"t":0.16215, "x":3.49628, "y":3.94835, "heading":-2.9903, "vx":-0.95845, "vy":-0.16032, "omega":1.56101, "ax":-5.8122, "ay":-1.47271, "alpha":2.93363, "fx":[-84.9987,-90.39858,-99.01357,-94.681], "fy":[-35.52549,-11.55168,-13.12654,-33.31774]}, + {"t":0.18532, "x":3.47252, "y":3.94424, "heading":-2.95414, "vx":-1.09309, "vy":-0.19444, "omega":1.62896, "ax":-5.76435, "ay":-1.64887, "alpha":1.80082, "fx":[-86.92188,-90.51874,-95.89624,-92.71651], "fy":[-33.14043,-19.07965,-19.80856,-32.67964]}, + {"t":0.20848, "x":3.44566, "y":3.9393, "heading":-2.9164, "vx":-1.22662, "vy":-0.23263, "omega":1.67068, "ax":-5.70135, "ay":-1.85379, "alpha":1.06147, "fx":[-87.68833,-90.13179,-93.22136,-91.01079], "fy":[-33.46016,-25.17016,-25.75545,-33.33541]}, + {"t":0.23165, "x":3.41571, "y":3.93341, "heading":-2.8777, "vx":-1.35868, "vy":-0.27557, "omega":1.69527, "ax":-5.61706, "ay":-2.09407, "alpha":0.59108, "fx":[-87.5497,-89.00453,-90.78518,-89.36017], "fy":[-35.30324,-30.97347,-31.16826,-35.53482]}, + {"t":0.25481, "x":3.38273, "y":3.92647, "heading":-2.83843, "vx":-1.4888, "vy":-0.32408, "omega":1.70896, "ax":-5.50235, "ay":-2.37786, "alpha":0.32698, "fx":[-86.39015,-87.38717,-88.26678,-87.37121], "fy":[-38.92522,-36.39099,-36.69889,-38.9862]}, + {"t":0.27797, "x":3.34677, "y":3.91832, "heading":-2.79884, "vx":-1.61626, "vy":-0.37916, "omega":1.71653, "ax":-5.34335, "ay":-2.71498, "alpha":0.16627, "fx":[-84.34809,-84.82734,-85.34264,-84.80061], "fy":[-43.57879,-42.4891,-42.5298,-43.81137]}, + {"t":0.30114, "x":3.30789, "y":3.90881, "heading":-2.75908, "vx":-1.74004, "vy":-0.44206, "omega":1.72039, "ax":-5.11894, "ay":-3.11611, "alpha":0.08826, "fx":[-80.96246,-81.38537,-81.50482,-81.21474], "fy":[-49.78883,-49.02609,-49.26361,-49.80372]}, + {"t":0.3243, "x":3.26621, "y":3.89773, "heading":-2.71923, "vx":-1.85861, "vy":-0.51424, "omega":1.72243, "ax":-4.79714, "ay":-3.59037, "alpha":0.03332, "fx":[-76.14158,-76.18594,-76.30811,-75.99673], "fy":[-56.98105,-56.8685,-56.86568,-57.28354]}, + {"t":0.34747, "x":3.22187, "y":3.88486, "heading":-2.67933, "vx":-1.96974, "vy":-0.59741, "omega":1.7232, "ax":-4.33171, "ay":-4.13857, "alpha":0.01533, "fx":[-68.68732,-68.89781,-68.76774,-68.72371], "fy":[-65.77344,-65.55411,-65.71386,-65.76967]}, + {"t":0.37063, "x":3.17508, "y":3.86991, "heading":-2.63941, "vx":-2.07008, "vy":-0.69328, "omega":1.72356, "ax":-3.66363, "ay":-4.73886, "alpha":-0.00843, "fx":[-58.34409,-58.17084,-58.17145,-57.96501], "fy":[-75.09736,-75.22907,-75.22436,-75.38051]}, + {"t":0.3938, "x":3.12615, "y":3.85258, "heading":-2.59949, "vx":-2.15495, "vy":-0.80305, "omega":1.72336, "ax":-2.73796, "ay":-5.32645, "alpha":-0.00478, "fx":[-43.456,-43.63291,-43.36425,-43.41502], "fy":[-84.57626,-84.47819,-84.61435,-84.57608]}, + {"t":0.41696, "x":3.07549, "y":3.83255, "heading":-2.55957, "vx":-2.21837, "vy":-0.92643, "omega":1.72325, "ax":-1.55317, "ay":-5.78337, "alpha":-0.01892, "fx":[-24.85935,-24.68398,-24.62599,-24.46141], "fy":[-91.75401,-91.82851,-91.82711,-91.85112]}, + {"t":0.44013, "x":3.02369, "y":3.80953, "heading":-2.51965, "vx":-2.25435, "vy":-1.0604, "omega":1.72281, "ax":-0.21212, "ay":-5.98443, "alpha":-0.01393, "fx":[-3.45129,-3.40121,-3.27333,-3.34466], "fy":[-95.00745,-95.01031,-95.01587,-94.99552]}, + {"t":0.46329, "x":2.97141, "y":3.78336, "heading":-2.47974, "vx":-2.25926, "vy":-1.19903, "omega":1.72249, "ax":1.09494, "ay":-5.88773, "alpha":-0.01119, "fx":[17.35608,17.33873,17.48248,17.35443], "fy":[-93.49937,-93.48959,-93.44911,-93.45023]}, + {"t":0.48646, "x":2.91937, "y":3.75401, "heading":-2.43984, "vx":-2.2339, "vy":-1.33542, "omega":1.72223, "ax":2.20815, "ay":-5.56767, "alpha":-0.02114, "fx":[34.88899,35.11571,35.13998,35.07932], "fy":[-88.43954,-88.41857,-88.35525,-88.3503]}, + {"t":0.50962, "x":2.86822, "y":3.72158, "heading":-2.39994, "vx":-2.18275, "vy":-1.46439, "omega":1.72174, "ax":3.0715, "ay":-5.14327, "alpha":-0.01803, "fx":[48.75286,48.68664,48.94467,48.66519], "fy":[-81.68251,-81.72411,-81.55595,-81.65023]}, + {"t":0.53278, "x":2.81848, "y":3.68628, "heading":-2.36006, "vx":-2.1116, "vy":-1.58353, "omega":1.72132, "ax":3.70959, "ay":-4.70515, "alpha":-0.0529, "fx":[58.64457,58.85292,59.07708,58.9953], "fy":[-74.82829,-74.85504,-74.57509,-74.53233]}, + {"t":0.55595, "x":2.77056, "y":3.64834, "heading":-2.32019, "vx":-2.02567, "vy":-1.69252, "omega":1.7201, "ax":4.17444, "ay":-4.29935, "alpha":-0.07265, "fx":[66.08929,66.08899,66.63893,66.27204], "fy":[-68.31877,-68.58025,-68.03096,-68.09122]}, + {"t":0.57911, "x":2.72476, "y":3.60798, "heading":-2.28034, "vx":-1.92897, "vy":-1.79212, "omega":1.71842, "ax":4.51465, "ay":-3.94178, "alpha":-0.15845, "fx":[71.06523,71.46941,72.20706,71.95179], "fy":[-63.01897,-63.08593,-62.21947,-61.99048]}, + {"t":0.60228, "x":2.68128, "y":3.5654, "heading":-2.24053, "vx":-1.82439, "vy":-1.88343, "omega":1.71475, "ax":4.76734, "ay":-3.6332, "alpha":-0.29489, "fx":[74.75479,75.19675,76.67834,76.11012], "fy":[-58.33328,-58.85161,-56.92908,-56.6051]}, + {"t":0.62544, "x":2.6403, "y":3.5208, "heading":-2.20081, "vx":-1.71395, "vy":-1.96759, "omega":1.70791, "ax":4.95841, "ay":-3.36877, "alpha":-0.56252, "fx":[76.93412,78.00497,80.43311,79.50138], "fy":[-54.94382,-55.63933,-52.10457,-51.23893]}, + {"t":0.64861, "x":2.60193, "y":3.47432, "heading":-2.16125, "vx":-1.5991, "vy":-2.04562, "omega":1.69488, "ax":5.10563, "ay":-3.14208, "alpha":-1.01309, "fx":[77.9893,79.90067,84.07884,82.25374], "fy":[-52.56418,-53.86977,-47.27215,-45.82543]}, + {"t":0.67177, "x":2.56626, "y":3.42609, "heading":-2.12199, "vx":-1.48083, "vy":-2.11841, "omega":1.67142, "ax":5.22117, "ay":-2.94694, "alpha":-1.7997, "fx":[77.51912,81.24273,87.9454,84.85231], "fy":[-51.91841,-53.75688,-42.19028,-39.27366]}, + {"t":0.69494, "x":2.53335, "y":3.37623, "heading":-2.08327, "vx":-1.35988, "vy":-2.18667, "omega":1.62973, "ax":5.31336, "ay":-2.77804, "alpha":-2.99244, "fx":[75.55796,82.1865,92.46201,87.20729], "fy":[-53.15344,-55.50434,-36.34948,-31.40636]}, + {"t":0.7181, "x":2.50328, "y":3.32483, "heading":-2.04552, "vx":-1.2368, "vy":-2.25103, "omega":1.56041, "ax":5.38808, "ay":-2.63088, "alpha":-4.70205, "fx":[71.74551,83.12514,97.60433,89.68373], "fy":[-57.19161,-58.96209,-29.82226,-21.09269]}, + {"t":0.74127, "x":2.47607, "y":3.27198, "heading":-2.00937, "vx":-1.11199, "vy":-2.31197, "omega":1.45149, "ax":5.44948, "ay":-2.50185, "alpha":-6.7563, "fx":[66.74148,84.33938,103.04243,91.93457], "fy":[-63.4879,-63.36629,-22.8588,-9.16207]}, + {"t":0.76443, "x":2.45178, "y":3.21775, "heading":-1.97575, "vx":-0.98575, "vy":-2.36992, "omega":1.29498, "ax":5.50055, "ay":-2.38806, "alpha":-8.96046, "fx":[61.08615,86.04386,108.16758,94.00353], "fy":[-71.72544,-67.61655,-16.22879,3.92223]}, + {"t":0.78759, "x":2.43042, "y":3.16221, "heading":-1.94575, "vx":-0.85833, "vy":-2.42524, "omega":1.08742, "ax":5.54355, "ay":-2.287, "alpha":-10.98959, "fx":[55.94622,88.0406,112.52622,95.5188], "fy":[-79.95996,-71.10524,-10.4514,16.28549]}, + {"t":0.81076, "x":2.41202, "y":3.10542, "heading":-1.92056, "vx":-0.72992, "vy":-2.47822, "omega":0.83285, "ax":5.58011, "ay":-2.19679, "alpha":-12.73126, "fx":[51.71076,90.08571,116.01557,96.54147], "fy":[-87.4823,-73.661,-5.67997,27.32093]}, + {"t":0.83392, "x":2.39661, "y":3.04742, "heading":-1.90127, "vx":-0.60066, "vy":-2.52911, "omega":0.53793, "ax":5.52501, "ay":-2.32294, "alpha":-12.22147, "fx":[51.2021,89.19582,114.74048,95.71571], "fy":[-87.57136,-73.48583,-7.68637,21.22983]}, + {"t":0.86019, "x":2.38274, "y":2.9802, "heading":-1.88714, "vx":-0.45556, "vy":-2.59011, "omega":0.21696, "ax":5.11503, "ay":-3.12387, "alpha":-10.6348, "fx":[43.45183,81.35635,109.45482,90.55633], "fy":[-91.91127,-79.2427,-21.42343,-5.7976]}, + {"t":0.88645, "x":2.37254, "y":2.9111, "heading":-1.88144, "vx":-0.32122, "vy":-2.67215, "omega":-0.06233, "ax":3.00717, "ay":-5.17036, "alpha":-8.53709, "fx":[7.12786,50.52313,85.85904,47.45396], "fy":[-105.46954,-99.78569,-60.03253,-63.0452]}, + {"t":0.91271, "x":2.36514, "y":2.83914, "heading":-1.88308, "vx":-0.24225, "vy":-2.80794, "omega":-0.28654, "ax":-0.35493, "ay":-5.94037, "alpha":-7.56012, "fx":[-36.26462,5.93809,36.3921,-28.60449], "fy":[-101.76585,-109.33659,-87.35819,-78.77043]}, + {"t":0.93897, "x":2.35866, "y":2.76335, "heading":-1.89061, "vx":-0.25157, "vy":-2.96395, "omega":-0.48509, "ax":0.18492, "ay":-0.94131, "alpha":-12.04457, "fx":[-35.38982,19.5695,45.43828,-17.87486], "fy":[-36.9885,-52.91299,4.68524,25.44038]}, + {"t":0.96524, "x":2.35212, "y":2.68518, "heading":-1.90335, "vx":-0.24671, "vy":-2.98867, "omega":-0.80141, "ax":-0.31691, "ay":0.0263, "alpha":-9.47825, "fx":[-35.37358,7.84737,26.24486,-18.84351], "fy":[-16.60034,-31.50872,17.15732,32.62205]}, + {"t":0.9915, "x":2.34553, "y":2.6067, "heading":-1.92439, "vx":-0.25503, "vy":-2.98798, "omega":-1.05034, "ax":-0.43153, "ay":0.03766, "alpha":-6.47122, "fx":[-27.98621,1.42049,14.60107,-15.43852], "fy":[-10.7154,-21.38688,11.75431,22.73975]}, + {"t":1.01776, "x":2.33868, "y":2.52824, "heading":-1.95198, "vx":-0.26637, "vy":-2.98699, "omega":-1.22029, "ax":-0.36391, "ay":0.03304, "alpha":-4.15367, "fx":[-19.56748,-0.88697,8.1382,-10.793], "fy":[-6.35702,-13.75741,7.28912,14.92361]}, + {"t":1.04403, "x":2.33156, "y":2.44981, "heading":-1.98403, "vx":-0.27592, "vy":-2.98612, "omega":-1.32937, "ax":-0.26946, "ay":0.02522, "alpha":-2.52959, "fx":[-12.79949,-1.59723,4.28665,-7.00118], "fy":[-3.47908,-8.44752,4.24205,9.2862]}, + {"t":1.07029, "x":2.32422, "y":2.37139, "heading":-2.01894, "vx":-0.283, "vy":-2.98546, "omega":-1.39581, "ax":-0.19273, "ay":0.01843, "alpha":-1.46987, "fx":[-8.06553,-1.69957,1.95817,-4.43218], "fy":[-1.74017,-4.96849,2.35913,5.52017]}, + {"t":1.09655, "x":2.31672, "y":2.29299, "heading":-2.0556, "vx":-0.28806, "vy":-2.98498, "omega":-1.43441, "ax":-0.14086, "ay":0.01368, "alpha":-0.82361, "fx":[-5.05132,-1.59783,0.58162,-2.8777], "fy":[-0.76814,-2.83089,1.28708,3.18075]}, + {"t":1.12281, "x":2.30911, "y":2.2146, "heading":-2.09327, "vx":-0.29176, "vy":-2.98462, "omega":-1.45604, "ax":-0.10874, "ay":0.01068, "alpha":-0.45236, "fx":[-3.254,-1.45162,-0.19817,-2.00169], "fy":[-0.25618,-1.60239,0.72085,1.81607]}, + {"t":1.14908, "x":2.30141, "y":2.13622, "heading":-2.13151, "vx":-0.29462, "vy":-2.98434, "omega":-1.46792, "ax":-0.08988, "ay":0.00891, "alpha":-0.24251, "fx":[-2.23526,-1.32129,-0.61857,-1.53278], "fy":[-0.0257,-0.85671,0.41541,1.03277]}, + {"t":1.17534, "x":2.29364, "y":2.05785, "heading":-2.17006, "vx":-0.29698, "vy":-2.9841, "omega":-1.47429, "ax":-0.07688, "ay":0.00768, "alpha":-0.12998, "fx":[-1.65656,-1.1859,-0.78453,-1.25528], "fy":[0.0527,-0.41758,0.24843,0.60402]}, + {"t":1.2016, "x":2.28581, "y":1.97948, "heading":-2.20878, "vx":-0.299, "vy":-2.9839, "omega":-1.4777, "ax":-0.06623, "ay":0.00666, "alpha":-0.07058, "fx":[-1.29063,-1.04499,-0.81231,-1.058], "fy":[0.07919,-0.18731,0.16209,0.36875]}, + {"t":1.22786, "x":2.27794, "y":1.90112, "heading":-2.24759, "vx":-0.30074, "vy":-2.98373, "omega":-1.47956, "ax":-0.05413, "ay":0.00547, "alpha":-0.04657, "fx":[-1.01001,-0.86238,-0.70879,-0.85643], "fy":[0.08601,-0.12319,0.12394,0.26059]}, + {"t":1.25413, "x":2.27002, "y":1.82276, "heading":-2.28644, "vx":-0.30216, "vy":-2.98358, "omega":-1.48078, "ax":-0.0396, "ay":0.00402, "alpha":-0.03216, "fx":[-0.73452,-0.63474,-0.523,-0.62274], "fy":[0.06727,-0.07962,0.08506,0.18247]}, + {"t":1.28039, "x":2.26207, "y":1.7444, "heading":-2.32533, "vx":-0.3032, "vy":-2.98348, "omega":-1.48162, "ax":-0.02114, "ay":0.00215, "alpha":-0.02797, "fx":[-0.4393,-0.34342,-0.23182,-0.32761], "fy":[0.02822,-0.0682,0.0405,0.136]}, + {"t":1.30665, "x":2.2541, "y":1.66605, "heading":-2.36425, "vx":-0.30375, "vy":-2.98342, "omega":-1.48236, "ax":0.00183, "ay":-0.00019, "alpha":-0.02495, "fx":[-0.07968,0.01873,0.13782,0.03961], "fy":[-0.021,-0.06328,-0.01332,0.08569]}, + {"t":1.33291, "x":2.24613, "y":1.5877, "heading":-2.40318, "vx":-0.30371, "vy":-2.98343, "omega":-1.48301, "ax":0.02366, "ay":-0.00241, "alpha":-0.02398, "fx":[0.24523,0.36017,0.50584,0.39138], "fy":[-0.07789,-0.04492,-0.07521,0.04492]}, + {"t":1.35918, "x":2.23816, "y":1.50934, "heading":-2.44212, "vx":-0.30308, "vy":-2.98349, "omega":-1.48364, "ax":-0.0045, "ay":0.00057, "alpha":0.00566, "fx":[-0.21527,-0.09223,0.07187,-0.04985], "fy":[-0.15078,0.36201,-0.15565,-0.01953]}, + {"t":1.38544, "x":2.2302, "y":1.43099, "heading":-2.48109, "vx":-0.3032, "vy":-2.98348, "omega":-1.4835, "ax":-0.10754, "ay":0.36849, "alpha":0.04436, "fx":[-1.85221,-1.7272,-1.56747,-1.68252], "fy":[5.54564,6.6435,5.53259,5.67839]}, + {"t":1.4117, "x":2.2222, "y":1.35276, "heading":-2.52005, "vx":-0.30603, "vy":-2.9738, "omega":-1.48233, "ax":0.61878, "ay":5.91534, "alpha":-0.05173, "fx":[9.7083,9.90069,9.87724,9.80834], "fy":[94.05262,93.46082,94.03648,94.09147]}, + {"t":1.43797, "x":2.21437, "y":1.2767, "heading":-2.55898, "vx":-0.28978, "vy":-2.81845, "omega":-1.48369, "ax":0.61399, "ay":5.94438, "alpha":-0.0156, "fx":[9.69313,9.8005,9.76052,9.73611], "fy":[94.40806,94.24561,94.39999,94.43164]}, + {"t":1.46423, "x":2.20697, "y":1.20473, "heading":-2.59794, "vx":-0.27365, "vy":-2.66233, "omega":-1.4841, "ax":0.61317, "ay":5.95296, "alpha":0.00919, "fx":[9.79972,9.80123,9.63681,9.70064], "fy":[94.51863,94.45602,94.53793,94.51792]}, + {"t":1.49049, "x":2.2, "y":1.13687, "heading":-2.63692, "vx":-0.25755, "vy":-2.50599, "omega":-1.48386, "ax":0.6129, "ay":5.95706, "alpha":0.05726, "fx":[9.99903,9.82946,9.44656,9.64604], "fy":[94.51042,94.71719,94.58612,94.47733]}, + {"t":1.51675, "x":2.19345, "y":1.07311, "heading":-2.67589, "vx":-0.24145, "vy":-2.34954, "omega":-1.48235, "ax":0.6128, "ay":5.95947, "alpha":0.11078, "fx":[10.29681,9.94461,9.14051,9.53269], "fy":[94.51862,94.78446,94.69476,94.44571]}, + {"t":1.54302, "x":2.18732, "y":1.01346, "heading":-2.71482, "vx":-0.22536, "vy":-2.19303, "omega":-1.47944, "ax":0.61277, "ay":5.96104, "alpha":0.19895, "fx":[10.73848,10.11529,8.70724,9.35191], "fy":[94.45238,94.96471,94.79194,94.33445]}, + {"t":1.56928, "x":2.18161, "y":0.95792, "heading":-2.75368, "vx":-0.20926, "vy":-2.03648, "omega":-1.47422, "ax":0.61278, "ay":5.96215, "alpha":0.32304, "fx":[11.36773,10.4104,8.08314,9.05224], "fy":[94.3481,95.13693,94.94523,94.18376]}, + {"t":1.59554, "x":2.17632, "y":0.90649, "heading":-2.79239, "vx":-0.19317, "vy":-1.87989, "omega":-1.46574, "ax":0.61281, "ay":5.96298, "alpha":0.50726, "fx":[12.26718,10.86004,7.20765,8.58023], "fy":[94.15011,95.42709,95.14514,93.94407]}, + {"t":1.6218, "x":2.17146, "y":0.85917, "heading":-2.83089, "vx":-0.17708, "vy":-1.72329, "omega":-1.45241, "ax":0.61284, "ay":5.96361, "alpha":0.76704, "fx":[13.532,11.55512,5.98591,7.84427], "fy":[93.85862,95.76401,95.45652,93.62769]}, + {"t":1.64807, "x":2.16702, "y":0.81597, "heading":-2.86903, "vx":-0.16098, "vy":-1.56667, "omega":-1.43227, "ax":0.61288, "ay":5.96412, "alpha":1.13627, "fx":[15.30271,12.56694,4.32591,6.72411], "fy":[93.40247,96.25265,95.89792,93.18593]}, + {"t":1.67433, "x":2.16301, "y":0.77688, "heading":-2.90665, "vx":-0.14489, "vy":-1.41004, "omega":-1.40243, "ax":0.61292, "ay":5.96453, "alpha":1.64443, "fx":[17.7206,14.0332,2.12298,5.04526], "fy":[92.74526,96.86038,96.54024,92.61923]}, + {"t":1.70059, "x":2.15941, "y":0.74191, "heading":-2.94348, "vx":-0.12879, "vy":-1.25339, "omega":-1.35924, "ax":0.61295, "ay":5.96487, "alpha":2.32708, "fx":[20.94746,16.05031,-0.68002,2.60648], "fy":[91.80952,97.64589,97.42763,91.90377]}, + {"t":1.72685, "x":2.15624, "y":0.71105, "heading":-2.97918, "vx":-0.11269, "vy":-1.09674, "omega":-1.29812, "ax":0.61298, "ay":5.96516, "alpha":3.20918, "fx":[25.11262,18.69109,-4.06487,-0.81275], "fy":[90.53335,98.61761,98.60662,91.04757]}, + {"t":1.75312, "x":2.15349, "y":0.6843, "heading":-3.01327, "vx":-0.09659, "vy":-0.94008, "omega":-1.21384, "ax":0.613, "ay":5.96541, "alpha":4.29615, "fx":[30.2727,21.94326,-7.89715,-5.39134], "fy":[88.87597,99.76222,100.09765,90.08498]}, + {"t":1.77938, "x":2.15117, "y":0.66167, "heading":-3.04515, "vx":-0.08049, "vy":-0.78341, "omega":-1.10101, "ax":0.61301, "ay":5.96562, "alpha":5.56451, "fx":[36.35223,25.69802,-11.92579,-11.1962], "fy":[86.84387,101.03585,101.87927,89.07542]}, + {"t":1.80564, "x":2.14927, "y":0.64316, "heading":-3.07406, "vx":-0.06439, "vy":-0.62674, "omega":-0.95488, "ax":0.61302, "ay":5.96581, "alpha":6.96026, "fx":[43.13615,29.73262,-15.84758,-18.09289], "fy":[84.50054,102.37731,103.88274,88.08575]}, + {"t":1.83191, "x":2.14779, "y":0.62875, "heading":-3.09914, "vx":-0.0483, "vy":-0.47006, "omega":-0.77208, "ax":0.613, "ay":5.96598, "alpha":8.40773, "fx":[50.28017,33.77931,-19.40388,-25.72803], "fy":[81.97053,103.72009,106.0026,87.16372]}, + {"t":1.85817, "x":2.14673, "y":0.61847, "heading":-3.11942, "vx":-0.0322, "vy":-0.31338, "omega":-0.55127, "ax":0.61298, "ay":5.96612, "alpha":9.82888, "fx":[57.4135,37.57261,-22.47419,-33.58581], "fy":[79.40247,105.0285,108.11742,86.31802]}, + {"t":1.88443, "x":2.14609, "y":0.61229, "heading":-3.13389, "vx":-0.0161, "vy":-0.15669, "omega":-0.29314, "ax":0.61295, "ay":5.96626, "alpha":11.1618, "fx":[64.23386,40.93113,-25.08472,-41.15646], "fy":[76.94053,106.28478,110.11752,85.53215]}, + {"t":1.91069, "x":2.14588, "y":0.61024, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.92659, "ay":-0.90997, "alpha":0.00028, "fx":[-94.08851,-94.09174,-94.08761,-94.08826], "fy":[-14.44889,-14.43319,-14.45406,-14.44968]}, + {"t":1.96077, "x":2.13845, "y":0.60909, "heading":3.14159, "vx":-0.2968, "vy":-0.04557, "omega":0.00001, "ax":-3.93987, "ay":-0.60493, "alpha":0.00118, "fx":[-62.54913,-62.55355,-62.54493,-62.54569], "fy":[-9.61328,-9.56526,-9.62196,-9.61423]}, + {"t":2.01085, "x":2.11865, "y":0.60605, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.00007, "ax":0.0, "ay":0.0, "alpha":-0.00154, "fx":[-0.0039,-0.0039,0.00378,0.00378], "fy":[0.00421,-0.00705,-0.0014,0.0042]}, + {"t":2.06093, "x":2.0939, "y":0.60225, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00024, "fx":[-0.0017,-0.0017,0.0017,0.0017], "fy":[-0.00061,0.0038,-0.00257,-0.00061]}, + {"t":2.11101, "x":2.06916, "y":0.59845, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00011, "fx":[-0.00039,-0.00039,0.00039,0.00039], "fy":[0.00018,-0.00075,0.00038,0.00019]}, + {"t":2.16109, "x":2.04441, "y":0.59466, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":0.00002, "fx":[0.00029,0.00029,-0.00029,-0.00029], "fy":[0.00024,-0.00196,0.00148,0.00024]}, + {"t":2.21117, "x":2.01967, "y":0.59086, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":0.00009, "fx":[0.00056,0.00055,-0.00056,-0.00055], "fy":[0.00014,-0.00202,0.00173,0.00015]}, + {"t":2.26125, "x":1.99492, "y":0.58706, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":0.00011, "fx":[0.00059,0.00059,-0.00059,-0.00059], "fy":[0.00006,-0.00175,0.0016,0.00008]}, + {"t":2.31133, "x":1.97018, "y":0.58326, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":0.0001, "fx":[0.0005,0.0005,-0.0005,-0.0005], "fy":[0.0,-0.00132,0.00131,0.00002]}, + {"t":2.36141, "x":1.94543, "y":0.57946, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":0.00008, "fx":[0.00037,0.00036,-0.00037,-0.00036], "fy":[-0.00006,-0.00087,0.00096,-0.00003]}, + {"t":2.41149, "x":1.92069, "y":0.57566, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[0.00023,0.00022,-0.00023,-0.00022], "fy":[-0.00009,-0.00048,0.00063,-0.00006]}, + {"t":2.46157, "x":1.89594, "y":0.57186, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00004, "fx":[0.0001,0.0001,-0.0001,-0.0001], "fy":[-0.0001,-0.0002,0.00037,-0.00007]}, + {"t":2.51165, "x":1.8712, "y":0.56806, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00002, "fx":[0.00001,0.00001,-0.00001,-0.00001], "fy":[-0.0001,-0.00002,0.00018,-0.00006]}, + {"t":2.56173, "x":1.84645, "y":0.56426, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00005,-0.00006,0.00005,0.00006], "fy":[-0.00009,0.00007,0.00006,-0.00004]}, + {"t":2.61181, "x":1.82171, "y":0.56046, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00009,-0.00009,0.00009,0.00009], "fy":[-0.00007,0.0001,0.0,-0.00003]}, + {"t":2.66189, "x":1.79696, "y":0.55666, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.0001,-0.00011,0.0001,0.00011], "fy":[-0.00006,0.0001,-0.00003,-0.00001]}, + {"t":2.71197, "x":1.77222, "y":0.55286, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.0001,-0.00011,0.0001,0.00011], "fy":[-0.00005,0.00008,-0.00004,0.00001]}, + {"t":2.76205, "x":1.74747, "y":0.54906, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.0001,-0.0001,0.0001,0.0001], "fy":[-0.00004,0.00006,-0.00003,0.00001]}, + {"t":2.81213, "x":1.72273, "y":0.54526, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.00009,-0.00009,0.00009,0.00009], "fy":[-0.00004,0.00004,-0.00002,0.00002]}, + {"t":2.86221, "x":1.69798, "y":0.54146, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00007,-0.00008,0.00007,0.00008], "fy":[-0.00003,0.00002,-0.00001,0.00002]}, + {"t":2.91229, "x":1.67324, "y":0.53766, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00006,-0.00006,0.00006,0.00006], "fy":[-0.00003,0.00001,0.0,0.00002]}, + {"t":2.96237, "x":1.64849, "y":0.53387, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00005,-0.00005,0.00005,0.00005], "fy":[-0.00004,0.0,0.00001,0.00002]}, + {"t":3.01245, "x":1.62375, "y":0.53007, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00004,-0.00004,0.00004,0.00004], "fy":[-0.00004,0.0,0.00002,0.00002]}, + {"t":3.06253, "x":1.599, "y":0.52627, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,0.00003,0.00003], "fy":[-0.00004,0.0,0.00002,0.00002]}, + {"t":3.11261, "x":1.57426, "y":0.52247, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00002,-0.00003,0.00002,0.00003], "fy":[-0.00004,0.0,0.00002,0.00002]}, + {"t":3.16269, "x":1.54951, "y":0.51867, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00002,-0.00002,0.00002,0.00002], "fy":[-0.00004,0.0,0.00003,0.00002]}, + {"t":3.21277, "x":1.52477, "y":0.51487, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00002,0.00001,0.00002], "fy":[-0.00004,0.0,0.00003,0.00002]}, + {"t":3.26285, "x":1.50002, "y":0.51107, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,0.00001,0.00001], "fy":[-0.00003,-0.00001,0.00002,0.00002]}, + {"t":3.31293, "x":1.47528, "y":0.50727, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,-0.00001,0.0,0.00001], "fy":[-0.00003,0.0,0.00002,0.00002]}, + {"t":3.36301, "x":1.45053, "y":0.50347, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,-0.00001,0.0,0.00001], "fy":[-0.00003,-0.00001,0.00002,0.00002]}, + {"t":3.41309, "x":1.42579, "y":0.49967, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.00002,-0.00002,0.00002,0.00002]}, + {"t":3.46317, "x":1.40104, "y":0.49587, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.00004,0.00003,0.0,0.00001]}, + {"t":3.51325, "x":1.3763, "y":0.49207, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.0,-0.00001,0.0], "fy":[-0.00001,-0.00006,0.00004,0.00003]}, + {"t":3.56333, "x":1.35155, "y":0.48827, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.0,-0.00001,0.0], "fy":[-0.00002,0.0,0.00001,0.00001]}, + {"t":3.61341, "x":1.32681, "y":0.48447, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.00001,-0.00001,-0.00001], "fy":[-0.00001,-0.00002,0.00002,0.00002]}, + {"t":3.66349, "x":1.30206, "y":0.48067, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.00001,-0.00001,-0.00001], "fy":[-0.00001,-0.00004,0.00002,0.00002]}, + {"t":3.71357, "x":1.27732, "y":0.47688, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00001,-0.00002,-0.00001], "fy":[-0.00001,-0.00004,0.00002,0.00002]}, + {"t":3.76365, "x":1.25257, "y":0.47308, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,-0.00002,-0.00002], "fy":[0.0,-0.00004,0.00002,0.00002]}, + {"t":3.81373, "x":1.22783, "y":0.46928, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00003,0.00002,-0.00003,-0.00002], "fy":[0.0,-0.00005,0.00002,0.00003]}, + {"t":3.86381, "x":1.20308, "y":0.46548, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00003,0.00003,-0.00003,-0.00003], "fy":[0.0,-0.00006,0.00003,0.00003]}, + {"t":3.91389, "x":1.17834, "y":0.46168, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00004,0.00004,-0.00004,-0.00004], "fy":[0.00001,-0.00007,0.00003,0.00003]}, + {"t":3.96397, "x":1.15359, "y":0.45788, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00005,0.00005,-0.00005,-0.00005], "fy":[0.00001,-0.00008,0.00004,0.00003]}, + {"t":4.01405, "x":1.12885, "y":0.45408, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00006,0.00005,-0.00006,-0.00005], "fy":[0.00001,-0.00009,0.00005,0.00003]}, + {"t":4.06412, "x":1.1041, "y":0.45028, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00007,0.00007,-0.00007,-0.00007], "fy":[0.00002,-0.00011,0.00006,0.00003]}, + {"t":4.1142, "x":1.07936, "y":0.44648, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00008,0.00008,-0.00008,-0.00008], "fy":[0.00002,-0.00013,0.00007,0.00004]}, + {"t":4.16428, "x":1.05461, "y":0.44268, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00009,0.00009,-0.00009,-0.00009], "fy":[0.00003,-0.00016,0.00009,0.00004]}, + {"t":4.21436, "x":1.02987, "y":0.43888, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.0001,0.0001,-0.0001,-0.0001], "fy":[0.00004,-0.00019,0.0001,0.00005]}, + {"t":4.26444, "x":1.00512, "y":0.43508, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00011,0.00011,-0.00011,-0.00011], "fy":[0.00005,-0.00022,0.00011,0.00006]}, + {"t":4.31452, "x":0.98038, "y":0.43128, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00011,0.00011,-0.00011,-0.00011], "fy":[0.00007,-0.00025,0.00011,0.00007]}, + {"t":4.3646, "x":0.95563, "y":0.42748, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.0001,0.0001,-0.0001,-0.0001], "fy":[0.00008,-0.00027,0.0001,0.00009]}, + {"t":4.41468, "x":0.93089, "y":0.42368, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00008,0.00008,-0.00008,-0.00008], "fy":[0.0001,-0.00027,0.00006,0.00011]}, + {"t":4.46476, "x":0.90614, "y":0.41989, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[0.00004,0.00004,-0.00004,-0.00004], "fy":[0.00012,-0.00023,-0.00002,0.00013]}, + {"t":4.51484, "x":0.8814, "y":0.41609, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.00003,-0.00003,0.00003,0.00003], "fy":[0.00014,-0.00012,-0.00016,0.00014]}, + {"t":4.56492, "x":0.85665, "y":0.41229, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00005, "fx":[-0.00013,-0.00013,0.00013,0.00013], "fy":[0.00013,0.00009,-0.00036,0.00014]}, + {"t":4.615, "x":0.83191, "y":0.40849, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[-0.00025,-0.00025,0.00025,0.00025], "fy":[0.00011,0.0004,-0.00062,0.00011]}, + {"t":4.66508, "x":0.80716, "y":0.40469, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00009, "fx":[-0.00037,-0.00037,0.00037,0.00037], "fy":[0.00006,0.00082,-0.00094,0.00006]}, + {"t":4.71516, "x":0.78242, "y":0.40089, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":-0.0001, "fx":[-0.00048,-0.00048,0.00048,0.00048], "fy":[-0.00001,0.00129,-0.00127,-0.00001]}, + {"t":4.76524, "x":0.75767, "y":0.39709, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00009, "fx":[-0.00052,-0.00052,0.00052,0.00052], "fy":[-0.0001,0.00172,-0.00152,-0.0001]}, + {"t":4.81532, "x":0.73293, "y":0.39329, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00005, "fx":[-0.00043,-0.00043,0.00043,0.00043], "fy":[-0.0002,0.00199,-0.00158,-0.0002]}, + {"t":4.8654, "x":0.70818, "y":0.38949, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":0.00004, "fx":[-0.0001,-0.0001,0.0001,0.0001], "fy":[-0.00034,0.00193,-0.00126,-0.00034]}, + {"t":4.91548, "x":0.68344, "y":0.38569, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":0.0002, "fx":[0.00065,0.00065,-0.00065,-0.00065], "fy":[-0.00037,0.00088,-0.00013,-0.00037]}, + {"t":4.96556, "x":0.65869, "y":0.38189, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":0.00038, "fx":[0.00202,0.00202,-0.00202,-0.00202], "fy":[0.0002,-0.00312,0.00271,0.0002]}, + {"t":5.01564, "x":0.63395, "y":0.37809, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.00146, "fx":[0.00428,0.00428,-0.00416,-0.00416], "fy":[-0.00337,0.00389,0.00289,-0.00336]}, + {"t":5.06572, "x":0.6092, "y":0.37429, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.00008, "ax":3.93792, "ay":0.60463, "alpha":-0.00107, "fx":[62.51819,62.5231,62.51358,62.51436], "fy":[9.60823,9.56093,9.61733,9.60919]}, + {"t":5.1158, "x":0.5894, "y":0.37125, "heading":3.14159, "vx":-0.2969, "vy":-0.04559, "omega":0.00003, "ax":5.92855, "ay":0.91027, "alpha":-0.00058, "fx":[94.11952,94.12295,94.11855,94.11917], "fy":[14.45544,14.43273,14.46049,14.45619]}, + {"t":5.16588, "x":0.58196, "y":0.37011, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.32316, "ay":0.37345, "alpha":-33.61304, "fx":[-38.22802,-53.24661,124.13387,114.86791], "fy":[128.95568,-123.36004,-52.16837,70.28782]}, + {"t":5.18772, "x":0.58252, "y":0.3702, "heading":3.14159, "vx":0.05073, "vy":0.00816, "omega":-0.73403, "ax":3.61122, "ay":0.37676, "alpha":-30.11793, "fx":[-5.86428,-9.83946,127.66607,117.36088], "fy":[134.20355,-133.60975,-42.57796,65.90971]}, + {"t":5.20956, "x":0.58449, "y":0.37047, "heading":3.12556, "vx":0.12959, "vy":0.01638, "omega":-1.39174, "ax":5.88841, "ay":0.82907, "alpha":-20.92485, "fx":[41.95793,80.76505,131.14183,120.0666], "fy":[127.23704,-105.70282,-29.34809,60.46239]}, + {"t":5.23139, "x":0.58872, "y":0.37102, "heading":3.09517, "vx":0.25818, "vy":0.03449, "omega":-1.84869, "ax":5.8331, "ay":1.35477, "alpha":-6.86855, "fx":[75.95932,87.43844,105.82964,101.19127], "fy":[57.66986,-5.58365,-3.34803,37.29354]}, + {"t":5.25323, "x":0.59575, "y":0.3721, "heading":3.0548, "vx":0.38557, "vy":0.06407, "omega":-1.99869, "ax":5.83267, "ay":1.35022, "alpha":-0.0005, "fx":[92.594,92.77457,92.52537,92.49757], "fy":[21.64727,21.03622,21.60392,21.45534]}, + {"t":5.27507, "x":0.60556, "y":0.37382, "heading":3.01115, "vx":0.51294, "vy":0.09356, "omega":-1.9987, "ax":5.8318, "ay":1.34554, "alpha":0.0, "fx":[92.57972,92.6447,92.54785,92.56386], "fy":[21.41336,21.11523,21.51086,21.4065]}, + {"t":5.29691, "x":0.61815, "y":0.37618, "heading":2.9675, "vx":0.64029, "vy":0.12294, "omega":-1.9987, "ax":5.83031, "ay":1.34066, "alpha":0.0, "fx":[92.55963,92.61376,92.53042,92.53778], "fy":[21.32428,21.06274,21.40441,21.3443]}, + {"t":5.31874, "x":0.63352, "y":0.37919, "heading":2.92386, "vx":0.76761, "vy":0.15222, "omega":-1.9987, "ax":5.82777, "ay":1.33568, "alpha":0.0, "fx":[92.5173,92.5605,92.49674,92.50593], "fy":[21.24263,21.01204,21.31168,21.25315]}, + {"t":5.34058, "x":0.65168, "y":0.38283, "heading":2.88021, "vx":0.89488, "vy":0.18139, "omega":-1.9987, "ax":5.82336, "ay":1.33062, "alpha":0.0, "fx":[92.44652,92.49118,92.42701,92.4359], "fy":[21.16805,20.90919,21.23972,21.1813]}, + {"t":5.36242, "x":0.67261, "y":0.38711, "heading":2.83656, "vx":1.02205, "vy":0.21045, "omega":-1.9987, "ax":5.81545, "ay":1.32414, "alpha":0.0, "fx":[92.32011,92.36647,92.30216,92.30943], "fy":[21.07131,20.78547,21.14158,21.08835]}, + {"t":5.38426, "x":0.69631, "y":0.39202, "heading":2.79292, "vx":1.14904, "vy":0.23936, "omega":-1.9987, "ax":5.79995, "ay":1.30783, "alpha":0.0, "fx":[92.07353,92.12411,92.05543,92.06057], "fy":[20.8251,20.48092,20.89832,20.8468]}, + {"t":5.4061, "x":0.72279, "y":0.39756, "heading":2.74927, "vx":1.2757, "vy":0.26792, "omega":-1.9987, "ax":5.75638, "ay":1.22996, "alpha":0.0, "fx":[91.38274,91.43227,91.36517,91.36713], "fy":[19.60195,19.20058,19.67667,19.62696]}, + {"t":5.42793, "x":0.75202, "y":0.4037, "heading":2.70562, "vx":1.40141, "vy":0.29478, "omega":-1.9987, "ax":2.92059, "ay":0.69163, "alpha":0.0, "fx":[46.36523,46.38006,46.3601,46.36091], "fy":[11.01542,10.81799,11.05755,11.02989]}, + {"t":5.44977, "x":0.78332, "y":0.41031, "heading":2.66197, "vx":1.46519, "vy":0.30989, "omega":-1.9987, "ax":-0.03975, "ay":0.2194, "alpha":0.0, "fx":[-0.63468,-0.63613,-0.62942,-0.62404], "fy":[3.47222,3.50778,3.47843,3.47437]}, + {"t":5.47161, "x":0.81531, "y":0.41713, "heading":2.61833, "vx":1.46432, "vy":0.31468, "omega":-1.9987, "ax":-0.01406, "ay":0.0654, "alpha":0.0, "fx":[-0.22533,-0.22871,-0.22134,-0.21718], "fy":[1.0214,1.09008,1.02058,1.02098]}, + {"t":5.49345, "x":0.84728, "y":0.42401, "heading":2.57468, "vx":1.46401, "vy":0.3161, "omega":-1.9987, "ax":0.00443, "ay":-0.0205, "alpha":0.0, "fx":[0.06868,0.06495,0.0718,0.07566], "fy":[-0.35062,-0.24204,-0.35607,-0.35296]}, + {"t":5.51528, "x":0.87925, "y":0.43091, "heading":2.53103, "vx":1.46411, "vy":0.31566, "omega":-1.9987, "ax":0.01333, "ay":-0.06195, "alpha":0.0, "fx":[0.21058,0.20744,0.21265,0.21582], "fy":[-1.01489,-0.88025,-1.02098,-1.01761]}, + {"t":5.53712, "x":0.91123, "y":0.43779, "heading":2.48739, "vx":1.4644, "vy":0.3143, "omega":-1.9987, "ax":0.02088, "ay":-0.09766, "alpha":0.0, "fx":[0.33102,0.32887,0.33211,0.33426], "fy":[-1.58775,-1.43142,-1.59237,-1.58991]}, + {"t":5.55896, "x":0.94321, "y":0.44463, "heading":2.44374, "vx":1.46486, "vy":0.31217, "omega":-1.9987, "ax":0.02855, "ay":-0.13464, "alpha":0.0, "fx":[0.45308,0.45228,0.45339,0.45419], "fy":[-2.18181,-2.00237,-2.18352,-2.18264]}, + {"t":5.5808, "x":0.97521, "y":0.45142, "heading":2.40009, "vx":1.46548, "vy":0.30923, "omega":-1.9987, "ax":0.03923, "ay":-0.18725, "alpha":0.0, "fx":[0.62295,0.62398,0.62269,0.62163], "fy":[-3.0258,-2.81727,-3.02332,-3.02453]}, + {"t":5.60264, "x":1.00722, "y":0.45812, "heading":2.35644, "vx":1.46634, "vy":0.30514, "omega":-1.9987, "ax":0.05592, "ay":-0.27156, "alpha":0.0, "fx":[0.88817,0.89164,0.88758,0.88374], "fy":[-4.37784,-4.1247,-4.36909,-4.37317]}, + {"t":5.62447, "x":1.03926, "y":0.46472, "heading":2.3128, "vx":1.46756, "vy":0.29921, "omega":-1.9987, "ax":0.08193, "ay":-0.40863, "alpha":0.0, "fx":[1.30154,1.30761,1.30093,1.29244], "fy":[-6.57481,-6.2546,-6.5557,-6.56416]}, + {"t":5.64631, "x":1.07132, "y":0.47116, "heading":2.26915, "vx":1.46935, "vy":0.29029, "omega":-1.9987, "ax":0.08945, "ay":-0.62783, "alpha":0.0, "fx":[1.42331,1.42732,1.42276,1.40703], "fy":[-10.081,-9.68564,-10.04363,-10.059]}, + {"t":5.66815, "x":1.10343, "y":0.47735, "heading":2.2255, "vx":1.4713, "vy":0.27658, "omega":-1.9987, "ax":-5.11645, "ay":-1.28065, "alpha":0.0, "fx":[-81.20727,-81.24095,-81.21348,-81.2477], "fy":[-20.55303,-19.85694,-20.43677,-20.47847]}, + {"t":5.68999, "x":1.13434, "y":0.48308, "heading":2.18186, "vx":1.35957, "vy":0.24861, "omega":-1.9987, "ax":-5.75918, "ay":-1.30743, "alpha":0.0, "fx":[-91.40455,-91.46339,-91.41821,-91.43858], "fy":[-20.93441,-20.4343,-20.80927,-20.8475]}, + {"t":5.71182, "x":1.16266, "y":0.4882, "heading":2.13821, "vx":1.2338, "vy":0.22006, "omega":-1.9987, "ax":-5.81834, "ay":-1.24707, "alpha":0.0, "fx":[-92.34702,-92.40164,-92.36269,-92.37014], "fy":[-19.9189,-19.60998,-19.81909,-19.84485]}, + {"t":5.73366, "x":1.18822, "y":0.49271, "heading":2.09456, "vx":1.10674, "vy":0.19283, "omega":-1.9987, "ax":-5.84344, "ay":-1.20542, "alpha":0.0, "fx":[-92.74811,-92.7968,-92.76411,-92.76673], "fy":[-19.234,-19.00018,-19.14718,-19.16625]}, + {"t":5.7555, "x":1.21099, "y":0.49663, "heading":2.05091, "vx":0.97913, "vy":0.1665, "omega":-1.9987, "ax":-5.85801, "ay":-1.17496, "alpha":0.0, "fx":[-92.98336,-93.02293,-92.99851,-92.99574], "fy":[-18.71766,-18.5761,-18.65444,-18.6655]}, + {"t":5.77734, "x":1.23098, "y":0.49999, "heading":2.00727, "vx":0.85121, "vy":0.14084, "omega":-1.9987, "ax":-5.86863, "ay":-1.14696, "alpha":0.0, "fx":[-93.15493,-93.18664,-93.16778,-93.1659], "fy":[-18.26547,-18.14173,-18.20974,-18.21839]}, + {"t":5.79918, "x":1.24817, "y":0.50279, "heading":1.96362, "vx":0.72305, "vy":0.1158, "omega":-1.9987, "ax":-5.87789, "ay":-1.11659, "alpha":0.0, "fx":[-93.30487,-93.32854,-93.31392,-93.31609], "fy":[-17.78659,-17.65062,-17.73029,-17.73914]}, + {"t":5.82101, "x":1.26255, "y":0.50505, "heading":1.91997, "vx":0.59469, "vy":0.09141, "omega":-1.9987, "ax":-5.88709, "ay":-1.08043, "alpha":0.0, "fx":[-93.45408,-93.46896,-93.4571,-93.4674], "fy":[-17.22784,-17.04904,-17.1618,-17.17162]}, + {"t":5.84285, "x":1.27414, "y":0.50679, "heading":1.87633, "vx":0.46613, "vy":0.06782, "omega":-1.9987, "ax":-5.89722, "ay":-1.03422, "alpha":0.0, "fx":[-93.61787,-93.62431,-93.61247,-93.63629], "fy":[-16.53308,-16.25586,-16.43585,-16.4509]}, + {"t":5.86469, "x":1.28291, "y":0.50803, "heading":1.83268, "vx":0.33735, "vy":0.04523, "omega":-1.9987, "ax":-5.90963, "ay":-0.96954, "alpha":0.0, "fx":[-93.82051,-93.8127,-93.80038,-93.84548], "fy":[-15.56051,-15.14869,-15.42101,-15.43831]}, + {"t":5.88653, "x":1.28887, "y":0.50878, "heading":1.78903, "vx":0.20829, "vy":0.02406, "omega":-1.9987, "ax":-5.9277, "ay":-0.86014, "alpha":0.0, "fx":[-94.11728,-94.08548,-94.07154,-94.15183], "fy":[-13.90987,-13.28748,-13.70334,-13.72044]}, + {"t":5.90836, "x":1.292, "y":0.5091, "heading":1.74538, "vx":0.07885, "vy":0.00528, "omega":-1.9987, "ax":-5.97497, "ay":0.43868, "alpha":0.0, "fx":[-94.86771,-94.84059,-94.84376,-94.87594], "fy":[6.90193,7.06571,6.94124,6.94847]}, + {"t":5.9302, "x":1.2923, "y":0.50932, "heading":1.70174, "vx":-0.05163, "vy":0.01486, "omega":-1.9987, "ax":-5.55094, "ay":2.25882, "alpha":0.0, "fx":[-88.04636,-88.27213,-88.10903,-88.0734], "fy":[36.05405,35.50959,35.91472,35.96344]}, + {"t":5.95204, "x":1.28985, "y":0.51019, "heading":1.65809, "vx":-0.17285, "vy":0.06419, "omega":-1.9987, "ax":-4.99507, "ay":3.31407, "alpha":0.00116, "fx":[-79.22186,-79.86568,-79.121,-78.99294], "fy":[52.76676,52.0491,52.9216,52.71576]}, + {"t":5.97388, "x":1.28488, "y":0.51238, "heading":1.61444, "vx":-0.28194, "vy":0.13656, "omega":-1.99867, "ax":-4.54297, "ay":3.91255, "alpha":11.24478, "fx":[-110.20392,-62.21754,-31.25638,-84.81416], "fy":[17.11083,41.05319,108.30166,81.99261]}, + {"t":5.99572, "x":1.27764, "y":0.51629, "heading":1.5708, "vx":-0.38114, "vy":0.222, "omega":-1.75311, "ax":-4.3572, "ay":4.1173, "alpha":9.95378, "fx":[-104.83486,-60.93019,-31.28985,-79.63983], "fy":[26.57205,47.04612,104.75124,83.09112]}, + {"t":6.03348, "x":1.26014, "y":0.52761, "heading":1.5046, "vx":-0.54567, "vy":0.37747, "omega":-1.37726, "ax":-4.35466, "ay":4.12251, "alpha":8.31396, "fx":[-99.09393,-64.85159,-36.46173,-76.12671], "fy":[34.24673,48.20672,98.00678,81.33131]}, + {"t":6.07124, "x":1.23644, "y":0.5448, "heading":1.45259, "vx":-0.7101, "vy":0.53313, "omega":-1.06333, "ax":-4.34621, "ay":4.1289, "alpha":6.88784, "fx":[-93.90594,-67.14681,-41.59059,-73.35366], "fy":[40.82539,49.59286,92.12218,79.65663]}, + {"t":6.109, "x":1.20652, "y":0.56788, "heading":1.41244, "vx":-0.87421, "vy":0.68904, "omega":-0.80325, "ax":-4.31952, "ay":4.14928, "alpha":5.57256, "fx":[-89.16126,-67.48181,-46.21856,-71.44071], "fy":[46.07788,52.91845,86.88058,77.61425]}, + {"t":6.14675, "x":1.17043, "y":0.59685, "heading":1.38211, "vx":-1.03732, "vy":0.84571, "omega":-0.59283, "ax":-1.77265, "ay":4.47283, "alpha":5.89515, "fx":[-50.48767,-22.17267,-5.92534,-33.9826], "fy":[55.58312,55.24664,88.01102,85.19702]}, + {"t":6.18451, "x":1.13, "y":0.63198, "heading":1.35973, "vx":-1.10425, "vy":1.01461, "omega":-0.37023, "ax":0.28438, "ay":0.3065, "alpha":7.75022, "fx":[-18.38522,18.67548,27.23601,-9.46707], "fy":[-12.1651,-20.15282,22.27935,29.50219]}, + {"t":6.22227, "x":1.08851, "y":0.67051, "heading":1.34575, "vx":-1.09351, "vy":1.02618, "omega":-0.07758, "ax":0.08188, "ay":0.087, "alpha":5.61862, "fx":[-15.73805,11.69494,18.06018,-8.81723], "fy":[-11.49745,-15.87998,13.52522,19.37696]}, + {"t":6.26003, "x":1.04728, "y":0.70932, "heading":1.34282, "vx":-1.09042, "vy":1.02946, "omega":0.13458, "ax":-0.05881, "ay":-0.06243, "alpha":4.07244, "fx":[-12.87693,6.43349,11.23705,-8.528], "fy":[-9.61005,-14.26289,7.88371,12.02491]}, + {"t":6.29779, "x":1.00606, "y":0.74814, "heading":1.3479, "vx":-1.09264, "vy":1.02711, "omega":0.28835, "ax":0.00133, "ay":0.00141, "alpha":2.84883, "fx":[-8.45811,5.21683,8.49096,-5.16551], "fy":[-6.34096,-8.97544,6.28091,9.12499]}, + {"t":6.33555, "x":0.9648, "y":0.78693, "heading":1.35879, "vx":-1.09259, "vy":1.02716, "omega":0.39592, "ax":-0.00974, "ay":-0.01036, "alpha":1.95022, "fx":[-5.81862,3.4264,5.53818,-3.76443], "fy":[-4.61142,-6.35287,4.23185,6.0744]}, + {"t":6.37331, "x":0.92354, "y":0.82571, "heading":1.37374, "vx":-1.09296, "vy":1.02677, "omega":0.46956, "ax":0.00673, "ay":0.00717, "alpha":1.28432, "fx":[-3.60072,2.52457,3.81439,-2.31062], "fy":[-2.77297,-4.00245,3.0541,4.17638]}, + {"t":6.41107, "x":0.88228, "y":0.86448, "heading":1.39147, "vx":-1.09271, "vy":1.02704, "omega":0.51806, "ax":-0.00142, "ay":-0.00151, "alpha":0.79662, "fx":[-2.25693,1.51664,2.23862,-1.58836], "fy":[-1.97101,-2.46477,1.85461,2.48536]}, + {"t":6.44883, "x":0.84101, "y":0.90326, "heading":1.41103, "vx":-1.09276, "vy":1.02698, "omega":0.54814, "ax":0.00062, "ay":0.00066, "alpha":0.41385, "fx":[-1.1273,0.82093,1.14694,-0.80141], "fy":[-0.93277,-1.34477,1.01537,1.30387]}, + {"t":6.48659, "x":0.79975, "y":0.94204, "heading":1.43173, "vx":-1.09274, "vy":1.02701, "omega":0.56376, "ax":0.00399, "ay":0.00424, "alpha":0.11049, "fx":[-0.21317,0.29335,0.36229,-0.18927], "fy":[-0.25388,-0.23226,0.34749,0.40793]}, + {"t":6.52435, "x":0.75849, "y":0.98082, "heading":1.45302, "vx":-1.09259, "vy":1.02717, "omega":0.56794, "ax":0.00033, "ay":0.00036, "alpha":-0.18133, "fx":[0.5279,-0.39267,-0.51731,0.40334], "fy":[0.47324,0.4993,-0.4232,-0.52667]}, + {"t":6.56211, "x":0.71724, "y":1.01961, "heading":1.47446, "vx":-1.09257, "vy":1.02718, "omega":0.56109, "ax":0.01692, "ay":0.01798, "alpha":-0.47416, "fx":[1.53995,-0.75105,-0.99835,1.2838], "fy":[1.48707,1.70547,-0.91651,-1.13396]}, + {"t":6.59987, "x":0.67599, "y":1.05841, "heading":1.49565, "vx":-1.09193, "vy":1.02786, "omega":0.54318, "ax":-0.01791, "ay":-0.01904, "alpha":-0.83721, "fx":[1.93129,-2.15725,-2.50045,1.58891], "fy":[1.82462,2.16787,-2.45137,-2.75047]}, + {"t":6.63763, "x":0.63475, "y":1.09721, "heading":1.51616, "vx":-1.09261, "vy":1.02714, "omega":0.51157, "ax":0.00698, "ay":0.00743, "alpha":-1.28404, "fx":[3.34694,-2.80874,-3.18449,3.08955], "fy":[3.5807,3.78003,-3.27157,-3.61753]}, + {"t":6.67539, "x":0.5935, "y":1.136, "heading":1.53547, "vx":-1.09235, "vy":1.02742, "omega":0.46309, "ax":-0.08894, "ay":-0.09488, "alpha":-1.90035, "fx":[3.33942,-5.82552,-6.18129,3.0193], "fy":[3.355,4.18873,-6.60551,-6.96325]}, + {"t":6.71315, "x":0.55219, "y":1.17472, "heading":1.55296, "vx":-1.09571, "vy":1.02384, "omega":0.39133, "ax":0.27501, "ay":0.29136, "alpha":-2.62305, "fx":[10.56209,-1.95881,-2.05839,10.91918], "fy":[11.73417,12.04936,-2.44122,-2.84026]}, + {"t":6.75091, "x":0.51101, "y":1.21359, "heading":1.56774, "vx":-1.08532, "vy":1.03484, "omega":0.29228, "ax":1.5293, "ay":1.51902, "alpha":-3.31473, "fx":[31.60851,15.81463,16.64245,33.04927], "fy":[32.69729,33.90783,15.37927,14.47814]}, + {"t":6.78867, "x":0.47112, "y":1.25375, "heading":1.57877, "vx":-1.02758, "vy":1.0922, "omega":0.16712, "ax":4.50988, "ay":3.65936, "alpha":-2.75823, "fx":[75.06777,62.72426,68.40198,80.1964], "fy":[63.39729,69.23344,51.82637,47.92297]}, + {"t":6.82643, "x":0.43553, "y":1.2976, "heading":1.58508, "vx":-0.85728, "vy":1.23038, "omega":0.06297, "ax":5.13986, "ay":3.01698, "alpha":-3.03318, "fx":[83.67342,72.72538,79.80179,90.19541], "fy":[55.30318,60.8193,39.92289,35.54167]}, + {"t":6.86419, "x":0.40683, "y":1.34621, "heading":1.58746, "vx":-0.6632, "vy":1.3443, "omega":-0.05156, "ax":5.42974, "ay":2.32798, "alpha":-2.0116, "fx":[87.72145,81.11127,84.73382,91.23768], "fy":[42.44309,45.41401,31.2083,28.76791]}, + {"t":6.89256, "x":0.3902, "y":1.38528, "heading":1.586, "vx":-0.50919, "vy":1.41033, "omega":-0.10862, "ax":5.63366, "ay":1.68714, "alpha":-1.60741, "fx":[91.08308,85.93648,88.07547,92.65904], "fy":[31.37266,33.67279,21.58099,20.51187]}, + {"t":6.92092, "x":0.37802, "y":1.42596, "heading":1.58292, "vx":-0.34938, "vy":1.45819, "omega":-0.15422, "ax":5.12725, "ay":0.96412, "alpha":-1.40238, "fx":[83.59245,78.39976,79.18382,84.41969], "fy":[19.69141,20.08488,10.91425,10.5339]}, + {"t":6.94929, "x":0.37018, "y":1.46771, "heading":1.57854, "vx":-0.20394, "vy":1.48554, "omega":-0.194, "ax":1.92344, "ay":0.2283, "alpha":-1.11759, "fx":[32.91188,27.9092,28.07309,33.25012], "fy":[6.95231,6.78481,0.38554,0.37509]}, + {"t":6.97765, "x":0.36516, "y":1.50994, "heading":1.57304, "vx":-0.14938, "vy":1.49201, "omega":-0.2257, "ax":-0.10574, "ay":-0.0107, "alpha":-0.41103, "fx":[-0.77926,-2.59864,-2.57839,-0.75877], "fy":[0.77018,1.32359,-1.38415,-1.38895]}, + {"t":7.00602, "x":0.36088, "y":1.55226, "heading":1.56664, "vx":-0.15238, "vy":1.49171, "omega":-0.23736, "ax":0.486, "ay":0.04739, "alpha":0.31235, "fx":[6.54859,8.34166,8.33496,7.63719], "fy":[-0.03511,-0.42608,1.72569,1.74468]}, + {"t":7.03438, "x":0.35676, "y":1.59459, "heading":1.55991, "vx":-0.1386, "vy":1.49305, "omega":-0.2285, "ax":-0.60109, "ay":-0.05927, "alpha":1.16664, "fx":[-12.43959,-6.69456,-6.64683,-12.39023], "fy":[-4.35676,-3.92582,2.21908,2.29936]}, + {"t":7.06275, "x":0.35258, "y":1.63692, "heading":1.55342, "vx":-0.15565, "vy":1.49137, "omega":-0.1954, "ax":0.6558, "ay":0.06432, "alpha":1.9243, "fx":[5.34102,14.70457,14.83529,6.76465], "fy":[-4.40013,-4.75843,6.48876,6.75428]}, + {"t":7.09112, "x":0.34843, "y":1.67925, "heading":1.54788, "vx":-0.13705, "vy":1.4932, "omega":-0.14082, "ax":0.57178, "ay":0.04934, "alpha":2.90243, "fx":[2.06422,15.76586,16.07284,2.40651], "fy":[-7.54208,-7.33741,8.74649,9.26618]}, + {"t":7.11948, "x":0.34478, "y":1.72162, "heading":1.54389, "vx":-0.12083, "vy":1.49459, "omega":-0.05849, "ax":3.15082, "ay":0.16011, "alpha":3.57864, "fx":[42.07631,57.54372,57.95471,42.51185], "fy":[-8.34839,-8.07858,12.73602,13.85848]}, + {"t":7.14785, "x":0.34262, "y":1.76408, "heading":1.54223, "vx":-0.03145, "vy":1.49914, "omega":0.04302, "ax":5.74503, "ay":-0.19265, "alpha":3.13126, "fx":[86.29365,95.25962,96.00489,87.26822], "fy":[-15.27393,-13.74274,7.66567,9.11731]}, + {"t":7.17621, "x":0.34403, "y":1.80653, "heading":1.54345, "vx":0.13151, "vy":1.49367, "omega":0.13184, "ax":5.86592, "ay":-0.85123, "alpha":3.52191, "fx":[87.29422,96.57559,99.00317,89.63001], "fy":[-28.56824,-24.76444,-0.62981,-0.09311]}, + {"t":7.20458, "x":0.35013, "y":1.84856, "heading":1.54719, "vx":0.2979, "vy":1.46953, "omega":0.23174, "ax":5.77044, "ay":-1.51531, "alpha":3.98052, "fx":[83.27002,94.69108,99.21933,89.25965], "fy":[-41.91874,-35.77231,-8.66689,-9.86838]}, + {"t":7.23294, "x":0.3609, "y":1.88963, "heading":1.55376, "vx":0.46158, "vy":1.42654, "omega":0.34465, "ax":5.57542, "ay":-2.16317, "alpha":4.63881, "fx":[76.92144,91.93409,99.05313,86.14687], "fy":[-55.4025,-46.44574,-15.83537,-19.68427]}, + {"t":7.26131, "x":0.37623, "y":1.92923, "heading":1.56354, "vx":0.61974, "vy":1.36518, "omega":0.47624, "ax":5.28874, "ay":-2.80626, "alpha":5.36758, "fx":[67.93461,88.08257,98.25475,81.57849], "fy":[-68.74704,-56.77491,-22.75281,-29.93084]}, + {"t":7.28968, "x":0.39594, "y":1.96682, "heading":1.57705, "vx":0.76975, "vy":1.28558, "omega":0.62849, "ax":4.95227, "ay":-3.34349, "alpha":5.762, "fx":[59.79005,83.35888,95.74983,75.58476], "fy":[-78.47898,-64.99498,-29.9967,-38.85071]}, + {"t":7.31943, "x":0.42103, "y":2.00359, "heading":1.59575, "vx":0.9171, "vy":1.1861, "omega":0.79993, "ax":4.50789, "ay":-3.93016, "alpha":5.18327, "fx":[53.20704,77.09196,89.60773,66.35754], "fy":[-83.68057,-71.31242,-41.23848,-53.34508]}, + {"t":7.34918, "x":0.45032, "y":2.03714, "heading":1.61955, "vx":1.05122, "vy":1.06917, "omega":0.95414, "ax":3.9997, "ay":-4.42511, "alpha":4.89457, "fx":[45.1827,70.18721,82.3927,56.23001], "fy":[-88.89615,-77.25711,-50.96802,-63.88614]}, + {"t":7.37893, "x":0.48336, "y":2.06699, "heading":1.64793, "vx":1.17022, "vy":0.93751, "omega":1.09977, "ax":3.42266, "ay":-4.82613, "alpha":4.89529, "fx":[36.92365,63.00056,74.66468,42.75982], "fy":[-93.33539,-82.49392,-58.81418,-71.82983]}, + {"t":7.40869, "x":0.51969, "y":2.09275, "heading":1.68065, "vx":1.27205, "vy":0.79392, "omega":1.24542, "ax":2.66539, "ay":-4.84214, "alpha":5.93305, "fx":[24.90715,55.79226,63.82534,24.73535], "fy":[-95.61834,-85.43155,-58.17799,-68.26244]}, + {"t":7.43844, "x":0.55872, "y":2.11423, "heading":1.71771, "vx":1.35136, "vy":0.64986, "omega":1.42194, "ax":0.95564, "ay":-2.10984, "alpha":7.714, "fx":[-1.44216,35.53093,33.98154,-7.3845], "fy":[-57.50359,-50.04265,-9.34242,-17.09209]}, + {"t":7.46819, "x":0.59935, "y":2.13263, "heading":1.76002, "vx":1.37979, "vy":0.58708, "omega":1.65145, "ax":0.22438, "ay":-0.53598, "alpha":7.80413, "fx":[-10.82652,25.91376,19.42335,-20.26165], "fy":[-33.28747,-25.22067,16.3452,8.12671]}, + {"t":7.49794, "x":0.6405, "y":2.14986, "heading":1.80915, "vx":1.38647, "vy":0.57114, "omega":1.88365, "ax":0.04085, "ay":-0.09944, "alpha":7.04782, "fx":[-11.41717,21.68559,12.86476,-20.53892], "fy":[-24.88784,-16.1266,21.87563,12.82417]}, + {"t":7.5277, "x":0.68177, "y":2.16681, "heading":1.86519, "vx":1.38768, "vy":0.56818, "omega":2.09334, "ax":-0.05119, "ay":0.12456, "alpha":6.43495, "fx":[-10.2246,19.51414,8.9072,-21.4476], "fy":[-19.61314,-10.13833,23.67947,13.98211]}, + {"t":7.55745, "x":0.72304, "y":2.18377, "heading":1.92748, "vx":1.38616, "vy":0.57188, "omega":2.2848, "ax":-0.01823, "ay":0.04411, "alpha":5.52809, "fx":[-7.45041,17.77002,6.57259,-18.04983], "fy":[-18.45384,-8.8395,20.08882,10.00537]}, + {"t":7.5872, "x":0.76427, "y":2.2008, "heading":1.99546, "vx":1.38562, "vy":0.57319, "omega":2.44927, "ax":-0.02039, "ay":0.04924, "alpha":4.85595, "fx":[-5.20684,16.19609,4.61378,-16.89796], "fy":[-16.33908,-6.25348,17.91807,7.80158]}, + {"t":7.61696, "x":0.80549, "y":2.21788, "heading":2.06833, "vx":1.38501, "vy":0.57466, "omega":2.59375, "ax":0.00241, "ay":-0.00582, "alpha":3.99452, "fx":[-3.12934,14.01544,2.89276,-13.62589], "fy":[-14.4325,-5.13681,14.48815,4.71139]}, + {"t":7.64671, "x":0.8467, "y":2.23497, "heading":2.1455, "vx":1.38508, "vy":0.57449, "omega":2.7126, "ax":0.00071, "ay":-0.00171, "alpha":3.34283, "fx":[-1.53876,12.00543,1.56712,-11.98859], "fy":[-12.1829,-3.1324,12.11408,3.09268]}, + {"t":7.67646, "x":0.88791, "y":2.25207, "heading":2.22621, "vx":1.3851, "vy":0.57444, "omega":2.81205, "ax":0.00743, "ay":-0.01792, "alpha":2.54237, "fx":[-0.43431,9.42278,0.43938,-8.95616], "fy":[-9.57395,-2.08621,9.18977,1.33239]}, + {"t":7.70621, "x":0.92912, "y":2.26915, "heading":2.30987, "vx":1.38532, "vy":0.5739, "omega":2.8877, "ax":0.00266, "ay":-0.00643, "alpha":1.89897, "fx":[0.29848,7.05884,-0.21372,-6.97445], "fy":[-7.04307,-0.78511,6.84251,0.57706]}, + {"t":7.73597, "x":0.97034, "y":2.28622, "heading":2.39579, "vx":1.3854, "vy":0.57371, "omega":2.9442, "ax":0.00394, "ay":-0.00951, "alpha":1.15771, "fx":[0.52376,4.37808,-0.52644,-4.12526], "fy":[-4.3503,-0.26223,4.14712,-0.1385]}, + {"t":7.76572, "x":1.01156, "y":2.30329, "heading":2.48339, "vx":1.38552, "vy":0.57343, "omega":2.97864, "ax":-0.00006, "ay":0.00013, "alpha":0.49878, "fx":[0.37675,1.85146,-0.37854,-1.8532], "fy":[-1.77257,0.11207,1.78869,-0.11992]}, + {"t":7.79547, "x":1.05278, "y":2.32035, "heading":2.57201, "vx":1.38552, "vy":0.57343, "omega":2.99348, "ax":0.00103, "ay":-0.00247, "alpha":-0.19518, "fx":[-0.20044,-0.69098,0.21133,0.74521], "fy":[0.64488,-0.15598,-0.71718,0.07113]}, + {"t":7.82522, "x":1.09401, "y":2.33741, "heading":2.66107, "vx":1.38555, "vy":0.57336, "omega":2.98767, "ax":-0.00146, "ay":0.00352, "alpha":-0.88864, "fx":[-1.24123,-3.21293,1.19493,3.16664], "fy":[3.08,-0.71632,-2.98066,0.84062]}, + {"t":7.85498, "x":1.13523, "y":2.35447, "heading":2.74996, "vx":1.38551, "vy":0.57346, "omega":2.96124, "ax":0.00041, "ay":-0.001, "alpha":-1.55693, "fx":[-2.56198,-5.4482,2.65219,5.38426], "fy":[5.05888,-1.86467,-5.16724,1.90962]}, + {"t":7.88473, "x":1.17645, "y":2.37153, "heading":2.83807, "vx":1.38552, "vy":0.57343, "omega":2.91491, "ax":-0.00125, "ay":0.00301, "alpha":-2.29583, "fx":[-4.47649,-7.8069,4.4369,7.76738], "fy":[7.1909,-3.24745,-7.16887,3.41655]}, + {"t":7.91448, "x":1.21767, "y":2.38859, "heading":2.9248, "vx":1.38548, "vy":0.57352, "omega":2.84661, "ax":0.00097, "ay":-0.00235, "alpha":-2.96743, "fx":[-6.46551,-9.60997,6.65808,9.47906], "fy":[8.65777,-5.27387,-8.87302,5.34014]}, + {"t":7.94423, "x":1.2589, "y":2.40566, "heading":3.00949, "vx":1.38551, "vy":0.57345, "omega":2.75832, "ax":-0.00032, "ay":0.00078, "alpha":-3.75618, "fx":[-9.17785,-11.65536,9.16754,11.64524], "fy":[10.33173,-7.33335,-10.47134,7.52237]}, + {"t":7.97399, "x":1.30012, "y":2.42272, "heading":3.09156, "vx":1.3855, "vy":0.57348, "omega":2.64656, "ax":0.00025, "ay":-0.00061, "alpha":-4.46012, "fx":[-11.74824,-12.95726,11.99911,12.72228], "fy":[11.30442,-10.12368,-11.51426,10.29502]}, + {"t":8.00374, "x":1.34134, "y":2.43978, "heading":-3.11289, "vx":1.38551, "vy":0.57346, "omega":2.51386, "ax":0.00053, "ay":-0.00128, "alpha":-5.30236, "fx":[-15.12137,-14.55808,15.13235,14.5808], "fy":[12.36511,-12.77024,-12.67115,12.99498]}, + {"t":8.03349, "x":1.38256, "y":2.45684, "heading":-3.03809, "vx":1.38552, "vy":0.57342, "omega":2.3561, "ax":-0.00586, "ay":0.01415, "alpha":-6.05597, "fx":[-18.18901,-15.3714,18.36734,14.82097], "fy":[13.00447,-15.9982,-12.81361,16.70593]}, + {"t":8.06325, "x":1.42378, "y":2.47391, "heading":-2.96799, "vx":1.38535, "vy":0.57384, "omega":2.17592, "ax":0.0048, "ay":-0.0116, "alpha":-6.95464, "fx":[-21.95525,-16.33181,22.05007,16.54197], "fy":[13.11537,-19.43342,-13.85823,19.43974]}, + {"t":8.093, "x":1.465, "y":2.49098, "heading":-2.90325, "vx":1.38549, "vy":0.5735, "omega":1.969, "ax":-0.01435, "ay":0.03464, "alpha":-7.76373, "fx":[-25.41131,-16.77222,25.38395,15.88818], "fy":[13.71632,-22.66413,-12.97284,24.12049]}, + {"t":8.12275, "x":1.50622, "y":2.50806, "heading":-2.84467, "vx":1.38506, "vy":0.57453, "omega":1.73801, "ax":0.02723, "ay":-0.06616, "alpha":-8.66959, "fx":[-28.92774,-16.66327,29.61416,17.70601], "fy":[12.06532,-27.32493,-14.85326,25.91127]}, + {"t":8.1525, "x":1.54744, "y":2.52512, "heading":-2.79296, "vx":1.38587, "vy":0.57256, "omega":1.48006, "ax":-2.27256, "ay":-0.9032, "alpha":-8.20493, "fx":[-62.8205,-49.03581,-7.99712,-24.4605], "fy":[-2.64724,-40.0596,-29.63535,14.98653]}, + {"t":8.18226, "x":1.58767, "y":2.54176, "heading":-2.74892, "vx":1.31826, "vy":0.54569, "omega":1.23595, "ax":-5.52802, "ay":-2.28763, "alpha":-4.02917, "fx":[-98.43581,-88.08186,-75.64121,-88.88649], "fy":[-26.29146,-51.41446,-50.19642,-17.36916]}, + {"t":8.21201, "x":1.62444, "y":2.55698, "heading":-2.71215, "vx":1.15379, "vy":0.47762, "omega":1.11607, "ax":-5.53586, "ay":-2.29156, "alpha":-4.28308, "fx":[-99.25872,-87.84537,-74.91246,-89.52687], "fy":[-26.21938,-52.73541,-50.61593,-15.95022]}, + {"t":8.24176, "x":1.65632, "y":2.57017, "heading":-2.67894, "vx":0.98908, "vy":0.40944, "omega":0.98863, "ax":-5.5385, "ay":-2.29272, "alpha":-4.61985, "fx":[-100.04449,-87.46567,-74.02799,-90.17283], "fy":[-26.36938,-54.46194,-51.07461,-13.68875]}, + {"t":8.27151, "x":1.6833, "y":2.58134, "heading":-2.64953, "vx":0.82429, "vy":0.34123, "omega":0.85118, "ax":-5.53982, "ay":-2.29329, "alpha":-4.98094, "fx":[-100.9047,-87.11382,-72.96783,-90.80827], "fy":[-26.29722,-56.12365,-51.75085,-11.459]}, + {"t":8.30127, "x":1.70537, "y":2.59048, "heading":-2.6242, "vx":0.65947, "vy":0.273, "omega":0.70298, "ax":-5.54061, "ay":-2.29361, "alpha":-5.34443, "fx":[-101.7124,-86.75952,-71.98928,-91.38401], "fy":[-26.43188,-57.79783,-52.34277,-9.07851]}, + {"t":8.33102, "x":1.72254, "y":2.59759, "heading":-2.60329, "vx":0.49462, "vy":0.20476, "omega":0.54397, "ax":-5.54114, "ay":-2.29383, "alpha":-5.72169, "fx":[-102.56809,-86.43913,-70.8849,-91.98657], "fy":[-26.35471,-59.42309,-53.13676,-6.75039]}, + {"t":8.36077, "x":1.7348, "y":2.60266, "heading":-2.5871, "vx":0.32976, "vy":0.13651, "omega":0.37374, "ax":-5.54152, "ay":-2.29398, "alpha":-6.0895, "fx":[-103.3764,-86.15357,-69.84419,-92.52867], "fy":[-26.36485,-60.97657,-53.91272,-4.42012]}, + {"t":8.39053, "x":1.74216, "y":2.60571, "heading":-2.57598, "vx":0.16488, "vy":0.06826, "omega":0.19256, "ax":-5.5418, "ay":-2.29411, "alpha":-6.47197, "fx":[-104.23079,-85.99826,-68.65503,-93.03636], "fy":[-26.15725,-62.42145,-54.99635,-2.10736]}, + {"t":8.42028, "x":1.74461, "y":2.60672, "heading":-2.57026, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[ + {"name":"IntakeDown", "from":{"target":1, "targetTimestamp":0.83392, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeDown"}}}, + {"name":"IntakeUp", "from":{"target":6, "targetTimestamp":7.28968, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeUp"}}}] +} diff --git a/src/main/deploy/choreo/OutpostStartPush.traj b/src/main/deploy/choreo/OutpostStartPush.traj new file mode 100644 index 0000000..9d9b3f5 --- /dev/null +++ b/src/main/deploy/choreo/OutpostStartPush.traj @@ -0,0 +1,360 @@ +{ + "name":"OutpostStartPush", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.574246883392334, "y":3.9598469734191895, "heading":-3.1230766652301596, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.3966126441955566, "y":3.047424077987671, "heading":-1.901270611327069, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.145883560180664, "y":0.6102350950241089, "heading":3.141592653589793, "intervals":65, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.5819629430770874, "y":0.3701103627681732, "heading":3.141592653589793, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.5275728702545166, "y":1.3532100915908811, "heading":3.135845623968652, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.2776426076889038, "y":0.5162930488586426, "heading":1.5707963267948966, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.40682798624038696, "y":1.3462082147598269, "heading":1.5874614232311806, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.39594003558158875, "y":1.9668220281600952, "heading":1.5770462432899754, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.7446147203445437, "y":2.606724500656128, "heading":-2.57025541936111, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":3.0}}, "enabled":true}, + {"from":0, "to":8, "data":{"type":"MaxAcceleration", "props":{"max":7.0}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":0.5}}, "enabled":true}, + {"from":0, "to":7, "data":{"type":"MaxAngularVelocity", "props":{"max":2.0}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":8, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":8, "to":8, "data":{"type":"MaxAngularVelocity", "props":{"max":10.0}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.574246883392334 m", "val":3.574246883392334}, "y":{"exp":"3.9598469734191895 m", "val":3.9598469734191895}, "heading":{"exp":"-3.1230766652301596 rad", "val":-3.1230766652301596}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.3966126441955566 m", "val":2.3966126441955566}, "y":{"exp":"3.047424077987671 m", "val":3.047424077987671}, "heading":{"exp":"-1.901270611327069 rad", "val":-1.901270611327069}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.145883560180664 m", "val":2.145883560180664}, "y":{"exp":"0.6102350950241089 m", "val":0.6102350950241089}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":65, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.5819629430770874 m", "val":0.5819629430770874}, "y":{"exp":"0.3701103627681732 m", "val":0.3701103627681732}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.5275728702545166 m", "val":0.5275728702545166}, "y":{"exp":"1.3532100915908813 m", "val":1.3532100915908811}, "heading":{"exp":"3.135845623968652 rad", "val":3.135845623968652}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.2776426076889038 m", "val":1.2776426076889038}, "y":{"exp":"0.5162930488586426 m", "val":0.5162930488586426}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.40682798624038696 m", "val":0.40682798624038696}, "y":{"exp":"1.3462082147598267 m", "val":1.3462082147598269}, "heading":{"exp":"1.5874614232311806 rad", "val":1.5874614232311806}, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.39594003558158875 m", "val":0.39594003558158875}, "y":{"exp":"1.9668220281600952 m", "val":1.9668220281600952}, "heading":{"exp":"1.5770462432899754 rad", "val":1.5770462432899754}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.7446147203445435 m", "val":1.7446147203445437}, "y":{"exp":"2.606724500656128 m", "val":2.606724500656128}, "heading":{"exp":"-2.5702554193611094 rad", "val":-2.57025541936111}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3 m / s", "val":3.0}}}, "enabled":true}, + {"from":0, "to":8, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"7 m / s ^ 2", "val":7.0}}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.5 m / s", "val":0.5}}}, "enabled":true}, + {"from":0, "to":7, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"2 rad / s", "val":2.0}}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":8, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":8, "to":8, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"10 rad / s", "val":10.0}}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.77004,1.80638,5.0515,5.82162,6.66853,7.41157,7.73529,8.63866], + "samples":[ + {"t":0.0, "x":3.57425, "y":3.95985, "heading":-3.12308, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.91622, "ay":-1.03522, "alpha":12.63027, "fx":[-82.6334,-108.50717,-127.72522,-120.33423], "fy":[-93.12783,54.68845,21.1791,-48.47912]}, + {"t":0.022, "x":3.57257, "y":3.9596, "heading":-3.12308, "vx":-0.15216, "vy":-0.02278, "omega":0.27788, "ax":-6.90665, "ay":-1.11737, "alpha":12.16752, "fx":[-82.67699,-109.1351,-127.24006,-119.54049], "fy":[-91.53772,49.66318,19.537,-48.619]}, + {"t":0.044, "x":3.56755, "y":3.95882, "heading":-3.11696, "vx":-0.30412, "vy":-0.04736, "omega":0.54558, "ax":-6.89113, "ay":-1.20828, "alpha":11.61691, "fx":[-82.95143,-109.53731,-126.62636,-118.49185], "fy":[-89.29358,44.1267,17.31887,-48.88145]}, + {"t":0.066, "x":3.5592, "y":3.95749, "heading":-3.10496, "vx":-0.45573, "vy":-0.07394, "omega":0.80116, "ax":-6.8723, "ay":-1.30997, "alpha":10.90958, "fx":[-83.65825,-109.80226,-125.79365,-117.15711], "fy":[-86.05996,37.57575,14.44455,-49.14741]}, + {"t":0.088, "x":3.54751, "y":3.95555, "heading":-3.08733, "vx":-0.60693, "vy":-0.10276, "omega":1.04118, "ax":-6.84927, "ay":-1.42428, "alpha":9.97725, "fx":[-85.00604,-109.85454,-124.62016,-115.46816], "fy":[-81.44137,29.67242,10.62903,-49.30592]}, + {"t":0.11001, "x":3.53249, "y":3.95294, "heading":-3.06443, "vx":-0.75762, "vy":-0.1341, "omega":1.26069, "ax":-6.82082, "ay":-1.5536, "alpha":8.71926, "fx":[-87.16987,-109.65161,-122.89368,-113.42661], "fy":[-74.98672,19.61506,5.71022,-48.99697]}, + {"t":0.13201, "x":3.51418, "y":3.94961, "heading":-3.03669, "vx":-0.90768, "vy":-0.16828, "omega":1.45252, "ax":-6.78528, "ay":-1.70081, "alpha":7.06817, "fx":[-90.42712,-109.04594,-120.39934,-111.01265], "fy":[-66.385,7.55363,-1.11633,-48.05866]}, + {"t":0.15401, "x":3.49256, "y":3.9455, "heading":-3.00473, "vx":-1.05696, "vy":-0.2057, "omega":1.60803, "ax":-6.74028, "ay":-1.86982, "alpha":5.10404, "fx":[-94.36834,-108.10236,-116.99456,-108.56221], "fy":[-56.80018,-6.08883,-9.75216,-46.09806]}, + {"t":0.17601, "x":3.46768, "y":3.94052, "heading":-2.96935, "vx":-1.20526, "vy":-0.24684, "omega":1.72032, "ax":-6.68245, "ay":-2.06552, "alpha":3.1857, "fx":[-98.07589,-106.91899,-112.94688,-106.41364], "fy":[-48.774,-18.79687,-19.72917,-43.86672]}, + {"t":0.19801, "x":3.43954, "y":3.93459, "heading":-2.93151, "vx":-1.35228, "vy":-0.29228, "omega":1.79041, "ax":-6.60687, "ay":-2.2943, "alpha":1.7217, "fx":[-100.43519,-105.48933,-108.96237,-104.66845], "fy":[-44.6034,-29.18072,-29.12575,-42.78479]}, + {"t":0.22001, "x":3.40819, "y":3.92761, "heading":-2.89212, "vx":-1.49764, "vy":-0.34276, "omega":1.82829, "ax":-6.50623, "ay":-2.56418, "alpha":0.83498, "fx":[-101.013,-103.68793,-105.45643,-103.00742], "fy":[-44.5434,-37.32589,-37.11307,-43.85071]}, + {"t":0.24201, "x":3.37367, "y":3.91944, "heading":-2.85189, "vx":-1.64078, "vy":-0.39917, "omega":1.84666, "ax":-6.36953, "ay":-2.88534, "alpha":0.3758, "fx":[-100.02841,-101.36392,-102.18234,-100.90896], "fy":[-47.4605,-44.31322,-44.21474,-47.23931]}, + {"t":0.26401, "x":3.33603, "y":3.90996, "heading":-2.81126, "vx":-1.78092, "vy":-0.46265, "omega":1.85493, "ax":-6.17971, "ay":-3.27037, "alpha":0.16027, "fx":[-97.58878,-98.2261,-98.61191,-98.00276], "fy":[-52.61966,-51.32591,-51.24483,-52.48763]}, + {"t":0.28601, "x":3.29535, "y":3.89899, "heading":-2.77045, "vx":-1.91688, "vy":-0.5346, "omega":1.85846, "ax":-5.91012, "ay":-3.73365, "alpha":0.06624, "fx":[-93.59379,-93.90123,-94.04807,-93.76707], "fy":[-59.5468,-59.01928,-59.02343,-59.5083]}, + {"t":0.30801, "x":3.25175, "y":3.88633, "heading":-2.72956, "vx":-2.0469, "vy":-0.61675, "omega":1.85991, "ax":-5.51917, "ay":-4.28851, "alpha":0.02411, "fx":[-87.54458,-87.64594,-87.72033,-87.57277], "fy":[-68.15067,-68.00403,-67.98647,-68.19147]}, + {"t":0.33002, "x":3.20538, "y":3.87172, "heading":-2.68864, "vx":-2.16833, "vy":-0.7111, "omega":1.86044, "ax":-4.94425, "ay":-4.93832, "alpha":0.00792, "fx":[-78.45323,-78.55162,-78.49591,-78.47353], "fy":[-78.43362,-78.32607,-78.40489,-78.43295]}, + {"t":0.35202, "x":3.15648, "y":3.85488, "heading":-2.64771, "vx":-2.27711, "vy":-0.81975, "omega":1.86062, "ax":-4.10146, "ay":-5.65587, "alpha":-0.00385, "fx":[-65.22652,-65.13142,-65.13244,-64.96445], "fy":[-89.70652,-89.77962,-89.78073,-89.89718]}, + {"t":0.37402, "x":3.10538, "y":3.83548, "heading":-2.60678, "vx":-2.36735, "vy":-0.94418, "omega":1.86053, "ax":-2.91111, "ay":-6.34948, "alpha":-0.00393, "fx":[-46.21628,-46.30864,-46.15167,-46.18732], "fy":[-100.80472,-100.76131,-100.83226,-100.81239]}, + {"t":0.39602, "x":3.0526, "y":3.81317, "heading":-2.56584, "vx":-2.43139, "vy":-1.08388, "omega":1.86045, "ax":-1.37497, "ay":-6.84739, "alpha":-0.00722, "fx":[-21.90644,-21.8564,-21.82303,-21.72894], "fy":[-108.6919,-108.70747,-108.70863,-108.72143]}, + {"t":0.41802, "x":2.99877, "y":3.78766, "heading":-2.52491, "vx":-2.46164, "vy":-1.23453, "omega":1.86029, "ax":0.33546, "ay":-6.97598, "alpha":-0.00678, "fx":[5.27856,5.32427,5.36527,5.33469], "fy":[-110.75038,-110.75442,-110.74802,-110.74263]}, + {"t":0.44002, "x":2.94469, "y":3.75881, "heading":-2.48398, "vx":-2.45426, "vy":-1.38801, "omega":1.86014, "ax":1.93524, "ay":-6.7115, "alpha":-0.00188, "fx":[30.75073,30.69993,30.78095,30.66165], "fy":[-106.53694,-106.5653,-106.53726,-106.56033]}, + {"t":0.46202, "x":2.89116, "y":3.72665, "heading":-2.44306, "vx":-2.41169, "vy":-1.53566, "omega":1.8601, "ax":3.22394, "ay":-6.19803, "alpha":-0.00467, "fx":[51.11636,51.22507,51.20146,51.18678], "fy":[-98.42965,-98.3803,-98.39295,-98.39012]}, + {"t":0.48402, "x":2.83889, "y":3.69137, "heading":-2.40213, "vx":-2.34076, "vy":-1.67203, "omega":1.86, "ax":4.17085, "ay":-5.6067, "alpha":-0.00196, "fx":[66.22667,66.20174,66.26766,66.16515], "fy":[-88.99519,-89.02961,-88.97893,-89.03799]}, + {"t":0.50602, "x":2.7884, "y":3.65322, "heading":-2.36121, "vx":-2.24899, "vy":-1.79538, "omega":1.85995, "ax":4.84091, "ay":-5.0415, "alpha":-0.00752, "fx":[76.8228,76.83905,76.8791,76.87075], "fy":[-80.05694,-80.06069,-80.02207,-80.01041]}, + {"t":0.52802, "x":2.74009, "y":3.6125, "heading":-2.32029, "vx":-2.14249, "vy":-1.9063, "omega":1.85979, "ax":5.31347, "ay":-4.54262, "alpha":-0.01274, "fx":[84.30713,84.33676,84.41895,84.35827], "fy":[-72.1531,-72.15926,-72.0651,-72.09236]}, + {"t":0.55003, "x":2.69424, "y":3.56946, "heading":-2.27937, "vx":-2.02559, "vy":-2.00624, "omega":1.85951, "ax":5.6513, "ay":-4.11652, "alpha":-0.0379, "fx":[89.6327,89.6133,89.83055,89.79769], "fy":[-65.41931,-65.54134,-65.24924,-65.20119]}, + {"t":0.57203, "x":2.65104, "y":3.52433, "heading":-2.23846, "vx":-1.90125, "vy":-2.09681, "omega":1.85867, "ax":5.89774, "ay":-3.75656, "alpha":-0.09156, "fx":[93.35048,93.47685,93.90259,93.79397], "fy":[-59.94614,-59.98667,-59.34616,-59.27334]}, + {"t":0.59403, "x":2.61064, "y":3.47729, "heading":-2.19757, "vx":-1.7715, "vy":-2.17946, "omega":1.85666, "ax":6.08151, "ay":-3.4526, "alpha":-0.24292, "fx":[95.95962,96.07516,97.16978,96.98897], "fy":[-55.48857,-55.93052,-54.07855,-53.75281]}, + {"t":0.61603, "x":2.57313, "y":3.4285, "heading":-2.15672, "vx":-1.6377, "vy":-2.25542, "omega":1.85131, "ax":6.2215, "ay":-3.1948, "alpha":-0.59782, "fx":[97.23745,97.85851,100.22402,99.76322], "fy":[-52.74472,-53.23893,-48.85915,-48.03605]}, + {"t":0.63803, "x":2.53861, "y":3.37811, "heading":-2.11599, "vx":-1.50082, "vy":-2.32571, "omega":1.83816, "ax":6.33028, "ay":-2.97465, "alpha":-1.3898, "fx":[97.18338,98.54515,103.67446,102.58812], "fy":[-51.85743,-53.28809,-42.94946,-40.80401]}, + {"t":0.66003, "x":2.50712, "y":3.32622, "heading":-2.07555, "vx":-1.36155, "vy":-2.39115, "omega":1.80758, "ax":6.41634, "ay":-2.78527, "alpha":-2.87492, "fx":[94.9013,98.66323,108.07748,105.81409], "fy":[-54.79537,-56.0175,-35.56848,-30.49166]}, + {"t":0.68203, "x":2.47872, "y":3.27294, "heading":-2.03578, "vx":-1.22038, "vy":-2.45243, "omega":1.74433, "ax":6.48553, "ay":-2.62112, "alpha":-5.03346, "fx":[90.81737,98.54735,113.21258,109.27288], "fy":[-61.40022,-61.27198,-27.10384,-16.6729]}, + {"t":0.70403, "x":2.45344, "y":3.21835, "heading":-1.9974, "vx":-1.07769, "vy":-2.5101, "omega":1.63359, "ax":6.54196, "ay":-2.47785, "alpha":-7.38215, "fx":[85.83579,99.23757,118.06268,112.29739], "fy":[-70.67846,-66.13395,-18.97792,-1.56034]}, + {"t":0.72603, "x":2.43131, "y":3.16252, "heading":-1.96146, "vx":-0.93376, "vy":-2.56461, "omega":1.47118, "ax":6.5886, "ay":-2.35187, "alpha":-9.3815, "fx":[81.72502,100.55154,121.82528,114.29337], "fy":[-79.31491,-69.4842,-12.32814,11.77683]}, + {"t":0.74803, "x":2.41236, "y":3.10553, "heading":-1.9291, "vx":-0.78881, "vy":-2.61636, "omega":1.26477, "ax":6.6276, "ay":-2.24036, "alpha":-10.88939, "fx":[78.81192,102.19437,124.48773,115.37831], "fy":[-86.15872,-71.20552,-7.1185,22.21326]}, + {"t":0.77004, "x":2.39661, "y":3.04742, "heading":-1.90127, "vx":-0.64299, "vy":-2.66565, "omega":1.0252, "ax":6.5916, "ay":-2.33022, "alpha":-10.71721, "fx":[77.46289,101.95971,124.02236,115.14107], "fy":[-87.42715,-71.11568,-7.88008,18.44729]}, + {"t":0.79594, "x":2.38217, "y":2.97758, "heading":-1.87471, "vx":-0.47221, "vy":-2.72602, "omega":0.74753, "ax":6.1613, "ay":-3.29709, "alpha":-9.63389, "fx":[64.92885,93.99537,120.12252,112.21401], "fy":[-96.93857,-79.53933,-22.79564,-10.10108]}, + {"t":0.82185, "x":2.372, "y":2.90585, "heading":-1.85534, "vx":-0.31258, "vy":-2.81144, "omega":0.49793, "ax":1.52384, "ay":-6.71059, "alpha":-7.53324, "fx":[-17.35812,31.04992,70.83323,12.24331], "fy":[-117.57375,-117.00426,-89.8745,-101.68944]}, + {"t":0.84776, "x":2.36441, "y":2.83075, "heading":-1.84244, "vx":-0.2731, "vy":-2.9853, "omega":0.30276, "ax":1.10202, "ay":-0.13719, "alpha":-22.29252, "fx":[-51.04177,53.21591,86.56677,-18.7593], "fy":[-52.47528,-70.53026,36.89294,77.4006]}, + {"t":0.87367, "x":2.35771, "y":2.75336, "heading":-1.8346, "vx":-0.24455, "vy":-2.98886, "omega":-0.27481, "ax":-0.04914, "ay":0.00404, "alpha":-19.32765, "fx":[-60.26592,29.7612,60.01021,-32.62634], "fy":[-39.02381,-63.26769,38.41297,64.13528]}, + {"t":0.89958, "x":2.35135, "y":2.67593, "heading":-1.84172, "vx":-0.24582, "vy":-2.98875, "omega":-0.77556, "ax":-0.3993, "ay":0.03355, "alpha":-14.37947, "fx":[-50.92815,15.64189,39.2412,-29.31139], "fy":[-27.70678,-47.35413,29.38381,47.80773]}, + {"t":0.92549, "x":2.34485, "y":2.5985, "heading":-1.86181, "vx":-0.25617, "vy":-2.98788, "omega":-1.14811, "ax":-0.42892, "ay":0.03758, "alpha":-9.41541, "fx":[-36.48628,6.8287,23.37526,-20.95546], "fy":[-17.34396,-31.0276,18.72245,32.03573]}, + {"t":0.95139, "x":2.33807, "y":2.5211, "heading":-1.89156, "vx":-0.26728, "vy":-2.98691, "omega":-1.39205, "ax":-0.35052, "ay":0.03191, "alpha":-5.38332, "fx":[-22.87098,1.65562,11.91185,-12.95537], "fy":[-9.19014,-17.87572,10.24735,18.84461]}, + {"t":0.9773, "x":2.33103, "y":2.44373, "heading":-1.92762, "vx":-0.27636, "vy":-2.98608, "omega":-1.53153, "ax":-0.26091, "ay":0.02445, "alpha":-2.72615, "fx":[-13.04332,-0.88116,4.80177,-7.44561], "fy":[-4.0761,-9.22076,4.97924,9.87]}, + {"t":1.00321, "x":2.32378, "y":2.36637, "heading":-1.9673, "vx":-0.28312, "vy":-2.98545, "omega":-1.60216, "ax":-0.1918, "ay":0.01835, "alpha":-1.23414, "fx":[-7.09141,-1.79907,1.00978,-4.29889], "fy":[-1.43238,-4.30089,2.21995,4.67859]}, + {"t":1.02912, "x":2.31638, "y":2.28903, "heading":-2.00881, "vx":-0.28809, "vy":-2.98497, "omega":-1.63413, "ax":-0.14483, "ay":0.01407, "alpha":-0.50775, "fx":[-3.92558,-1.91868,-0.67179,-2.68134], "fy":[-0.28363,-1.86684,0.96479,2.07919]}, + {"t":1.05503, "x":2.30887, "y":2.2117, "heading":-2.05115, "vx":-0.29185, "vy":-2.98461, "omega":-1.64729, "ax":-0.1134, "ay":0.01114, "alpha":-0.18612, "fx":[-2.35211,-1.73499,-1.24819,-1.86578], "fy":[0.12034,-0.73103,0.43375,0.88466]}, + {"t":1.08094, "x":2.30127, "y":2.13438, "heading":-2.09383, "vx":-0.29478, "vy":-2.98432, "omega":-1.65211, "ax":-0.09133, "ay":0.00906, "alpha":-0.04725, "fx":[-1.54383,-1.4835,-1.35583,-1.41629], "fy":[0.22917,-0.20563,0.20867,0.34296]}, + {"t":1.10685, "x":2.2936, "y":2.05706, "heading":-2.13663, "vx":-0.29715, "vy":-2.98409, "omega":-1.65333, "ax":-0.07641, "ay":0.00764, "alpha":-0.00002, "fx":[-1.1904,-1.25776,-1.23579,-1.16861], "fy":[0.18601,0.05703,0.10307,0.13874]}, + {"t":1.13275, "x":2.28588, "y":1.97975, "heading":-2.17947, "vx":-0.29913, "vy":-2.98389, "omega":-1.65333, "ax":-0.06262, "ay":0.00629, "alpha":0.02434, "fx":[-0.90823,-1.03393,-1.07987,-0.95425], "fy":[0.14815,0.18115,0.05007,0.02036]}, + {"t":1.15866, "x":2.2781, "y":1.90244, "heading":-2.2223, "vx":-0.30075, "vy":-2.98372, "omega":-1.6527, "ax":-0.05129, "ay":0.00518, "alpha":0.02009, "fx":[-0.76855,-0.84174,-0.86009,-0.78696], "fy":[0.08966,0.20284,0.02375,0.01283]}, + {"t":1.18457, "x":2.2703, "y":1.82514, "heading":-2.26512, "vx":-0.30208, "vy":-2.98359, "omega":-1.65218, "ax":-0.04151, "ay":0.00421, "alpha":0.02436, "fx":[-0.60571,-0.67718,-0.71223,-0.64077], "fy":[0.0607,0.22166,0.00811,-0.02309]}, + {"t":1.21048, "x":2.26246, "y":1.74784, "heading":-2.30793, "vx":-0.30316, "vy":-2.98348, "omega":-1.65155, "ax":-0.03251, "ay":0.00331, "alpha":0.0122, "fx":[-0.52019,-0.52962,-0.51207,-0.50265], "fy":[0.02198,0.18978,-0.00631,0.00464]}, + {"t":1.23639, "x":2.25459, "y":1.67054, "heading":-2.35072, "vx":-0.304, "vy":-2.9834, "omega":-1.65123, "ax":-0.02283, "ay":0.00233, "alpha":0.01246, "fx":[-0.38325,-0.372,-0.34178,-0.35303], "fy":[-0.01585,0.21057,-0.03188,-0.01492]}, + {"t":1.2623, "x":2.24671, "y":1.59325, "heading":-2.3935, "vx":-0.30459, "vy":-2.98334, "omega":-1.65091, "ax":-0.01019, "ay":0.00104, "alpha":0.00201, "fx":[-0.2408,-0.17614,-0.08287,-0.1475], "fy":[-0.05864,0.19192,-0.06786,0.0007]}, + {"t":1.28821, "x":2.23881, "y":1.51596, "heading":-2.43627, "vx":-0.30485, "vy":-2.98331, "omega":-1.65086, "ax":0.00611, "ay":-0.00063, "alpha":0.00406, "fx":[0.00277,0.08109,0.19114,0.11294], "fy":[-0.10846,0.21985,-0.11515,-0.03596]}, + {"t":1.31411, "x":2.23091, "y":1.43866, "heading":-2.47904, "vx":-0.3047, "vy":-2.98332, "omega":-1.65075, "ax":0.0072, "ay":-0.00074, "alpha":0.01941, "fx":[0.02374,0.09717,0.20447,0.13162], "fy":[-0.16337,0.38501,-0.17155,-0.09718]}, + {"t":1.34002, "x":2.22302, "y":1.36137, "heading":-2.52181, "vx":-0.30451, "vy":-2.98334, "omega":-1.65025, "ax":-0.1015, "ay":0.01204, "alpha":0.10666, "fx":[-1.56901,-1.59812,-1.65432,-1.62441], "fy":[-0.17353,1.3064,-0.15432,-0.21398]}, + {"t":1.36593, "x":2.2151, "y":1.28408, "heading":-2.56457, "vx":-0.30714, "vy":-2.98303, "omega":-1.64749, "ax":0.43159, "ay":3.87892, "alpha":0.08697, "fx":[7.2095,6.95282,6.4849,6.7597], "fy":[61.5086,61.86021,61.61102,61.34319]}, + {"t":1.39184, "x":2.20729, "y":1.2081, "heading":-2.60725, "vx":-0.29596, "vy":-2.88253, "omega":-1.64523, "ax":0.70873, "ay":6.91543, "alpha":0.08341, "fx":[11.7443,11.40627,10.73425,11.12191], "fy":[109.73101,109.85128,109.84888,109.71884]}, + {"t":1.41775, "x":2.19986, "y":1.13573, "heading":-2.64988, "vx":-0.27759, "vy":-2.70337, "omega":-1.64307, "ax":0.71174, "ay":6.94033, "alpha":0.16153, "fx":[12.25408,11.57722,10.33931,11.02722], "fy":[110.06222,110.32556,110.30285,110.04049]}, + {"t":1.44366, "x":2.1929, "y":1.06802, "heading":-2.69245, "vx":-0.25915, "vy":-2.52355, "omega":-1.63889, "ax":0.71286, "ay":6.94822, "alpha":0.28357, "fx":[12.97202,11.85538,9.66583,10.7753], "fy":[110.08334,110.55546,110.52756,110.06589]}, + {"t":1.46957, "x":2.18643, "y":1.00497, "heading":-2.73491, "vx":-0.24069, "vy":-2.34353, "omega":-1.63154, "ax":0.71348, "ay":6.95208, "alpha":0.46649, "fx":[14.02156,12.28747,8.66721,10.33194], "fy":[109.97722,110.76693,110.74329,109.99013]}, + {"t":1.49547, "x":2.18043, "y":0.94659, "heading":-2.77718, "vx":-0.2222, "vy":-2.16342, "omega":-1.61946, "ax":0.7139, "ay":6.95437, "alpha":0.73632, "fx":[15.52441,13.00218,7.22463,9.58361], "fy":[109.75448,111.00767,111.01553,109.84519]}, + {"t":1.52138, "x":2.17492, "y":0.89287, "heading":-2.81913, "vx":-0.2037, "vy":-1.98324, "omega":-1.60038, "ax":0.71421, "ay":6.95588, "alpha":1.12417, "fx":[17.65829,14.07626,5.22765,8.39225], "fy":[109.38138,111.3175,111.38604,109.63396]}, + {"t":1.54729, "x":2.16988, "y":0.84382, "heading":-2.8606, "vx":-0.1852, "vy":-1.80302, "omega":-1.57125, "ax":0.71445, "ay":6.95695, "alpha":1.66394, "fx":[20.59422,15.64216,2.57671,6.55638], "fy":[108.80984,111.71879,111.89466,109.3637]}, + {"t":1.5732, "x":2.16532, "y":0.79945, "heading":-2.90131, "vx":-0.16669, "vy":-1.62278, "omega":-1.52814, "ax":0.71463, "ay":6.95776, "alpha":2.38166, "fx":[24.4652,17.80441,-0.73885,3.85032], "fy":[107.98745,112.22351,112.57312,109.05377]}, + {"t":1.59911, "x":2.16124, "y":0.75974, "heading":-2.9409, "vx":-0.14817, "vy":-1.44251, "omega":-1.46644, "ax":0.71477, "ay":6.95838, "alpha":3.28202, "fx":[29.31427,20.56294,-4.57373,0.08628], "fy":[106.87253,112.82995,113.4364,108.73843]}, + {"t":1.62502, "x":2.15764, "y":0.7247, "heading":-2.97889, "vx":-0.12966, "vy":-1.26223, "omega":-1.38141, "ax":0.71486, "ay":6.95887, "alpha":4.33789, "fx":[34.99445,23.83726,-8.62766,-4.80833], "fy":[105.46807,113.50476,114.47557,108.46048]}, + {"t":1.65092, "x":2.15452, "y":0.69433, "heading":-3.01468, "vx":-0.11114, "vy":-1.08194, "omega":-1.26902, "ax":0.71491, "ay":6.95928, "alpha":5.49043, "fx":[41.19579,27.41017,-12.51645,-10.69035], "fy":[103.83015,114.20073,115.65537,108.24854]}, + {"t":1.67683, "x":2.15188, "y":0.66864, "heading":-3.04756, "vx":-0.09261, "vy":-0.90163, "omega":-1.12677, "ax":0.71494, "ay":6.95962, "alpha":6.66395, "fx":[47.50009,31.01729,-15.92244,-17.19441], "fy":[102.06775,114.86877,116.91842,108.10152]}, + {"t":1.70274, "x":2.14972, "y":0.64761, "heading":-3.07675, "vx":-0.07409, "vy":-0.72132, "omega":-0.95412, "ax":0.71494, "ay":6.95991, "alpha":7.78771, "fx":[53.49769,34.44579,-18.70971,-23.83324], "fy":[100.31961,115.46928,118.19347,107.9925]}, + {"t":1.72865, "x":2.14804, "y":0.63126, "heading":-3.10147, "vx":-0.05557, "vy":-0.541, "omega":-0.75235, "ax":0.71493, "ay":6.96016, "alpha":8.81389, "fx":[58.97123,37.48708,-20.89304,-30.16522], "fy":[98.67483,116.00959,119.41614,107.89012]}, + {"t":1.75456, "x":2.14684, "y":0.61958, "heading":-3.12097, "vx":-0.03704, "vy":-0.36067, "omega":-0.52399, "ax":0.71492, "ay":6.96038, "alpha":9.71998, "fx":[63.81485,40.08317,-22.61352,-35.88513], "fy":[97.19836,116.49671,120.53315,107.77617]}, + {"t":1.78047, "x":2.14612, "y":0.61257, "heading":-3.13454, "vx":-0.01852, "vy":-0.18034, "omega":-0.27216, "ax":0.7149, "ay":6.96057, "alpha":10.50473, "fx":[68.06614,42.21533,-24.03153,-40.85185], "fy":[95.89792,116.95446,121.51366,107.65039]}, + {"t":1.80638, "x":2.14588, "y":0.61024, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.91495, "ay":-1.06172, "alpha":-0.00017, "fx":[-109.78008,-109.77936,-109.78011,-109.78009], "fy":[-16.85446,-16.85932,-16.85431,-16.85444]}, + {"t":1.8563, "x":2.13727, "y":0.60891, "heading":3.14159, "vx":-0.34523, "vy":-0.05301, "omega":-0.00001, "ax":-2.98208, "ay":-0.45787, "alpha":0.00045, "fx":[-47.34325,-47.34436,-47.34154,-47.34172], "fy":[-7.27268,-7.25556,-7.27488,-7.27291]}, + {"t":1.90623, "x":2.11631, "y":0.60569, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00063, "fx":[-0.0011,-0.00103,0.00103,0.00097], "fy":[0.00233,-0.00657,0.00182,0.0024]}, + {"t":1.95615, "x":2.09165, "y":0.60191, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":0.00021, "fx":[-0.00074,-0.00065,0.00073,0.00066], "fy":[-0.00204,0.00623,-0.00223,-0.00195]}, + {"t":2.00608, "x":2.06698, "y":0.59812, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":0.00013, "fx":[-0.0004,-0.00031,0.0004,0.00031], "fy":[-0.00117,0.0032,-0.00097,-0.00106]}, + {"t":2.056, "x":2.04231, "y":0.59433, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00003, "fx":[-0.00009,0.00001,0.00009,0.0], "fy":[-0.00027,0.00011,0.00032,-0.00016]}, + {"t":2.10593, "x":2.01764, "y":0.59054, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.0001,0.0002,-0.00011,-0.00019], "fy":[0.00007,-0.00115,0.00089,0.00019]}, + {"t":2.15585, "x":1.99297, "y":0.58676, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00017,0.00026,-0.00017,-0.00026], "fy":[0.00014,-0.00137,0.00099,0.00025]}, + {"t":2.20578, "x":1.9683, "y":0.58297, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00015,0.00024,-0.00015,-0.00023], "fy":[0.0001,-0.00114,0.00084,0.00021]}, + {"t":2.2557, "x":1.94363, "y":0.57918, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00009,0.00017,-0.00009,-0.00016], "fy":[0.00003,-0.00077,0.00061,0.00013]}, + {"t":2.30563, "x":1.91897, "y":0.57539, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00002,0.00009,-0.00002,-0.00009], "fy":[-0.00002,-0.00046,0.0004,0.00008]}, + {"t":2.35555, "x":1.8943, "y":0.57161, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,0.00003,0.00003,-0.00003], "fy":[-0.00003,-0.00027,0.00025,0.00005]}, + {"t":2.40548, "x":1.86963, "y":0.56782, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00007,-0.00001,0.00006,0.00002], "fy":[-0.00004,-0.00014,0.00015,0.00004]}, + {"t":2.4554, "x":1.84496, "y":0.56403, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00009,-0.00004,0.00008,0.00004], "fy":[-0.00004,-0.00004,0.00007,0.00002]}, + {"t":2.50533, "x":1.82029, "y":0.56024, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00009,-0.00005,0.00009,0.00006], "fy":[-0.00005,0.00001,0.00003,0.0]}, + {"t":2.55525, "x":1.79562, "y":0.55646, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00008,-0.00005,0.00008,0.00006], "fy":[-0.00004,0.00003,0.00002,0.0]}, + {"t":2.60518, "x":1.77096, "y":0.55267, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00007,-0.00005,0.00007,0.00005], "fy":[-0.00002,-0.00002,0.00003,0.00001]}, + {"t":2.6551, "x":1.74629, "y":0.54888, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00006,-0.00004,0.00006,0.00005], "fy":[0.0,-0.00009,0.00006,0.00003]}, + {"t":2.70503, "x":1.72162, "y":0.54509, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00006,-0.00005,0.00006,0.00005], "fy":[-0.00001,-0.00001,0.00002,0.00001]}, + {"t":2.75495, "x":1.69695, "y":0.54131, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00002, "fx":[-0.00006,-0.00005,0.00006,0.00005], "fy":[-0.0002,0.00057,-0.00019,-0.00019]}, + {"t":2.80488, "x":1.67228, "y":0.53752, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00002,-0.00002,0.00002,0.00002], "fy":[0.00004,-0.00019,0.0001,0.00005]}, + {"t":2.8548, "x":1.64761, "y":0.53373, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[0.0,0.0,0.0,0.00001], "fy":[0.00014,-0.0005,0.00022,0.00014]}, + {"t":2.90473, "x":1.62294, "y":0.52994, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00002, "fx":[-0.00002,-0.00003,0.00002,0.00003], "fy":[-0.00013,0.00037,-0.00011,-0.00013]}, + {"t":2.95465, "x":1.59828, "y":0.52616, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[-0.00002,-0.00003,0.00002,0.00003], "fy":[-0.00005,0.00015,-0.00004,-0.00006]}, + {"t":3.00458, "x":1.57361, "y":0.52237, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00002,0.00001,0.00002], "fy":[0.0,-0.00003,0.00003,0.0]}, + {"t":3.0545, "x":1.54894, "y":0.51858, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,-0.00001,0.0,0.00001], "fy":[0.00001,-0.00004,0.00003,0.0]}, + {"t":3.10443, "x":1.52427, "y":0.51479, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,-0.00001,0.0,0.00001], "fy":[-0.00001,0.00001,0.00002,-0.00002]}, + {"t":3.15435, "x":1.4996, "y":0.511, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,-0.00001,0.0,0.00001], "fy":[-0.00001,0.00003,0.0,-0.00002]}, + {"t":3.20428, "x":1.47493, "y":0.50722, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,-0.00001,0.0,0.00001], "fy":[0.0,0.00002,0.0,-0.00002]}, + {"t":3.2542, "x":1.45027, "y":0.50343, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,-0.00001,0.0,0.00001], "fy":[0.0,0.00001,0.0,-0.00001]}, + {"t":3.30413, "x":1.4256, "y":0.49964, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,-0.00001,0.0,0.00001], "fy":[-0.00001,0.00004,-0.00001,-0.00002]}, + {"t":3.35405, "x":1.40093, "y":0.49585, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,-0.00001,0.0,0.00001], "fy":[0.0,0.0,0.0,-0.00001]}, + {"t":3.40398, "x":1.37626, "y":0.49207, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,-0.00001,0.0,0.00001], "fy":[0.0,0.00001,0.0,-0.00001]}, + {"t":3.4539, "x":1.35159, "y":0.48828, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.0,-0.00001,0.00001], "fy":[0.0,0.00001,0.0,-0.00001]}, + {"t":3.50383, "x":1.32692, "y":0.48449, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.0,-0.00001,0.0], "fy":[0.00003,-0.00006,0.00002,0.00001]}, + {"t":3.55375, "x":1.30225, "y":0.4807, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.0,-0.00001,0.0], "fy":[-0.00001,0.00004,-0.00002,-0.00002]}, + {"t":3.60368, "x":1.27759, "y":0.47692, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.0,-0.00001,0.0], "fy":[0.00001,-0.00001,0.0,0.0]}, + {"t":3.6536, "x":1.25292, "y":0.47313, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.0,-0.00001,0.0], "fy":[0.00002,-0.00004,0.00001,0.00001]}, + {"t":3.70353, "x":1.22825, "y":0.46934, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.00001,-0.00001,-0.00001], "fy":[0.00002,-0.00004,0.00001,0.00001]}, + {"t":3.75345, "x":1.20358, "y":0.46555, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00001,-0.00002,-0.00001], "fy":[0.00002,-0.00004,0.00001,0.00001]}, + {"t":3.80338, "x":1.17891, "y":0.46177, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00001,-0.00002,-0.00001], "fy":[0.00002,-0.00005,0.00002,0.00001]}, + {"t":3.8533, "x":1.15424, "y":0.45798, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,-0.00002,-0.00002], "fy":[0.00002,-0.00006,0.00002,0.00002]}, + {"t":3.90323, "x":1.12958, "y":0.45419, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00003,0.00003,-0.00003,-0.00003], "fy":[0.00002,-0.00007,0.00003,0.00002]}, + {"t":3.95315, "x":1.10491, "y":0.4504, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00004,0.00003,-0.00004,-0.00003], "fy":[0.00003,-0.00009,0.00004,0.00002]}, + {"t":4.00308, "x":1.08024, "y":0.44662, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00004,0.00004,-0.00004,-0.00004], "fy":[0.00003,-0.00011,0.00005,0.00003]}, + {"t":4.053, "x":1.05557, "y":0.44283, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00005,0.00005,-0.00005,-0.00005], "fy":[0.00003,-0.00013,0.00007,0.00003]}, + {"t":4.10293, "x":1.0309, "y":0.43904, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00006,0.00006,-0.00006,-0.00006], "fy":[0.00004,-0.00016,0.00008,0.00004]}, + {"t":4.15285, "x":1.00623, "y":0.43525, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00007,0.00007,-0.00007,-0.00007], "fy":[0.00004,-0.00018,0.0001,0.00004]}, + {"t":4.20278, "x":0.98156, "y":0.43147, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00008,0.00008,-0.00008,-0.00008], "fy":[0.00005,-0.00019,0.0001,0.00005]}, + {"t":4.2527, "x":0.9569, "y":0.42768, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00008,0.00008,-0.00008,-0.00008], "fy":[0.00005,-0.0002,0.0001,0.00005]}, + {"t":4.30263, "x":0.93223, "y":0.42389, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00007,0.00007,-0.00007,-0.00007], "fy":[0.00005,-0.00018,0.00008,0.00005]}, + {"t":4.35255, "x":0.90756, "y":0.4201, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00005,0.00006,-0.00005,-0.00006], "fy":[0.00005,-0.00014,0.00004,0.00005]}, + {"t":4.40248, "x":0.88289, "y":0.41631, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,-0.00002,-0.00002], "fy":[0.00005,-0.00006,-0.00003,0.00005]}, + {"t":4.4524, "x":0.85822, "y":0.41253, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00002,-0.00002,0.00002,0.00002], "fy":[0.00004,0.00007,-0.00015,0.00004]}, + {"t":4.50233, "x":0.83355, "y":0.40874, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.00009,-0.00009,0.00009,0.00009], "fy":[0.00002,0.00028,-0.00031,0.00002]}, + {"t":4.55225, "x":0.80889, "y":0.40495, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.00017,-0.00017,0.00017,0.00017], "fy":[-0.00004,0.00062,-0.00054,-0.00004]}, + {"t":4.60218, "x":0.78422, "y":0.40116, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.00024,-0.00024,0.00024,0.00024], "fy":[-0.00012,0.00103,-0.00079,-0.00012]}, + {"t":4.6521, "x":0.75955, "y":0.39738, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.00027,-0.00027,0.00027,0.00027], "fy":[-0.00019,0.00136,-0.00098,-0.00019]}, + {"t":4.70203, "x":0.73488, "y":0.39359, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00021,-0.00021,0.00021,0.00021], "fy":[-0.0002,0.00137,-0.00097,-0.0002]}, + {"t":4.75195, "x":0.71021, "y":0.3898, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,0.00001,0.00001], "fy":[0.00001,0.00052,-0.00053,0.00001]}, + {"t":4.80188, "x":0.68554, "y":0.38601, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[0.00034,0.00034,-0.00034,-0.00034], "fy":[0.00076,-0.00219,0.00067,0.00076]}, + {"t":4.8518, "x":0.66087, "y":0.38223, "heading":-3.14159, "vx":-0.49411, "vy":-0.07587, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00015, "fx":[0.00075,0.00075,-0.00075,-0.00075], "fy":[0.00169,-0.00551,0.00212,0.00169]}, + {"t":4.90173, "x":0.63621, "y":0.37844, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":0.00054, "fx":[0.00119,0.00119,-0.00113,-0.00112], "fy":[-0.00171,0.00427,-0.00082,-0.00171]}, + {"t":4.95165, "x":0.61154, "y":0.37465, "heading":3.14159, "vx":-0.49411, "vy":-0.07587, "omega":0.00001, "ax":2.9801, "ay":0.45756, "alpha":-0.00018, "fx":[47.31183,47.31273,47.31007,47.31023], "fy":[7.26628,7.25532,7.26855,7.26655]}, + {"t":5.00158, "x":0.59058, "y":0.37143, "heading":3.14159, "vx":-0.34533, "vy":-0.05302, "omega":0.0, "ax":6.91693, "ay":1.06203, "alpha":-0.00007, "fx":[109.81133,109.81157,109.81138,109.81136], "fy":[16.86092,16.85929,16.86078,16.8609]}, + {"t":5.0515, "x":0.58196, "y":0.37011, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.43544, "ay":6.98308, "alpha":3.35656, "fx":[8.00443,6.68057,-19.33767,-22.99899], "fy":[109.22613,113.95053,112.73773,107.5314]}, + {"t":5.0968, "x":0.58152, "y":0.37728, "heading":3.14159, "vx":-0.01973, "vy":0.31634, "omega":0.15205, "ax":-0.43628, "ay":6.98437, "alpha":2.65849, "fx":[4.67904,4.06,-16.97061,-19.47374], "fy":[109.57077,113.34073,112.34414,108.27222]}, + {"t":5.14211, "x":0.58017, "y":0.39877, "heading":-3.1347, "vx":-0.03949, "vy":0.63274, "omega":0.27249, "ax":-0.4373, "ay":6.9838, "alpha":2.05411, "fx":[1.94209,1.63909,-14.88937,-16.46178], "fy":[109.86337,112.79175,111.96933,108.86704]}, + {"t":5.18741, "x":0.57794, "y":0.4346, "heading":-3.12236, "vx":-0.0593, "vy":0.94911, "omega":0.36554, "ax":-0.43868, "ay":6.98286, "alpha":1.52039, "fx":[-0.40833,-0.58648,-13.00349,-13.85921], "fy":[110.12222,112.29865,111.63808,109.37302]}, + {"t":5.23271, "x":0.5748, "y":0.48476, "heading":-3.1058, "vx":-0.07917, "vy":1.26544, "omega":0.43441, "ax":-0.44051, "ay":6.98103, "alpha":1.04596, "fx":[-2.47532,-2.61155,-11.26556,-11.62143], "fy":[110.33602,111.83698,111.34072,109.80221]}, + {"t":5.27801, "x":0.57076, "y":0.54925, "heading":-3.08612, "vx":-0.09913, "vy":1.58168, "omega":0.4818, "ax":-0.44293, "ay":6.97566, "alpha":0.61979, "fx":[-4.33838,-4.44973,-9.63565,-9.70351], "fy":[110.46366,111.35056,111.02995,110.13079]}, + {"t":5.32331, "x":0.56582, "y":0.62806, "heading":-3.0643, "vx":-0.11919, "vy":1.89768, "omega":0.50987, "ax":-0.2026, "ay":2.15747, "alpha":0.66975, "fx":[-1.19608,-1.43476,-5.22502,-5.01001], "fy":[32.83386,36.0663,35.55661,32.54897]}, + {"t":5.36861, "x":0.56021, "y":0.71624, "heading":-3.0412, "vx":-0.12837, "vy":1.99542, "omega":0.54021, "ax":-0.06424, "ay":-0.00414, "alpha":-0.85193, "fx":[-3.62918,-3.2432,1.59022,1.20287], "fy":[1.70962,-2.09463,-2.02841,2.15027]}, + {"t":5.41391, "x":0.55433, "y":0.80663, "heading":-3.01673, "vx":-0.13128, "vy":1.99523, "omega":0.50162, "ax":-0.08881, "ay":-0.00593, "alpha":-2.61157, "fx":[-9.40679,-7.9394,6.58985,5.11652], "fy":[5.28953,-7.02108,-5.6174,6.97216]}, + {"t":5.45921, "x":0.54829, "y":0.89701, "heading":-2.994, "vx":-0.1353, "vy":1.99496, "omega":0.38331, "ax":-0.119, "ay":-0.00825, "alpha":-4.77911, "fx":[-16.70775,-13.54052,12.94102,9.75045], "fy":[9.40706,-13.12656,-9.89862,13.0945]}, + {"t":5.50451, "x":0.54204, "y":0.98737, "heading":-2.97664, "vx":-0.14069, "vy":1.99459, "omega":0.16682, "ax":0.0153, "ay":-0.8007, "alpha":-7.45018, "fx":[-23.69324,-17.30822,23.52182,18.45124], "fy":[1.57264,-32.83404,-27.6391,8.05386]}, + {"t":5.54981, "x":0.53568, "y":1.07691, "heading":-2.96908, "vx":-0.14, "vy":1.95832, "omega":-0.17068, "ax":0.52655, "ay":-6.96974, "alpha":-2.89257, "fx":[-6.06972,-1.76897,20.78113,20.49508], "fy":[-109.84967,-113.73456,-111.4906,-107.52378]}, + {"t":5.59511, "x":0.52988, "y":1.15847, "heading":-2.97681, "vx":-0.11615, "vy":1.64258, "omega":-0.30172, "ax":0.55904, "ay":-6.97251, "alpha":-3.86222, "fx":[-10.45883,-4.47962,24.85628,25.58299], "fy":[-109.6091,-114.89623,-111.90262,-106.36697]}, + {"t":5.64041, "x":0.52519, "y":1.22573, "heading":-2.99048, "vx":-0.09082, "vy":1.32672, "omega":-0.47668, "ax":0.62425, "ay":-6.96871, "alpha":-4.94539, "fx":[-14.89167,-7.07317,29.33043,32.2759], "fy":[-109.35459,-116.10668,-112.26505,-104.80707]}, + {"t":5.68571, "x":0.52172, "y":1.27868, "heading":-3.01208, "vx":-0.06254, "vy":1.01103, "omega":-0.70071, "ax":1.18026, "ay":-6.89723, "alpha":-6.11015, "fx":[-10.63653,-2.96778,40.20949,48.34505], "fy":[-109.83636,-117.58447,-110.74327,-99.82995]}, + {"t":5.73102, "x":0.52009, "y":1.3174, "heading":-3.04382, "vx":-0.00908, "vy":0.69859, "omega":-0.9775, "ax":1.90382, "ay":-6.73408, "alpha":-7.28919, "fx":[-2.07233,3.12978,52.39738,67.44322], "fy":[-110.07783,-118.89658,-107.76419,-90.89515]}, + {"t":5.77632, "x":0.52164, "y":1.34214, "heading":-3.0881, "vx":0.07717, "vy":0.39353, "omega":-1.30771, "ax":2.37802, "ay":-6.58198, "alpha":-8.51189, "fx":[3.05036,4.89838,60.53597,82.52664], "fy":[-110.04697,-120.45446,-105.91144,-81.56191]}, + {"t":5.82162, "x":0.52757, "y":1.35321, "heading":3.13585, "vx":0.18489, "vy":0.09536, "omega":-1.6933, "ax":2.64967, "ay":-6.47216, "alpha":-4.17379, "fx":[28.92671,22.84798,52.66662,63.82045], "fy":[-104.15161,-111.92444,-103.31518,-91.60988]}, + {"t":5.84333, "x":0.53221, "y":1.35375, "heading":3.09907, "vx":0.24243, "vy":-0.04519, "omega":-1.78394, "ax":2.87246, "ay":-6.3796, "alpha":-2.20448, "fx":[39.8527,34.70819,50.77411,57.0748], "fy":[-101.60116,-106.71274,-101.55081,-95.25844]}, + {"t":5.86505, "x":0.53815, "y":1.35127, "heading":3.06033, "vx":0.30481, "vy":-0.18373, "omega":-1.83181, "ax":3.13551, "ay":-6.25408, "alpha":-1.00207, "fx":[47.66689,44.60656,51.82388,55.01651], "fy":[-99.24557,-102.0526,-99.44801,-96.40634]}, + {"t":5.88676, "x":0.54551, "y":1.3458, "heading":3.02056, "vx":0.3729, "vy":-0.31954, "omega":-1.85357, "ax":3.44849, "ay":-6.08657, "alpha":-0.41207, "fx":[54.08636,52.57491,55.42925,56.89872], "fy":[-96.51407,-97.8971,-96.74657,-95.35727]}, + {"t":5.90848, "x":0.55442, "y":1.33743, "heading":2.9803, "vx":0.44779, "vy":-0.45172, "omega":-1.86252, "ax":3.81446, "ay":-5.86347, "alpha":-0.15956, "fx":[60.37607,59.72119,60.7561,61.37628], "fy":[-92.992,-93.62679,-93.17229,-92.55649]}, + {"t":5.9302, "x":0.56505, "y":1.32624, "heading":2.93986, "vx":0.53062, "vy":-0.57905, "omega":-1.86599, "ax":4.17338, "ay":-5.6128, "alpha":-0.06312, "fx":[66.2245,65.9166,66.32333,66.55717], "fy":[-89.03756,-89.34958,-89.14858,-88.89364]}, + {"t":5.95191, "x":0.57755, "y":1.31234, "heading":2.89934, "vx":0.62125, "vy":-0.70093, "omega":-1.86736, "ax":4.38922, "ay":-5.44454, "alpha":-0.02372, "fx":[69.67796,69.55558,69.70603,69.78855], "fy":[-86.39875,-86.5303,-86.45693,-86.3584]}, + {"t":5.97363, "x":0.59208, "y":1.29584, "heading":2.85878, "vx":0.71656, "vy":-0.81916, "omega":-1.86787, "ax":4.4657, "ay":-5.38045, "alpha":-0.00698, "fx":[70.89027,70.86676,70.89817,70.92958], "fy":[-85.40623,-85.43724,-85.43503,-85.39602]}, + {"t":5.99534, "x":0.60869, "y":1.27678, "heading":2.81822, "vx":0.81354, "vy":-0.93601, "omega":-1.86802, "ax":4.50984, "ay":-5.34128, "alpha":-0.00539, "fx":[71.60541,71.56085,71.6043,71.61778], "fy":[-84.7842,-84.82626,-84.79575,-84.78077]}, + {"t":6.01706, "x":0.62742, "y":1.25519, "heading":2.77766, "vx":0.91148, "vy":-1.052, "omega":-1.86814, "ax":4.54088, "ay":-5.31144, "alpha":-0.00108, "fx":[72.08419,72.09677,72.07492,72.10348], "fy":[-84.32458,-84.31333,-84.34048,-84.31347]}, + {"t":6.03877, "x":0.64829, "y":1.2311, "heading":2.73709, "vx":1.01008, "vy":-1.16734, "omega":-1.86816, "ax":4.5656, "ay":-5.28399, "alpha":0.00192, "fx":[72.45959,72.53423,72.43876,72.49657], "fy":[-83.90067,-83.83244,-83.93549,-83.8804]}, + {"t":6.06049, "x":0.6713, "y":1.2045, "heading":2.69652, "vx":1.10923, "vy":-1.28208, "omega":-1.86812, "ax":4.58839, "ay":-5.25007, "alpha":0.00536, "fx":[72.80432,72.94676,72.77072,72.85437], "fy":[-83.37353,-83.23888,-83.43173,-83.35056]}, + {"t":6.08221, "x":0.69647, "y":1.17542, "heading":2.65595, "vx":1.20887, "vy":-1.39609, "omega":-1.86801, "ax":4.64284, "ay":-5.13856, "alpha":0.02096, "fx":[73.6241,73.95749,73.58311,73.66901], "fy":[-81.65617,-81.2853,-81.72986,-81.64239]}, + {"t":6.10392, "x":0.72382, "y":1.14389, "heading":2.61539, "vx":1.30969, "vy":-1.50768, "omega":-1.86755, "ax":0.03461, "ay":-0.09004, "alpha":-0.01526, "fx":[0.54169,0.55229,0.54603,0.5578], "fy":[-1.16717,-2.17994,-1.19531,-1.17561]}, + {"t":6.12564, "x":0.75226, "y":1.11113, "heading":2.57483, "vx":1.31045, "vy":-1.50964, "omega":-1.86788, "ax":0.30464, "ay":0.26572, "alpha":-0.00119, "fx":[4.84131,4.85858,4.82994,4.81593], "fy":[4.3614,3.74082,4.39567,4.37602]}, + {"t":6.14735, "x":0.78079, "y":1.07841, "heading":2.53427, "vx":1.31706, "vy":-1.50387, "omega":-1.86791, "ax":0.31328, "ay":0.27567, "alpha":0.00141, "fx":[4.97732,4.99136,4.96912,4.95631], "fy":[4.46244,4.07701,4.4912,4.47521]}, + {"t":6.16907, "x":0.80947, "y":1.04582, "heading":2.49371, "vx":1.32386, "vy":-1.49788, "omega":-1.86788, "ax":0.26254, "ay":0.23296, "alpha":0.00102, "fx":[4.16986,4.17779,4.16608,4.1586], "fy":[3.75872,3.49617,3.77307,3.76539]}, + {"t":6.19079, "x":0.83828, "y":1.01334, "heading":2.45314, "vx":1.32957, "vy":-1.49282, "omega":-1.86786, "ax":0.22104, "ay":0.19752, "alpha":0.00041, "fx":[3.50977,3.51309,3.50853,3.50536], "fy":[3.18041,2.9962,3.18407,3.18218]}, + {"t":6.2125, "x":0.8672, "y":0.98097, "heading":2.41258, "vx":1.33437, "vy":-1.48853, "omega":-1.86785, "ax":0.19259, "ay":0.17313, "alpha":-0.00004, "fx":[3.05747,3.05777,3.05737,3.05713], "fy":[2.78728,2.6371,2.78432,2.78577]}, + {"t":6.23422, "x":0.89623, "y":0.94869, "heading":2.37202, "vx":1.33855, "vy":-1.48477, "omega":-1.86785, "ax":0.17295, "ay":0.15632, "alpha":-0.00028, "fx":[2.74553,2.74379,2.7458,2.74759], "fy":[2.51794,2.38335,2.51095,2.51424]}, + {"t":6.25593, "x":0.92533, "y":0.91648, "heading":2.33146, "vx":1.3423, "vy":-1.48138, "omega":-1.86785, "ax":0.15601, "ay":0.14168, "alpha":-0.00025, "fx":[2.47664,2.47412,2.47677,2.47933], "fy":[2.27912,2.17227,2.27122,2.27476]}, + {"t":6.27765, "x":0.95452, "y":0.88435, "heading":2.2909, "vx":1.34569, "vy":-1.4783, "omega":-1.86786, "ax":0.13536, "ay":0.12346, "alpha":0.00011, "fx":[2.14909,2.14903,2.14893,2.14894], "fy":[1.96754,1.94045,1.96561,1.96639]}, + {"t":6.29936, "x":0.98378, "y":0.85227, "heading":2.25033, "vx":1.34863, "vy":-1.47562, "omega":-1.86786, "ax":0.1021, "ay":0.09342, "alpha":0.00095, "fx":[1.62076,1.62938,1.62115,1.6121], "fy":[1.44634,1.56783,1.46258,1.45582]}, + {"t":6.32108, "x":1.01309, "y":0.82025, "heading":2.20977, "vx":1.35085, "vy":-1.47359, "omega":-1.86784, "ax":0.05907, "ay":0.05411, "alpha":0.002, "fx":[0.9364,0.96211,0.93996,0.91258], "fy":[0.74916,1.10741,0.79963,0.77977]}, + {"t":6.3428, "x":1.04244, "y":0.78826, "heading":2.16921, "vx":1.35213, "vy":-1.47242, "omega":-1.86779, "ax":0.09932, "ay":0.09216, "alpha":-0.0006, "fx":[1.57387,1.61746,1.58391,1.53206], "fy":[1.22452,2.02503,1.31894,1.28401]}, + {"t":6.36451, "x":1.07182, "y":0.75631, "heading":2.12865, "vx":1.35429, "vy":-1.47041, "omega":-1.86781, "ax":-1.60726, "ay":1.44101, "alpha":-0.00506, "fx":[-25.50229,-25.52746,-25.48731,-25.54858], "fy":[22.63694,23.41583,22.7466,22.70882]}, + {"t":6.38623, "x":1.10085, "y":0.72472, "heading":2.08809, "vx":1.31938, "vy":-1.43912, "omega":-1.86792, "ax":-4.84374, "ay":4.98657, "alpha":-0.00456, "fx":[-76.85832,-77.05147,-76.8278,-76.85405], "fy":[79.20913,78.98587,79.24553,79.22141]}, + {"t":6.40794, "x":1.12836, "y":0.69464, "heading":2.04752, "vx":1.2142, "vy":-1.33083, "omega":-1.86801, "ax":-4.84684, "ay":5.0189, "alpha":-0.0007, "fx":[-76.93594,-77.00048,-76.91928,-76.93269], "fy":[79.69123,79.61564,79.71032,79.69798]}, + {"t":6.42966, "x":1.15359, "y":0.66693, "heading":2.00696, "vx":1.10895, "vy":-1.22185, "omega":-1.86803, "ax":-4.86435, "ay":5.01293, "alpha":0.00134, "fx":[-77.22607,-77.23674,-77.21299,-77.22466], "fy":[79.58215,79.56394,79.59954,79.5899]}, + {"t":6.45137, "x":1.17652, "y":0.64158, "heading":1.96639, "vx":1.00331, "vy":-1.11299, "omega":-1.868, "ax":-4.88591, "ay":4.99728, "alpha":0.00618, "fx":[-77.59119,-77.53444,-77.55653,-77.5877], "fy":[79.30665,79.35419,79.35268,79.3284]}, + {"t":6.47309, "x":1.19716, "y":0.61858, "heading":1.92583, "vx":0.89721, "vy":-1.00447, "omega":-1.86787, "ax":-4.90992, "ay":4.97688, "alpha":0.01738, "fx":[-78.01943,-77.87057,-77.90484,-77.99961], "fy":[78.92722,79.05701,79.06879,78.99358]}, + {"t":6.49481, "x":1.21548, "y":0.59795, "heading":1.88527, "vx":0.79059, "vy":-0.89639, "omega":-1.86749, "ax":-4.93469, "ay":4.95444, "alpha":0.04408, "fx":[-78.51455,-78.24281,-78.17484,-78.43504], "fy":[78.45224,78.67483,78.85241,78.6421]}, + {"t":6.51652, "x":1.23149, "y":0.57965, "heading":1.84471, "vx":0.68343, "vy":-0.7888, "omega":-1.86653, "ax":-4.95888, "ay":4.93174, "alpha":0.11456, "fx":[-79.19505,-78.50692,-78.26715,-78.93414], "fy":[77.75653,78.32739,78.81981,78.27629]}, + {"t":6.53824, "x":1.24516, "y":0.56368, "heading":1.80418, "vx":0.57574, "vy":-0.6817, "omega":-1.86404, "ax":-4.98166, "ay":4.90986, "alpha":0.28947, "fx":[-80.29712,-78.62772,-77.89044,-79.53444], "fy":[76.57535,77.95902,79.29247,77.96389]}, + {"t":6.55995, "x":1.25649, "y":0.55003, "heading":1.7637, "vx":0.46756, "vy":-0.57508, "omega":-1.85776, "ax":-5.00249, "ay":4.88951, "alpha":0.70156, "fx":[-82.37031,-78.47751,-76.48187,-80.34337], "fy":[74.28283,77.49846,80.87516,77.84177]}, + {"t":6.58167, "x":1.26546, "y":0.5387, "heading":1.72336, "vx":0.35893, "vy":-0.4689, "omega":-1.84252, "ax":-5.02122, "ay":4.87098, "alpha":1.56959, "fx":[-86.29155,-77.96901,-73.10741,-81.49421], "fy":[69.84404,76.71403,84.51091,78.25273]}, + {"t":6.60339, "x":1.27207, "y":0.52967, "heading":1.68335, "vx":0.24989, "vy":-0.36312, "omega":-1.80844, "ax":-5.03787, "ay":4.85433, "alpha":3.07101, "fx":[-92.72118,-77.20563,-66.96822,-83.02463], "fy":[62.3847,75.35815,90.83522,79.68635]}, + {"t":6.6251, "x":1.27631, "y":0.52292, "heading":1.64407, "vx":0.14049, "vy":-0.25771, "omega":-1.74175, "ax":-5.05261, "ay":4.83947, "alpha":5.02548, "fx":[-100.76923,-76.94756,-58.54587,-84.5931], "fy":[52.80294,73.2482,98.95892,82.31025]}, + {"t":6.64682, "x":1.27817, "y":0.51847, "heading":1.60625, "vx":0.03076, "vy":-0.15261, "omega":-1.63262, "ax":-5.06561, "ay":4.82626, "alpha":6.92594, "fx":[-108.25876,-78.11983,-49.64851,-85.65403], "fy":[43.72118,70.42713,106.75586,85.5777]}, + {"t":6.66853, "x":1.27764, "y":0.51629, "heading":1.5708, "vx":-0.07924, "vy":-0.04781, "omega":-1.48221, "ax":-5.07049, "ay":4.82045, "alpha":7.90334, "fx":[-111.82808,-80.28265,-44.3754,-85.50503], "fy":[39.67529,67.78244,110.71382,87.94109]}, + {"t":6.70764, "x":1.27067, "y":0.51811, "heading":1.51283, "vx":-0.27753, "vy":0.14071, "omega":-1.17314, "ax":-5.07057, "ay":4.82267, "alpha":7.12367, "fx":[-108.53827,-82.24896,-47.75473,-83.454], "fy":[44.95408,66.1204,107.26612,87.91325]}, + {"t":6.74675, "x":1.25594, "y":0.5273, "heading":1.46695, "vx":-0.47583, "vy":0.32931, "omega":-0.89455, "ax":-5.0688, "ay":4.82388, "alpha":6.34622, "fx":[-105.32732,-83.24177,-51.40788,-81.90688], "fy":[49.48255,65.6138,103.75645,87.47758]}, + {"t":6.78585, "x":1.23345, "y":0.54387, "heading":1.43197, "vx":-0.67405, "vy":0.51795, "omega":-0.64637, "ax":-5.06612, "ay":4.82572, "alpha":5.52003, "fx":[-102.07717,-83.31784,-55.37962,-80.93887], "fy":[53.61009,66.25657,100.06676,86.51377]}, + {"t":6.82496, "x":1.20322, "y":0.56781, "heading":1.40669, "vx":-0.87217, "vy":0.70667, "omega":-0.4305, "ax":-5.06177, "ay":4.82864, "alpha":4.67677, "fx":[-98.78844,-83.06081,-59.37883,-80.2093], "fy":[57.45682,67.41956,96.38028,85.37623]}, + {"t":6.86407, "x":1.16524, "y":0.59914, "heading":1.38986, "vx":-1.07013, "vy":0.89551, "omega":-0.2476, "ax":-5.05285, "ay":4.83468, "alpha":3.81162, "fx":[-95.38471,-82.38946,-63.27717,-79.81964], "fy":[61.21543,69.11315,92.73629,83.95129]}, + {"t":6.90318, "x":1.11953, "y":0.63786, "heading":1.38017, "vx":-1.26773, "vy":1.08458, "omega":-0.09854, "ax":-5.02636, "ay":4.85214, "alpha":3.02726, "fx":[-91.92902,-81.78552,-66.3448,-79.12945], "fy":[64.81241,70.74128,89.70763,82.86368]}, + {"t":6.94228, "x":1.0661, "y":0.68398, "heading":1.37632, "vx":-1.46429, "vy":1.27433, "omega":0.01985, "ax":0.01293, "ay":2.24361, "alpha":8.05291, "fx":[-24.57523,16.2067,23.69367,-14.50415], "fy":[17.93857,10.35522,54.20535,59.97674]}, + {"t":6.98139, "x":1.00885, "y":0.73553, "heading":1.3771, "vx":-1.46379, "vy":1.36207, "omega":0.33477, "ax":0.34229, "ay":0.36435, "alpha":5.734, "fx":[-11.54183,16.66896,22.10136,-5.49204], "fy":[-7.79845,-11.54253,18.60846,23.87007]}, + {"t":7.0205, "x":0.95187, "y":0.78908, "heading":1.39019, "vx":-1.4504, "vy":1.37632, "omega":0.55901, "ax":0.01849, "ay":0.01947, "alpha":3.64613, "fx":[-10.22473,7.39858,10.79405,-6.79351], "fy":[-7.67664,-11.4347,8.67733,11.67032]}, + {"t":7.0596, "x":0.89516, "y":0.84292, "heading":1.41205, "vx":-1.44968, "vy":1.37708, "omega":0.7016, "ax":-0.01817, "ay":-0.01914, "alpha":1.95044, "fx":[-5.88231,3.67784,5.30176,-4.25113], "fy":[-5.16931,-6.00169,4.26674,5.68906]}, + {"t":7.09871, "x":0.83845, "y":0.89676, "heading":1.43949, "vx":-1.45039, "vy":1.37633, "omega":0.77788, "ax":0.01531, "ay":0.01613, "alpha":0.55248, "fx":[-1.34403,1.40324,1.78904,-0.87608], "fy":[-0.89274,-1.56938,1.5715,1.91486]}, + {"t":7.13782, "x":0.78174, "y":0.95059, "heading":1.46991, "vx":-1.44979, "vy":1.37696, "omega":0.79948, "ax":-0.03468, "ay":-0.03656, "alpha":-0.75536, "fx":[1.45917,-2.16409,-2.56047,1.06306], "fy":[1.21107,1.81035,-2.49462,-2.84832]}, + {"t":7.17693, "x":0.72502, "y":1.00442, "heading":1.50117, "vx":-1.45115, "vy":1.37554, "omega":0.76994, "ax":0.02728, "ay":0.02877, "alpha":-2.13499, "fx":[5.90667,-4.33213,-5.11422,5.2721], "fy":[6.10521,6.65419,-5.09154,-5.84102]}, + {"t":7.21603, "x":0.66829, "y":1.05823, "heading":1.53128, "vx":-1.45008, "vy":1.37666, "omega":0.68645, "ax":-0.12574, "ay":-0.13294, "alpha":-3.7962, "fx":[7.54454,-10.80485,-11.60598,6.88117], "fy":[7.71097,9.08896,-12.17867,-13.06306]}, + {"t":7.25514, "x":0.61149, "y":1.11197, "heading":1.55813, "vx":-1.455, "vy":1.37146, "omega":0.53799, "ax":0.31997, "ay":0.33639, "alpha":-5.59406, "fx":[18.20637,-8.49122,-8.50696,19.11088], "fy":[20.02146,21.67417,-9.7029,-10.631]}, + {"t":7.29425, "x":0.55483, "y":1.16586, "heading":1.57917, "vx":-1.44249, "vy":1.38462, "omega":0.31922, "ax":1.42825, "ay":1.43018, "alpha":-7.21611, "fx":[37.76827,4.05433,6.51127,42.36434], "fy":[40.84671,44.78796,3.46042,1.7253]}, + {"t":7.33335, "x":0.49951, "y":1.2211, "heading":1.59165, "vx":-1.38663, "vy":1.44055, "omega":0.03702, "ax":4.92846, "ay":4.17692, "alpha":-4.63347, "fx":[81.25121,60.74125,76.22402,94.75542], "fy":[74.41693,86.40411,57.17631,47.24918]}, + {"t":7.37246, "x":0.44905, "y":1.28063, "heading":1.5931, "vx":-1.19389, "vy":1.60389, "omega":-0.14418, "ax":5.8381, "ay":3.73377, "alpha":-4.25938, "fx":[93.20399,77.47033,94.39241,105.66978], "fy":[68.14484,80.1821,48.97849,39.79996]}, + {"t":7.41157, "x":0.40683, "y":1.34621, "heading":1.58746, "vx":-0.96558, "vy":1.74991, "omega":-0.31075, "ax":6.09866, "ay":3.05772, "alpha":-2.34983, "fx":[96.92887,89.94919,97.36068,103.04425], "fy":[54.25742,60.26296,42.18211,37.47162]}, + {"t":7.43647, "x":0.38467, "y":1.39073, "heading":1.57972, "vx":-0.81371, "vy":1.82605, "omega":-0.36926, "ax":6.30504, "ay":2.49724, "alpha":-1.3572, "fx":[100.25022,96.73786,100.21813,103.18225], "fy":[43.26582,46.47138,35.55226,33.29244]}, + {"t":7.46137, "x":0.36637, "y":1.43698, "heading":1.57053, "vx":-0.65671, "vy":1.88824, "omega":-0.40306, "ax":5.84166, "ay":1.78614, "alpha":-0.31799, "fx":[92.94395,92.08326,92.51635,93.41923], "fy":[29.43214,29.62206,27.38023,26.99072]}, + {"t":7.48627, "x":0.35182, "y":1.48455, "heading":1.56049, "vx":-0.51124, "vy":1.93272, "omega":-0.41098, "ax":4.25358, "ay":1.00227, "alpha":1.53634, "fx":[64.97338,70.72215,69.82203,64.59728], "fy":[11.34128,10.24327,20.47374,21.58901]}, + {"t":7.51117, "x":0.34041, "y":1.53299, "heading":1.55026, "vx":-0.40532, "vy":1.95768, "omega":-0.37272, "ax":3.43592, "ay":0.63367, "alpha":3.82145, "fx":[47.7139,62.52197,61.45698,46.49823], "fy":[-2.24674,-2.03909,21.00268,23.52305]}, + {"t":7.53608, "x":0.33138, "y":1.58194, "heading":1.54098, "vx":-0.31976, "vy":1.97345, "omega":-0.27756, "ax":5.48615, "ay":0.69556, "alpha":4.75314, "fx":[81.00596,94.93864,93.31245,79.12952], "fy":[-5.9367,-6.21195,25.46722,30.85161]}, + {"t":7.56098, "x":0.32512, "y":1.6313, "heading":1.53406, "vx":-0.18314, "vy":1.99078, "omega":-0.1592, "ax":6.6862, "ay":0.33392, "alpha":4.35508, "fx":[103.17036,110.25296,109.45835,101.7115], "fy":[-13.83533,-11.70967,20.26451,26.4851]}, + {"t":7.58588, "x":0.32264, "y":1.68097, "heading":1.5301, "vx":-0.01665, "vy":1.99909, "omega":-0.05075, "ax":6.88296, "ay":-0.23961, "alpha":4.93821, "fx":[104.66636,112.39713,113.68659,106.33836], "fy":[-27.43469,-21.8824,13.95811,20.14323]}, + {"t":7.61078, "x":0.32435, "y":1.73068, "heading":1.52884, "vx":0.15475, "vy":1.99312, "omega":0.07222, "ax":6.89105, "ay":-0.83834, "alpha":5.63222, "fx":[101.84036,111.81306,115.78161,108.16681], "fy":[-41.46368,-32.4957,7.82837,12.89393]}, + {"t":7.63568, "x":0.33035, "y":1.78005, "heading":1.53063, "vx":0.32635, "vy":1.97225, "omega":0.21247, "ax":6.81453, "ay":-1.43715, "alpha":6.36009, "fx":[96.62063,110.08047,117.13463,108.90673], "fy":[-55.76161,-42.47561,1.83011,5.14401]}, + {"t":7.66059, "x":0.34058, "y":1.82872, "heading":1.53592, "vx":0.49604, "vy":1.93646, "omega":0.37085, "ax":6.6747, "ay":-2.02766, "alpha":7.02404, "fx":[89.61297,107.49113,117.9781,108.78111], "fy":[-69.1578,-51.91763,-4.05459,-3.6325]}, + {"t":7.68549, "x":0.35501, "y":1.87631, "heading":1.54516, "vx":0.66226, "vy":1.88597, "omega":0.54576, "ax":6.47811, "ay":-2.60572, "alpha":7.58923, "fx":[81.14377,104.3598,118.39568,107.4796], "fy":[-81.40638,-60.47669,-9.80153,-13.78622]}, + {"t":7.71039, "x":0.37351, "y":1.92247, "heading":1.55875, "vx":0.82357, "vy":1.82108, "omega":0.73474, "ax":6.21003, "ay":-3.20151, "alpha":8.04887, "fx":[71.07758,100.48499,118.33749,104.45474], "fy":[-92.62237,-68.76527,-15.93783,-25.9799]}, + {"t":7.73529, "x":0.39594, "y":1.96682, "heading":1.57705, "vx":0.97821, "vy":1.74136, "omega":0.93517, "ax":5.94513, "ay":-3.64761, "alpha":8.10197, "fx":[63.5284,96.8259,117.13237,100.04652], "fy":[-98.89614,-74.27298,-21.84625,-36.61877]}, + {"t":7.76186, "x":0.42403, "y":2.0118, "heading":1.60189, "vx":1.13617, "vy":1.64444, "omega":1.15044, "ax":5.55926, "ay":-4.22593, "alpha":7.45206, "fx":[57.26561,92.01449,113.4899,90.25951], "fy":[-102.71939,-79.31772,-31.80304,-54.51874]}, + {"t":7.78843, "x":0.45618, "y":2.054, "heading":1.63246, "vx":1.28388, "vy":1.53216, "omega":1.34844, "ax":5.13622, "ay":-4.72046, "alpha":6.9623, "fx":[51.1889,87.09612,108.91888,78.96137], "fy":[-105.78233,-83.77775,-41.14284,-69.06022]}, + {"t":7.815, "x":0.4921, "y":2.09305, "heading":1.66829, "vx":1.42035, "vy":1.40674, "omega":1.53343, "ax":4.66749, "ay":-5.16653, "alpha":6.56514, "fx":[45.3792,82.05019,103.25992,65.70987], "fy":[-108.25734,-87.73426,-50.3515,-81.74682]}, + {"t":7.84157, "x":0.53149, "y":2.1286, "heading":1.70903, "vx":1.54436, "vy":1.26947, "omega":1.70786, "ax":4.15288, "ay":-5.55236, "alpha":6.2759, "fx":[39.34034,76.3014,96.30349,51.77475], "fy":[-110.24562,-91.6354,-59.29225,-91.41793]}, + {"t":7.86814, "x":0.57399, "y":2.16037, "heading":1.75441, "vx":1.6547, "vy":1.12194, "omega":1.87461, "ax":3.58176, "ay":-5.83579, "alpha":6.33255, "fx":[32.94661,70.63959,89.00017,34.86558], "fy":[-111.59498,-94.47923,-66.02224,-98.4933]}, + {"t":7.89471, "x":0.61922, "y":2.18812, "heading":1.80422, "vx":1.74987, "vy":0.96689, "omega":2.04286, "ax":2.79905, "ay":-5.60337, "alpha":8.41754, "fx":[20.82012,65.23767,83.54785,8.14236], "fy":[-111.4471,-94.36654,-58.98728,-91.02941]}, + {"t":7.92128, "x":0.6667, "y":2.21183, "heading":1.85849, "vx":1.82424, "vy":0.81801, "omega":2.26651, "ax":1.37764, "ay":-3.277, "alpha":13.40425, "fx":[-1.13104,59.97427,56.67196,-28.03122], "fy":[-93.293,-72.95171,-6.17015,-35.68447]}, + {"t":7.94785, "x":0.71566, "y":2.23241, "heading":1.91871, "vx":1.86084, "vy":0.73094, "omega":2.62266, "ax":0.53015, "ay":-1.39003, "alpha":14.47, "fx":[-10.08293,53.60009,34.19466,-44.04555], "fy":[-69.55137,-44.86962,28.1125,-1.96217]}, + {"t":7.97442, "x":0.76529, "y":2.25134, "heading":1.9884, "vx":1.87493, "vy":0.69401, "omega":3.00712, "ax":0.20952, "ay":-0.57302, "alpha":12.95362, "fx":[-10.60028,46.20192,19.2746,-41.57138], "fy":[-53.6213,-28.23497,37.87122,7.59671]}, + {"t":8.00099, "x":0.81518, "y":2.26957, "heading":2.0683, "vx":1.8805, "vy":0.67878, "omega":3.3513, "ax":0.01476, "ay":-0.04102, "alpha":11.41647, "fx":[-7.94659,40.00421,9.70625,-40.82629], "fy":[-40.83696,-14.26675,40.70491,11.79373]}, + {"t":8.02756, "x":0.86514, "y":2.28759, "heading":2.15734, "vx":1.88089, "vy":0.67769, "omega":3.65463, "ax":-0.0061, "ay":0.01695, "alpha":8.89399, "fx":[-4.17629,31.94111,3.31295,-31.46529], "fy":[-31.95641,-7.90851,33.51573,7.42525]}, + {"t":8.05413, "x":0.91512, "y":2.30561, "heading":2.25444, "vx":1.88073, "vy":0.67814, "omega":3.89094, "ax":-0.03988, "ay":0.11037, "alpha":6.72752, "fx":[-0.88201,24.09473,-0.29329,-25.45193], "fy":[-22.58301,-2.12978,26.51308,5.20884]}, + {"t":8.0807, "x":0.96507, "y":2.32366, "heading":2.35782, "vx":1.87967, "vy":0.68107, "omega":4.06969, "ax":-0.01453, "ay":0.04002, "alpha":4.02549, "fx":[0.85223,14.74527,-1.91813,-14.60229], "fy":[-14.14406,-0.12469,15.70601,1.10406]}, + {"t":8.10727, "x":1.01501, "y":2.34177, "heading":2.46595, "vx":1.87928, "vy":0.68214, "omega":4.17665, "ax":-0.01517, "ay":0.04182, "alpha":1.56913, "fx":[0.88475,5.59931,-1.36672,-6.08097], "fy":[-4.82327,0.62784,6.40692,0.44412]}, + {"t":8.13384, "x":1.06494, "y":2.35991, "heading":2.57693, "vx":1.87888, "vy":0.68325, "omega":4.21834, "ax":-0.00037, "ay":0.00099, "alpha":-1.03004, "fx":[-1.20105,-3.8306,1.16246,3.84548], "fy":[3.43547,-0.58289,-3.57012,0.78021]}, + {"t":8.16041, "x":1.11486, "y":2.37807, "heading":2.68901, "vx":1.87887, "vy":0.68327, "omega":4.19097, "ax":0.00229, "ay":-0.00626, "alpha":-3.6618, "fx":[-5.41457,-12.97994,5.47919,13.06047], "fy":[12.22362,-3.90903,-12.34148,3.62918]}, + {"t":8.18698, "x":1.16478, "y":2.39622, "heading":2.80036, "vx":1.87893, "vy":0.68311, "omega":4.09368, "ax":0.0007, "ay":-0.00194, "alpha":-6.08107, "fx":[-11.12369,-20.94189,11.64069,20.46954], "fy":[19.00068,-8.35889,-19.59841,8.83317]}, + {"t":8.21354, "x":1.2147, "y":2.41437, "heading":2.90913, "vx":1.87895, "vy":0.68306, "omega":3.9321, "ax":0.00808, "ay":-0.02224, "alpha":-8.7412, "fx":[-18.84788,-28.44969,19.04399,28.76686], "fy":[25.5941,-15.04659,-26.51364,14.5536]}, + {"t":8.24011, "x":1.26463, "y":2.43251, "heading":3.0136, "vx":1.87916, "vy":0.68246, "omega":3.69985, "ax":-0.00708, "ay":0.01949, "alpha":-10.99238, "fx":[-27.00489,-33.84021,27.54884,32.84689], "fy":[29.99719,-22.18494,-30.11701,23.54219]}, + {"t":8.26668, "x":1.31455, "y":2.45065, "heading":3.1119, "vx":1.87897, "vy":0.68298, "omega":3.40779, "ax":0.0076, "ay":-0.02094, "alpha":-13.45198, "fx":[-36.51408,-38.44456,36.49834,38.94325], "fy":[33.2254,-30.77243,-34.39696,30.61399]}, + {"t":8.29325, "x":1.36448, "y":2.46879, "heading":-3.08074, "vx":1.87917, "vy":0.68243, "omega":3.05037, "ax":-0.01305, "ay":0.03596, "alpha":-15.49967, "fx":[-45.30135,-40.82044,45.62769,39.66532], "fy":[34.94772,-39.3727,-34.62082,41.32911]}, + {"t":8.31982, "x":1.41441, "y":2.48693, "heading":-2.99969, "vx":1.87883, "vy":0.68338, "omega":2.63855, "ax":0.05136, "ay":-0.14618, "alpha":-17.421, "fx":[-53.56235,-40.82248,54.08664,43.55957], "fy":[31.95071,-48.82682,-39.37603,46.9695]}, + {"t":8.34639, "x":1.46434, "y":2.50504, "heading":-2.92958, "vx":1.88019, "vy":0.6795, "omega":2.17568, "ax":-5.04238, "ay":-1.7241, "alpha":-9.94541, "fx":[-105.15504,-90.93403,-50.75909,-73.35788], "fy":[-3.96647,-55.02346,-67.47572,16.98009]}, + {"t":8.37296, "x":1.51252, "y":2.52248, "heading":-2.87178, "vx":1.74622, "vy":0.63369, "omega":1.91144, "ax":-6.55362, "ay":-2.37566, "alpha":-5.41895, "fx":[-115.82033,-103.47048,-88.79126,-108.09221], "fy":[-19.93919,-56.44238,-64.46574,-10.01416]}, + {"t":8.39953, "x":1.5566, "y":2.53848, "heading":-2.82099, "vx":1.57209, "vy":0.57057, "omega":1.76746, "ax":-6.56684, "ay":-2.3824, "alpha":-5.78763, "fx":[-116.75847,-103.08654,-88.12003,-109.04869], "fy":[-20.10679,-58.63479,-65.42131,-7.12665]}, + {"t":8.4261, "x":1.59605, "y":2.5528, "heading":-2.77403, "vx":1.39761, "vy":0.50727, "omega":1.61368, "ax":-6.57121, "ay":-2.38453, "alpha":-6.22745, "fx":[-117.57882,-102.42378,-87.28483,-110.00342], "fy":[-20.41471,-61.1058,-66.50479,-3.39937]}, + {"t":8.45267, "x":1.63087, "y":2.56544, "heading":-2.73115, "vx":1.22302, "vy":0.44391, "omega":1.44822, "ax":-6.57337, "ay":-2.38559, "alpha":-6.66295, "fx":[-118.32341,-101.76489,-86.43999,-110.90004], "fy":[-20.8077,-63.38335,-67.60808,0.30685]}, + {"t":8.47924, "x":1.66104, "y":2.57639, "heading":-2.69268, "vx":1.04836, "vy":0.38053, "omega":1.27119, "ax":-6.57468, "ay":-2.38619, "alpha":-7.08238, "fx":[-119.00321,-101.06348,-85.70364,-111.74119], "fy":[-21.2268,-65.5961,-68.591,3.88402]}, + {"t":8.50581, "x":1.68658, "y":2.58566, "heading":-2.6589, "vx":0.87368, "vy":0.31713, "omega":1.08301, "ax":-6.57554, "ay":-2.3866, "alpha":-7.47122, "fx":[-119.60963,-100.41282,-85.0727,-112.47119], "fy":[-21.7314,-67.58972,-69.47545,7.24021]}, + {"t":8.53238, "x":1.70747, "y":2.59324, "heading":-2.63013, "vx":0.69897, "vy":0.25372, "omega":0.8845, "ax":-6.57614, "ay":-2.38694, "alpha":-7.83745, "fx":[-120.18466,-99.79991,-84.49132,-113.12857], "fy":[-22.10871,-69.43439,-70.35091,10.31615]}, + {"t":8.55895, "x":1.72372, "y":2.59914, "heading":-2.60662, "vx":0.52424, "vy":0.1903, "omega":0.67626, "ax":-6.5766, "ay":-2.38717, "alpha":-8.17387, "fx":[-120.70818,-99.27433,-83.97839,-113.67272], "fy":[-22.48613,-71.05454,-71.20496,13.15325]}, + {"t":8.58552, "x":1.73533, "y":2.60335, "heading":-2.58866, "vx":0.3495, "vy":0.12687, "omega":0.45909, "ax":-6.57695, "ay":-2.38738, "alpha":-8.49102, "fx":[-121.221,-98.84587,-83.445,-114.14356], "fy":[-22.67856,-72.48496,-72.16694,15.72455]}, + {"t":8.61209, "x":1.74229, "y":2.60588, "heading":-2.57646, "vx":0.17475, "vy":0.06344, "omega":0.23348, "ax":-6.57722, "ay":-2.38757, "alpha":-8.78753, "fx":[-121.71692,-98.56073,-82.8703,-114.52461], "fy":[-22.74598,-73.65709,-73.28563,18.07108]}, + {"t":8.63866, "x":1.74461, "y":2.60672, "heading":-2.57026, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[ + {"name":"IntakeDown", "from":{"target":1, "targetTimestamp":0.77004, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeDown"}}}, + {"name":"IntakeUp", "from":{"target":7, "targetTimestamp":7.73529, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeUp"}}}] +} diff --git a/src/main/deploy/choreo/OutpostSweep.traj b/src/main/deploy/choreo/OutpostSweep.traj new file mode 100644 index 0000000..2e0b0e9 --- /dev/null +++ b/src/main/deploy/choreo/OutpostSweep.traj @@ -0,0 +1,324 @@ +{ + "name":"OutpostSweep", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.561471939086914, "y":4.870401382446289, "heading":2.487401236737339, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.073505401611328, "y":4.4281907081604, "heading":3.141592653589793, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.1846022605895996, "y":0.616378128528595, "heading":3.141592653589793, "intervals":66, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.6016157269477844, "y":0.3701103627681732, "heading":3.141592653589793, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.3091161251068115, "y":0.40941593050956726, "heading":1.570210126881145, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.3919796049594879, "y":1.4547430276870728, "heading":1.5824236926779165, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.382836639881134, "y":2.0947506427764893, "heading":1.5771253534851022, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.7975572347640991, "y":2.6912648677825928, "heading":-2.646040927986983, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":5.0}}, "enabled":true}, + {"from":0, "to":7, "data":{"type":"MaxAcceleration", "props":{"max":7.0}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":0.5}}, "enabled":true}, + {"from":0, "to":6, "data":{"type":"MaxAngularVelocity", "props":{"max":2.0}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":7, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":7, "to":7, "data":{"type":"MaxAngularVelocity", "props":{"max":10.0}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.561471939086914 m", "val":1.561471939086914}, "y":{"exp":"4.870401382446289 m", "val":4.870401382446289}, "heading":{"exp":"2.487401236737339 rad", "val":2.487401236737339}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.073505401611328 m", "val":2.073505401611328}, "y":{"exp":"4.4281907081604 m", "val":4.4281907081604}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.1846022605895996 m", "val":2.1846022605895996}, "y":{"exp":"0.616378128528595 m", "val":0.616378128528595}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":66, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.6016157269477844 m", "val":0.6016157269477844}, "y":{"exp":"0.3701103627681732 m", "val":0.3701103627681732}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.3091161251068115 m", "val":1.3091161251068115}, "y":{"exp":"0.40941593050956726 m", "val":0.40941593050956726}, "heading":{"exp":"1.570210126881145 rad", "val":1.570210126881145}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.3919796049594879 m", "val":0.3919796049594879}, "y":{"exp":"1.4547430276870728 m", "val":1.4547430276870728}, "heading":{"exp":"1.5824236926779163 rad", "val":1.5824236926779165}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.38283663988113403 m", "val":0.382836639881134}, "y":{"exp":"2.0947506427764893 m", "val":2.0947506427764893}, "heading":{"exp":"1.5771253534851022 rad", "val":1.5771253534851022}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.7975572347640991 m", "val":1.7975572347640991}, "y":{"exp":"2.6912648677825928 m", "val":2.6912648677825928}, "heading":{"exp":"-2.6460409279869834 rad", "val":-2.646040927986983}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"5 m / s", "val":5.0}}}, "enabled":true}, + {"from":0, "to":7, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"7 m / s ^ 2", "val":7.0}}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.5 m / s", "val":0.5}}}, "enabled":true}, + {"from":0, "to":6, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"2 rad / s", "val":2.0}}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":7, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":7, "to":7, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"10 rad / s", "val":10.0}}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.52851,1.73619,4.99062,5.8185,6.78964,7.22639,8.36687], + "samples":[ + {"t":0.0, "x":1.56147, "y":4.8704, "heading":2.4874, "vx":0.0, "vy":0.0, "omega":0.0, "ax":6.99322, "ay":-0.09106, "alpha":9.56732, "fx":[109.80635,125.85084,103.86013,104.57269], "fy":[-56.9479,1.39506,63.08344,-13.31309]}, + {"t":0.02402, "x":1.56349, "y":4.87038, "heading":2.4874, "vx":0.168, "vy":-0.00219, "omega":0.22984, "ax":6.98873, "ay":-0.33144, "alpha":9.08257, "fx":[108.84842,124.99218,106.16333,103.80119], "fy":[-56.92068,-1.42018,56.37491,-19.08142]}, + {"t":0.04805, "x":1.56954, "y":4.87023, "heading":2.49292, "vx":0.33589, "vy":-0.01015, "omega":0.44803, "ax":6.9682, "ay":-0.62533, "alpha":8.53868, "fx":[107.69248,123.91424,108.41433,102.48034], "fy":[-57.11363,-4.74569,48.21503,-26.06583]}, + {"t":0.07207, "x":1.57962, "y":4.8698, "heading":2.50369, "vx":0.50329, "vy":-0.02517, "omega":0.65315, "ax":6.92524, "ay":-0.99061, "alpha":7.90556, "fx":[106.17194,122.52484,110.53536,100.54121], "fy":[-57.83951,-9.00289,37.9048,-33.96933]}, + {"t":0.09609, "x":1.59371, "y":4.86891, "heading":2.51938, "vx":0.66965, "vy":-0.04897, "omega":0.84307, "ax":6.84269, "ay":-1.45249, "alpha":7.16325, "fx":[104.10109,120.64028,112.14364,97.64613], "fy":[-59.38389,-14.65218,24.67158,-42.87267]}, + {"t":0.12012, "x":1.61177, "y":4.86732, "heading":2.53963, "vx":0.83403, "vy":-0.08386, "omega":1.01515, "ax":6.68867, "ay":-2.04542, "alpha":6.29785, "fx":[101.02636,117.88103,112.37755,93.46497], "fy":[-62.38985,-22.44473,7.68556,-52.7409]}, + {"t":0.14414, "x":1.63374, "y":4.86471, "heading":2.56402, "vx":0.99472, "vy":-0.133, "omega":1.16644, "ax":6.40315, "ay":-2.81228, "alpha":5.31771, "fx":[96.30283,113.49346,109.64763,87.17485], "fy":[-67.50006,-33.28905,-13.82981,-63.96933]}, + {"t":0.16816, "x":1.65948, "y":4.86071, "heading":2.59204, "vx":1.14854, "vy":-0.20056, "omega":1.29419, "ax":5.87655, "ay":-3.78947, "alpha":4.28744, "fx":[88.51333,105.85771,101.29479,77.51264], "fy":[-75.7251,-48.39448,-39.77339,-76.74947]}, + {"t":0.19218, "x":1.68877, "y":4.85479, "heading":2.62313, "vx":1.28971, "vy":-0.29159, "omega":1.39719, "ax":4.9357, "ay":-4.95141, "alpha":3.33173, "fx":[75.40443,91.96552,83.83441,62.22714], "fy":[-87.37081,-68.14521,-68.07604,-90.83719]}, + {"t":0.21621, "x":1.72118, "y":4.84636, "heading":2.65669, "vx":1.40828, "vy":-0.41054, "omega":1.47723, "ax":3.4171, "ay":-6.09823, "alpha":2.53977, "fx":[54.17203,67.89867,55.24426,39.68115], "fy":[-100.69443,-89.89055,-92.9971,-103.67363]}, + {"t":0.24023, "x":1.75599, "y":4.83474, "heading":2.69218, "vx":1.49037, "vy":-0.55704, "omega":1.53824, "ax":1.42046, "ay":-6.84437, "alpha":1.87991, "fx":[25.13018,33.69356,19.78775,11.59209], "fy":[-110.44324,-106.06481,-107.17465,-110.9546]}, + {"t":0.26425, "x":1.79221, "y":4.81938, "heading":2.72913, "vx":1.5245, "vy":-0.72146, "omega":1.5834, "ax":-0.58207, "ay":-6.96665, "alpha":1.2769, "fx":[-5.81618,-2.26643,-12.9692,-15.91116], "fy":[-112.15597,-110.60283,-109.08872,-110.55533]}, + {"t":0.28828, "x":1.82866, "y":4.80004, "heading":2.76717, "vx":1.51051, "vy":-0.88882, "omega":1.61408, "ax":-2.17802, "ay":-6.64418, "alpha":0.73982, "fx":[-31.98697,-31.20643,-37.30623,-37.81122], "fy":[-106.93288,-106.05835,-103.9893,-104.94466]}, + {"t":0.3123, "x":1.86432, "y":4.77677, "heading":2.80595, "vx":1.45819, "vy":-1.04844, "omega":1.63185, "ax":-3.30222, "ay":-6.16443, "alpha":0.28955, "fx":[-51.28105,-51.37686,-53.57776,-53.46527], "fy":[-98.67007,-98.14844,-97.05424,-97.58681]}, + {"t":0.33632, "x":1.8984, "y":4.7498, "heading":2.84515, "vx":1.37886, "vy":-1.19653, "omega":1.63881, "ax":-4.06703, "ay":-5.69014, "alpha":-0.11816, "fx":[-65.05851,-64.91384,-64.10364,-64.19262], "fy":[-89.92425,-90.22145,-90.72592,-90.4692]}, + {"t":0.36035, "x":1.93035, "y":4.71942, "heading":2.88452, "vx":1.28116, "vy":-1.33322, "omega":1.63597, "ax":-4.59303, "ay":-5.27575, "alpha":-0.52449, "fx":[-75.07749,-74.14293,-70.78842,-71.662], "fy":[-81.66509,-83.42078,-85.79976,-84.14003]}, + {"t":0.38437, "x":1.9598, "y":4.68587, "heading":2.92382, "vx":1.17082, "vy":-1.45996, "omega":1.62337, "ax":-4.96496, "ay":-4.92823, "alpha":-0.99415, "fx":[-82.84874,-80.75435,-74.8377,-76.84846], "fy":[-73.89732,-77.88346,-82.44472,-78.73144]}, + {"t":0.40839, "x":1.9865, "y":4.64937, "heading":2.96282, "vx":1.05155, "vy":-1.57835, "omega":1.59949, "ax":-5.2363, "ay":-4.63966, "alpha":-1.56172, "fx":[-89.30901,-85.63952,-76.92606,-80.64573], "fy":[-66.39919,-73.64212,-80.66914,-73.92135]}, + {"t":0.43241, "x":2.01025, "y":4.61012, "heading":3.00124, "vx":0.92575, "vy":-1.68981, "omega":1.56197, "ax":-5.44023, "ay":-4.39942, "alpha":-2.25516, "fx":[-95.01939,-89.46266,-77.4727,-83.51552], "fy":[-58.95269,-70.52395,-80.39882,-69.50051]}, + {"t":0.45644, "x":2.03092, "y":4.56825, "heading":3.03877, "vx":0.79506, "vy":-1.7955, "omega":1.50779, "ax":-5.59761, "ay":-4.1979, "alpha":-3.07139, "fx":[-100.24271,-92.47706,-76.76575,-85.97906], "fy":[-51.48189,-68.54248,-81.48866,-65.06616]}, + {"t":0.48046, "x":2.0484, "y":4.52391, "heading":3.07499, "vx":0.66059, "vy":-1.89634, "omega":1.43401, "ax":-5.72192, "ay":-4.0273, "alpha":-3.97308, "fx":[-104.97036,-94.95823,-75.10147,-88.32863], "fy":[-44.24029,-67.46854,-83.66062,-60.37605]}, + {"t":0.50448, "x":2.06262, "y":4.47719, "heading":3.10944, "vx":0.52313, "vy":-1.99309, "omega":1.33856, "ax":-5.82211, "ay":-3.88148, "alpha":-4.91653, "fx":[-109.16934,-96.92975,-72.75335,-90.86861], "fy":[-37.46928,-67.249,-86.59116,-55.17573]}, + {"t":0.52851, "x":2.07351, "y":4.42819, "heading":3.14159, "vx":0.38327, "vy":-2.08634, "omega":1.22045, "ax":-5.4094, "ay":-4.43689, "alpha":-5.57886, "fx":[-107.30142,-91.70976,-61.29585,-83.20541], "fy":[-44.16365,-75.76428,-96.40934,-65.41842]}, + {"t":0.56876, "x":2.08455, "y":4.34061, "heading":-3.09246, "vx":0.16551, "vy":-2.26495, "omega":0.99587, "ax":-2.89992, "ay":-6.3677, "alpha":-5.1446, "fx":[-73.3019,-57.3336,-21.54758,-31.97024], "fy":[-85.98471,-102.64656,-113.21393,-102.52234]}, + {"t":0.60902, "x":2.08886, "y":4.24427, "heading":-3.05237, "vx":0.04877, "vy":-2.52129, "omega":0.78877, "ax":0.09483, "ay":-6.99551, "alpha":-4.58517, "fx":[-21.64789,-14.22167,20.04315,21.84831], "fy":[-108.11987,-115.02154,-113.80376,-107.29038]}, + {"t":0.64928, "x":2.0909, "y":4.13711, "heading":-3.02062, "vx":0.05259, "vy":-2.8029, "omega":0.60419, "ax":0.14312, "ay":-6.99414, "alpha":-3.83009, "fx":[-17.02016,-10.91126,18.32088,18.69919], "fy":[-108.83643,-114.48609,-113.1133,-107.71249]}, + {"t":0.68953, "x":2.09314, "y":4.0186, "heading":-2.9963, "vx":0.05835, "vy":-3.08445, "omega":0.45, "ax":0.17172, "ay":-6.99276, "alpha":-3.16291, "fx":[-13.17371,-8.17814,16.40735,15.84898], "fy":[-109.34248,-113.94296,-112.59781,-108.17737]}, + {"t":0.72979, "x":2.09563, "y":3.88877, "heading":-2.97818, "vx":0.06526, "vy":-3.36596, "omega":0.32268, "ax":0.19124, "ay":-6.99122, "alpha":-2.5731, "fx":[-9.86116,-5.85777,14.46375,13.39942], "fy":[-109.71795,-113.41709,-112.19871,-108.62901]}, + {"t":0.77004, "x":2.09841, "y":3.74761, "heading":-2.96519, "vx":0.07296, "vy":-3.64739, "omega":0.21909, "ax":0.20528, "ay":-6.98926, "alpha":-2.05042, "fx":[-6.98076,-3.85583,12.56232,11.3099], "fy":[-109.99873,-112.91617,-111.87609,-109.04771]}, + {"t":0.8103, "x":2.10151, "y":3.59511, "heading":-2.95637, "vx":0.08122, "vy":-3.92875, "omega":0.13655, "ax":0.21582, "ay":-6.98634, "alpha":-1.58539, "fx":[-4.45652,-2.10564,10.74568,9.52199], "fy":[-110.20244,-112.43535,-111.59571,-109.41978]}, + {"t":0.85056, "x":2.10496, "y":3.4313, "heading":-2.95088, "vx":0.08991, "vy":-4.21, "omega":0.07273, "ax":0.225, "ay":-6.98082, "alpha":-1.17245, "fx":[-2.23273,-0.54885,9.0606,8.00894], "fy":[-110.32101,-111.95164,-111.31334,-109.71655]}, + {"t":0.89081, "x":2.10876, "y":3.25616, "heading":-2.94795, "vx":0.09897, "vy":-4.49102, "omega":0.02553, "ax":0.23939, "ay":-6.96444, "alpha":-0.81354, "fx":[-0.21337,0.90925,7.65041,6.85561], "fy":[-110.23609,-111.34029,-110.88596,-109.8001]}, + {"t":0.93107, "x":2.11294, "y":3.06973, "heading":-2.94692, "vx":0.10861, "vy":-4.77138, "omega":-0.00722, "ax":0.58226, "ay":-5.60313, "alpha":-0.87227, "fx":[5.53929,6.48035,12.80858,12.14715], "fy":[-88.30078,-90.47603,-89.63285,-87.40524]}, + {"t":0.97132, "x":2.11778, "y":2.87311, "heading":-2.94721, "vx":0.13205, "vy":-4.99694, "omega":-0.04233, "ax":0.19521, "ay":0.00353, "alpha":-1.17492, "fx":[-0.54075,0.51036,6.72112,5.70575], "fy":[2.33312,-3.77861,-1.83928,3.5088]}, + {"t":1.01158, "x":2.12325, "y":2.67196, "heading":-2.94891, "vx":0.1399, "vy":-4.9968, "omega":-0.08963, "ax":0.17675, "ay":0.00604, "alpha":-1.27087, "fx":[-1.31545,-0.16234,6.93915,5.76272], "fy":[2.38883,-3.29049,-2.43551,3.72058]}, + {"t":1.05184, "x":2.12903, "y":2.47081, "heading":-2.95252, "vx":0.14702, "vy":-4.99655, "omega":-0.14079, "ax":0.22148, "ay":4.41448, "alpha":-1.36198, "fx":[-1.60061,-0.34326,8.81105,7.19745], "fy":[71.98551,67.20825,68.18672,72.952]}, + {"t":1.09209, "x":2.13513, "y":2.27324, "heading":-2.95819, "vx":0.15593, "vy":-4.81884, "omega":-0.19562, "ax":-0.15971, "ay":6.96584, "alpha":-0.66447, "fx":[-5.69841,-5.00729,0.73504,-0.17121], "fy":[110.88873,109.9859,110.28137,111.19548]}, + {"t":1.13235, "x":2.14127, "y":2.0849, "heading":-2.96606, "vx":0.14951, "vy":-4.53843, "omega":-0.22237, "ax":-0.16636, "ay":6.98225, "alpha":-0.60578, "fx":[-5.51011,-4.89723,0.31047,-0.46731], "fy":[111.13271,110.29052,110.55962,111.41034]}, + {"t":1.1726, "x":2.14716, "y":1.90786, "heading":-2.97502, "vx":0.14281, "vy":-4.25735, "omega":-0.24675, "ax":-0.16826, "ay":6.98756, "alpha":-0.52895, "fx":[-5.16543,-4.65144,-0.11598,-0.75205], "fy":[111.18747,110.44507,110.6747,111.42307]}, + {"t":1.21286, "x":2.15277, "y":1.74214, "heading":-2.98495, "vx":0.13604, "vy":-3.97606, "omega":-0.26805, "ax":-0.17025, "ay":6.99015, "alpha":-0.44565, "fx":[-4.79357,-4.38419,-0.56856,-1.06521], "fy":[111.1933,110.56431,110.75266,111.38487]}, + {"t":1.25312, "x":2.15811, "y":1.58774, "heading":-2.99574, "vx":0.12918, "vy":-3.69466, "omega":-0.28599, "ax":-0.17344, "ay":6.99165, "alpha":-0.36066, "fx":[-4.43741,-4.12467,-1.04214,-1.40998], "fy":[111.17868,110.66812,110.8153,111.32808]}, + {"t":1.29337, "x":2.16317, "y":1.44467, "heading":-3.00725, "vx":0.1222, "vy":-3.4132, "omega":-0.30051, "ax":-0.17835, "ay":6.99257, "alpha":-0.27658, "fx":[-4.11458,-3.89305,-1.53201,-1.78596], "fy":[111.15368,110.76079,110.87023,111.26401]}, + {"t":1.33363, "x":2.16794, "y":1.31294, "heading":-3.01935, "vx":0.11502, "vy":-3.13171, "omega":-0.31164, "ax":-0.18538, "ay":6.99313, "alpha":-0.19329, "fx":[-3.83328,-3.69249,-2.04491,-2.20126], "fy":[111.12179,110.84605,110.9203,111.19621]}, + {"t":1.37388, "x":2.17242, "y":1.19253, "heading":-3.0319, "vx":0.10756, "vy":-2.8502, "omega":-0.31942, "ax":-0.195, "ay":6.99343, "alpha":-0.11029, "fx":[-3.59951,-3.52808,-2.58979,-2.66596], "fy":[111.08433,110.92597,110.96726,111.12551]}, + {"t":1.41414, "x":2.17659, "y":1.08346, "heading":-3.04475, "vx":0.09971, "vy":-2.56867, "omega":-0.32386, "ax":-0.20789, "ay":6.99349, "alpha":-0.02613, "fx":[-3.41782,-3.40379,-3.18296,-3.19723], "fy":[111.04144,111.00232,111.01215,111.05102]}, + {"t":1.4544, "x":2.18044, "y":0.98572, "heading":-3.05779, "vx":0.09134, "vy":-2.28714, "omega":-0.32491, "ax":-0.22508, "ay":6.9933, "alpha":0.06083, "fx":[-3.29911,-3.32819,-3.84679,-3.81941], "fy":[110.99229,111.07696,111.05529,110.97052]}, + {"t":1.49465, "x":2.18393, "y":0.89932, "heading":-3.07087, "vx":0.08228, "vy":-2.00561, "omega":-0.32246, "ax":-0.24843, "ay":6.99279, "alpha":0.15267, "fx":[-3.26484,-3.32084,-4.61768,-4.5726], "fy":[110.93532,111.15106,111.09615,110.88024]}, + {"t":1.53491, "x":2.18705, "y":0.82425, "heading":-3.08385, "vx":0.07228, "vy":-1.72411, "omega":-0.31632, "ax":-0.28184, "ay":6.99176, "alpha":0.25171, "fx":[-3.37046,-3.43145,-5.56357,-5.53194], "fy":[110.86734,111.22503,111.13171,110.77317]}, + {"t":1.57516, "x":2.18973, "y":0.76051, "heading":-3.09659, "vx":0.06093, "vy":-1.44265, "omega":-0.30618, "ax":-0.33716, "ay":6.98951, "alpha":0.36053, "fx":[-3.8003,-3.83099,-6.87495,-6.90423], "fy":[110.77941,111.2943,111.14942,110.63122]}, + {"t":1.61542, "x":2.19191, "y":0.70809, "heading":-3.10891, "vx":0.04736, "vy":-1.16128, "omega":-0.29167, "ax":-0.85546, "ay":6.94533, "alpha":0.48128, "fx":[-11.64937,-11.44033,-15.45978,-15.77444], "fy":[110.11963,110.83519,110.41455,109.6795]}, + {"t":1.65568, "x":2.19312, "y":0.66697, "heading":-3.12065, "vx":0.01292, "vy":-0.88169, "omega":-0.2723, "ax":-2.52763, "ay":6.52565, "alpha":0.60715, "fx":[-38.30702,-37.19427,-41.88811,-43.12233], "fy":[103.6801,104.98862,103.56011,102.16903]}, + {"t":1.69593, "x":2.19159, "y":0.63677, "heading":-3.13161, "vx":-0.08883, "vy":-0.61899, "omega":-0.24786, "ax":-4.2127, "ay":5.58849, "alpha":0.73114, "fx":[-65.61211,-63.42483,-68.13797,-70.34394], "fy":[88.66682,91.38893,88.83106,85.99896]}, + {"t":1.73619, "x":2.1846, "y":0.61638, "heading":3.14159, "vx":-0.25842, "vy":-0.39402, "omega":-0.21842, "ax":-4.53808, "ay":5.32412, "alpha":0.51137, "fx":[-71.32191,-69.67442,-72.77049,-74.41463], "fy":[84.38073,86.54212,84.69269,82.48142]}, + {"t":1.7855, "x":2.16634, "y":0.60342, "heading":3.13082, "vx":-0.48219, "vy":-0.13149, "omega":-0.19321, "ax":-0.35081, "ay":2.25497, "alpha":1.38213, "fx":[-1.56825,-1.44061,-9.47886,-9.78999], "fy":[32.7541,38.9213,38.85313,32.66859]}, + {"t":1.83481, "x":2.14214, "y":0.59968, "heading":3.1213, "vx":-0.49949, "vy":-0.0203, "omega":-0.12506, "ax":0.08015, "ay":-0.92491, "alpha":1.01346, "fx":[4.02576,4.14072,-1.49696,-1.57987], "fy":[-17.19112,-12.19514,-12.25461,-17.09384]}, + {"t":1.88412, "x":2.11761, "y":0.59755, "heading":3.11513, "vx":-0.49554, "vy":-0.06591, "omega":-0.07508, "ax":0.00351, "ay":-0.02613, "alpha":0.65087, "fx":[1.83615,1.91654,-1.72578,-1.80402], "fy":[-2.01909,1.02762,1.26125,-1.92908]}, + {"t":1.93343, "x":2.09318, "y":0.59427, "heading":3.11143, "vx":-0.49536, "vy":-0.0672, "omega":-0.04299, "ax":0.00244, "ay":-0.01786, "alpha":0.41859, "fx":[1.17328,1.23151,-1.09606,-1.1538], "fy":[-1.32793,0.66824,0.78677,-1.26152]}, + {"t":1.98274, "x":2.06875, "y":0.59094, "heading":3.10931, "vx":-0.49524, "vy":-0.06808, "omega":-0.02235, "ax":0.00201, "ay":-0.01456, "alpha":0.26731, "fx":[0.75083,0.79051,-0.68702,-0.72655], "fy":[-0.90473,0.39097,0.44838,-0.85909]}, + {"t":2.03205, "x":2.04434, "y":0.58756, "heading":3.10821, "vx":-0.49514, "vy":-0.0688, "omega":-0.00917, "ax":0.00172, "ay":-0.01231, "alpha":0.16959, "fx":[0.48009,0.50606,-0.42555,-0.45147], "fy":[-0.62678,0.20775,0.23385,-0.59667]}, + {"t":2.08136, "x":2.01992, "y":0.58416, "heading":3.10775, "vx":-0.49506, "vy":-0.0694, "omega":-0.0008, "ax":0.0015, "ay":-0.01067, "alpha":0.10683, "fx":[0.30712,0.32364,-0.25945,-0.27595], "fy":[-0.44346,0.08911,0.10094,-0.42412]}, + {"t":2.13067, "x":1.99551, "y":0.58072, "heading":3.10771, "vx":-0.49498, "vy":-0.06993, "omega":0.00446, "ax":0.00133, "ay":-0.00941, "alpha":0.06685, "fx":[0.1972,0.20746,-0.15486,-0.16512], "fy":[-0.32235,0.0145,0.02065,-0.31017]}, + {"t":2.17997, "x":1.97111, "y":0.57726, "heading":3.10793, "vx":-0.49492, "vy":-0.07039, "omega":0.00776, "ax":0.0012, "ay":-0.0084, "alpha":0.04151, "fx":[0.12752,0.13374,-0.08946,-0.09568], "fy":[-0.24177,-0.03097,-0.02653,-0.23426]}, + {"t":2.22928, "x":1.94671, "y":0.57378, "heading":3.10832, "vx":-0.49486, "vy":-0.07081, "omega":0.00981, "ax":0.00109, "ay":-0.00758, "alpha":0.02557, "fx":[0.08359,0.08725,-0.04906,-0.05272], "fy":[-0.18773,-0.05744,-0.05289,-0.18321]}, + {"t":2.27859, "x":1.92231, "y":0.57028, "heading":3.1088, "vx":-0.49481, "vy":-0.07118, "omega":0.01107, "ax":0.00099, "ay":-0.00689, "alpha":0.01557, "fx":[0.05586,0.0579,-0.02431,-0.02635], "fy":[-0.15081,-0.07201,-0.06656,-0.14818]}, + {"t":2.3279, "x":1.89791, "y":0.56676, "heading":3.10935, "vx":-0.49476, "vy":-0.07152, "omega":0.01184, "ax":0.00091, "ay":-0.0063, "alpha":0.00935, "fx":[0.03842,0.03945,-0.00942,-0.01045], "fy":[-0.12511,-0.07906,-0.07247,-0.12369]}, + {"t":2.37721, "x":1.87351, "y":0.56323, "heading":3.10993, "vx":-0.49471, "vy":-0.07183, "omega":0.0123, "ax":0.00084, "ay":-0.0058, "alpha":0.00546, "fx":[0.02737,0.02777,-0.00058,-0.00099], "fy":[-0.10668,-0.08159,-0.07389,-0.10602]}, + {"t":2.42652, "x":1.84912, "y":0.55968, "heading":3.11054, "vx":-0.49467, "vy":-0.07212, "omega":0.01257, "ax":0.00078, "ay":-0.00536, "alpha":0.00307, "fx":[0.02036,0.02038,0.00448,0.00446], "fy":[-0.09311,-0.08132,-0.07269,-0.09294]}, + {"t":2.47583, "x":1.82473, "y":0.55611, "heading":3.11116, "vx":-0.49463, "vy":-0.07238, "omega":0.01272, "ax":0.00073, "ay":-0.00496, "alpha":0.00158, "fx":[0.01584,0.01562,0.00727,0.00748], "fy":[-0.08276,-0.07944,-0.07014,-0.08289]}, + {"t":2.52514, "x":1.80034, "y":0.55254, "heading":3.11178, "vx":-0.4946, "vy":-0.07263, "omega":0.0128, "ax":0.00068, "ay":-0.00462, "alpha":0.00068, "fx":[0.01289,0.01254,0.00866,0.00902], "fy":[-0.07464,-0.07658,-0.0669,-0.07497]}, + {"t":2.57445, "x":1.77595, "y":0.54895, "heading":3.11241, "vx":-0.49456, "vy":-0.07285, "omega":0.01283, "ax":0.00063, "ay":-0.0043, "alpha":0.00014, "fx":[0.01093,0.01049,0.00922,0.00966], "fy":[-0.06809,-0.07319,-0.06341,-0.06853]}, + {"t":2.62376, "x":1.75157, "y":0.54535, "heading":3.11305, "vx":-0.49453, "vy":-0.07307, "omega":0.01284, "ax":0.00059, "ay":-0.00402, "alpha":-0.00017, "fx":[0.00959,0.0091,0.0093,0.00978], "fy":[-0.06265,-0.06953,-0.05991,-0.06316]}, + {"t":2.67307, "x":1.72718, "y":0.54175, "heading":3.11368, "vx":-0.4945, "vy":-0.07326, "omega":0.01283, "ax":0.00056, "ay":-0.00376, "alpha":-0.00033, "fx":[0.00865,0.00815,0.00907,0.00957], "fy":[-0.05807,-0.06573,-0.0565,-0.05862]}, + {"t":2.72238, "x":1.7028, "y":0.53813, "heading":3.11431, "vx":-0.49447, "vy":-0.07345, "omega":0.01281, "ax":0.00052, "ay":-0.00353, "alpha":-0.00041, "fx":[0.00795,0.00745,0.00871,0.0092], "fy":[-0.0541,-0.06195,-0.05328,-0.05466]}, + {"t":2.77169, "x":1.67842, "y":0.5345, "heading":3.11494, "vx":-0.49445, "vy":-0.07362, "omega":0.01279, "ax":0.00049, "ay":-0.00331, "alpha":-0.00041, "fx":[0.00745,0.00696,0.00823,0.00871], "fy":[-0.05066,-0.05819,-0.05021,-0.05122]}, + {"t":2.821, "x":1.65404, "y":0.53087, "heading":3.11557, "vx":-0.49442, "vy":-0.07379, "omega":0.01277, "ax":0.00047, "ay":-0.00311, "alpha":-0.00039, "fx":[0.00701,0.00655,0.00775,0.00822], "fy":[-0.04758,-0.05457,-0.04738,-0.04812]}, + {"t":2.87031, "x":1.62966, "y":0.52723, "heading":3.1162, "vx":-0.4944, "vy":-0.07394, "omega":0.01275, "ax":0.00044, "ay":-0.00293, "alpha":-0.00034, "fx":[0.00669,0.00625,0.00723,0.00767], "fy":[-0.04485,-0.05105,-0.0447,-0.04538]}, + {"t":2.91962, "x":1.60528, "y":0.52358, "heading":3.11683, "vx":-0.49438, "vy":-0.07408, "omega":0.01273, "ax":0.00041, "ay":-0.00276, "alpha":-0.00029, "fx":[0.00637,0.00596,0.00676,0.00718], "fy":[-0.04234,-0.04773,-0.04224,-0.04284]}, + {"t":2.96893, "x":1.5809, "y":0.51992, "heading":3.11746, "vx":-0.49436, "vy":-0.07422, "omega":0.01272, "ax":0.00039, "ay":-0.0026, "alpha":-0.00022, "fx":[0.00612,0.00574,0.00628,0.00666], "fy":[-0.04008,-0.04453,-0.0399,-0.04055]}, + {"t":3.01824, "x":1.55653, "y":0.51626, "heading":3.11809, "vx":-0.49434, "vy":-0.07435, "omega":0.01271, "ax":0.00037, "ay":-0.00245, "alpha":-0.00017, "fx":[0.00585,0.00549,0.00587,0.00623], "fy":[-0.03795,-0.04156,-0.03776,-0.03838]}, + {"t":3.06755, "x":1.53215, "y":0.51259, "heading":3.11871, "vx":-0.49432, "vy":-0.07447, "omega":0.0127, "ax":0.00035, "ay":-0.00231, "alpha":-0.0001, "fx":[0.00562,0.00529,0.00545,0.00578], "fy":[-0.03601,-0.03872,-0.03571,-0.03641]}, + {"t":3.11686, "x":1.50778, "y":0.50891, "heading":3.11934, "vx":-0.4943, "vy":-0.07458, "omega":0.0127, "ax":0.00033, "ay":-0.00218, "alpha":-0.00006, "fx":[0.00535,0.00505,0.00511,0.00541], "fy":[-0.03414,-0.03612,-0.03384,-0.03451]}, + {"t":3.16617, "x":1.4834, "y":0.50523, "heading":3.11997, "vx":-0.49429, "vy":-0.07469, "omega":0.01269, "ax":0.00031, "ay":-0.00206, "alpha":-0.00001, "fx":[0.00513,0.00486,0.00476,0.00504], "fy":[-0.03242,-0.03364,-0.03204,-0.03276]}, + {"t":3.21547, "x":1.45903, "y":0.50155, "heading":3.12059, "vx":-0.49427, "vy":-0.07479, "omega":0.01269, "ax":0.00029, "ay":-0.00195, "alpha":0.00001, "fx":[0.00486,0.00462,0.00449,0.00474], "fy":[-0.03074,-0.03139,-0.03039,-0.03105]}, + {"t":3.26478, "x":1.43466, "y":0.49786, "heading":3.12122, "vx":-0.49426, "vy":-0.07489, "omega":0.01269, "ax":0.00028, "ay":-0.00184, "alpha":0.00005, "fx":[0.00464,0.00442,0.00421,0.00443], "fy":[-0.02919,-0.02926,-0.02879,-0.02946]}, + {"t":3.31409, "x":1.41029, "y":0.49416, "heading":3.12184, "vx":-0.49424, "vy":-0.07498, "omega":0.0127, "ax":0.00026, "ay":-0.00174, "alpha":0.00006, "fx":[0.00437,0.00418,0.00399,0.00419], "fy":[-0.02766,-0.02734,-0.02732,-0.02791]}, + {"t":3.3634, "x":1.38592, "y":0.49046, "heading":3.12247, "vx":-0.49423, "vy":-0.07506, "omega":0.0127, "ax":0.00025, "ay":-0.00164, "alpha":0.00008, "fx":[0.00415,0.00398,0.00376,0.00393], "fy":[-0.02625,-0.02551,-0.02588,-0.02646]}, + {"t":3.41271, "x":1.36155, "y":0.48676, "heading":3.1231, "vx":-0.49422, "vy":-0.07515, "omega":0.0127, "ax":0.00024, "ay":-0.00155, "alpha":0.00008, "fx":[0.00389,0.00374,0.00358,0.00373], "fy":[-0.02485,-0.02387,-0.02456,-0.02504]}, + {"t":3.46202, "x":1.33718, "y":0.48305, "heading":3.12372, "vx":-0.49421, "vy":-0.07522, "omega":0.01271, "ax":0.00022, "ay":-0.00146, "alpha":0.00009, "fx":[0.00368,0.00355,0.00339,0.00351], "fy":[-0.02354,-0.02231,-0.02325,-0.0237]}, + {"t":3.51133, "x":1.31281, "y":0.47934, "heading":3.12435, "vx":-0.4942, "vy":-0.07529, "omega":0.01271, "ax":0.00021, "ay":-0.00138, "alpha":0.00008, "fx":[0.00344,0.00333,0.00323,0.00334], "fy":[-0.02225,-0.0209,-0.02204,-0.02239]}, + {"t":3.56064, "x":1.28844, "y":0.47563, "heading":3.12498, "vx":-0.49419, "vy":-0.07536, "omega":0.01272, "ax":0.0002, "ay":-0.0013, "alpha":0.00009, "fx":[0.00324,0.00315,0.00306,0.00315], "fy":[-0.02105,-0.01957,-0.02085,-0.02117]}, + {"t":3.60995, "x":1.26407, "y":0.47191, "heading":3.1256, "vx":-0.49418, "vy":-0.07543, "omega":0.01272, "ax":0.00019, "ay":-0.00123, "alpha":0.00008, "fx":[0.00303,0.00296,0.00292,0.00299], "fy":[-0.01987,-0.01836,-0.01974,-0.01996]}, + {"t":3.65926, "x":1.23971, "y":0.46819, "heading":3.12623, "vx":-0.49417, "vy":-0.07549, "omega":0.01272, "ax":0.00018, "ay":-0.00116, "alpha":0.00008, "fx":[0.00286,0.0028,0.00276,0.00281], "fy":[-0.01876,-0.01721,-0.01864,-0.01883]}, + {"t":3.70857, "x":1.21534, "y":0.46447, "heading":3.12686, "vx":-0.49416, "vy":-0.07554, "omega":0.01273, "ax":0.00017, "ay":-0.00109, "alpha":0.00007, "fx":[0.00267,0.00263,0.00262,0.00266], "fy":[-0.01767,-0.01616,-0.0176,-0.01772]}, + {"t":3.75788, "x":1.19097, "y":0.46074, "heading":3.12748, "vx":-0.49415, "vy":-0.0756, "omega":0.01273, "ax":0.00016, "ay":-0.00102, "alpha":0.00007, "fx":[0.00252,0.00249,0.00246,0.00249], "fy":[-0.01664,-0.01516,-0.01658,-0.01668]}, + {"t":3.80719, "x":1.16661, "y":0.45701, "heading":3.12811, "vx":-0.49414, "vy":-0.07565, "omega":0.01273, "ax":0.00015, "ay":-0.00096, "alpha":0.00007, "fx":[0.00237,0.00235,0.00232,0.00233], "fy":[-0.01564,-0.01424,-0.01562,-0.01566]}, + {"t":3.8565, "x":1.14224, "y":0.45328, "heading":3.12874, "vx":-0.49413, "vy":-0.0757, "omega":0.01274, "ax":0.00014, "ay":-0.0009, "alpha":0.00007, "fx":[0.00224,0.00224,0.00216,0.00216], "fy":[-0.01469,-0.01336,-0.01466,-0.0147]}, + {"t":3.90581, "x":1.11788, "y":0.44955, "heading":3.12937, "vx":-0.49413, "vy":-0.07574, "omega":0.01274, "ax":0.00013, "ay":-0.00085, "alpha":0.00006, "fx":[0.00211,0.00212,0.00201,0.00201], "fy":[-0.01376,-0.01255,-0.01376,-0.01376]}, + {"t":3.95512, "x":1.09351, "y":0.44581, "heading":3.13, "vx":-0.49412, "vy":-0.07578, "omega":0.01274, "ax":0.00012, "ay":-0.00079, "alpha":0.00006, "fx":[0.002,0.00202,0.00186,0.00185], "fy":[-0.01287,-0.01177,-0.01288,-0.01286]}, + {"t":4.00443, "x":1.06915, "y":0.44207, "heading":3.13063, "vx":-0.49412, "vy":-0.07582, "omega":0.01275, "ax":0.00011, "ay":-0.00074, "alpha":0.00006, "fx":[0.00189,0.00191,0.00172,0.00171], "fy":[-0.012,-0.01105,-0.01205,-0.01198]}, + {"t":4.05374, "x":1.04478, "y":0.43833, "heading":3.13125, "vx":-0.49411, "vy":-0.07586, "omega":0.01275, "ax":0.00011, "ay":-0.00069, "alpha":0.00005, "fx":[0.00178,0.0018,0.00159,0.00157], "fy":[-0.01116,-0.01037,-0.01125,-0.01113]}, + {"t":4.10305, "x":1.02042, "y":0.43459, "heading":3.13188, "vx":-0.4941, "vy":-0.07589, "omega":0.01275, "ax":0.0001, "ay":-0.00064, "alpha":0.00003, "fx":[0.00165,0.00168,0.00148,0.00146], "fy":[-0.01031,-0.00974,-0.01051,-0.01028]}, + {"t":4.15236, "x":0.99605, "y":0.43085, "heading":3.13251, "vx":-0.4941, "vy":-0.07592, "omega":0.01275, "ax":0.00009, "ay":-0.0006, "alpha":0.00001, "fx":[0.0015,0.00153,0.00141,0.00138], "fy":[-0.00946,-0.00916,-0.00983,-0.00943]}, + {"t":4.20166, "x":0.97169, "y":0.4271, "heading":3.13314, "vx":-0.4941, "vy":-0.07595, "omega":0.01275, "ax":0.00008, "ay":-0.00055, "alpha":-0.00004, "fx":[0.0013,0.00133,0.00139,0.00136], "fy":[-0.00859,-0.00866,-0.00923,-0.00855]}, + {"t":4.25097, "x":0.94733, "y":0.42336, "heading":3.13377, "vx":-0.49409, "vy":-0.07598, "omega":0.01275, "ax":0.00008, "ay":-0.00051, "alpha":-0.00012, "fx":[0.00102,0.00104,0.00147,0.00144], "fy":[-0.00765,-0.00825,-0.00874,-0.00762]}, + {"t":4.30028, "x":0.92296, "y":0.41961, "heading":3.1344, "vx":-0.49409, "vy":-0.07601, "omega":0.01275, "ax":0.00007, "ay":-0.00047, "alpha":-0.00025, "fx":[0.00059,0.00061,0.00169,0.00167], "fy":[-0.00661,-0.00797,-0.00842,-0.00658]}, + {"t":4.34959, "x":0.8986, "y":0.41586, "heading":3.13503, "vx":-0.49408, "vy":-0.07603, "omega":0.01273, "ax":0.00007, "ay":-0.00042, "alpha":-0.00047, "fx":[-0.00007,-0.00005,0.00215,0.00213], "fy":[-0.00537,-0.0079,-0.00835,-0.00536]}, + {"t":4.3989, "x":0.87424, "y":0.41211, "heading":3.13565, "vx":-0.49408, "vy":-0.07605, "omega":0.01271, "ax":0.00006, "ay":-0.00038, "alpha":-0.00082, "fx":[-0.0011,-0.00109,0.00298,0.00297], "fy":[-0.00383,-0.00814,-0.00864,-0.00383]}, + {"t":4.44821, "x":0.84987, "y":0.40836, "heading":3.13628, "vx":-0.49408, "vy":-0.07607, "omega":0.01267, "ax":0.00005, "ay":-0.00035, "alpha":-0.00139, "fx":[-0.00272,-0.00273,0.00441,0.00442], "fy":[-0.00179,-0.00888,-0.0095,-0.0018]}, + {"t":4.49752, "x":0.82551, "y":0.40461, "heading":3.13691, "vx":-0.49407, "vy":-0.07609, "omega":0.0126, "ax":0.00005, "ay":-0.00031, "alpha":-0.0023, "fx":[-0.00529,-0.00531,0.00679,0.00682], "fy":[0.00107,-0.01041,-0.01124,0.00104]}, + {"t":4.54683, "x":0.80115, "y":0.40086, "heading":3.13753, "vx":-0.49407, "vy":-0.0761, "omega":0.01249, "ax":0.00004, "ay":-0.00027, "alpha":-0.00374, "fx":[-0.00935,-0.0094,0.01068,0.01073], "fy":[0.00521,-0.01319,-0.01434,0.00515]}, + {"t":4.59614, "x":0.77679, "y":0.39711, "heading":3.13814, "vx":-0.49407, "vy":-0.07611, "omega":0.0123, "ax":0.00004, "ay":-0.00023, "alpha":-0.00604, "fx":[-0.01578,-0.01586,0.01692,0.017], "fy":[0.01138,-0.01795,-0.01957,0.01128]}, + {"t":4.64545, "x":0.75242, "y":0.39335, "heading":3.13875, "vx":-0.49407, "vy":-0.07613, "omega":0.01201, "ax":0.00003, "ay":-0.0002, "alpha":-0.00966, "fx":[-0.02589,-0.02601,0.02686,0.02698], "fy":[0.02074,-0.02586,-0.02807,0.02061]}, + {"t":4.69476, "x":0.72806, "y":0.3896, "heading":3.13934, "vx":-0.49407, "vy":-0.07613, "omega":0.01153, "ax":0.00003, "ay":-0.00016, "alpha":-0.01535, "fx":[-0.04177,-0.04192,0.04256,0.04271], "fy":[0.0351,-0.03873,-0.04161,0.03493]}, + {"t":4.74407, "x":0.7037, "y":0.38585, "heading":3.13991, "vx":-0.49407, "vy":-0.07614, "omega":0.01077, "ax":0.00002, "ay":-0.00013, "alpha":-0.02425, "fx":[-0.06656,-0.06674,0.06718,0.06736], "fy":[0.05724,-0.05946,-0.06289,0.05703]}, + {"t":4.79338, "x":0.67934, "y":0.38209, "heading":3.14044, "vx":-0.49406, "vy":-0.07615, "omega":0.00958, "ax":0.00001, "ay":-0.00009, "alpha":-0.03812, "fx":[-0.1051,-0.1053,0.10556,0.10576], "fy":[0.09147,-0.09267,-0.09592,0.09124]}, + {"t":4.84269, "x":0.65498, "y":0.37834, "heading":3.14091, "vx":-0.49406, "vy":-0.07615, "omega":0.0077, "ax":0.00001, "ay":-0.00006, "alpha":-0.05932, "fx":[-0.16473,-0.16491,0.16509,0.16527], "fy":[0.14258,-0.13995,-0.14867,0.14237]}, + {"t":4.892, "x":0.63061, "y":0.37458, "heading":3.14129, "vx":-0.49406, "vy":-0.07616, "omega":0.00477, "ax":3.10339, "ay":0.47835, "alpha":-0.07078, "fx":[49.08459,49.09733,49.45279,49.43949], "fy":[7.79506,7.41726,7.38463,7.77949]}, + {"t":4.94131, "x":0.61002, "y":0.37141, "heading":3.14153, "vx":-0.34104, "vy":-0.05257, "omega":0.00128, "ax":6.91625, "ay":1.06611, "alpha":-0.02602, "fx":[109.75872,109.78665,109.84287,109.81421], "fy":[17.05563,16.83068,16.79239,17.02253]}, + {"t":4.99062, "x":0.60162, "y":0.37011, "heading":-3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":1.83194, "ay":0.09202, "alpha":-34.70801, "fx":[-57.59301,-61.36549,118.74417,116.54801], "fy":[121.52857,-119.64015,-63.48353,67.43858]}, + {"t":5.0124, "x":0.60205, "y":0.37013, "heading":-3.14159, "vx":0.03991, "vy":0.002, "omega":-0.75616, "ax":2.76529, "ay":0.09568, "alpha":-32.61819, "fx":[-31.31278,-34.64544,122.04517,119.51701], "fy":[130.5574,-129.64633,-56.69542,61.86033]}, + {"t":5.03419, "x":0.60358, "y":0.3702, "heading":3.12512, "vx":0.10016, "vy":0.00409, "omega":-1.46679, "ax":5.7533, "ay":0.15149, "alpha":-22.04163, "fx":[52.66496,60.07591,127.36957,125.24073], "fy":[122.97744,-119.25793,-42.9168,48.81714]}, + {"t":5.05598, "x":0.60712, "y":0.37032, "heading":3.09316, "vx":0.2255, "vy":0.00739, "omega":-1.94699, "ax":6.97244, "ay":0.42015, "alpha":-2.37491, "fx":[107.71425,108.81883,113.42091,112.81654], "fy":[19.5448,-4.50789,-3.17894,14.82277]}, + {"t":5.07776, "x":0.61369, "y":0.37058, "heading":3.05075, "vx":0.3774, "vy":0.01654, "omega":-1.99873, "ax":6.97031, "ay":0.41896, "alpha":-0.00028, "fx":[110.6621,110.66692,110.65303,110.65319], "fy":[6.66412,6.60076,6.68138,6.65865]}, + {"t":5.09955, "x":0.62357, "y":0.37104, "heading":3.0072, "vx":0.52926, "vy":0.02567, "omega":-1.99874, "ax":6.9674, "ay":0.41764, "alpha":0.0, "fx":[110.61249,110.61343,110.6119,110.61252], "fy":[6.63169,6.61568,6.64218,6.63208]}, + {"t":5.12134, "x":0.63675, "y":0.3717, "heading":2.96365, "vx":0.68105, "vy":0.03477, "omega":-1.99874, "ax":6.96323, "ay":0.41634, "alpha":0.0, "fx":[110.54635,110.54723,110.54573,110.54613], "fy":[6.61073,6.5966,6.61918,6.61201]}, + {"t":5.14312, "x":0.65324, "y":0.37256, "heading":2.92011, "vx":0.83276, "vy":0.04384, "omega":-1.99874, "ax":6.95679, "ay":0.41515, "alpha":0.0, "fx":[110.44411,110.44512,110.44341,110.44373], "fy":[6.59223,6.57646,6.60053,6.59396]}, + {"t":5.16491, "x":0.67304, "y":0.37361, "heading":2.87656, "vx":0.98432, "vy":0.05288, "omega":-1.99874, "ax":6.94563, "ay":0.41406, "alpha":0.0, "fx":[110.26696,110.26867,110.2658,110.26615], "fy":[6.57642,6.54988,6.58842,6.57934]}, + {"t":5.18669, "x":0.69613, "y":0.37486, "heading":2.83302, "vx":1.13564, "vy":0.06191, "omega":-1.99874, "ax":6.92192, "ay":0.41131, "alpha":0.0, "fx":[109.89071,109.89331,109.88898,109.88925], "fy":[6.53514,6.49407,6.55079,6.53926]}, + {"t":5.20848, "x":0.72251, "y":0.37631, "heading":2.78947, "vx":1.28644, "vy":0.07087, "omega":-1.99874, "ax":6.83988, "ay":0.39073, "alpha":0.0, "fx":[108.58854,108.5921,108.58607,108.58594], "fy":[6.21216,6.1522,6.23083,6.21736]}, + {"t":5.23027, "x":0.75216, "y":0.37795, "heading":2.74593, "vx":1.43546, "vy":0.07938, "omega":-1.99874, "ax":2.73412, "ay":0.18961, "alpha":0.0, "fx":[43.40579,43.405,43.40615,43.40742], "fy":[3.01067,3.00334,3.01602,3.01076]}, + {"t":5.25205, "x":0.78409, "y":0.37972, "heading":2.70238, "vx":1.49502, "vy":0.08351, "omega":-1.99874, "ax":0.00344, "ay":0.05369, "alpha":0.0, "fx":[0.05314,0.04927,0.05594,0.05999], "fy":[0.84155,0.89036,0.83855,0.83889]}, + {"t":5.27384, "x":0.81666, "y":0.38155, "heading":2.65884, "vx":1.4951, "vy":0.08468, "omega":-1.99874, "ax":-0.00026, "ay":0.00477, "alpha":0.0, "fx":[-0.00515,-0.00919,-0.00304,0.00103], "fy":[0.06323,0.1181,0.0604,0.0609]}, + {"t":5.29563, "x":0.84923, "y":0.3834, "heading":2.61529, "vx":1.49509, "vy":0.08478, "omega":-1.99874, "ax":0.00122, "ay":-0.02155, "alpha":0.0, "fx":[0.01854,0.01497,0.02011,0.02368], "fy":[-0.35553,-0.2972,-0.35809,-0.35746]}, + {"t":5.31741, "x":0.8818, "y":0.38524, "heading":2.57175, "vx":1.49512, "vy":0.08431, "omega":-1.99874, "ax":0.00201, "ay":-0.03581, "alpha":0.0, "fx":[0.03134,0.02832,0.03246,0.03547], "fy":[-0.58344,-0.52009,-0.58573,-0.58504]}, + {"t":5.3392, "x":0.91438, "y":0.38707, "heading":2.5282, "vx":1.49516, "vy":0.08353, "omega":-1.99874, "ax":0.00255, "ay":-0.046, "alpha":0.0, "fx":[0.04019,0.03778,0.04089,0.0433], "fy":[-0.74673,-0.67837,-0.74839,-0.7479]}, + {"t":5.36098, "x":0.94695, "y":0.38888, "heading":2.48466, "vx":1.49522, "vy":0.08253, "omega":-1.99874, "ax":0.00319, "ay":-0.05825, "alpha":0.0, "fx":[0.05047,0.04873,0.0508,0.05254], "fy":[-0.94291,-0.86924,-0.94359,-0.94352]}, + {"t":5.38277, "x":0.97953, "y":0.39066, "heading":2.44111, "vx":1.49529, "vy":0.08126, "omega":-1.99874, "ax":0.00421, "ay":-0.07825, "alpha":0.0, "fx":[0.06677,0.06577,0.06678,0.06778], "fy":[-1.2624,-1.18258,-1.2618,-1.26231]}, + {"t":5.40456, "x":1.0121, "y":0.39241, "heading":2.39757, "vx":1.49538, "vy":0.07956, "omega":-1.99874, "ax":0.00587, "ay":-0.11209, "alpha":0.0, "fx":[0.09327,0.09315,0.09307,0.09317], "fy":[-1.80262,-1.71324,-1.80031,-1.80158]}, + {"t":5.42634, "x":1.04468, "y":0.39412, "heading":2.35402, "vx":1.49551, "vy":0.07712, "omega":-1.99874, "ax":0.00843, "ay":-0.16754, "alpha":0.0, "fx":[0.13397,0.13502,0.13365,0.13257], "fy":[-2.68816,-2.58181,-2.6834,-2.68572]}, + {"t":5.44813, "x":1.07727, "y":0.39576, "heading":2.31048, "vx":1.49569, "vy":0.07347, "omega":-1.99874, "ax":0.01208, "ay":-0.25589, "alpha":0.0, "fx":[0.1919,0.19465,0.19165,0.18875], "fy":[-4.09893,-3.96608,-4.09034,-4.09422]}, + {"t":5.46992, "x":1.10986, "y":0.3973, "heading":2.26693, "vx":1.49596, "vy":0.06789, "omega":-1.99874, "ax":0.01582, "ay":-0.39169, "alpha":0.0, "fx":[0.25132,0.25642,0.25142,0.2456], "fy":[-6.26735,-6.09493,-6.25249,-6.2588]}, + {"t":5.4917, "x":1.14245, "y":0.39869, "heading":2.22339, "vx":1.4963, "vy":0.05936, "omega":-1.99874, "ax":-0.35762, "ay":-0.59287, "alpha":0.0, "fx":[-5.67723,-5.6691,-5.67625,-5.68738], "fy":[-9.48333,-9.24188,-9.45648,-9.46712]}, + {"t":5.51349, "x":1.17497, "y":0.39984, "heading":2.17984, "vx":1.48851, "vy":0.04644, "omega":-1.99874, "ax":-6.76218, "ay":-0.60231, "alpha":0.0, "fx":[-107.35217,-107.35859,-107.35377,-107.35381], "fy":[-9.58943,-9.51349,-9.56941,-9.57605]}, + {"t":5.53527, "x":1.20579, "y":0.40071, "heading":2.1363, "vx":1.34119, "vy":0.03332, "omega":-1.99874, "ax":-6.90887, "ay":-0.45202, "alpha":0.0, "fx":[-109.6839,-109.68363,-109.68403,-109.68201], "fy":[-7.15845,-7.21077,-7.16977,-7.16573]}, + {"t":5.55706, "x":1.23337, "y":0.40133, "heading":2.09275, "vx":1.19067, "vy":0.02347, "omega":-1.99874, "ax":-6.94313, "ay":-0.38624, "alpha":0.0, "fx":[-110.22877,-110.226,-110.22823,-110.22638], "fy":[-6.09516,-6.1957,-6.1227,-6.11395]}, + {"t":5.57885, "x":1.25766, "y":0.40175, "heading":2.04921, "vx":1.0394, "vy":0.01506, "omega":-1.99874, "ax":-6.95837, "ay":-0.34686, "alpha":0.0, "fx":[-110.47091,-110.4674,-110.47011,-110.46857], "fy":[-5.46182,-5.57945,-5.49782,-5.48729]}, + {"t":5.60063, "x":1.27866, "y":0.40199, "heading":2.00566, "vx":0.88781, "vy":0.0075, "omega":-1.99874, "ax":-6.9673, "ay":-0.31336, "alpha":0.0, "fx":[-110.61238,-110.60939,-110.61166,-110.61044], "fy":[-4.93251,-5.0401,-4.96818,-4.95855]}, + {"t":5.62242, "x":1.29634, "y":0.40208, "heading":1.96212, "vx":0.73601, "vy":0.00067, "omega":-1.99874, "ax":-6.97346, "ay":-0.27837, "alpha":0.0, "fx":[-110.7097,-110.70776,-110.70924,-110.7084], "fy":[-4.38721,-4.4666,-4.41535,-4.40837]}, + {"t":5.64421, "x":1.31072, "y":0.40203, "heading":1.91857, "vx":0.58409, "vy":-0.00539, "omega":-1.99874, "ax":-6.9782, "ay":-0.23765, "alpha":0.0, "fx":[-110.78449,-110.78369,-110.78428,-110.78398], "fy":[-3.75757,-3.79418,-3.77139,-3.76823]}, + {"t":5.66599, "x":1.32179, "y":0.40186, "heading":1.87503, "vx":0.43206, "vy":-0.01057, "omega":-1.99874, "ax":-6.98214, "ay":-0.18643, "alpha":0.0, "fx":[-110.84642,-110.84653,-110.84628,-110.8469], "fy":[-2.97297,-2.94146,-2.96094,-2.9633]}, + {"t":5.68778, "x":1.32955, "y":0.40158, "heading":1.83148, "vx":0.27994, "vy":-0.01463, "omega":-1.99874, "ax":-6.98548, "ay":-0.11594, "alpha":0.0, "fx":[-110.89954,-110.89929,-110.89878,-110.9009], "fy":[-1.89612,-1.7679,-1.84454,-1.85382]}, + {"t":5.70957, "x":1.33399, "y":0.40123, "heading":1.78794, "vx":0.12776, "vy":-0.01716, "omega":-1.99874, "ax":-6.98788, "ay":-0.00004, "alpha":0.0, "fx":[-110.93956,-110.93467,-110.93603,-110.94083], "fy":[-0.12284,0.15476,-0.00771,-0.02657]}, + {"t":5.73135, "x":1.33512, "y":0.40086, "heading":1.74439, "vx":-0.02448, "vy":-0.01716, "omega":-1.99874, "ax":-6.88508, "ay":1.2026, "alpha":0.0, "fx":[-109.31336,-109.29446,-109.30612,-109.30859], "fy":[19.04876,19.15438,19.0872,19.07858]}, + {"t":5.75314, "x":1.33295, "y":0.40077, "heading":1.70084, "vx":-0.17448, "vy":0.00904, "omega":-1.99874, "ax":-6.2518, "ay":3.12948, "alpha":0.0, "fx":[-99.25111,-99.25238,-99.25128,-99.25294], "fy":[49.68497,49.68098,49.68484,49.68063]}, + {"t":5.77492, "x":1.32766, "y":0.40171, "heading":1.6573, "vx":-0.31069, "vy":0.07722, "omega":-1.99874, "ax":-5.43908, "ay":4.3956, "alpha":0.00139, "fx":[-86.34899,-86.40426,-86.32764,-86.31638], "fy":[69.78434,69.73329,69.81065,69.80545]}, + {"t":5.79671, "x":1.3196, "y":0.40444, "heading":1.61375, "vx":-0.42919, "vy":0.17299, "omega":-1.99871, "ax":-4.79119, "ay":5.09585, "alpha":8.30139, "fx":[-111.88009,-69.81794,-38.35373,-84.20303], "fy":[40.66237,78.1091,114.4476,90.38244]}, + {"t":5.8185, "x":1.30912, "y":0.40942, "heading":1.57021, "vx":-0.53357, "vy":0.28401, "omega":-1.81785, "ax":-4.52666, "ay":5.3313, "alpha":7.2465, "fx":[-104.95557,-65.97647,-37.85568,-78.66835], "fy":[51.58142,81.63696,112.49695,92.83803]}, + {"t":5.85896, "x":1.28382, "y":0.42527, "heading":1.49665, "vx":-0.71674, "vy":0.49973, "omega":-1.52463, "ax":-4.51229, "ay":5.34547, "alpha":5.8485, "fx":[-98.22385,-69.50587,-43.44493,-75.36909], "fy":[59.92465,79.59091,107.4654,92.47219]}, + {"t":5.89943, "x":1.25112, "y":0.44987, "heading":1.43496, "vx":-0.89932, "vy":0.71603, "omega":-1.28797, "ax":-4.46334, "ay":5.38024, "alpha":4.61951, "fx":[-92.04167,-70.25907,-48.3627,-72.77157], "fy":[66.62803,80.09598,103.0375,91.89982]}, + {"t":5.93989, "x":1.21108, "y":0.48325, "heading":1.38284, "vx":-1.07993, "vy":0.93374, "omega":-1.10105, "ax":2.04954, "ay":4.60828, "alpha":8.53196, "fx":[-0.74962,59.19974,56.9475,14.75433], "fy":[58.6987,50.23457,85.68594,98.02036]}, + {"t":5.98035, "x":1.16906, "y":0.5248, "heading":1.33829, "vx":-0.997, "vy":1.12021, "omega":-0.75581, "ax":0.53076, "ay":0.46349, "alpha":10.07554, "fx":[-22.76585,27.66704,37.41857,-8.61464], "fy":[-14.94062,-24.42202,28.11098,40.68489]}, + {"t":6.02082, "x":1.12915, "y":0.57051, "heading":1.30771, "vx":-0.97552, "vy":1.13897, "omega":-0.34811, "ax":-0.03488, "ay":-0.02992, "alpha":7.55619, "fx":[-23.59933,11.98764,22.50227,-13.10548], "fy":[-15.91698,-25.48299,15.16753,24.33275]}, + {"t":6.06128, "x":1.08965, "y":0.61658, "heading":1.29362, "vx":-0.97693, "vy":1.13776, "omega":-0.04235, "ax":-0.00608, "ay":-0.00522, "alpha":5.2917, "fx":[-16.43019,8.48494,16.1632,-8.60385], "fy":[-11.02727,-17.43198,10.66824,17.45956]}, + {"t":6.10175, "x":1.05011, "y":0.66261, "heading":1.29191, "vx":-0.97718, "vy":1.13755, "omega":0.17177, "ax":-0.00847, "ay":-0.00728, "alpha":3.66744, "fx":[-11.31971,5.79178,11.15334,-6.16334], "fy":[-7.62151,-12.23618,7.36121,12.03428]}, + {"t":6.14221, "x":1.01056, "y":0.70863, "heading":1.29886, "vx":-0.97752, "vy":1.13725, "omega":0.32017, "ax":0.01241, "ay":0.01066, "alpha":2.47407, "fx":[-7.3397,4.21204,7.72905,-3.81361], "fy":[-4.92053,-8.04954,5.28021,8.36671]}, + {"t":6.18268, "x":0.97102, "y":0.75466, "heading":1.31181, "vx":-0.97702, "vy":1.13768, "omega":0.42028, "ax":-0.00415, "ay":-0.00357, "alpha":1.65749, "fx":[-5.01757,2.72615,4.96255,-2.93481], "fy":[-3.62783,-5.42706,3.43787,5.39054]}, + {"t":6.22314, "x":0.93148, "y":0.80069, "heading":1.32882, "vx":-0.97718, "vy":1.13754, "omega":0.48735, "ax":0.00371, "ay":0.00318, "alpha":1.07554, "fx":[-3.11586,1.88677,3.23328,-1.76878], "fy":[-2.21372,-3.55173,2.39033,3.57733]}, + {"t":6.2636, "x":0.89194, "y":0.84673, "heading":1.34854, "vx":-0.97703, "vy":1.13767, "omega":0.53088, "ax":-0.00341, "ay":-0.00293, "alpha":0.67861, "fx":[-2.00031,1.17274,1.95055,-1.33974], "fy":[-1.62848,-2.16704,1.46495,2.14439]}, + {"t":6.30407, "x":0.85241, "y":0.89276, "heading":1.37002, "vx":-0.97717, "vy":1.13755, "omega":0.55833, "ax":0.00207, "ay":0.00178, "alpha":0.38397, "fx":[-1.04419,0.72183,1.10993,-0.65611], "fy":[-0.79779,-1.28455,0.92455,1.27071]}, + {"t":6.34453, "x":0.81287, "y":0.93879, "heading":1.39261, "vx":-0.97709, "vy":1.13762, "omega":0.57387, "ax":-0.00143, "ay":-0.00123, "alpha":0.16294, "fx":[-0.44515,0.30116,0.44182,-0.38881], "fy":[-0.45661,-0.4843,0.36992,0.49283]}, + {"t":6.385, "x":0.77333, "y":0.98482, "heading":1.41583, "vx":-0.97715, "vy":1.13757, "omega":0.58047, "ax":0.00009, "ay":0.00008, "alpha":-0.03451, "fx":[0.12491,-0.0798,-0.12201,0.0827], "fy":[0.11287,0.04812,-0.06126,-0.09475]}, + {"t":6.42546, "x":0.73379, "y":1.03085, "heading":1.43932, "vx":-0.97714, "vy":1.13757, "omega":0.57907, "ax":0.00006, "ay":0.00006, "alpha":-0.23043, "fx":[0.66232,-0.46282,-0.63534,0.43996], "fy":[0.53905,0.7303,-0.55752,-0.70829]}, + {"t":6.46593, "x":0.69425, "y":1.07688, "heading":1.46275, "vx":-0.97714, "vy":1.13758, "omega":0.56974, "ax":-0.00269, "ay":-0.00231, "alpha":-0.46203, "fx":[1.22093,-1.02565,-1.30625,0.94021], "fy":[1.13462,1.32638,-1.18159,-1.4261]}, + {"t":6.50639, "x":0.65471, "y":1.12291, "heading":1.48581, "vx":-0.97725, "vy":1.13748, "omega":0.55105, "ax":0.00714, "ay":0.00613, "alpha":-0.76309, "fx":[2.1427,-1.54316,-1.90766,1.76164], "fy":[2.05917,2.36268,-1.85668,-2.17566]}, + {"t":6.54685, "x":0.61517, "y":1.16895, "heading":1.50811, "vx":-0.97696, "vy":1.13773, "omega":0.52017, "ax":-0.00176, "ay":-0.00151, "alpha":-1.17585, "fx":[3.02954,-2.66415,-3.08514,2.60817], "fy":[3.04675,3.41447,-3.09319,-3.46386]}, + {"t":6.58732, "x":0.57564, "y":1.21498, "heading":1.52915, "vx":-0.97703, "vy":1.13767, "omega":0.47259, "ax":0.01015, "ay":0.00872, "alpha":-1.7631, "fx":[4.6011,-3.89164,-4.32078,4.25611], "fy":[4.88658,5.17473,-4.56293,-4.94476]}, + {"t":6.62778, "x":0.53611, "y":1.26103, "heading":1.54828, "vx":-0.97662, "vy":1.13802, "omega":0.40125, "ax":-0.06587, "ay":-0.05666, "alpha":-2.58998, "fx":[5.35226,-7.11242,-7.46212,5.03952], "fy":[5.95586,6.67414,-7.94446,-8.28371]}, + {"t":6.66825, "x":0.49654, "y":1.30703, "heading":1.56451, "vx":-0.97929, "vy":1.13573, "omega":0.29645, "ax":0.3283, "ay":0.27976, "alpha":-3.62418, "fx":[13.60427,-3.59606,-3.53129,14.37112], "fy":[14.27001,14.74757,-5.45539,-5.7966]}, + {"t":6.70871, "x":0.45718, "y":1.35321, "heading":1.57651, "vx":-0.966, "vy":1.14705, "omega":0.1498, "ax":3.25464, "ay":2.44814, "alpha":-3.79183, "fx":[58.32113,41.38393,44.64123,62.3332], "fy":[48.28693,51.41981,29.02364,26.73384]}, + {"t":6.74918, "x":0.42076, "y":1.40163, "heading":1.58257, "vx":-0.8343, "vy":1.24611, "omega":-0.00364, "ax":6.08584, "ay":3.2827, "alpha":-2.06743, "fx":[96.58757,90.23234,97.23346,102.41549], "fy":[56.91302,62.48842,46.78882,42.27108]}, + {"t":6.78964, "x":0.39198, "y":1.45474, "heading":1.58242, "vx":-0.58805, "vy":1.37895, "omega":-0.08729, "ax":6.44958, "ay":2.23744, "alpha":-1.39447, "fx":[102.43852,99.29778,102.55005,105.28083], "fy":[39.6215,42.49384,31.07413,28.8947]}, + {"t":6.82084, "x":0.37677, "y":1.49885, "heading":1.5797, "vx":-0.38684, "vy":1.44875, "omega":-0.1308, "ax":6.00724, "ay":1.20045, "alpha":-1.59084, "fx":[96.88443,92.49427,94.09195,98.0067], "fy":[24.18297,25.87191,13.4462,12.73123]}, + {"t":6.85203, "x":0.36763, "y":1.54463, "heading":1.57562, "vx":-0.19943, "vy":1.4862, "omega":-0.18043, "ax":1.58022, "ay":0.18554, "alpha":-1.79911, "fx":[29.04518,20.89276,21.12236,29.28799], "fy":[7.93646,8.43339,-2.29067,-2.29692]}, + {"t":6.88323, "x":0.36218, "y":1.59108, "heading":1.56999, "vx":-0.15014, "vy":1.49198, "omega":-0.23655, "ax":0.47068, "ay":0.04503, "alpha":-0.9562, "fx":[9.42479,5.09293,5.11743,10.25457], "fy":[3.38808,3.26333,-1.89061,-1.90146]}, + {"t":6.91443, "x":0.35772, "y":1.63765, "heading":1.56261, "vx":-0.13545, "vy":1.49339, "omega":-0.26638, "ax":-0.266, "ay":-0.02488, "alpha":0.01469, "fx":[-4.28528,-4.16093,-4.16065,-4.2852], "fy":[-0.52236,-0.30327,-0.37773,-0.37635]}, + {"t":6.94562, "x":0.35337, "y":1.68423, "heading":1.5543, "vx":-0.14375, "vy":1.49261, "omega":-0.26592, "ax":0.16466, "ay":0.01558, "alpha":0.93985, "fx":[-0.0066,4.73753,4.80817,0.91702], "fy":[-2.31755,-2.60379,2.90981,3.00063]}, + {"t":6.97682, "x":0.34896, "y":1.7308, "heading":1.54601, "vx":-0.13862, "vy":1.4931, "omega":-0.2366, "ax":-0.22631, "ay":-0.02155, "alpha":2.02913, "fx":[-8.66748,1.22683,1.48219,-8.41286], "fy":[-5.98431,-5.91135,5.14696,5.38005]}, + {"t":7.00802, "x":0.34453, "y":1.77737, "heading":1.53862, "vx":-0.14568, "vy":1.49243, "omega":-0.1733, "ax":0.31026, "ay":0.02927, "alpha":3.08594, "fx":[-3.14173,11.88897,12.35636,-1.40087], "fy":[-7.99552,-8.66149,8.9335,9.58234]}, + {"t":7.03921, "x":0.34013, "y":1.82394, "heading":1.53322, "vx":-0.136, "vy":1.49334, "omega":-0.07703, "ax":0.30572, "ay":0.02685, "alpha":4.44438, "fx":[-6.14835,15.02091,15.8122,-5.27049], "fy":[-12.13263,-12.03384,12.34457,13.52709]}, + {"t":7.07041, "x":0.33604, "y":1.87054, "heading":1.53081, "vx":-0.12646, "vy":1.49418, "omega":0.06162, "ax":3.30056, "ay":0.16519, "alpha":5.01024, "fx":[41.46933,62.38578,63.07133,42.66868], "fy":[-13.09088,-12.60996,16.74889,19.44176]}, + {"t":7.10161, "x":0.3337, "y":1.91724, "heading":1.53274, "vx":-0.02349, "vy":1.49933, "omega":0.21792, "ax":6.72641, "ay":-0.36741, "alpha":2.82542, "fx":[103.97825,108.35543,109.44482,105.3683], "fy":[-18.92039,-16.68188,5.18095,7.08957]}, + {"t":7.1328, "x":0.33624, "y":1.96383, "heading":1.53954, "vx":0.18635, "vy":1.48787, "omega":0.30606, "ax":6.80871, "ay":-1.35973, "alpha":2.93969, "fx":[103.30015,108.70978,112.21132,108.15204], "fy":[-36.44423,-31.32523,-8.93518,-9.64236]}, + {"t":7.164, "x":0.34537, "y":2.00959, "heading":1.54908, "vx":0.39876, "vy":1.44545, "omega":0.39777, "ax":6.57085, "ay":-2.34096, "alpha":3.31216, "fx":[96.44676,104.52565,111.01638,105.27927], "fy":[-54.14351,-46.51512,-22.04322,-25.95578]}, + {"t":7.1952, "x":0.361, "y":2.05354, "heading":1.56149, "vx":0.60374, "vy":1.37242, "omega":0.5011, "ax":6.15977, "ay":-3.29602, "alpha":3.75831, "fx":[85.85194,98.2918,108.0576,98.96215], "fy":[-71.41555,-60.58389,-34.52963,-42.77805]}, + {"t":7.22639, "x":0.38284, "y":2.09475, "heading":1.57713, "vx":0.79591, "vy":1.26959, "omega":0.61835, "ax":5.6508, "ay":-4.08307, "alpha":3.67763, "fx":[75.64974,90.69377,102.21293,90.28566], "fy":[-82.64364,-71.23139,-47.39604,-58.01586]}, + {"t":7.25641, "x":0.40927, "y":2.13102, "heading":1.59568, "vx":0.9655, "vy":1.14705, "omega":0.72872, "ax":5.01236, "ay":-4.85118, "alpha":3.31229, "fx":[65.13983,81.77596,93.32707,78.05655], "fy":[-91.67102,-80.90488,-61.71519,-73.77276]}, + {"t":7.28642, "x":0.4405, "y":2.16326, "heading":1.61755, "vx":1.11594, "vy":1.00146, "omega":0.82813, "ax":4.27067, "ay":-5.4794, "alpha":3.13844, "fx":[53.06141,71.5359,82.86788,63.73509], "fy":[-99.0738,-89.28102,-73.47943,-86.12383]}, + {"t":7.31643, "x":0.47592, "y":2.19085, "heading":1.64241, "vx":1.24411, "vy":0.837, "omega":0.92232, "ax":3.40513, "ay":-5.8896, "alpha":3.39363, "fx":[38.6386,60.43081,71.9802,45.18609], "fy":[-104.08174,-94.92158,-80.56741,-94.43594]}, + {"t":7.34644, "x":0.51479, "y":2.21331, "heading":1.67009, "vx":1.34631, "vy":0.66024, "omega":1.02418, "ax":1.62364, "ay":-3.67751, "alpha":7.7649, "fx":[6.22794,44.73509,50.16592,1.97693], "fy":[-81.55033,-73.01994,-35.13216,-43.83038]}, + {"t":7.37646, "x":0.55593, "y":2.23147, "heading":1.70083, "vx":1.39504, "vy":0.54987, "omega":1.25722, "ax":0.38008, "ay":-0.99521, "alpha":9.0678, "fx":[-12.85845,30.10269,27.03983,-20.14818], "fy":[-43.82513,-36.36754,12.17707,4.81709]}, + {"t":7.40647, "x":0.59797, "y":2.24753, "heading":1.73856, "vx":1.40644, "vy":0.52, "omega":1.52937, "ax":0.02197, "ay":-0.05957, "alpha":8.79055, "fx":[-16.0779,25.33352,17.89538,-25.75554], "fy":[-28.57795,-20.78198,26.84575,18.73144]}, + {"t":7.43648, "x":0.64019, "y":2.26311, "heading":1.78446, "vx":1.4071, "vy":0.51822, "omega":1.79319, "ax":-0.01185, "ay":0.03214, "alpha":7.89464, "fx":[-13.99852,23.23758,13.68721,-23.67875], "fy":[-25.2391,-16.42645,26.3985,17.30835]}, + {"t":7.46649, "x":0.68241, "y":2.27867, "heading":1.83828, "vx":1.40675, "vy":0.51918, "omega":2.03013, "ax":-0.02151, "ay":0.05817, "alpha":7.09029, "fx":[-11.22982,21.75498,10.69783,-22.58878], "fy":[-22.80729,-12.90251,24.64398,14.76007]}, + {"t":7.49651, "x":0.72463, "y":2.29428, "heading":1.89921, "vx":1.4061, "vy":0.52093, "omega":2.24293, "ax":0.00192, "ay":-0.0052, "alpha":6.13209, "fx":[-8.30522,19.86956,8.11153,-19.55376], "fy":[-21.08023,-11.1437,21.15022,10.74356]}, + {"t":7.52652, "x":0.76683, "y":2.30991, "heading":1.96652, "vx":1.40616, "vy":0.52077, "omega":2.42697, "ax":0.00002, "ay":-0.00005, "alpha":5.33661, "fx":[-5.81058,17.91547,5.8246,-17.92839], "fy":[-18.82072,-8.09992,18.74049,8.17724]}, + {"t":7.55653, "x":0.80903, "y":2.32554, "heading":2.03936, "vx":1.40616, "vy":0.52077, "omega":2.58714, "ax":0.00559, "ay":-0.0151, "alpha":4.42824, "fx":[-3.73981,15.4467,3.66137,-15.01321], "fy":[-16.03952,-6.17532,15.7855,5.47024]}, + {"t":7.58654, "x":0.85123, "y":2.34117, "heading":2.11701, "vx":1.40633, "vy":0.52032, "omega":2.72004, "ax":0.0005, "ay":-0.00135, "alpha":3.67667, "fx":[-2.00612,13.06342,2.02194,-13.04767], "fy":[-13.42379,-3.72122,13.33386,3.72567]}, + {"t":7.61656, "x":0.89344, "y":2.35678, "heading":2.19865, "vx":1.40634, "vy":0.52027, "omega":2.83038, "ax":0.0017, "ay":-0.0046, "alpha":2.82256, "fx":[-0.78716,10.29604,0.6443,-10.04509], "fy":[-10.38117,-2.28063,10.40205,1.96764]}, + {"t":7.64657, "x":0.93565, "y":2.37239, "heading":2.28359, "vx":1.40639, "vy":0.52014, "omega":2.9151, "ax":-0.0012, "ay":0.00324, "alpha":2.09485, "fx":[0.09935,7.67382,-0.13754,-7.7118], "fy":[-7.64644,-0.85384,7.74415,0.96201]}, + {"t":7.67658, "x":0.97786, "y":2.38801, "heading":2.37108, "vx":1.40636, "vy":0.52023, "omega":2.97797, "ax":0.00009, "ay":-0.00025, "alpha":1.28842, "fx":[0.4292,4.79775,-0.54853,-4.67255], "fy":[-4.6876,-0.21866,4.7807,0.10969]}, + {"t":7.70659, "x":1.02007, "y":2.40362, "heading":2.46046, "vx":1.40636, "vy":0.52023, "omega":3.01664, "ax":-0.00097, "ay":0.00263, "alpha":0.5614, "fx":[0.37068,2.06795,-0.40154,-2.09879], "fy":[-1.96631,0.12417,2.06534,-0.0564]}, + {"t":7.73661, "x":1.06228, "y":2.41923, "heading":2.551, "vx":1.40633, "vy":0.52031, "omega":3.03349, "ax":0.00007, "ay":-0.0002, "alpha":-0.21112, "fx":[-0.22917,-0.75406,0.18879,0.79904], "fy":[0.75486,-0.12991,-0.73056,0.09317]}, + {"t":7.76662, "x":1.10448, "y":2.43485, "heading":2.64204, "vx":1.40633, "vy":0.5203, "omega":3.02715, "ax":-0.00033, "ay":0.0009, "alpha":-0.95912, "fx":[-1.27148,-3.43521,1.26095,3.42465], "fy":[3.32573,-0.81532,-3.28918,0.8358]}, + {"t":7.79663, "x":1.14669, "y":2.45047, "heading":2.73289, "vx":1.40632, "vy":0.52033, "omega":2.99836, "ax":0.00012, "ay":-0.00033, "alpha":-1.71909, "fx":[-2.77781,-6.02482,2.81536,5.99496], "fy":[5.66222,-1.99134,-5.70751,2.01583]}, + {"t":7.82664, "x":1.1889, "y":2.46608, "heading":2.82288, "vx":1.40633, "vy":0.52032, "omega":2.94677, "ax":0.00005, "ay":-0.00012, "alpha":-2.50416, "fx":[-4.76076,-8.47612,4.76213,8.47769], "fy":[7.89726,-3.61607,-7.92898,3.63988]}, + {"t":7.85666, "x":1.23111, "y":2.4817, "heading":2.91132, "vx":1.40633, "vy":0.52031, "omega":2.87161, "ax":-0.00014, "ay":0.00039, "alpha":-3.27855, "fx":[-7.08973,-10.66716,7.1845,10.56325], "fy":[9.72224,-5.70169,-9.79627,5.80042]}, + {"t":7.88667, "x":1.27331, "y":2.49731, "heading":2.9975, "vx":1.40632, "vy":0.52032, "omega":2.77322, "ax":0.00044, "ay":-0.00119, "alpha":-4.1106, "fx":[-9.89838,-12.71769,9.91169,12.73233], "fy":[11.43013,-8.14663,-11.55528,8.19623]}, + {"t":7.91668, "x":1.31552, "y":2.51293, "heading":3.08073, "vx":1.40634, "vy":0.52029, "omega":2.64985, "ax":-0.00058, "ay":0.00156, "alpha":-4.92707, "fx":[-12.92122,-14.39271,13.05343,14.2238], "fy":[12.67826,-11.06374,-12.74766,11.23237]}, + {"t":7.94669, "x":1.35773, "y":2.52855, "heading":-3.12292, "vx":1.40632, "vy":0.52034, "omega":2.50197, "ax":0.00147, "ay":-0.00397, "alpha":-5.81107, "fx":[-16.38491,-15.91901,16.42854,15.9686], "fy":[13.70838,-14.18882,-13.99688,14.22532]}, + {"t":7.97671, "x":1.39994, "y":2.54416, "heading":-3.04783, "vx":1.40636, "vy":0.52022, "omega":2.32757, "ax":-0.00136, "ay":0.00368, "alpha":-6.69219, "fx":[-19.95187,-17.05416,20.0918,16.82787], "fy":[14.42931,-17.76359,-14.44441,18.01218]}, + {"t":8.00672, "x":1.44214, "y":2.55978, "heading":-2.97798, "vx":1.40632, "vy":0.52033, "omega":2.12672, "ax":0.00302, "ay":-0.00818, "alpha":-7.62746, "fx":[-23.86664,-18.01353,23.94774,18.12449], "fy":[14.72244,-21.39882,-15.23429,21.39143]}, + {"t":8.03673, "x":1.48435, "y":2.57539, "heading":-2.91415, "vx":1.40641, "vy":0.52008, "omega":1.8978, "ax":-0.0056, "ay":0.01512, "alpha":-8.57405, "fx":[-27.80189,-18.70031,27.89495,18.25191], "fy":[15.16307,-25.26145,-14.84506,25.90391]}, + {"t":8.06674, "x":1.52656, "y":2.591, "heading":-2.85719, "vx":1.40625, "vy":0.52054, "omega":1.64047, "ax":0.00734, "ay":-0.01985, "alpha":-9.55989, "fx":[-31.91877,-19.06221,32.03512,19.41223], "fy":[14.60732,-29.50154,-15.57961,29.21307]}, + {"t":8.09676, "x":1.56877, "y":2.60662, "heading":-2.80795, "vx":1.40647, "vy":0.51994, "omega":1.35355, "ax":-0.01448, "ay":0.03898, "alpha":-10.55371, "fx":[-36.04417,-19.62754,35.96532,18.78678], "fy":[15.31116,-33.0306,-14.31088,34.50563]}, + {"t":8.12677, "x":1.61097, "y":2.62224, "heading":-2.76733, "vx":1.40603, "vy":0.52111, "omega":1.03681, "ax":-0.93216, "ay":-0.38339, "alpha":-11.09896, "fx":[-52.5501,-33.50654,23.98654,2.87524], "fy":[7.30401,-40.80543,-22.84194,31.9967]}, + {"t":8.15678, "x":1.65275, "y":2.63771, "heading":-2.73621, "vx":1.37806, "vy":0.5096, "omega":0.7037, "ax":-6.54944, "ay":-2.42002, "alpha":-2.73256, "fx":[-110.09598,-102.23463,-96.99015,-106.58798], "fy":[-29.7539,-50.20265,-49.65828,-24.06367]}, + {"t":8.18679, "x":1.69116, "y":2.65191, "heading":-2.71509, "vx":1.18149, "vy":0.43697, "omega":0.62169, "ax":-6.55747, "ay":-2.42458, "alpha":-2.86891, "fx":[-110.5309,-102.18821,-96.83927,-106.86044], "fy":[-29.82264,-51.10572,-49.79071,-23.24879]}, + {"t":8.21681, "x":1.72367, "y":2.66393, "heading":-2.69644, "vx":0.98468, "vy":0.3642, "omega":0.53559, "ax":-6.56007, "ay":-2.42601, "alpha":-3.08905, "fx":[-111.00829,-101.98385,-96.36804,-107.2236], "fy":[-29.57083,-52.21481,-50.43346,-21.8399]}, + {"t":8.24682, "x":1.75027, "y":2.67377, "heading":-2.68036, "vx":0.7878, "vy":0.29139, "omega":0.44288, "ax":-6.56135, "ay":-2.42674, "alpha":-3.32657, "fx":[-111.48389,-101.74604,-95.83475,-107.60011], "fy":[-29.27374,-53.36197,-51.17595,-20.2933]}, + {"t":8.27683, "x":1.77096, "y":2.68143, "heading":-2.66707, "vx":0.59088, "vy":0.21856, "omega":0.34304, "ax":-6.5621, "ay":-2.42718, "alpha":-3.56721, "fx":[-111.9539,-101.49962,-95.29258,-107.96671], "fy":[-28.97605,-54.50322,-51.93987,-18.71381]}, + {"t":8.30684, "x":1.78573, "y":2.68689, "heading":-2.65677, "vx":0.39393, "vy":0.14571, "omega":0.23598, "ax":-6.56261, "ay":-2.42746, "alpha":-3.8101, "fx":[-112.42492,-101.27534,-94.71869,-108.32599], "fy":[-28.6419,-55.59495,-52.78185,-17.13215]}, + {"t":8.33686, "x":1.7946, "y":2.69017, "heading":-2.64969, "vx":0.19697, "vy":0.07286, "omega":0.12163, "ax":-6.56297, "ay":-2.42767, "alpha":-4.05252, "fx":[-112.89804,-101.07033,-94.13499,-108.66422], "fy":[-28.28114,-56.64687,-53.6699,-15.56631]}, + {"t":8.36687, "x":1.79756, "y":2.69126, "heading":-2.64604, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[ + {"name":"IntakeDown", "from":{"target":2, "targetTimestamp":1.73619, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeDown"}}}, + {"name":"IntakeUp", "from":{"target":6, "targetTimestamp":7.22639, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeUp"}}}] +} diff --git a/src/main/deploy/choreo/RightIntakeSweepShort.traj b/src/main/deploy/choreo/RightIntakeSweepShort.traj new file mode 100644 index 0000000..d4da1e9 --- /dev/null +++ b/src/main/deploy/choreo/RightIntakeSweepShort.traj @@ -0,0 +1,197 @@ +{ + "name":"RightIntakeSweepShort", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":5.61296272277832, "y":2.1455330848693848, "heading":1.57, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.789466857910156, "y":1.0683220624923706, "heading":1.57, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.74118185043335, "y":1.225956916809082, "heading":1.57, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.74118185043335, "y":3.2714996337890625, "heading":1.57, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.803861618041992, "y":3.0815274715423584, "heading":1.5707963267948966, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.622209548950195, "y":2.4440572261810303, "heading":1.57, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":2, "to":3, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":5.0}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":7.0}}, "enabled":true}, + {"from":0, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":10.0}}, "enabled":true}, + {"from":3, "to":5, "data":{"type":"MaxVelocity", "props":{"max":5.0}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"5.61296272277832 m", "val":5.61296272277832}, "y":{"exp":"2.1455330848693848 m", "val":2.1455330848693848}, "heading":{"exp":"3.14 / 2 rad", "val":1.57}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.789466857910156 m", "val":6.789466857910156}, "y":{"exp":"1.0683220624923706 m", "val":1.0683220624923706}, "heading":{"exp":"3.14 / 2 rad", "val":1.57}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.74118185043335 m", "val":7.74118185043335}, "y":{"exp":"1.225956916809082 m", "val":1.225956916809082}, "heading":{"exp":"3.14 / 2 rad", "val":1.57}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.74118185043335 m", "val":7.74118185043335}, "y":{"exp":"3.2714996337890625 m", "val":3.2714996337890625}, "heading":{"exp":"3.14 / 2 rad", "val":1.57}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.803861618041992 m", "val":6.803861618041992}, "y":{"exp":"3.0815274715423584 m", "val":3.0815274715423584}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.622209548950195 m", "val":5.622209548950195}, "y":{"exp":"2.4440572261810303 m", "val":2.4440572261810303}, "heading":{"exp":"3.14 / 2 rad", "val":1.57}, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":2, "to":3, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"5 m / s", "val":5.0}}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"7 m / s ^ 2", "val":7.0}}}, "enabled":true}, + {"from":0, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"10 rad / s", "val":10.0}}}, "enabled":true}, + {"from":3, "to":5, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"5 m / s", "val":5.0}}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.42339,0.93184,3.00243,3.50834,4.13403], + "samples":[ + {"t":0.0, "x":5.61296, "y":2.14553, "heading":1.57, "vx":2.81602, "vy":-4.00734, "omega":-0.00123, "ax":0.17519, "ay":6.33517, "alpha":0.07981, "fx":[2.7796,2.81451,2.7501,2.7806], "fy":[100.98036,99.39184,100.96501,100.96435]}, + {"t":0.02117, "x":5.67262, "y":2.06212, "heading":1.56997, "vx":2.81973, "vy":-3.87322, "omega":0.00046, "ax":-0.11057, "ay":6.92432, "alpha":-0.00111, "fx":[-1.7482,-1.75914,-1.76634,-1.74782], "fy":[109.93263,109.92273,109.92958,109.92984]}, + {"t":0.04234, "x":5.73228, "y":1.98168, "heading":1.56998, "vx":2.81739, "vy":-3.72664, "omega":0.00044, "ax":-0.16552, "ay":6.9564, "alpha":-0.0077, "fx":[-2.64663,-2.67095,-2.67079,-2.5227], "fy":[110.43745,110.43845,110.43621,110.43951]}, + {"t":0.06351, "x":5.79189, "y":1.90434, "heading":1.56999, "vx":2.81388, "vy":-3.57937, "omega":0.00027, "ax":-0.19105, "ay":6.96859, "alpha":-0.00109, "fx":[-3.02784,-3.03716,-3.03918,-3.02792], "fy":[110.6325,110.63213,110.63051,110.63078]}, + {"t":0.08468, "x":5.85142, "y":1.83013, "heading":1.57, "vx":2.80984, "vy":-3.43185, "omega":0.00025, "ax":-0.20805, "ay":6.97491, "alpha":-0.00503, "fx":[-3.32135,-3.33097,-3.3311,-3.22805], "fy":[110.73188,110.73179,110.73029,110.73324]}, + {"t":0.10585, "x":5.91085, "y":1.75904, "heading":1.57, "vx":2.80544, "vy":-3.28419, "omega":0.00014, "ax":-0.21918, "ay":6.97878, "alpha":-0.0007, "fx":[-3.47638,-3.48199,-3.4836,-3.47654], "fy":[110.79389,110.79384,110.7925,110.79269]}, + {"t":0.12702, "x":5.97019, "y":1.69108, "heading":1.57001, "vx":2.8008, "vy":-3.13646, "omega":0.00013, "ax":-0.22794, "ay":6.98136, "alpha":-0.00337, "fx":[-3.63141,-3.63751,-3.63765,-3.5683], "fy":[110.83423,110.83417,110.8331,110.83527]}, + {"t":0.14819, "x":6.02944, "y":1.62625, "heading":1.57001, "vx":2.79597, "vy":-2.98866, "omega":0.00006, "ax":-0.23457, "ay":6.98321, "alpha":-0.00039, "fx":[-3.72228,-3.72362,-3.72733,-3.72243], "fy":[110.86397,110.86428,110.86295,110.86309]}, + {"t":0.16936, "x":6.08857, "y":1.56454, "heading":1.57001, "vx":2.791, "vy":-2.84083, "omega":0.00005, "ax":-0.24008, "ay":6.98459, "alpha":-0.00233, "fx":[-3.81868,-3.82433,-3.82446,-3.77829], "fy":[110.88565,110.88543,110.88465,110.88617]}, + {"t":0.19053, "x":6.1476, "y":1.50597, "heading":1.57001, "vx":2.78592, "vy":-2.69297, "omega":0.0, "ax":-0.24456, "ay":6.98566, "alpha":-0.00027, "fx":[-3.88149,-3.8824,-3.88503,-3.88161], "fy":[110.90278,110.90297,110.90205,110.90215]}, + {"t":0.2117, "x":6.20653, "y":1.45052, "heading":1.57001, "vx":2.78074, "vy":-2.54508, "omega":-0.00001, "ax":-0.24842, "ay":6.98651, "alpha":-0.00166, "fx":[-3.94521,-3.95282,-3.95293,-3.92416], "fy":[110.91648,110.91588,110.91533,110.91631]}, + {"t":0.23287, "x":6.26534, "y":1.39821, "heading":1.57001, "vx":2.77549, "vy":-2.39718, "omega":-0.00004, "ax":-0.25169, "ay":6.98721, "alpha":-0.00021, "fx":[-3.99478,-3.99605,-3.99717,-3.99488], "fy":[110.92724,110.92726,110.92675,110.92682]}, + {"t":0.25404, "x":6.32404, "y":1.34903, "heading":1.57001, "vx":2.77016, "vy":-2.24926, "omega":-0.00004, "ax":-0.25455, "ay":6.98778, "alpha":-0.00091, "fx":[-4.04154,-4.04616,-4.04626,-4.031], "fy":[110.93639,110.9361,110.93577,110.9363]}, + {"t":0.27521, "x":6.38262, "y":1.30298, "heading":1.57001, "vx":2.76477, "vy":-2.10133, "omega":-0.00006, "ax":-0.25706, "ay":6.98826, "alpha":-0.0001, "fx":[-4.08059,-4.08095,-4.08205,-4.08068], "fy":[110.94393,110.94398,110.94367,110.94371]}, + {"t":0.29638, "x":6.4411, "y":1.26006, "heading":1.57001, "vx":2.75933, "vy":-1.9534, "omega":-0.00007, "ax":-0.25929, "ay":6.98868, "alpha":-0.00037, "fx":[-4.11487,-4.11842,-4.1185,-4.11401], "fy":[110.95055,110.95034,110.95021,110.95037]}, + {"t":0.31755, "x":6.49945, "y":1.22027, "heading":1.57001, "vx":2.75384, "vy":-1.80545, "omega":-0.00007, "ax":-0.26128, "ay":6.98903, "alpha":-0.00001, "fx":[-4.14803,-4.1475,-4.14862,-4.14809], "fy":[110.95601,110.95608,110.95596,110.95598]}, + {"t":0.33872, "x":6.55769, "y":1.18362, "heading":1.57001, "vx":2.74831, "vy":-1.65749, "omega":-0.00007, "ax":-0.26307, "ay":6.98934, "alpha":0.0001, "fx":[-4.17353,-4.17578,-4.17583,-4.18055], "fy":[110.96102,110.9609,110.96097,110.9608]}, + {"t":0.35989, "x":6.61581, "y":1.15009, "heading":1.57, "vx":2.74274, "vy":-1.50953, "omega":-0.00007, "ax":-0.26469, "ay":6.98961, "alpha":0.00006, "fx":[-4.20241,-4.20161,-4.20216,-4.20245], "fy":[110.96515,110.96517,110.96531,110.96531]}, + {"t":0.38105, "x":6.67382, "y":1.1197, "heading":1.57, "vx":2.73713, "vy":-1.36156, "omega":-0.00007, "ax":-0.26616, "ay":6.98985, "alpha":0.00049, "fx":[-4.22145,-4.22265,-4.22268,-4.23488], "fy":[110.96907,110.96902,110.96929,110.96884]}, + {"t":0.40222, "x":6.7317, "y":1.09245, "heading":1.57, "vx":2.7315, "vy":-1.21359, "omega":-0.00006, "ax":-0.2675, "ay":6.99007, "alpha":0.00015, "fx":[-4.2474,-4.24597,-4.2463,-4.24743], "fy":[110.97226,110.97228,110.97265,110.97262]}, + {"t":0.42339, "x":6.78947, "y":1.06832, "heading":1.57, "vx":2.72584, "vy":-1.06561, "omega":-0.00006, "ax":-0.51101, "ay":6.97525, "alpha":0.00019, "fx":[-8.11338,-8.11169,-8.11207,-8.11348], "fy":[110.73699,110.73703,110.73755,110.73748]}, + {"t":0.45729, "x":6.88157, "y":1.03621, "heading":1.57, "vx":2.70851, "vy":-0.82917, "omega":-0.00005, "ax":-1.05114, "ay":6.91734, "alpha":0.00096, "fx":[-16.68116,-16.68126,-16.6815,-16.70652], "fy":[109.81854,109.81847,109.81888,109.81524]}, + {"t":0.49119, "x":6.97277, "y":1.01208, "heading":1.57, "vx":2.67288, "vy":-0.5947, "omega":-0.00002, "ax":-1.66575, "ay":6.79532, "alpha":-0.00003, "fx":[-26.44433,-26.44731,-26.44396,-26.44472], "fy":[107.88071,107.87992,107.88106,107.88096]}, + {"t":0.52508, "x":7.06242, "y":0.99582, "heading":1.57, "vx":2.61642, "vy":-0.36436, "omega":-0.00002, "ax":-2.35329, "ay":6.58861, "alpha":0.00081, "fx":[-37.35492,-37.35307,-37.35365,-37.37919], "fy":[104.60078,104.60128,104.60137,104.59261]}, + {"t":0.55898, "x":7.14975, "y":0.98726, "heading":1.57, "vx":2.53665, "vy":-0.14103, "omega":0.00001, "ax":-3.1009, "ay":6.27129, "alpha":-0.00023, "fx":[-49.22578,-49.23754,-49.22633,-49.22654], "fy":[99.56287,99.55695,99.5627,99.56271]}, + {"t":0.59288, "x":7.23396, "y":0.98608, "heading":1.57, "vx":2.43154, "vy":0.07154, "omega":0.0, "ax":-3.88028, "ay":5.82113, "alpha":0.00067, "fx":[-61.60123,-61.59502,-61.59592,-61.61691], "fy":[92.41537,92.41921,92.41879,92.40518]}, + {"t":0.62677, "x":7.31415, "y":0.99185, "heading":1.57, "vx":2.30002, "vy":0.26886, "omega":0.00002, "ax":-4.64731, "ay":5.22904, "alpha":-0.00007, "fx":[-73.7749,-73.7909,-73.776,-73.7759], "fy":[83.019,83.00445,83.01801,83.01821]}, + {"t":0.66067, "x":7.38944, "y":1.00397, "heading":1.57, "vx":2.14249, "vy":0.4461, "omega":0.00002, "ax":-5.34911, "ay":4.50854, "alpha":0.00039, "fx":[-84.9229,-84.91675,-84.91774,-84.92657], "fy":[71.57413,71.58125,71.58013,71.56983]}, + {"t":0.69457, "x":7.45899, "y":1.02168, "heading":1.57, "vx":1.96117, "vy":0.59893, "omega":0.00003, "ax":-5.93869, "ay":3.69764, "alpha":0.00021, "fx":[-94.27815,-94.28792,-94.27925,-94.27905], "fy":[58.70752,58.69143,58.70573,58.70605]}, + {"t":0.72846, "x":7.52206, "y":1.0441, "heading":1.57, "vx":1.75987, "vy":0.72426, "omega":0.00004, "ax":-6.38908, "ay":2.84993, "alpha":-0.0001, "fx":[-101.43138,-101.43036,-101.4311,-101.43234], "fy":[45.24458,45.24692,45.24519,45.24231]}, + {"t":0.76236, "x":7.57804, "y":1.07029, "heading":1.57, "vx":1.54331, "vy":0.82087, "omega":0.00004, "ax":-6.69856, "ay":2.01852, "alpha":-0.00007, "fx":[-106.34388,-106.34519,-106.34462,-106.34443], "fy":[32.04752,32.04343,32.04531,32.04559]}, + {"t":0.79626, "x":7.6265, "y":1.09927, "heading":1.57, "vx":1.31625, "vy":0.88929, "omega":0.00004, "ax":-6.88498, "ay":1.24313, "alpha":-0.00062, "fx":[-109.30291,-109.30422,-109.30456,-109.30453], "fy":[19.74152,19.73512,19.73305,19.73259]}, + {"t":0.83015, "x":7.66716, "y":1.13013, "heading":1.57, "vx":1.08287, "vy":0.93142, "omega":0.00001, "ax":-6.97523, "ay":0.54572, "alpha":-0.00087, "fx":[-110.73693,-110.73617,-110.73732,-110.73708], "fy":[8.66155,8.67458,8.65925,8.65935]}, + {"t":0.86405, "x":7.69986, "y":1.16202, "heading":1.57, "vx":0.84644, "vy":0.94992, "omega":-0.00001, "ax":-6.99646, "ay":-0.06683, "alpha":-0.00127, "fx":[-111.07397,-111.07408,-111.07406,-111.07376], "fy":[-1.04495,-1.06483,-1.06712,-1.06709]}, + {"t":0.89794, "x":7.72453, "y":1.19418, "heading":1.57, "vx":0.60928, "vy":0.94766, "omega":-0.00006, "ax":-6.97145, "ay":-0.59745, "alpha":-0.00156, "fx":[-110.67634,-110.67867,-110.67656,-110.67617], "fy":[-9.48963,-9.46622,-9.49191,-9.49206]}, + {"t":0.93184, "x":7.74118, "y":1.22596, "heading":1.57, "vx":0.37297, "vy":0.92741, "omega":-0.00011, "ax":-6.82577, "ay":1.50853, "alpha":-0.00046, "fx":[-108.3637,-108.36365,-108.36476,-108.36422], "fy":[23.95,23.95233,23.9467,23.94714]}, + {"t":0.9789, "x":7.75118, "y":1.27127, "heading":1.56999, "vx":0.05176, "vy":0.9984, "omega":-0.00013, "ax":-2.31444, "ay":-0.00511, "alpha":0.00618, "fx":[-36.6997,-36.7039,-36.70393,-36.86647], "fy":[-0.0719,-0.08168,-0.08533,-0.08539]}, + {"t":1.02596, "x":7.75105, "y":1.31825, "heading":1.56999, "vx":-0.05716, "vy":0.99816, "omega":0.00016, "ax":0.7704, "ay":0.03009, "alpha":-0.00078, "fx":[12.23134,12.22986,12.22986,12.23132], "fy":[0.47451,0.48736,0.47462,0.47462]}, + {"t":1.07302, "x":7.74921, "y":1.36525, "heading":1.57, "vx":-0.0209, "vy":0.99957, "omega":0.00012, "ax":0.08762, "ay":0.00165, "alpha":-0.00988, "fx":[1.33911,1.34025,1.34024,1.54451], "fy":[0.03964,0.02085,0.02219,0.02218]}, + {"t":1.12008, "x":7.74833, "y":1.41229, "heading":1.57, "vx":-0.01678, "vy":0.99965, "omega":-0.00034, "ax":0.05364, "ay":0.00083, "alpha":-0.00071, "fx":[0.85194,0.85107,0.85107,0.85194], "fy":[0.00972,0.02292,0.01011,0.01011]}, + {"t":1.16714, "x":7.7476, "y":1.45934, "heading":1.56999, "vx":-0.01425, "vy":0.99969, "omega":-0.00038, "ax":0.0413, "ay":0.00055, "alpha":-0.00123, "fx":[0.64986,0.65013,0.65012,0.67223], "fy":[0.01285,0.00712,0.00744,0.00743]}, + {"t":1.21419, "x":7.74697, "y":1.50638, "heading":1.56997, "vx":-0.01231, "vy":0.99971, "omega":-0.00043, "ax":0.03342, "ay":0.00039, "alpha":-0.0003, "fx":[0.53111,0.53011,0.5301,0.5311], "fy":[0.00515,0.00914,0.00509,0.00508]}, + {"t":1.26125, "x":7.74643, "y":1.55343, "heading":1.56995, "vx":-0.01074, "vy":0.99973, "omega":-0.00045, "ax":0.02822, "ay":0.00028, "alpha":0.00033, "fx":[0.45105,0.45083,0.45082,0.43907], "fy":[0.00689,0.00389,0.00364,0.00363]}, + {"t":1.30831, "x":7.74595, "y":1.60047, "heading":1.56993, "vx":-0.00941, "vy":0.99975, "omega":-0.00043, "ax":0.02424, "ay":0.00021, "alpha":-0.00025, "fx":[0.38526,0.38427,0.38427,0.38525], "fy":[0.00296,0.00544,0.00261,0.0026]}, + {"t":1.35537, "x":7.74554, "y":1.64752, "heading":1.56991, "vx":-0.00827, "vy":0.99976, "omega":-0.00044, "ax":0.02127, "ay":0.00017, "alpha":0.00022, "fx":[0.34023,0.33987,0.33986,0.33074], "fy":[0.00468,0.00221,0.00181,0.00179]}, + {"t":1.40243, "x":7.74517, "y":1.69457, "heading":1.56989, "vx":-0.00727, "vy":0.99976, "omega":-0.00043, "ax":0.01885, "ay":0.00013, "alpha":-0.00022, "fx":[0.29967,0.29896,0.29895,0.29966], "fy":[0.00162,0.00399,0.00129,0.00127]}, + {"t":1.44949, "x":7.74485, "y":1.74162, "heading":1.56986, "vx":-0.00638, "vy":0.99977, "omega":-0.00044, "ax":0.01694, "ay":0.0001, "alpha":0.0001, "fx":[0.27071,0.27044,0.27042,0.26441], "fy":[0.00355,0.00117,0.00086,0.00085]}, + {"t":1.49655, "x":7.74457, "y":1.78866, "heading":1.56984, "vx":-0.00558, "vy":0.99977, "omega":-0.00044, "ax":0.01535, "ay":0.00008, "alpha":-0.00017, "fx":[0.24379,0.24349,0.24348,0.24378], "fy":[0.00075,0.00317,0.0006,0.00058]}, + {"t":1.54361, "x":7.74432, "y":1.83571, "heading":1.56982, "vx":-0.00486, "vy":0.99978, "omega":-0.00045, "ax":0.01405, "ay":0.00006, "alpha":0.00011, "fx":[0.2244,0.22433,0.22431,0.21893], "fy":[0.00283,0.00047,0.00038,0.00036]}, + {"t":1.59066, "x":7.74411, "y":1.88276, "heading":1.5698, "vx":-0.0042, "vy":0.99978, "omega":-0.00044, "ax":0.01296, "ay":0.00005, "alpha":-0.0001, "fx":[0.2056,0.20579,0.20578,0.20558], "fy":[0.00013,0.00258,0.00026,0.00024]}, + {"t":1.63772, "x":7.74393, "y":1.92981, "heading":1.56978, "vx":-0.00359, "vy":0.99978, "omega":-0.00045, "ax":0.01206, "ay":0.00004, "alpha":0.00014, "fx":[0.19263,0.19283,0.1928,0.18759], "fy":[0.0023,-0.00006,0.00016,0.00014]}, + {"t":1.68478, "x":7.74377, "y":1.97686, "heading":1.56976, "vx":-0.00302, "vy":0.99979, "omega":-0.00044, "ax":0.01132, "ay":0.00003, "alpha":-0.00001, "fx":[0.17942,0.18018,0.18016,0.1794], "fy":[-0.00035,0.00212,0.00012,0.00009]}, + {"t":1.73184, "x":7.74364, "y":2.02391, "heading":1.56974, "vx":-0.00249, "vy":0.99979, "omega":-0.00044, "ax":0.01073, "ay":0.00002, "alpha":0.00018, "fx":[0.17113,0.17165,0.17162,0.16681], "fy":[0.00188,-0.0005,0.00009,0.00006]}, + {"t":1.7789, "x":7.74354, "y":2.07096, "heading":1.56972, "vx":-0.00199, "vy":0.99979, "omega":-0.00043, "ax":0.01027, "ay":0.00002, "alpha":0.00008, "fx":[0.1623,0.16369,0.16366,0.16227], "fy":[-0.00076,0.00172,0.0001,0.00007]}, + {"t":1.82596, "x":7.74345, "y":2.11801, "heading":1.5697, "vx":-0.0015, "vy":0.99979, "omega":-0.00043, "ax":0.00991, "ay":0.00001, "alpha":0.00022, "fx":[0.1578,0.15868,0.15865,0.15425], "fy":[0.00149,-0.0009,0.00012,0.00009]}, + {"t":1.87302, "x":7.74339, "y":2.16505, "heading":1.56968, "vx":-0.00104, "vy":0.99979, "omega":-0.00042, "ax":0.00968, "ay":0.00001, "alpha":0.00019, "fx":[0.15265,0.15474,0.15471,0.15262], "fy":[-0.00115,0.00135,0.00016,0.00013]}, + {"t":1.92008, "x":7.74336, "y":2.2121, "heading":1.56966, "vx":-0.00058, "vy":0.99979, "omega":-0.00041, "ax":0.00954, "ay":0.0, "alpha":0.00028, "fx":[0.15156,0.15287,0.15283,0.14845], "fy":[0.00111,-0.00128,0.00021,0.00018]}, + {"t":1.96714, "x":7.74334, "y":2.25915, "heading":1.56964, "vx":-0.00013, "vy":0.99979, "omega":-0.0004, "ax":0.00952, "ay":0.0, "alpha":0.00031, "fx":[0.14969,0.15256,0.15253,0.14966], "fy":[-0.00156,0.00097,0.00028,0.00025]}, + {"t":2.01419, "x":7.74334, "y":2.3062, "heading":1.56962, "vx":0.00032, "vy":0.99979, "omega":-0.00038, "ax":0.00958, "ay":-0.00001, "alpha":0.00039, "fx":[0.15201,0.1538,0.15376,0.14871], "fy":[0.0007,-0.0017,0.00035,0.00032]}, + {"t":2.06125, "x":7.74337, "y":2.35325, "heading":1.5696, "vx":0.00077, "vy":0.99979, "omega":-0.00036, "ax":0.00977, "ay":-0.00001, "alpha":0.00046, "fx":[0.1532,0.15697,0.15693,0.15317], "fy":[-0.00202,0.00053,0.00045,0.00042]}, + {"t":2.10831, "x":7.74342, "y":2.4003, "heading":1.56959, "vx":0.00123, "vy":0.99979, "omega":-0.00034, "ax":0.01004, "ay":-0.00001, "alpha":0.00056, "fx":[0.15921,0.1616,0.16156,0.15499], "fy":[0.00021,-0.0022,0.00055,0.00051]}, + {"t":2.15537, "x":7.74349, "y":2.44735, "heading":1.56957, "vx":0.0017, "vy":0.99979, "omega":-0.00032, "ax":0.01045, "ay":-0.00002, "alpha":0.00063, "fx":[0.16346,0.16831,0.16828,0.16342], "fy":[-0.00259,-0.00003,0.00068,0.00064]}, + {"t":2.20243, "x":7.74358, "y":2.4944, "heading":1.56955, "vx":0.00219, "vy":0.99979, "omega":-0.00029, "ax":0.01095, "ay":-0.00003, "alpha":0.00079, "fx":[0.17369,0.17689,0.17684,0.16785], "fy":[-0.00043,-0.00285,0.00081,0.00077]}, + {"t":2.24949, "x":7.74369, "y":2.54145, "heading":1.56954, "vx":0.00271, "vy":0.99979, "omega":-0.00025, "ax":0.01162, "ay":-0.00003, "alpha":0.00087, "fx":[0.18133,0.18757,0.18753,0.18129], "fy":[-0.00337,-0.00079,0.001,0.00096]}, + {"t":2.29655, "x":7.74383, "y":2.58849, "heading":1.56953, "vx":0.00325, "vy":0.99978, "omega":-0.00021, "ax":0.0124, "ay":-0.00004, "alpha":0.00112, "fx":[0.19669,0.20102,0.20098,0.18861], "fy":[-0.00133,-0.00378,0.00118,0.00114]}, + {"t":2.34361, "x":7.744, "y":2.63554, "heading":1.56952, "vx":0.00384, "vy":0.99978, "omega":-0.00015, "ax":0.01339, "ay":-0.00006, "alpha":0.00119, "fx":[0.20852,0.21663,0.2166,0.20849], "fy":[-0.00451,-0.00185,0.00144,0.0014]}, + {"t":2.39067, "x":7.74419, "y":2.68259, "heading":1.56951, "vx":0.00447, "vy":0.99978, "omega":-0.0001, "ax":0.01454, "ay":-0.00007, "alpha":0.00161, "fx":[0.23065,0.23667,0.23663,0.21942], "fy":[-0.0026,-0.00519,0.00169,0.00166]}, + {"t":2.43772, "x":7.74442, "y":2.72964, "heading":1.56951, "vx":0.00515, "vy":0.99978, "omega":-0.00002, "ax":0.01598, "ay":-0.00009, "alpha":0.00166, "fx":[0.24827,0.25901,0.25898,0.24825], "fy":[-0.0063,-0.00335,0.00204,0.00201]}, + {"t":2.48478, "x":7.74468, "y":2.77669, "heading":1.56951, "vx":0.0059, "vy":0.99977, "omega":0.00006, "ax":0.01767, "ay":-0.00011, "alpha":0.00236, "fx":[0.28044,0.28906,0.28902,0.26367], "fy":[-0.00437,-0.00747,0.00239,0.00236]}, + {"t":2.53184, "x":7.74498, "y":2.82374, "heading":1.56951, "vx":0.00673, "vy":0.99977, "omega":0.00017, "ax":0.01979, "ay":-0.00014, "alpha":0.00233, "fx":[0.3069,0.32147,0.32145,0.30688], "fy":[-0.00925,-0.00537,0.0028,0.00277]}, + {"t":2.5789, "x":7.74532, "y":2.87078, "heading":1.56952, "vx":0.00766, "vy":0.99976, "omega":0.00028, "ax":0.0224, "ay":-0.00018, "alpha":0.00351, "fx":[0.35562,0.3683,0.36828,0.33012], "fy":[-0.00668,-0.01133,0.00319,0.00317]}, + {"t":2.62596, "x":7.7457, "y":2.91783, "heading":1.56953, "vx":0.00872, "vy":0.99975, "omega":0.00044, "ax":0.02573, "ay":-0.00024, "alpha":0.0033, "fx":[0.3984,0.41871,0.4187,0.3984], "fy":[-0.01436,-0.00782,0.00348,0.00346]}, + {"t":2.67302, "x":7.74614, "y":2.96488, "heading":1.56955, "vx":0.00993, "vy":0.99974, "omega":0.0006, "ax":0.03016, "ay":-0.00032, "alpha":0.00443, "fx":[0.47486,0.49391,0.4939,0.45264], "fy":[-0.00886,-0.01838,0.00343,0.00342]}, + {"t":2.72008, "x":7.74664, "y":3.01192, "heading":1.56958, "vx":0.01135, "vy":0.99973, "omega":0.00081, "ax":0.03627, "ay":-0.00044, "alpha":0.00458, "fx":[0.56146,0.5902,0.59021,0.56148], "fy":[-0.02382,-0.00997,0.00284,0.00284]}, + {"t":2.76714, "x":7.74722, "y":3.05897, "heading":1.56962, "vx":0.01306, "vy":0.9997, "omega":0.00102, "ax":0.04578, "ay":-0.00065, "alpha":0.00141, "fx":[0.69408,0.72265,0.72267,0.76777], "fy":[-0.00899,-0.03256,0.00022,0.00023]}, + {"t":2.81419, "x":7.74788, "y":3.10601, "heading":1.56967, "vx":0.01521, "vy":0.99967, "omega":0.00109, "ax":0.08042, "ay":-0.00138, "alpha":0.00696, "fx":[1.25691,1.29653,1.29656,1.25698], "fy":[-0.0439,-0.03329,-0.00508,-0.0051]}, + {"t":2.86125, "x":7.74869, "y":3.15306, "heading":1.56972, "vx":0.01899, "vy":0.99961, "omega":0.00142, "ax":1.38547, "ay":-0.07167, "alpha":0.02393, "fx":[22.04876,22.08725,22.08744,21.75775], "fy":[-1.20274,-1.14637,-1.10055,-1.10139]}, + {"t":2.90831, "x":7.75111, "y":3.20002, "heading":1.56978, "vx":0.08419, "vy":0.99624, "omega":0.00254, "ax":-3.52063, "ay":-5.29334, "alpha":-0.00993, "fx":[-55.8609,-55.98574,-55.85431,-55.86947], "fy":[-84.06834,-83.95351,-84.06364,-84.05685]}, + {"t":2.95537, "x":7.75118, "y":3.24104, "heading":1.5699, "vx":-0.08148, "vy":0.74714, "omega":0.00207, "ax":-5.56433, "ay":-4.2427, "alpha":0.0016, "fx":[-88.33126,-88.33458,-88.34198,-88.34339], "fy":[-67.36866,-67.35995,-67.34689,-67.3487]}, + {"t":3.00243, "x":7.74118, "y":3.2715, "heading":1.57, "vx":-0.34333, "vy":0.54748, "omega":0.00215, "ax":-6.02336, "ay":-3.55732, "alpha":0.00173, "fx":[-95.6273,-95.61702,-95.62737,-95.62916], "fy":[-56.47258,-56.48917,-56.47003,-56.46845]}, + {"t":3.03405, "x":7.72731, "y":3.28703, "heading":1.57007, "vx":-0.53379, "vy":0.435, "omega":0.0022, "ax":-6.0197, "ay":-3.56776, "alpha":-0.00029, "fx":[-95.56323,-95.57004,-95.56912,-95.56612], "fy":[-56.64706,-56.63589,-56.63788,-56.64225]}, + {"t":3.06567, "x":7.70743, "y":3.299, "heading":1.57014, "vx":-0.72412, "vy":0.32219, "omega":0.00219, "ax":-6.0133, "ay":-3.57822, "alpha":0.00009, "fx":[-95.46784,-95.46226,-95.46765,-95.46412], "fy":[-56.80057,-56.81322,-56.8053,-56.80862]}, + {"t":3.09729, "x":7.68153, "y":3.3074, "heading":1.57021, "vx":-0.91426, "vy":0.20905, "omega":0.0022, "ax":-6.00597, "ay":-3.59014, "alpha":-0.00199, "fx":[-95.34526,-95.35764,-95.35113,-95.34287], "fy":[-56.99908,-56.98272,-56.9968,-57.00588]}, + {"t":3.12891, "x":7.64962, "y":3.31222, "heading":1.57028, "vx":-1.10416, "vy":0.09554, "omega":0.00213, "ax":-5.99752, "ay":-3.60383, "alpha":-0.00142, "fx":[-95.2179,-95.21613,-95.21748,-95.20857], "fy":[-57.20295,-57.21323,-57.21478,-57.22302]}, + {"t":3.16053, "x":7.6117, "y":3.31344, "heading":1.57034, "vx":-1.2938, "vy":-0.01841, "omega":0.00209, "ax":-5.98765, "ay":-3.61973, "alpha":-0.00365, "fx":[-95.05237,-95.07322,-95.06109,-95.04674], "fy":[-57.46883,-57.44247,-57.4685,-57.48353]}, + {"t":3.19214, "x":7.5678, "y":3.31105, "heading":1.57041, "vx":-1.48312, "vy":-0.13287, "omega":0.00197, "ax":-5.97598, "ay":-3.6384, "alpha":-0.00294, "fx":[-94.87644,-94.87861,-94.87585,-94.86144], "fy":[-57.74769,-57.75542,-57.76644,-57.77966]}, + {"t":3.22376, "x":7.51792, "y":3.30503, "heading":1.57047, "vx":-1.67208, "vy":-0.24791, "omega":0.00188, "ax":-5.96197, "ay":-3.66065, "alpha":-0.00531, "fx":[-94.64399,-94.67132,-94.65346,-94.6337], "fy":[-58.1162,-58.08357,-58.12141,-58.14108]}, + {"t":3.25538, "x":7.46207, "y":3.29536, "heading":1.57053, "vx":-1.86059, "vy":-0.36365, "omega":0.00171, "ax":-5.94483, "ay":-3.68762, "alpha":-0.00428, "fx":[-94.38266,-94.38762,-94.38198,-94.36183], "fy":[-58.52459,-58.53176,-58.54994,-58.56826]}, + {"t":3.287, "x":7.40027, "y":3.28201, "heading":1.57059, "vx":-2.04856, "vy":-0.48025, "omega":0.00158, "ax":-5.92339, "ay":-3.72097, "alpha":-0.0068, "fx":[-94.02882,-94.06544,-94.04166,-94.01679], "fy":[-59.07471,-59.03173,-59.08113,-59.10468]}, + {"t":3.31862, "x":7.33253, "y":3.26497, "heading":1.57064, "vx":-2.23585, "vy":-0.59791, "omega":0.00136, "ax":-5.89581, "ay":-3.76326, "alpha":-0.00566, "fx":[-93.60513,-93.61334,-93.60457,-93.57823], "fy":[-59.72147,-59.72737,-59.75287,-59.77653]}, + {"t":3.35024, "x":7.25889, "y":3.24418, "heading":1.57068, "vx":-2.42227, "vy":-0.7169, "omega":0.00118, "ax":-5.85902, "ay":-3.81866, "alpha":-0.00801, "fx":[-93.00228,-93.05042,-93.0204,-92.99201], "fy":[-60.62934,-60.57373,-60.63419,-60.65876]}, + {"t":3.38186, "x":7.17937, "y":3.21961, "heading":1.57072, "vx":-2.60753, "vy":-0.83764, "omega":0.00093, "ax":-5.80753, "ay":-3.89432, "alpha":-0.00488, "fx":[-92.20844,-92.20308,-92.20846,-92.17501], "fy":[-61.79201,-61.82292,-61.82823,-61.85765]}, + {"t":3.41348, "x":7.09402, "y":3.19117, "heading":1.57075, "vx":-2.79116, "vy":-0.96077, "omega":0.00078, "ax":-5.73041, "ay":-4.00377, "alpha":-0.00811, "fx":[-90.94591,-91.01715,-90.98021,-90.95469], "fy":[-63.58538,-63.50316,-63.57335,-63.5894]}, + {"t":3.4451, "x":7.0029, "y":3.15879, "heading":1.57077, "vx":-2.97235, "vy":-1.08737, "omega":0.00052, "ax":-5.60263, "ay":-4.17583, "alpha":-0.00709, "fx":[-88.95404,-88.96002,-88.95633,-88.91295], "fy":[-66.26242,-66.27849,-66.30016,-66.33638]}, + {"t":3.47672, "x":6.90612, "y":3.12233, "heading":1.57079, "vx":-3.1495, "vy":-1.21941, "omega":0.0003, "ax":-5.35213, "ay":-4.48387, "alpha":-0.01754, "fx":[-84.97189,-85.0332,-84.98749,-84.88357], "fy":[-71.16073,-71.10933,-71.18465,-71.28437]}, + {"t":3.50834, "x":6.80386, "y":3.08153, "heading":1.5708, "vx":-3.31873, "vy":-1.36118, "omega":-0.00026, "ax":-4.6733, "ay":-5.13165, "alpha":-0.01756, "fx":[-74.16913,-74.28988,-74.18638,-74.12263], "fy":[-81.47117,-81.37515,-81.49387,-81.53492]}, + {"t":3.5431, "x":6.68568, "y":3.03111, "heading":1.57079, "vx":-3.48117, "vy":-1.53956, "omega":-0.00087, "ax":1.06841, "ay":-6.80151, "alpha":-0.00785, "fx":[17.16761,16.95728,16.94457,16.77751], "fy":[-107.92831,-107.97184,-107.99441,-108.0215]}, + {"t":3.57786, "x":6.56531, "y":2.97349, "heading":1.57076, "vx":-3.44404, "vy":-1.77599, "omega":-0.00114, "ax":6.84392, "ay":1.27207, "alpha":0.00895, "fx":[108.63891,108.68122,108.6367,108.65237], "fy":[20.316,19.99823,20.23814,20.22795]}, + {"t":3.61262, "x":6.44973, "y":2.91252, "heading":1.57072, "vx":-3.20613, "vy":-1.73177, "omega":-0.00083, "ax":6.49941, "ay":2.55024, "alpha":-0.00663, "fx":[103.20232,103.15485,103.17604,103.19847], "fy":[40.45734,40.55086,40.48356,40.45602]}, + {"t":3.64738, "x":6.34221, "y":2.85386, "heading":1.57069, "vx":-2.98021, "vy":-1.64312, "omega":-0.00106, "ax":6.33889, "ay":2.94179, "alpha":-0.0044, "fx":[100.62932,100.62816,100.63157,100.64885], "fy":[46.73017,46.71096,46.69401,46.6772]}, + {"t":3.68214, "x":6.24244, "y":2.79852, "heading":1.57065, "vx":-2.75986, "vy":-1.54086, "omega":-0.00121, "ax":6.25198, "ay":3.12912, "alpha":-0.00918, "fx":[99.24605,99.23374,99.25007,99.28928], "fy":[49.70636,49.71507,49.67379,49.61307]}, + {"t":3.7169, "x":6.15028, "y":2.74685, "heading":1.57061, "vx":-2.54254, "vy":-1.43209, "omega":-0.00153, "ax":6.198, "ay":3.23863, "alpha":-0.00509, "fx":[98.39875,98.38079,98.40016,98.41123], "fy":[51.4219,51.44647,51.40244,51.39172]}, + {"t":3.75166, "x":6.06565, "y":2.69903, "heading":1.57056, "vx":-2.32709, "vy":-1.31951, "omega":-0.00171, "ax":6.16131, "ay":3.3104, "alpha":-0.00351, "fx":[97.81373,97.80557,97.81399,97.82786], "fy":[52.56309,52.5716,52.55192,52.53324]}, + {"t":3.78642, "x":5.98848, "y":2.65516, "heading":1.5705, "vx":-2.11291, "vy":-1.20444, "omega":-0.00183, "ax":6.13478, "ay":3.36104, "alpha":-0.00102, "fx":[97.39333,97.3917,97.39418,97.39754], "fy":[53.3627,53.36295,53.3568,53.35331]}, + {"t":3.82118, "x":5.91874, "y":2.61532, "heading":1.57043, "vx":-1.89966, "vy":-1.08761, "omega":-0.00187, "ax":6.11472, "ay":3.39868, "alpha":-0.00011, "fx":[97.07401,97.07616,97.07606,97.0767], "fy":[53.95887,53.95578,53.95656,53.95464]}, + {"t":3.85595, "x":5.8564, "y":2.57957, "heading":1.57037, "vx":-1.68711, "vy":-0.96947, "omega":-0.00187, "ax":6.09903, "ay":3.42774, "alpha":0.00153, "fx":[96.82639,96.83112,96.82681,96.8219], "fy":[54.41447,54.41067,54.42122,54.42538]}, + {"t":3.89071, "x":5.80144, "y":2.54794, "heading":1.5703, "vx":-1.4751, "vy":-0.85032, "omega":-0.00182, "ax":6.08642, "ay":3.45087, "alpha":0.00293, "fx":[96.62703,96.63482,96.62586,96.6175], "fy":[54.77703,54.77143,54.7925,54.7992]}, + {"t":3.92547, "x":5.75384, "y":2.52047, "heading":1.57024, "vx":-1.26353, "vy":-0.73036, "omega":-0.00172, "ax":6.07606, "ay":3.4697, "alpha":0.00465, "fx":[96.4614,96.47675,96.46147,96.4479], "fy":[55.07524,55.05989,55.0944,55.1065]}, + {"t":3.96023, "x":5.71359, "y":2.49717, "heading":1.57018, "vx":-1.05232, "vy":-0.60975, "omega":-0.00155, "ax":6.0674, "ay":3.48533, "alpha":0.00587, "fx":[96.32502,96.34234,96.32426,96.30628], "fy":[55.31864,55.30369,55.34516,55.36127]}, + {"t":3.99499, "x":5.68067, "y":2.47808, "heading":1.57013, "vx":-0.84141, "vy":-0.4886, "omega":-0.00135, "ax":6.06006, "ay":3.49852, "alpha":0.00756, "fx":[96.20759,96.23197,96.20736,96.18484], "fy":[55.52658,55.50294,55.55812,55.57837]}, + {"t":4.02975, "x":5.65509, "y":2.46321, "heading":1.57008, "vx":-0.63076, "vy":-0.36698, "omega":-0.00109, "ax":6.05376, "ay":3.50978, "alpha":0.00882, "fx":[96.1094,96.13459,96.10715,96.08031], "fy":[55.69911,55.67809,55.74027,55.76417]}, + {"t":4.06451, "x":5.63682, "y":2.45258, "heading":1.57004, "vx":-0.42032, "vy":-0.24498, "omega":-0.00078, "ax":6.04829, "ay":3.51953, "alpha":0.01053, "fx":[96.02074,96.05445,96.02025,95.9885], "fy":[55.85394,55.82174,55.89801,55.92663]}, + {"t":4.09927, "x":5.62586, "y":2.44619, "heading":1.57001, "vx":-0.21008, "vy":-0.12264, "omega":-0.00041, "ax":6.04349, "ay":3.52803, "alpha":0.01193, "fx":[95.94533,95.98235,95.94402,95.90777], "fy":[55.98464,55.95087,56.03625,56.06876]}, + {"t":4.13403, "x":5.62221, "y":2.44406, "heading":1.57, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[ + {"name":"IntakeDown", "from":{"target":1, "targetTimestamp":0.42339, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeDown"}}}, + {"name":"IntakeUp", "from":{"target":4, "targetTimestamp":3.50834, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeUp"}}}] +} diff --git a/src/main/deploy/choreo/RightSecondDip.traj b/src/main/deploy/choreo/RightSecondDip.traj new file mode 100644 index 0000000..7388fb3 --- /dev/null +++ b/src/main/deploy/choreo/RightSecondDip.traj @@ -0,0 +1,273 @@ +{ + "name":"RightSecondDip", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":5.607864856719971, "y":2.2545297145843506, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.374622821807861, "y":1.920681953430176, "heading":0.0, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.616988658905029, "y":1.900594472885132, "heading":1.1780352886194658, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.415774345397949, "y":3.393817186355591, "heading":2.7446892183710663, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.060224533081055, "y":3.563260793685913, "heading":3.141592653589793, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.796180248260498, "y":2.8797600269317627, "heading":2.2617395073648874, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.6100335121154785, "y":2.2714898586273193, "heading":1.5707963267948966, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":0, "to":6, "data":{"type":"MaxAcceleration", "props":{"max":4.0}}, "enabled":true}, + {"from":0, "to":6, "data":{"type":"MaxAngularVelocity", "props":{"max":4.0}}, "enabled":true}, + {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"first", "to":3, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":4, "to":6, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"5.607864856719971 m", "val":5.607864856719971}, "y":{"exp":"2.2545297145843506 m", "val":2.2545297145843506}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.374622821807861 m", "val":6.374622821807861}, "y":{"exp":"1.9206819534301758 m", "val":1.920681953430176}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.616988658905029 m", "val":7.616988658905029}, "y":{"exp":"1.9005944728851318 m", "val":1.900594472885132}, "heading":{"exp":"1.1780352886194658 rad", "val":1.1780352886194658}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.415774345397949 m", "val":7.415774345397949}, "y":{"exp":"3.393817186355591 m", "val":3.393817186355591}, "heading":{"exp":"2.7446892183710663 rad", "val":2.7446892183710663}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.060224533081055 m", "val":6.060224533081055}, "y":{"exp":"3.563260793685913 m", "val":3.563260793685913}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.796180248260498 m", "val":5.796180248260498}, "y":{"exp":"2.8797600269317627 m", "val":2.8797600269317627}, "heading":{"exp":"2.2617395073648874 rad", "val":2.2617395073648874}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.6100335121154785 m", "val":5.6100335121154785}, "y":{"exp":"2.2714898586273193 m", "val":2.2714898586273193}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":0, "to":6, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"4 m / s ^ 2", "val":4.0}}}, "enabled":true}, + {"from":0, "to":6, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"4 rad / s", "val":4.0}}}, "enabled":true}, + {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"first", "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, + {"from":4, "to":6, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.4186,1.11724,2.10307,3.48652,4.04495,4.36393], + "samples":[ + {"t":0.0, "x":5.60786, "y":2.25453, "heading":1.5708, "vx":1.83691, "vy":-0.77605, "omega":-3.74654, "ax":0.28522, "ay":-0.15373, "alpha":-3.6123, "fx":[13.38203,-4.2876,-4.28645,13.30444], "fy":[7.70136,7.36616,-12.48027,-12.34931]}, + {"t":0.0135, "x":5.63269, "y":2.24404, "heading":1.52021, "vx":1.84076, "vy":-0.77813, "omega":-3.79531, "ax":-0.01925, "ay":-0.04939, "alpha":-1.69035, "fx":[4.04022,-4.1625,-4.64583,3.54576], "fy":[3.72465,4.06821,-5.26625,-5.66279]}, + {"t":0.02701, "x":5.65755, "y":2.23352, "heading":1.46896, "vx":1.8405, "vy":-0.7788, "omega":-3.81814, "ax":0.00408, "ay":0.00966, "alpha":-0.70417, "fx":[1.93074,-1.44153,-1.84352,1.61357], "fy":[1.93168,2.25242,-1.61196,-1.95871]}, + {"t":0.04051, "x":5.6824, "y":2.22301, "heading":1.4174, "vx":1.84056, "vy":-0.77866, "omega":-3.82765, "ax":-0.00288, "ay":-0.00668, "alpha":-0.25145, "fx":[0.6422,-0.52098,-0.73371,0.42938], "fy":[0.47251,0.7262,-0.71549,-0.90739]}, + {"t":0.05401, "x":5.70725, "y":2.21249, "heading":1.36572, "vx":1.84052, "vy":-0.77875, "omega":-3.83104, "ax":0.006, "ay":0.01396, "alpha":-0.07712, "fx":[0.2996,-0.05282,-0.14424,0.27861], "fy":[0.38733,0.46239,0.05805,-0.02137]}, + {"t":0.06752, "x":5.73211, "y":2.20198, "heading":1.31399, "vx":1.8406, "vy":-0.77857, "omega":-3.83208, "ax":0.00275, "ay":0.00681, "alpha":-0.01316, "fx":[0.07574,0.03115,0.01145,0.05618], "fy":[0.14459,0.15614,0.07951,0.05224]}, + {"t":0.08102, "x":5.75696, "y":2.19147, "heading":1.26224, "vx":1.84064, "vy":-0.77847, "omega":-3.83226, "ax":0.00758, "ay":0.01755, "alpha":-0.00218, "fx":[0.12709,0.11471,0.10339,0.13604], "fy":[0.24963,0.30126,0.28666,0.27675]}, + {"t":0.09452, "x":5.78182, "y":2.18096, "heading":1.21049, "vx":1.84074, "vy":-0.77824, "omega":-3.83229, "ax":0.00588, "ay":0.01433, "alpha":0.00326, "fx":[0.08463,0.10047,0.10195,0.08613], "fy":[0.25274,0.19027,0.24057,0.22611]}, + {"t":0.10802, "x":5.80667, "y":2.17045, "heading":1.15875, "vx":1.84082, "vy":-0.77804, "omega":-3.83225, "ax":0.0096, "ay":0.02226, "alpha":-0.00507, "fx":[0.18031,0.15135,0.13048,0.14732], "fy":[0.31759,0.39363,0.36037,0.34206]}, + {"t":0.12153, "x":5.83153, "y":2.15995, "heading":1.107, "vx":1.84095, "vy":-0.77774, "omega":-3.83231, "ax":0.00863, "ay":0.0209, "alpha":0.00159, "fx":[0.13633,0.14017,0.1377,0.13384], "fy":[0.37704,0.2845,0.34878,0.31679]}, + {"t":0.13503, "x":5.85639, "y":2.14945, "heading":1.05525, "vx":1.84106, "vy":-0.77746, "omega":-3.83229, "ax":0.01182, "ay":0.02756, "alpha":-0.00947, "fx":[0.23468,0.18963,0.15366,0.17284], "fy":[0.39945,0.49492,0.44341,0.41215]}, + {"t":0.14853, "x":5.88125, "y":2.13895, "heading":1.0035, "vx":1.84122, "vy":-0.77709, "omega":-3.83242, "ax":0.01099, "ay":0.02646, "alpha":0.00497, "fx":[0.16031,0.17873,0.18854,0.17025], "fy":[0.48077,0.34729,0.45134,0.40114]}, + {"t":0.16204, "x":5.90611, "y":2.12846, "heading":0.95175, "vx":1.84137, "vy":-0.77673, "omega":-3.83235, "ax":0.0132, "ay":0.03095, "alpha":-0.01193, "fx":[0.26373,0.2146,0.16758,0.19209], "fy":[0.45019,0.55602,0.49982,0.45951]}, + {"t":0.17554, "x":5.93098, "y":2.11797, "heading":0.90001, "vx":1.84155, "vy":-0.77631, "omega":-3.83252, "ax":0.01147, "ay":0.02747, "alpha":0.01278, "fx":[0.13588,0.18364,0.22804,0.18097], "fy":[0.51594,0.31804,0.49035,0.42041]}, + {"t":0.18904, "x":5.95585, "y":2.10749, "heading":0.84825, "vx":1.84171, "vy":-0.77594, "omega":-3.83234, "ax":0.0115, "ay":0.02721, "alpha":-0.01301, "fx":[0.23303,0.18457,0.13367,0.17914], "fy":[0.39982,0.4941,0.43833,0.39583]}, + {"t":0.20255, "x":5.98072, "y":2.09702, "heading":0.79651, "vx":1.84186, "vy":-0.77558, "omega":-3.83252, "ax":0.00693, "ay":0.0164, "alpha":0.02542, "fx":[0.01309,0.09563,0.2059,0.12554], "fy":[0.36751,0.07415,0.34239,0.25713]}, + {"t":0.21605, "x":6.00559, "y":2.08655, "heading":0.74476, "vx":1.84195, "vy":-0.77535, "omega":-3.83218, "ax":0.0027, "ay":0.00672, "alpha":-0.01144, "fx":[0.07405,0.02569,-0.01392,0.08543], "fy":[0.11695,0.15106,0.0946,0.06404]}, + {"t":0.22955, "x":6.03046, "y":2.07608, "heading":0.69301, "vx":1.84199, "vy":-0.77526, "omega":-3.83233, "ax":-0.00832, "ay":-0.02031, "alpha":0.03998, "fx":[-0.28138,-0.18386,0.01445,-0.07745], "fy":[-0.17211,-0.59091,-0.21514,-0.31182]}, + {"t":0.24306, "x":6.05533, "y":2.06561, "heading":0.64126, "vx":1.84188, "vy":-0.77554, "omega":-3.83179, "ax":-0.02019, "ay":-0.04707, "alpha":-0.01174, "fx":[-0.30245,-0.38115,-0.41726,-0.18141], "fy":[-0.63082,-0.7165,-0.8085,-0.83329]}, + {"t":0.25656, "x":6.0802, "y":2.05513, "heading":0.58952, "vx":1.84161, "vy":-0.77617, "omega":-3.83195, "ax":-0.04402, "ay":-0.1055, "alpha":0.04598, "fx":[-0.84682,-0.80278,-0.55709,-0.58881], "fy":[-1.45836,-2.0149,-1.54627,-1.67987]}, + {"t":0.27006, "x":6.10506, "y":2.04464, "heading":0.53778, "vx":1.84101, "vy":-0.7776, "omega":-3.83133, "ax":-0.06886, "ay":-0.16131, "alpha":-0.0088, "fx":[-1.09106,-1.23428,-1.27225,-0.77537], "fy":[-2.23099,-2.55291,-2.71701,-2.74305]}, + {"t":0.28356, "x":6.12992, "y":2.03413, "heading":0.48604, "vx":1.84008, "vy":-0.77978, "omega":-3.83145, "ax":-0.1166, "ay":-0.27608, "alpha":0.09648, "fx":[-2.10668,-2.07332,-1.60644,-1.61773], "fy":[-4.01909,-4.99931,-4.27759,-4.23611]}, + {"t":0.29707, "x":6.15475, "y":2.02357, "heading":0.43431, "vx":1.83851, "vy":-0.7835, "omega":-3.83014, "ax":-0.16281, "ay":-0.37875, "alpha":0.22412, "fx":[-3.38857,-3.18767,-2.13448,-1.62789], "fy":[-5.06504,-6.78992,-6.5548,-5.64228]}, + {"t":0.31057, "x":6.17956, "y":2.01296, "heading":0.38259, "vx":1.83631, "vy":-0.78862, "omega":-3.82712, "ax":-0.25339, "ay":-0.58834, "alpha":0.80328, "fx":[-6.62473,-5.47988,-1.43941,-2.54724], "fy":[-7.94881,-12.37907,-10.14284,-6.89041]}, + {"t":0.32407, "x":6.20434, "y":2.00226, "heading":0.33091, "vx":1.83289, "vy":-0.79656, "omega":-3.81627, "ax":-0.33412, "ay":-0.76157, "alpha":2.09144, "fx":[-12.43165,-9.48804,1.21276,-0.51083], "fy":[-7.95808,-18.71662,-15.50695,-6.1803]}, + {"t":0.33758, "x":6.22906, "y":1.99143, "heading":0.27938, "vx":1.82838, "vy":-0.80685, "omega":-3.78803, "ax":-0.4967, "ay":-1.11375, "alpha":4.61704, "fx":[-23.35851,-17.1782,7.3772,1.61746], "fy":[-10.10152,-31.77505,-25.19402,-3.65572]}, + {"t":0.35108, "x":6.2537, "y":1.98043, "heading":0.22823, "vx":1.82167, "vy":-0.82189, "omega":-3.72569, "ax":-0.62603, "ay":-1.36911, "alpha":8.60384, "fx":[-38.52768,-28.22925,17.30031,9.70204], "fy":[-5.69059,-46.57731,-37.76047,3.08569]}, + {"t":0.36458, "x":6.27824, "y":1.96921, "heading":0.17792, "vx":1.81321, "vy":-0.84037, "omega":-3.60951, "ax":-0.88018, "ay":-1.86271, "alpha":13.07176, "fx":[-57.92435,-42.60202,26.92473,17.70779], "fy":[-5.81462,-63.76865,-56.71807,8.01383]}, + {"t":0.37809, "x":6.30264, "y":1.95769, "heading":0.12918, "vx":1.80133, "vy":-0.86552, "omega":-3.433, "ax":-1.04736, "ay":-2.13714, "alpha":17.30986, "fx":[-75.32319,-54.61652,34.45311,28.97587], "fy":[0.52526,-76.28986,-73.5525,13.60251]}, + {"t":0.39159, "x":6.32687, "y":1.94581, "heading":0.08282, "vx":1.78719, "vy":-0.89438, "omega":-3.19926, "ax":-1.35234, "ay":-2.63065, "alpha":19.60931, "fx":[-91.0583,-63.88739,32.80967,36.25818], "fy":[-3.45502,-83.63853,-90.77235,10.81178]}, + {"t":0.40509, "x":6.35088, "y":1.9335, "heading":0.03962, "vx":1.76893, "vy":-0.9299, "omega":-2.93448, "ax":-1.58873, "ay":-2.81929, "alpha":21.21365, "fx":[-101.30221,-71.2722,28.83242,42.85321], "fy":[0.27892,-86.19518,-101.18984,8.07293]}, + {"t":0.4186, "x":6.37462, "y":1.92068, "heading":0.0, "vx":1.74747, "vy":-0.96797, "omega":-2.64803, "ax":-1.51522, "ay":-2.76229, "alpha":23.3199, "fx":[-107.64331,-77.05817,29.47234,59.00819], "fy":[6.21658,-86.34402,-106.23983,10.9538]}, + {"t":0.43977, "x":6.41128, "y":1.89957, "heading":-0.05606, "vx":1.7154, "vy":-1.02645, "omega":-2.15432, "ax":-1.00017, "ay":-1.63411, "alpha":25.65442, "fx":[-95.42784,-80.49702,43.20719,69.20417], "fy":[36.13224,-76.98563,-96.68708,33.76989]}, + {"t":0.46094, "x":6.44737, "y":1.87747, "heading":-0.10167, "vx":1.69422, "vy":-1.06105, "omega":-1.6112, "ax":0.11681, "ay":0.18695, "alpha":26.50099, "fx":[-64.32717,-78.61169,69.75501,80.60139], "fy":[74.32913,-53.83591,-67.78649,59.16505]}, + {"t":0.48211, "x":6.48327, "y":1.85505, "heading":-0.13578, "vx":1.69669, "vy":-1.05709, "omega":-1.05015, "ax":1.28401, "ay":2.12328, "alpha":23.36043, "fx":[-28.95002,-62.97667,91.86525,81.59963], "fy":[100.6082,-12.92026,-27.08969,74.23654]}, + {"t":0.50328, "x":6.51947, "y":1.83315, "heading":-0.15801, "vx":1.72388, "vy":-1.01214, "omega":-0.55558, "ax":1.79879, "ay":3.20607, "alpha":19.36196, "fx":[-11.24169,-47.40408,96.05299,76.8211], "fy":[105.02384,19.45236,0.70878,78.40994]}, + {"t":0.52445, "x":6.55637, "y":1.81244, "heading":-0.16978, "vx":1.76196, "vy":-0.94426, "omega":-0.14567, "ax":1.74366, "ay":3.42011, "alpha":18.01972, "fx":[-10.68999,-43.98564,91.3153,74.08779], "fy":[103.2224,30.23013,5.77578,77.9584]}, + {"t":0.54562, "x":6.59407, "y":1.79321, "heading":-0.17286, "vx":1.79887, "vy":-0.87186, "omega":0.23582, "ax":1.62828, "ay":3.54631, "alpha":16.89847, "fx":[-10.87969,-41.42984,85.91145,69.79862], "fy":[101.24991,33.13578,11.97553,78.83986]}, + {"t":0.56679, "x":6.63251, "y":1.77555, "heading":-0.16787, "vx":1.83335, "vy":-0.79678, "omega":0.59358, "ax":1.49166, "ay":3.64037, "alpha":15.6725, "fx":[-12.2536,-37.77605,79.34109,65.41332], "fy":[98.41151,37.81898,16.16996,78.7734]}, + {"t":0.58796, "x":6.67166, "y":1.7595, "heading":-0.1553, "vx":1.86493, "vy":-0.71971, "omega":0.92538, "ax":1.34436, "ay":3.71676, "alpha":14.47963, "fx":[-13.20246,-34.63335,72.82252,60.38415], "fy":[95.70391,38.97839,22.16558,79.17713]}, + {"t":0.60913, "x":6.71145, "y":1.74509, "heading":-0.13571, "vx":1.89339, "vy":-0.64102, "omega":1.23192, "ax":1.19063, "ay":3.78094, "alpha":13.05551, "fx":[-14.13714,-30.12465,65.38843,54.48202], "fy":[92.06408,41.66169,27.20648,79.16823]}, + {"t":0.6303, "x":6.7518, "y":1.73237, "heading":-0.10963, "vx":1.91859, "vy":-0.56098, "omega":1.50832, "ax":1.03244, "ay":3.83519, "alpha":11.83582, "fx":[-14.98225,-27.07206,58.67085,48.94668], "fy":[89.0338,42.69508,32.73906,79.0778]}, + {"t":0.65148, "x":6.79265, "y":1.72135, "heading":-0.0777, "vx":1.94045, "vy":-0.47978, "omega":1.7589, "ax":0.87098, "ay":3.88066, "alpha":10.38197, "fx":[-15.20341,-22.85897,51.23104,42.14096], "fy":[85.26024,44.5324,37.7881,78.85257]}, + {"t":0.67265, "x":6.83392, "y":1.71207, "heading":-0.04046, "vx":1.95889, "vy":-0.39762, "omega":1.97869, "ax":0.70697, "ay":3.91793, "alpha":9.20198, "fx":[-15.7818,-20.34893,44.58606,36.43905], "fy":[82.17307,45.83195,42.54999,78.24516]}, + {"t":0.69382, "x":6.87555, "y":1.70453, "heading":0.00143, "vx":1.97386, "vy":-0.31468, "omega":2.17351, "ax":0.54087, "ay":3.94734, "alpha":7.86839, "fx":[-15.58501,-17.21587,37.58766,29.55988], "fy":[78.65825,47.51824,46.88095,77.61044]}, + {"t":0.71499, "x":6.91746, "y":1.69875, "heading":0.04745, "vx":1.98531, "vy":-0.23111, "omega":2.34009, "ax":0.3714, "ay":3.96922, "alpha":6.79367, "fx":[-15.9023,-15.58717,31.05427,24.01999], "fy":[75.8766,48.98282,50.63883,76.55908]}, + {"t":0.73616, "x":6.95958, "y":1.69474, "heading":0.09699, "vx":1.99317, "vy":-0.14708, "omega":2.48391, "ax":0.17908, "ay":3.98438, "alpha":5.67611, "fx":[-16.03643,-14.24529,24.34329,17.31077], "fy":[72.84069,50.62259,53.94331,75.61345]}, + {"t":0.75733, "x":7.00181, "y":1.69252, "heading":0.14958, "vx":1.99696, "vy":-0.06272, "omega":2.60408, "ax":-0.15474, "ay":3.98691, "alpha":4.75383, "fx":[-18.7005,-16.23475,15.39904,9.7098], "fy":[70.44443,51.86931,56.58456,74.282]}, + {"t":0.7785, "x":7.04406, "y":1.69209, "heading":0.20471, "vx":1.99369, "vy":0.02168, "omega":2.70473, "ax":-0.64693, "ay":3.93843, "alpha":3.89688, "fx":[-23.78525,-21.04528,4.57658,-0.82787], "fy":[67.2797,52.38625,58.00969,72.4262]}, + {"t":0.79967, "x":7.08612, "y":1.69343, "heading":0.26197, "vx":1.97999, "vy":0.10506, "omega":2.78723, "ax":-1.07892, "ay":3.84378, "alpha":3.10386, "fx":[-28.09592,-25.26104,-5.49327,-9.66446], "fy":[64.12532,52.47261,58.04927,69.44383]}, + {"t":0.82084, "x":7.1278, "y":1.69652, "heading":0.32098, "vx":1.95715, "vy":0.18644, "omega":2.85294, "ax":-1.41647, "ay":3.7336, "alpha":2.47859, "fx":[-31.27332,-28.52971,-13.08655,-17.06035], "fy":[61.2422,51.90413,57.52885,66.41921]}, + {"t":0.84201, "x":7.16891, "y":1.7013, "heading":0.38138, "vx":1.92716, "vy":0.26548, "omega":2.90541, "ax":-1.67656, "ay":3.62511, "alpha":1.81181, "fx":[-33.22477,-30.64689,-19.80134,-22.79318], "fy":[58.55075,52.04823,56.54896,63.05716]}, + {"t":0.86318, "x":7.20934, "y":1.70773, "heading":0.44289, "vx":1.89167, "vy":0.34223, "omega":2.94377, "ax":-1.87903, "ay":3.52513, "alpha":1.33362, "fx":[-34.6869,-32.53811,-24.77103,-27.3276], "fy":[56.46775,51.65071,55.58459,60.15321]}, + {"t":0.88436, "x":7.24897, "y":1.71577, "heading":0.50521, "vx":1.85189, "vy":0.41686, "omega":2.972, "ax":-2.03914, "ay":3.43561, "alpha":0.75205, "fx":[-35.25327,-33.65529,-29.48837,-31.09421], "fy":[54.62495,52.20502,54.33266,57.00847]}, + {"t":0.90553, "x":7.28771, "y":1.72536, "heading":0.56813, "vx":1.80872, "vy":0.48959, "omega":2.98793, "ax":-2.16793, "ay":3.35635, "alpha":0.33405, "fx":[-35.60218,-34.91326,-33.10688,-34.04732], "fy":[53.42408,52.0835,53.26529,54.36492]}, + {"t":0.9267, "x":7.32552, "y":1.73648, "heading":0.63139, "vx":1.76282, "vy":0.56065, "omega":2.995, "ax":-2.27321, "ay":3.2864, "alpha":-0.19791, "fx":[-35.37476,-35.70919,-36.81147,-36.45982], "fy":[52.31204,52.91264,51.94549,51.526]}, + {"t":0.94787, "x":7.36233, "y":1.74909, "heading":0.69479, "vx":1.71469, "vy":0.63023, "omega":2.99081, "ax":-2.36054, "ay":3.22463, "alpha":-0.64296, "fx":[-34.9592,-36.67721,-39.90008,-38.36478], "fy":[51.77427,53.3142,50.72187,48.96337]}, + {"t":0.96904, "x":7.3981, "y":1.76315, "heading":0.75811, "vx":1.66472, "vy":0.6985, "omega":2.9772, "ax":-2.43397, "ay":3.16992, "alpha":-1.20266, "fx":[-34.00426,-37.42284,-43.18524,-39.95175], "fy":[51.44984,54.43837,49.23539,46.17552]}, + {"t":0.99021, "x":7.4328, "y":1.77865, "heading":0.82114, "vx":1.61319, "vy":0.76561, "omega":2.95173, "ax":-2.49643, "ay":3.12127, "alpha":-1.74469, "fx":[-32.85535,-38.37441,-46.22748,-41.07307], "fy":[51.62506,55.40094,47.71675,43.46703]}, + {"t":1.01138, "x":7.4664, "y":1.79556, "heading":0.88363, "vx":1.56034, "vy":0.83169, "omega":2.9148, "ax":-2.55013, "ay":3.07782, "alpha":-2.40323, "fx":[-31.16423,-39.26947,-49.50363,-42.00327], "fy":[52.16597,56.88806,45.90302,40.49335]}, + {"t":1.03255, "x":7.49886, "y":1.81386, "heading":0.94534, "vx":1.50635, "vy":0.89685, "omega":2.86392, "ax":-2.59672, "ay":3.03885, "alpha":-3.10311, "fx":[-29.24599,-40.45032,-52.77074,-42.43255], "fy":[53.23397,58.41477,43.90245,37.42447]}, + {"t":1.05372, "x":7.53017, "y":1.83352, "heading":1.00597, "vx":1.45137, "vy":0.96118, "omega":2.79822, "ax":-2.63751, "ay":3.00373, "alpha":-3.91483, "fx":[-26.8536,-41.69801,-56.29732,-42.64092], "fy":[54.77386,60.28964,41.56433,34.11766]}, + {"t":1.07489, "x":7.5603, "y":1.85455, "heading":1.06521, "vx":1.39554, "vy":1.02477, "omega":2.71534, "ax":-2.67347, "ay":2.97196, "alpha":-4.79607, "fx":[-24.27203,-43.32519,-59.86541,-42.31062], "fy":[56.82913,62.28954,38.96252,30.64716]}, + {"t":1.09606, "x":7.58925, "y":1.87691, "heading":1.1227, "vx":1.33894, "vy":1.08769, "omega":2.61381, "ax":-2.7054, "ay":2.9431, "alpha":-5.78309, "fx":[-21.30389,-45.15034,-63.71997,-41.62657], "fy":[59.60135,64.35166,35.94006,27.00268]}, + {"t":1.11724, "x":7.61699, "y":1.90059, "heading":1.17804, "vx":1.28166, "vy":1.15, "omega":2.49137, "ax":-2.74017, "ay":2.90883, "alpha":-5.12532, "fx":[-24.7825,-46.35519,-61.73738,-41.13404], "fy":[58.27801,61.6006,35.6668,29.1738]}, + {"t":1.14388, "x":7.65016, "y":1.93227, "heading":1.24442, "vx":1.20865, "vy":1.2275, "omega":2.35481, "ax":-2.78789, "ay":2.86561, "alpha":-4.38728, "fx":[-28.66522,-47.60717,-59.55512,-41.21163], "fy":[56.50409,58.48272,35.53506,31.45258]}, + {"t":1.17052, "x":7.68138, "y":1.96599, "heading":1.30716, "vx":1.13437, "vy":1.30386, "omega":2.23792, "ax":-2.83921, "ay":2.81459, "alpha":-3.81801, "fx":[-31.96825,-48.72826,-58.13593,-41.46569], "fy":[54.97273,55.60801,35.17438,32.97973]}, + {"t":1.19717, "x":7.7106, "y":2.00173, "heading":1.36679, "vx":1.05872, "vy":1.37885, "omega":2.13619, "ax":-2.89614, "ay":2.75578, "alpha":-3.30589, "fx":[-34.9718,-49.77483,-56.91813,-42.24848], "fy":[53.17519,52.93414,34.91083,33.97983]}, + {"t":1.22381, "x":7.73778, "y":2.03945, "heading":1.4237, "vx":0.98155, "vy":1.45227, "omega":2.04811, "ax":-2.95952, "ay":2.68736, "alpha":-2.89226, "fx":[-37.77063,-50.77489,-56.28762,-43.10485], "fy":[51.30667,50.45462,34.39009,34.50413]}, + {"t":1.25046, "x":7.76288, "y":2.0791, "heading":1.47827, "vx":0.9027, "vy":1.52388, "omega":1.97104, "ax":-3.03029, "ay":2.60703, "alpha":-2.51483, "fx":[-40.38471,-51.83657,-55.85708,-44.35387], "fy":[49.27736,47.9051,33.79983,34.57145]}, + {"t":1.2771, "x":7.78585, "y":2.12062, "heading":1.53079, "vx":0.82196, "vy":1.59334, "omega":1.90404, "ax":-3.10953, "ay":2.51166, "alpha":-2.21347, "fx":[-42.91116,-52.95843,-55.91838,-45.67615], "fy":[47.09299,45.40987,32.84098,34.1538]}, + {"t":1.30374, "x":7.80665, "y":2.16397, "heading":1.58152, "vx":0.73911, "vy":1.66026, "omega":1.84506, "ax":-3.19833, "ay":2.39716, "alpha":-1.93912, "fx":[-45.361,-54.18513,-56.19128,-47.36589], "fy":[44.69447,42.69183,31.62407,33.21639]}, + {"t":1.33039, "x":7.82521, "y":2.20906, "heading":1.63068, "vx":0.65389, "vy":1.72413, "omega":1.79339, "ax":-3.29776, "ay":2.25794, "alpha":-1.71373, "fx":[-47.89451,-55.56495,-56.8974,-49.06031], "fy":[41.84903,39.84486,29.91699,31.77501]}, + {"t":1.35703, "x":7.84146, "y":2.2558, "heading":1.67847, "vx":0.56603, "vy":1.78429, "omega":1.74773, "ax":-3.40845, "ay":2.08648, "alpha":-1.51917, "fx":[-50.35016,-57.1051,-57.8124,-51.17896], "fy":[38.80479,36.43078,27.68332,29.57859]}, + {"t":1.38368, "x":7.85533, "y":2.30408, "heading":1.72503, "vx":0.47521, "vy":1.83988, "omega":1.70726, "ax":-3.53004, "ay":1.87262, "alpha":-1.34132, "fx":[-53.05027,-58.81674,-59.10002,-53.20117], "fy":[34.71841,32.68793,24.74979,26.76084]}, + {"t":1.41032, "x":7.86674, "y":2.35376, "heading":1.77052, "vx":0.38116, "vy":1.88978, "omega":1.67152, "ax":-3.65992, "ay":1.60293, "alpha":-1.18934, "fx":[-55.6679,-60.68169,-60.53496,-55.5313], "fy":[30.09408,27.90502,20.91398,22.87781]}, + {"t":1.43697, "x":7.8756, "y":2.40468, "heading":1.81506, "vx":0.28364, "vy":1.93249, "omega":1.63983, "ax":-3.79073, "ay":1.26125, "alpha":-1.06338, "fx":[-58.2932,-62.56355,-62.11187,-57.75399], "fy":[24.16833,22.29923,15.85823,17.76704]}, + {"t":1.46361, "x":7.88181, "y":2.45662, "heading":1.85875, "vx":0.18264, "vy":1.96609, "omega":1.6115, "ax":-3.90669, "ay":0.83287, "alpha":-0.95518, "fx":[-60.57306,-64.25607,-63.48752,-59.76942], "fy":[17.12705,15.10808,9.4317,11.22265]}, + {"t":1.49025, "x":7.88529, "y":2.5093, "heading":1.90169, "vx":0.07855, "vy":1.98828, "omega":1.58605, "ax":-3.98043, "ay":0.32753, "alpha":-0.85711, "fx":[-62.14114,-65.29344,-64.26802,-61.06611], "fy":[8.62158,7.02396,1.75441,3.3989]}, + {"t":1.5169, "x":7.88597, "y":2.5624, "heading":1.94395, "vx":-0.02751, "vy":1.99701, "omega":1.56321, "ax":-3.99176, "ay":-0.10986, "alpha":-0.78225, "fx":[-62.62823,-65.36903,-64.14632,-61.34519], "fy":[1.5103,-0.25002,-4.87907,-3.35777]}, + {"t":1.54354, "x":7.88382, "y":2.61556, "heading":1.9856, "vx":-0.13387, "vy":1.99408, "omega":1.54237, "ax":-3.97581, "ay":-0.36645, "alpha":-0.70637, "fx":[-62.57828,-65.0146,-63.67613,-61.20645], "fy":[-2.96861,-4.40926,-8.67156,-7.22121]}, + {"t":1.57019, "x":7.87884, "y":2.66857, "heading":2.02669, "vx":-0.2398, "vy":1.98432, "omega":1.52355, "ax":-3.94941, "ay":-0.58174, "alpha":-0.66328, "fx":[-62.3661,-64.53002,-63.12278,-60.7799], "fy":[-6.50427,-8.07303,-11.87582,-10.48933]}, + {"t":1.59683, "x":7.87105, "y":2.72123, "heading":2.06729, "vx":-0.34503, "vy":1.96882, "omega":1.50587, "ax":-3.91209, "ay":-0.79161, "alpha":-0.60619, "fx":[-61.86861,-63.84428,-62.3616,-60.35457], "fy":[-10.09766,-11.4986,-15.02448,-13.64854]}, + {"t":1.62348, "x":7.86047, "y":2.77341, "heading":2.10741, "vx":-0.44926, "vy":1.94773, "omega":1.48972, "ax":-3.86374, "ay":-0.9988, "alpha":-0.59359, "fx":[-61.29021,-63.0894,-61.54443,-59.43484], "fy":[-13.4689,-14.92318,-18.17385,-16.86073]}, + {"t":1.65012, "x":7.84713, "y":2.82495, "heading":2.1471, "vx":-0.55221, "vy":1.92112, "omega":1.47391, "ax":-3.80447, "ay":-1.20316, "alpha":-0.5504, "fx":[-60.38365,-62.061,-60.4359,-58.71441], "fy":[-16.82758,-18.31565,-21.31298,-19.94797]}, + {"t":1.67676, "x":7.83106, "y":2.87571, "heading":2.18637, "vx":-0.65357, "vy":1.88906, "omega":1.45924, "ax":-3.73291, "ay":-1.40828, "alpha":-0.54313, "fx":[-59.36629,-60.98111,-59.24235,-57.46082], "fy":[-20.24658,-21.5397,-24.48496,-23.15878]}, + {"t":1.70341, "x":7.81232, "y":2.92554, "heading":2.22525, "vx":-0.75304, "vy":1.85154, "omega":1.44477, "ax":-3.60164, "ay":-1.71567, "alpha":-0.53519, "fx":[-57.38451,-58.88521,-57.01812,-55.42697], "fy":[-25.00487,-26.72633,-29.30003,-27.91885]}, + {"t":1.73005, "x":7.79098, "y":2.97426, "heading":2.26375, "vx":-0.849, "vy":1.80582, "omega":1.43051, "ax":-3.04447, "ay":-2.57842, "alpha":-0.56162, "fx":[-48.70472,-50.34271,-48.07859,-46.20655], "fy":[-38.93991,-40.19189,-42.91284,-41.69287]}, + {"t":1.7567, "x":7.76728, "y":3.02146, "heading":2.30186, "vx":-0.93012, "vy":1.73712, "omega":1.41555, "ax":-2.23455, "ay":-3.30634, "alpha":-0.57228, "fx":[-35.90525,-37.69424,-35.08517,-33.21591], "fy":[-50.5224,-51.97792,-54.36647,-53.09575]}, + {"t":1.78334, "x":7.7417, "y":3.06657, "heading":2.33958, "vx":-0.98965, "vy":1.64903, "omega":1.4003, "ax":-1.47469, "ay":-3.70955, "alpha":-0.60829, "fx":[-23.84988,-25.92987,-22.9924,-20.87475], "fy":[-56.96643,-58.44132,-60.77776,-59.38211]}, + {"t":1.80999, "x":7.71481, "y":3.10919, "heading":2.37689, "vx":-1.02895, "vy":1.55019, "omega":1.38409, "ax":-0.86967, "ay":-3.89727, "alpha":-0.65607, "fx":[-14.28318,-16.62563,-13.36186,-10.95623], "fy":[-59.87185,-61.65997,-63.78959,-62.16681]}, + {"t":1.83663, "x":7.68709, "y":3.14911, "heading":2.41377, "vx":-1.05212, "vy":1.44635, "omega":1.36661, "ax":-0.4129, "ay":-3.97271, "alpha":-0.70578, "fx":[-7.02909,-9.67601,-6.01528,-3.49999], "fy":[-61.01208,-62.91893,-65.1425,-63.20543]}, + {"t":1.86327, "x":7.65891, "y":3.18624, "heading":2.45018, "vx":-1.06312, "vy":1.3405, "omega":1.3478, "ax":-0.06933, "ay":-3.9943, "alpha":-0.80897, "fx":[-1.72142,-4.66254,-0.50507,2.48629], "fy":[-61.03683,-63.57767,-65.67372,-63.36125]}, + {"t":1.88992, "x":7.63056, "y":3.22054, "heading":2.48609, "vx":-1.06497, "vy":1.23407, "omega":1.32625, "ax":0.19294, "ay":-3.99086, "alpha":-0.89085, "fx":[2.35175,-0.90926,3.84815,6.9616], "fy":[-60.83582,-63.56109,-65.89007,-63.14423]}, + {"t":1.91656, "x":7.60225, "y":3.252, "heading":2.52143, "vx":-1.05983, "vy":1.12774, "omega":1.30251, "ax":0.39723, "ay":-3.97623, "alpha":-1.04046, "fx":[5.36275,1.71697,7.22914,10.91616], "fy":[-60.15782,-63.69959,-65.96535,-62.6793]}, + {"t":1.94321, "x":7.57415, "y":3.28064, "heading":2.55613, "vx":-1.04924, "vy":1.0218, "omega":1.27479, "ax":0.55964, "ay":-3.95705, "alpha":-1.16463, "fx":[7.7084,3.72547,10.08468,14.02039], "fy":[-59.63353,-63.49198,-66.01607,-62.14246]}, + {"t":1.96985, "x":7.5464, "y":3.30646, "heading":2.5901, "vx":-1.03433, "vy":0.91636, "omega":1.24376, "ax":0.69123, "ay":-3.93653, "alpha":-1.35647, "fx":[9.47316,5.04127,12.42967,16.95114], "fy":[-58.76032,-63.55253,-66.13011,-61.53842]}, + {"t":1.9965, "x":7.51908, "y":3.32948, "heading":2.62324, "vx":-1.01591, "vy":0.81148, "omega":1.20762, "ax":0.79967, "ay":-3.91623, "alpha":-1.54489, "fx":[10.81192,5.97712,14.5453,19.44703], "fy":[-57.99016,-63.54468,-66.26609,-60.89117]}, + {"t":2.02314, "x":7.4923, "y":3.34971, "heading":2.65541, "vx":-0.99461, "vy":0.70713, "omega":1.16646, "ax":0.89037, "ay":-3.89685, "alpha":-1.782, "fx":[11.77431,6.46113,16.39859,21.9069], "fy":[-57.09451,-63.74427,-66.46888,-60.15402]}, + {"t":2.04978, "x":7.46611, "y":3.36717, "heading":2.68649, "vx":-0.97088, "vy":0.60331, "omega":1.11898, "ax":0.96723, "ay":-3.8787, "alpha":-2.01489, "fx":[12.45622,6.73284,18.20958,24.0234], "fy":[-56.29515,-63.86763,-66.72755,-59.4185]}, + {"t":2.07643, "x":7.44059, "y":3.38187, "heading":2.71631, "vx":-0.94511, "vy":0.49996, "omega":1.06529, "ax":1.03312, "ay":-3.86185, "alpha":-2.33292, "fx":[12.79636,6.55149,19.79744,26.46108], "fy":[-55.26645,-64.31472,-67.09314,-58.56469]}, + {"t":2.10307, "x":7.41577, "y":3.39382, "heading":2.74469, "vx":-0.91759, "vy":0.39706, "omega":1.00313, "ax":-1.30677, "ay":-3.77443, "alpha":-2.26185, "fx":[-26.10619,-29.15264,-15.89013,-11.83463], "fy":[-53.30438,-61.21984,-66.29751,-58.86563]}, + {"t":2.13948, "x":7.3815, "y":3.40577, "heading":2.78121, "vx":-0.96516, "vy":0.25965, "omega":0.92079, "ax":-0.76028, "ay":-3.91792, "alpha":-1.88549, "fx":[-16.57689,-19.23651,-7.92433,-4.54251], "fy":[-57.04513,-63.71942,-67.18303,-60.85175]}, + {"t":2.17588, "x":7.34586, "y":3.41263, "heading":2.81473, "vx":-0.99284, "vy":0.11701, "omega":0.85214, "ax":-0.07095, "ay":-0.66849, "alpha":-2.19957, "fx":[-5.31468,-8.63492,2.96204,6.48175], "fy":[-3.61341,-14.01881,-17.28079,-7.53778]}, + {"t":2.21229, "x":7.30967, "y":3.41644, "heading":2.84576, "vx":-0.99542, "vy":0.09268, "omega":0.77206, "ax":0.02079, "ay":0.2142, "alpha":-1.86859, "fx":[-3.2936,-5.95572,3.96469,6.60493], "fy":[9.26093,0.49444,-2.39393,6.24087]}, + {"t":2.2487, "x":7.27344, "y":3.41996, "heading":2.87386, "vx":-0.99467, "vy":0.10047, "omega":0.70404, "ax":0.01227, "ay":0.11889, "alpha":-1.53023, "fx":[-2.92174,-4.92003,3.32442,5.2966], "fy":[6.55231,-0.43468,-2.86288,4.2948]}, + {"t":2.2851, "x":7.23724, "y":3.4237, "heading":2.8995, "vx":-0.99422, "vy":0.1048, "omega":0.64833, "ax":0.0087, "ay":0.08133, "alpha":-1.26183, "fx":[-2.53621,-4.03302,2.81989,4.30159], "fy":[5.0628,-0.67784,-2.58625,3.36626]}, + {"t":2.32151, "x":7.20105, "y":3.42757, "heading":2.9231, "vx":-0.9939, "vy":0.10776, "omega":0.60239, "ax":0.00708, "ay":0.06461, "alpha":-1.04428, "fx":[-2.17569,-3.29691,2.40438,3.51803], "fy":[4.09598,-0.67851,-2.13581,2.82139]}, + {"t":2.35792, "x":7.16487, "y":3.43153, "heading":2.94503, "vx":-0.99364, "vy":0.11012, "omega":0.56437, "ax":0.00591, "ay":0.05282, "alpha":-0.86682, "fx":[-1.86121,-2.70083,2.05051,2.88654], "fy":[3.34827,-0.64551,-1.74079,2.39221]}, + {"t":2.39432, "x":7.1287, "y":3.43558, "heading":2.96558, "vx":-0.99343, "vy":0.11204, "omega":0.53281, "ax":0.00508, "ay":0.04472, "alpha":-0.72212, "fx":[-1.58997,-2.21825,1.75214,2.37872], "fy":[2.77043,-0.58664,-1.39864,2.05456]}, + {"t":2.43073, "x":7.09253, "y":3.43969, "heading":2.98497, "vx":-0.99324, "vy":0.11367, "omega":0.50652, "ax":0.00452, "ay":0.03923, "alpha":-0.60506, "fx":[-1.3602,-1.83032,1.50408,1.97336], "fy":[2.32524,-0.51482,-1.10886,1.78963]}, + {"t":2.46714, "x":7.05638, "y":3.44385, "heading":3.00341, "vx":-0.99308, "vy":0.1151, "omega":0.48449, "ax":0.00411, "ay":0.03522, "alpha":-0.51091, "fx":[-1.16883,-1.52064,1.2994,1.65076], "fy":[1.9773,-0.44475,-0.87262,1.57674]}, + {"t":2.50354, "x":7.02022, "y":3.44806, "heading":3.02105, "vx":-0.99293, "vy":0.11638, "omega":0.46589, "ax":0.00378, "ay":0.03209, "alpha":-0.43652, "fx":[-1.01398,-1.27778,1.13415,1.39769], "fy":[1.70513,-0.38467,-0.68751,1.40511]}, + {"t":2.53995, "x":6.98408, "y":3.45232, "heading":3.03801, "vx":-0.99279, "vy":0.11755, "omega":0.45, "ax":0.00352, "ay":0.02961, "alpha":-0.37879, "fx":[-0.89193,-1.09028,1.00386,1.20204], "fy":[1.49401,-0.33656,-0.54574,1.2688]}, + {"t":2.57636, "x":6.94794, "y":3.45662, "heading":3.0544, "vx":-0.99266, "vy":0.11862, "omega":0.43621, "ax":0.00332, "ay":0.02767, "alpha":-0.33592, "fx":[-0.80122,-0.95107,0.90671,1.05644], "fy":[1.33526,-0.30191,-0.44182,1.16547]}, + {"t":2.61276, "x":6.9118, "y":3.46096, "heading":3.07028, "vx":-0.99254, "vy":0.11963, "omega":0.42398, "ax":0.00317, "ay":0.02617, "alpha":-0.30609, "fx":[-0.73944,-0.85307,0.84005,0.95359], "fy":[1.22097,-0.28157,-0.36995,1.09255]}, + {"t":2.64917, "x":6.87567, "y":3.46533, "heading":3.08571, "vx":-0.99243, "vy":0.12058, "omega":0.41284, "ax":0.00306, "ay":0.02505, "alpha":-0.28845, "fx":[-0.7064,-0.79249,0.80345,0.88948], "fy":[1.14619,-0.27719,-0.32724,1.04917]}, + {"t":2.68557, "x":6.83954, "y":3.46974, "heading":3.10074, "vx":-0.99232, "vy":0.1215, "omega":0.40234, "ax":0.00298, "ay":0.02426, "alpha":-0.28223, "fx":[-0.7016,-0.7658,0.79628,0.86042], "fy":[1.10685,-0.29049,-0.31073,1.03474]}, + {"t":2.72198, "x":6.80341, "y":3.47418, "heading":3.11539, "vx":-0.99221, "vy":0.12238, "omega":0.39206, "ax":0.00294, "ay":0.02375, "alpha":-0.28732, "fx":[-0.72618,-0.77148,0.81955,0.8648], "fy":[1.10069,-0.32335,-0.31915,1.05]}, + {"t":2.75839, "x":6.76729, "y":3.47865, "heading":3.12966, "vx":-0.9921, "vy":0.12324, "omega":0.3816, "ax":0.00293, "ay":0.02351, "alpha":-0.30391, "fx":[-0.78183,-0.80891,0.87494,0.90196], "fy":[1.1268,-0.37855,-0.35175,1.0967]}, + {"t":2.79479, "x":6.73117, "y":3.48315, "heading":-3.13963, "vx":-0.99199, "vy":0.1241, "omega":0.37054, "ax":0.00296, "ay":0.02354, "alpha":-0.33257, "fx":[-0.87135,-0.87855,0.96522,0.97236], "fy":[1.18504,-0.45866,-0.40876,1.17736]}, + {"t":2.8312, "x":6.69506, "y":3.48768, "heading":-3.12614, "vx":-0.99189, "vy":0.12496, "omega":0.35843, "ax":0.00301, "ay":0.02383, "alpha":-0.37443, "fx":[-0.99895,-0.98236,1.09467,1.07799], "fy":[1.27698,-0.568,-0.49156,1.2961]}, + {"t":2.86761, "x":6.65895, "y":3.49225, "heading":-3.11309, "vx":-0.99178, "vy":0.12582, "omega":0.3448, "ax":0.00311, "ay":0.0244, "alpha":-0.4309, "fx":[-1.16974,-1.12302,1.26845,1.2216], "fy":[1.4046,-0.71067,-0.60212,1.45769]}, + {"t":2.90401, "x":6.62285, "y":3.49685, "heading":-3.10054, "vx":-0.99166, "vy":0.12671, "omega":0.32911, "ax":0.00324, "ay":0.02526, "alpha":-0.5041, "fx":[-1.39082,-1.30499,1.49379,1.40777], "fy":[1.57182,-0.89276,-0.74383,1.66904]}, + {"t":2.94042, "x":6.58675, "y":3.50148, "heading":-3.08855, "vx":-0.99155, "vy":0.12763, "omega":0.31076, "ax":0.00342, "ay":0.02646, "alpha":-0.59656, "fx":[-1.67064,-1.53385,1.77934,1.64226], "fy":[1.78349,-1.12073,-0.92084,1.9383]}, + {"t":2.97683, "x":6.55065, "y":3.50614, "heading":-3.07724, "vx":-0.99142, "vy":0.1286, "omega":0.28904, "ax":0.00365, "ay":0.02804, "alpha":-0.71156, "fx":[-2.01968,-1.81691,2.13588,1.93263], "fy":[2.04631,-1.40269,-1.13861,2.27581]}, + {"t":3.01323, "x":6.51456, "y":3.51084, "heading":-3.06672, "vx":-0.99129, "vy":0.12962, "omega":0.26313, "ax":0.00395, "ay":0.03009, "alpha":-0.85324, "fx":[-2.45105,-2.16382,2.57691,2.28885], "fy":[2.3692,-1.74867,-1.40439,2.69444]}, + {"t":3.04964, "x":6.47847, "y":3.51558, "heading":-3.05714, "vx":-0.99115, "vy":0.13071, "omega":0.23207, "ax":0.00433, "ay":0.03268, "alpha":-1.02635, "fx":[-2.97953,-2.58585,3.11772,2.72258], "fy":[2.76263,-2.16964,-1.7267,3.20873]}, + {"t":3.08604, "x":6.44239, "y":3.52036, "heading":-3.04869, "vx":-0.99099, "vy":0.1319, "omega":0.1947, "ax":0.00481, "ay":0.03596, "alpha":-1.23726, "fx":[-3.62418,-3.09861,3.77826,3.24997], "fy":[3.24207,-2.68016,-2.11684,3.83823]}, + {"t":3.12245, "x":6.40632, "y":3.52519, "heading":-3.0416, "vx":-0.99081, "vy":0.13321, "omega":0.14966, "ax":0.00544, "ay":0.04027, "alpha":-1.49285, "fx":[-4.40466,-3.71929,4.58028,3.88945], "fy":[3.82773,-3.29181,-2.5847,4.60628]}, + {"t":3.15886, "x":6.37025, "y":3.53006, "heading":-3.03615, "vx":-0.99061, "vy":0.13468, "omega":0.09531, "ax":0.00634, "ay":0.04637, "alpha":-1.80243, "fx":[-5.34563,-4.47159,5.55286,4.66725], "fy":[4.5532,-4.01704,-3.13948,5.54798]}, + {"t":3.19526, "x":6.33419, "y":3.535, "heading":-3.03268, "vx":-0.99038, "vy":0.13637, "omega":0.02969, "ax":0.00765, "ay":0.05515, "alpha":-2.17579, "fx":[-6.47109,-5.38233,6.72652,5.61277], "fy":[5.45788,-4.86517,-3.79014,6.69993]}, + {"t":3.23167, "x":6.29814, "y":3.54, "heading":-3.0316, "vx":-0.9901, "vy":0.13837, "omega":-0.04952, "ax":0.00941, "ay":0.06677, "alpha":-2.62585, "fx":[-7.81218,-6.49061,8.13741,6.76321], "fy":[6.58101,-5.86772,-4.56505,8.09155]}, + {"t":3.26808, "x":6.2621, "y":3.54508, "heading":-3.0334, "vx":-0.98976, "vy":0.1408, "omega":-0.14512, "ax":0.01217, "ay":0.0846, "alpha":-3.16763, "fx":[-9.39185,-7.83478,9.82899,8.1704], "fy":[8.02787,-7.01579,-5.44644,9.80666]}, + {"t":3.30448, "x":6.22607, "y":3.55026, "heading":-3.03869, "vx":-0.98932, "vy":0.14388, "omega":-0.26044, "ax":0.0215, "ay":0.14509, "alpha":-3.82497, "fx":[-11.15942,-9.385,11.91985,9.98964], "fy":[10.46956,-7.84072,-5.90448,12.48917]}, + {"t":3.34089, "x":6.19007, "y":3.5556, "heading":-3.04817, "vx":-0.98854, "vy":0.14917, "omega":-0.39969, "ax":0.03664, "ay":0.23523, "alpha":-4.62151, "fx":[-13.16128,-11.14517,14.29945,12.33399], "fy":[13.80676,-8.80349,-6.11848,16.05326]}, + {"t":3.3773, "x":6.1541, "y":3.56118, "heading":-3.06272, "vx":-0.9872, "vy":0.15773, "omega":-0.56795, "ax":2.98354, "ay":-1.62637, "alpha":-4.10304, "fx":[37.02077,36.15418,57.31094,58.97775], "fy":[-17.4489,-39.42436,-33.27144,-13.13425]}, + {"t":3.4137, "x":6.12014, "y":3.56585, "heading":-3.0834, "vx":-0.87858, "vy":0.09852, "omega":-0.71732, "ax":1.68179, "ay":-3.62493, "alpha":-4.49988, "fx":[13.03798,12.45802,38.79489,42.50754], "fy":[-51.05937,-69.32935,-64.09276,-45.71224]}, + {"t":3.45011, "x":6.08927, "y":3.56703, "heading":-3.10951, "vx":-0.81735, "vy":-0.03345, "omega":-0.88115, "ax":1.07704, "ay":-3.85077, "alpha":-5.31926, "fx":[-0.31699,0.38725,31.93303,36.39218], "fy":[-53.03795,-73.3002,-69.43057,-48.76649]}, + {"t":3.48652, "x":6.06022, "y":3.56326, "heading":3.14159, "vx":-0.77814, "vy":-0.17364, "omega":-1.0748, "ax":0.99093, "ay":-3.8714, "alpha":-3.24687, "fx":[5.62503,4.98467,24.83952,27.47751], "fy":[-56.17028,-68.66984,-66.75377,-54.25141]}, + {"t":3.50799, "x":6.04374, "y":3.55864, "heading":3.11851, "vx":-0.75686, "vy":-0.25679, "omega":-1.14454, "ax":0.9947, "ay":-3.8723, "alpha":-2.91717, "fx":[6.94117,5.87945,23.7903,26.5554], "fy":[-56.6395,-67.75842,-66.39675,-55.10757]}, + {"t":3.52947, "x":6.02771, "y":3.55223, "heading":3.09392, "vx":-0.7355, "vy":-0.33997, "omega":-1.2072, "ax":0.99833, "ay":-3.87126, "alpha":-2.64276, "fx":[8.12509,6.67633,22.96408,25.63141], "fy":[-56.82184,-67.11556,-66.05642,-55.84257]}, + {"t":3.55095, "x":6.01215, "y":3.54403, "heading":3.068, "vx":-0.71405, "vy":-0.42311, "omega":-1.26396, "ax":1.00234, "ay":-3.87011, "alpha":-2.38849, "fx":[9.13334,7.43634,22.15202,24.92976], "fy":[-57.14896,-66.3774,-65.75917,-56.4779]}, + {"t":3.57243, "x":5.99704, "y":3.53405, "heading":3.04085, "vx":-0.69252, "vy":-0.50624, "omega":-1.31526, "ax":1.00679, "ay":-3.86883, "alpha":-2.16825, "fx":[10.07905,8.12969,21.50561,24.21967], "fy":[-57.32422,-65.83976,-65.46679,-57.05133]}, + {"t":3.59391, "x":5.9824, "y":3.52229, "heading":3.0126, "vx":-0.6709, "vy":-0.58933, "omega":-1.36183, "ax":1.01175, "ay":-3.86739, "alpha":-1.96481, "fx":[10.9102,8.80194,20.88055,23.65671], "fy":[-57.56568,-65.25557,-65.21282,-57.55684]}, + {"t":3.61539, "x":5.96822, "y":3.50874, "heading":2.98335, "vx":-0.64917, "vy":-0.6724, "omega":-1.40403, "ax":1.01733, "ay":-3.86577, "alpha":-1.79338, "fx":[11.66687,9.39601,20.40098,23.13974], "fy":[-57.72073,-64.82951,-64.95811,-57.9796]}, + {"t":3.63686, "x":5.95451, "y":3.49341, "heading":2.95319, "vx":-0.62732, "vy":-0.75543, "omega":-1.44255, "ax":1.02364, "ay":-3.86393, "alpha":-1.63658, "fx":[12.36258,9.98085,19.9421,22.71887], "fy":[-57.86268,-64.36876,-64.76057,-58.37877]}, + {"t":3.65834, "x":5.94128, "y":3.47629, "heading":2.92221, "vx":-0.60533, "vy":-0.83842, "omega":-1.4777, "ax":1.03084, "ay":-3.86181, "alpha":-1.51462, "fx":[12.96362,10.45267,19.64558,22.39952], "fy":[-57.9847,-64.11473,-64.50551,-58.63135]}, + {"t":3.67982, "x":5.92851, "y":3.45739, "heading":2.89047, "vx":-0.58319, "vy":-0.92137, "omega":-1.51023, "ax":1.03913, "ay":-3.85935, "alpha":-1.37952, "fx":[13.50014,11.01872,19.35582,22.11304], "fy":[-58.25871,-63.59766,-64.32245,-58.90141]}, + {"t":3.7013, "x":5.91623, "y":3.43671, "heading":2.85803, "vx":-0.56087, "vy":-1.00426, "omega":-1.53986, "ax":1.04876, "ay":-3.85647, "alpha":-1.30216, "fx":[14.0457,11.41075,19.17859,21.96448], "fy":[-58.18116,-63.43099,-64.16429,-59.1208]}, + {"t":3.72278, "x":5.90442, "y":3.41425, "heading":2.82496, "vx":-0.53835, "vy":-1.08709, "omega":-1.56783, "ax":1.06012, "ay":-3.85304, "alpha":-1.20709, "fx":[14.5475,11.9046,19.04714,21.82131], "fy":[-58.3157,-63.03263,-64.02597,-59.30511]}, + {"t":3.74426, "x":5.8931, "y":3.39001, "heading":2.79128, "vx":-0.51558, "vy":-1.16985, "omega":-1.59376, "ax":1.07369, "ay":-3.84889, "alpha":-1.1667, "fx":[15.03287,12.22738,19.02204,21.9], "fy":[-58.20214,-62.9284,-63.88593,-59.39951]}, + {"t":3.76573, "x":5.88228, "y":3.364, "heading":2.75705, "vx":-0.49252, "vy":-1.25251, "omega":-1.61882, "ax":1.09021, "ay":-3.84377, "alpha":-1.09803, "fx":[15.48851,12.7033,19.05787,21.98191], "fy":[-58.36919,-62.50486,-63.75841,-59.45819]}, + {"t":3.78721, "x":5.87195, "y":3.33621, "heading":2.72228, "vx":-0.4691, "vy":-1.33507, "omega":-1.6424, "ax":1.11071, "ay":-3.8373, "alpha":-1.09685, "fx":[16.05578,13.00663,19.19625,22.27475], "fy":[-58.03951,-62.43878,-63.69104,-59.51064]}, + {"t":3.80869, "x":5.86213, "y":3.30665, "heading":2.68701, "vx":-0.44524, "vy":-1.41749, "omega":-1.66596, "ax":1.13691, "ay":-3.82886, "alpha":-1.0608, "fx":[16.64746,13.53067,19.45395,22.56522], "fy":[-57.99788,-62.04879,-63.59711,-59.50004]}, + {"t":3.83017, "x":5.85283, "y":3.27532, "heading":2.65123, "vx":-0.42083, "vy":-1.49973, "omega":-1.68874, "ax":1.17147, "ay":-3.81742, "alpha":-1.09676, "fx":[17.36058,13.90506,19.83769,23.28821], "fy":[-57.55971,-61.96258,-63.51109,-59.38373]}, + {"t":3.85165, "x":5.84406, "y":3.24223, "heading":2.61495, "vx":-0.39566, "vy":-1.58172, "omega":-1.7123, "ax":1.21928, "ay":-3.801, "alpha":-1.0986, "fx":[18.33356,14.65405,20.47953,23.96069], "fy":[-57.15394,-61.49246,-63.43547,-59.29276]}, + {"t":3.87313, "x":5.83584, "y":3.20738, "heading":2.57818, "vx":-0.36948, "vy":-1.66336, "omega":-1.7359, "ax":1.28943, "ay":-3.77567, "alpha":-1.16365, "fx":[19.54103,15.46037,21.40252,25.47863], "fy":[-56.58706,-60.9936,-63.26714,-58.91814]}, + {"t":3.8946, "x":5.82821, "y":3.17078, "heading":2.54089, "vx":-0.34178, "vy":-1.74445, "omega":-1.76089, "ax":1.40285, "ay":-3.73152, "alpha":-1.19535, "fx":[21.59374,17.09937,23.13544,27.25628], "fy":[-55.64379,-60.24858,-62.78706,-58.28321]}, + {"t":3.91608, "x":5.82119, "y":3.13245, "heading":2.50307, "vx":-0.31165, "vy":-1.8246, "omega":-1.78656, "ax":1.6148, "ay":-3.63796, "alpha":-1.33133, "fx":[25.11746,19.97687,26.19052,31.25993], "fy":[-53.6353,-58.77185,-61.81363,-56.80034]}, + {"t":3.93756, "x":5.81487, "y":3.09243, "heading":2.4647, "vx":-0.27697, "vy":-1.90274, "omega":-1.81516, "ax":2.14072, "ay":-3.33612, "alpha":-1.44055, "fx":[33.94937,28.10801,34.55683,39.32808], "fy":[-47.77848,-54.05942,-57.77436,-52.24115]}, + {"t":3.95904, "x":5.80941, "y":3.05079, "heading":2.42571, "vx":-0.23099, "vy":-1.97439, "omega":-1.8461, "ax":3.79077, "ay":-0.88431, "alpha":-1.7677, "fx":[59.89019,54.11633,60.67906,66.0392], "fy":[-6.8381,-14.76405,-20.99282,-13.56154]}, + {"t":3.98052, "x":5.80533, "y":3.00818, "heading":2.38606, "vx":-0.14957, "vy":-1.99339, "omega":-1.88407, "ax":3.43195, "ay":-0.19724, "alpha":-2.05324, "fx":[54.4663,47.17937,55.92935,60.364], "fy":[5.27681,-2.72728,-11.7272,-3.34757]}, + {"t":4.002, "x":5.8029, "y":2.96532, "heading":2.34559, "vx":-0.07586, "vy":-1.99762, "omega":-1.92817, "ax":-3.71501, "ay":0.21681, "alpha":-2.22763, "fx":[-59.36879,-66.09545,-58.88533,-51.56476], "fy":[12.61196,3.85682,-5.66918,2.9686]}, + {"t":4.02347, "x":5.80042, "y":2.92246, "heading":2.30418, "vx":-0.15565, "vy":-1.99297, "omega":-1.97601, "ax":-3.88169, "ay":0.4508, "alpha":-2.3442, "fx":[-61.65473,-68.828,-62.28082,-53.73535], "fy":[16.78066,7.89945,-2.64306,6.59009]}, + {"t":4.04495, "x":5.79618, "y":2.87976, "heading":2.26174, "vx":-0.23902, "vy":-1.98328, "omega":-2.02636, "ax":-3.73301, "ay":0.43904, "alpha":-1.90563, "fx":[-58.98763,-65.20382,-59.77152,-53.09403], "fy":[14.95767,7.81357,-0.86807,5.97695]}, + {"t":4.05824, "x":5.79267, "y":2.85344, "heading":2.23481, "vx":-0.28863, "vy":-1.97745, "omega":-2.05169, "ax":-3.79676, "ay":0.60095, "alpha":-1.74235, "fx":[-60.07598,-65.74976,-61.18968,-54.09006], "fy":[16.35666,10.65601,2.55407,8.59531]}, + {"t":4.07153, "x":5.7885, "y":2.82721, "heading":2.20754, "vx":-0.3391, "vy":-1.96946, "omega":-2.07485, "ax":-3.73339, "ay":0.6904, "alpha":-1.50492, "fx":[-58.65989,-63.89591,-60.01756,-54.50802], "fy":[17.31787,11.79681,4.76795,9.95949]}, + {"t":4.08482, "x":5.78367, "y":2.8011, "heading":2.17996, "vx":-0.38872, "vy":-1.96029, "omega":-2.09485, "ax":-3.64575, "ay":0.76915, "alpha":-1.45924, "fx":[-57.49031,-62.5875,-58.96466,-52.47323], "fy":[17.63655,13.43751,6.58047,11.18895]}, + {"t":4.09812, "x":5.77818, "y":2.77511, "heading":2.15212, "vx":-0.43717, "vy":-1.95006, "omega":-2.11424, "ax":-3.51919, "ay":0.8329, "alpha":-1.23994, "fx":[-55.10857,-59.76444,-56.71147,-51.89425], "fy":[18.36385,14.04884,8.29352,12.18555]}, + {"t":4.11141, "x":5.77206, "y":2.74927, "heading":2.12402, "vx":-0.48394, "vy":-1.93899, "omega":-2.13072, "ax":-3.23481, "ay":0.84515, "alpha":-1.22907, "fx":[-50.86514,-55.50661,-52.49373,-46.55439], "fy":[17.64028,14.75729,8.94379,12.32795]}, + {"t":4.1247, "x":5.76534, "y":2.72357, "heading":2.0957, "vx":-0.52694, "vy":-1.92776, "omega":-2.14706, "ax":-3.03052, "ay":0.86218, "alpha":-1.024, "fx":[-47.30081,-51.45228,-48.96286,-44.73071], "fy":[17.78925,14.49741,9.8304,12.63393]}, + {"t":4.13799, "x":5.75807, "y":2.69802, "heading":2.06717, "vx":-0.56721, "vy":-1.9163, "omega":-2.16067, "ax":-2.09735, "ay":0.63757, "alpha":-0.97345, "fx":[-32.64521,-36.64901,-34.27565,-29.61819], "fy":[13.38943,11.42377,6.66701,9.00738]}, + {"t":4.15128, "x":5.75034, "y":2.67261, "heading":2.03845, "vx":-0.59509, "vy":-1.90783, "omega":-2.1736, "ax":-1.90426, "ay":0.60711, "alpha":-0.81603, "fx":[-29.45037,-32.96929,-31.02873,-27.4774], "fy":[12.72855,10.51311,6.71281,8.59856]}, + {"t":4.16457, "x":5.74227, "y":2.64731, "heading":2.00956, "vx":-0.6204, "vy":-1.89976, "omega":-2.18445, "ax":-1.20849, "ay":0.40052, "alpha":-0.70433, "fx":[-18.49784,-21.56825,-19.95959,-16.71716], "fy":[8.75976,7.44224,3.86913,5.36317]}, + {"t":4.17786, "x":5.73391, "y":2.6221, "heading":1.98053, "vx":-0.63646, "vy":-1.89444, "omega":-2.19381, "ax":-0.89299, "ay":0.30321, "alpha":-0.57956, "fx":[-13.51373,-16.10432,-14.84508,-12.24439], "fy":[6.8833,5.66311,2.78419,3.92408]}, + {"t":4.19115, "x":5.72538, "y":2.59694, "heading":1.95137, "vx":-0.64833, "vy":-1.89041, "omega":-2.20151, "ax":-0.67562, "ay":0.23325, "alpha":-0.45099, "fx":[-10.15186,-12.19359,-11.27242,-9.28602], "fy":[5.28588,4.46843,2.11278,2.94489]}, + {"t":4.20444, "x":5.7167, "y":2.57184, "heading":1.92211, "vx":-0.65731, "vy":-1.88731, "omega":-2.20751, "ax":-0.33289, "ay":0.11674, "alpha":-0.34007, "fx":[-4.83075,-6.38357,-5.74005,-4.18532], "fy":[2.9836,2.50752,0.67985,1.2421]}, + {"t":4.21773, "x":5.70793, "y":2.54677, "heading":1.89277, "vx":-0.66173, "vy":-1.88575, "omega":-2.21203, "ax":-0.37322, "ay":0.1312, "alpha":-0.21148, "fx":[-5.58681,-6.56818,-6.19686,-5.34856], "fy":[2.87797,2.45956,1.33173,1.66253]}, + {"t":4.23102, "x":5.69911, "y":2.52171, "heading":1.86337, "vx":-0.66669, "vy":-1.88401, "omega":-2.21484, "ax":-0.06804, "ay":0.02435, "alpha":-0.10424, "fx":[-0.92417,-1.39916,-1.23625,-0.76128], "fy":[0.66405,0.6971,0.0256,0.15928]}, + {"t":4.24431, "x":5.69024, "y":2.49668, "heading":1.83393, "vx":-0.6676, "vy":-1.88369, "omega":-2.21622, "ax":-0.20508, "ay":0.07269, "alpha":0.02257, "fx":[-3.26006,-3.15073,-3.1837,-3.42894], "fy":[1.1808,1.07105,1.19681,1.16733]}, + {"t":4.2576, "x":5.68135, "y":2.47165, "heading":1.80448, "vx":-0.67032, "vy":-1.88272, "omega":-2.21592, "ax":0.03754, "ay":-0.01329, "alpha":0.13289, "fx":[0.35997,1.00659,0.83204,0.1856], "fy":[-0.72512,-0.37413,0.20811,0.04713]}, + {"t":4.2709, "x":5.67244, "y":2.44662, "heading":1.77503, "vx":-0.66982, "vy":-1.8829, "omega":-2.21416, "ax":-0.11167, "ay":0.03976, "alpha":0.26089, "fx":[-2.22608,-0.97159,-1.26709,-2.62632], "fy":[-0.10191,0.004,1.44172,1.18124]}, + {"t":4.28419, "x":5.66353, "y":2.4216, "heading":1.7456, "vx":-0.67131, "vy":-1.88237, "omega":-2.21069, "ax":0.06354, "ay":-0.02269, "alpha":0.37989, "fx":[0.27334,2.11346,1.74367,-0.09566], "fy":[-1.63271,-1.12588,0.82344,0.49448]}, + {"t":4.29748, "x":5.65461, "y":2.39658, "heading":1.71622, "vx":-0.67046, "vy":-1.88267, "omega":-2.20564, "ax":-0.05808, "ay":0.02075, "alpha":0.51386, "fx":[-1.94278,0.54529,0.13001,-2.42047], "fy":[-1.17155,-0.94806,1.90115,1.53622]}, + {"t":4.31077, "x":5.6457, "y":2.37156, "heading":1.6869, "vx":-0.67124, "vy":-1.88239, "omega":-2.19881, "ax":0.05, "ay":-0.01788, "alpha":0.647, "fx":[-0.56927,2.57396,2.15618,-0.98569], "fy":[-2.30487,-1.79886,1.66819,1.30013]}, + {"t":4.32406, "x":5.63678, "y":2.34654, "heading":1.65768, "vx":-0.67057, "vy":-1.88263, "omega":-2.19021, "ax":-0.02501, "ay":0.00892, "alpha":0.79222, "fx":[-2.12629,1.72442,1.34151,-2.52816], "fy":[-2.15071,-1.92024,2.48641,2.15081]}, + {"t":4.33735, "x":5.62787, "y":2.32152, "heading":1.62857, "vx":-0.6709, "vy":-1.88251, "omega":-2.17968, "ax":0.00285, "ay":-0.00067, "alpha":0.94323, "fx":[-2.10272,2.49612,2.19129,-2.40398], "fy":[-2.79959,-2.41773,2.72003,2.45481]}, + {"t":4.35064, "x":5.61895, "y":2.2965, "heading":1.5996, "vx":-0.67087, "vy":-1.88252, "omega":-2.16715, "ax":0.00918, "ay":0.11587, "alpha":1.10519, "fx":[-2.46227,2.93397,2.74877,-2.63725], "fy":[-1.33962,-1.08134,4.96458,4.81434]}, + {"t":4.36393, "x":5.61003, "y":2.27149, "heading":1.5708, "vx":-0.67074, "vy":-1.88098, "omega":-2.15246, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[ + {"name":"IntakeDown", "from":{"target":0, "targetTimestamp":0.0, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeDown"}}}, + {"name":"IntakeUp", "from":{"target":5, "targetTimestamp":4.04495, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeUp"}}}] +} diff --git a/src/main/deploy/choreo/RightSecondDipLong.traj b/src/main/deploy/choreo/RightSecondDipLong.traj new file mode 100644 index 0000000..a6c66a0 --- /dev/null +++ b/src/main/deploy/choreo/RightSecondDipLong.traj @@ -0,0 +1,218 @@ +{ + "name":"RightSecondDipLong", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":5.607864856719971, "y":2.2545297145843506, "heading":1.5707963267948966, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.045049667358398, "y":3.3438565731048584, "heading":0.5147387030254468, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.347577571868896, "y":3.3716142177581787, "heading":-0.5372850784864176, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.390566825866699, "y":2.0691564083099365, "heading":-2.3403229496705253, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.5508832931518555, "y":1.5936731100082395, "heading":-3.12425334769889, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.6302666664123535, "y":2.281606435775757, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":0, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":8.0}}, "enabled":true}, + {"from":0, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxAngularVelocity", "props":{"max":10.0}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":false}, + {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"5.607864856719971 m", "val":5.607864856719971}, "y":{"exp":"2.2545297145843506 m", "val":2.2545297145843506}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.045049667358398 m", "val":6.045049667358398}, "y":{"exp":"3.3438565731048584 m", "val":3.3438565731048584}, "heading":{"exp":"0.5147387030254468 rad", "val":0.5147387030254468}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.3475775718688965 m", "val":7.347577571868896}, "y":{"exp":"3.3716142177581787 m", "val":3.3716142177581787}, "heading":{"exp":"-0.5372850784864176 rad", "val":-0.5372850784864176}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.390566825866699 m", "val":7.390566825866699}, "y":{"exp":"2.0691564083099365 m", "val":2.0691564083099365}, "heading":{"exp":"-2.3403229496705253 rad", "val":-2.3403229496705253}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.5508832931518555 m", "val":6.5508832931518555}, "y":{"exp":"1.5936731100082397 m", "val":1.5936731100082395}, "heading":{"exp":"-3.12425334769889 rad", "val":-3.12425334769889}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.6302666664123535 m", "val":5.6302666664123535}, "y":{"exp":"2.281606435775757 m", "val":2.281606435775757}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":0, "to":5, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"8 m / s ^ 2", "val":8.0}}}, "enabled":true}, + {"from":0, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}, + {"from":0, "to":5, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"10 rad / s", "val":10.0}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":false}, + {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.5876,1.37018,2.03238,2.5622,3.26637], + "samples":[ + {"t":0.0, "x":5.60786, "y":2.25453, "heading":1.5708, "vx":0.74185, "vy":1.85352, "omega":-2.23879, "ax":-0.10652, "ay":0.1689, "alpha":-0.02315, "fx":[-1.69645,-1.68573,-1.68646,-1.69595], "fy":[2.55246,3.04575,2.56394,2.56355]}, + {"t":0.0226, "x":5.6246, "y":2.29646, "heading":1.5202, "vx":0.73945, "vy":1.85733, "omega":-2.23931, "ax":0.00976, "ay":-0.00388, "alpha":0.1594, "fx":[-0.28152,0.54466,0.59125,-0.23486], "fy":[-0.56461,-0.39924,0.3384,0.37887]}, + {"t":0.0452, "x":5.64132, "y":2.33844, "heading":1.46959, "vx":0.73967, "vy":1.85724, "omega":-2.23571, "ax":0.01013, "ay":-0.00404, "alpha":0.33461, "fx":[-0.75225,0.88846,1.07375,-0.56695], "fy":[-0.94405,-1.01317,0.76971,0.93108]}, + {"t":0.0678, "x":5.65804, "y":2.38041, "heading":1.41906, "vx":0.7399, "vy":1.85715, "omega":-2.22815, "ax":-0.00503, "ay":0.002, "alpha":0.50614, "fx":[-1.51297,0.93682,1.35325,-1.09654], "fy":[-1.20556,-1.49422,1.23206,1.59495]}, + {"t":0.0904, "x":5.67476, "y":2.42238, "heading":1.36871, "vx":0.73978, "vy":1.8572, "omega":-2.21671, "ax":-0.01498, "ay":0.00597, "alpha":0.67947, "fx":[-2.23738,1.01998,1.76171,-1.49565], "fy":[-1.46569,-2.0267,1.61234,2.25892]}, + {"t":0.113, "x":5.69147, "y":2.46436, "heading":1.31861, "vx":0.73944, "vy":1.85733, "omega":-2.20135, "ax":-0.0214, "ay":0.00852, "alpha":0.85519, "fx":[-2.94899,1.10776,2.26942,-1.78732], "fy":[-1.70583,-2.60729,1.92064,2.9334]}, + {"t":0.1356, "x":5.70818, "y":2.50634, "heading":1.26886, "vx":0.73896, "vy":1.85753, "omega":-2.18202, "ax":-0.02645, "ay":0.01052, "alpha":1.03677, "fx":[-3.68748,1.1675,2.84763,-2.00739], "fy":[-1.91225,-3.23644,2.17583,3.64079]}, + {"t":0.1582, "x":5.72487, "y":2.54832, "heading":1.21955, "vx":0.73836, "vy":1.85776, "omega":-2.15859, "ax":-0.0315, "ay":0.01251, "alpha":1.22338, "fx":[-4.46984,1.17251,3.46969,-2.1728], "fy":[-2.07071,-3.89849,2.38039,4.38344]}, + {"t":0.1808, "x":5.74155, "y":2.59031, "heading":1.17076, "vx":0.73765, "vy":1.85805, "omega":-2.13094, "ax":-0.0373, "ay":0.0148, "alpha":1.41877, "fx":[-5.31535,1.11211,4.13111,-2.29672], "fy":[-2.17754,-4.60138,2.54308,5.17566]}, + {"t":0.2034, "x":5.75821, "y":2.6323, "heading":1.1226, "vx":0.73681, "vy":1.85838, "omega":-2.09888, "ax":-0.04426, "ay":0.01754, "alpha":1.62162, "fx":[-6.22478,0.97452,4.8198,-2.38044], "fy":[-2.22523,-5.3315,2.65872,6.01161]}, + {"t":0.226, "x":5.77485, "y":2.67431, "heading":1.07517, "vx":0.73581, "vy":1.85878, "omega":-2.06223, "ax":-0.05252, "ay":0.02077, "alpha":1.83559, "fx":[-7.2081,0.75768,5.54153,-2.42635], "fy":[-2.2136,-6.09922,2.73051,6.90137]}, + {"t":0.2486, "x":5.79147, "y":2.71632, "heading":1.02856, "vx":0.73462, "vy":1.85925, "omega":-2.02075, "ax":-0.06205, "ay":0.02449, "alpha":2.05958, "fx":[-8.25984,0.45696,6.29174,-2.4295], "fy":[-2.13885,-6.89353,2.75093,7.83677]}, + {"t":0.2712, "x":5.80806, "y":2.75835, "heading":0.98289, "vx":0.73322, "vy":1.8598, "omega":-1.9742, "ax":-0.07267, "ay":0.02861, "alpha":2.29656, "fx":[-9.38278,0.07387,7.07998,-2.38569], "fy":[-2.00167,-7.72372,2.71903,8.82335]}, + {"t":0.2938, "x":5.82461, "y":2.80038, "heading":0.93827, "vx":0.73157, "vy":1.86045, "omega":-1.9223, "ax":-0.08391, "ay":0.03295, "alpha":2.54644, "fx":[-10.56646,-0.38882,7.91078,-2.28429], "fy":[-1.80161,-8.58716,2.62719,9.85393]}, + {"t":0.3164, "x":5.84112, "y":2.84244, "heading":0.89483, "vx":0.72968, "vy":1.86119, "omega":-1.86475, "ax":-0.0952, "ay":0.03726, "alpha":2.81118, "fx":[-11.80274,-0.92489,8.79594,-2.11379], "fy":[-1.53976,-9.49325,2.47113,10.92802]}, + {"t":0.339, "x":5.85759, "y":2.88451, "heading":0.85269, "vx":0.72753, "vy":1.86203, "omega":-1.80121, "ax":-0.10841, "ay":0.04227, "alpha":3.09103, "fx":[-13.11918,-1.5685,9.70567,-1.90206], "fy":[-1.20528,-10.42331,2.25834,12.05475]}, + {"t":0.3616, "x":5.874, "y":2.92661, "heading":0.81198, "vx":0.72508, "vy":1.86299, "omega":-1.73136, "ax":-0.12655, "ay":0.04914, "alpha":3.38592, "fx":[-14.5567,-2.37163,10.58779,-1.69549], "fy":[-0.77786,-11.3566,2.00557,13.24934]}, + {"t":0.3842, "x":5.89036, "y":2.96872, "heading":0.77285, "vx":0.72222, "vy":1.8641, "omega":-1.65483, "ax":-0.15676, "ay":0.06056, "alpha":3.69919, "fx":[-16.23015,-3.45009,11.3334,-1.60786], "fy":[-0.21124,-12.2684,1.75997,14.56545]}, + {"t":0.4068, "x":5.90664, "y":3.01087, "heading":0.73545, "vx":0.71867, "vy":1.86547, "omega":-1.57123, "ax":-0.21203, "ay":0.08137, "alpha":4.03235, "fx":[-18.3414,-5.00997,11.73345,-1.84681], "fy":[0.57876,-13.09536,1.60107,16.08289]}, + {"t":0.4294, "x":5.92283, "y":3.05305, "heading":0.69994, "vx":0.71388, "vy":1.86731, "omega":-1.4801, "ax":-0.30889, "ay":0.11743, "alpha":4.39584, "fx":[-21.18062,-7.30943,11.55034,-2.67562], "fy":[1.6998,-13.80256,1.63126,17.92881]}, + {"t":0.452, "x":5.93888, "y":3.09528, "heading":0.66649, "vx":0.7069, "vy":1.86996, "omega":-1.38075, "ax":-0.45148, "ay":0.16929, "alpha":4.78694, "fx":[-24.85454,-10.40446,10.74005,-4.15112], "fy":[3.10593,-14.25255,1.77035,20.1265]}, + {"t":0.4746, "x":5.95474, "y":3.13758, "heading":0.63528, "vx":0.6967, "vy":1.87379, "omega":-1.27257, "ax":-0.53202, "ay":0.19592, "alpha":5.22495, "fx":[-27.75994,-12.52976,11.01445,-4.50937], "fy":[4.28987,-15.2925,1.45057,21.99361]}, + {"t":0.4972, "x":5.97035, "y":3.17998, "heading":0.60652, "vx":0.68467, "vy":1.87822, "omega":-1.15448, "ax":-0.00312, "ay":0.00104, "alpha":5.57334, "fx":[-20.68881,-4.8219,20.19684,5.11587], "fy":[1.80703,-19.74287,-2.11681,20.11878]}, + {"t":0.5198, "x":5.98582, "y":3.22243, "heading":0.58043, "vx":0.6846, "vy":1.87824, "omega":-1.02853, "ax":4.38282, "ay":-1.73363, "alpha":3.6654, "fx":[58.82611,64.68355,79.6134,75.19883], "fy":[-28.33992,-43.19648,-27.28733,-11.26702]}, + {"t":0.5424, "x":6.00242, "y":3.26443, "heading":0.55719, "vx":0.78366, "vy":1.83906, "omega":-0.94569, "ax":7.1426, "ay":-3.44873, "alpha":0.57906, "fx":[112.72337,111.77568,114.0935,114.98367], "fy":[-55.64257,-58.14478,-53.89625,-51.32069]}, + {"t":0.565, "x":6.02195, "y":3.30512, "heading":0.53582, "vx":0.94508, "vy":1.76112, "omega":-0.9326, "ax":6.81591, "ay":-4.15267, "alpha":0.56539, "fx":[107.35103,106.38161,109.07153,110.02627], "fy":[-66.96828,-68.98852,-64.94245,-62.80748]}, + {"t":0.5876, "x":6.04505, "y":3.34386, "heading":0.51474, "vx":1.09912, "vy":1.66727, "omega":-0.91982, "ax":6.43113, "ay":-4.7175, "alpha":0.63976, "fx":[100.89645,99.83339,103.28847,104.37737], "fy":[-76.1801,-78.07417,-73.69425,-71.62684]}, + {"t":0.61659, "x":6.07961, "y":3.3902, "heading":0.48808, "vx":1.28552, "vy":1.53053, "omega":-0.90128, "ax":5.81101, "ay":-5.47284, "alpha":0.54873, "fx":[90.90586,90.12289,93.56114,94.42628], "fy":[-88.06785,-89.24876,-85.78119,-84.44334]}, + {"t":0.64557, "x":6.11931, "y":3.43226, "heading":0.46196, "vx":1.45395, "vy":1.37191, "omega":-0.88538, "ax":5.13021, "ay":-6.10183, "alpha":0.47215, "fx":[80.02684,79.50386,82.83013,83.4225], "fy":[-97.87688,-98.60721,-95.9113,-95.089]}, + {"t":0.67456, "x":6.16361, "y":3.46946, "heading":0.43629, "vx":1.60265, "vy":1.19505, "omega":-0.87169, "ax":4.37762, "ay":-6.63195, "alpha":0.40416, "fx":[68.05372,67.79812,70.89087,71.24923], "fy":[-106.09653,-106.51014,-104.5188,-104.0227]}, + {"t":0.70354, "x":6.2119, "y":3.50131, "heading":0.41103, "vx":1.72953, "vy":1.00283, "omega":-0.85998, "ax":3.54866, "ay":-7.00894, "alpha":0.36285, "fx":[54.84921,54.82501,57.7834,57.89287], "fy":[-111.89702,-112.15123,-110.67053,-110.36946]}, + {"t":0.73252, "x":6.26352, "y":3.52744, "heading":0.3861, "vx":1.83238, "vy":0.79968, "omega":-0.84946, "ax":2.58107, "ay":-6.89449, "alpha":0.51566, "fx":[38.75211,38.84401,43.19685,43.11266], "fy":[-110.05521,-110.49987,-108.85772,-108.40755]}, + {"t":0.76151, "x":6.31771, "y":3.54772, "heading":0.36148, "vx":1.90719, "vy":0.59984, "omega":-0.83451, "ax":1.17619, "ay":-4.19798, "alpha":0.83459, "fx":[15.38794,16.65764,21.797,20.84863], "fy":[-66.23427,-68.83642,-67.19474,-64.31855]}, + {"t":0.79049, "x":6.37348, "y":3.56334, "heading":0.33729, "vx":1.94129, "vy":0.47817, "omega":-0.81032, "ax":0.35876, "ay":-1.53143, "alpha":0.14331, "fx":[5.13649,5.20964,6.23859,6.1979], "fy":[-24.66099,-24.59289,-24.26461,-23.73156]}, + {"t":0.81948, "x":6.4299, "y":3.57656, "heading":0.31381, "vx":1.95168, "vy":0.43378, "omega":-0.80617, "ax":0.06561, "ay":-0.29836, "alpha":-0.79242, "fx":[3.99432,2.79841,-1.95098,-0.6754], "fy":[-5.64146,-3.00766,-3.23131,-7.0661]}, + {"t":0.84846, "x":6.4865, "y":3.58901, "heading":0.29044, "vx":1.95359, "vy":0.42513, "omega":-0.82914, "ax":-0.01005, "ay":0.0461, "alpha":-1.92248, "fx":[6.22715,3.66957,-6.69966,-3.83519], "fy":[-2.63161,6.74613,3.78364,-4.97054]}, + {"t":0.87745, "x":6.54312, "y":3.60135, "heading":0.26641, "vx":1.95329, "vy":0.42647, "omega":-0.88486, "ax":-0.03746, "ay":0.17058, "alpha":-2.95847, "fx":[9.49905,5.90737,-10.76086,-7.02435], "fy":[-1.80196,10.79423,7.86086,-6.02059]}, + {"t":0.90643, "x":6.59972, "y":3.61378, "heading":0.24076, "vx":1.95221, "vy":0.43141, "omega":-0.97061, "ax":-0.02118, "ay":0.09553, "alpha":-4.08598, "fx":[12.92349,8.55252,-14.03323,-8.78803], "fy":[-5.64527,13.71654,8.49755,-10.50255]}, + {"t":0.93541, "x":6.65629, "y":3.62632, "heading":0.21263, "vx":1.95159, "vy":0.43418, "omega":-1.08904, "ax":0.03915, "ay":-0.17712, "alpha":-5.11361, "fx":[17.38709,12.76669,-16.49989,-11.16747], "fy":[-11.55475,10.91852,6.65987,-17.27118]}, + {"t":0.9644, "x":6.71287, "y":3.63883, "heading":0.18106, "vx":1.95273, "vy":0.42905, "omega":-1.23725, "ax":0.22903, "ay":-1.0839, "alpha":-6.03777, "fx":[22.63328,18.57385,-16.69984,-9.96293], "fy":[-28.18244,-0.36645,-6.35191,-33.92975]}, + {"t":0.99338, "x":6.76957, "y":3.65081, "heading":0.1452, "vx":1.95937, "vy":0.39763, "omega":-1.41225, "ax":0.59891, "ay":-3.3818, "alpha":-6.19228, "fx":[29.09681,27.82862,-12.40166,-6.49127], "fy":[-63.80762,-38.34912,-43.71667,-68.881]}, + {"t":1.02237, "x":6.82661, "y":3.66092, "heading":0.10427, "vx":1.97673, "vy":0.29961, "omega":-1.59173, "ax":0.69356, "ay":-6.91861, "alpha":-3.07685, "fx":[23.19763,25.34642,-3.94214,-0.55911], "fy":[-110.59931,-106.43569,-109.32528,-112.99196]}, + {"t":1.05135, "x":6.88419, "y":3.6667, "heading":0.05813, "vx":1.99683, "vy":0.09908, "omega":-1.68091, "ax":-0.05565, "ay":-7.7959, "alpha":-1.32369, "fx":[5.2314,5.1955,-7.65942,-6.30155], "fy":[-124.19138,-123.36705,-123.30748,-124.19639]}, + {"t":1.08034, "x":6.94205, "y":3.66629, "heading":0.00941, "vx":1.99522, "vy":-0.12688, "omega":-1.71928, "ax":-0.96038, "ay":-7.8754, "alpha":-1.15004, "fx":[-9.58033,-10.30786,-21.27287,-19.82617], "fy":[-125.96106,-125.29885,-124.00994,-124.84099]}, + {"t":1.10932, "x":6.99947, "y":3.65931, "heading":-0.04042, "vx":1.96738, "vy":-0.35514, "omega":-1.75261, "ax":-1.87123, "ay":-7.74466, "alpha":-1.17457, "fx":[-23.75559,-25.08371,-36.0271,-33.96197], "fy":[-124.59444,-123.73248,-121.15165,-122.3303]}, + {"t":1.1383, "x":7.05571, "y":3.64576, "heading":-0.09122, "vx":1.91314, "vy":-0.57961, "omega":-1.78666, "ax":-2.94224, "ay":-7.4183, "alpha":-1.24668, "fx":[-40.294,-42.47752,-53.43457,-50.63497], "fy":[-120.47888,-119.07596,-114.82386,-116.7049]}, + {"t":1.16729, "x":7.10993, "y":3.62585, "heading":-0.143, "vx":1.82786, "vy":-0.79463, "omega":-1.82279, "ax":-5.33983, "ay":-5.94026, "alpha":-1.26763, "fx":[-78.60775,-82.80667,-90.95279,-86.72781], "fy":[-99.80002,-95.56934,-88.51322,-93.34163]}, + {"t":1.19627, "x":7.16066, "y":3.60032, "heading":-0.19583, "vx":1.67309, "vy":-0.9668, "omega":-1.85953, "ax":-6.39009, "ay":-4.79889, "alpha":-1.3209, "fx":[-95.9022,-100.45138,-106.81062,-102.62539], "fy":[-83.34165,-76.85469,-68.83,-75.71719]}, + {"t":1.22526, "x":7.20647, "y":3.57028, "heading":-0.24973, "vx":1.48788, "vy":-1.10589, "omega":-1.89782, "ax":-6.86175, "ay":-4.10042, "alpha":-1.39512, "fx":[-103.80731,-108.22489,-113.80734,-109.9016], "fy":[-73.31078,-65.48395,-56.71451,-64.8795]}, + {"t":1.25424, "x":7.24672, "y":3.53651, "heading":-0.30474, "vx":1.289, "vy":-1.22474, "omega":-1.93825, "ax":-7.11347, "ay":-3.6492, "alpha":-1.48797, "fx":[-108.00358,-112.27191,-117.53426,-113.91669], "fy":[-67.05905,-58.2876,-48.67543,-57.71273]}, + {"t":1.28323, "x":7.28109, "y":3.49947, "heading":-0.36092, "vx":1.08282, "vy":-1.33051, "omega":-1.98138, "ax":-7.26558, "ay":-3.33821, "alpha":-1.58368, "fx":[-110.54691,-114.60124,-119.79646,-116.44078], "fy":[-62.88,-53.56976,-42.92751,-52.60905]}, + {"t":1.31221, "x":7.30942, "y":3.45951, "heading":-0.41835, "vx":0.87223, "vy":-1.42727, "omega":-2.02728, "ax":-7.36585, "ay":-3.11233, "alpha":-1.69105, "fx":[-112.17603,-116.05974,-121.3098,-118.20738], "fy":[-60.05903,-50.34052,-38.54582,-48.69646]}, + {"t":1.34119, "x":7.33161, "y":3.41683, "heading":-0.47711, "vx":0.65874, "vy":-1.51748, "omega":-2.0763, "ax":-7.43626, "ay":-2.94139, "alpha":-1.79774, "fx":[-113.33332,-116.97969,-122.38429,-119.52726], "fy":[-57.99396,-48.17753,-35.06435,-45.55075]}, + {"t":1.37018, "x":7.34758, "y":3.37161, "heading":-0.53729, "vx":0.4432, "vy":-1.60273, "omega":-2.1284, "ax":-7.39804, "ay":-3.01905, "alpha":-1.33582, "fx":[-114.01713,-116.34543,-120.71646,-118.7183], "fy":[-56.123,-49.59671,-39.47225,-46.5265]}, + {"t":1.39225, "x":7.35556, "y":3.3355, "heading":-0.58427, "vx":0.2799, "vy":-1.66937, "omega":-2.15789, "ax":-7.09348, "ay":-3.67907, "alpha":-1.25748, "fx":[-108.9913,-110.9331,-116.20337,-114.32932], "fy":[-65.42809,-60.85886,-51.04812,-56.29671]}, + {"t":1.41433, "x":7.36001, "y":3.29776, "heading":-0.6319, "vx":0.12332, "vy":-1.75058, "omega":-2.18565, "ax":-5.74174, "ay":-5.5247, "alpha":-1.25827, "fx":[-86.83901,-87.30184,-95.75339,-94.72328], "fy":[-92.51954,-91.21441,-82.5062,-84.5945]}, + {"t":1.4364, "x":7.36133, "y":3.25777, "heading":-0.68014, "vx":-0.00342, "vy":-1.87253, "omega":-2.21342, "ax":0.18857, "ay":-5.7309, "alpha":-5.72713, "fx":[8.67076,33.31455,-4.66426,-25.34649], "fy":[-102.35867,-90.7595,-78.5537,-92.25704]}, + {"t":1.45847, "x":7.3613, "y":3.21504, "heading":-0.729, "vx":0.00075, "vy":-1.99903, "omega":-2.33984, "ax":0.65802, "ay":-0.00033, "alpha":-10.86868, "fx":[15.63332,50.47089,5.93001,-30.24771], "fy":[-38.85239,-1.48531,40.02212,0.29489]}, + {"t":1.48055, "x":7.36148, "y":3.17091, "heading":-0.78065, "vx":0.01527, "vy":-1.99904, "omega":-2.57975, "ax":0.5283, "ay":0.00558, "alpha":-9.78738, "fx":[11.13466,43.91154,6.51583,-28.01314], "fy":[-35.96629,-2.24859,36.07141,2.49792]}, + {"t":1.50262, "x":7.36194, "y":3.12679, "heading":-0.83759, "vx":0.02693, "vy":-1.99891, "omega":-2.79579, "ax":0.37144, "ay":0.00577, "alpha":-8.56132, "fx":[6.40935,36.87994,5.99942,-25.70144], "fy":[-31.64983,-3.54632,31.50143,4.0611]}, + {"t":1.52469, "x":7.36263, "y":3.08267, "heading":-0.89931, "vx":0.03513, "vy":-1.99879, "omega":-2.98477, "ax":0.26983, "ay":0.00515, "alpha":-7.36018, "fx":[2.95861,30.76681,5.97714,-22.56784], "fy":[-27.21381,-4.56682,26.98444,5.12301]}, + {"t":1.54677, "x":7.36347, "y":3.03855, "heading":-0.96519, "vx":0.04109, "vy":-1.99867, "omega":-3.14723, "ax":0.21399, "ay":0.00465, "alpha":-6.17449, "fx":[0.74046,25.38144,6.25138,-18.78439], "fy":[-22.64784,-5.29958,22.5059,5.73701]}, + {"t":1.56884, "x":7.36443, "y":2.99443, "heading":-1.03466, "vx":0.04581, "vy":-1.99857, "omega":-3.28352, "ax":0.18347, "ay":0.00439, "alpha":-5.00694, "fx":[-0.53132,20.43307,6.44891,-14.69975], "fy":[-18.21254,-5.39278,18.03842,5.84584]}, + {"t":1.59091, "x":7.36549, "y":2.95032, "heading":-1.10714, "vx":0.04986, "vy":-1.99847, "omega":-3.39404, "ax":0.1649, "ay":0.00427, "alpha":-3.8473, "fx":[-1.03635,15.76919,6.30915,-10.57027], "fy":[-13.77986,-5.00517,13.6376,5.41829]}, + {"t":1.61299, "x":7.36663, "y":2.90621, "heading":-1.18206, "vx":0.0535, "vy":-1.99838, "omega":-3.47897, "ax":0.15173, "ay":0.00419, "alpha":-2.69998, "fx":[-0.86821,11.36187,5.69763,-6.55597], "fy":[-9.46538,-4.09592,9.3687,4.45867]}, + {"t":1.63506, "x":7.36784, "y":2.8621, "heading":-1.25885, "vx":0.05685, "vy":-1.99829, "omega":-3.53856, "ax":0.14179, "ay":0.00415, "alpha":-1.56348, "fx":[-0.06127,7.24832,4.5657,-2.7487], "fy":[-5.32747,-2.66114,5.28588,2.96598]}, + {"t":1.65713, "x":7.36913, "y":2.81799, "heading":-1.33696, "vx":0.05998, "vy":-1.9982, "omega":-3.57307, "ax":0.1348, "ay":0.00415, "alpha":-0.43517, "fx":[1.37095,3.48742,2.90912,0.79244], "fy":[-1.4188,-0.71594,1.44614,0.95193]}, + {"t":1.67921, "x":7.37049, "y":2.77388, "heading":-1.41583, "vx":0.06295, "vy":-1.9981, "omega":-3.58268, "ax":0.13106, "ay":0.00422, "alpha":0.68915, "fx":[3.41683,0.13883,0.74485,4.02232], "fy":[2.20803,1.7245,-2.09904,-1.56524]}, + {"t":1.70128, "x":7.37191, "y":2.72978, "heading":-1.49491, "vx":0.06585, "vy":-1.99801, "omega":-3.56747, "ax":0.13087, "ay":0.00441, "alpha":1.81446, "fx":[6.05653,-2.745,-1.90035,6.89975], "fy":[5.50126,4.64055,-5.30281,-4.5591]}, + {"t":1.72335, "x":7.3734, "y":2.68568, "heading":-1.57366, "vx":0.06874, "vy":-1.99791, "omega":-3.52742, "ax":0.13423, "ay":0.00472, "alpha":2.9455, "fx":[9.25747,-5.12245,-4.9956,9.38483], "fy":[8.41487,7.99847,-8.12148,-7.99228]}, + {"t":1.74543, "x":7.37495, "y":2.64158, "heading":-1.65152, "vx":0.0717, "vy":-1.99781, "omega":-3.4624, "ax":0.14088, "ay":0.00517, "alpha":4.08591, "fx":[12.97262,-6.96089,-8.50542,11.43987], "fy":[10.9076,11.7528,-10.51561,-11.81678]}, + {"t":1.7675, "x":7.37656, "y":2.59748, "heading":-1.72795, "vx":0.07481, "vy":-1.9977, "omega":-3.37221, "ax":0.15055, "ay":0.00576, "alpha":5.23855, "fx":[17.14331,-8.23314,-12.38636,13.03673], "fy":[12.94316,15.85531,-12.45477,-15.97775]}, + {"t":1.78957, "x":7.37825, "y":2.55338, "heading":-1.80238, "vx":0.07813, "vy":-1.99757, "omega":-3.25658, "ax":0.16361, "ay":0.00655, "alpha":6.40527, "fx":[21.70608,-8.90766,-16.57697,14.16849], "fy":[14.48949,20.2619,-13.9202,-20.41544]}, + {"t":1.81165, "x":7.38002, "y":2.50929, "heading":-1.87427, "vx":0.08174, "vy":-1.99742, "omega":-3.11519, "ax":0.1822, "ay":0.00764, "alpha":7.58666, "fx":[26.61193,-8.93287,-20.98137,14.87272], "fy":[15.5262,24.92777,-14.90367,-25.06519]}, + {"t":1.83372, "x":7.38187, "y":2.4652, "heading":-1.94303, "vx":0.08577, "vy":-1.99726, "omega":-2.94773, "ax":0.21207, "ay":0.00935, "alpha":8.78107, "fx":[31.86103,-8.21351,-25.44545,15.26515], "fy":[16.05714,29.79183,-15.40065,-29.85432]}, + {"t":1.85579, "x":7.38381, "y":2.42112, "heading":-2.00809, "vx":0.09045, "vy":-1.99705, "omega":-2.7539, "ax":0.26462, "ay":0.01237, "alpha":9.98704, "fx":[37.55176,-6.58638,-29.73471,15.57325], "fy":[16.11778,34.78976,-15.41166,-34.71036]}, + {"t":1.87787, "x":7.38587, "y":2.37704, "heading":-2.06888, "vx":0.09629, "vy":-1.99678, "omega":-2.53345, "ax":0.35672, "ay":0.0179, "alpha":11.18317, "fx":[43.88894,-3.82703,-33.54895,16.14001], "fy":[15.84766,39.60853,-14.80284,-39.51642]}, + {"t":1.89994, "x":7.38808, "y":2.33297, "heading":-2.1248, "vx":0.10416, "vy":-1.99638, "omega":-2.2866, "ax":0.49189, "ay":0.027, "alpha":12.42326, "fx":[50.99229,-0.00628,-36.78392,17.03415], "fy":[15.21865,44.71251,-13.84022,-44.37619]}, + {"t":1.92202, "x":7.3905, "y":2.28891, "heading":-2.17528, "vx":0.11502, "vy":-1.99578, "omega":-2.01238, "ax":0.59063, "ay":0.03597, "alpha":13.6953, "fx":[57.85377,3.27538,-40.44625,16.82362], "fy":[14.40486,49.71435,-12.47042,-49.36462]}, + {"t":1.94409, "x":7.39319, "y":2.24487, "heading":-2.2197, "vx":0.12806, "vy":-1.99499, "omega":-1.71007, "ax":0.18677, "ay":0.01213, "alpha":14.82544, "fx":[57.28969,-2.49064,-49.85335,6.91457], "fy":[13.24872,53.46862,-11.14574,-54.8014]}, + {"t":1.96616, "x":7.39606, "y":2.20083, "heading":-2.25744, "vx":0.13218, "vy":-1.99472, "omega":-1.38283, "ax":-5.34249, "ay":-0.18858, "alpha":8.13664, "fx":[-65.17653,-87.20565,-102.87819,-84.00356], "fy":[6.39835,35.69549,-9.74482,-44.32434]}, + {"t":1.98824, "x":7.39767, "y":2.15676, "heading":-2.28797, "vx":0.01425, "vy":-1.99889, "omega":-1.20322, "ax":-7.95768, "ay":0.44683, "alpha":1.56815, "fx":[-125.76373,-125.61346,-127.24623,-126.71289], "fy":[9.19653,17.55435,5.172,-3.54815]}, + {"t":2.01031, "x":7.39605, "y":2.11274, "heading":-2.31453, "vx":-0.1614, "vy":-1.98902, "omega":-1.16861, "ax":-7.88306, "ay":1.30844, "alpha":1.65257, "fx":[-124.20049,-123.32071,-126.39707,-126.67941], "fy":[23.81179,31.31679,18.12154,9.83981]}, + {"t":2.03238, "x":7.39057, "y":2.06916, "heading":-2.34032, "vx":-0.33541, "vy":-1.96014, "omega":-1.13213, "ax":-7.81411, "ay":1.64856, "alpha":1.57144, "fx":[-122.98219,-121.91915,-125.37031,-125.94754], "fy":[29.1686,36.05057,23.55429,15.91468]}, + {"t":2.05542, "x":7.38077, "y":2.02444, "heading":-2.3664, "vx":-0.51541, "vy":-1.92217, "omega":-1.09593, "ax":-7.66498, "ay":2.26188, "alpha":1.39728, "fx":[-120.38356,-119.21143,-123.11495,-124.03894], "fy":[38.93665,44.32431,33.27433,27.10091]}, + {"t":2.07845, "x":7.36686, "y":1.98076, "heading":-2.39165, "vx":-0.69198, "vy":-1.87006, "omega":-1.06374, "ax":-7.40636, "ay":2.99761, "alpha":1.24923, "fx":[-115.97342,-114.7825,-119.229,-120.34043], "fy":[50.6092,54.66456,44.90289,40.18058]}, + {"t":2.10149, "x":7.34896, "y":1.93848, "heading":-2.41615, "vx":-0.86258, "vy":-1.80101, "omega":-1.03497, "ax":-7.06026, "ay":3.73538, "alpha":1.0905, "fx":[-110.20166,-109.19487,-113.9311,-115.01944], "fy":[62.18825,64.98274,56.71317,53.32357]}, + {"t":2.12452, "x":7.32721, "y":1.89798, "heading":-2.43999, "vx":-1.02522, "vy":-1.71496, "omega":-1.00985, "ax":-6.66749, "ay":4.39173, "alpha":0.92574, "fx":[-103.85563,-103.08096,-107.78532,-108.68333], "fy":[72.26289,74.14821,67.38455,65.09194]}, + {"t":2.14756, "x":7.30183, "y":1.85964, "heading":-2.46325, "vx":-1.17881, "vy":-1.6138, "omega":-0.98852, "ax":-6.22588, "ay":4.98917, "alpha":0.75418, "fx":[-96.8683,-96.39467,-100.72906,-101.36939], "fy":[81.31831,82.45941,77.24812,75.80096]}, + {"t":2.17059, "x":7.27302, "y":1.82379, "heading":-2.48603, "vx":-1.32223, "vy":-1.49887, "omega":-0.97115, "ax":-5.73215, "ay":5.53581, "alpha":0.57605, "fx":[-89.24731,-89.01081,-92.70388,-93.04614], "fy":[89.46408,90.11435,86.3786,85.58316]}, + {"t":2.19363, "x":7.24104, "y":1.79074, "heading":-2.5084, "vx":-1.45427, "vy":-1.37135, "omega":-0.95788, "ax":-5.18737, "ay":6.02558, "alpha":0.39565, "fx":[-80.9664,-80.93872,-83.69381,-83.81411], "fy":[96.70602,97.00189,94.659,94.27479]}, + {"t":2.21667, "x":7.20617, "y":1.76074, "heading":-2.53046, "vx":-1.57376, "vy":-1.23255, "omega":-0.94877, "ax":-4.59056, "ay":6.44123, "alpha":0.20295, "fx":[-72.08291,-72.14045,-73.66038,-73.63042], "fy":[102.75327,102.85718,101.77549,101.65129]}, + {"t":2.2397, "x":7.1687, "y":1.73406, "heading":-2.55232, "vx":-1.67951, "vy":-1.08417, "omega":-0.94409, "ax":-3.92768, "ay":6.72525, "alpha":-0.02671, "fx":[-62.49672,-62.4296,-62.23438,-62.25841], "fy":[106.69014,106.719,106.83365,106.83023]}, + {"t":2.26274, "x":7.12897, "y":1.71087, "heading":-2.57406, "vx":-1.76998, "vy":-0.92925, "omega":-0.94471, "ax":-3.1485, "ay":6.66879, "alpha":-0.55342, "fx":[-52.36321,-52.07867,-47.39094,-48.10601], "fy":[104.98123,104.56546,106.86748,107.07333]}, + {"t":2.28577, "x":7.08736, "y":1.69123, "heading":-2.59583, "vx":-1.84251, "vy":-0.77563, "omega":-0.95745, "ax":-1.9412, "ay":5.04369, "alpha":-2.56762, "fx":[-41.034,-36.2467,-20.127,-25.86429], "fy":[79.64767,72.50117,80.65437,87.48598]}, + {"t":2.30881, "x":7.0444, "y":1.67471, "heading":-2.61788, "vx":-1.88723, "vy":-0.65945, "omega":-1.0166, "ax":-0.85167, "ay":2.56441, "alpha":-5.07844, "fx":[-32.72178,-22.35625,6.54381,-5.54906], "fy":[43.01513,25.17059,37.88813,56.77399]}, + {"t":2.33184, "x":7.0007, "y":1.6602, "heading":-2.6413, "vx":-1.90685, "vy":-0.60038, "omega":-1.13358, "ax":-0.37609, "ay":1.22581, "alpha":-7.36077, "fx":[-32.35512,-18.12193,21.97182,4.6223], "fy":[25.56965,-5.18912,14.48401,42.9781]}, + {"t":2.35488, "x":6.95668, "y":1.64669, "heading":-2.66741, "vx":-1.91551, "vy":-0.57214, "omega":-1.30314, "ax":-0.14943, "ay":0.50629, "alpha":-9.26351, "fx":[-36.4154,-19.0754,33.37072,12.63057], "fy":[15.2079,-20.02626,-0.40255,37.37174]}, + {"t":2.37791, "x":6.91251, "y":1.63365, "heading":-2.69743, "vx":-1.91895, "vy":-0.56048, "omega":-1.51653, "ax":-0.20066, "ay":0.69751, "alpha":-11.21637, "fx":[-42.42591,-23.99805,39.19957,14.48198], "fy":[22.67079,-25.47475,1.07274,46.02494]}, + {"t":2.40095, "x":6.86825, "y":1.62092, "heading":-2.73236, "vx":-1.92357, "vy":-0.54441, "omega":-1.77491, "ax":-0.39574, "ay":1.44618, "alpha":-12.49266, "fx":[-50.42575,-33.05579,42.46573,15.88486], "fy":[35.37168,-13.91455,10.14559,60.23388]}, + {"t":2.42399, "x":6.82384, "y":1.60876, "heading":-2.77325, "vx":-1.93269, "vy":-0.5111, "omega":-2.06268, "ax":-0.77255, "ay":3.16096, "alpha":-12.82064, "fx":[-55.65933,-44.48852,39.91744,11.17116], "fy":[63.53417,12.19743,39.36949,85.62933]}, + {"t":2.44702, "x":6.77911, "y":1.59783, "heading":-2.82077, "vx":-1.95049, "vy":-0.43828, "omega":-2.35801, "ax":-1.09405, "ay":5.7806, "alpha":-9.50519, "fx":[-50.48759,-53.25126,29.63592,4.62758], "fy":[96.66175,70.23622,90.11376,110.07314]}, + {"t":2.47006, "x":6.73389, "y":1.58927, "heading":-2.87508, "vx":-1.97569, "vy":-0.30512, "omega":-2.57697, "ax":-0.8197, "ay":7.42003, "alpha":-4.48643, "fx":[-32.20122,-33.4919,12.42304,1.21692], "fy":[117.32914,113.38494,118.39901,122.08061]}, + {"t":2.49309, "x":6.68816, "y":1.58421, "heading":-2.93444, "vx":-1.99457, "vy":-0.1342, "omega":-2.68031, "ax":-0.17076, "ay":7.81829, "alpha":-2.96132, "fx":[-16.96838,-15.44721,14.24218,7.32964], "fy":[124.45798,122.86566,123.48438,125.67613]}, + {"t":2.51613, "x":6.64217, "y":1.58319, "heading":-2.99619, "vx":-1.9985, "vy":0.0459, "omega":-2.74853, "ax":0.54647, "ay":7.901, "alpha":-2.71178, "fx":[-4.94943,-2.54572,24.27843,17.91933], "fy":[126.81589,125.46094,123.55186,125.90805]}, + {"t":2.53916, "x":6.59628, "y":1.58634, "heading":-3.0595, "vx":-1.98592, "vy":0.2279, "omega":-2.811, "ax":1.31292, "ay":7.84595, "alpha":-2.72539, "fx":[6.92411,10.09533,36.64696,29.70806], "fy":[127.24176,125.66525,121.11721,124.21647]}, + {"t":2.5622, "x":6.55088, "y":1.59367, "heading":-3.12425, "vx":-1.95567, "vy":0.40864, "omega":-2.87378, "ax":1.95991, "ay":7.6732, "alpha":-2.84305, "fx":[16.80085,20.24994,47.38164,40.02771], "fy":[125.84517,123.80802,116.77013,120.84727]}, + {"t":2.58735, "x":6.50232, "y":1.60638, "heading":3.08666, "vx":-1.90638, "vy":0.60161, "omega":-2.94528, "ax":2.75467, "ay":7.42726, "alpha":-2.53501, "fx":[30.99991,34.76392,57.83346,51.33224], "fy":[122.92521,120.50606,111.91659,116.30511]}, + {"t":2.6125, "x":6.45525, "y":1.62386, "heading":3.01259, "vx":-1.8371, "vy":0.7884, "omega":-3.00903, "ax":3.42187, "ay":7.00787, "alpha":-2.72541, "fx":[41.333,45.18246,68.89492,61.8884], "fy":[117.97891,114.70728,103.15939,109.17479]}, + {"t":2.63765, "x":6.41013, "y":1.6459, "heading":2.93691, "vx":-1.75105, "vy":0.96464, "omega":-3.07757, "ax":3.79257, "ay":6.19817, "alpha":-4.54043, "fx":[41.80552,44.50139,82.47687,72.05531], "fy":[110.94383,105.0592,82.04376,95.55545]}, + {"t":2.66279, "x":6.36729, "y":1.67212, "heading":2.85951, "vx":-1.65567, "vy":1.12052, "omega":-3.19176, "ax":2.93072, "ay":4.05039, "alpha":-9.08499, "fx":[23.88975,13.84075,76.32767,72.0512], "fy":[92.526,60.15126,34.64623,69.88799]}, + {"t":2.68794, "x":6.32658, "y":1.70158, "heading":2.77924, "vx":-1.58196, "vy":1.22238, "omega":-3.42024, "ax":1.42611, "ay":1.79178, "alpha":-9.73459, "fx":[4.9599,-12.51628,44.45251,53.66613], "fy":[59.47681,19.25043,-3.96233,39.01819]}, + {"t":2.71309, "x":6.28724, "y":1.73289, "heading":2.69323, "vx":-1.5461, "vy":1.26745, "omega":-3.66506, "ax":0.42217, "ay":0.51076, "alpha":-7.7856, "fx":[-5.06447,-21.52342,19.33107,34.06564], "fy":[33.94107,2.58341,-18.68355,14.59398]}, + {"t":2.73824, "x":6.24849, "y":1.76493, "heading":2.60105, "vx":-1.53548, "vy":1.28029, "omega":-3.86086, "ax":0.20534, "ay":0.24519, "alpha":-4.61806, "fx":[-1.90626,-13.75391,9.23748,19.46228], "fy":[19.89161,1.26171,-12.61335,7.03042]}, + {"t":2.76339, "x":6.20994, "y":1.7972, "heading":2.50396, "vx":-1.53031, "vy":1.28646, "omega":-3.977, "ax":-0.11247, "ay":-0.13412, "alpha":-1.47698, "fx":[-2.9384,-7.27027,-0.64168,3.7082], "fy":[2.94714,-1.93337,-7.63115,-1.89934]}, + {"t":2.78854, "x":6.17142, "y":1.82951, "heading":2.40394, "vx":-1.53314, "vy":1.28309, "omega":-4.01414, "ax":0.0107, "ay":0.01284, "alpha":1.86991, "fx":[1.11884,7.17644,-0.83262,-6.78309], "fy":[-6.3904,-0.01689,7.07668,0.14603]}, + {"t":2.81369, "x":6.13287, "y":1.86178, "heading":2.30299, "vx":-1.53287, "vy":1.28341, "omega":-3.96712, "ax":-0.10205, "ay":-0.12221, "alpha":5.32019, "fx":[-0.98789,17.92909,-2.19157,-21.23039], "fy":[-21.7327,-3.43171,17.38223,0.02125]}, + {"t":2.83884, "x":6.09428, "y":1.89402, "heading":2.20322, "vx":-1.53544, "vy":1.28033, "omega":-3.83332, "ax":0.04463, "ay":0.05353, "alpha":8.39936, "fx":[-1.64598,31.31115,2.56682,-29.398], "fy":[-29.69345,-5.87525,31.90854,7.05943]}, + {"t":2.86399, "x":6.05568, "y":1.92624, "heading":2.10681, "vx":-1.53432, "vy":1.28168, "omega":-3.62208, "ax":-0.04523, "ay":-0.05423, "alpha":11.74712, "fx":[-7.74889,40.89654,6.29563,-42.31566], "fy":[-43.78193,-12.75314,41.61807,11.47341]}, + {"t":2.88914, "x":6.01708, "y":1.95846, "heading":2.01572, "vx":-1.53546, "vy":1.28032, "omega":-3.32665, "ax":0.06142, "ay":0.07359, "alpha":14.41518, "fx":[-12.80631,50.43893,14.02534,-47.75737], "fy":[-49.97913,-19.45891,52.7244,21.38697]}, + {"t":2.91429, "x":5.97849, "y":1.99068, "heading":1.93206, "vx":-1.53391, "vy":1.28217, "omega":-2.96412, "ax":-0.02675, "ay":-0.03202, "alpha":17.26531, "fx":[-21.41446,56.70169,20.57324,-57.55909], "fy":[-60.7086,-28.39776,59.57194,27.50092]}, + {"t":2.93944, "x":5.9399, "y":2.02291, "heading":1.85751, "vx":-1.53458, "vy":1.28136, "omega":-2.52991, "ax":0.06235, "ay":0.07461, "alpha":19.43687, "fx":[-28.35009,62.51721,29.38597,-59.59339], "fy":[-64.29833,-36.39233,66.7136,38.71502]}, + {"t":2.96458, "x":5.90133, "y":2.05516, "heading":1.79389, "vx":-1.53302, "vy":1.28324, "omega":-2.04109, "ax":-0.07152, "ay":-0.08559, "alpha":21.52285, "fx":[-37.05408,63.80962,35.87398,-67.17121], "fy":[-71.22591,-46.68784,69.84548,42.63323]}, + {"t":2.98973, "x":5.86275, "y":2.08741, "heading":1.74255, "vx":-1.53481, "vy":1.28109, "omega":-1.49981, "ax":0.00076, "ay":-0.01319, "alpha":22.5666, "fx":[-42.14228,62.81678,45.44095,-66.06698], "fy":[-72.50566,-50.7509,71.21268,51.2064]}, + {"t":3.01488, "x":5.82415, "y":2.11962, "heading":1.70484, "vx":-1.5348, "vy":1.28075, "omega":-0.93228, "ax":5.84965, "ay":-4.81492, "alpha":4.37673, "fx":[73.69355,95.36529,110.40353,92.00724], "fy":[-98.31142,-80.15508,-52.52344,-74.77154]}, + {"t":3.04003, "x":5.7874, "y":2.15031, "heading":1.68139, "vx":-1.38768, "vy":1.15966, "omega":-0.82221, "ax":6.11175, "ay":-5.11133, "alpha":2.86924, "fx":[83.83248,97.84025,109.32177,97.11961], "fy":[-96.08221,-83.02806,-65.24595,-80.22818]}, + {"t":3.06518, "x":5.75444, "y":2.17786, "heading":1.66071, "vx":-1.23398, "vy":1.03112, "omega":-0.75005, "ax":6.12588, "ay":-5.12013, "alpha":3.03561, "fx":[83.20492,97.82515,110.19327,97.7882], "fy":[-97.07422,-83.59732,-64.53094,-79.94069]}, + {"t":3.09033, "x":5.72534, "y":2.20217, "heading":1.64185, "vx":-1.07992, "vy":0.90235, "omega":-0.67371, "ax":6.13034, "ay":-5.12327, "alpha":3.24667, "fx":[82.20585,97.71353,111.06523,98.31001], "fy":[-98.19365,-84.09689,-63.51316,-79.53895]}, + {"t":3.11548, "x":5.70012, "y":2.22324, "heading":1.62491, "vx":-0.92574, "vy":0.7735, "omega":-0.59206, "ax":6.13272, "ay":-5.12463, "alpha":3.45407, "fx":[81.20732,97.55864,111.88397,98.79583], "fy":[-99.24097,-84.59022,-62.48649,-79.11138]}, + {"t":3.14063, "x":5.67878, "y":2.24107, "heading":1.61002, "vx":-0.77151, "vy":0.64462, "omega":-0.50519, "ax":6.13406, "ay":-5.12554, "alpha":3.65743, "fx":[80.18781,97.41163,112.64864,99.28292], "fy":[-100.27326,-85.04668,-61.5013,-78.66543]}, + {"t":3.16578, "x":5.66131, "y":2.25567, "heading":1.59731, "vx":-0.61724, "vy":0.51572, "omega":-0.41321, "ax":6.13497, "ay":-5.12612, "alpha":3.84756, "fx":[79.23843,97.2577,113.35795,99.73468], "fy":[-101.21572,-85.48896,-60.56958,-78.2494]}, + {"t":3.19093, "x":5.64773, "y":2.26701, "heading":1.58692, "vx":-0.46295, "vy":0.3868, "omega":-0.31644, "ax":6.13564, "ay":-5.12651, "alpha":4.02961, "fx":[78.30125,97.13533,114.01894,100.17565], "fy":[-102.13109,-85.87933,-59.69031,-77.84779]}, + {"t":3.21608, "x":5.63803, "y":2.27512, "heading":1.57896, "vx":-0.30865, "vy":0.25788, "omega":-0.2151, "ax":6.13613, "ay":-5.12682, "alpha":4.19762, "fx":[77.44337,97.02462,114.63609,100.5582], "fy":[-102.96115,-86.24057,-58.85489,-77.51131]}, + {"t":3.24123, "x":5.63221, "y":2.27999, "heading":1.57355, "vx":-0.15433, "vy":0.12894, "omega":-0.10954, "ax":6.13651, "ay":-5.12705, "alpha":4.35547, "fx":[76.61701,96.95669,115.21251,100.90046], "fy":[-103.75352,-86.54036,-58.06257,-77.22627]}, + {"t":3.26637, "x":5.63027, "y":2.28161, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[ + {"name":"IntakeDown", "from":{"target":0, "targetTimestamp":0.0, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeDown"}}}, + {"name":"IntakeUp", "from":{"target":4, "targetTimestamp":2.5622, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeUp"}}}] +} diff --git a/src/main/deploy/choreo/RightSecondDipShort.traj b/src/main/deploy/choreo/RightSecondDipShort.traj new file mode 100644 index 0000000..2154cb2 --- /dev/null +++ b/src/main/deploy/choreo/RightSecondDipShort.traj @@ -0,0 +1,207 @@ +{ + "name":"RightSecondDipShort", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":5.607864856719971, "y":2.2545297145843506, "heading":1.5707963267948966, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.415774345397949, "y":3.393817186355591, "heading":2.7446892183710663, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.060224533081055, "y":3.563260793685913, "heading":3.141592653589793, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.796180248260498, "y":2.8797600269317627, "heading":2.2617395073648874, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.6100335121154785, "y":2.2714898586273193, "heading":1.5707963267948966, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":0, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":4.0}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxAngularVelocity", "props":{"max":4.0}}, "enabled":true}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"first", "to":1, "data":{"type":"MaxVelocity", "props":{"max":3.0}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":2, "to":4, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"5.607864856719971 m", "val":5.607864856719971}, "y":{"exp":"2.2545297145843506 m", "val":2.2545297145843506}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.415774345397949 m", "val":7.415774345397949}, "y":{"exp":"3.393817186355591 m", "val":3.393817186355591}, "heading":{"exp":"2.7446892183710663 rad", "val":2.7446892183710663}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.060224533081055 m", "val":6.060224533081055}, "y":{"exp":"3.563260793685913 m", "val":3.563260793685913}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.796180248260498 m", "val":5.796180248260498}, "y":{"exp":"2.8797600269317627 m", "val":2.8797600269317627}, "heading":{"exp":"2.2617395073648874 rad", "val":2.2617395073648874}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.6100335121154785 m", "val":5.6100335121154785}, "y":{"exp":"2.2714898586273193 m", "val":2.2714898586273193}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":0, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"4 m / s ^ 2", "val":4.0}}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"4 rad / s", "val":4.0}}}, "enabled":true}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"first", "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3 m / s", "val":3.0}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, + {"from":2, "to":4, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.0994,2.56815,3.12829,3.44732], + "samples":[ + {"t":0.0, "x":5.60786, "y":2.25453, "heading":1.5708, "vx":2.68018, "vy":1.33827, "omega":0.9643, "ax":0.0652, "ay":0.12419, "alpha":0.50595, "fx":[-0.19131,2.27287,2.25961,-0.20106], "fy":[0.54036,0.60274,3.36542,3.37797]}, + {"t":0.02819, "x":5.68344, "y":2.2923, "heading":1.59798, "vx":2.68202, "vy":1.34177, "omega":0.97857, "ax":0.00478, "ay":-0.00946, "alpha":0.4392, "fx":[-0.96953,1.18933,1.12086,-1.03715], "fy":[-1.44042,-1.26958,1.08349,1.02553]}, + {"t":0.05638, "x":5.75905, "y":2.33013, "heading":1.62557, "vx":2.68215, "vy":1.34151, "omega":0.99095, "ax":0.01789, "ay":-0.03578, "alpha":0.38497, "fx":[-0.59872,1.2895,1.17037,-0.72498], "fy":[-1.62593,-1.62106,0.53954,0.43546]}, + {"t":0.08457, "x":5.83467, "y":2.36793, "heading":1.6535, "vx":2.68265, "vy":1.3405, "omega":1.0018, "ax":0.02171, "ay":-0.0435, "alpha":0.34149, "fx":[-0.41998,1.27,1.1091,-0.58031], "fy":[-1.76128,-1.4682,0.30377,0.16315]}, + {"t":0.11276, "x":5.9103, "y":2.4057, "heading":1.68174, "vx":2.68327, "vy":1.33927, "omega":1.01143, "ax":0.00685, "ay":-0.01375, "alpha":0.30503, "fx":[-0.56993,0.95928,0.76355,-0.71806], "fy":[-1.04671,-1.05106,0.69807,0.52642]}, + {"t":0.14095, "x":5.98594, "y":2.44345, "heading":1.71025, "vx":2.68346, "vy":1.33888, "omega":1.02003, "ax":-0.0072, "ay":0.0145, "alpha":0.28833, "fx":[-0.71605,0.71826,0.48719,-0.9463], "fy":[-0.73445,-0.33255,1.09464,0.89285]}, + {"t":0.16914, "x":6.06159, "y":2.4812, "heading":1.73901, "vx":2.68326, "vy":1.33929, "omega":1.02815, "ax":-0.0659, "ay":0.13165, "alpha":0.27611, "fx":[-1.5991,-0.23865,-0.50354,-1.84336], "fy":[1.28862,1.39896,2.953,2.71972]}, + {"t":0.19733, "x":6.1372, "y":2.519, "heading":1.76799, "vx":2.6814, "vy":1.343, "omega":1.03594, "ax":-0.15033, "ay":0.2992, "alpha":0.28073, "fx":[-2.90794,-1.5575,-1.86573,-3.21504], "fy":[3.84532,4.14346,5.64084,5.37031]}, + {"t":0.22552, "x":6.21273, "y":2.55698, "heading":1.79719, "vx":2.67716, "vy":1.35144, "omega":1.04385, "ax":-0.33034, "ay":0.64874, "alpha":0.29108, "fx":[-5.7644,-4.41306,-4.76565,-6.03456], "fy":[9.23695,9.73662,11.26951,10.95375]}, + {"t":0.25371, "x":6.28807, "y":2.59534, "heading":1.82662, "vx":2.66785, "vy":1.36972, "omega":1.05206, "ax":-0.64174, "ay":1.2301, "alpha":0.31783, "fx":[-10.68767,-9.28279,-9.68739,-11.09439], "fy":[18.70603,18.48872,20.64728,20.27294]}, + {"t":0.2819, "x":6.36302, "y":2.63444, "heading":1.85628, "vx":2.64976, "vy":1.4044, "omega":1.06102, "ax":-1.10411, "ay":2.02889, "alpha":0.36254, "fx":[-18.11654,-16.63486,-17.08989,-18.27294], "fy":[30.32058,31.91768,33.52328,33.07923]}, + {"t":0.31009, "x":6.43728, "y":2.67483, "heading":1.88619, "vx":2.61863, "vy":1.4616, "omega":1.07124, "ax":-2.02003, "ay":2.76712, "alpha":0.35896, "fx":[-32.54689,-31.20563,-31.5516,-32.97379], "fy":[43.2925,42.11496,45.42707,44.8855]}, + {"t":0.33828, "x":6.51029, "y":2.71713, "heading":1.91638, "vx":2.56169, "vy":1.5396, "omega":1.08135, "ax":-3.69689, "ay":-1.41366, "alpha":0.22964, "fx":[-59.05927,-57.96567,-58.57572,-59.16262], "fy":[-23.13618,-23.2079,-21.46226,-21.96536]}, + {"t":0.36647, "x":6.58104, "y":2.75997, "heading":1.94687, "vx":2.45748, "vy":1.49975, "omega":1.08783, "ax":-3.64015, "ay":-1.60903, "alpha":0.27443, "fx":[-57.93961,-56.97369,-57.61035,-58.63666], "fy":[-26.73945,-25.88295,-24.56696,-24.98852]}, + {"t":0.39466, "x":6.64887, "y":2.80161, "heading":1.97753, "vx":2.35486, "vy":1.45439, "omega":1.09556, "ax":-3.61995, "ay":-1.67056, "alpha":0.2448, "fx":[-57.74224,-56.65593,-57.41717,-58.06228], "fy":[-27.29907,-27.24629,-25.48666,-26.05319]}, + {"t":0.42285, "x":6.71381, "y":2.84195, "heading":2.00842, "vx":2.25281, "vy":1.4073, "omega":1.10247, "ax":-3.60968, "ay":-1.70058, "alpha":0.27529, "fx":[-57.38323,-56.47321,-57.19941,-58.16956], "fy":[-28.16279,-27.37326,-25.98888,-26.46671]}, + {"t":0.45104, "x":6.77589, "y":2.88094, "heading":2.0395, "vx":2.15106, "vy":1.35936, "omega":1.11023, "ax":-3.60348, "ay":-1.71833, "alpha":0.24226, "fx":[-57.36687,-56.39111,-57.18566,-57.88792], "fy":[-28.08467,-27.89361,-26.27564,-26.86534]}, + {"t":0.47923, "x":6.83509, "y":2.91858, "heading":2.07079, "vx":2.04948, "vy":1.31092, "omega":1.11706, "ax":-3.5993, "ay":-1.73013, "alpha":0.24639, "fx":[-57.15206,-56.38933,-57.10191,-57.92284], "fy":[-28.52123,-27.76451,-26.54315,-27.03922]}, + {"t":0.50742, "x":6.89144, "y":2.95485, "heading":2.10228, "vx":1.94801, "vy":1.26215, "omega":1.124, "ax":-3.5963, "ay":-1.73851, "alpha":0.19545, "fx":[-57.20263,-56.40388,-57.13233,-57.63673], "fy":[-28.23676,-28.07634,-26.74405,-27.34362]}, + {"t":0.53561, "x":6.94492, "y":2.98974, "heading":2.13397, "vx":1.84663, "vy":1.21314, "omega":1.12951, "ax":-3.59404, "ay":-1.74478, "alpha":0.1924, "fx":[-57.01664,-56.47419,-57.06693,-57.67447], "fy":[-28.5633,-27.84348,-26.9717,-27.42029]}, + {"t":0.5638, "x":6.99555, "y":3.02324, "heading":2.16581, "vx":1.74532, "vy":1.16395, "omega":1.13493, "ax":-3.59228, "ay":-1.74966, "alpha":0.1442, "fx":[-57.06181,-56.519,-57.08408,-57.45511], "fy":[-28.25973,-28.0867,-27.14901,-27.61305]}, + {"t":0.59199, "x":7.04332, "y":3.05536, "heading":2.1978, "vx":1.64405, "vy":1.11463, "omega":1.139, "ax":-3.59087, "ay":-1.75354, "alpha":0.12018, "fx":[-56.9469,-56.65179,-57.03851,-57.39336], "fy":[-28.4112,-27.85606,-27.38416,-27.70326]}, + {"t":0.62018, "x":7.08824, "y":3.08608, "heading":2.22991, "vx":1.54283, "vy":1.0652, "omega":1.14239, "ax":-3.58971, "ay":-1.75672, "alpha":0.04857, "fx":[-57.05901,-56.77089,-57.06966,-57.05728], "fy":[-27.99963,-28.0414,-27.58471,-27.9309]}, + {"t":0.64837, "x":7.13031, "y":3.11541, "heading":2.26211, "vx":1.44163, "vy":1.01568, "omega":1.14376, "ax":-3.58874, "ay":-1.75936, "alpha":0.02818, "fx":[-56.93532,-56.91189,-56.98496,-57.06329], "fy":[-28.12537,-27.82583,-27.84082,-27.93243]}, + {"t":0.67656, "x":7.16952, "y":3.14335, "heading":2.29436, "vx":1.34047, "vy":0.96608, "omega":1.14455, "ax":-3.58792, "ay":-1.76159, "alpha":-0.05196, "fx":[-57.06574,-57.06598,-56.98689,-56.72493], "fy":[-27.67242,-27.98421,-28.08111,-28.12837]}, + {"t":0.70475, "x":7.20588, "y":3.16988, "heading":2.32662, "vx":1.23932, "vy":0.91642, "omega":1.14309, "ax":-3.58722, "ay":-1.76351, "alpha":-0.08615, "fx":[-56.98679,-57.25518,-56.88455,-56.67225], "fy":[-27.7229,-27.77003,-28.37539,-28.11957]}, + {"t":0.73294, "x":7.23939, "y":3.19501, "heading":2.35884, "vx":1.1382, "vy":0.86671, "omega":1.14066, "ax":-3.58661, "ay":-1.76516, "alpha":-0.17567, "fx":[-57.1444,-57.44467,-56.85605,-56.31494], "fy":[-27.23032,-27.93299,-28.65346,-28.27637]}, + {"t":0.76113, "x":7.27005, "y":3.21874, "heading":2.391, "vx":1.03709, "vy":0.81695, "omega":1.13571, "ax":-3.58607, "ay":-1.76661, "alpha":-0.22489, "fx":[-57.11736,-57.67295,-56.72236,-56.21348], "fy":[-27.1872,-27.75551,-28.99652,-28.24592]}, + {"t":0.78932, "x":7.29786, "y":3.24107, "heading":2.42302, "vx":0.936, "vy":0.76715, "omega":1.12937, "ax":-3.5856, "ay":-1.76789, "alpha":-0.32528, "fx":[-57.30392,-57.89261,-56.63806,-55.86151], "fy":[-26.64456,-27.92985,-29.34303,-28.34906]}, + {"t":0.81751, "x":7.32283, "y":3.26199, "heading":2.45485, "vx":0.83493, "vy":0.71731, "omega":1.1202, "ax":-3.58518, "ay":-1.76903, "alpha":-0.39454, "fx":[-57.34276,-58.18065,-56.47518,-55.6709], "fy":[-26.51299,-27.77995,-29.73742,-28.30821]}, + {"t":0.8457, "x":7.34494, "y":3.28151, "heading":2.48643, "vx":0.73386, "vy":0.66744, "omega":1.10907, "ax":-3.5848, "ay":-1.77005, "alpha":-0.51296, "fx":[-57.58134,-58.4476,-56.33782,-55.27882], "fy":[-25.89564,-27.9789,-30.15753,-28.3711]}, + {"t":0.87389, "x":7.3642, "y":3.29962, "heading":2.51769, "vx":0.63281, "vy":0.61755, "omega":1.09461, "ax":-3.58447, "ay":-1.77096, "alpha":-0.6029, "fx":[-57.68823,-58.79096,-56.1181,-55.02678], "fy":[-25.67666,-27.8682,-30.62817,-28.28825]}, + {"t":0.90208, "x":7.38061, "y":3.31633, "heading":2.54855, "vx":0.53176, "vy":0.56762, "omega":1.07762, "ax":-3.58416, "ay":-1.77179, "alpha":-0.74424, "fx":[-57.99647,-59.11589,-55.91613,-54.57609], "fy":[-24.9735,-28.10877,-31.13489,-28.29668]}, + {"t":0.93027, "x":7.39418, "y":3.33163, "heading":2.57893, "vx":0.43072, "vy":0.51768, "omega":1.05664, "ax":-3.58388, "ay":-1.77254, "alpha":-0.86059, "fx":[-58.18671,-59.52263,-55.61967,-54.25781], "fy":[-24.64506,-28.04943,-31.70662,-28.16057]}, + {"t":0.95845, "x":7.4049, "y":3.34552, "heading":2.60872, "vx":0.32969, "vy":0.46771, "omega":1.03238, "ax":-3.58362, "ay":-1.77323, "alpha":-1.02969, "fx":[-58.58044,-59.91341,-55.33485,-53.74196], "fy":[-23.84741,-28.35368,-32.31869,-28.08548]}, + {"t":0.98664, "x":7.41277, "y":3.358, "heading":2.63782, "vx":0.22867, "vy":0.41772, "omega":1.00335, "ax":-3.58339, "ay":-1.77386, "alpha":-1.18081, "fx":[-58.88338,-60.38303,-54.94825,-53.34118], "fy":[-23.35646,-28.42024,-32.99908,-27.86939]}, + {"t":1.01483, "x":7.41779, "y":3.36907, "heading":2.6661, "vx":0.12766, "vy":0.36772, "omega":0.97006, "ax":-3.58318, "ay":-1.77444, "alpha":-1.38112, "fx":[-59.36548,-60.87218,-54.55341,-52.75109], "fy":[-22.50548,-28.73247,-33.74856,-27.69545]}, + {"t":1.04302, "x":7.41997, "y":3.37873, "heading":2.69345, "vx":0.02665, "vy":0.3177, "omega":0.93113, "ax":-3.58298, "ay":-1.77497, "alpha":-1.58111, "fx":[-59.82022,-61.43091,-54.04666,-52.23175], "fy":[-21.79813,-28.91066,-34.59768,-27.4094]}, + {"t":1.07121, "x":7.41929, "y":3.38698, "heading":2.7197, "vx":-0.07436, "vy":0.26766, "omega":0.88656, "ax":-3.58279, "ay":-1.77547, "alpha":-1.81632, "fx":[-60.39561,-62.03113,-53.49967,-51.59145], "fy":[-20.89889,-29.2318,-35.51915,-27.09747]}, + {"t":1.0994, "x":7.41577, "y":3.39382, "heading":2.74469, "vx":-0.17535, "vy":0.21761, "omega":0.83536, "ax":-3.63903, "ay":-1.65507, "alpha":-1.71237, "fx":[-61.11151,-62.56008,-54.53642,-52.88096], "fy":[-19.43708,-27.48233,-33.21207,-24.97062]}, + {"t":1.13806, "x":7.40628, "y":3.40099, "heading":2.77698, "vx":-0.31601, "vy":0.15364, "omega":0.76917, "ax":-3.78502, "ay":-1.28902, "alpha":-1.42607, "fx":[-62.78287,-63.98054,-57.44109,-56.15563], "fy":[-14.77169,-21.78091,-26.3248,-18.9793]}, + {"t":1.17671, "x":7.39124, "y":3.40597, "heading":2.80671, "vx":-0.4623, "vy":0.10382, "omega":0.71405, "ax":-3.95702, "ay":-0.56989, "alpha":-1.18539, "fx":[-64.84671,-66.00659,-60.821,-59.60832], "fy":[-4.3492,-10.58214,-13.93344,-7.32509]}, + {"t":1.21536, "x":7.37041, "y":3.40955, "heading":2.83431, "vx":-0.61525, "vy":0.08179, "omega":0.66824, "ax":-3.99305, "ay":0.17012, "alpha":-0.98938, "fx":[-64.91368,-66.09307,-61.89393,-60.66944], "fy":[6.51559,0.94778,-1.21098,4.5507]}, + {"t":1.25401, "x":7.34365, "y":3.41284, "heading":2.86013, "vx":-0.76958, "vy":0.08837, "omega":0.63, "ax":-3.97921, "ay":0.33529, "alpha":-0.82838, "fx":[-64.45319,-65.44432,-61.90998,-60.88406], "fy":[8.43843,3.66703,2.15978,7.02692]}, + {"t":1.29266, "x":7.31093, "y":3.41651, "heading":2.88448, "vx":-0.92338, "vy":0.10133, "omega":0.59798, "ax":-1.84911, "ay":-0.0668, "alpha":-0.94212, "fx":[-31.23903,-32.36971,-27.48863,-26.32652], "fy":[1.8962,-2.51962,-4.15636,0.53753]}, + {"t":1.33131, "x":7.27386, "y":3.42037, "heading":2.9076, "vx":-0.99485, "vy":0.09874, "omega":0.56156, "ax":0.01307, "ay":0.1286, "alpha":-0.8514, "fx":[-1.61743,-2.59479,2.03475,3.00723], "fy":[4.57254,0.65658,-0.53643,3.47346]}, + {"t":1.36996, "x":7.23542, "y":3.42429, "heading":2.9293, "vx":-0.99435, "vy":0.10371, "omega":0.52866, "ax":0.00975, "ay":0.09186, "alpha":-0.69996, "fx":[-1.39329,-2.12379,1.7045,2.43161], "fy":[3.50636,0.27204,-0.62635,2.68158]}, + {"t":1.40861, "x":7.19699, "y":3.42836, "heading":2.94973, "vx":-0.99397, "vy":0.10726, "omega":0.5016, "ax":0.0073, "ay":0.06679, "alpha":-0.57762, "fx":[-1.19549,-1.74132,1.42805,1.97206], "fy":[2.72747,0.02908,-0.62579,2.11061]}, + {"t":1.44726, "x":7.15858, "y":3.43256, "heading":2.96912, "vx":-0.99369, "vy":0.10985, "omega":0.47928, "ax":0.006, "ay":0.05379, "alpha":-0.47889, "fx":[-1.01762,-1.42535,1.20869,1.6155], "fy":[2.21769,-0.04441,-0.5147,1.75716]}, + {"t":1.48592, "x":7.12018, "y":3.43685, "heading":2.98765, "vx":-0.99346, "vy":0.11192, "omega":0.46077, "ax":0.00515, "ay":0.04533, "alpha":-0.40026, "fx":[-0.86901,-1.17425,1.03269,1.33744], "fy":[1.84348,-0.06261,-0.40158,1.49921]}, + {"t":1.52457, "x":7.08178, "y":3.44121, "heading":3.00546, "vx":-0.99326, "vy":0.11368, "omega":0.4453, "ax":0.00453, "ay":0.03934, "alpha":-0.33786, "fx":[-0.7475,-0.97648,0.89156,1.12028], "fy":[1.55887,-0.05678,-0.30493,1.30114]}, + {"t":1.56322, "x":7.0434, "y":3.44563, "heading":3.02267, "vx":-0.99308, "vy":0.1152, "omega":0.43224, "ax":0.00408, "ay":0.03499, "alpha":-0.29011, "fx":[-0.65312,-0.82609,0.78283,0.95565], "fy":[1.3449,-0.04401,-0.22977,1.15072]}, + {"t":1.60187, "x":7.00502, "y":3.45011, "heading":3.03937, "vx":-0.99293, "vy":0.11655, "omega":0.42103, "ax":0.00374, "ay":0.03173, "alpha":-0.25434, "fx":[-0.58235,-0.71391,0.70128,0.83276], "fy":[1.18414,-0.03146,-0.17469,1.03692]}, + {"t":1.64052, "x":6.96664, "y":3.45464, "heading":3.05565, "vx":-0.99278, "vy":0.11778, "omega":0.41119, "ax":0.00349, "ay":0.02924, "alpha":-0.23006, "fx":[-0.53608,-0.63739,0.64679,0.74803], "fy":[1.06912,-0.02758,-0.14091,0.95616]}, + {"t":1.67917, "x":6.92827, "y":3.45921, "heading":3.07154, "vx":-0.99265, "vy":0.11891, "omega":0.4023, "ax":0.00329, "ay":0.02733, "alpha":-0.2155, "fx":[-0.51168,-0.59028,0.61613,0.69469], "fy":[0.99078,-0.03324,-0.12524,0.90347]}, + {"t":1.71782, "x":6.88991, "y":3.46383, "heading":3.08709, "vx":-0.99252, "vy":0.11996, "omega":0.39397, "ax":0.00314, "ay":0.0259, "alpha":-0.21098, "fx":[-0.51126,-0.57248,0.61109,0.67227], "fy":[0.94719,-0.05355,-0.12847,0.87945]}, + {"t":1.75647, "x":6.85155, "y":3.46848, "heading":3.10232, "vx":-0.9924, "vy":0.12096, "omega":0.38582, "ax":0.00304, "ay":0.02486, "alpha":-0.21559, "fx":[-0.5339,-0.58053,0.63052,0.67711], "fy":[0.93349,-0.08831,-0.14861,0.88212]}, + {"t":1.79512, "x":6.81319, "y":3.47318, "heading":3.11723, "vx":-0.99228, "vy":0.12192, "omega":0.37749, "ax":0.00298, "ay":0.02417, "alpha":-0.23028, "fx":[-0.58317,-0.61617,0.67784,0.71081], "fy":[0.95037,-0.14243,-0.1874,0.91424]}, + {"t":1.83378, "x":6.77484, "y":3.47791, "heading":3.13182, "vx":-0.99216, "vy":0.12286, "omega":0.36859, "ax":0.00296, "ay":0.02379, "alpha":-0.25514, "fx":[-0.66055,-0.67875,0.75447,0.77263], "fy":[0.99616,-0.21704,-0.24471,0.97655]}, + {"t":1.87243, "x":6.7365, "y":3.48267, "heading":-3.13712, "vx":-0.99205, "vy":0.12378, "omega":0.35872, "ax":0.00297, "ay":0.02372, "alpha":-0.29174, "fx":[-0.77127,-0.77152,0.86561,0.86581], "fy":[1.0734,-0.31789,-0.32316,1.07384]}, + {"t":1.91108, "x":6.69815, "y":3.48748, "heading":-3.12325, "vx":-0.99194, "vy":0.12469, "omega":0.34745, "ax":0.00302, "ay":0.02394, "alpha":-0.34135, "fx":[-0.92007,-0.89694,1.01602,0.99282], "fy":[1.18364,-0.44883,-0.42482,1.21026]}, + {"t":1.94973, "x":6.65982, "y":3.49231, "heading":-3.10983, "vx":-0.99182, "vy":0.12562, "omega":0.33425, "ax":0.00311, "ay":0.02447, "alpha":-0.40625, "fx":[-1.11435,-1.05988,1.21317,1.15859], "fy":[1.33114,-0.61699,-0.55342,1.39291]}, + {"t":1.98838, "x":6.62148, "y":3.49719, "heading":-3.09691, "vx":-0.9917, "vy":0.12657, "omega":0.31855, "ax":0.00324, "ay":0.02532, "alpha":-0.48923, "fx":[-1.36322,-1.26654,1.46632,1.36945], "fy":[1.52113,-0.82992,-0.7137,1.63038]}, + {"t":2.02703, "x":6.58316, "y":3.5021, "heading":-3.08459, "vx":-0.99157, "vy":0.12754, "omega":0.29964, "ax":0.00343, "ay":0.02655, "alpha":-0.59354, "fx":[-1.67714,-1.52403,1.78616,1.63274], "fy":[1.76013,-1.09631,-0.91103,1.93296]}, + {"t":2.06568, "x":6.54483, "y":3.50705, "heading":-3.07301, "vx":-0.99144, "vy":0.12857, "omega":0.2767, "ax":0.00367, "ay":0.02821, "alpha":-0.7239, "fx":[-2.07101,-1.84339,2.18794,1.95979], "fy":[2.05813,-1.42799,-1.1537,2.31508]}, + {"t":2.10433, "x":6.50652, "y":3.51204, "heading":-3.06232, "vx":-0.9913, "vy":0.12966, "omega":0.24872, "ax":0.004, "ay":0.0304, "alpha":-0.88502, "fx":[-2.55979,-2.23548,2.68711,2.36185], "fy":[2.42502,-1.83545,-1.45018,2.79131]}, + {"t":2.14299, "x":6.4682, "y":3.51707, "heading":-3.0527, "vx":-0.99114, "vy":0.13084, "omega":0.21452, "ax":0.00441, "ay":0.03323, "alpha":-1.08412, "fx":[-3.16575,-2.71806,3.30663,2.85712], "fy":[2.87685,-2.33537,-1.81433,3.38297]}, + {"t":2.18164, "x":6.4299, "y":3.52215, "heading":-3.04441, "vx":-0.99097, "vy":0.13212, "omega":0.17261, "ax":0.00494, "ay":0.03689, "alpha":-1.32809, "fx":[-3.9101,-3.30857,4.06893,3.46374], "fy":[3.43024,-2.93945,-2.25962,4.11118]}, + {"t":2.22029, "x":6.3916, "y":3.52729, "heading":-3.03774, "vx":-0.99078, "vy":0.13355, "omega":0.12128, "ax":0.00568, "ay":0.04189, "alpha":-1.62738, "fx":[-4.82291,-4.03416,5.00726,4.21055], "fy":[4.11589,-3.66355,-2.80284,5.0105]}, + {"t":2.25894, "x":6.35331, "y":3.53248, "heading":-3.03305, "vx":-0.99056, "vy":0.13517, "omega":0.05838, "ax":0.00677, "ay":0.04929, "alpha":-1.99212, "fx":[-5.93104,-4.92259,6.15514,5.12867], "fy":[4.97383,-4.5098,-3.45455,6.12066]}, + {"t":2.29759, "x":6.31503, "y":3.53774, "heading":-3.0308, "vx":-0.9903, "vy":0.13707, "omega":-0.01862, "ax":0.0084, "ay":0.06018, "alpha":-2.43668, "fx":[-7.27036,-6.01581,7.5575,6.26222], "fy":[6.05851,-5.49151,-4.23483,7.48971]}, + {"t":2.33624, "x":6.27676, "y":3.54308, "heading":-3.03152, "vx":-0.98998, "vy":0.1394, "omega":-0.1128, "ax":0.0108, "ay":0.07586, "alpha":-2.97619, "fx":[-8.87277,-7.3613,9.25868,7.66094], "fy":[7.43735,-6.62023,-5.16512,9.16507]}, + {"t":2.37489, "x":6.2385, "y":3.54853, "heading":-3.03588, "vx":-0.98956, "vy":0.14233, "omega":-0.22783, "ax":0.01663, "ay":0.11385, "alpha":-3.63492, "fx":[-10.74058,-8.98483,11.34536,9.43633], "fy":[9.47866,-7.70732,-6.02244,11.48115]}, + {"t":2.41354, "x":6.20027, "y":3.55412, "heading":-3.04468, "vx":-0.98892, "vy":0.14673, "omega":-0.36832, "ax":0.0318, "ay":0.20825, "alpha":-4.44418, "fx":[-12.81475,-10.83285,13.86707,11.79963], "fy":[12.85969,-8.43537,-6.30607,15.10605]}, + {"t":2.45219, "x":6.16207, "y":3.55994, "heading":-3.05892, "vx":-0.98769, "vy":0.15478, "omega":-0.5401, "ax":2.11125, "ay":-0.99747, "alpha":-4.50414, "fx":[21.13309,21.64591,45.36369,45.92789], "fy":[-6.33805,-29.53407,-24.60328,-2.86685]}, + {"t":2.49085, "x":6.12547, "y":3.56518, "heading":-3.07979, "vx":-0.90608, "vy":0.11622, "omega":-0.71419, "ax":1.75893, "ay":-3.58844, "alpha":-4.41156, "fx":[14.4687,14.02106,39.76893,43.43856], "fy":[-50.73851,-68.49962,-63.38669,-45.25181]}, + {"t":2.5295, "x":6.09176, "y":3.56699, "heading":-3.1074, "vx":-0.8381, "vy":-0.02247, "omega":-0.8847, "ax":1.14437, "ay":-3.83141, "alpha":-5.30079, "fx":[0.8102,1.50848,32.9155,37.43639], "fy":[-52.87703,-73.00296,-69.02613,-48.3994]}, + {"t":2.56815, "x":6.06022, "y":3.56326, "heading":3.14159, "vx":-0.79387, "vy":-0.17056, "omega":-1.08958, "ax":1.05622, "ay":-3.85415, "alpha":-3.07554, "fx":[7.22375,6.53229,25.38835,27.92888], "fy":[-56.24022,-67.97181,-66.21318,-54.32472]}, + {"t":2.58969, "x":6.04337, "y":3.55869, "heading":3.11812, "vx":-0.77111, "vy":-0.2536, "omega":-1.15584, "ax":1.05989, "ay":-3.85499, "alpha":-2.76692, "fx":[8.57518,7.40786,24.44506,26.87777], "fy":[-56.4058,-67.27723,-65.894,-55.22633]}, + {"t":2.61124, "x":6.027, "y":3.55233, "heading":3.09322, "vx":-0.74828, "vy":-0.33665, "omega":-1.21545, "ax":1.06337, "ay":-3.85393, "alpha":-2.50167, "fx":[9.58358,8.15996,23.6046,26.17884], "fy":[-56.8446,-66.47456,-65.56836,-55.84854]}, + {"t":2.63278, "x":6.01113, "y":3.54419, "heading":3.06703, "vx":-0.72537, "vy":-0.41968, "omega":-1.26935, "ax":1.06721, "ay":-3.85276, "alpha":-2.26675, "fx":[10.64203,8.88594,22.89275,25.35034], "fy":[-56.90358,-65.94839,-65.29781,-56.51172]}, + {"t":2.65432, "x":5.99575, "y":3.53425, "heading":3.03968, "vx":-0.70238, "vy":-0.50268, "omega":-1.31818, "ax":1.07147, "ay":-3.85145, "alpha":-2.05542, "fx":[11.43026,9.53922,22.22037,24.85195], "fy":[-57.29161,-65.26959,-65.02559,-56.9917]}, + {"t":2.67587, "x":5.98086, "y":3.52253, "heading":3.01129, "vx":-0.67929, "vy":-0.58566, "omega":-1.36246, "ax":1.07623, "ay":-3.84999, "alpha":-1.8681, "fx":[12.31471,10.17455,21.69221,24.16244], "fy":[-57.30645,-64.87382,-64.78997,-57.5152]}, + {"t":2.69741, "x":5.96648, "y":3.50902, "heading":2.98193, "vx":-0.65611, "vy":-0.6686, "omega":-1.40271, "ax":1.08157, "ay":-3.84833, "alpha":-1.70398, "fx":[12.91558,10.73513,21.18903,23.84335], "fy":[-57.65619,-64.29717,-64.55657,-57.87057]}, + {"t":2.71896, "x":5.95259, "y":3.49372, "heading":2.95171, "vx":-0.63281, "vy":-0.75151, "omega":-1.43942, "ax":1.08761, "ay":-3.84645, "alpha":-1.55912, "fx":[13.6649,11.28936,20.82395,23.2885], "fy":[-57.61504,-64.01524,-64.3602,-58.27069]}, + {"t":2.7405, "x":5.93921, "y":3.47664, "heading":2.9207, "vx":-0.60938, "vy":-0.83438, "omega":-1.47301, "ax":1.0945, "ay":-3.8443, "alpha":-1.43173, "fx":[14.12087,11.78749,20.46173,23.13369], "fy":[-57.94274,-63.468,-64.17947,-58.53418]}, + {"t":2.76204, "x":5.92634, "y":3.45777, "heading":2.88897, "vx":-0.5858, "vy":-0.9172, "omega":-1.50386, "ax":1.10242, "ay":-3.8418, "alpha":-1.33273, "fx":[14.7925,12.23713,20.24543,22.7318], "fy":[-57.7663,-63.34524,-64.02074,-58.83359]}, + {"t":2.78359, "x":5.91397, "y":3.43712, "heading":2.85657, "vx":-0.56204, "vy":-0.99997, "omega":-1.53257, "ax":1.11162, "ay":-3.83888, "alpha":-1.23864, "fx":[15.13908,12.67162,20.03554,22.74501], "fy":[-58.0943,-62.83509,-63.8652,-58.98566]}, + {"t":2.80513, "x":5.90212, "y":3.41468, "heading":2.82355, "vx":-0.5381, "vy":-1.08267, "omega":-1.55925, "ax":1.12246, "ay":-3.83541, "alpha":-1.17893, "fx":[15.7707,13.04942,19.95606,22.50315], "fy":[-57.79586,-62.80941,-63.75809,-59.19634]}, + {"t":2.82668, "x":5.89079, "y":3.39047, "heading":2.78996, "vx":-0.51391, "vy":-1.1653, "omega":-1.58465, "ax":1.13539, "ay":-3.83122, "alpha":-1.11406, "fx":[16.06187,13.44569,19.88906,22.70429], "fy":[-58.10556,-62.28949,-63.63886,-59.25959]}, + {"t":2.84822, "x":5.87998, "y":3.36447, "heading":2.75582, "vx":-0.48945, "vy":-1.24784, "omega":-1.60865, "ax":1.15112, "ay":-3.82606, "alpha":-1.08517, "fx":[16.69529,13.78799,19.98041,22.63581], "fy":[-57.73393,-62.32692,-63.54152,-59.36335]}, + {"t":2.86976, "x":5.8697, "y":3.3367, "heading":2.72116, "vx":-0.46465, "vy":-1.33027, "omega":-1.63203, "ax":1.17061, "ay":-3.81956, "alpha":-1.0625, "fx":[17.03158,14.13045,20.07155,23.10384], "fy":[-57.88323,-61.89285,-63.45113,-59.32573]}, + {"t":2.89131, "x":5.85997, "y":3.30716, "heading":2.686, "vx":-0.43943, "vy":-1.41256, "omega":-1.65492, "ax":1.19547, "ay":-3.81111, "alpha":-1.06279, "fx":[17.76025,14.52108,20.35111,23.28313], "fy":[-57.38,-61.87425,-63.39829,-59.36382]}, + {"t":2.91285, "x":5.85078, "y":3.27584, "heading":2.65035, "vx":-0.41368, "vy":-1.49467, "omega":-1.67782, "ax":1.22818, "ay":-3.7997, "alpha":-1.07015, "fx":[18.25812,14.95762,20.67651,24.10095], "fy":[-57.41303,-61.32401,-63.33239,-59.22263]}, + {"t":2.9344, "x":5.84215, "y":3.24276, "heading":2.6142, "vx":-0.38722, "vy":-1.57653, "omega":-1.70088, "ax":1.27331, "ay":-3.78344, "alpha":-1.10086, "fx":[19.30523,15.54772,21.34288,24.6633], "fy":[-56.6698,-61.28031,-63.22267,-59.08675]}, + {"t":2.95594, "x":5.8341, "y":3.20791, "heading":2.57756, "vx":-0.35979, "vy":-1.65804, "omega":-1.72459, "ax":1.33914, "ay":-3.7586, "alpha":-1.15787, "fx":[20.32373,16.31426,22.16104,26.24012], "fy":[-56.34425,-60.58062,-63.06772,-58.68974]}, + {"t":2.97748, "x":5.82666, "y":3.17132, "heading":2.5404, "vx":-0.33094, "vy":-1.73901, "omega":-1.74954, "ax":1.44502, "ay":-3.71583, "alpha":-1.22882, "fx":[22.41626,17.77596,23.82988,27.74108], "fy":[-54.84558,-60.17324,-62.7293,-58.21799]}, + {"t":2.99903, "x":5.81987, "y":3.13299, "heading":2.50271, "vx":-0.2998, "vy":-1.81907, "omega":-1.77601, "ax":1.64032, "ay":-3.62735, "alpha":-1.33456, "fx":[25.49437,20.41575,26.575,31.67975], "fy":[-53.52149,-58.29332,-61.79975,-56.73307]}, + {"t":3.02057, "x":5.81379, "y":3.09296, "heading":2.46445, "vx":-0.26446, "vy":-1.89721, "omega":-1.80476, "ax":2.11598, "ay":-3.35425, "alpha":-1.48326, "fx":[33.59778,27.61272,34.1854,38.97523], "fy":[-47.71747,-54.43068,-58.21295,-52.64376]}, + {"t":3.04212, "x":5.80858, "y":3.05131, "heading":2.42557, "vx":-0.21888, "vy":-1.96948, "omega":-1.83672, "ax":3.7259, "ay":-1.14492, "alpha":-1.80113, "fx":[58.90752,52.89572,59.57464,65.22754], "fy":[-11.10029,-18.44735,-25.35752,-17.8007]}, + {"t":3.06366, "x":5.80473, "y":3.00861, "heading":2.38599, "vx":-0.13861, "vy":-1.99414, "omega":-1.87552, "ax":3.37427, "ay":-0.17803, "alpha":-2.1349, "fx":[53.52541,45.99487,55.06403,59.69155], "fy":[5.99487,-2.47623,-11.69574,-3.1285]}, + {"t":3.0852, "x":5.80253, "y":2.96561, "heading":2.34559, "vx":-0.06591, "vy":-1.99798, "omega":-1.92152, "ax":-3.74217, "ay":0.20055, "alpha":-2.27211, "fx":[-59.82217,-66.64515,-59.30921,-51.86213], "fy":[12.41872,3.97184,-6.22534,2.57009]}, + {"t":3.10675, "x":5.80024, "y":2.92261, "heading":2.30419, "vx":-0.14653, "vy":-1.99366, "omega":-1.97047, "ax":-3.88967, "ay":0.43378, "alpha":-2.42985, "fx":[-61.81439,-69.19284,-62.47543,-53.52301], "fy":[16.90556,7.59397,-3.20418,6.25083]}, + {"t":3.12829, "x":5.79618, "y":2.87976, "heading":2.26174, "vx":-0.23033, "vy":-1.98431, "omega":-2.02282, "ax":-3.74904, "ay":0.42531, "alpha":-1.94294, "fx":[-59.25529,-65.5748,-60.02513,-53.2199], "fy":[14.80797,7.81225,-1.27611,5.66419]}, + {"t":3.14159, "x":5.79279, "y":2.85342, "heading":2.23485, "vx":-0.28017, "vy":-1.97866, "omega":-2.04864, "ax":-3.80971, "ay":0.58632, "alpha":-1.79377, "fx":[-60.31671,-66.11742,-61.44009,-54.05354], "fy":[16.32919,10.42173,2.17599,8.30613]}, + {"t":3.15488, "x":5.78873, "y":2.82717, "heading":2.20762, "vx":-0.33081, "vy":-1.97087, "omega":-2.07249, "ax":-3.75142, "ay":0.67758, "alpha":-1.53214, "fx":[-58.95146,-64.25637,-60.3087,-54.70939], "fy":[17.16646,11.75904,4.4217,9.68106]}, + {"t":3.16817, "x":5.784, "y":2.80103, "heading":2.18007, "vx":-0.38068, "vy":-1.96186, "omega":-2.09285, "ax":-3.67173, "ay":0.75921, "alpha":-1.49863, "fx":[-57.93144,-63.12904,-59.42362,-52.68175], "fy":[17.61775,13.28698,6.31608,10.99115]}, + {"t":3.18146, "x":5.77861, "y":2.77502, "heading":2.15225, "vx":-0.42949, "vy":-1.95177, "omega":-2.11278, "ax":-3.55869, "ay":0.82789, "alpha":-1.25899, "fx":[-55.73482,-60.43628,-57.34671,-52.46965], "fy":[18.32619,14.08197,8.11222,12.05302]}, + {"t":3.19476, "x":5.77259, "y":2.74915, "heading":2.12416, "vx":-0.47679, "vy":-1.94076, "omega":-2.12951, "ax":-3.31396, "ay":0.85372, "alpha":-1.25603, "fx":[-52.15254,-56.85122,-53.80212,-47.64046], "fy":[17.85079,14.90299,9.01375,12.44598]}, + {"t":3.20805, "x":5.76596, "y":2.72342, "heading":2.09586, "vx":-0.52084, "vy":-1.92941, "omega":-2.14621, "ax":-3.13172, "ay":0.88142, "alpha":-1.03332, "fx":[-48.90298,-53.06572,-50.57804,-46.32675], "fy":[18.1232,14.86488,10.07323,12.91119]}, + {"t":3.22134, "x":5.75876, "y":2.69785, "heading":2.06733, "vx":-0.56247, "vy":-1.9177, "omega":-2.15994, "ax":-2.20201, "ay":0.66447, "alpha":-0.99494, "fx":[-34.33768,-38.38711,-35.98947,-31.11972], "fy":[13.85618,11.86527,7.0499,9.42442]}, + {"t":3.23464, "x":5.75109, "y":2.67242, "heading":2.03861, "vx":-0.59175, "vy":-1.90886, "omega":-2.17317, "ax":-2.06677, "ay":0.65583, "alpha":-0.82289, "fx":[-32.0252,-35.55468,-33.61597,-30.05022], "fy":[13.52947,11.31866,7.44365,9.35509]}, + {"t":3.24793, "x":5.74304, "y":2.6471, "heading":2.00973, "vx":-0.61922, "vy":-1.90015, "omega":-2.18411, "ax":-1.29167, "ay":0.42786, "alpha":-0.7218, "fx":[-19.84113,-22.95235,-21.32361,-17.90756], "fy":[9.22568,7.8905,4.26831,5.78582]}, + {"t":3.26122, "x":5.73469, "y":2.62188, "heading":1.98069, "vx":-0.63639, "vy":-1.89446, "omega":-2.1937, "ax":-1.01262, "ay":0.34406, "alpha":-0.58675, "fx":[-15.40744,-18.01542,-16.75057,-14.1306], "fy":[7.55348,6.34699,3.39516,4.55313]}, + {"t":3.27452, "x":5.72614, "y":2.59673, "heading":1.95153, "vx":-0.64985, "vy":-1.88989, "omega":-2.2015, "ax":-0.73355, "ay":0.25418, "alpha":-0.46529, "fx":[-11.08765,-13.16363,-12.22729,-10.10418], "fy":[5.65058,4.81134,2.41592,3.26344]}, + {"t":3.28781, "x":5.71744, "y":2.57163, "heading":1.92227, "vx":-0.6596, "vy":-1.88651, "omega":-2.20769, "ax":-0.40484, "ay":0.14259, "alpha":-0.34661, "fx":[-5.96805,-7.53721,-6.88767,-5.3158], "fy":[3.40725,2.95859,1.0576,1.63154]}, + {"t":3.3011, "x":5.70864, "y":2.54657, "heading":1.89292, "vx":-0.66498, "vy":-1.88461, "omega":-2.2123, "ax":-0.41006, "ay":0.14499, "alpha":-0.22284, "fx":[-6.18295,-7.19097,-6.8095,-5.85628], "fy":[3.12845,2.68507,1.52677,1.86704]}, + {"t":3.31439, "x":5.69976, "y":2.52153, "heading":1.86351, "vx":-0.67043, "vy":-1.88268, "omega":-2.21526, "ax":-0.10568, "ay":0.03802, "alpha":-0.10949, "fx":[-1.51782,-2.00452,-1.8377,-1.35074], "fy":[0.88767,0.95319,0.21656,0.35711]}, + {"t":3.32769, "x":5.69084, "y":2.4965, "heading":1.83406, "vx":-0.67184, "vy":-1.88218, "omega":-2.21671, "ax":-0.22662, "ay":0.08085, "alpha":0.01413, "fx":[-3.61013,-3.51979,-3.54687,-3.71434], "fy":[1.33714,1.20436,1.30856,1.28431]}, + {"t":3.34098, "x":5.68189, "y":2.47149, "heading":1.8046, "vx":-0.67485, "vy":-1.8811, "omega":-2.21653, "ax":0.02126, "ay":-0.00747, "alpha":0.12923, "fx":[0.10368,0.7442,0.57127,-0.06892], "fy":[-0.63052,-0.25034,0.28192,0.12433]}, + {"t":3.35427, "x":5.67292, "y":2.44649, "heading":1.77513, "vx":-0.67457, "vy":-1.8812, "omega":-2.21481, "ax":-0.12314, "ay":0.04414, "alpha":0.25513, "fx":[-2.41463,-1.17182,-1.46461,-2.76884], "fy":[-0.01218,0.07476,1.49928,1.24111]}, + {"t":3.36757, "x":5.66394, "y":2.42148, "heading":1.74569, "vx":-0.67621, "vy":-1.88062, "omega":-2.21142, "ax":0.0586, "ay":-0.02105, "alpha":0.37776, "fx":[0.19517,2.03476,1.66495,-0.17371], "fy":[-1.60807,-1.07604,0.83762,0.50974]}, + {"t":3.38086, "x":5.65496, "y":2.39648, "heading":1.7163, "vx":-0.67543, "vy":-1.8809, "omega":-2.20639, "ax":-0.06346, "ay":0.02281, "alpha":0.50997, "fx":[-2.03449,0.44884,0.03418,-2.47838], "fy":[-1.12,-0.91818,1.92571,1.56126]}, + {"t":3.39415, "x":5.64597, "y":2.37148, "heading":1.68697, "vx":-0.67627, "vy":-1.88059, "omega":-2.19962, "ax":0.0485, "ay":-0.01746, "alpha":0.64584, "fx":[-0.59502,2.55294,2.13431,-1.01213], "fy":[-2.30514,-1.76815,1.66637,1.29806]}, + {"t":3.40745, "x":5.63699, "y":2.34648, "heading":1.65773, "vx":-0.67563, "vy":-1.88082, "omega":-2.19103, "ax":-0.02704, "ay":0.00969, "alpha":0.78973, "fx":[-2.16569,1.68677,1.30347,-2.54196], "fy":[-2.11796,-1.916,2.49262,2.15667]}, + {"t":3.42074, "x":5.628, "y":2.32148, "heading":1.6286, "vx":-0.67599, "vy":-1.8807, "omega":-2.18053, "ax":-0.00003, "ay":0.00038, "alpha":0.94373, "fx":[-2.15279,2.45551,2.14978,-2.45467], "fy":[-2.7902,-2.39016,2.73508,2.46934]}, + {"t":3.43403, "x":5.61902, "y":2.29648, "heading":1.59962, "vx":-0.67599, "vy":-1.88069, "omega":-2.16799, "ax":0.00453, "ay":0.11766, "alpha":1.10687, "fx":[-2.54152,2.86602,2.67984,-2.71695], "fy":[-1.3154,-1.05534,4.99651,4.84628]}, + {"t":3.44732, "x":5.61003, "y":2.27149, "heading":1.5708, "vx":-0.67593, "vy":-1.87913, "omega":-2.15327, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[ + {"name":"IntakeDown", "from":{"target":0, "targetTimestamp":0.0, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeDown"}}}, + {"name":"IntakeUp", "from":{"target":3, "targetTimestamp":3.12829, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"IntakeUp"}}}] +} diff --git a/src/main/deploy/choreo/RightShoot.traj b/src/main/deploy/choreo/RightShoot.traj new file mode 100644 index 0000000..660d258 --- /dev/null +++ b/src/main/deploy/choreo/RightShoot.traj @@ -0,0 +1,95 @@ +{ + "name":"RightShoot", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.5869221687316895, "y":2.4568724632263184, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.3642568588256836, "y":2.448859214782715, "heading":-2.1975393825258824, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":5.0}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":5.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.5869221687316895 m", "val":3.5869221687316895}, "y":{"exp":"2.4568724632263184 m", "val":2.4568724632263184}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.3642568588256836 m", "val":3.3642568588256836}, "y":{"exp":"2.448859214782715 m", "val":2.448859214782715}, "heading":{"exp":"-2.1975393825258824 rad", "val":-2.1975393825258824}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"5 m / s", "val":5.0}}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"5 rad / s", "val":5.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.64113], + "samples":[ + {"t":0.0, "x":3.58692, "y":2.45687, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.20729, "ay":-0.00346, "alpha":36.50204, "fx":[-91.63546,85.05154,85.17919,-91.75858], "fy":[-98.573,-104.28633,104.18154,98.45789]}, + {"t":0.02068, "x":3.58688, "y":2.45687, "heading":1.5708, "vx":-0.00429, "vy":-0.00007, "omega":0.75492, "ax":-0.23127, "ay":-0.00387, "alpha":36.48239, "fx":[-91.93951,84.59277,84.73646,-92.07602], "fy":[-98.21767,-104.5846,104.46747,98.08905]}, + {"t":0.04136, "x":3.58674, "y":2.45687, "heading":1.58641, "vx":-0.00907, "vy":-0.00015, "omega":1.50943, "ax":-0.26674, "ay":-0.00548, "alpha":36.45233, "fx":[-90.90175,85.59383,82.36518,-93.99629], "fy":[-99.074,-103.65477,106.23808,96.14276]}, + {"t":0.06204, "x":3.5865, "y":2.45687, "heading":1.61763, "vx":-0.01459, "vy":-0.00026, "omega":2.26332, "ax":-0.32417, "ay":-0.00909, "alpha":36.4009, "fx":[-88.6553,87.84456,77.72652,-97.50146], "fy":[-100.92235,-101.56425,109.50333,92.40605]}, + {"t":0.08273, "x":3.58612, "y":2.45686, "heading":1.66444, "vx":-0.02129, "vy":-0.00045, "omega":3.01615, "ax":-0.43707, "ay":-0.01612, "alpha":36.29239, "fx":[-85.65632,90.71708,69.77657,-102.59245], "fy":[-103.17062,-98.61866,114.40709,86.35827]}, + {"t":0.10341, "x":3.58559, "y":2.45684, "heading":1.72681, "vx":-0.03033, "vy":-0.00079, "omega":3.76673, "ax":-0.7438, "ay":-0.04405, "alpha":35.93817, "fx":[-83.8916,91.70367,54.66564,-109.7111], "fy":[-103.80905,-96.54605,121.48641,76.07107]}, + {"t":0.12409, "x":3.5848, "y":2.45682, "heading":1.80472, "vx":-0.04571, "vy":-0.0017, "omega":4.50998, "ax":-4.94111, "ay":-0.53173, "alpha":23.55581, "fx":[-108.07673,-24.40234,-59.21015,-122.08583], "fy":[-68.09106,-113.57234,108.06273,39.8343]}, + {"t":0.14477, "x":3.5828, "y":2.45667, "heading":1.89799, "vx":-0.1479, "vy":-0.01269, "omega":4.99715, "ax":-5.74937, "ay":-0.14408, "alpha":0.01968, "fx":[-90.82099,-92.34333,-91.98576,-89.95191], "fy":[-1.65503,-5.11337,-0.23202,-2.1493]}, + {"t":0.16545, "x":3.57851, "y":2.45638, "heading":2.00134, "vx":-0.26681, "vy":-0.01567, "omega":4.99756, "ax":-5.70469, "ay":-0.15383, "alpha":-0.00003, "fx":[-90.57929,-90.68745,-90.64148,-90.35621], "fy":[-2.66141,-2.39319,-2.3343,-2.37947]}, + {"t":0.18613, "x":3.57178, "y":2.45602, "heading":2.10469, "vx":-0.38479, "vy":-0.01886, "omega":4.99756, "ax":-5.63974, "ay":-0.14116, "alpha":-0.00001, "fx":[-89.53194,-89.55197,-89.53934,-89.51678], "fy":[-2.24089,-2.29089,-2.23586,-2.19627]}, + {"t":0.20682, "x":3.56261, "y":2.4556, "heading":2.20805, "vx":-0.50143, "vy":-0.02178, "omega":4.99756, "ax":-5.54616, "ay":-0.15794, "alpha":0.00001, "fx":[-88.15227,-88.14508,-88.15942,-87.74038], "fy":[-2.73725,-2.49178,-2.34837,-2.45215]}, + {"t":0.2275, "x":3.55105, "y":2.45512, "heading":2.31141, "vx":-0.61613, "vy":-0.02504, "omega":4.99756, "ax":-5.38518, "ay":-0.12499, "alpha":0.0, "fx":[-85.49448,-85.51205,-85.4938,-85.47456], "fy":[-2.00307,-1.98516,-1.96606,-1.98304]}, + {"t":0.24818, "x":3.53716, "y":2.45457, "heading":2.41477, "vx":-0.72751, "vy":-0.02763, "omega":4.99756, "ax":-4.57691, "ay":-0.12438, "alpha":-0.00001, "fx":[-72.96572,-72.67973,-73.04994,-71.95173], "fy":[-2.30228,-2.01197,-1.57936,-2.00519]}, + {"t":0.26886, "x":3.52114, "y":2.45397, "heading":2.51812, "vx":-0.82216, "vy":-0.0302, "omega":4.99756, "ax":-4.4439, "ay":-0.07323, "alpha":0.00001, "fx":[-70.54237,-70.50281,-70.55827,-70.59713], "fy":[-1.11948,-1.14388,-1.22158,-1.16524]}, + {"t":0.28954, "x":3.50318, "y":2.45333, "heading":2.62148, "vx":-0.91407, "vy":-0.03171, "omega":4.99756, "ax":-1.58383, "ay":-0.02384, "alpha":-0.00001, "fx":[-25.26456,-25.13437,-25.39534,-24.78345], "fy":[-0.50901,-0.35865,-0.20706,-0.43911]}, + {"t":0.31022, "x":3.48394, "y":2.45267, "heading":2.72484, "vx":-0.94683, "vy":-0.03221, "omega":4.99756, "ax":0.19504, "ay":0.03314, "alpha":0.0, "fx":[3.09954,3.10377,3.09317,3.08895], "fy":[0.53507,0.51998,0.51973,0.52986]}, + {"t":0.3309, "x":3.4644, "y":2.45201, "heading":2.8282, "vx":-0.94279, "vy":-0.03152, "omega":4.99756, "ax":1.85983, "ay":0.08882, "alpha":0.00001, "fx":[29.62329,29.54656,29.82031,29.11442], "fy":[1.53073,1.34832,1.25731,1.50406]}, + {"t":0.35159, "x":3.4453, "y":2.45138, "heading":2.93155, "vx":-0.90433, "vy":-0.02968, "omega":4.99756, "ax":4.57168, "ay":0.12402, "alpha":0.0, "fx":[72.56161,72.55402,72.59546,72.60389], "fy":[1.94164,1.96105,2.01778,1.9552]}, + {"t":0.37227, "x":3.42757, "y":2.45079, "heading":3.03491, "vx":-0.80978, "vy":-0.02712, "omega":4.99756, "ax":4.74278, "ay":0.17559, "alpha":0.00001, "fx":[75.45243,75.42791,75.7628,74.53721], "fy":[2.97745,2.61557,2.56561,2.99215]}, + {"t":0.39295, "x":3.41184, "y":2.45027, "heading":3.13827, "vx":-0.71169, "vy":-0.02349, "omega":4.99756, "ax":5.40925, "ay":0.14831, "alpha":0.0, "fx":[85.89479,85.8993,85.85451,85.85483], "fy":[2.37913,2.24801,2.41215,2.37863]}, + {"t":0.41363, "x":3.39828, "y":2.44982, "heading":-3.04156, "vx":-0.59982, "vy":-0.02042, "omega":4.99756, "ax":5.55961, "ay":0.18032, "alpha":-0.00001, "fx":[88.35004,88.36151,88.36229,87.97771], "fy":[3.0165,2.74606,2.78225,2.90599]}, + {"t":0.43431, "x":3.38706, "y":2.44943, "heading":-2.9382, "vx":-0.48484, "vy":-0.01669, "omega":4.99756, "ax":5.64907, "ay":0.14848, "alpha":0.00001, "fx":[89.71365,89.70992,89.64863,89.6602], "fy":[2.36624,2.21397,2.46492,2.38364]}, + {"t":0.45499, "x":3.37824, "y":2.44912, "heading":-2.83485, "vx":-0.36801, "vy":-0.01362, "omega":4.99756, "ax":5.71085, "ay":0.16081, "alpha":0.00006, "fx":[90.76357,90.75545,90.6417,90.49509], "fy":[2.73735,2.37963,2.52621,2.56874]}, + {"t":0.47568, "x":3.37185, "y":2.44887, "heading":-2.73149, "vx":-0.2499, "vy":-0.01029, "omega":4.99756, "ax":5.75426, "ay":0.1406, "alpha":-0.02133, "fx":[92.7849,92.24207,89.67166,90.7136], "fy":[4.30456,-1.28173,3.20184,2.70393]}, + {"t":0.49636, "x":3.36792, "y":2.44869, "heading":-2.62813, "vx":-0.13089, "vy":-0.00739, "omega":4.99712, "ax":4.40376, "ay":0.5603, "alpha":-23.48289, "fx":[-20.85605,68.69324,128.3529,103.46143], "fy":[79.06527,-104.64724,-13.99534,75.15814]}, + {"t":0.51704, "x":3.36615, "y":2.44865, "heading":-2.52478, "vx":-0.03981, "vy":0.0042, "omega":4.51146, "ax":0.62125, "ay":-0.06013, "alpha":-35.98348, "fx":[-132.07149,-10.87221,133.07304,49.3221], "fy":[16.40364,-132.86892,-11.32893,123.97577]}, + {"t":0.53772, "x":3.36546, "y":2.44873, "heading":-2.43148, "vx":-0.02697, "vy":0.00296, "omega":3.76726, "ax":0.37665, "ay":-0.04172, "alpha":-36.30527, "fx":[-133.96615,-6.91811,134.11931,30.68317], "fy":[1.45397,-133.86702,-0.74732,130.51087]}, + {"t":0.5584, "x":3.36498, "y":2.44878, "heading":-2.35357, "vx":-0.01918, "vy":0.00209, "omega":3.01642, "ax":0.2864, "ay":-0.03218, "alpha":-36.40674, "fx":[-133.90816,0.41446,134.04538,17.63562], "fy":[-9.8598,-134.30793,8.9722,133.15207]}, + {"t":0.57908, "x":3.36465, "y":2.44882, "heading":-2.29118, "vx":-0.01325, "vy":0.00143, "omega":2.26347, "ax":0.23976, "ay":-0.0265, "alpha":-36.45574, "fx":[-133.12451,7.17966,133.39153,7.77903], "fy":[-18.60937,-134.25162,16.95889,134.2193]}, + {"t":0.59976, "x":3.36442, "y":2.44884, "heading":-2.24437, "vx":-0.00829, "vy":0.00088, "omega":1.50951, "ax":0.21053, "ay":-0.02264, "alpha":-36.48464, "fx":[-132.15961,12.45671,132.55773,0.51425], "fy":[-25.02379,-133.94746,23.00999,134.52369]}, + {"t":0.62045, "x":3.3643, "y":2.44885, "heading":-2.21315, "vx":-0.00394, "vy":0.00041, "omega":0.75495, "ax":0.19051, "ay":-0.01997, "alpha":-36.50364, "fx":[-131.35643,15.96549,131.84018,-4.35144], "fy":[-29.21939,-133.63011,27.07252,134.50877]}, + {"t":0.64113, "x":3.36426, "y":2.44886, "heading":-2.19754, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/RightShootToBump.traj b/src/main/deploy/choreo/RightShootToBump.traj new file mode 100644 index 0000000..4739087 --- /dev/null +++ b/src/main/deploy/choreo/RightShootToBump.traj @@ -0,0 +1,97 @@ +{ + "name":"RightShootToBump", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.3642568588256836, "y":2.448859214782715, "heading":-2.1975393825258824, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.60064959526062, "y":2.3620588779449463, "heading":1.57, "intervals":0, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.60064959526062, "y":2.3620588779449463, "heading":1.57, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":5.0}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":8.0}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":5.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.3642568588256836 m", "val":3.3642568588256836}, "y":{"exp":"2.448859214782715 m", "val":2.448859214782715}, "heading":{"exp":"-2.1975393825258824 rad", "val":-2.1975393825258824}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.60064959526062 m", "val":3.60064959526062}, "y":{"exp":"2.3620588779449463 m", "val":2.3620588779449463}, "heading":{"exp":"3.14 / 2 rad", "val":1.57}, "intervals":0, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.60064959526062 m", "val":3.60064959526062}, "y":{"exp":"2.3620588779449463 m", "val":2.3620588779449463}, "heading":{"exp":"3.14 / 2 rad", "val":1.57}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"5 m / s", "val":5.0}}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"8 m / s ^ 2", "val":8.0}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"5 rad / s", "val":5.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.64126,0.64126], + "samples":[ + {"t":0.0, "x":3.36426, "y":2.44886, "heading":-2.19754, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.16326, "ay":-0.07016, "alpha":-36.50693, "fx":[-130.49399,16.76289,131.72926,-7.63084], "fy":[-32.86919,-133.53554,27.59095,134.35821]}, + {"t":0.02069, "x":3.36429, "y":2.44884, "heading":-2.19754, "vx":0.00338, "vy":-0.00145, "omega":-0.75517, "ax":0.18213, "ay":-0.07825, "alpha":-36.48846, "fx":[-130.35574,17.26477,131.73753,-7.08066], "fy":[-33.17801,-133.41749,27.29488,134.33169]}, + {"t":0.04137, "x":3.3644, "y":2.4488, "heading":-2.21316, "vx":0.00714, "vy":-0.00307, "omega":-1.50996, "ax":0.20949, "ay":-0.09078, "alpha":-36.4604, "fx":[-130.67719,15.96704,132.1366,-4.12337], "fy":[-31.51794,-133.499,24.88104,134.371]}, + {"t":0.06206, "x":3.36459, "y":2.44871, "heading":-2.2444, "vx":0.01148, "vy":-0.00495, "omega":-2.26417, "ax":0.25304, "ay":-0.11163, "alpha":-36.41287, "fx":[-131.33523,13.10707,132.79773,1.49939], "fy":[-27.96234,-133.67976,20.26516,134.28833]}, + {"t":0.08274, "x":3.36488, "y":2.44859, "heading":-2.29123, "vx":0.01671, "vy":-0.00726, "omega":-3.01739, "ax":0.33816, "ay":-0.15346, "alpha":-36.31419, "fx":[-132.03134,9.45016,133.45132,10.60422], "fy":[-22.81812,-133.7383,13.20177,133.60976]}, + {"t":0.10343, "x":3.3653, "y":2.44841, "heading":-2.35365, "vx":0.02371, "vy":-0.01043, "omega":-3.76858, "ax":0.56677, "ay":-0.26975, "alpha":-36.00174, "fx":[-131.96888,8.05819,133.47263,26.42974], "fy":[-17.5184,-133.15715,2.89942,130.64627]}, + {"t":0.12411, "x":3.36591, "y":2.44813, "heading":-2.4316, "vx":0.03543, "vy":-0.01601, "omega":-4.5133, "ax":4.07353, "ay":-2.34497, "alpha":-23.39207, "fx":[-39.05131,65.26737,126.73363,105.73115], "fy":[-86.37221,-108.5697,-20.97501,67.00446]}, + {"t":0.1448, "x":3.36752, "y":2.4473, "heading":-2.52496, "vx":0.1197, "vy":-0.06452, "omega":-4.99718, "ax":6.89835, "ay":-2.41079, "alpha":-0.01865, "fx":[109.54655,109.44724,109.52306,109.54852], "fy":[-38.24676,-38.47404,-38.18606,-38.18534]}, + {"t":0.16549, "x":3.37147, "y":2.44545, "heading":-2.62833, "vx":0.26239, "vy":-0.11439, "omega":-4.99756, "ax":6.71043, "ay":-2.57894, "alpha":0.0, "fx":[106.66259,106.46726,106.52781,106.47454], "fy":[-40.6373,-41.12569,-40.95143,-41.056]}, + {"t":0.18617, "x":3.37833, "y":2.44253, "heading":-2.73171, "vx":0.4012, "vy":-0.16773, "omega":-4.99756, "ax":6.66713, "ay":-2.1615, "alpha":0.0, "fx":[105.80478,105.86901,105.8388,105.86948], "fy":[-34.40157,-34.21947,-34.38908,-34.25141]}, + {"t":0.20686, "x":3.38806, "y":2.4386, "heading":-2.83509, "vx":0.53912, "vy":-0.21245, "omega":-4.99756, "ax":6.01141, "ay":-2.29272, "alpha":0.00001, "fx":[95.66524,95.48976,95.65611,94.93106], "fy":[-35.78599,-36.66324,-36.49811,-36.64732]}, + {"t":0.22754, "x":3.4005, "y":2.43371, "heading":-2.93847, "vx":0.66347, "vy":-0.25987, "omega":-4.99756, "ax":6.20433, "ay":-1.7347, "alpha":0.0, "fx":[98.43868,98.50739,98.51346,98.53349], "fy":[-27.5987,-27.34277,-27.70626,-27.51092]}, + {"t":0.24823, "x":3.41555, "y":2.42797, "heading":-3.04185, "vx":0.79181, "vy":-0.29576, "omega":-4.99756, "ax":4.09883, "ay":-1.51793, "alpha":0.0, "fx":[65.31493,65.26765,65.4466,64.25868], "fy":[-23.48031,-24.35754,-24.32495,-24.22994]}, + {"t":0.26891, "x":3.43281, "y":2.42152, "heading":3.13796, "vx":0.87659, "vy":-0.32716, "omega":-4.99756, "ax":4.3544, "ay":-0.9729, "alpha":0.0, "fx":[69.05316,69.07222,69.19744,69.19465], "fy":[-15.52718,-15.16245,-15.57495,-15.51716]}, + {"t":0.2896, "x":3.45187, "y":2.41455, "heading":3.03458, "vx":0.96667, "vy":-0.34728, "omega":-4.99756, "ax":1.43015, "ay":-0.44955, "alpha":0.0, "fx":[22.79712,22.7891,22.87369,22.35909], "fy":[-6.88877,-7.23936,-7.24611,-7.17356]}, + {"t":0.31029, "x":3.47217, "y":2.40727, "heading":2.93121, "vx":0.99625, "vy":-0.35658, "omega":-4.99756, "ax":-0.04082, "ay":0.12186, "alpha":0.0, "fx":[-0.64993,-0.65084,-0.64622,-0.64532], "fy":[1.93051,1.95761,1.92181,1.92878]}, + {"t":0.33097, "x":3.49277, "y":2.39992, "heading":2.82783, "vx":0.99541, "vy":-0.35406, "omega":-4.99756, "ax":-1.43475, "ay":0.69067, "alpha":0.0, "fx":[-22.88296,-22.8388,-22.98272,-22.40614], "fy":[10.73048,11.05316,11.09926,10.97676]}, + {"t":0.35166, "x":3.51306, "y":2.39274, "heading":2.72445, "vx":0.96573, "vy":-0.33977, "omega":-4.99756, "ax":-4.39509, "ay":1.19221, "alpha":0.0, "fx":[-69.74463,-69.73777,-69.796,-69.82292], "fy":[19.017,18.66982,19.01389,19.00789]}, + {"t":0.37234, "x":3.53209, "y":2.38597, "heading":2.62107, "vx":0.87481, "vy":-0.31511, "omega":-4.99756, "ax":-4.09219, "ay":1.73626, "alpha":0.0, "fx":[-65.28419,-65.09613,-65.31388,-64.17189], "fy":[26.91218,27.73718,27.86241,27.74562]}, + {"t":0.39303, "x":3.54931, "y":2.37982, "heading":2.51769, "vx":0.79016, "vy":-0.2792, "omega":-4.99756, "ax":-6.18281, "ay":1.89489, "alpha":-0.00002, "fx":[-98.14153,-98.20728,-98.12837,-98.14927], "fy":[30.12994,29.8043,30.22728,30.16938]}, + {"t":0.41371, "x":3.56433, "y":2.37445, "heading":2.41432, "vx":0.66227, "vy":-0.24, "omega":-4.99756, "ax":-5.98321, "ay":2.46223, "alpha":0.00003, "fx":[-95.32989,-95.1031,-95.11476,-94.4037], "fy":[38.48983,39.15113,39.21907,39.4986]}, + {"t":0.4344, "x":3.57675, "y":2.37001, "heading":2.31094, "vx":0.5385, "vy":-0.18907, "omega":-4.99756, "ax":-6.64311, "ay":2.27, "alpha":-0.00003, "fx":[-105.45111,-105.52631,-105.43637,-105.44327], "fy":[36.07818,35.83318,36.12944,36.11071]}, + {"t":0.45508, "x":3.58647, "y":2.36659, "heading":2.20756, "vx":0.40108, "vy":-0.14211, "omega":-4.99756, "ax":-6.67413, "ay":2.69485, "alpha":0.00003, "fx":[-106.10307,-105.98489,-106.00602,-105.73295], "fy":[42.47632,42.85256,42.65175,43.15006]}, + {"t":0.47577, "x":3.59334, "y":2.36423, "heading":2.10418, "vx":0.26302, "vy":-0.08636, "omega":-4.99756, "ax":-6.89209, "ay":2.44516, "alpha":0.01912, "fx":[-109.4832,-109.49577,-109.37356,-109.31527], "fy":[38.63819,38.63176,38.94065,39.06403]}, + {"t":0.49646, "x":3.59731, "y":2.36296, "heading":2.0008, "vx":0.12046, "vy":-0.03578, "omega":-4.99717, "ax":-4.05635, "ay":1.29776, "alpha":23.3761, "fx":[-112.28964,26.7308,-53.5522,-118.47896], "fy":[-56.78504,-25.60738,114.63647,50.16751]}, + {"t":0.51714, "x":3.59893, "y":2.3625, "heading":1.89743, "vx":0.03655, "vy":-0.00894, "omega":-4.51362, "ax":-0.58012, "ay":0.13217, "alpha":36.01263, "fx":[-66.96027,113.25426,33.69065,-116.82384], "fy":[-115.33251,-69.93992,129.01934,64.64657]}, + {"t":0.53783, "x":3.59956, "y":2.36234, "heading":1.80407, "vx":0.02455, "vy":-0.00621, "omega":-3.76867, "ax":-0.34773, "ay":0.08188, "alpha":36.31709, "fx":[-71.50927,107.27238,52.86038,-110.70522], "fy":[-113.39676,-80.27646,123.18125,75.69158]}, + {"t":0.55851, "x":3.6, "y":2.36223, "heading":1.72611, "vx":0.01736, "vy":-0.00451, "omega":-3.01743, "ax":-0.26194, "ay":0.06493, "alpha":36.4138, "fx":[-78.00867,101.06474,64.90727,-104.59754], "fy":[-109.34137,-88.40892,117.57668,84.29693]}, + {"t":0.5792, "x":3.6003, "y":2.36215, "heading":1.66369, "vx":0.01194, "vy":-0.00317, "omega":-2.26418, "ax":-0.21728, "ay":0.05602, "alpha":36.46087, "fx":[-83.61401,95.55185,73.44506,-99.18106], "fy":[-105.28554,-94.54854,112.60203,90.78918]}, + {"t":0.59988, "x":3.6005, "y":2.3621, "heading":1.61686, "vx":0.00744, "vy":-0.00201, "omega":-1.50997, "ax":-0.18942, "ay":0.0506, "alpha":36.48867, "fx":[-87.82488,91.17701,79.43234,-94.81302], "fy":[-101.90579,-98.89374,108.56379,95.44908]}, + {"t":0.62057, "x":3.60061, "y":2.36207, "heading":1.58562, "vx":0.00352, "vy":-0.00096, "omega":-0.75517, "ax":-0.17038, "ay":0.04655, "alpha":36.50702, "fx":[-90.55911,88.18236,83.3044,-91.74709], "fy":[-99.5572,-101.65061,105.69288,98.47072]}, + {"t":0.64126, "x":3.60065, "y":2.36206, "heading":1.57, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/anthony.traj b/src/main/deploy/choreo/anthony.traj new file mode 100644 index 0000000..4f9589c --- /dev/null +++ b/src/main/deploy/choreo/anthony.traj @@ -0,0 +1,99 @@ +{ + "name":"anthony", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.5258588790893555, "y":4.001440048217773, "heading":3.141592653589793, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8122551441192627, "y":1.0705667734146118, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":5.0}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":4.0}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":10.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.5258588790893555 m", "val":3.5258588790893555}, "y":{"exp":"4.001440048217773 m", "val":4.001440048217773}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8122551441192627 m", "val":2.8122551441192627}, "y":{"exp":"1.0705667734146118 m", "val":1.0705667734146118}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"5 m / s", "val":5.0}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"4 m / s ^ 2", "val":4.0}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"10 rad / s", "val":10.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.263525, + "y":0.301625 + }, + "backLeft":{ + "x":-0.263525, + "y":0.301625 + }, + "mass":63.502931800000006, + "inertia":5.903712368280681, + "gearing":5.27, + "radius":0.0508, + "vmax":555.0147021341968, + "tmax":1.3, + "cof":1.0, + "bumper":{ + "front":0.4460875, + "side":0.4079875, + "back":0.4460875 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.73787], + "samples":[ + {"t":0.0, "x":3.52586, "y":4.00144, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.94602, "ay":-3.88481, "alpha":0.02889, "fx":[-15.0042,-15.05957,-15.00559,-15.00584], "fy":[-61.85087,-61.14823,-61.84675,-61.85126]}, + {"t":0.04965, "x":3.52469, "y":3.99665, "heading":3.14159, "vx":-0.04697, "vy":-0.19289, "omega":0.00143, "ax":-0.94621, "ay":-3.88557, "alpha":0.02063, "fx":[-15.01258,-15.04291,-15.01575,-15.016], "fy":[-61.80837,-61.32506,-61.80273,-61.80879]}, + {"t":0.09931, "x":3.52119, "y":3.98228, "heading":-3.14152, "vx":-0.09396, "vy":-0.38583, "omega":0.00246, "ax":-0.94608, "ay":-3.88554, "alpha":-0.02734, "fx":[-15.05754,-15.02517,-15.03468,-14.96151], "fy":[-61.28787,-61.81408,-61.81427,-61.82692]}, + {"t":0.14896, "x":3.51536, "y":3.95834, "heading":-3.1414, "vx":-0.14093, "vy":-0.57876, "omega":0.0011, "ax":-0.94619, "ay":-3.88544, "alpha":0.02632, "fx":[-15.00661,-15.04991,-15.01466,-15.01497], "fy":[-61.83909,-61.22903,-61.82932,-61.83954]}, + {"t":0.19861, "x":3.5072, "y":3.92481, "heading":-3.14134, "vx":-0.18791, "vy":-0.77168, "omega":0.00241, "ax":-0.94622, "ay":-3.88536, "alpha":-0.03314, "fx":[-15.05753,-15.01066,-15.02547,-14.99405], "fy":[-61.15544,-61.85456,-61.85462,-61.86691]}, + {"t":0.24827, "x":3.4967, "y":3.8817, "heading":-3.14123, "vx":-0.2349, "vy":-0.9646, "omega":0.00076, "ax":-0.94618, "ay":-3.88527, "alpha":0.02812, "fx":[-15.00367,-15.04608,-15.01757,-15.01774], "fy":[-61.84297,-61.21107,-61.82898,-61.84325]}, + {"t":0.29792, "x":3.48387, "y":3.82902, "heading":-3.14119, "vx":-0.28188, "vy":-1.15752, "omega":0.00216, "ax":-0.94585, "ay":-3.88524, "alpha":-0.03884, "fx":[-15.06975,-15.01927,-15.0396,-14.93543], "fy":[-61.10458,-61.86619,-61.86598,-61.88765]}, + {"t":0.34757, "x":3.46871, "y":3.76675, "heading":-3.14108, "vx":-0.32884, "vy":-1.35043, "omega":0.00023, "ax":-0.94615, "ay":-3.88504, "alpha":0.03159, "fx":[-14.99427,-15.05936,-15.01495,-15.01477], "fy":[-61.86154,-61.14515,-61.84321,-61.86141]}, + {"t":0.39723, "x":3.45122, "y":3.69491, "heading":-3.14107, "vx":-0.37582, "vy":-1.54334, "omega":0.0018, "ax":-0.94593, "ay":-3.88493, "alpha":-0.05179, "fx":[-15.10689,-15.01676,-15.04255,-14.90332], "fy":[-60.9298,-61.91583,-61.9151,-61.9437]}, + {"t":0.44688, "x":3.43139, "y":3.61349, "heading":-3.14098, "vx":-0.42279, "vy":-1.73624, "omega":-0.00077, "ax":-0.94604, "ay":-3.8847, "alpha":0.03676, "fx":[-14.98953,-15.05794,-15.01455,-15.0141], "fy":[-61.88389,-61.05945,-61.86328,-61.88339]}, + {"t":0.49653, "x":3.40923, "y":3.52249, "heading":-3.14102, "vx":-0.46976, "vy":-1.92913, "omega":0.00105, "ax":-0.94577, "ay":-3.88451, "alpha":-0.03195, "fx":[-15.09244,-15.04096,-15.06764,-14.8583], "fy":[-61.27524,-61.78955,-61.78864,-61.82418]}, + {"t":0.54619, "x":3.38474, "y":3.42192, "heading":-3.14097, "vx":-0.51672, "vy":-2.12201, "omega":-0.00053, "ax":-0.94618, "ay":-3.88406, "alpha":0.02955, "fx":[-14.99356,-15.05155,-15.02042,-15.01982], "fy":[-61.82947,-61.18253,-61.80853,-61.82877]}, + {"t":0.59584, "x":3.35791, "y":3.31176, "heading":-3.14099, "vx":-0.56371, "vy":-2.31486, "omega":0.00093, "ax":-0.94526, "ay":-3.8838, "alpha":-0.07469, "fx":[-15.15044,-15.05098,-15.08144,-14.74413], "fy":[-60.68278,-61.96979,-61.9686,-62.01156]}, + {"t":0.64549, "x":3.32876, "y":3.19203, "heading":-3.14095, "vx":-0.61064, "vy":-2.50771, "omega":-0.00277, "ax":-0.94668, "ay":-3.88273, "alpha":0.08631, "fx":[-14.98381,-15.1287,-15.00227,-15.00204], "fy":[-62.1553,-60.11333,-62.14101,-62.15499]}, + {"t":0.69515, "x":3.29727, "y":3.06273, "heading":-3.14108, "vx":-0.65765, "vy":-2.7005, "omega":0.00151, "ax":-0.94361, "ay":-3.88226, "alpha":-0.07081, "fx":[-15.12575,-15.03957,-15.06578,-14.69069], "fy":[-60.75484,-61.91408,-61.91311,-61.95302]}, + {"t":0.7448, "x":3.26345, "y":2.92386, "heading":-3.14101, "vx":-0.7045, "vy":-2.89326, "omega":-0.002, "ax":-0.94987, "ay":-3.8783, "alpha":0.05016, "fx":[-15.05272,-15.1357,-15.06572,-15.06564], "fy":[-61.868,-60.68938,-61.85825,-61.86789]}, + {"t":0.79445, "x":3.2273, "y":2.77542, "heading":-3.14111, "vx":-0.75166, "vy":-3.08583, "omega":0.00049, "ax":-0.92431, "ay":-3.87716, "alpha":-0.15965, "fx":[-14.89262,-14.82836,-14.85719,-14.11843], "fy":[-59.47014,-62.23435,-62.23381,-62.27255]}, + {"t":0.84411, "x":3.18884, "y":2.61742, "heading":-3.14108, "vx":-0.79756, "vy":-3.27835, "omega":-0.00744, "ax":0.00503, "ay":-0.00207, "alpha":-0.00081, "fx":[0.07776,0.07776,0.08181,0.08181], "fy":[-0.03065,-0.03742,-0.03282,-0.03066]}, + {"t":0.89376, "x":3.14925, "y":2.45463, "heading":-3.14145, "vx":-0.79731, "vy":-3.27845, "omega":-0.00748, "ax":0.92227, "ay":3.87764, "alpha":0.16361, "fx":[14.86484,14.79985,14.83392,14.06846], "fy":[59.42263,62.25915,62.25842,62.30162]}, + {"t":0.94341, "x":3.11079, "y":2.29663, "heading":3.14136, "vx":-0.75152, "vy":-3.08591, "omega":0.00064, "ax":0.94759, "ay":3.87886, "alpha":-0.05315, "fx":[15.0119,15.09814,15.03243,15.03204], "fy":[61.89091,60.66017,61.87775,61.89026]}, + {"t":0.99307, "x":3.07465, "y":2.14818, "heading":3.14139, "vx":-0.70447, "vy":-2.89331, "omega":-0.002, "ax":0.94368, "ay":3.88224, "alpha":0.07166, "fx":[15.12622,15.03894,15.0701,14.69118], "fy":[60.73391,61.91985,61.91848,61.96168]}, + {"t":1.04272, "x":3.04083, "y":2.00931, "heading":3.14129, "vx":-0.65761, "vy":-2.70055, "omega":0.00156, "ax":0.94666, "ay":3.88273, "alpha":-0.0879, "fx":[14.97894,15.12554,15.00581,15.00514], "fy":[62.16098,60.10136,62.14264,62.16001]}, + {"t":1.09237, "x":3.00934, "y":1.88, "heading":3.14137, "vx":-0.6106, "vy":-2.50776, "omega":-0.0028, "ax":0.94524, "ay":3.88381, "alpha":0.07322, "fx":[15.15012,15.05233,15.08876,14.73413], "fy":[60.70708,61.96054,61.95883,62.00667]}, + {"t":1.14203, "x":2.98019, "y":1.76027, "heading":3.14123, "vx":-0.56367, "vy":-2.31491, "omega":0.00083, "ax":0.94601, "ay":3.8841, "alpha":-0.03066, "fx":[14.98563,15.04478,15.02251,15.02135], "fy":[61.83166,61.18481,61.80534,61.83018]}, + {"t":1.19168, "x":2.95337, "y":1.65011, "heading":3.14127, "vx":-0.5167, "vy":-2.12205, "omega":-0.00069, "ax":0.94579, "ay":3.8845, "alpha":0.03099, "fx":[15.09347,15.04267,15.07702,14.84752], "fy":[61.2895,61.78302,61.78147,61.82329]}, + {"t":1.24133, "x":2.92888, "y":1.54954, "heading":3.14124, "vx":-0.46974, "vy":-1.92918, "omega":0.00085, "ax":0.94593, "ay":3.88473, "alpha":-0.03748, "fx":[14.98199,15.05034,15.01916,15.01804], "fy":[61.8822,61.07406,61.85456,61.88079]}, + {"t":1.29099, "x":2.90672, "y":1.45854, "heading":3.14128, "vx":-0.42277, "vy":-1.73629, "omega":-0.00101, "ax":0.94588, "ay":3.88494, "alpha":0.05096, "fx":[15.10611,15.0164,15.05115,14.89253], "fy":[60.93846,61.91069,61.91027,61.94583]}, + {"t":1.34064, "x":2.8869, "y":1.37711, "heading":3.14123, "vx":-0.3758, "vy":-1.54339, "omega":0.00152, "ax":0.94606, "ay":3.88506, "alpha":-0.03335, "fx":[14.98549,15.05171,15.02063,15.01964], "fy":[61.86475,61.14671,61.83766,61.86362]}, + {"t":1.39029, "x":2.8694, "y":1.30527, "heading":3.14131, "vx":-0.32883, "vy":-1.35048, "omega":-0.00014, "ax":0.94585, "ay":3.88524, "alpha":0.03752, "fx":[15.06763,15.01825,15.05072,14.92745], "fy":[61.11259,61.86087,61.8598,61.89114]}, + {"t":1.43995, "x":2.85424, "y":1.243, "heading":3.1413, "vx":-0.28186, "vy":-1.15756, "omega":0.00173, "ax":0.94606, "ay":3.8853, "alpha":-0.02991, "fx":[14.99326,15.03614,15.02428,15.02358], "fy":[61.84474,61.21948,61.82,61.84395]}, + {"t":1.4896, "x":2.84141, "y":1.19031, "heading":3.14138, "vx":-0.23489, "vy":-0.96465, "omega":0.00024, "ax":0.94615, "ay":3.88537, "alpha":0.03142, "fx":[15.0528,15.00662,15.03619,14.988], "fy":[61.16167,61.84942,61.84853,61.87291]}, + {"t":1.53925, "x":2.83092, "y":1.1472, "heading":3.1414, "vx":-0.18791, "vy":-0.77172, "omega":0.0018, "ax":0.94612, "ay":3.88546, "alpha":-0.02914, "fx":[14.99562,15.03909,15.02375,15.02313], "fy":[61.84463,61.22781,61.82174,61.84392]}, + {"t":1.58891, "x":2.82275, "y":1.11368, "heading":3.14149, "vx":-0.14093, "vy":-0.5788, "omega":0.00035, "ax":0.94604, "ay":3.88555, "alpha":0.02495, "fx":[15.05103,15.01938,15.04661,14.95907], "fy":[61.29584,61.80758,61.80676,61.83364]}, + {"t":1.63856, "x":2.81692, "y":1.08973, "heading":3.1415, "vx":-0.09395, "vy":-0.38587, "omega":0.00159, "ax":0.94615, "ay":3.88558, "alpha":-0.02808, "fx":[14.99727,15.03898,15.02382,15.02313], "fy":[61.84109,61.24504,61.81948,61.84032]}, + {"t":1.68821, "x":2.81342, "y":1.07536, "heading":3.14158, "vx":-0.04698, "vy":-0.19294, "omega":0.0002, "ax":0.94607, "ay":3.88566, "alpha":-0.00398, "fx":[15.00454,15.01441,15.02994,15.02918], "fy":[61.69878,61.67565,61.67804,61.6981]}, + {"t":1.73787, "x":2.81226, "y":1.07057, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/java/frc/robot/AutoRoutines.java b/src/main/java/frc/robot/AutoRoutines.java new file mode 100644 index 0000000..7e236de --- /dev/null +++ b/src/main/java/frc/robot/AutoRoutines.java @@ -0,0 +1,787 @@ +package frc.robot; + +import choreo.auto.AutoChooser; +import choreo.auto.AutoFactory; +import choreo.auto.AutoRoutine; +import choreo.auto.AutoTrajectory; +import com.ctre.phoenix6.swerve.utility.WheelForceCalculator.Feedforwards; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.Swerve.Auto.Depot; +import frc.robot.Constants.Swerve.Auto.Intake; +import frc.robot.Constants.Swerve.Auto.MiscPaths; +import frc.robot.Constants.Swerve.Auto.Outpost; +import frc.robot.Constants.Swerve.Auto.ShootPos; +import frc.robot.commandGroups.IntakeToBumpDTP; +import frc.robot.commandGroups.ShootCommandGroups.ShootWithAim; +import frc.robot.commandGroups.ShootToBumpDTP; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import java.util.function.BooleanSupplier; + +public class AutoRoutines { + private final AutoFactory autoFactory; + private final AutoChooser autoChooser; + private final IntakeSubsystem intakeSubsystem; + private final ShooterSubsystem lebronShooterSubsystem; + private final HopperSubsystem hopperSubsystem; + private final CommandSwerveDrivetrain swerveSubsystem; + private final BooleanSupplier redSide; + + public AutoRoutines( + IntakeSubsystem intake, + ShooterSubsystem lebron, + HopperSubsystem hopper, + CommandSwerveDrivetrain swerve, + BooleanSupplier redSide) { + this.intakeSubsystem = intake; + this.lebronShooterSubsystem = lebron; + this.hopperSubsystem = hopper; + this.swerveSubsystem = swerve; + this.redSide = redSide; + + autoFactory = swerveSubsystem.createAutoFactory(); + autoChooser = new AutoChooser(); + addCommandstoAutoChooser(); + } + + private AutoTrajectory shoot(AutoRoutine routine, ShootPos type) { + if (type == null) return null; + + AutoTrajectory traj = routine.trajectory(type + ".traj"); + return traj; + } + + // Trajectory loading and specific cmds + private AutoTrajectory intake(AutoRoutine routine, Intake type) { + if (type == null) return null; + + AutoTrajectory traj = routine.trajectory(type + ".traj"); + + if (traj != null) { + traj.atTime("IntakeDown").onTrue(intakeSubsystem.intakeUntilInterruptedCommand()); + traj.atTime("IntakeUp").onTrue(intakeSubsystem.retractIntakeCommand()); + } + return traj; + } + + private AutoTrajectory depot(AutoRoutine routine, Depot type) { + if (type == null) return null; + + AutoTrajectory traj = routine.trajectory(type + ".traj"); + + if (traj != null) { + traj.atTime("IntakeDown").onTrue(intakeSubsystem.intakeUntilInterruptedCommand()); + traj.atTime("IntakeUp").onTrue(intakeSubsystem.retractIntakeCommand()); + } + return traj; + } + + private AutoTrajectory outpost(AutoRoutine routine, Outpost type) { + if (type == null) return null; + + AutoTrajectory traj = routine.trajectory(type + ".traj"); + + if (traj != null) { + traj.atTime("IntakeDown").onTrue(intakeSubsystem.intakeUntilInterruptedCommand()); + traj.atTime("IntakeUp").onTrue(intakeSubsystem.retractIntakeCommand()); + } + return traj; + } + + private AutoTrajectory miscPaths(AutoRoutine routine, MiscPaths type) { + if (type == null) return null; + + AutoTrajectory traj = routine.trajectory(type + ".traj"); + return traj; + } + + public Command returnBasicShoot(BooleanSupplier isRedSide) { + Command shoot = + new ShootWithAim( + () -> 0.0, + () -> 0.0, + lebronShooterSubsystem, + intakeSubsystem, + hopperSubsystem, + swerveSubsystem, + isRedSide, + () -> false) + .withTimeout(3.5); + + return shoot; + } + + public Command returnBasicShootLessTime(BooleanSupplier isRedSide) { + Command shoot = + new ShootWithAim( + () -> 0.0, + () -> 0.0, + lebronShooterSubsystem, + intakeSubsystem, + hopperSubsystem, + swerveSubsystem, + isRedSide, + () -> false) + .withTimeout(2.5); + + return shoot; + } + + public Command returnBasicShootMoreTime(BooleanSupplier isRedSide) { + Command shoot = + new ShootWithAim( + () -> 0.0, + () -> 0.0, + lebronShooterSubsystem, + intakeSubsystem, + hopperSubsystem, + swerveSubsystem, + isRedSide, + () -> false) + .withTimeout(4.2); + + return shoot; + } + + public Command driveForward(double time) { + return Commands.run( + () -> swerveSubsystem.applyFieldSpeeds(new ChassisSpeeds(3, 0, 0), new Feedforwards(4)), + swerveSubsystem) + .withTimeout(time) + .andThen( + () -> + swerveSubsystem.applyFieldSpeeds(new ChassisSpeeds(0, 0, 0), new Feedforwards(4))); + } + + public Command driveBackward(double time) { + return Commands.run( + () -> + swerveSubsystem.applyFieldSpeeds(new ChassisSpeeds(-3, 0, 0), new Feedforwards(4)), + swerveSubsystem) + .withTimeout(time) + .andThen( + () -> + swerveSubsystem.applyFieldSpeeds(new ChassisSpeeds(0, 0, 0), new Feedforwards(4))); + } + + public Command driveForwardSlower(double time) { + return Commands.run( + () -> + swerveSubsystem.applyFieldSpeeds(new ChassisSpeeds(2.7, 0, 0), new Feedforwards(4)), + swerveSubsystem) + .withTimeout(time); + // .andThen( + // () -> + // swerveSubsystem.applyFieldSpeeds(new ChassisSpeeds(0, 0, 0), new Feedforwards(4))); + } + + public Command driveBackwardSlower(double time) { + return Commands.run( + () -> + swerveSubsystem.applyFieldSpeeds( + new ChassisSpeeds(-2.7, 0, 0), new Feedforwards(4)), + swerveSubsystem) + .withTimeout(time); + // .andThen( + // () -> + // swerveSubsystem.applyFieldSpeeds(new ChassisSpeeds(0, 0, 0), new Feedforwards(4))); + } + + public Command driveToBumpAfterIntake(BooleanSupplier isRedSide, BooleanSupplier isLeftSide) { + // return new IntakeToBumpDTP(swerveSubsystem, isRedSide).withTimeout(0.5); + return new IntakeToBumpDTP(swerveSubsystem, isRedSide, isLeftSide); + } + + public Command driveToBumpAfterShoot(BooleanSupplier isRedSide, BooleanSupplier isLeftSide) { + // return new IntakeToBumpDTP(swerveSubsystem, isRedSide).withTimeout(0.5); + return new ShootToBumpDTP(swerveSubsystem, isRedSide, isLeftSide); + } + + // Middle + public AutoRoutine Nike() { + AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + + AutoTrajectory move = miscPaths(routine, Constants.Swerve.Auto.MiscPaths.Nike); + + routine.active().onTrue(Commands.sequence(move.resetOdometry(), move.cmd())); + + move.done().onTrue(returnBasicShoot(redSide)); + + return routine; + } + + // Sweeps + public AutoRoutine PedriShortRight() { + AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + + Command forward = + redSide.getAsBoolean() + ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) + : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); + Command backward = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + Command forward2 = + redSide.getAsBoolean() + ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) + : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); + Command backward2 = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + + AutoTrajectory intake1 = intake(routine, Constants.Swerve.Auto.Intake.RightIntakeSweepShort); + AutoTrajectory intakeToShoot1 = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); + AutoTrajectory shootToBump = + miscPaths(routine, Constants.Swerve.Auto.MiscPaths.RightShootToBump); + AutoTrajectory intake2 = intake(routine, Constants.Swerve.Auto.Intake.RightSecondDip); + AutoTrajectory intakeToShoot2 = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); + + routine.active().onTrue(Commands.sequence(forward, intake1.resetOdometry(), intake1.cmd())); + + intake1 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> false), + backward, + intakeToShoot1.resetOdometry(), + intakeToShoot1.cmd())); + + intakeToShoot1.done().onTrue(Commands.sequence(returnBasicShoot(redSide), shootToBump.cmd())); + + shootToBump.done().onTrue(Commands.sequence(forward2, intake2.resetOdometry(), intake2.cmd())); + + intake2 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> false), + backward2, + intakeToShoot2.resetOdometry(), + intakeToShoot2.cmd())); + + intakeToShoot2.done().onTrue(returnBasicShoot(redSide)); + + return routine; + } + + public AutoRoutine PedriShortLeft() { + AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + + Command forward = + redSide.getAsBoolean() + ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) + : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); + Command backward = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + Command forward2 = + redSide.getAsBoolean() + ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) + : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); + Command backward2 = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + + AutoTrajectory intake1 = intake(routine, Constants.Swerve.Auto.Intake.LeftIntakeSweepShort); + AutoTrajectory intakeToShoot1 = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); + AutoTrajectory shootToBump = + miscPaths(routine, Constants.Swerve.Auto.MiscPaths.LeftShootToBump); + AutoTrajectory intake2 = intake(routine, Constants.Swerve.Auto.Intake.LeftSecondDip); + AutoTrajectory intakeToShoot2 = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); + + routine.active().onTrue(Commands.sequence(forward, intake1.resetOdometry(), intake1.cmd())); + + intake1 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> true), + backward, + intakeToShoot1.resetOdometry(), + intakeToShoot1.cmd())); + + intakeToShoot1.done().onTrue(Commands.sequence(returnBasicShoot(redSide), shootToBump.cmd())); + + shootToBump.done().onTrue(Commands.sequence(forward2, intake2.resetOdometry(), intake2.cmd())); + + intake2 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> true), + backward2, + intakeToShoot2.resetOdometry(), + intakeToShoot2.cmd())); + + intakeToShoot2.done().onTrue(returnBasicShoot(redSide)); + + return routine; + } + + public AutoRoutine PedriShortRightWait() { + AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + + Command forward = + redSide.getAsBoolean() + ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) + : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); + Command backward = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + Command forward2 = + redSide.getAsBoolean() + ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) + : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); + Command backward2 = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + + AutoTrajectory intake1 = intake(routine, Constants.Swerve.Auto.Intake.RightIntakeSweepShort); + AutoTrajectory intakeToShoot1 = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); + AutoTrajectory shootToBump = + miscPaths(routine, Constants.Swerve.Auto.MiscPaths.RightShootToBump); + AutoTrajectory intake2 = intake(routine, Constants.Swerve.Auto.Intake.RightSecondDip); + AutoTrajectory intakeToShoot2 = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); + + routine + .active() + .onTrue( + Commands.sequence( + Commands.waitSeconds(Constants.Swerve.Auto.WAIT_TIME_FOR_ALLIANCE), + forward, + intake1.resetOdometry(), + intake1.cmd())); + + intake1 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> false), + backward, + intakeToShoot1.resetOdometry(), + intakeToShoot1.cmd())); + + intakeToShoot1.done().onTrue(Commands.sequence(returnBasicShoot(redSide), shootToBump.cmd())); + + shootToBump.done().onTrue(Commands.sequence(forward2, intake2.resetOdometry(), intake2.cmd())); + + intake2 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> false), + backward2, + intakeToShoot2.resetOdometry(), + intakeToShoot2.cmd())); + + intakeToShoot2.done().onTrue(returnBasicShoot(redSide)); + + return routine; + } + + public AutoRoutine PedriShortLeftWait() { + AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + + Command forward = + redSide.getAsBoolean() + ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) + : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); + Command backward = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + Command forward2 = + redSide.getAsBoolean() + ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) + : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); + Command backward2 = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + + AutoTrajectory intake1 = intake(routine, Constants.Swerve.Auto.Intake.LeftIntakeSweepShort); + AutoTrajectory intakeToShoot1 = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); + AutoTrajectory shootToBump = + miscPaths(routine, Constants.Swerve.Auto.MiscPaths.LeftShootToBump); + AutoTrajectory intake2 = intake(routine, Constants.Swerve.Auto.Intake.LeftSecondDip); + AutoTrajectory intakeToShoot2 = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); + + routine + .active() + .onTrue( + Commands.sequence( + Commands.waitSeconds(Constants.Swerve.Auto.WAIT_TIME_FOR_ALLIANCE), + forward, + intake1.resetOdometry(), + intake1.cmd())); + + intake1 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> true), + backward, + intakeToShoot1.resetOdometry(), + intakeToShoot1.cmd())); + + intakeToShoot1.done().onTrue(Commands.sequence(returnBasicShoot(redSide), shootToBump.cmd())); + + shootToBump.done().onTrue(Commands.sequence(forward2, intake2.resetOdometry(), intake2.cmd())); + + intake2 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> true), + backward2, + intakeToShoot2.resetOdometry(), + intakeToShoot2.cmd())); + + intakeToShoot2.done().onTrue(returnBasicShoot(redSide)); + + return routine; + } + + public AutoRoutine HubSweepRight() { + AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + + Command forward = + redSide.getAsBoolean() + ? driveBackwardSlower(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS_SLOWER) + : driveForwardSlower(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS_SLOWER); + Command backward = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + Command forward2 = + redSide.getAsBoolean() + ? driveBackwardSlower(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS_SLOWER) + : driveForwardSlower(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS_SLOWER); + Command backward2 = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + + AutoTrajectory intake1 = intake(routine, Constants.Swerve.Auto.Intake.HubRight); + AutoTrajectory intakeToShoot1 = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); + AutoTrajectory shootToBump = + miscPaths(routine, Constants.Swerve.Auto.MiscPaths.RightShootToBump); + AutoTrajectory intake2 = intake(routine, Constants.Swerve.Auto.Intake.HubRight); + AutoTrajectory intakeToShoot2 = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); + + routine + .active() + .onTrue( + Commands.sequence( + Commands.waitSeconds(Constants.Swerve.Auto.WAIT_TIME_FOR_ALLIANCE), + forward, + intake1.resetOdometry(), + intake1.cmd())); + + intake1 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> false), + backward, + intakeToShoot1.resetOdometry(), + intakeToShoot1.cmd())); + + intakeToShoot1.done().onTrue(Commands.sequence(returnBasicShoot(redSide), shootToBump.cmd())); + + shootToBump.done().onTrue(Commands.sequence(forward2, intake2.resetOdometry(), intake2.cmd())); + + intake2 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> false), + backward2, + intakeToShoot2.resetOdometry(), + intakeToShoot2.cmd())); + + intakeToShoot2.done().onTrue(returnBasicShoot(redSide)); + + return routine; + } + + public AutoRoutine HubSweepLeft() { + AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + + Command forward = + redSide.getAsBoolean() + ? driveBackwardSlower(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS_SLOWER) + : driveForwardSlower(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS_SLOWER); + Command backward = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + Command forward2 = + redSide.getAsBoolean() + ? driveBackwardSlower(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS_SLOWER) + : driveForwardSlower(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS_SLOWER); + Command backward2 = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + + AutoTrajectory intake1 = intake(routine, Constants.Swerve.Auto.Intake.HubLeft); + AutoTrajectory intakeToShoot1 = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); + AutoTrajectory shootToBump = + miscPaths(routine, Constants.Swerve.Auto.MiscPaths.LeftShootToBump); + AutoTrajectory intake2 = intake(routine, Constants.Swerve.Auto.Intake.HubLeft); + AutoTrajectory intakeToShoot2 = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); + + routine + .active() + .onTrue( + Commands.sequence( + Commands.waitSeconds(Constants.Swerve.Auto.WAIT_TIME_FOR_ALLIANCE), + forward, + intake1.resetOdometry(), + intake1.cmd())); + + intake1 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> true), + backward, + intakeToShoot1.resetOdometry(), + intakeToShoot1.cmd())); + + intakeToShoot1.done().onTrue(Commands.sequence(returnBasicShoot(redSide), shootToBump.cmd())); + + shootToBump.done().onTrue(Commands.sequence(forward2, intake2.resetOdometry(), intake2.cmd())); + + intake2 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> true), + backward2, + intakeToShoot2.resetOdometry(), + intakeToShoot2.cmd())); + + intakeToShoot2.done().onTrue(returnBasicShoot(redSide)); + + return routine; + } + + // //Outpost and Depot + public AutoRoutine DrakeOutpostShort() { + AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + + AutoTrajectory outpostIntake = outpost(routine, Constants.Swerve.Auto.Outpost.OutpostStart); + + routine.active().onTrue(Commands.sequence(outpostIntake.resetOdometry(), outpostIntake.cmd())); + + outpostIntake.done().onTrue(returnBasicShoot(redSide)); + + return routine; + } + + public AutoRoutine DrakeDepotShort() { + AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + + AutoTrajectory depotIntake = depot(routine, Constants.Swerve.Auto.Depot.DepotStart); + + routine.active().onTrue(Commands.sequence(depotIntake.resetOdometry(), depotIntake.cmd())); + + depotIntake.done().onTrue(returnBasicShoot(redSide)); + + return routine; + } + + public AutoRoutine DrakeOutpostLong() { + AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + + AutoTrajectory outpostIntake = outpost(routine, Constants.Swerve.Auto.Outpost.OutpostStart); + AutoTrajectory depotIntake = depot(routine, Constants.Swerve.Auto.Depot.DepotSweep); + Command shoot = returnBasicShootLessTime(redSide); + Trigger shootIsFinished = new Trigger(() -> shoot.isFinished()); + + // routine.active().onTrue(Commands.sequence(outpostIntake.resetOdometry(), + // outpostIntake.cmd())); + // outpostIntake.done().onTrue(Commands.sequence(returnBasicShoot(redSide).asProxy(), + // depotIntake.cmd())); + // depotIntake.done().onTrue(returnBasicShoot(redSide)); + routine.active().onTrue(Commands.sequence(outpostIntake.resetOdometry(), outpostIntake.cmd())); + outpostIntake.done().onTrue(shoot); + shootIsFinished.onTrue(depotIntake.cmd()); + depotIntake.done().onTrue(returnBasicShoot(redSide)); + + return routine; + } + + public AutoRoutine DrakeDepotLong() { + AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + + AutoTrajectory depotIntake = depot(routine, Constants.Swerve.Auto.Depot.DepotStart); + AutoTrajectory outpostIntake = outpost(routine, Constants.Swerve.Auto.Outpost.OutpostSweep); + Command shoot = returnBasicShoot(redSide); + Trigger shootIsFinished = new Trigger(() -> shoot.isFinished()); + + // routine.active().onTrue(Commands.sequence(depotIntake.resetOdometry(), depotIntake.cmd())); + // depotIntake.done().onTrue(Commands.sequence(returnBasicShoot(redSide).asProxy(), + // outpostIntake.cmd())); + // outpostIntake.done().onTrue(returnBasicShoot(redSide)); + routine.active().onTrue(Commands.sequence(depotIntake.resetOdometry(), depotIntake.cmd())); + depotIntake.done().onTrue(shoot); + shootIsFinished.onTrue(outpostIntake.cmd()); + outpostIntake.done().onTrue(returnBasicShoot(redSide)); + + return routine; + } + + public AutoRoutine PedriShortRightHub() { + AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + + Command forward = + redSide.getAsBoolean() + ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) + : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); + Command backward = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + Command forward2 = + redSide.getAsBoolean() + ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) + : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); + Command backward2 = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + + AutoTrajectory intake1 = intake(routine, Constants.Swerve.Auto.Intake.RightIntakeSweepShort); + AutoTrajectory intakeToShoot1 = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); + AutoTrajectory shootToBump = + miscPaths(routine, Constants.Swerve.Auto.MiscPaths.RightShootToBump); + AutoTrajectory intake2 = intake(routine, Constants.Swerve.Auto.Intake.HubRight); + AutoTrajectory intakeToShoot2 = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); + + routine.active().onTrue(Commands.sequence(forward, intake1.resetOdometry(), intake1.cmd())); + + intake1 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> false), + backward, + intakeToShoot1.resetOdometry(), + intakeToShoot1.cmd())); + + intakeToShoot1 + .done() + .onTrue(Commands.sequence(returnBasicShootMoreTime(redSide), shootToBump.cmd())); + + shootToBump.done().onTrue(Commands.sequence(forward2, intake2.resetOdometry(), intake2.cmd())); + + intake2 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> false), + backward2, + intakeToShoot2.resetOdometry(), + intakeToShoot2.cmd())); + + intakeToShoot2.done().onTrue(returnBasicShootMoreTime(redSide)); + + return routine; + } + + public AutoRoutine PedriShortLeftHub() { + AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + + Command forward = + redSide.getAsBoolean() + ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) + : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); + Command backward = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + Command forward2 = + redSide.getAsBoolean() + ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) + : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); + Command backward2 = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + + AutoTrajectory intake1 = intake(routine, Constants.Swerve.Auto.Intake.LeftIntakeSweepShort); + AutoTrajectory intakeToShoot1 = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); + AutoTrajectory shootToBump = + miscPaths(routine, Constants.Swerve.Auto.MiscPaths.LeftShootToBump); + AutoTrajectory intake2 = intake(routine, Constants.Swerve.Auto.Intake.HubLeft); + AutoTrajectory intakeToShoot2 = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); + + routine.active().onTrue(Commands.sequence(forward, intake1.resetOdometry(), intake1.cmd())); + + intake1 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> true), + backward, + intakeToShoot1.resetOdometry(), + intakeToShoot1.cmd())); + + intakeToShoot1 + .done() + .onTrue(Commands.sequence(returnBasicShootMoreTime(redSide), shootToBump.cmd())); + + shootToBump.done().onTrue(Commands.sequence(forward2, intake2.resetOdometry(), intake2.cmd())); + + intake2 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> true), + backward2, + intakeToShoot2.resetOdometry(), + intakeToShoot2.cmd())); + + intakeToShoot2.done().onTrue(returnBasicShootMoreTime(redSide)); + + return routine; + } + + // Add paths to chooser + public void addCommandstoAutoChooser() { + autoChooser.addRoutine("Right", () -> PedriShortRight()); + autoChooser.addRoutine("Left", () -> PedriShortLeft()); + autoChooser.addRoutine("Right Wait", () -> PedriShortRightWait()); + autoChooser.addRoutine("Left Wait", () -> PedriShortLeftWait()); + // autoChooser.addRoutine("Right then Hub", () -> PedriShortRightHub()); + // autoChooser.addRoutine("Left then Hub", () -> PedriShortLeftHub()); + autoChooser.addRoutine("Hub Sweep Left Wait", () -> HubSweepLeft()); + autoChooser.addRoutine("Hub Sweep Right Wait", () -> HubSweepRight()); + + autoChooser.addRoutine("Just Outpost", () -> DrakeOutpostShort()); + autoChooser.addRoutine("Just Depot", () -> DrakeDepotShort()); + autoChooser.addRoutine("Outpost to Depot", () -> DrakeOutpostLong()); + autoChooser.addRoutine("Depot to Outpost", () -> DrakeDepotLong()); + + autoChooser.addRoutine("We are genuinely the worst robot on the field", () -> Nike()); + } + + public AutoChooser getAutoChooser() { + return autoChooser; + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..fe1260d --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,885 @@ +package frc.robot; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.CANBus; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +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.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N8; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import frc.robot.util.VisionCameraConstants; + +public final class Constants { + public static final boolean hopperOnRobot = true; + public static final boolean intakeOnRobot = true; + public static final boolean visionOnRobot = true; + public static final boolean fuelGaugeOnRobot = false; + public static final boolean intakeVisionOnRobot = false; + public static final boolean shooterOnRobot = true; + + public static class OperatorConstants { + public static final int kDriverControllerPort = 0; + } + + public static final class Intake { + public static final double HAS_PIECE_CURRENT_AMPS = 35.0; + public static final double HAS_PIECE_THRESHOLD_SEC = 0.15; + + public static final class Arm { + public static final int CAN_ID = 14; + public static final int ENCODER_PORT = 15; + + // Current Limits + public static final double ARM_POS_RETRACTED = 122.0; + public static final double ARM_POS_EXTENDED = 27.3; + public static final double ARM_POS_MAX = ARM_POS_RETRACTED; + public static final double ARM_POS_MIN = 16.4; + public static final double ARM_POS_IDLE = 72.0; + + public static final double POSITION_TOLERANCE_DEGREES = 1.0; + + public static final double POWER_RETRACT_TORQUE_CURRENT = 45.0; + public static final double POWER_RETRACT_DELAY = 0.2; + + public static final double kV = 0.124; + public static final double kP = 63.0; // 63.0; + public static final double kI = 0.1; // 0.1; + public static final double kD = 0.0; + public static final double kG = 1.15; // 1.15; // 0.69 recalc + public static final double kS = 0.4; // 0.4; + + public static final double mmcV = 6.0; + public static final double mmcA = 14.0; + + public static final double STATOR_CURRENT_LIMIT = 80.0; + public static final double SUPPLY_CURRENT_LIMIT = 60.0; + + public static final double MOTOR_ROTS_PER_ARM_ROT = + (12.0 / 1.0) * (42.0 / 36.0) * (30.0 / 18.0) * (32.0 / 20.0); + public static final double ARM_ROTS_PER_MOTOR_ROT = 1.0 / MOTOR_ROTS_PER_ARM_ROT; + public static final double ARM_DEGREES_PER_MOTOR_ROT = 360.0 / MOTOR_ROTS_PER_ARM_ROT; + public static final double MOTOR_ROTS_PER_ARM_DEGREE = MOTOR_ROTS_PER_ARM_ROT / 360.0; + public static final double CANCODER_ROTS_PER_ARM_ROT = (8.0 / 3.0); + public static final double ARM_ROTS_PER_CANCODER_ROT = 1.0 / CANCODER_ROTS_PER_ARM_ROT; + public static final double ENCODER_OFFSET = 0.18; // -0.55 + public static final double GRAVITY_POS_OFFSET = -2 / 360.0; + } + + public static final class Rollers { + // Hardware Configuration + public static final int CAN_ID = 16; + + public static final double TOLERANCE_MOTOR_ROTS_PER_SEC = 2.0; + + public static final double kV = 0.1167; + public static final double kP = 0.0; + public static final double kI = 0.0; + public static final double kD = 0.0; + + // Current Limits + public static final double STATOR_CURRENT_LIMIT = 120.0; + public static final double SUPPLY_CURRENT_LIMIT = 80.0; + + public static final double ROLLER_CIRCUMFERENCE_INCHES = 3.0 * Math.PI; + public static final double MOTOR_ROTS_PER_ROLLERS_ROT = 2.0; // 8.0 / 3.0; + public static final double ROLLER_ROTS_PER_MOTOR_ROT = 1.0 / MOTOR_ROTS_PER_ROLLERS_ROT; + public static final double DESIGNED_SURFACE_SPEED_FT_PER_SEC = 25.0; + public static final double DESIGNED_SURFACE_SPEED_METERS_PER_SEC = + DESIGNED_SURFACE_SPEED_FT_PER_SEC * 0.3048; + public static final double DESIGNED_SURFACE_SPEED_IN_PER_SEC = + DESIGNED_SURFACE_SPEED_FT_PER_SEC * 12.0; + + // public static final double TARGET_ROLLER_RPM = + // (DESIGNED_SURFACE_SPEED_IN_PER_SEC * 60.0) / ROLLER_CIRCUMFERENCE_INCHES; + public static final double TARGET_ROLLER_RPM = 2700; + public static final double TARGET_ROLLER_RPS = TARGET_ROLLER_RPM / 60.0; + public static final double TARGET_MOTOR_RPS = TARGET_ROLLER_RPS * MOTOR_ROTS_PER_ROLLERS_ROT; + + public static final double IDLE_ROLLER_VELO_RPS = 5.0; + } + } + + public static class Swerve { + public static final double FF_RADIUS_M2 = 0.1; + public static final double MAX_TRANSLATIONAL_MOVEMENT_SQUARED = 0.0625; + public static final SwerveType WHICH_SWERVE_ROBOT = SwerveType.COBRA; + public static final CANBus CAN_BUS = + new CANBus(WHICH_SWERVE_ROBOT.CANBUS_NAME, "./logs/example.hoot"); + // the distance over the bump in meters + + public static final double TARGET_POS_ERROR = 0.07; + public static final double TARGET_ANGLE_ERROR = 0.1; + public static final double MAX_HEADING_TRACKING_ROT_RATE_RADS_PER_SECOND = 4; + + public static enum SwerveLevel { + L2(6.75, 21.428571428571427), + L3(6.12, 21.428571428571427), + FIVEN_L3(5.2734375, 26.09090909091); + public final double DRIVE_GEAR_RATIO, STEER_GEAR_RATIO; + + SwerveLevel(double drive, double steer) { + DRIVE_GEAR_RATIO = drive; + STEER_GEAR_RATIO = steer; + } + } + + public static enum SwerveDrivePIDValues { + SERRANO(0.18014, 0d, 0d, -0.023265, 0.12681, 0.058864), + PROTO(0.053218, 0d, 0d, 0.19977, 0.11198, 0.0048619), + // JAMES_HARDEN(0.16901, 0d, 0d, 0.1593, 0.12143, 0.0091321); //0.041539 + // //0.12301 + JAMES_HARDEN(0.36, 0d, 0d, 0.2425, 0.11560693641, 0), // 0.041539 //0.12301 + COBRA(0.25, 0d, 0d, 0.2d, 0.124, 0d); // 0.041539 //0.12301 + public final double KP, KI, KD, KS, KV, KA; + + SwerveDrivePIDValues(double KP, double KI, double KD, double KS, double KV, double KA) { + this.KP = KP; + this.KI = KI; + this.KD = KD; + this.KS = KS; + this.KV = KV; + this.KA = KA; + } + } + + public static enum SwerveSteerPIDValues { + SERRANO(50d, 0d, 0.2, 0d, 1.5, 0d), + PROTO(20d, 0d, 0d, 0d, 0d, 0d), + JAMES_HARDEN(38.982d, 2.4768d, 0d, 0.23791d, 0d, 0.1151d), + COBRA(100d, 0d, 0.5, 0.1, 2.49, 0d); + public final double KP, KI, KD, KS, KV, KA; + + SwerveSteerPIDValues(double KP, double KI, double KD, double KS, double KV, double KA) { + this.KP = KP; + this.KI = KI; + this.KD = KD; + this.KS = KS; + this.KV = KV; + this.KA = KA; + } + } + + public static enum SwerveDriveToPosePIDValues { + SERRANO(3.067, 0, 0, 4.167, 0, 0, 3.667, 0, 0), + PROTO(0, 0, 0, 0, 0, 0, 0, 0, 0), + JAMES_HARDEN(0, 0, 0, 0, 0, 0, 0, 0, 0), + COBRA(3.467, 0, 0, 3.567, 0, 0, 2.867, 0, 0); + + public final double kPX; + public final double kIX; + public final double kDX; + public final double kPY; + public final double kIY; + public final double kDY; + public final double kPR; + public final double kIR; + public final double kDR; + + SwerveDriveToPosePIDValues( + double kPX, + double kIX, + double kDX, + double kPY, + double kIY, + double kDY, + double kPR, + double kIR, + double kDR) { + this.kPX = kPX; + this.kIX = kIX; + this.kDX = kDX; + this.kPY = kPY; + this.kIY = kIY; + this.kDY = kDY; + this.kPR = kPR; + this.kIR = kIR; + this.kDR = kDR; + } + } + + public static enum SwerveDriveToPoseProfileValues { + SERRANO(2, 2, 2, 2), + PROTO(0.5, 0.5, 0.2, 0.2), + JAMES_HARDEN(0.5, 0.5, 0.2, 0.2), + COBRA(5, 8, 1.9, 10); + public final double maxVelocityLinear, + maxAccelerationLinear, + maxVelocityAngular, + maxAccelerationAngular; + + SwerveDriveToPoseProfileValues( + double maxVelocityLinear, + double maxAccelerationLinear, + double maxVelocityAngular, + double maxAccelerationAngular) { + this.maxVelocityLinear = maxVelocityLinear; + this.maxAccelerationLinear = maxAccelerationLinear; + this.maxVelocityAngular = maxVelocityAngular; + this.maxAccelerationAngular = maxAccelerationAngular; + } + } + + public static enum ChoreoPIDValues { + SERRANO(0.1d, 0d, 0d, 0.1d, 0d, 0d, 3.867d, 0d, 0d), + PROTO(0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d), + JAMES_HARDEN(0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d), + COBRA(3.467, 0, 0, 3.567, 0, 0, 2.5, 0, 0); // 3.75 m/s^2 max accel while turning max, + public final double kPX, kIX, kDX, kPY, kIY, kDY, kPR, kIR, kDR; + + ChoreoPIDValues( + double kPX, + double kIX, + double kDX, + double kPY, + double kIY, + double kDY, + double kPR, + double kIR, + double kDR) { + this.kPX = kPX; + this.kIX = kIX; + this.kDX = kDX; + this.kPY = kPY; + this.kIY = kIY; + this.kDY = kDY; + this.kPR = kPR; + this.kIR = kIR; + this.kDR = kDR; + } + } + + public static enum RobotDimensions { + SERRANO(Inches.of(22.52), Inches.of(22.834)), // length, width + PROTO(Inches.of(22.52), Inches.of(22.834)), // length, width + JAMES_HARDEN(Inches.of(26.75), Inches.of(22.75)), // length, width + COBRA(Inches.of(29.0), Inches.of(26.0)); // length, width + public final Distance length, width; + + RobotDimensions(Distance length, Distance width) { + this.length = length; + this.width = width; + } + } + + public static enum BumperThickness { + SERRANO(Inches.of(2.625)), // thickness + PROTO(Inches.of(2.625)), // thickness + JAMES_HARDEN(Inches.of(3.313)), // thickness + COBRA(Inches.of(0)); // thickness + + public final Distance thickness; + + BumperThickness(Distance thickness) { + this.thickness = thickness; + } + } + + public static enum SwerveType { + SERRANO( + Rotations.of(-0.466552734375), // front left + Rotations.of(-0.436767578125), // front right + Rotations.of(-0.165283203125), // back left + Rotations.of(-0.336181640625), // back right + SwerveLevel.L3, // what level the swerve drive is + SwerveDrivePIDValues.SERRANO, + SwerveSteerPIDValues.SERRANO, + SwerveDriveToPosePIDValues.SERRANO, + SwerveDriveToPoseProfileValues.SERRANO, + ChoreoPIDValues.SERRANO, + RobotDimensions.SERRANO, + "Patrice the Pineapple", + BumperThickness.SERRANO, + 3.5714285714285716, + true), + PROTO( + Rotations.of(0.3876953125), // front left + Rotations.of(0.159912109375), // front right + Rotations.of(0.213134765625), // back left + Rotations.of(-0.3818359375), // back right + SwerveLevel.L2, // what level the swerve drive is + SwerveDrivePIDValues.PROTO, + SwerveSteerPIDValues.PROTO, + SwerveDriveToPosePIDValues.PROTO, + SwerveDriveToPoseProfileValues.PROTO, + ChoreoPIDValues.PROTO, + RobotDimensions.PROTO, + "rio", + BumperThickness.PROTO, + 3.5714285714285716, + true), + JAMES_HARDEN( + Rotations.of(-0.0834960938), // front left + Rotations.of(-0.4912109375), // front right + Rotations.of(0.1931152344), // back left + Rotations.of(-0.15576171875), // back right + SwerveLevel.L3, + SwerveDrivePIDValues.JAMES_HARDEN, + SwerveSteerPIDValues.JAMES_HARDEN, + SwerveDriveToPosePIDValues.JAMES_HARDEN, + SwerveDriveToPoseProfileValues.JAMES_HARDEN, + ChoreoPIDValues.JAMES_HARDEN, + RobotDimensions.JAMES_HARDEN, + "JamesHarden", + BumperThickness.JAMES_HARDEN, + 3.5714285714285716, + true), + COBRA( + Rotations.of(0.096923828125), // front left, 21 + Rotations.of(0.03271484375), // front right, 22 + Rotations.of(0.02587890625), // back left, 20 + Rotations.of(-0.09765625), // back right, 23 + SwerveLevel.FIVEN_L3, + SwerveDrivePIDValues.COBRA, + SwerveSteerPIDValues.COBRA, + SwerveDriveToPosePIDValues.COBRA, + SwerveDriveToPoseProfileValues.COBRA, + ChoreoPIDValues.COBRA, + RobotDimensions.COBRA, + "Viper", + BumperThickness.COBRA, + 3.5714285714285716, + false); + public final Angle FRONT_LEFT_ENCODER_OFFSET, + FRONT_RIGHT_ENCODER_OFFSET, + BACK_LEFT_ENCODER_OFFSET, + BACK_RIGHT_ENCODER_OFFSET; + public final SwerveLevel SWERVE_LEVEL; + public final SwerveDrivePIDValues SWERVE_DRIVE_PID_VALUES; + public final SwerveSteerPIDValues SWERVE_STEER_PID_VALUES; + public final SwerveDriveToPosePIDValues SWERVE_DRIVE_TO_POSE_PID_VALUES; + public final SwerveDriveToPoseProfileValues SWERVE_DRIVE_TO_POSE_PROFILE_VALUES; + public final ChoreoPIDValues CHOREO_PID_VALUES; + public final RobotDimensions ROBOT_DIMENSIONS; + public final String CANBUS_NAME; + public final double COUPLE_RATIO; + public final BumperThickness BUMPER_THICKNESS; + public final boolean INVERTED_MODULES; + + SwerveType( + Angle fl, + Angle fr, + Angle bl, + Angle br, + SwerveLevel swerveLevel, + SwerveDrivePIDValues swerveDrivePIDValues, + SwerveSteerPIDValues swerveSteerPIDValues, + SwerveDriveToPosePIDValues swerveDriveToPosePIDValues, + SwerveDriveToPoseProfileValues swerveDriveToPoseProfileValues, + ChoreoPIDValues choreoPIDValues, + RobotDimensions robotDimensions, + String canbus_name, + BumperThickness thickness, + double coupled_ratio, + boolean invertedModules) { + FRONT_LEFT_ENCODER_OFFSET = fl; + FRONT_RIGHT_ENCODER_OFFSET = fr; + BACK_LEFT_ENCODER_OFFSET = bl; + BACK_RIGHT_ENCODER_OFFSET = br; + SWERVE_LEVEL = swerveLevel; + SWERVE_DRIVE_PID_VALUES = swerveDrivePIDValues; + SWERVE_STEER_PID_VALUES = swerveSteerPIDValues; + SWERVE_DRIVE_TO_POSE_PID_VALUES = swerveDriveToPosePIDValues; + SWERVE_DRIVE_TO_POSE_PROFILE_VALUES = swerveDriveToPoseProfileValues; + CHOREO_PID_VALUES = choreoPIDValues; + ROBOT_DIMENSIONS = robotDimensions; + CANBUS_NAME = canbus_name; + BUMPER_THICKNESS = thickness; + COUPLE_RATIO = coupled_ratio; + INVERTED_MODULES = invertedModules; + } + } + + // these outline the speed calculations + public static final double PHYSICAL_MAX_SPEED_METERS_PER_SECOND = 4.868; + // 5.944; // before: 4.8768;// 18ft/s = 5.486, 19m/s = 5.791ft/s, 19.5m/s = + // 5.944 ft/s, + public static final double PHYSICAL_MAX_ANGLUAR_SPEED_RADIANS_PER_SECOND = 10.917; + public static final double TELE_DRIVE_FAST_MODE_SPEED_PERCENT = 0.7; + public static final double TELE_DRIVE_SLOW_MODE_SPEED_PERCENT = 0.3; + public static final double TELE_DRIVE_MAX_ACCELERATION_METERS_PER_SECOND_PER_SECOND = 8; + public static final double TELE_DRIVE_PERCENT_SPEED_RANGE = + (TELE_DRIVE_FAST_MODE_SPEED_PERCENT - TELE_DRIVE_SLOW_MODE_SPEED_PERCENT); + public static final double TELE_DRIVE_MAX_ANGULAR_RATE_RADIANS_PER_SECOND = 10.917; + public static final double TELE_DRIVE_MAX_ANGULAR_ACCELERATION_RADIANS_PER_SECOND_PER_SECOND = + 26.971; + + public static class Auto { + public static final double TIME_FOR_OUTPOST_INTAKE = 3.0; + public static final double TIME_FOR_BUMP_FORWARDS = 0.95; + public static final double TIME_FOR_BUMP_BACKWARDS = 1.05; + public static final double TIME_FOR_BUMP_FORWARDS_SLOWER = 1.30; + public static final double WAIT_TIME_FOR_ALLIANCE = 4.0; + + public static enum Intake { + LeftIntakeSweepShort, + RightIntakeSweepShort, + RightSecondDip, + LeftSecondDip, // outside in + LeftSecondDipLong, // inside out + RightSecondDipLong, + HubLeft, + HubRight + } + + public static enum ShootPos { + LeftShoot, + RightShoot + } + + public static enum ClimbPos {} + + public static enum Depot { + DepotStart, + DepotSweep + } + + public static enum Outpost { + OutpostStart, + OutpostSweep, + OutpostStartPush + } + + public static enum MiscPaths { + LeftShootToBump, + RightShootToBump, + Nike, + anthony + } + } + } + + public static class Hopper { + // TODO: Motor Ports + public static final int MOTOR_1_PORT = 17; + public static final int MOTOR_2_PORT = 13; + + // TODO: subject to change, ask Jeff + public static final double TARGET_SURFACE_SPEED_MPS = + 2.63; // TARGET_SURFACE_SPEED_FPS * 0.3048; + + // TODO: Tune these + public static final double kP = 0.8; + public static final double kV = 0.124; + + // TODO: Do we want stator limit to still be this high? + public static final double STATOR_LIMIT_AMPS = 150.0; // 50.0 + public static final double SUPPLY_LIMIT_AMPS = 50.0; // 30.0 + + public static final double MOTOR_ROTS_PER_AGITATOR_ROT = 3.57142857; + public static final double AGITATOR_ROTS_PER_MOTOR_ROT = 1.0 / MOTOR_ROTS_PER_AGITATOR_ROT; + + public static final double MOTOR_ROTS_PER_FLOOR_METER = 250d / 7d; + public static final double FLOOR_METERS_PER_MOTOR_ROT = 1.0 / MOTOR_ROTS_PER_FLOOR_METER; + + public static final double FLOOR_SPEED_TOLERANCE_MPS = 0.05; + + public static final InterpolatingDoubleTreeMap HOPPER_SPEED_MAP = + new InterpolatingDoubleTreeMap(); + } + + public static class Vision { + public static VisionCamera FALLBACK_CAMERA = VisionCamera.FRONT_LEFT_CAM; + public static boolean SKIP_TO_FALLBACK = false; + + public static void updateFallbackCamera(VisionCamera cam) { + FALLBACK_CAMERA = cam; + } + + public static final double MAX_TAG_DISTANCE = 15.0; // meters, beyond which readings are dropped + + public static final double CALIBRATION_FACTOR = 1.0; + public static final double BASE_NOISE_X = 0.0008; // m + public static final double BASE_NOISE_Y = 0.0008; // m + public static final double BASE_NOISE_THETA = 0.5; // rad + + // Constants for noise calculation + public static final double DISTANCE_EXPONENTIAL_COEFFICIENT_X = 0.00046074; + public static final double DISTANCE_EXPONENTIAL_BASE_X = 2.97294; + public static final double DISTANCE_EXPONENTIAL_COEFFICIENT_Y = 0.0046074; + public static final double DISTANCE_EXPONENTIAL_BASE_Y = 2.97294; + + public static final double DISTANCE_COEFFICIENT_THETA = 0.9; + + public static final double ANGLE_COEFFICIENT_X = + 0.5; // noise growth per radian of viewing angle + public static final double ANGLE_COEFFICIENT_Y = 0.5; + public static final double ANGLE_COEFFICIENT_THETA = 0.5; + + public static final double SPEED_COEFFICIENT_X = 0.5; // noise growth per fraction of max speed + public static final double SPEED_COEFFICIENT_Y = 0.5; + public static final double SPEED_COEFFICIENT_THETA = 0.5; + + public static final double TIMESTAMP_THRESHOLD = 0.5; + public static final double TIMESTAMP_FPGA_CORRECTION = -0.03; + public static final double TAG_VISIBLE_THRESHOLD_SEC = 0.2; + + // initializes cameras for use in VisionSubsystem + public static enum VisionCamera { + FRONT_RIGHT_CAM( + "frontRightCam", + VisionCameraConstants.FrontRight.transform, + VisionCameraConstants.FrontRight.cameraMatrix, + VisionCameraConstants.FrontRight.distCoeffs), + + FRONT_LEFT_CAM( + "frontLeftCam", + VisionCameraConstants.FrontLeft.transform, + VisionCameraConstants.FrontLeft.cameraMatrix, + VisionCameraConstants.FrontLeft.distCoeffs), + + REAR_RIGHT_CAM( + "rearRightCam", + VisionCameraConstants.RearRight.transform, + VisionCameraConstants.RearRight.cameraMatrix, + VisionCameraConstants.RearRight.distCoeffs), + + REAR_LEFT_CAM( + "rearLeftCam", + VisionCameraConstants.RearLeft.transform, + VisionCameraConstants.RearLeft.cameraMatrix, + VisionCameraConstants.RearLeft.distCoeffs); + + private String loggingName; + private Transform3d cameraTransform; + private Matrix cameraMatrix; + private Matrix distCoeffs; + + VisionCamera( + String name, Transform3d transform, Matrix matrix, Matrix coeffs) { + loggingName = name; + cameraTransform = transform; + cameraMatrix = matrix; + distCoeffs = coeffs; + } + + public String getLoggingName() { + return loggingName; + } + + public Transform3d getCameraTransform() { + return cameraTransform; + } + + public Matrix getCameraMatrix() { + return cameraMatrix; + } + + public Matrix getDistCoeffs() { + return distCoeffs; + } + } + + public static final CameraSelectionMethod CAMERA_SELECTION_METHOD = CameraSelectionMethod.MIN; + public static final double MAX_HEADING_DIFF = 25.0; + public static final double MAX_HEADING_DIFF_AUTO = 20.0; + public static final double HDG_PENALTY = 0.5; + + public static enum CameraSelectionMethod { + MIN(), + AVG(), + MAX(), + POSE_AMBIGUITY(); + } + } + + public static class FuelGaugeDetection { + public static final int BALLS_TO_AVG = 3; + public static final int MAX_FUEL_GAUGE_MEASUREMENTS = 33; + public static final double MAX_DETECTABLE_FUEL_AREA_PERCENTAGE = 60.00; + public static final double REALISTIC_MAX_DETECTABLE_AREA_PERCENTAGE = 15.00; + + public static enum FuelGaugeCamera { + FUEL_GAUGE_CAM("fuelGaugeCam", VisionCameraConstants.FuelGauge.transform); + + private String loggingName; + private Transform3d cameraTransform; + + FuelGaugeCamera(String name, Transform3d transform) { + loggingName = name; + cameraTransform = transform; + } + + public String getLoggingName() { + return loggingName; + } + + public Transform3d getCameraTransform() { + return cameraTransform; + } + } + + public static enum GaugeCalculationType { + RAW(), + SMOOTHED(), + MULTIPLE_BALLS(), + SMOOTHED_MULTIPLE_BALLS(); + } + + public static enum FuelGauge { // LAST: 20, 50, 70, 100 + EMPTY(2.0, "#000000"), + LOW(9.0, "#FF0000"), + MEDIUM(12.0, "#FFFF00"), + FULL(100.0, "#00FF00"); + + private double threshold; + private String color; + + FuelGauge(double threshold, String color) { + this.threshold = threshold; + this.color = color; + } + + public double getThreshold() { + return threshold; + } + + public String getColor() { + return color; + } + } + } + + public static class IntakeVision { + public static final double INTAKE_X = Units.inchesToMeters(0.0); + public static final double INTAKE_Y = Units.inchesToMeters(22.342); + public static final double INTAKE_Z = Units.inchesToMeters(18.9); + public static final double INTAKE_ROLL = Units.degreesToRadians(0.0); + public static final double INTAKE_PITCH = Units.degreesToRadians(9.789); + public static final double INTAKE_YAW = Units.degreesToRadians(0.0); + + public static final double OVERRIDE_ROT_INPUT = 0.5; + + public static final double kP = 0.2d; + public static final double lookAheadTime = 0.02; + + public static final double headingPIDDampen = 0.5; + + // public static final double CAM_HEIGHT_METERS = 1; + + public static final double MIN_DETECTION_DIST = 0.72; + public static final double MAX_DETECTION_DIST = 8.07; + public static final double MIN_DETECTION_SLOPE = 0.05; + + public static enum IntakeVisionCamera { + INTAKE_CAM( + "intakeCam", + new Transform3d( + new Translation3d(INTAKE_X, INTAKE_Y, INTAKE_Z), + new Rotation3d(INTAKE_ROLL, INTAKE_PITCH, INTAKE_YAW))); + + private String loggingName; + private Transform3d cameraTransform; + + IntakeVisionCamera(String name, Transform3d transform) { + loggingName = name; + cameraTransform = transform; + } + + public String getLoggingName() { + return loggingName; + } + + public Transform3d getCameraTransform() { + return cameraTransform; + } + } + + public static enum TargetingMode { + HDG_SEL(), + LOC_SEL(); + } + } + + public static final class Shooter { + + public static final class Hood { + public static final int HOOD_ID = 12; + + public static final double KP = 0.0; + public static final double KI = 0.0; + public static final double KD = 0.0; + public static final double KV = 0.0; + public static final double KS = 0.0; + public static final double KG = 0.0; + + public static final double STATOR_CURRENT_LIMIT = 40.0; + public static final double SUPPLY_CURRENT_LIMIT = 40.0; + + public static final double MOTOR_ROTS_PER_HOOD_ROT = 996.0 / 5.0; + public static final double HOOD_ROTS_PER_MOTOR_ROT = 1.0 / MOTOR_ROTS_PER_HOOD_ROT; + public static final double HOOD_DEGREES_PER_MOTOR_ROT = HOOD_ROTS_PER_MOTOR_ROT * 360.0; + public static final double MOTOR_ROTS_PER_DEGREE = 1.0 / HOOD_DEGREES_PER_MOTOR_ROT; + + public static final double MOTOR_ROTS_PER_ENCODER_ROT = 12.0; + public static final double ENCODER_ROTS_PER_HOOD_ROT = + MOTOR_ROTS_PER_HOOD_ROT / MOTOR_ROTS_PER_ENCODER_ROT; + public static final double HOOD_ROTS_PER_ENCODER_ROT = 1.0 / ENCODER_ROTS_PER_HOOD_ROT; + + public static final double HOOD_TOLERANCE_DEG = 0.5; + + public static final double MIN_HOOD_POSITION = 3.8; + public static final double MAX_HOOD_POSITION = 18.369; + + public static double ENCODER_OFFSET = 0.296; // TODO + public static final int ENCODER_PORT = 18; + + public static final double ZERO_STATOR_CURRENT_LIMIT = 15.0; + public static final double ZERO_SUPPLY_CURRENT_LIMIT = 15.0; + public static final double ZERO_VOLTAGE = -2.0; + public static final double ZERO_MAX_SUPPLY = 0; + public static final double ZERO_MAX_STATOR = 0; + public static final int MAX_TIMES_EXCEEDED = 10; + } + + public static final class Rollers { + public static final double TOLERANCE_RPS = 1.0; + + public static final int WARMUP_1_ID = 10; + public static final int WARMUP_2_ID = 9; + public static final int WARMUP_3_ID = 11; + + public static final double KP = 0.0; + public static final double KI = 0.0; + public static final double KD = 0.0; + public static final double KV = 0.0; + public static final double KS = 0.0; + + public static final double STATOR_CURRENT_LIMIT = 120.0; + public static final double SUPPLY_CURRENT_LIMIT = 40.0; + + public static final double MOTOR_ROTS_PER_WHEEL_ROT = 15.0 / 12.0; + public static final double WHEEL_ROTS_PER_MOTOR_ROT = 1.0 / MOTOR_ROTS_PER_WHEEL_ROT; + public static final double SHOOTER_WHEEL_DIAMETER = 3.0; + } + + } + + public static class OI { + public static final double LEFT_JOYSTICK_DEADBAND = 0.07; + public static final double RIGHT_JOYSTICK_DEADBAND = 0.07; + public static final int JOYSTICK_A_PORT = 0; + + public enum XBoxButtonID { + A(1), + B(2), + X(3), + Y(4), + LeftBumper(5), + RightBumper(6), + LeftStick(9), + RightStick(10), + Back(7), + Start(8); + + public final int value; + + XBoxButtonID(int value) { + this.value = value; + } + } + + public enum AxisID { + /** Left X. */ + LeftX(0), + /** Right X. */ + RightX(4), + /** Left Y. */ + LeftY(1), + /** Right Y. */ + RightY(5), + /** Left trigger. */ + LeftTrigger(2), + /** Right trigger. */ + RightTrigger(3); + + /** Axis value. */ + public final int value; + + AxisID(int value) { + this.value = value; + } + } + } + + public static class Landmarks { + public static Pose2d BLUE_HUB = + new Pose2d(4.621390342712402, 4.032095909118652, new Rotation2d()); + public static Pose2d RED_HUB = + new Pose2d(11.917659759521484, 4.032095909118652, new Rotation2d()); + + public static Pose2d RED_TOWER_R = + new Pose2d(14.871597290039062, 4.749175071716309, new Rotation2d(0)); + public static Pose2d RED_TOWER_L = + new Pose2d(14.871597290039062, 3.892498254776001, new Rotation2d(0)); + public static Pose2d BLUE_TOWER_R = + new Pose2d(1.6428194046020508, 3.320095539093017, new Rotation2d(Math.PI)); + public static Pose2d BLUE_TOWER_L = + new Pose2d(1.6428194046020508, 4.1721110343933105, new Rotation2d(Math.PI)); + + // public static Pose2d RED_LEFT_INTAKE_TO_BUMP = + // new Pose2d( + // new Translation2d(10.908942222595215, 2.54630184173584), + // new Rotation2d(1.5707963267948966)); + // public static Pose2d RED_RIGHT_INTAKE_TO_BUMP = + // new Pose2d( + // new Translation2d(10.921140670776367, 5.613290309906006), + // new Rotation2d(-1.5707963267948966)); + // public static Pose2d BLUE_LEFT_INTAKE_TO_BUMP = + // new Pose2d( + // new Translation2d(5.6342058181762695, 5.505496978759766), + // new Rotation2d(-1.5649821399611368)); + // public static Pose2d BLUE_RIGHT_INTAKE_TO_BUMP = + // new Pose2d( + // new Translation2d(5.624283313751221, 2.4593770503997803), + // new Rotation2d(1.57873264137917)); + public static Pose2d RED_LEFT_INTAKE_TO_BUMP = + new Pose2d( + new Translation2d(10.908942222595215, 2.2220301628112793), + new Rotation2d(1.5707963267948966)); + public static Pose2d RED_RIGHT_INTAKE_TO_BUMP = + new Pose2d( + new Translation2d(10.921140670776367, 5.914291858673096), + new Rotation2d(-1.5707963267948966)); + public static Pose2d BLUE_LEFT_INTAKE_TO_BUMP = + new Pose2d( + new Translation2d(5.6342058181762695, 5.8613409996032715), + new Rotation2d(-1.5649821399611368)); + public static Pose2d BLUE_RIGHT_INTAKE_TO_BUMP = + new Pose2d( + new Translation2d(5.624283313751221, 2.281606435775757), + new Rotation2d(1.57873264137917)); + + public static Pose2d RED_LEFT_SHOOT_TO_BUMP = + new Pose2d( + new Translation2d(12.9309492111206055, 2.3620588779449463), + new Rotation2d(1.5707963267948966)); + public static Pose2d RED_RIGHT_SHOOT_TO_BUMP = + new Pose2d( + new Translation2d(12.951465606689453, 5.671574592590332), + new Rotation2d(-1.5707963267948966)); + public static Pose2d BLUE_LEFT_SHOOT_TO_BUMP = + new Pose2d( + new Translation2d(3.60064959526062, 5.671574592590332), + new Rotation2d(-1.5649821399611368)); + public static Pose2d BLUE_RIGHT_SHOOT_TO_BUMP = + new Pose2d( + new Translation2d(3.60064959526062, 2.3620588779449463), + new Rotation2d(1.57873264137917)); + + public static Pose2d BLUE_PASSING_L = new Pose2d(2.111, 6.003, new Rotation2d()); + public static Pose2d BLUE_PASSING_R = new Pose2d(2.111, 2.007, new Rotation2d()); + + public static Pose2d RED_PASSING_L = new Pose2d(14.27, 2.22, new Rotation2d()); + public static Pose2d RED_PASSING_R = new Pose2d(14.27, 5.94, new Rotation2d()); + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index efbb977..1009df6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,9 +4,16 @@ package frc.robot; +import dev.doglog.DogLog; +import dev.doglog.DogLogOptions; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.util.LoggedTalonFX; +import frc.robot.util.MiscUtils; +import frc.robot.util.VisionUtils; /** * The methods in this class are called automatically corresponding to each mode, as described in @@ -14,19 +21,34 @@ * this project, you must also update the Main.java file in the project. */ public class Robot extends TimedRobot { - private static final String kDefaultAuto = "Default"; - private static final String kCustomAuto = "My Auto"; - private String m_autoSelected; - private final SendableChooser m_chooser = new SendableChooser<>(); + private Command m_autonomousCommand; + + private final RobotContainer m_robotContainer; + + private static double simulatedTime = 160; /** * This function is run when the robot is first started up and should be used for any * initialization code. */ public Robot() { - m_chooser.setDefaultOption("Default Auto", kDefaultAuto); - m_chooser.addOption("My Auto", kCustomAuto); - SmartDashboard.putData("Auto choices", m_chooser); + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + } + + @Override + public void robotInit() { + RobotContainer.isRedAlliance(); + DogLog.setOptions( + new DogLogOptions() + .withNtPublish(false) + .withCaptureDs(true) + .withLogExtras(false) + .withNtTunables(false)); + DogLog.log("Elastic/FieldPose", m_robotContainer.drivetrain.getCurrentState().Pose); + DogLog.log("Elastic/RedSide", RobotContainer.isRedAlliance()); + RobotController.setBrownoutVoltage(6.0); } /** @@ -37,58 +59,76 @@ public Robot() { * SmartDashboard integrated updating. */ @Override - public void robotPeriodic() {} + 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(); + LoggedTalonFX.periodic_static(); + m_robotContainer.visionPeriodic(); + elasticLogging(); + MiscUtils.shiftSwitchIndicator(simulatedTime); + m_robotContainer.doTelemetry(); + } + + private void elasticLogging() { + simulatedTime -= 0.02; + if (simulatedTime < 0) simulatedTime = 160; + DogLog.log("Elastic/FieldPose", m_robotContainer.drivetrain.getCurrentState().Pose); + DogLog.log("Elastic/BatteryVoltage", RobotController.getBatteryVoltage()); + DogLog.log("Elastic/AreWeActive", MiscUtils.areWeActive()); + SmartDashboard.putString("Elastic/ActiveFirst", MiscUtils.activeFirst()); + DogLog.log("Elastic/TimeUntilNextShift", MiscUtils.countdownTillNextShift(simulatedTime)); + SmartDashboard.putNumber( + "Elastic/timeUntilNextShift", MiscUtils.countdownTillNextShift(simulatedTime)); + DogLog.log("Elastic/CurrentShiftName", MiscUtils.currentShiftName(simulatedTime)); + } + + /** This function is called once each time the robot enters Disabled mode. */ + @Override + public void disabledInit() {} - /** - * This autonomous (along with the chooser code above) shows how to select between different - * autonomous modes using the dashboard. The sendable chooser code works with the Java - * SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the chooser code and - * uncomment the getString line to get the auto name from the text box below the Gyro - * - *

You can add additional auto modes by adding additional comparisons to the switch structure - * below with additional strings. If using the SendableChooser make sure to add them to the - * chooser code above as well. - */ + @Override + public void disabledPeriodic() {} + + /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { - m_autoSelected = m_chooser.getSelected(); - // m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto); - System.out.println("Auto selected: " + m_autoSelected); + // m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + VisionUtils.setHeadingThreshold(Constants.Vision.MAX_HEADING_DIFF_AUTO); + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) CommandScheduler.getInstance().schedule(m_autonomousCommand); } /** This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() { - switch (m_autoSelected) { - case kCustomAuto: - // Put custom auto code here - break; - case kDefaultAuto: - default: - // Put default auto code here - break; - } - } + public void autonomousPeriodic() {} - /** This function is called once when teleop is enabled. */ @Override - public void teleopInit() {} + public void teleopInit() { + m_robotContainer.intakeSubsystem.applyBrakeConfigArm(); + VisionUtils.setHeadingThreshold(Constants.Vision.MAX_HEADING_DIFF); + + // 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_autonomousCommand != null) m_autonomousCommand.cancel(); + DogLog.log("Elastic/FlashDriveConnected", MiscUtils.isFlashDriveConnected()); + } /** This function is called periodically during operator control. */ @Override public void teleopPeriodic() {} - /** This function is called once when the robot is disabled. */ @Override - public void disabledInit() {} - - /** This function is called periodically when disabled. */ - @Override - public void disabledPeriodic() {} - - /** This function is called once when test mode is enabled. */ - @Override - public void testInit() {} + 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 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java new file mode 100644 index 0000000..9760900 --- /dev/null +++ b/src/main/java/frc/robot/RobotContainer.java @@ -0,0 +1,230 @@ +// 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; + +import choreo.auto.AutoChooser; +// * KEEP FOR WIN COMMAND TESTING +// 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; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +// * KEEP FOR WIN COMMAND TESTING +import frc.robot.commandGroups.ShootCommandGroups.ShootPassing; +import frc.robot.commandGroups.ShootCommandGroups.ShootWithAim; +import frc.robot.commands.SwerveCommands.SwerveJoystickCommand; +import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.FuelGaugeDetection; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.IntakeVisionDetection; +import frc.robot.subsystems.LEDSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.VisionSubsystem; +import frc.robot.util.CustomController; +import frc.robot.util.MiscUtils; +import frc.robot.util.Targeting; +import frc.robot.util.VisionUtils; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +public class RobotContainer { + private BooleanSupplier redside = RobotContainer::isRedAlliance; + + // private Field2d field = new Field2d(); + private final Telemetry logger = + new Telemetry(Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND); + + private final CommandXboxController joystick = new CommandXboxController(0); + private final CustomController secondController = new CustomController(4); + + public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + + public final HopperSubsystem hopperSubsystem = + Constants.hopperOnRobot ? new HopperSubsystem(drivetrain, redside) : null; + public final IntakeSubsystem intakeSubsystem = + Constants.intakeOnRobot ? new IntakeSubsystem() : null; + + +// private final AutoRoutines autoRoutines; +// private final AutoChooser autoChooser; + + public final VisionSubsystem visionFrontRight = + Constants.visionOnRobot + ? new VisionSubsystem(Constants.Vision.VisionCamera.FRONT_RIGHT_CAM, drivetrain) + : null; + public final VisionSubsystem visionFrontLeft = + Constants.visionOnRobot + ? new VisionSubsystem(Constants.Vision.VisionCamera.FRONT_LEFT_CAM, drivetrain) + : null; + public final VisionSubsystem visionRearRight = + Constants.visionOnRobot + ? new VisionSubsystem(Constants.Vision.VisionCamera.REAR_RIGHT_CAM, drivetrain) + : null; + public final VisionSubsystem visionRearLeft = + Constants.visionOnRobot + ? new VisionSubsystem(Constants.Vision.VisionCamera.REAR_LEFT_CAM, drivetrain) + : null; + + public final FuelGaugeDetection visionFuelGauge = + Constants.fuelGaugeOnRobot + ? new FuelGaugeDetection(Constants.FuelGaugeDetection.FuelGaugeCamera.FUEL_GAUGE_CAM) + : null; + public final IntakeVisionDetection visionIntake = + Constants.intakeVisionOnRobot + ? new IntakeVisionDetection(Constants.IntakeVision.IntakeVisionCamera.INTAKE_CAM) + : null; + + public final LEDSubsystem leds = + new LEDSubsystem( + () -> MiscUtils.areWeActive(2.0), + () -> + Targeting.distMeters(drivetrain, Targeting.getHub(redside)) < 4.47 + && inAllianceSide()); + + // * KEEP FOR INTERMAP TESTING + // private double hoodAngle = 18.369; + // private double shooterSpeed = 58.0; + + public RobotContainer() { + // autoRoutines = new AutoRoutines(intakeSubsystem, lebron, hopperSubsystem, drivetrain, redside); + // autoChooser = autoRoutines.getAutoChooser(); + // SmartDashboard.putData("Auto Chooser", autoChooser); + // SmartDashboard.putData("Elastic/Field2d", field); + configureBindings(); + } + + // public void doTelemetry() { + // logger.telemeterize(drivetrain.getCurrentState()); + + // String commandName = "nah"; + + // if (drivetrain.getCurrentCommand() != null) { + // commandName = drivetrain.getCurrentCommand().getName(); + // } + // DogLog.log("Robot/SwerveDriveCommand", commandName); + // } + + // public CommandSwerveDrivetrain getDrivetrain() { + // return drivetrain; + // } + + private void configureBindings() { + // Swerve + DoubleSupplier frontBackFunction = () -> -joystick.getLeftY(); + DoubleSupplier leftRightFunction = () -> -joystick.getLeftX(); + DoubleSupplier rotationFunction = () -> -joystick.getRightX(); + DoubleSupplier speedFunction = () -> 1d; + + SwerveJoystickCommand swerveJoystickDefaultCommand = + new SwerveJoystickCommand( + frontBackFunction, + leftRightFunction, + rotationFunction, + speedFunction, + () -> true, + joystick.leftTrigger()::getAsBoolean, + () -> false, + redside, + drivetrain, + () -> false, + () -> false); + + joystick.x().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); + drivetrain.setDefaultCommand(swerveJoystickDefaultCommand); + + // Intake + intakeSubsystem.setDefaultCommand(intakeSubsystem.intakeDefault()); + joystick.leftBumper().whileTrue(intakeSubsystem.intakeUntilInterruptedCommand()); + + joystick + .b() + .whileTrue( + intakeSubsystem + .outtakeUntilInterruptedCommand() + .alongWith( + hopperSubsystem.runHopperUntilInterruptedCommand( + -Constants.Hopper.TARGET_SURFACE_SPEED_MPS))); + + // * KEEP FOR WIN COMMAND + // joystick + // .a() + // .whileTrue( + // new DriveToAndShoot( + // () -> (new Pose2d(new Translation2d(3.0, 5.0), new Rotation2d())), + // lebron, + // intakeSubsystem, + // hopperSubsystem, + // drivetrain, + // redside)); + + // joystick + // .a() + // .whileTrue( + // new DriveToPose( + // drivetrain, + // () -> + // new Pose2d( + // new Translation2d(2.462480068206787, 2.26101016998291), new + // Rotation2d()))); + + // * KEEP FOR INTERMAP TESTING + // joystick + // .rightTrigger() + // .whileTrue( + // lebron + // .shootAtSpeedHoodCommand(() -> shooterSpeed, () -> hoodAngle) + // .alongWith(Commands.waitUntil(lebron::isAtSpeed).andThen + // + // (hopperSubsystem.runHopperUntilInterruptedCommand().alongWith(Commands.waitSeconds(0.4).andThen(intakeSubsystem.powerRetractRollersCommand()))))); + + secondController.intakeOverride().whileTrue(intakeSubsystem.retractIntakeCommand()); + + // Hopper + hopperSubsystem.setDefaultCommand(hopperSubsystem.run(hopperSubsystem::stop)); + + // * KEEP FOR INTERMAP TESTING + // joystick.x().onTrue(new InstantCommand(() -> hoodAngle+=0.2)); + // joystick.y().onTrue(new InstantCommand(() -> hoodAngle-=0.2)); + // joystick.a().onTrue(new InstantCommand(() -> shooterSpeed+=0.5)); + // joystick.b().onTrue(new InstantCommand(() -> shooterSpeed-=0.5)); + } + + public static boolean isRedAlliance() { + return DriverStation.getAlliance().isEmpty() + ? false + : DriverStation.getAlliance().get() == Alliance.Red; + } + + public void visionPeriodic() { + VisionUtils.visionPeriodic( + visionFrontRight, visionFrontLeft, visionRearRight, visionRearLeft, drivetrain); + leds.visionStatusIndicators(visionFrontLeft, visionFrontRight, visionRearLeft, visionRearRight); + } + + public void doTelemetry() { + logger.telemeterize(drivetrain.getCurrentState()); + + String commandName = "nah"; + + if (drivetrain.getCurrentCommand() != null) { + commandName = drivetrain.getCurrentCommand().getName(); + } + } + + public boolean inAllianceSide() { + return redside.getAsBoolean() + ? drivetrain.getPose().getX() > Constants.Landmarks.RED_HUB.getX() + : drivetrain.getPose().getX() < Constants.Landmarks.BLUE_HUB.getX(); + } + +// public Command getAutonomousCommand() { +// return autoChooser.selectedCommand(); +// } +} diff --git a/src/main/java/frc/robot/Telemetry.java b/src/main/java/frc/robot/Telemetry.java new file mode 100644 index 0000000..d58991f --- /dev/null +++ b/src/main/java/frc/robot/Telemetry.java @@ -0,0 +1,142 @@ +package frc.robot; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; +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.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class Telemetry { + private final double MaxSpeed; + + /** + * Construct a telemetry object, with the specified max speed of the robot + * + * @param maxSpeed Maximum speed in meters per second + */ + public Telemetry(double maxSpeed) { + MaxSpeed = maxSpeed; + SignalLogger.start(); + } + + /* What to publish over networktables for telemetry */ + private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); + + /* Robot swerve drive state */ + private final NetworkTable driveStateTable = inst.getTable("DriveState"); + private final StructPublisher drivePose = + driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); + private final StructPublisher driveSpeeds = + driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); + private final StructArrayPublisher driveModuleStates = + driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish(); + private final StructArrayPublisher driveModuleTargets = + driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish(); + private final StructArrayPublisher driveModulePositions = + driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish(); + private final DoublePublisher driveTimestamp = + driveStateTable.getDoubleTopic("Timestamp").publish(); + private final DoublePublisher driveOdometryFrequency = + driveStateTable.getDoubleTopic("OdometryFrequency").publish(); + + /* Robot pose for field positioning */ + private final NetworkTable table = inst.getTable("Pose"); + private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); + private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); + + /* Mechanisms to represent the swerve module states */ + private final Mechanism2d[] m_moduleMechanisms = + new Mechanism2d[] { + new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), + }; + /* A direction and length changing ligament for speed representation */ + private final MechanismLigament2d[] m_moduleSpeeds = + new MechanismLigament2d[] { + m_moduleMechanisms[0] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + }; + /* A direction changing and length constant ligament for module direction */ + private final MechanismLigament2d[] m_moduleDirections = + new MechanismLigament2d[] { + m_moduleMechanisms[0] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[1] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[2] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[3] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + }; + + private final double[] m_poseArray = new double[3]; + private final double[] m_moduleStatesArray = new double[8]; + private final double[] m_moduleTargetsArray = new double[8]; + + /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ + public void telemeterize(SwerveDriveState state) { + /* Telemeterize the swerve drive state */ + drivePose.set(state.Pose); + driveSpeeds.set(state.Speeds); + driveModuleStates.set(state.ModuleStates); + driveModuleTargets.set(state.ModuleTargets); + driveModulePositions.set(state.ModulePositions); + driveTimestamp.set(state.Timestamp); + driveOdometryFrequency.set(1.0 / state.OdometryPeriod); + + /* Also write to log file */ + m_poseArray[0] = state.Pose.getX(); + m_poseArray[1] = state.Pose.getY(); + m_poseArray[2] = state.Pose.getRotation().getDegrees(); + for (int i = 0; i < 4; ++i) { + m_moduleStatesArray[i * 2 + 0] = state.ModuleStates[i].angle.getRadians(); + m_moduleStatesArray[i * 2 + 1] = state.ModuleStates[i].speedMetersPerSecond; + m_moduleTargetsArray[i * 2 + 0] = state.ModuleTargets[i].angle.getRadians(); + m_moduleTargetsArray[i * 2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; + } + + SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray); + SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray); + SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray); + SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); + + /* Telemeterize the pose to a Field2d */ + fieldTypePub.set("Field2d"); + fieldPub.set(m_poseArray); + + /* Telemeterize the module states to a Mechanism2d */ + for (int i = 0; i < 4; ++i) { + m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); + m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); + m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); + + SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); + } + } +} diff --git a/src/main/java/frc/robot/UnusedAutos.txt b/src/main/java/frc/robot/UnusedAutos.txt new file mode 100644 index 0000000..7d37e3b --- /dev/null +++ b/src/main/java/frc/robot/UnusedAutos.txt @@ -0,0 +1,386 @@ +// public AutoRoutine PedriMidRight() { +// AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + +// Command forward = +// redSide.getAsBoolean() +// ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) +// : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); +// Command backward = +// redSide.getAsBoolean() +// ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) +// : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + +// AutoTrajectory intake = intake(routine, Constants.Swerve.Auto.Intake.RightIntakeSweep); +// AutoTrajectory intakeToShoot = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); + +// routine.active().onTrue(Commands.sequence(forward, intake.resetOdometry(), intake.cmd())); + +// intake +// .done() +// .onTrue(Commands.sequence(backward, intakeToShoot.resetOdometry(), intakeToShoot.cmd())); + +// intakeToShoot.done().onTrue(returnBasicShoot(redSide)); + +// return routine; +// } + +// public AutoRoutine PedriMidLeft() { +// AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + +// Command forward = +// redSide.getAsBoolean() +// ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) +// : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); +// Command backward = +// redSide.getAsBoolean() +// ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) +// : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + +// AutoTrajectory intake = intake(routine, Constants.Swerve.Auto.Intake.LeftIntakeSweep); +// AutoTrajectory intakeToShoot = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); + +// routine.active().onTrue(Commands.sequence(forward, intake.resetOdometry(), intake.cmd())); + +// intake +// .done() +// .onTrue(Commands.sequence(backward, intakeToShoot.resetOdometry(), intakeToShoot.cmd())); + +// intakeToShoot.done().onTrue(returnBasicShoot(redSide)); + +// return routine; +// } + +// public AutoRoutine PedriDepotRight() { +// AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + +// Command forward = +// redSide.getAsBoolean() +// ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) +// : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); +// Command backward = +// redSide.getAsBoolean() +// ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) +// : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + +// AutoTrajectory intake = intake(routine, Constants.Swerve.Auto.Intake.RightIntakeSweep); +// AutoTrajectory intakeToShoot = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); +// AutoTrajectory depotIntake = depot(routine, Constants.Swerve.Auto.Depot.DepotL); +// AutoTrajectory depotToShoot = shoot(routine, Constants.Swerve.Auto.ShootPos.DepotToShoot); + +// routine.active().onTrue(Commands.sequence(forward, intake.resetOdometry(), intake.cmd())); + +// intake +// .done() +// .onTrue(Commands.sequence(backward, intakeToShoot.resetOdometry(), intakeToShoot.cmd())); + +// intakeToShoot.done().onTrue(Commands.sequence(returnBasicShoot(redSide), depotIntake.cmd())); +// depotIntake.done().onTrue(depotToShoot.cmd()); +// depotToShoot.done().onTrue(returnBasicShoot(redSide)); + +// return routine; +// } + +// public AutoRoutine PedriDepotLeft() { +// AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + +// Command forward = +// redSide.getAsBoolean() +// ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) +// : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); +// Command backward = +// redSide.getAsBoolean() +// ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) +// : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + +// AutoTrajectory intake = intake(routine, Constants.Swerve.Auto.Intake.LeftIntakeSweep); +// AutoTrajectory intakeToShoot = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); +// AutoTrajectory depotIntake = depot(routine, Constants.Swerve.Auto.Depot.DepotR); +// AutoTrajectory depotToShoot = shoot(routine, Constants.Swerve.Auto.ShootPos.DepotToShoot); + +// routine.active().onTrue(Commands.sequence(forward, intake.resetOdometry(), intake.cmd())); + +// intake +// .done() +// .onTrue(Commands.sequence(backward, intakeToShoot.resetOdometry(), intakeToShoot.cmd())); + +// intakeToShoot.done().onTrue(Commands.sequence(returnBasicShoot(redSide), depotIntake.cmd())); +// depotIntake.done().onTrue(depotToShoot.cmd()); +// depotToShoot.done().onTrue(returnBasicShoot(redSide)); + +// return routine; +// } + +// public AutoRoutine PedriOutpostRight() { +// AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + +// Command forward = +// redSide.getAsBoolean() +// ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) +// : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); +// Command backward = +// redSide.getAsBoolean() +// ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) +// : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + +// AutoTrajectory intake = intake(routine, Constants.Swerve.Auto.Intake.RightIntakeSweep); +// AutoTrajectory intakeToShoot = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); +// AutoTrajectory outpostIntake = outpost(routine, Constants.Swerve.Auto.Outpost.OutpostL); +// AutoTrajectory outpostToShoot = shoot(routine, Constants.Swerve.Auto.ShootPos.OutpostToShoot); + +// routine.active().onTrue(Commands.sequence(forward, intake.resetOdometry(), intake.cmd())); + +// intake +// .done() +// .onTrue(Commands.sequence(backward, intakeToShoot.resetOdometry(), intakeToShoot.cmd())); + +// intakeToShoot.done().onTrue(Commands.sequence(returnBasicShoot(redSide), outpostIntake.cmd())); + +// outpostIntake +// .done() +// .onTrue( +// Commands.sequence( +// new WaitCommand(Constants.Swerve.Auto.TIME_FOR_OUTPOST_INTAKE), +// outpostToShoot.cmd())); +// outpostToShoot.done().onTrue(returnBasicShoot(redSide)); + +// return routine; +// } + +// public AutoRoutine PedriOutpostLeft() { +// AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + +// Command forward = +// redSide.getAsBoolean() +// ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) +// : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); +// Command backward = +// redSide.getAsBoolean() +// ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) +// : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + +// AutoTrajectory intake = intake(routine, Constants.Swerve.Auto.Intake.LeftIntakeSweep); +// AutoTrajectory intakeToShoot = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); +// AutoTrajectory outpostIntake = outpost(routine, Constants.Swerve.Auto.Outpost.OutpostR); +// AutoTrajectory outpostToShoot = shoot(routine, Constants.Swerve.Auto.ShootPos.OutpostToShoot); + +// routine.active().onTrue(Commands.sequence(forward, intake.resetOdometry(), intake.cmd())); + +// intake +// .done() +// .onTrue(Commands.sequence(backward, intakeToShoot.resetOdometry(), intakeToShoot.cmd())); + +// intakeToShoot.done().onTrue(Commands.sequence(returnBasicShoot(redSide), outpostIntake.cmd())); +// outpostIntake +// .done() +// .onTrue( +// Commands.sequence( +// new WaitCommand(Constants.Swerve.Auto.TIME_FOR_OUTPOST_INTAKE), +// outpostToShoot.cmd())); +// outpostToShoot.done().onTrue(returnBasicShoot(redSide)); + +// return routine; +// } + + + + + + + + +// public AutoRoutine DrakeOutpostShort() { +// AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + +// AutoTrajectory outpostIntake = outpost(routine, Constants.Swerve.Auto.Outpost.OutpostStart); +// AutoTrajectory outpostToShoot = shoot(routine, Constants.Swerve.Auto.ShootPos.OutpostToShoot); + +// routine.active().onTrue(Commands.sequence(outpostIntake.resetOdometry(), outpostIntake.cmd())); + +// outpostIntake +// .done() +// .onTrue( +// Commands.sequence( +// new WaitCommand(Constants.Swerve.Auto.TIME_FOR_OUTPOST_INTAKE), +// outpostToShoot.cmd())); +// outpostToShoot.done().onTrue(returnBasicShoot(redSide)); + +// return routine; +// } + +// public AutoRoutine DrakeOutpostLong() { +// AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + +// AutoTrajectory outpostIntake = outpost(routine, Constants.Swerve.Auto.Outpost.OutpostStart); +// AutoTrajectory outpostToShoot = shoot(routine, Constants.Swerve.Auto.ShootPos.OutpostToShoot); +// AutoTrajectory rightSweep = miscPaths(routine, Constants.Swerve.Auto.MiscPaths.RightSweep); +// AutoTrajectory depotIntake = depot(routine, Constants.Swerve.Auto.Depot.DepotL); +// AutoTrajectory depotToShoot = shoot(routine, Constants.Swerve.Auto.ShootPos.DepotToShoot); + +// routine.active().onTrue(Commands.sequence(outpostIntake.resetOdometry(), outpostIntake.cmd())); + +// outpostIntake +// .done() +// .onTrue( +// Commands.sequence( +// new WaitCommand(Constants.Swerve.Auto.TIME_FOR_OUTPOST_INTAKE), +// outpostToShoot.cmd())); +// outpostToShoot.done().onTrue(Commands.sequence(returnBasicShoot(redSide), rightSweep.cmd())); +// rightSweep.done().onTrue(depotIntake.cmd()); +// depotIntake.done().onTrue(depotToShoot.cmd()); +// depotToShoot.done().onTrue(returnBasicShoot(redSide)); + +// return routine; +// } + +// public AutoRoutine DrakeDepotShort() { +// AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + +// AutoTrajectory depotIntake = depot(routine, Constants.Swerve.Auto.Depot.DepotStart); +// AutoTrajectory depotToShoot = shoot(routine, Constants.Swerve.Auto.ShootPos.DepotToShoot); + +// routine.active().onTrue(Commands.sequence(depotIntake.resetOdometry(), depotIntake.cmd())); + +// depotIntake.done().onTrue(depotToShoot.cmd()); +// depotToShoot.done().onTrue(returnBasicShoot(redSide)); + +// return routine; +// } + +// public AutoRoutine DrakeDepotLong() { +// AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + +// AutoTrajectory depotIntake = depot(routine, Constants.Swerve.Auto.Depot.DepotStart); +// AutoTrajectory depotToShoot = shoot(routine, Constants.Swerve.Auto.ShootPos.DepotToShoot); +// AutoTrajectory leftSweep = miscPaths(routine, Constants.Swerve.Auto.MiscPaths.LeftSweep); +// AutoTrajectory outpostIntake = outpost(routine, Constants.Swerve.Auto.Outpost.OutpostR); +// AutoTrajectory outpostToShoot = shoot(routine, Constants.Swerve.Auto.ShootPos.OutpostToShoot); + +// routine.active().onTrue(Commands.sequence(depotIntake.resetOdometry(), depotIntake.cmd())); + +// depotIntake.done().onTrue(depotToShoot.cmd()); +// depotToShoot.done().onTrue(Commands.sequence(returnBasicShoot(redSide), leftSweep.cmd())); +// leftSweep.done().onTrue(outpostIntake.cmd()); +// outpostIntake +// .done() +// .onTrue( +// Commands.sequence( +// new WaitCommand(Constants.Swerve.Auto.TIME_FOR_OUTPOST_INTAKE), +// outpostToShoot.cmd())); +// outpostToShoot.done().onTrue(returnBasicShoot(redSide)); + +// return routine; +// } + + public AutoRoutine OCRight() { + AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + + Command forward = + redSide.getAsBoolean() + ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) + : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); + Command backward = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + + Command forward2 = + redSide.getAsBoolean() + ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) + : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); + Command backward2 = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + + AutoTrajectory intake1 = intake(routine, Constants.Swerve.Auto.Intake.OCRight); + AutoTrajectory intakeToShoot1 = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); + AutoTrajectory shootToBump = + miscPaths(routine, Constants.Swerve.Auto.MiscPaths.RightShootToBump); + AutoTrajectory intake2 = intake(routine, Constants.Swerve.Auto.Intake.OCRight); + AutoTrajectory intakeToShoot2 = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); + + routine.active().onTrue(Commands.sequence(forward, intake1.resetOdometry(), intake1.cmd())); + + intake1 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> false), + backward, + intakeToShoot1.resetOdometry(), + intakeToShoot1.cmd())); + + intakeToShoot1.done().onTrue(Commands.sequence(returnBasicShoot(redSide), shootToBump.cmd())); + + shootToBump.done().onTrue(Commands.sequence(forward2, intake2.resetOdometry(), intake2.cmd())); + + intake2 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> false), + backward2, + intakeToShoot2.resetOdometry(), + intakeToShoot2.cmd())); + + intakeToShoot2.done().onTrue(returnBasicShoot(redSide)); + + return routine; + } + + public AutoRoutine OCLeft() { + AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); + + Command forward = + redSide.getAsBoolean() + ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) + : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); + Command backward = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + + Command forward2 = + redSide.getAsBoolean() + ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) + : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); + Command backward2 = + redSide.getAsBoolean() + ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) + : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); + + AutoTrajectory intake1 = intake(routine, Constants.Swerve.Auto.Intake.OCLeft); + AutoTrajectory intakeToShoot1 = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); + AutoTrajectory shootToBump = + miscPaths(routine, Constants.Swerve.Auto.MiscPaths.LeftShootToBump); + AutoTrajectory intake2 = intake(routine, Constants.Swerve.Auto.Intake.OCLeft); + AutoTrajectory intakeToShoot2 = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); + + routine.active().onTrue(Commands.sequence(forward, intake1.resetOdometry(), intake1.cmd())); + + intake1 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> true), + backward, + intakeToShoot1.resetOdometry(), + intakeToShoot1.cmd())); + + intakeToShoot1.done().onTrue(Commands.sequence(returnBasicShoot(redSide), shootToBump.cmd())); + + shootToBump.done().onTrue(Commands.sequence(forward2, intake2.resetOdometry(), intake2.cmd())); + + intake2 + .done() + .onTrue( + Commands.sequence( + driveToBumpAfterIntake(redSide, () -> true), + backward2, + intakeToShoot2.resetOdometry(), + intakeToShoot2.cmd())); + + intakeToShoot2.done().onTrue(returnBasicShoot(redSide)); + + return routine; + } \ No newline at end of file diff --git a/src/main/java/frc/robot/commandGroups/IntakeToBumpDTP.java b/src/main/java/frc/robot/commandGroups/IntakeToBumpDTP.java new file mode 100644 index 0000000..77e6c96 --- /dev/null +++ b/src/main/java/frc/robot/commandGroups/IntakeToBumpDTP.java @@ -0,0 +1,24 @@ +package frc.robot.commandGroups; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants; +import frc.robot.commands.DriveToPose; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import java.util.function.BooleanSupplier; + +public class IntakeToBumpDTP extends SequentialCommandGroup { + public IntakeToBumpDTP( + CommandSwerveDrivetrain swerve, BooleanSupplier isRedSide, BooleanSupplier isLeftSide) { + Pose2d pose = + isRedSide.getAsBoolean() + ? isLeftSide.getAsBoolean() + ? Constants.Landmarks.RED_LEFT_INTAKE_TO_BUMP + : Constants.Landmarks.RED_RIGHT_INTAKE_TO_BUMP + : isLeftSide.getAsBoolean() + ? Constants.Landmarks.BLUE_LEFT_INTAKE_TO_BUMP + : Constants.Landmarks.BLUE_RIGHT_INTAKE_TO_BUMP; + + addCommands(new DriveToPose(swerve, () -> pose)); + } +} diff --git a/src/main/java/frc/robot/commandGroups/ReverseIntakeAndHopper.java b/src/main/java/frc/robot/commandGroups/ReverseIntakeAndHopper.java new file mode 100644 index 0000000..ced930b --- /dev/null +++ b/src/main/java/frc/robot/commandGroups/ReverseIntakeAndHopper.java @@ -0,0 +1,16 @@ +package frc.robot.commandGroups; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.Constants; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.IntakeSubsystem; + +public class ReverseIntakeAndHopper extends ParallelCommandGroup { + public ReverseIntakeAndHopper(IntakeSubsystem intakeSubsystem, HopperSubsystem hopperSubsystem) { + addCommands( + intakeSubsystem.runRollersUntilInterruptedCommand( + -Constants.Intake.Rollers.TARGET_ROLLER_RPS), + hopperSubsystem.runHopperUntilInterruptedCommand( + -Constants.Hopper.TARGET_SURFACE_SPEED_MPS)); + } +} diff --git a/src/main/java/frc/robot/commandGroups/ShootCommandGroups/DriveToAndShoot.java b/src/main/java/frc/robot/commandGroups/ShootCommandGroups/DriveToAndShoot.java new file mode 100644 index 0000000..314e831 --- /dev/null +++ b/src/main/java/frc/robot/commandGroups/ShootCommandGroups/DriveToAndShoot.java @@ -0,0 +1,154 @@ +package frc.robot.commandGroups.ShootCommandGroups; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.swerve.utility.LinearPath; +import dev.doglog.DogLog; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.Constants; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.util.Targeting; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class DriveToAndShoot extends ParallelCommandGroup { + + public DriveToAndShoot( + Supplier targetPoseSupplier, + ShooterSubsystem shooterSubsystem, + IntakeSubsystem intakeSubsystem, + HopperSubsystem hopperSubsystem, + CommandSwerveDrivetrain drivetrain, + BooleanSupplier redside) { + + DoubleSupplier dist = + () -> + drivetrain + .getPose() + .getTranslation() + .getDistance(drivetrain.getVirtualTarget(redside, () -> false)); + + addCommands( + new DriveToPoseAimed(drivetrain, targetPoseSupplier, redside), + shooterSubsystem.shootAtSpeedHoodCommand( + () -> shooterSubsystem.grabTargetShootingSpeed(dist.getAsDouble()), + () -> shooterSubsystem.grabTargetHoodAngle(dist.getAsDouble())), + Commands.waitUntil(shooterSubsystem::isAtSpeed) + .andThen( + Commands.parallel( + hopperSubsystem.runHopperUntilInterruptedCommand( + () -> Constants.Hopper.TARGET_SURFACE_SPEED_MPS, + () -> + Targeting.pointingAtTarget( + drivetrain + .travelAngleTo( + new Pose2d( + drivetrain.getVirtualTarget(redside, () -> false), + new Rotation2d())) + .getRadians() + + Math.PI, + drivetrain)), + intakeSubsystem.powerRetractRollersCommand()))); + } + + private static final class DriveToPoseAimed extends Command { + + private final CommandSwerveDrivetrain swerve; + private final Supplier targetPoseSupplier; + private final BooleanSupplier redside; + + private LinearPath path = null; + private LinearPath.State pathState = null; + private Pose2d targetPose = null; + + private final PIDController xController = + new PIDController( + Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kPX, + Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kIX, + Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kDX); + private final PIDController yController = + new PIDController( + Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kPY, + Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kIY, + Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kDY); + + private double startTime; + + DriveToPoseAimed( + CommandSwerveDrivetrain swerve, + Supplier targetPoseSupplier, + BooleanSupplier redside) { + this.swerve = swerve; + this.targetPoseSupplier = targetPoseSupplier; + this.redside = redside; + + path = + new LinearPath( + new TrapezoidProfile.Constraints(1.5, 2.0), + new TrapezoidProfile.Constraints( + Constants.Swerve.WHICH_SWERVE_ROBOT + .SWERVE_DRIVE_TO_POSE_PROFILE_VALUES + .maxVelocityAngular, + Constants.Swerve.WHICH_SWERVE_ROBOT + .SWERVE_DRIVE_TO_POSE_PROFILE_VALUES + .maxAccelerationAngular)); + + addRequirements(swerve); + } + + @Override + public void initialize() { + startTime = Utils.getCurrentTimeSeconds(); + targetPose = targetPoseSupplier.get(); + + pathState = + new LinearPath.State(swerve.getCurrentState().Pose, swerve.getCurrentState().Speeds); + + DogLog.log("Subsystems/Swerve/DriveToAndShoot/InitTargetPoseX", targetPose.getX()); + DogLog.log("Subsystems/Swerve/DriveToAndShoot/InitTargetPoseY", targetPose.getY()); + } + + @Override + public void execute() { + if (pathState == null) return; + + double currTime = Utils.getCurrentTimeSeconds() - startTime; + + pathState = path.calculate(currTime, pathState, targetPose); + + double vx = + pathState.speeds.vxMetersPerSecond + + xController.calculate(swerve.getCurrentState().Pose.getX(), pathState.pose.getX()); + double vy = + pathState.speeds.vyMetersPerSecond + + yController.calculate(swerve.getCurrentState().Pose.getY(), pathState.pose.getY()); + + double omega = + swerve.calculateRequiredRotationalRateWithFF( + swerve.getVirtualTarget(redside, () -> false)); + + swerve.applyOneFieldSpeeds(new ChassisSpeeds(vx, vy, omega)); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return Math.abs(swerve.getCurrentState().Pose.getX() - targetPose.getX()) + <= Constants.Swerve.TARGET_POS_ERROR + && Math.abs(swerve.getCurrentState().Pose.getY() - targetPose.getY()) + <= Constants.Swerve.TARGET_POS_ERROR; + } + } +} diff --git a/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootBasic.java b/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootBasic.java new file mode 100644 index 0000000..618b5d3 --- /dev/null +++ b/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootBasic.java @@ -0,0 +1,21 @@ +package frc.robot.commandGroups.ShootCommandGroups; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import java.util.function.DoubleSupplier; + +public class ShootBasic extends ParallelCommandGroup { + public ShootBasic( + DoubleSupplier speed, + ShooterSubsystem shooterSubsystem, + IntakeSubsystem intakeSubsystem, + HopperSubsystem hopperSubsystem) { + addCommands( + shooterSubsystem.shootAtSpeedCommand(speed), + Commands.waitUntil(shooterSubsystem::isAtSpeed) + .andThen(hopperSubsystem.runHopperUntilInterruptedCommand())); + } +} diff --git a/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootBasicRetract.java b/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootBasicRetract.java new file mode 100644 index 0000000..94d4de0 --- /dev/null +++ b/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootBasicRetract.java @@ -0,0 +1,24 @@ +package frc.robot.commandGroups.ShootCommandGroups; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import java.util.function.DoubleSupplier; + +public class ShootBasicRetract extends ParallelCommandGroup { + public ShootBasicRetract( + DoubleSupplier speed, + ShooterSubsystem shooterSubsystem, + IntakeSubsystem intakeSubsystem, + HopperSubsystem hopperSubsystem) { + addCommands( + shooterSubsystem.shootAtSpeedCommand(speed), + Commands.waitUntil(shooterSubsystem::isAtSpeed) + .andThen( + Commands.parallel( + hopperSubsystem.runHopperUntilInterruptedCommand(), + intakeSubsystem.powerRetractRollersCommand()))); + } +} diff --git a/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootPassing.java b/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootPassing.java new file mode 100644 index 0000000..41cd6e3 --- /dev/null +++ b/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootPassing.java @@ -0,0 +1,53 @@ +package frc.robot.commandGroups.ShootCommandGroups; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.Constants; +import frc.robot.commands.SwerveCommands.SwerveJoystickCommandWithPointing; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.util.Targeting; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class ShootPassing extends ParallelCommandGroup { + public ShootPassing( + DoubleSupplier translationalX, + DoubleSupplier translationalY, + ShooterSubsystem shooter, + IntakeSubsystem intake, + HopperSubsystem hopper, + CommandSwerveDrivetrain drivetrain, + BooleanSupplier redside) { + Supplier passingTranslation = + () -> + Targeting.computeVirtualTarget( + new Pose2d(drivetrain.getPassingTarget(redside), new Rotation2d()), drivetrain); + DoubleSupplier dist = + () -> drivetrain.getPose().getTranslation().getDistance(passingTranslation.get()); + + addCommands( + shooter.shootAtSpeedHoodCommand( + () -> shooter.grabTargetShootingSpeed(dist.getAsDouble()), + () -> Constants.Shooter.Hood.MAX_HOOD_POSITION), + new SwerveJoystickCommandWithPointing( + translationalX, translationalY, () -> 0.0, () -> false, passingTranslation, drivetrain), + Commands.waitUntil( + () -> + (shooter.isShooterReady() + && Targeting.pointingAtTarget( + new Pose2d(drivetrain.getPassingTarget(redside), new Rotation2d()), + drivetrain))) + .andThen( + Commands.parallel( + hopper.runHopperUntilInterruptedCommand( + () -> Constants.Hopper.TARGET_SURFACE_SPEED_MPS, () -> true), + intake.powerRetractRollersCommand()))); + } +} diff --git a/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootWithAim.java b/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootWithAim.java new file mode 100644 index 0000000..0a8a7d9 --- /dev/null +++ b/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootWithAim.java @@ -0,0 +1,78 @@ +package frc.robot.commandGroups.ShootCommandGroups; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.Constants; +import frc.robot.commands.SwerveCommands.SwerveJoystickCommand; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.util.Targeting; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +public class ShootWithAim extends ParallelCommandGroup { + public ShootWithAim( + DoubleSupplier translationalX, + DoubleSupplier translationalY, + ShooterSubsystem shooterSubsystem, + IntakeSubsystem intakeSubsystem, + HopperSubsystem hopperSubsystem, + CommandSwerveDrivetrain drivetrain, + BooleanSupplier redside, + BooleanSupplier manualOverride) { + DoubleSupplier dist = + () -> + (drivetrain + .getPose() + .getTranslation() + .getDistance(drivetrain.getVirtualTarget(redside, () -> false))); + addCommands( + Commands.either( + Commands.parallel( // shoot without aim + shooterSubsystem.shootAtSpeedHoodCommand( + 44.2, Constants.Shooter.Hood.MAX_HOOD_POSITION), + Commands.waitUntil(shooterSubsystem::isShooterReady) + .andThen( + Commands.parallel( + hopperSubsystem.runHopperUntilInterruptedCommand(), + intakeSubsystem.powerRetractRollersCommand()))), + Commands.parallel( // shoot with aim + shooterSubsystem.shootAtSpeedHoodCommand( + () -> shooterSubsystem.grabTargetShootingSpeed(dist.getAsDouble()), + () -> shooterSubsystem.grabTargetHoodAngle(dist.getAsDouble())), + new SwerveJoystickCommand( + translationalX, + translationalY, + () -> 0.0, + () -> 1.0, + () -> true, + () -> true, + () -> false, + redside, + drivetrain, + () -> true, + () -> true), + Commands.waitUntil(shooterSubsystem::isAtSpeed) + .andThen( + Commands.parallel( + hopperSubsystem.runHopperUntilInterruptedCommand( + () -> Constants.Hopper.TARGET_SURFACE_SPEED_MPS, + () -> + Targeting.pointingAtTarget( + drivetrain + .travelAngleTo( + new Pose2d( + drivetrain.getVirtualTarget( + redside, () -> false), + new Rotation2d())) + .getRadians() + + Math.PI, + drivetrain)), + intakeSubsystem.powerRetractThenAgitateArmCommand()))), + manualOverride)); + } +} diff --git a/src/main/java/frc/robot/commandGroups/ShootToBumpDTP.java b/src/main/java/frc/robot/commandGroups/ShootToBumpDTP.java new file mode 100644 index 0000000..4a85d5a --- /dev/null +++ b/src/main/java/frc/robot/commandGroups/ShootToBumpDTP.java @@ -0,0 +1,24 @@ +package frc.robot.commandGroups; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants; +import frc.robot.commands.DriveToPose; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import java.util.function.BooleanSupplier; + +public class ShootToBumpDTP extends SequentialCommandGroup { + public ShootToBumpDTP( + CommandSwerveDrivetrain swerve, BooleanSupplier isRedSide, BooleanSupplier isLeftSide) { + Pose2d pose = + isRedSide.getAsBoolean() + ? isLeftSide.getAsBoolean() + ? Constants.Landmarks.RED_LEFT_SHOOT_TO_BUMP + : Constants.Landmarks.RED_RIGHT_SHOOT_TO_BUMP + : isLeftSide.getAsBoolean() + ? Constants.Landmarks.BLUE_LEFT_SHOOT_TO_BUMP + : Constants.Landmarks.BLUE_RIGHT_SHOOT_TO_BUMP; + + addCommands(new DriveToPose(swerve, () -> pose)); + } +} diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java new file mode 100644 index 0000000..d61d330 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveToPose.java @@ -0,0 +1,132 @@ +package frc.robot.commands; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.swerve.utility.LinearPath; +import dev.doglog.DogLog; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import java.util.function.Supplier; + +/** This Command drives the robot in a linear path to a specific pose. */ +public class DriveToPose extends Command { + private final CommandSwerveDrivetrain swerve; + + // Initialize the LinearPath and the LinearPath.State to a null value + private LinearPath path = null; + private LinearPath.State pathState = null; + + // Initialize the Target Pose and the Target Pose Supplier to a null value + private Pose2d targetPose = null; + private Supplier targetPoseSupplier = null; + + private final PIDController xController = + new PIDController( + Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kPX, + Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kIX, + Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kDX); + private final PIDController yController = + new PIDController( + Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kPY, + Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kIY, + Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kDY); + private final PIDController headingController = + new PIDController( + Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kPR, + Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kIR, + Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kDR); + + private double startTime; + + /** + * @param swerve Swerve Subsystem. + * @param targetPoseSupplier Target Pose Supplier (for changing values of pose not just runtime) + */ + public DriveToPose(CommandSwerveDrivetrain swerve, Supplier targetPoseSupplier) { + this.swerve = swerve; + this.targetPoseSupplier = targetPoseSupplier; + this.targetPose = targetPoseSupplier.get(); + + path = + new LinearPath( + new TrapezoidProfile.Constraints( + Constants.Swerve.WHICH_SWERVE_ROBOT + .SWERVE_DRIVE_TO_POSE_PROFILE_VALUES + .maxVelocityLinear, + Constants.Swerve.WHICH_SWERVE_ROBOT + .SWERVE_DRIVE_TO_POSE_PROFILE_VALUES + .maxAccelerationLinear), + new TrapezoidProfile.Constraints( + Constants.Swerve.WHICH_SWERVE_ROBOT + .SWERVE_DRIVE_TO_POSE_PROFILE_VALUES + .maxVelocityAngular, + Constants.Swerve.WHICH_SWERVE_ROBOT + .SWERVE_DRIVE_TO_POSE_PROFILE_VALUES + .maxAccelerationAngular)); // constants + + addRequirements(swerve); + } + + @Override + public void initialize() { + headingController.enableContinuousInput(-Math.PI, Math.PI); + + startTime = Utils.getCurrentTimeSeconds(); + + if (targetPoseSupplier != null) { + targetPose = targetPoseSupplier.get(); + } + + pathState = + new LinearPath.State(swerve.getCurrentState().Pose, swerve.getCurrentState().Speeds); + + DogLog.log("Subsystems/Swerve/DTP/InitTargetPoseX", targetPose.getX()); + DogLog.log("Subsystems/Swerve/DTP/InitTargetPoseY", targetPose.getY()); + DogLog.log( + "Subsystems/Swerve/DTP/InitTargetPoseRotation", targetPose.getRotation().getRadians()); + } + + @Override + public void execute() { + double currTime = Utils.getCurrentTimeSeconds() - startTime; + + if (pathState == null) return; + + pathState = path.calculate(currTime, pathState, targetPose); + + // Generate the next speeds for the robot + ChassisSpeeds speeds = + new ChassisSpeeds( + pathState.speeds.vxMetersPerSecond + + xController.calculate( + swerve.getCurrentState().Pose.getX(), pathState.pose.getX()), + pathState.speeds.vyMetersPerSecond + + yController.calculate( + swerve.getCurrentState().Pose.getY(), pathState.pose.getY()), + pathState.speeds.omegaRadiansPerSecond + + headingController.calculate( + swerve.getCurrentState().Pose.getRotation().getRadians(), + pathState.pose.getRotation().getRadians())); + + swerve.applyOneFieldSpeeds(speeds); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return Math.abs(swerve.getCurrentState().Pose.getX() - targetPose.getX()) + <= Constants.Swerve.TARGET_POS_ERROR + && Math.abs(swerve.getCurrentState().Pose.getY() - targetPose.getY()) + <= Constants.Swerve.TARGET_POS_ERROR + && Math.abs( + swerve.getCurrentState().Pose.getRotation().getRadians() + - targetPose.getRotation().getRadians()) + <= Constants.Swerve.TARGET_ANGLE_ERROR; + } +} diff --git a/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java new file mode 100644 index 0000000..e83da5a --- /dev/null +++ b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java @@ -0,0 +1,171 @@ +package frc.robot.commands.SwerveCommands; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; +import dev.doglog.DogLog; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.util.Targeting; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +public class SwerveJoystickCommand extends Command { + protected final DoubleSupplier xSpdFunction, + ySpdFunction, + turningSpdFunction, + speedControlFunction; + protected final BooleanSupplier fieldRelativeFunction, + doPointing, + doPassing, + redsideIfPointing, + capper, + braking; + + protected final CommandSwerveDrivetrain swerveDrivetrain; + + private final SwerveRequest.FieldCentric fieldCentricDrive = + new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.Velocity); + private final SwerveRequest.RobotCentric robotCentricDrive = + new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.Velocity); + private boolean squaredTurn; + + public SwerveJoystickCommand( + DoubleSupplier frontBackFunction, + DoubleSupplier leftRightFunction, + DoubleSupplier turningSpdFunction, + DoubleSupplier speedControlFunction, + BooleanSupplier fieldRelativeFunction, + BooleanSupplier doPointing, + BooleanSupplier doPassing, + BooleanSupplier redSideIfPointing, + CommandSwerveDrivetrain swerveSubsystem, + BooleanSupplier capper, + BooleanSupplier braking) { + this.xSpdFunction = frontBackFunction; + this.ySpdFunction = leftRightFunction; + this.turningSpdFunction = turningSpdFunction; + this.speedControlFunction = speedControlFunction; + this.fieldRelativeFunction = fieldRelativeFunction; + this.squaredTurn = true; + this.swerveDrivetrain = swerveSubsystem; + this.doPointing = doPointing; + this.doPassing = doPassing; + this.redsideIfPointing = redSideIfPointing; + this.capper = capper; + this.braking = braking; + + // Adds the subsystem as a requirement (prevents two commands from acting on subsystem at once) + addRequirements(swerveDrivetrain); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + // 1. Get real-time joystick inputs + double xSpeed = xSpdFunction.getAsDouble(); // xSpeed is actually front back (front +, back -) + double ySpeed = ySpdFunction.getAsDouble(); // ySpeed is actually left right (left +, right -) + double turningSpeed = + turningSpdFunction.getAsDouble(); // turning speed is (anti-clockwise +, clockwise -) + + // 2. Normalize inputs + double length = xSpeed * xSpeed + ySpeed * ySpeed; // acutally length squared + if (length > 1d) { + length = Math.sqrt(length); + xSpeed /= length; + ySpeed /= length; + } + + // Apply Square (will be [0,1] since `speed` is [0,1]) + xSpeed = xSpeed * xSpeed * Math.signum(xSpeed); + ySpeed = ySpeed * ySpeed * Math.signum(ySpeed); + if (squaredTurn) { + turningSpeed = + Math.abs(turningSpeed * turningSpeed * turningSpeed) * Math.signum(turningSpeed); + } + // 3. Apply deadband + xSpeed = Math.abs(xSpeed) > Constants.OI.LEFT_JOYSTICK_DEADBAND ? xSpeed : 0.0; + ySpeed = Math.abs(ySpeed) > Constants.OI.LEFT_JOYSTICK_DEADBAND ? ySpeed : 0.0; + turningSpeed = + Math.abs(turningSpeed) > Constants.OI.RIGHT_JOYSTICK_DEADBAND ? turningSpeed : 0.0; + + // Applies slew rate limiter + xSpeed = xSpeed * Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND; + ySpeed = ySpeed * Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND; + turningSpeed = turningSpeed * Constants.Swerve.PHYSICAL_MAX_ANGLUAR_SPEED_RADIANS_PER_SECOND; + + double speedMagnitude = Math.sqrt(xSpeed * xSpeed + ySpeed * ySpeed); + + if (capper.getAsBoolean()) { + xSpeed *= 0.3; + ySpeed *= 0.3; + // if (speedMagnitude > 2.0) { + // xSpeed *= (2.0 / speedMagnitude); + // ySpeed *= (2.0 / speedMagnitude); + // } + } + + // Final values to apply to drivetrain + final double x = xSpeed; + final double y = ySpeed; + + double turn = + (doPointing.getAsBoolean()) + ? (swerveDrivetrain.calculateRequiredRotationalRateWithFF( + swerveDrivetrain.getVirtualTarget(redsideIfPointing, () -> false))) + : (turningSpeed); + + if (doPassing.getAsBoolean()) { + turn = + swerveDrivetrain.calculateRequiredRotationalRateWithFF( + swerveDrivetrain.getPassingTarget(redsideIfPointing)); + } + + // 5. Applying the drive request on the swerve drivetrain + // Uses SwerveRequestFieldCentric (from java.frc.robot.util to apply module optimization) + SwerveRequest drive = + fieldRelativeFunction.getAsBoolean() + ? fieldCentricDrive.withVelocityX(x).withVelocityY(y).withRotationalRate(turn) + : robotCentricDrive.withVelocityX(x).withVelocityY(y).withRotationalRate(turn); + + boolean isPointed = + Targeting.pointingAtTarget( + swerveDrivetrain + .travelAngleTo( + new Pose2d( + swerveDrivetrain.getVirtualTarget(redsideIfPointing, () -> false), + new Rotation2d())) + .getRadians() + + Math.PI, + swerveDrivetrain); + boolean intentionallyStationary = + (xSpdFunction.getAsDouble() < 0.1) + && (ySpdFunction.getAsDouble() < 0.1) + && (turningSpdFunction.getAsDouble() < 0.1); + if (braking.getAsBoolean() + && doPointing.getAsBoolean() + && isPointed + && intentionallyStationary) { + swerveDrivetrain.brakeSwerve(); + DogLog.log("IsItBraking?", true); + } else { + DogLog.log("IsItBraking?", false); + this.swerveDrivetrain.setControl(drive); + } + } + + @Override + public void end(boolean interrupted) { + // Applies SwerveDriveBrake (brakes the robot by turning wheels) + this.swerveDrivetrain.setControl(new SwerveRequest.SwerveDriveBrake()); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommandInArc.java b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommandInArc.java new file mode 100644 index 0000000..a7bb3db --- /dev/null +++ b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommandInArc.java @@ -0,0 +1,123 @@ +package frc.robot.commands.SwerveCommands; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; +import dev.doglog.DogLog; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.util.MathUtils.Vector3; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class SwerveJoystickCommandInArc extends Command { + protected final DoubleSupplier tangentSpdFunction, speedControlFunction; + + protected final Supplier poseToTarget; + + private final Pose2d center; + + protected final BooleanSupplier fieldRelativeFunction; + + protected final CommandSwerveDrivetrain swerveDrivetrain; + private final SwerveRequest.FieldCentric fieldCentricDrive = + new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.Velocity); + private final SwerveRequest.RobotCentric robotCentricDrive = + new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.Velocity); + private BooleanSupplier redSide; + + public SwerveJoystickCommandInArc( + Pose2d center, + DoubleSupplier tangentSpeedFunction, + DoubleSupplier speedControlFunction, + BooleanSupplier fieldRelativeFunction, + Supplier poseToTarget, + CommandSwerveDrivetrain swerveSubsystem) { + this.center = center; + this.tangentSpdFunction = tangentSpeedFunction; + this.fieldRelativeFunction = fieldRelativeFunction; + this.speedControlFunction = speedControlFunction; + this.poseToTarget = poseToTarget; + this.swerveDrivetrain = swerveSubsystem; + // Adds the subsystem as a requirement (prevents two commands from acting on subsystem at once) + addRequirements(swerveDrivetrain); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + // 1. Get real-time joystick inputs + double thetaFromCenter = + Math.atan2( + Vector3.subtract(new Vector3(swerveDrivetrain.getState().Pose), new Vector3(center)).y, + Vector3.subtract(new Vector3(swerveDrivetrain.getState().Pose), new Vector3(center)).x); + double tangentialSpeed = tangentSpdFunction.getAsDouble(); + tangentialSpeed = + Math.abs(tangentialSpeed) > Constants.OI.LEFT_JOYSTICK_DEADBAND ? tangentialSpeed : 0.0; + + double driveSpeed = + (Constants.Swerve.TELE_DRIVE_PERCENT_SPEED_RANGE * (speedControlFunction.getAsDouble())) + + Constants.Swerve.TELE_DRIVE_SLOW_MODE_SPEED_PERCENT; + + double xSpeed = tangentialSpeed * Math.cos(thetaFromCenter + Math.PI / 2f); + double ySpeed = tangentialSpeed * Math.sin(thetaFromCenter + Math.PI / 2f); + + // 2. Normalize inputs + double length = xSpeed * xSpeed + ySpeed * ySpeed; // actually length squared + if (length > 1d) { + length = Math.sqrt(length); + xSpeed /= length; + ySpeed /= length; + } + + // Apply Square (will be [0,1] since `speed` is [0,1]) + xSpeed = xSpeed * xSpeed * Math.signum(xSpeed); + ySpeed = ySpeed * ySpeed * Math.signum(ySpeed); + // 3. Apply deadband + xSpeed = Math.abs(xSpeed) > Constants.OI.LEFT_JOYSTICK_DEADBAND ? xSpeed : 0.0; + ySpeed = Math.abs(ySpeed) > Constants.OI.LEFT_JOYSTICK_DEADBAND ? ySpeed : 0.0; + + // 4. Make the driving smoother + // This is a double between TELE_DRIVE_SLOW_MODE_SPEED_PERCENT and + // TELE_DRIVE_FAST_MODE_SPEED_PERCENT + + xSpeed = xSpeed * driveSpeed * Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND; + ySpeed = ySpeed * driveSpeed * Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND; + + DogLog.log("Commands/arcjoystickCommand/xSpeed", xSpeed); + DogLog.log("Commands/arcjoystickCommand/ySpeed", ySpeed); + DogLog.log("Information/fieldCentric", fieldRelativeFunction.getAsBoolean()); + // 5. Applying the drive request on the swerve drivetrain + // Uses SwerveRequestFieldCentric (from java.frc.robot.util to apply module optimization) + double turn = swerveDrivetrain.calculateRequiredRotationalRateWithFF(poseToTarget.get()); + + DogLog.log("Commands/arcjoystickCommand/turnReq", turn); + + SwerveRequest drive = + !fieldRelativeFunction.getAsBoolean() + ? fieldCentricDrive.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(turn) + : robotCentricDrive + .withVelocityX(xSpeed) + .withVelocityY(ySpeed) + .withRotationalRate(turn); + + // Applies request + this.swerveDrivetrain.setControl(drive); + } // Drive counterclockwise with negative X (left)) + + @Override + public void end(boolean interrupted) { + // Applies SwerveDriveBrake (brakes the robot by turning wheels) + this.swerveDrivetrain.setControl(new SwerveRequest.Idle()); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommandWithCorrection.java b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommandWithCorrection.java new file mode 100644 index 0000000..32fcfc2 --- /dev/null +++ b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommandWithCorrection.java @@ -0,0 +1,246 @@ +package frc.robot.commands.SwerveCommands; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; +import dev.doglog.DogLog; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.IntakeVisionDetection; +import frc.robot.util.MathUtils.Vector2; +import frc.robot.util.VisionUtils; +import frc.robot.util.VisionUtils.IntakeVisionTarget; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +public class SwerveJoystickCommandWithCorrection extends Command { + protected final DoubleSupplier xSpdFunction, + ySpdFunction, + turningSpdFunction, + speedControlFunction; + protected final BooleanSupplier fieldRelativeFunction, + doPointing, + redsideIfPointing, + intakeExtended; + + protected final CommandSwerveDrivetrain swerveDrivetrain; + protected final BooleanSupplier doDriveAssist; + + private final IntakeVisionDetection intakeVision; + + private final SwerveRequest.FieldCentric fieldCentricDrive = + new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.Velocity); + private final SwerveRequest.RobotCentric robotCentricDrive = + new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.Velocity); + private boolean squaredTurn; + + public SwerveJoystickCommandWithCorrection( + DoubleSupplier frontBackFunction, + DoubleSupplier leftRightFunction, + DoubleSupplier turningSpdFunction, + DoubleSupplier speedControlFunction, + BooleanSupplier fieldRelativeFunction, + BooleanSupplier doPointing, + BooleanSupplier redSideIfPointing, + CommandSwerveDrivetrain swerveSubsystem, + IntakeVisionDetection intakeVision, + BooleanSupplier doDriveAssist, + BooleanSupplier intakeExtended) { + this.xSpdFunction = frontBackFunction; + this.ySpdFunction = leftRightFunction; + this.turningSpdFunction = turningSpdFunction; + this.speedControlFunction = speedControlFunction; + this.fieldRelativeFunction = fieldRelativeFunction; + this.squaredTurn = true; + this.swerveDrivetrain = swerveSubsystem; + this.doPointing = doPointing; + this.redsideIfPointing = redSideIfPointing; + this.intakeVision = intakeVision; + this.doDriveAssist = doDriveAssist; + this.intakeExtended = intakeExtended; + + // Adds the subsystem as a requirement (prevents two commands from acting on subsystem at once) + addRequirements(swerveDrivetrain); + } + + // Sets everything, not field relative + public SwerveJoystickCommandWithCorrection( + DoubleSupplier frontBackFunction, + DoubleSupplier leftRightFunction, + DoubleSupplier turningSpdFunction, + DoubleSupplier speedControlFunction, + CommandSwerveDrivetrain swerveSubsystem, + IntakeVisionDetection intakeVision, + BooleanSupplier intakeExtended) { + + this( + frontBackFunction, + leftRightFunction, + turningSpdFunction, + speedControlFunction, + () -> false, + () -> false, + () -> false, + swerveSubsystem, + intakeVision, + () -> false, + intakeExtended); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + // 1. Get real-time joystick inputs + double xSpeed = xSpdFunction.getAsDouble(); // xSpeed is actually front back (front +, back -) + double ySpeed = ySpdFunction.getAsDouble(); // ySpeed is actually left right (left +, right -) + double turningSpeed = + turningSpdFunction.getAsDouble(); // turning speed is (anti-clockwise +, clockwise -) + + // 2. Normalize inputs + double length = xSpeed * xSpeed + ySpeed * ySpeed; // acutally length squared + if (length > 1d) { + length = Math.sqrt(length); + xSpeed /= length; + ySpeed /= length; + } + + // Apply Square (will be [0,1] since `speed` is [0,1]) + xSpeed = xSpeed * xSpeed * Math.signum(xSpeed); + ySpeed = ySpeed * ySpeed * Math.signum(ySpeed); + if (squaredTurn) { + turningSpeed = turningSpeed * turningSpeed * Math.signum(turningSpeed); + } + // 3. Apply deadband + xSpeed = Math.abs(xSpeed) > Constants.OI.LEFT_JOYSTICK_DEADBAND ? xSpeed : 0.0; + ySpeed = Math.abs(ySpeed) > Constants.OI.LEFT_JOYSTICK_DEADBAND ? ySpeed : 0.0; + turningSpeed = + Math.abs(turningSpeed) > Constants.OI.RIGHT_JOYSTICK_DEADBAND ? turningSpeed : 0.0; + + // Applies slew rate limiter + xSpeed = xSpeed * Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND; + ySpeed = ySpeed * Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND; + turningSpeed = turningSpeed * Constants.Swerve.PHYSICAL_MAX_ANGLUAR_SPEED_RADIANS_PER_SECOND; + + // Final values to apply to drivetrain + final double x = xSpeed; + final double y = ySpeed; + + Pose2d currPose = swerveDrivetrain.getCurrentState().Pose; + IntakeVisionTarget targetResult = VisionUtils.intakeVisionTargetPose(currPose, intakeVision); + DogLog.log("Subsystems/IntakeVision/RetrievedPose", targetResult.pose()); + DogLog.log("Subsystems/IntakeVision/RetievedMode", targetResult.mode()); + + DogLog.log("Subsystems/Swerve/IntakeNull", targetResult == null); + + Pose2d targetPose = + (targetResult != null) && (intakeExtended.getAsBoolean()) ? targetResult.pose() : currPose; + + double turn = + (doPointing.getAsBoolean()) + ? (swerveDrivetrain.calculateRequiredRotationalRate( + swerveDrivetrain.travelAngleTo( + ((redsideIfPointing.getAsBoolean()) + ? (Constants.Landmarks.RED_HUB) + : (Constants.Landmarks.BLUE_HUB))))) + : (turningSpeed); + + if (Math.abs(turningSpdFunction.getAsDouble()) < Constants.IntakeVision.OVERRIDE_ROT_INPUT + && doDriveAssist.getAsBoolean() + && !doPointing.getAsBoolean() + && (Math.abs(targetPose.getX() - currPose.getX()) >= 0.01 + || Math.abs(targetPose.getY() - currPose.getY()) >= 0.01)) { + double omegaAssist = + Math.atan2(targetPose.getY() - currPose.getY(), targetPose.getX() - currPose.getX()); + turn += + Constants.IntakeVision.headingPIDDampen + * swerveDrivetrain.calculateRequiredRotationalRate(new Rotation2d(omegaAssist)); + DogLog.log("AssistHeading", omegaAssist); + } + + double velocityX = x; + double velocityY = y; + Vector2 translationAssist = translationAssist(targetPose); + if (doDriveAssist.getAsBoolean()) { + velocityX = + MathUtil.clamp( + x + translationAssist.x, + -Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND, + Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND); + velocityY = + MathUtil.clamp( + y + translationAssist.y, + -Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND, + Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND); + } + + DogLog.log("AssistVelocityX", translationAssist.x); + DogLog.log("AssistVelocityY", translationAssist.y); + + // 5. Applying the drive request on the swerve drivetrain + // Uses SwerveRequestFieldCentric (from java.frc.robot.util to apply module optimization) + SwerveRequest drive = + fieldRelativeFunction.getAsBoolean() + ? fieldCentricDrive + .withVelocityX(velocityX) + .withVelocityY(velocityY) + .withRotationalRate(turn) + : robotCentricDrive + .withVelocityX(velocityX) + .withVelocityY(velocityY) + .withRotationalRate(turn); + + // Applies request + this.swerveDrivetrain.setControl(drive); + } // Drive counterclockwise with negative X (left)) + + public Vector2 translationAssist(Pose2d targetPose) { + int n = 2; + + double p1x = swerveDrivetrain.getCurrentState().Pose.getX(); + double p1y = swerveDrivetrain.getCurrentState().Pose.getY(); + + Pose2d p2 = + new Pose2d( + p1x + Constants.IntakeVision.lookAheadTime * xSpdFunction.getAsDouble(), + p1y + Constants.IntakeVision.lookAheadTime * ySpdFunction.getAsDouble(), + new Rotation2d()); + + double dist = + Math.abs( + ((p2.getY() - p1y) * targetPose.getX()) + - ((p2.getX() - p1x) * targetPose.getY()) + + p2.getX() * p1y + - p2.getY() * p1x) + / Math.sqrt(Math.pow(((p2.getY() - p1y)), 2) + Math.pow((p2.getX() - p1x), 2)); + + DogLog.log("AssistDistance", dist); + + double assistMagnitude = Math.pow((dist * Constants.IntakeVision.kP), 1.0 / n); + // double assistDirection = Math.atan2(p1y - targetPose.getY(), p1x - targetPose.getX()); + // incorrect + double assistDirection = Math.atan2(targetPose.getY() - p1y, targetPose.getX() - p1x); + + DogLog.log("AssistMag", assistMagnitude); + DogLog.log("AssistDir", assistDirection); + DogLog.log("AssistP2", p2); + + return new Vector2( + assistMagnitude * Math.cos(assistDirection), assistMagnitude * Math.sin(assistDirection)); + } + + @Override + public void end(boolean interrupted) { + // Applies SwerveDriveBrake (brakes the robot by turning wheels) + this.swerveDrivetrain.setControl(new SwerveRequest.SwerveDriveBrake()); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommandWithPointing.java b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommandWithPointing.java new file mode 100644 index 0000000..2b6d8ba --- /dev/null +++ b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommandWithPointing.java @@ -0,0 +1,114 @@ +package frc.robot.commands.SwerveCommands; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; +import dev.doglog.DogLog; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class SwerveJoystickCommandWithPointing extends Command { + protected final DoubleSupplier xSpdFunction, ySpdFunction; + + protected Supplier poseToTarget; + + protected final DoubleSupplier speedControlFunction; + + protected final BooleanSupplier fieldRelativeFunction; + + protected final CommandSwerveDrivetrain swerveDrivetrain; + private final SwerveRequest.FieldCentric fieldCentricDrive = + new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.Velocity); + private final SwerveRequest.RobotCentric robotCentricDrive = + new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.Velocity); + + public SwerveJoystickCommandWithPointing( + DoubleSupplier frontBackFunction, + DoubleSupplier leftRightFunction, + DoubleSupplier speedControlFunction, + BooleanSupplier fieldRelativeFunction, + Supplier poseToTarget, + CommandSwerveDrivetrain swerveSubsystem) { + this.xSpdFunction = frontBackFunction; + this.ySpdFunction = leftRightFunction; + this.speedControlFunction = speedControlFunction; + this.fieldRelativeFunction = fieldRelativeFunction; + this.swerveDrivetrain = swerveSubsystem; + this.poseToTarget = poseToTarget; + // Adds the subsystem as a requirement (prevents two commands from acting on subsystem at once) + addRequirements(swerveDrivetrain); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + // 1. Get real-time joystick inputs + double xSpeed = xSpdFunction.getAsDouble(); // xSpeed is actually front back (front +, back -) + double ySpeed = ySpdFunction.getAsDouble(); // ySpeed is actually left right (left +, right -) + + // 2. Normalize inputs + double length = xSpeed * xSpeed + ySpeed * ySpeed; // actually length squared + if (length > 1d) { + length = Math.sqrt(length); + xSpeed /= length; + ySpeed /= length; + } + + // Apply Square (will be [0,1] since `speed` is [0,1]) + xSpeed = xSpeed * xSpeed * Math.signum(xSpeed); + ySpeed = ySpeed * ySpeed * Math.signum(ySpeed); + + // 3. Apply deadband + xSpeed = Math.abs(xSpeed) > Constants.OI.LEFT_JOYSTICK_DEADBAND ? xSpeed : 0.0; + ySpeed = Math.abs(ySpeed) > Constants.OI.LEFT_JOYSTICK_DEADBAND ? ySpeed : 0.0; + + // 4. Make the driving smoother + // This is a double between TELE_DRIVE_SLOW_MODE_SPEED_PERCENT and + // TELE_DRIVE_FAST_MODE_SPEED_PERCENT + double driveSpeed = + (Constants.Swerve.TELE_DRIVE_PERCENT_SPEED_RANGE * (speedControlFunction.getAsDouble())) + + Constants.Swerve.TELE_DRIVE_SLOW_MODE_SPEED_PERCENT; + + xSpeed = xSpeed * driveSpeed * Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND; + ySpeed = ySpeed * driveSpeed * Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND; + + // Final values to apply to drivetrain + + DogLog.log("Commands/joystickCommand/xSpeed", xSpeed); + DogLog.log("Commands/joystickCommand/ySpeed", ySpeed); + DogLog.log("Information/fieldCentric", fieldRelativeFunction.getAsBoolean()); + // 5. Applying the drive request on the swerve drivetrain + // Uses SwerveRequestFieldCentric (from java.frc.robot.util to apply module optimization) + double turn = swerveDrivetrain.calculateRequiredRotationalRateWithFF(poseToTarget.get()); + + DogLog.log("Commands/joystickCommand/turnReq", turn); + + SwerveRequest drive = + !fieldRelativeFunction.getAsBoolean() + ? fieldCentricDrive.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(turn) + : robotCentricDrive + .withVelocityX(xSpeed) + .withVelocityY(ySpeed) + .withRotationalRate(turn); + + // Applies request + this.swerveDrivetrain.setControl(drive); + } // Drive counterclockwise with negative X (left)) + + @Override + public void end(boolean interrupted) { + // Applies SwerveDriveBrake (brakes the robot by turning wheels) + this.swerveDrivetrain.setControl(new SwerveRequest.Idle()); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickDefaultCommand.java b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickDefaultCommand.java new file mode 100644 index 0000000..93da52a --- /dev/null +++ b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickDefaultCommand.java @@ -0,0 +1,52 @@ +package frc.robot.commands.SwerveCommands; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.IntakeVisionDetection; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +public class SwerveJoystickDefaultCommand extends SequentialCommandGroup { + public SwerveJoystickDefaultCommand( + DoubleSupplier frontBackFunction, + DoubleSupplier leftRightFunction, + DoubleSupplier rotationFunction, + DoubleSupplier speedFunction, + BooleanSupplier fieldRelativeFunction, + BooleanSupplier doPointing, + BooleanSupplier redSideIfPointing, + CommandSwerveDrivetrain swerveSubsystem, + IntakeVisionDetection intakeVision, + BooleanSupplier doDriveAssist, + BooleanSupplier intakeVisionLockout, + BooleanSupplier intakeExtended) { + addCommands( + Commands.either( + new SwerveJoystickCommand( + frontBackFunction, + leftRightFunction, + rotationFunction, + speedFunction, // slowmode when left shoulder is pressed, otherwise fast + () -> true, + doPointing, // joystick.a().getAsBoolean() + () -> false, + redSideIfPointing, + swerveSubsystem, + () -> false, + () -> false), + new SwerveJoystickCommandWithCorrection( + frontBackFunction, + leftRightFunction, + rotationFunction, + speedFunction, + () -> true, + doPointing, + redSideIfPointing, + swerveSubsystem, + intakeVision, + doDriveAssist, + intakeExtended), + () -> !intakeVisionLockout.getAsBoolean())); + } +} diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java new file mode 100644 index 0000000..4d5b9df --- /dev/null +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -0,0 +1,349 @@ +package frc.robot.generated; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.hardware.*; +import com.ctre.phoenix6.signals.*; +import com.ctre.phoenix6.swerve.*; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.*; +import frc.robot.Constants; +import frc.robot.subsystems.CommandSwerveDrivetrain; + +// Generated by the Tuner X Swerve Project Generator +// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +public class TunerConstants { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = + new Slot0Configs() + .withKP(Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_STEER_PID_VALUES.KP) + .withKI(Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_STEER_PID_VALUES.KI) + .withKD(Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_STEER_PID_VALUES.KD) + .withKS(Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_STEER_PID_VALUES.KS) + .withKV(Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_STEER_PID_VALUES.KV) + .withKA(Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_STEER_PID_VALUES.KA) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = + new Slot0Configs() + .withKP(Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_PID_VALUES.KP) + .withKI(Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_PID_VALUES.KI) + .withKD(Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_PID_VALUES.KD) + .withKS(Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_PID_VALUES.KS) + .withKV(Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_PID_VALUES.KV); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = + DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = + SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(100.0); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively + // low + // stator current limit to help avoid brownouts without impacting performance. + .withSupplyCurrentLimit(Amps.of(52.5)) + .withSupplyCurrentLimitEnable(true)); + + private static final TalonFXConfiguration steerInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively + // low + // stator current limit to help avoid brownouts without impacting performance. + .withSupplyCurrentLimit(Amps.of(35)) + .withSupplyCurrentLimitEnable(true) + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true)); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = Constants.Swerve.CAN_BUS; + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.85); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = Constants.Swerve.WHICH_SWERVE_ROBOT.COUPLE_RATIO; + + private static final double kDriveGearRatio = + Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_LEVEL.DRIVE_GEAR_RATIO; + private static final double kSteerGearRatio = + Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_LEVEL.STEER_GEAR_RATIO; + private static final Distance kWheelRadius = Inches.of(2); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 40; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = + new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + ConstantCreator = + new SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + // Front Left + private static final int kFrontLeftDriveMotorId = 4; + private static final int kFrontLeftSteerMotorId = 3; + private static final int kFrontLeftEncoderId = 21; + private static final Angle kFrontLeftEncoderOffset = + Constants.Swerve.WHICH_SWERVE_ROBOT.FRONT_LEFT_ENCODER_OFFSET; + private static final boolean kFrontLeftSteerMotorInverted = + Constants.Swerve.WHICH_SWERVE_ROBOT.INVERTED_MODULES; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = + Constants.Swerve.WHICH_SWERVE_ROBOT.ROBOT_DIMENSIONS.length.div(2.0); + private static final Distance kFrontLeftYPos = + Constants.Swerve.WHICH_SWERVE_ROBOT.ROBOT_DIMENSIONS.width.div(2.0); + + // Front Right + private static final int kFrontRightDriveMotorId = 6; + private static final int kFrontRightSteerMotorId = 5; + private static final int kFrontRightEncoderId = 22; + private static final Angle kFrontRightEncoderOffset = + Constants.Swerve.WHICH_SWERVE_ROBOT.FRONT_RIGHT_ENCODER_OFFSET; + private static final boolean kFrontRightSteerMotorInverted = + Constants.Swerve.WHICH_SWERVE_ROBOT.INVERTED_MODULES; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = + Constants.Swerve.WHICH_SWERVE_ROBOT.ROBOT_DIMENSIONS.length.div(2.0); + private static final Distance kFrontRightYPos = + Constants.Swerve.WHICH_SWERVE_ROBOT.ROBOT_DIMENSIONS.width.div(-2.0); + + // Back Left + private static final int kBackLeftDriveMotorId = 2; + private static final int kBackLeftSteerMotorId = 1; + private static final int kBackLeftEncoderId = 20; + private static final Angle kBackLeftEncoderOffset = + Constants.Swerve.WHICH_SWERVE_ROBOT.BACK_LEFT_ENCODER_OFFSET; + private static final boolean kBackLeftSteerMotorInverted = + Constants.Swerve.WHICH_SWERVE_ROBOT.INVERTED_MODULES; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = + Constants.Swerve.WHICH_SWERVE_ROBOT.ROBOT_DIMENSIONS.length.div(-2.0); + private static final Distance kBackLeftYPos = + Constants.Swerve.WHICH_SWERVE_ROBOT.ROBOT_DIMENSIONS.width.div(2.0); + + // Back Right + private static final int kBackRightDriveMotorId = 8; + private static final int kBackRightSteerMotorId = 7; + private static final int kBackRightEncoderId = 23; + private static final Angle kBackRightEncoderOffset = + Constants.Swerve.WHICH_SWERVE_ROBOT.BACK_RIGHT_ENCODER_OFFSET; + private static final boolean kBackRightSteerMotorInverted = + Constants.Swerve.WHICH_SWERVE_ROBOT.INVERTED_MODULES; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = + Constants.Swerve.WHICH_SWERVE_ROBOT.ROBOT_DIMENSIONS.length.div(-2.0); + private static final Distance kBackRightYPos = + Constants.Swerve.WHICH_SWERVE_ROBOT.ROBOT_DIMENSIONS.width.div(-2.0); + + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + kFrontLeftXPos, + kFrontLeftYPos, + kInvertLeftSide, + kFrontLeftSteerMotorInverted, + kFrontLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + kFrontRightXPos, + kFrontRightYPos, + kInvertRightSide, + kFrontRightSteerMotorInverted, + kFrontRightEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + kBackLeftXPos, + kBackLeftYPos, + kInvertLeftSide, + kBackLeftSteerMotorInverted, + kBackLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + kBackRightXPos, + kBackRightYPos, + kInvertRightSide, + kBackRightSteerMotorInverted, + kBackRightEncoderInverted); + + /** + * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot + * program,. + */ + public static CommandSwerveDrivetrain createDrivetrain() { + return new CommandSwerveDrivetrain( + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); + } + + /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) { + super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set + * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules) { + super( + TalonFX::new, + TalonFX::new, + CANcoder::new, + drivetrainConstants, + odometryUpdateFrequency, + modules); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set + * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation in the form + * [x, y, theta]áµ€, with units in meters and radians + * @param visionStandardDeviation The standard deviation for vision calculation in the form [x, + * y, theta]áµ€, with units in meters and radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super( + TalonFX::new, + TalonFX::new, + CANcoder::new, + drivetrainConstants, + odometryUpdateFrequency, + odometryStandardDeviation, + visionStandardDeviation, + modules); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java new file mode 100644 index 0000000..dd5f0a8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -0,0 +1,429 @@ +package frc.robot.subsystems; + +import choreo.Choreo.TrajectoryLogger; +import choreo.auto.AutoFactory; +import choreo.trajectory.SwerveSample; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.ctre.phoenix6.swerve.utility.WheelForceCalculator; +import dev.doglog.DogLog; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.controller.PIDController; +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.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; +import frc.robot.Constants; +import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; +import frc.robot.util.MathUtils.MiscMath; +import frc.robot.util.Targeting; +import java.util.Arrays; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; + +/** + * Class that extends the Phoenix 6 SwerveDrivetrain class and implements Subsystem so it can easily + * be used in command-based projects. + */ +public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { + private Translation2d virtualTarget = null; + private boolean virtualTargetComputedThisLoop = false; + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; + /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ + private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; + /* Keep track if we've ever applied the operator perspective before or not */ + private boolean m_hasAppliedOperatorPerspective = false; + + private final PIDController m_pathXController = + new PIDController( + Constants.Swerve.WHICH_SWERVE_ROBOT.CHOREO_PID_VALUES.kPX, + Constants.Swerve.WHICH_SWERVE_ROBOT.CHOREO_PID_VALUES.kIX, + Constants.Swerve.WHICH_SWERVE_ROBOT.CHOREO_PID_VALUES.kDX); + private final PIDController m_pathYController = + new PIDController( + Constants.Swerve.WHICH_SWERVE_ROBOT.CHOREO_PID_VALUES.kPY, + Constants.Swerve.WHICH_SWERVE_ROBOT.CHOREO_PID_VALUES.kIY, + Constants.Swerve.WHICH_SWERVE_ROBOT.CHOREO_PID_VALUES.kDY); + private final PIDController m_pathThetaController = + new PIDController( + Constants.Swerve.WHICH_SWERVE_ROBOT.CHOREO_PID_VALUES.kPR, + Constants.Swerve.WHICH_SWERVE_ROBOT.CHOREO_PID_VALUES.kIR, + Constants.Swerve.WHICH_SWERVE_ROBOT.CHOREO_PID_VALUES.kDR); + + private SwerveDriveState currentState; + + private final SwerveRequest.SwerveDriveBrake m_brake = new SwerveRequest.SwerveDriveBrake(); + // private final SwerveRequest m_better_brake = new SwerveRequest().apply(new + // SwerveControlParameters(), getModules()) + // * Front Left, Front Right, Back Left, Back Right. This means if you need + private final SwerveRequest.ApplyFieldSpeeds m_pathApplyFieldSpeeds = + new SwerveRequest.ApplyFieldSpeeds(); + + // private final Field2d field = new Field2d(); + + // private final StructPublisher posePublisher = + // NetworkTableInstance.getDefault().getStructTopic("RobotPose", Pose2d.struct).publish(); + private PIDController headingPIDController = + new PIDController( + 3.65, // 4 was good + 0, // + 0); // -13 was good + + // 15, 0, 0 w/o FF + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) { + super(drivetrainConstants, modules); + + currentState = getState(); + + headingPIDController.setIZone(0.14); + headingPIDController.setIntegratorRange(0.0, Math.PI / 4); // 0.3 before + headingPIDController.enableContinuousInput(-Math.PI, Math.PI); // 0.3 before + m_pathThetaController.enableContinuousInput(-Math.PI, Math.PI); + + // SmartDashboard.putData(field); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set to + * 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules) { + super(drivetrainConstants, odometryUpdateFrequency, modules); + // SmartDashboard.putData(field); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set to + * 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation in the form + * [x, y, theta]áµ€, with units in meters and radians + * @param visionStandardDeviation The standard deviation for vision calculation in the form [x, y, + * theta]áµ€, with units in meters and radians + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super( + drivetrainConstants, + odometryUpdateFrequency, + odometryStandardDeviation, + visionStandardDeviation, + modules); + // SmartDashboard.putData(field); + } + + public AutoFactory createAutoFactory() { + return createAutoFactory((sample, isStart) -> {}); + } + + public AutoFactory createAutoFactory(TrajectoryLogger trajLogger) { + return new AutoFactory( + () -> getCurrentState().Pose, + this::resetPose, + this::followPath, + true, + this, + trajLogger); // getState().pose + } + + public void followPath(SwerveSample sample) { + m_pathThetaController.enableContinuousInput(-Math.PI, Math.PI); // every run? + + Pose2d pose = getState().Pose; + + ChassisSpeeds targetSpeeds = sample.getChassisSpeeds(); + targetSpeeds.vxMetersPerSecond += m_pathXController.calculate(pose.getX(), sample.x); + targetSpeeds.vyMetersPerSecond += m_pathYController.calculate(pose.getY(), sample.y); + targetSpeeds.omegaRadiansPerSecond += + m_pathThetaController.calculate(pose.getRotation().getRadians(), sample.heading); + + setControl( + m_pathApplyFieldSpeeds + .withSpeeds(targetSpeeds) + .withWheelForceFeedforwardsX(sample.moduleForcesX()) + .withWheelForceFeedforwardsY(sample.moduleForcesY())); + + DogLog.log("Subsystems/Swerve/SampleX", sample.x); + DogLog.log("Subsystems/Swerve/SampleY", sample.y); + DogLog.log("Subsystems/Swerve/SampleHeading", sample.heading); + DogLog.log("Subsystems/Swerve/SampleVX", sample.vx); + DogLog.log("Subsystems/Swerve/SampleVY", sample.vy); + DogLog.log("Subsystems/Swerve/SampleAngularVelocity", sample.omega); + DogLog.log("Subsystems/Swerve/SampleAX", sample.ax); + DogLog.log("Subsystems/Swerve/SampleAY", sample.ay); + DogLog.log("Subsystems/Swerve/SampleAngularAcceleration", sample.alpha); + } + + /** + * Returns a command that applies the specified control request to this swerve drivetrain. + * + * @param request Function returning the request to apply + * @return Command to run + */ + public Command applyRequest(Supplier requestSupplier) { + return run(() -> this.setControl(requestSupplier.get())); + } + + public Translation2d getVirtualTarget(BooleanSupplier redside, BooleanSupplier override) { + if (override.getAsBoolean()) { + Translation2d target = + (redside.getAsBoolean()) + ? (Constants.Landmarks.RED_HUB.getTranslation()) + : (Constants.Landmarks.BLUE_HUB.getTranslation()); + DogLog.log("VirtualTarget", new Pose2d(target, new Rotation2d())); + return target; + } + if (virtualTarget == null || !virtualTargetComputedThisLoop) { + virtualTarget = Targeting.computeVirtualTarget(Targeting.getHub(redside), this); + virtualTargetComputedThisLoop = true; + } + DogLog.log("VirtualTarget", new Pose2d(virtualTarget, new Rotation2d())); + return virtualTarget; + } + + public Translation2d getPassingTarget(BooleanSupplier redside) { + return redside.getAsBoolean() + ? this.getPose() + .nearest( + Arrays.asList(Constants.Landmarks.RED_PASSING_L, Constants.Landmarks.RED_PASSING_R)) + .getTranslation() + : this.getPose() + .nearest( + Arrays.asList( + Constants.Landmarks.BLUE_PASSING_L, Constants.Landmarks.BLUE_PASSING_R)) + .getTranslation(); + } + + public SwerveDriveState getCurrentState() { + return currentState; + } + + public ChassisSpeeds getRobotSpeeds() { + return currentState.Speeds; + } + + public void applyFieldSpeeds( + ChassisSpeeds speeds, WheelForceCalculator.Feedforwards feedforwards) { + setControl( + m_pathApplyFieldSpeeds + .withSpeeds(speeds) + .withWheelForceFeedforwardsX(feedforwards.x_newtons) + .withWheelForceFeedforwardsY(feedforwards.y_newtons)); + } + + public void applyOneFieldSpeeds(ChassisSpeeds speeds) { + setControl( + m_pathApplyFieldSpeeds.withSpeeds(speeds).withDriveRequestType(DriveRequestType.Velocity)); + } + + public double getSpeedMagnitude() { + double xSpeed = getFieldSpeeds().vxMetersPerSecond; + double ySpeed = getFieldSpeeds().vyMetersPerSecond; + return Math.sqrt((xSpeed * xSpeed) + (ySpeed * ySpeed)); + } + + public Pose2d getPose() { + return currentState.Pose; + } + + public double distanceToPose(Pose2d target) { + return getPose().getTranslation().getDistance(target.getTranslation()); + } + + public Rotation2d travelAngleTo(Pose2d targetPose) { + double deltaX = targetPose.getX() - getCurrentState().Pose.getX(); + double deltaY = targetPose.getY() - getCurrentState().Pose.getY(); + return new Rotation2d(Math.atan2(deltaY, deltaX)); + } + + public void resetPose(Pose2d pose) { + super.resetPose(pose); + } + + @Override + public void periodic() { + virtualTargetComputedThisLoop = false; + DogLog.log("SpeedMagnitude", getSpeedMagnitude()); + /* + * Periodically try to apply the operator perspective. + * If we haven't applied the operator perspective before, then we should apply + * it regardless of DS state. + * This allows us to correct the perspective in case the robot code restarts + * mid-match. + * Otherwise, only check and apply the operator perspective if the DS is + * disabled. + * This ensures driving behavior doesn't change until an explicit disable event + * occurs during testing. + */ + currentState = getState(); + DogLog.log("Subsystems/Swerve/AccumulatedError", headingPIDController.getAccumulatedError()); + + if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance() + .ifPresent( + allianceColor -> { + setOperatorPerspectiveForward( + allianceColor == Alliance.Red + ? kRedAlliancePerspectiveRotation + : kBlueAlliancePerspectiveRotation); + m_hasAppliedOperatorPerspective = true; + }); + } + + if (this.getCurrentCommand() != null) { + DogLog.log("Subsystems/Swerve/CurrentCommand", this.getCurrentCommand().toString()); + } + DogLog.log("Subsystems/Swerve/Pose", getCurrentState().Pose); + + DogLog.log("Subsystems/Swerve/CurrPoseX", getCurrentState().Pose.getX()); + DogLog.log("Subsystems/Swerve/CurrPoseY", getCurrentState().Pose.getY()); + DogLog.log("Subsystems/Swerve/CurrPoseRotRads", getCurrentState().Pose.getRotation()); + DogLog.log( + "Subsystems/Swerve/CurrPoseRotDegs", getCurrentState().Pose.getRotation().getDegrees()); + + // DogLog.log( + // "Subsystems/Swerve/DistanceToHub", + // MiscUtils.getDistanceToHub(RobotContainer::isRedAlliance, this)); + DogLog.log("Subsystems/Swerve/TurningSpeedActual", getFieldSpeeds().omegaRadiansPerSecond); + } + + @Override + public void addVisionMeasurement(Pose2d visionRobotPose, double timestampSeconds) { + super.addVisionMeasurement(visionRobotPose, Utils.fpgaToCurrentTime(timestampSeconds)); + } + + @Override + public void addVisionMeasurement( + Pose2d visionRobotPose, double timestampSeconds, Matrix visionMeasurementStdDevs) { + super.addVisionMeasurement( + visionRobotPose, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); + } + + public ChassisSpeeds getFieldSpeeds() { + return ChassisSpeeds.fromRobotRelativeSpeeds( + currentState.Speeds, currentState.Pose.getRotation()); + } + + public double calculateRequiredRotationalRate(Rotation2d targetRotation) { + double omega = + // headingProfiledPIDController.getSetpoint().velocity+ + headingPIDController.calculate( + currentState.Pose.getRotation().getRadians(), targetRotation.getRadians()); + double max = Constants.Swerve.MAX_HEADING_TRACKING_ROT_RATE_RADS_PER_SECOND; + boolean clamp = Math.abs(omega) > max; + if (clamp) { + omega = MathUtil.clamp(omega, -max, max); + } + DogLog.log("Subsystems/Swerve/RotationController/clamped", clamp); + DogLog.log("Subsystems/Swerve/TargetRotationsDegrees", targetRotation.getDegrees()); + return omega; + } + + public double calculateRequiredRotationalRateWithFF(Translation2d targetPoint) { + Translation2d robotPos = getCurrentState().Pose.getTranslation(); + double dx = targetPoint.getX() - robotPos.getX(); + double dy = targetPoint.getY() - robotPos.getY(); + double r2 = dx * dx + dy * dy; + + ChassisSpeeds fieldSpeeds = + ChassisSpeeds.fromRobotRelativeSpeeds(currentState.Speeds, currentState.Pose.getRotation()); + double vx = fieldSpeeds.vxMetersPerSecond; + double vy = fieldSpeeds.vyMetersPerSecond; + + double omegaFF = 0.0; + if (r2 > Constants.Swerve.FF_RADIUS_M2) { + DogLog.log("vx", vx); + DogLog.log("vy", vy); + + omegaFF = (dy * vx - dx * vy) / r2; + } + + Rotation2d targetRotation = new Rotation2d(Math.atan2(dy, dx) + Math.PI); + double currentRads = MiscMath.normalizeAngle(currentState.Pose.getRotation()).getRadians(); + double targetRads = MiscMath.normalizeAngle(targetRotation).getRadians(); + double omegaPID = headingPIDController.calculate(currentRads, targetRads); + + DogLog.log("CurrentRads", currentRads); + DogLog.log("TargetRads", targetRads); + // double sign = 1; + // if (omegaPID < 0) { + // sign = -1; + // } + + double angleDifference = + Math.atan2( + Math.sin(targetRotation.getRadians() - currentState.Pose.getRotation().getRadians()), + Math.cos(targetRotation.getRadians() - currentState.Pose.getRotation().getRadians())); + + // if ((Math.abs(angleDifference) < 0.87) && Math.abs(omegaPID) >= 1.295) { + // omegaPID = Math.abs(angleDifference) * 2.2 * sign; + // } + + if (Math.abs(angleDifference) < 0.52) { + omegaFF = 0; + } + + // if (Math.abs(angleDifference) < 0.1) { + // omegaPID = 0; + // } + + // if (Math.abs(angleDifference) < 0.06) { + // omegaFF = 0; + // } + + double omega = (omegaFF) + omegaPID; + + DogLog.log("Subsystems/Swerve/RotationController/omegaFF", omegaFF); + DogLog.log("Subsystems/Swerve/RotationController/omegaPID", omegaPID); + DogLog.log("Subsystems/Swerve/TargetRotationsDegrees", targetRotation.getDegrees()); + DogLog.log("AngleDifference", angleDifference); + DogLog.log("dx", dx); + DogLog.log("dy", dy); + return omega; + } + + public void brakeSwerve() { + setControl(m_brake); + } +} diff --git a/src/main/java/frc/robot/subsystems/FuelGaugeDetection.java b/src/main/java/frc/robot/subsystems/FuelGaugeDetection.java new file mode 100644 index 0000000..606ffe7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/FuelGaugeDetection.java @@ -0,0 +1,218 @@ +package frc.robot.subsystems; + +import dev.doglog.DogLog; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Constants.FuelGaugeDetection.FuelGauge; +import frc.robot.Constants.FuelGaugeDetection.GaugeCalculationType; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +public class FuelGaugeDetection extends SubsystemBase { + private static ArrayList latestRawMeasurements = new ArrayList<>(); + private static ArrayList latestMultipleMeasurements = new ArrayList<>(); + + private final PhotonCamera photonCamera; + private PhotonPipelineResult latestVisionResult; + + private double rawArea; + private double smoothedArea; + private double multipleBallsArea; + private double smoothedMultipleBallsArea; + + private FuelGauge latestRawGauge; + private FuelGauge latestSmoothedGauge; + private FuelGauge latestMultipleBallsGauge; + private FuelGauge latestSmoothedMultipleBallsGauge; + + public FuelGaugeDetection(Constants.FuelGaugeDetection.FuelGaugeCamera cameraID) { + photonCamera = new PhotonCamera(cameraID.toString()); + } + + @Override + public void periodic() { + if (!cameraConnected()) return; + if (!validVisionResult(photonCamera.getAllUnreadResults())) return; + + updateVisionResult(); + } + + private boolean cameraConnected() { + boolean cameraConnected = photonCamera.isConnected(); + DogLog.log("Subsystems/FuelGauge/CameraStatus", cameraConnected); + return cameraConnected; + } + + private boolean validVisionResult(List results) { + if (results.isEmpty()) return false; + latestVisionResult = results.get(results.size() - 1); + + return latestVisionResult != null; + } + + private void updateVisionResult() { + Optional ball = getLargestBall(); + ball.ifPresentOrElse( + b -> { + DogLog.log("Subsystems/FuelGauge/BallPresent", true); + DogLog.log("Subsystems/FuelGauge/BallYaw", b.getYaw()); + DogLog.log("Subsystems/FuelGauge/BallPitch", b.getPitch()); + DogLog.log("Subsystems/FuelGauge/BallSkew", b.getSkew()); + + rawArea = b.getArea(); + smoothedArea = smoothFromList(latestRawMeasurements, rawArea); + multipleBallsArea = getLargestBallsAvg(Constants.FuelGaugeDetection.BALLS_TO_AVG); + smoothedMultipleBallsArea = smoothFromList(latestMultipleMeasurements, multipleBallsArea); + + DogLog.log("Subsystems/FuelGauge/Area/RawArea", rawArea); + DogLog.log("Subsystems/FuelGauge/Area/SmoothedRawArea", smoothedArea); + DogLog.log("Subsystems/FuelGauge/Area/MultipleBallsArea", multipleBallsArea); + DogLog.log( + "Subsystems/FuelGauge/Area/SmoothedMultipleBallsArea", smoothedMultipleBallsArea); + + calculateFuelGaugeState( + rawArea, smoothedArea, multipleBallsArea, smoothedMultipleBallsArea); + }, + () -> DogLog.log("Subsystems/FuelGauge/BallPresent", false)); + } + + private double smoothFromList(ArrayList list, double area) { + double smoothedArea = 0.0; + + list.add(area); + while (list.size() > Constants.FuelGaugeDetection.MAX_FUEL_GAUGE_MEASUREMENTS) { + list.remove(0); + } + + if (!list.isEmpty()) { + for (double rawArea : list) smoothedArea += rawArea; + smoothedArea /= list.size(); + } else smoothedArea = area; + + return smoothedArea; + } + + private void calculateFuelGaugeState( + double rawArea, double smoothedArea, double avgMultipleBalls, double smoothedMultipleBalls) { + latestRawGauge = setFuelGauge(rawArea); + latestSmoothedGauge = setFuelGauge(smoothedArea); + latestMultipleBallsGauge = setFuelGauge(avgMultipleBalls); + latestSmoothedMultipleBallsGauge = setFuelGauge(smoothedMultipleBalls); + + DogLog.log("Subsystems/FuelGauge/Gauge/RawGauge", latestRawGauge.toString()); + DogLog.log("Subsystems/FuelGauge/Gauge/SmoothedGauge", latestSmoothedGauge.toString()); + DogLog.log( + "Subsystems/FuelGauge/Gauge/MultipleBallsGauge", latestMultipleBallsGauge.toString()); + DogLog.log( + "Subsystems/FuelGauge/Gauge/SmoothedMultipleBallsGauge", + latestSmoothedMultipleBallsGauge.toString()); + } + + private FuelGauge setFuelGauge(double area) { + if (area < FuelGauge.EMPTY.getThreshold()) return FuelGauge.EMPTY; + if (area < FuelGauge.LOW.getThreshold()) return FuelGauge.LOW; + if (area < FuelGauge.MEDIUM.getThreshold()) return FuelGauge.MEDIUM; + return FuelGauge.FULL; + } + + private Optional getLargestBall() { + if (latestVisionResult == null) return Optional.empty(); + + List targets = latestVisionResult.getTargets(); + if (targets.isEmpty()) return Optional.empty(); + + return targets.stream().max((a, b) -> Double.compare(a.getArea(), b.getArea())); + } + + public double getArea(GaugeCalculationType type) { + switch (type) { + case RAW: + default: + return rawArea; + case SMOOTHED: + return smoothedArea; + case MULTIPLE_BALLS: + return multipleBallsArea; + case SMOOTHED_MULTIPLE_BALLS: + return smoothedMultipleBallsArea; + } + } + + public FuelGauge getGauge(GaugeCalculationType type) { + switch (type) { + case RAW: + default: + return latestRawGauge; + case SMOOTHED: + return latestSmoothedGauge; + case MULTIPLE_BALLS: + return latestMultipleBallsGauge; + case SMOOTHED_MULTIPLE_BALLS: + return latestSmoothedMultipleBallsGauge; + } + } + + public boolean GaugeLessThanEqualTo(GaugeCalculationType type, FuelGauge target) { + return getGauge(type).getThreshold() <= target.getThreshold(); + } + + private double getLargestBallsAvg(int numBalls) { + double sum = 0.0; + if (latestVisionResult == null) return 0.0; + List targets = latestVisionResult.getTargets(); + if (targets.isEmpty()) return 0.0; + + numBalls = Math.min(numBalls, targets.size()); + + for (int i = 0; i < numBalls; i++) { + sum += targets.get(i).getArea(); + } + + return sum / numBalls; + } + + public Optional getYawToBall() { + return getLargestBall().map(PhotonTrackedTarget::getYaw); + } + + public Optional getAreaOfBall() { + return getLargestBall().map(PhotonTrackedTarget::getArea); + } + + public Optional getPitchToBall() { + return getLargestBall().map(PhotonTrackedTarget::getPitch); + } + + public Optional getSkewToBall() { + return getLargestBall().map(PhotonTrackedTarget::getSkew); + } + + public FuelGauge getGaugeDefault() { + // return getGauge(GaugeCalculationType.SMOOTHED_MULTIPLE_BALLS); + + double currentMeasurement = smoothedMultipleBallsArea; // or whichever measurement we use + + if (currentMeasurement <= FuelGauge.EMPTY.getThreshold()) return FuelGauge.EMPTY; + if (currentMeasurement <= FuelGauge.LOW.getThreshold()) return FuelGauge.LOW; + if (currentMeasurement <= FuelGauge.MEDIUM.getThreshold()) return FuelGauge.MEDIUM; + return FuelGauge.FULL; + } + + public String getCurrentFuelGaugeStateAsHex() { + double currentMeasurement = smoothedMultipleBallsArea; // or whichever measurement we use + + if (currentMeasurement <= FuelGauge.EMPTY.getThreshold()) { + return "#000000"; + } else if (currentMeasurement <= FuelGauge.LOW.getThreshold()) { + return "#FF0000"; + } else if (currentMeasurement <= FuelGauge.MEDIUM.getThreshold()) { + return "#FFFF00"; + } else { + return "#00FF00"; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java new file mode 100644 index 0000000..8594d71 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java @@ -0,0 +1,164 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import dev.doglog.DogLog; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Constants.FuelGaugeDetection.FuelGauge; +import frc.robot.Constants.FuelGaugeDetection.GaugeCalculationType; +import frc.robot.util.LoggedTalonFX; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +// spotless is clapping out +public class HopperSubsystem extends SubsystemBase { + private final CommandSwerveDrivetrain drivetrain; + private final BooleanSupplier redside; + + private final LoggedTalonFX hopperMotor1, hopperMotor2, hopper; + private double targetSurfaceSpeedMps = 0.0; + + private final VelocityVoltage m_velocityRequest = new VelocityVoltage(0); + + public HopperSubsystem(CommandSwerveDrivetrain drivetrain, BooleanSupplier redside) { + this.drivetrain = drivetrain; + this.redside = redside; + + CurrentLimitsConfigs currentLimitConfigs = + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Constants.Hopper.STATOR_LIMIT_AMPS) + .withSupplyCurrentLimit(Constants.Hopper.SUPPLY_LIMIT_AMPS); + + Slot0Configs s0c = new Slot0Configs().withKP(Constants.Hopper.kP).withKV(Constants.Hopper.kV); + + MotorOutputConfigs motorOutputConfigs = + new MotorOutputConfigs() + .withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake); + + ClosedLoopRampsConfigs clrc = new ClosedLoopRampsConfigs().withVoltageClosedLoopRampPeriod(0.3); + + hopperMotor1 = + new LoggedTalonFX( + "HopperFloorMain", Constants.Hopper.MOTOR_1_PORT, Constants.Swerve.CAN_BUS); + hopperMotor2 = + new LoggedTalonFX( + "HopperFloorFollower", Constants.Hopper.MOTOR_2_PORT, Constants.Swerve.CAN_BUS); + hopper = hopperMotor1; + + TalonFXConfiguration hopperConfig = + new TalonFXConfiguration() + .withSlot0(s0c) + .withCurrentLimits(currentLimitConfigs) + .withMotorOutput(motorOutputConfigs) + .withClosedLoopRamps(clrc); + + TalonFXConfigurator hopperMotor1Config = hopperMotor1.getConfigurator(); + TalonFXConfigurator hopperMotor2Config = hopperMotor2.getConfigurator(); + hopperMotor1Config.apply(hopperConfig); + hopperMotor2Config.apply(hopperConfig); + + hopperMotor2.setControl(new Follower(hopper.getDeviceID(), MotorAlignmentValue.Aligned)); + + DogLog.log("Subsystems/Hopper/Gains/kP", Constants.Hopper.kP); + DogLog.log("Subsystems/Hopper/Gains/kV", Constants.Hopper.kV); + } + + // Ruth's version + public void runHopperMps(DoubleSupplier targetSurfaceSpeedMps, BooleanSupplier readyToRun) { + if (readyToRun.getAsBoolean()) { + this.targetSurfaceSpeedMps = targetSurfaceSpeedMps.getAsDouble(); + hopper.setControl( + m_velocityRequest.withVelocity( + this.targetSurfaceSpeedMps * Constants.Hopper.MOTOR_ROTS_PER_FLOOR_METER)); + } else { + stop(); + } + } + + public void runHopperMps(double targetSurfaceSpeedMps) { + this.targetSurfaceSpeedMps = targetSurfaceSpeedMps; + hopper.setControl( + m_velocityRequest.withVelocity( + targetSurfaceSpeedMps * Constants.Hopper.MOTOR_ROTS_PER_FLOOR_METER)); + } + + public void stop() { + targetSurfaceSpeedMps = 0.0; + hopper.stopMotor(); + } + + public double getFloorSpeedMPS() { + return hopper.getCachedVelocityRps() * Constants.Hopper.FLOOR_METERS_PER_MOTOR_ROT; + } + + public double getAgitatorSpeedRPS() { + return hopper.getCachedVelocityRps() * Constants.Hopper.AGITATOR_ROTS_PER_MOTOR_ROT; + } + + public boolean atTargetSpeed() { + return Math.abs(getFloorSpeedMPS() - targetSurfaceSpeedMps) + <= Constants.Hopper.FLOOR_SPEED_TOLERANCE_MPS; + } + + public boolean isHopperSufficientlyEmpty(FuelGaugeDetection fuelGaugeDetection) { + return fuelGaugeDetection != null + ? fuelGaugeDetection.GaugeLessThanEqualTo( + GaugeCalculationType.SMOOTHED_MULTIPLE_BALLS, FuelGauge.LOW) + : false; + } + + // Does not stop the Hopper when interrupted + public Command runHopperCommand() { + return runOnce(() -> runHopperMps(Constants.Hopper.TARGET_SURFACE_SPEED_MPS)); + } + + // Does not stop the Hopper when interrupted + public Command runHopperCommand(double targetSurfaceSpeedMps) { + return runOnce(() -> runHopperMps(targetSurfaceSpeedMps)); + } + + // Stops the Hopper when interrupted + public Command runHopperUntilInterruptedCommand() { + return startEnd(() -> runHopperMps(Constants.Hopper.TARGET_SURFACE_SPEED_MPS), this::stop); + } + + // Stops the Hopper when interrupted + // Ruth's Version + public Command runHopperUntilInterruptedCommand( + DoubleSupplier targetSurfaceSpeedMps, BooleanSupplier readyToRun) { + return runEnd(() -> runHopperMps(targetSurfaceSpeedMps, readyToRun), this::stop); + } + + public Command runHopperUntilInterruptedCommand(double targetSurfaceSpeedMps) { + return runEnd(() -> runHopperMps(targetSurfaceSpeedMps), this::stop); + } + + // public double grabHopperRecommendedSpeed(double distanceToTarget) { + // return Constants.Hopper.HOPPER_SPEED_MAP.get(distanceToTarget); + // } + + @Override + public void periodic() { + DogLog.log( + "Subsystems/Hopper/CurrentSurfaceSpeed (mps)", + hopper.getCachedVelocityRps() * Constants.Hopper.FLOOR_METERS_PER_MOTOR_ROT); + DogLog.log("Subsystems/Hopper/TargetSurfaceSpeed (mps)", targetSurfaceSpeedMps); + DogLog.log("Subsystems/Hopper/AtTargetSpeed", atTargetSpeed()); + DogLog.log( + "Subsystems/Hopper/TargetMotorSpeed (rps)", + targetSurfaceSpeedMps * Constants.Hopper.MOTOR_ROTS_PER_FLOOR_METER); + DogLog.log("Subsystems/Hopper/CurrentMotorSpeed (rps)", hopper.getCachedVelocityRps()); + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..2a68c43 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -0,0 +1,340 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Rotations; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MagnetSensorConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import dev.doglog.DogLog; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.util.LoggedTalonFX; + +public class IntakeSubsystem extends SubsystemBase { + private LoggedTalonFX armMotor, rollersMotor; + private CANcoder cancoder; + + private double targetAngleDeg; + private double targetRollersRPS; + + private final VelocityVoltage m_velocityRequest = new VelocityVoltage(0); + private final MotionMagicVoltage m_motionMagicRequest = new MotionMagicVoltage(0); + private final PositionVoltage m_positionRequest = new PositionVoltage(0); + private final DutyCycleOut m_dutyCycleRequest = new DutyCycleOut(0); + private final TorqueCurrentFOC m_torqueCurrentRequest = new TorqueCurrentFOC(0); + + public IntakeSubsystem() { + rollersMotor = new LoggedTalonFX("IntakeRollers", Constants.Intake.Rollers.CAN_ID); + armMotor = + new LoggedTalonFX("IntakeArm", Constants.Intake.Arm.CAN_ID, Constants.Swerve.CAN_BUS); + targetAngleDeg = 0; + targetRollersRPS = 0; + + Slot0Configs rollersSlot0Configs = + new Slot0Configs() + .withKV(Constants.Intake.Rollers.kV) + .withKP(Constants.Intake.Rollers.kP) + .withKI(Constants.Intake.Rollers.kI) + .withKD(Constants.Intake.Rollers.kD); + + Slot0Configs armSlot0Configs = + new Slot0Configs() + .withKV(Constants.Intake.Arm.kV) + .withKP(Constants.Intake.Arm.kP) + .withKI(Constants.Intake.Arm.kI) + .withKD(Constants.Intake.Arm.kD) + .withKG(Constants.Intake.Arm.kG) + .withGravityArmPositionOffset(Constants.Intake.Arm.GRAVITY_POS_OFFSET) + .withKS(Constants.Intake.Arm.kS) + .withGravityType(GravityTypeValue.Arm_Cosine); + + CurrentLimitsConfigs rollersCurrentLimitsConfigs = + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Constants.Intake.Rollers.STATOR_CURRENT_LIMIT) + .withSupplyCurrentLimit(Constants.Intake.Rollers.SUPPLY_CURRENT_LIMIT); + + CurrentLimitsConfigs armCurrentLimitsConfigs = + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Constants.Intake.Arm.STATOR_CURRENT_LIMIT) + .withSupplyCurrentLimit(Constants.Intake.Arm.SUPPLY_CURRENT_LIMIT); + + MotionMagicConfigs mmc = + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(Constants.Intake.Arm.mmcV) + .withMotionMagicAcceleration(Constants.Intake.Arm.mmcA); + + ClosedLoopRampsConfigs clrc = new ClosedLoopRampsConfigs().withVoltageClosedLoopRampPeriod(0.1); + + // Creates a FusedCANcoder, which combines data from the CANcoder and the arm + // motor's encoder + cancoder = new CANcoder(Constants.Intake.Arm.ENCODER_PORT, Constants.Swerve.CAN_BUS); + CANcoderConfiguration ccConfig = new CANcoderConfiguration(); + MagnetSensorConfigs magnetSensorConfigs = + new MagnetSensorConfigs() + .withAbsoluteSensorDiscontinuityPoint(Rotations.of(1)) + .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withMagnetOffset(Rotations.of(Constants.Intake.Arm.ENCODER_OFFSET)); + + cancoder.getConfigurator().apply(ccConfig); + cancoder.getConfigurator().apply(magnetSensorConfigs); + + // Add the CANcoder as a feedback source for the motor's built-in encoder + FeedbackConfigs feedbackConfigs = + new FeedbackConfigs() + .withFeedbackRemoteSensorID(cancoder.getDeviceID()) + .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder) + .withRotorToSensorRatio( + Constants.Intake.Arm.MOTOR_ROTS_PER_ARM_ROT + / Constants.Intake.Arm.CANCODER_ROTS_PER_ARM_ROT) + .withSensorToMechanismRatio(Constants.Intake.Arm.CANCODER_ROTS_PER_ARM_ROT); + + TalonFXConfiguration rollersConfig = + new TalonFXConfiguration() + .withSlot0(rollersSlot0Configs) + .withCurrentLimits(rollersCurrentLimitsConfigs) + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast)) + .withClosedLoopRamps(clrc); + + TalonFXConfiguration armConfig = + new TalonFXConfiguration() + .withSlot0(armSlot0Configs) + .withMotionMagic(mmc) + .withCurrentLimits(armCurrentLimitsConfigs) + .withFeedback(feedbackConfigs) + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake)); + + TalonFXConfigurator armMotorConfig = armMotor.getConfigurator(); + TalonFXConfigurator rollersMotorConfig = rollersMotor.getConfigurator(); + + armMotorConfig.apply(armConfig); + rollersMotorConfig.apply(rollersConfig); + + DogLog.log("Subsystems/Intake/Arm/Gains/kP", Constants.Intake.Arm.kP); + DogLog.log("Subsystems/Intake/Arm/Gains/kI", Constants.Intake.Arm.kI); + DogLog.log("Subsystems/Intake/Arm/Gains/kD", Constants.Intake.Arm.kD); + DogLog.log("Subsystems/Intake/Arm/Gains/kV", Constants.Intake.Arm.kV); + DogLog.log("Subsystems/Intake/Arm/Gains/mmcV", Constants.Intake.Arm.mmcV); + DogLog.log("Subsystems/Intake/Arm/Gains/mmcA", Constants.Intake.Arm.mmcA); + + DogLog.log("Subsystems/Intake/Rollers/Gains/kP", Constants.Intake.Rollers.kP); + DogLog.log("Subsystems/Intake/Rollers/Gains/kI", Constants.Intake.Rollers.kI); + DogLog.log("Subsystems/Intake/Rollers/Gains/kD", Constants.Intake.Rollers.kD); + DogLog.log("Subsystems/Intake/Rollers/Gains/kV", Constants.Intake.Rollers.kV); + } + + public void runRollers(double output) { + rollersMotor.setControl(m_dutyCycleRequest.withOutput(output)); + } + + public void runRollers() { + runRollers(1); + } + + public void runRollersInReverse() { + runRollers(-1); + } + + public void runRollersWithVelocity(double speedRollersRotationsPerSecond) { + targetRollersRPS = speedRollersRotationsPerSecond; + rollersMotor.setControl( + m_velocityRequest.withVelocity( + targetRollersRPS * Constants.Intake.Rollers.MOTOR_ROTS_PER_ROLLERS_ROT)); + } + + public void stopRollers() { + targetRollersRPS = 0; + rollersMotor.setControl(m_dutyCycleRequest.withOutput(0)); + } + + public Command stopRollersCommand() { + return runOnce(this::stopRollers); + } + + public void stopArm() { + armMotor.stopMotor(); + } + + public void setArmDegrees(double angleDeg) { + targetAngleDeg = + MathUtil.clamp( + angleDeg, Constants.Intake.Arm.ARM_POS_MIN, Constants.Intake.Arm.ARM_POS_RETRACTED); + double targetArmRotations = targetAngleDeg / 360.0; + armMotor.setControl(m_motionMagicRequest.withPosition(targetArmRotations)); + } + + public void setPowerRetract() { + setTorqueCurrent(Constants.Intake.Arm.POWER_RETRACT_TORQUE_CURRENT); + } + + public void setTorqueCurrent(double current) { + armMotor.setControl(m_torqueCurrentRequest.withOutput(current)); + } + + public void setPowerRetractPosition() { + double targetRotations = Constants.Intake.Arm.ARM_POS_RETRACTED / 360.0; + armMotor.setControl(m_positionRequest.withPosition(targetRotations)); + } + + public Rotation2d getArmUnfusedPosition() { + return new Rotation2d( + Units.rotationsToRadians( + getCancoderPositionRaw() * Constants.Intake.Arm.ARM_ROTS_PER_CANCODER_ROT)); + } + + public Rotation2d getArmPosition() { + return new Rotation2d(Units.rotationsToRadians(armMotor.getCachedPositionRotations())); + } + + public void applyCoastConfigArm() { + armMotor.setNeutralMode(NeutralModeValue.Coast); + } + + public void applyBrakeConfigArm() { + armMotor.setNeutralMode(NeutralModeValue.Brake); + } + + public double getCancoderPositionRaw() { + return cancoder.getAbsolutePosition().getValueAsDouble(); + } + + public boolean atTargetAngle() { + return Math.abs(getArmPosition().getDegrees() - targetAngleDeg) + <= Constants.Intake.Arm.POSITION_TOLERANCE_DEGREES; + } + + public boolean atExtendedPosition() { + return Math.abs(getArmPosition().getDegrees() - Constants.Intake.Arm.ARM_POS_EXTENDED) + <= Constants.Intake.Arm.POSITION_TOLERANCE_DEGREES; + } + + public boolean atTargetSpeed() { + return Math.abs( + rollersMotor.getCachedVelocityRps() + - targetRollersRPS * Constants.Intake.Rollers.MOTOR_ROTS_PER_ROLLERS_ROT) + <= Constants.Intake.Rollers.TOLERANCE_MOTOR_ROTS_PER_SEC; + } + + public Command runRollersUntilInterruptedCommand() { + return startEnd(this::runRollers, this::stopRollers); + } + + public Command runRollersUntilInterruptedCommand(double targetRollers_RPS) { + return startEnd(this::runRollers, this::stopRollers); + } + + public Command setArmToDegreesCommand(double degrees) { + return runOnce(() -> setArmDegrees(degrees)); + } + + public Command retractIntakeCommand() { + return run( + () -> { + setArmDegrees(Constants.Intake.Arm.ARM_POS_RETRACTED); + stopRollers(); + }); + } + + public Command powerRetractRollersCommand() { + return runOnce( + () -> { + setPowerRetract(); + runRollers(); + }); + // .beforeStarting(Commands.waitSeconds(Constants.Intake.Arm.POWER_RETRACT_DELAY)); + } + + public Command torqueRetractCommand() { + return runOnce(this::setPowerRetract); + } + + public Command intakeUntilInterruptedCommand() { + return runEnd( + () -> { + setArmDegrees(Constants.Intake.Arm.ARM_POS_EXTENDED); + runRollers(); + }, + this::stopRollers); + } + + public Command outtakeUntilInterruptedCommand() { + return runEnd( + () -> { + setArmDegrees(Constants.Intake.Arm.ARM_POS_EXTENDED); + runRollersInReverse(); + }, + this::stopRollers); + } + + public Command intakeDefault() { + return runOnce( + () -> { + runRollersWithVelocity(Constants.Intake.Rollers.IDLE_ROLLER_VELO_RPS); + setArmDegrees(Constants.Intake.Arm.ARM_POS_IDLE); + }); + } + + public Command agitateArmCommand() { + // stole these numbers from 5507, subject to change + return Commands.sequence( + run(() -> setArmDegrees(Constants.Intake.Arm.ARM_POS_RETRACTED)).withTimeout(4.5), + Commands.sequence( + runOnce(this::setPowerRetract).withTimeout(0.04), + Commands.waitSeconds(0.3), + setArmToDegreesCommand(Constants.Intake.Arm.ARM_POS_IDLE).withTimeout(0.04), + Commands.waitSeconds(0.3)) + .repeatedly()); + } + + public Command powerRetractThenAgitateArmCommand() { + return Commands.sequence( + powerRetractRollersCommand(), Commands.waitSeconds(1), agitateArmCommand()); + } + + @Override + public void periodic() { + DogLog.log( + "Subsystems/Intake/Rollers/CurrentSpeed (rps)", + rollersMotor.getCachedVelocityRps() * Constants.Intake.Rollers.ROLLER_ROTS_PER_MOTOR_ROT); + DogLog.log("Subsystems/Intake/Rollers/TargetSpeed (rps)", targetRollersRPS); + DogLog.log("Subsystems/Intake/Rollers/AtTargetSpeed", atTargetSpeed()); + DogLog.log("Subsystems/Intake/Arm/AtTargetAngle", atTargetAngle()); + // DogLog.log("Subsystems/Intake/Arm/AbsoluteEncoderRaw (rots)", getCancoderPositionRaw()); + DogLog.log("Subsystems/Intake/Arm/FusedCurrentPosition (degs)", getArmPosition().getDegrees()); + // DogLog.log( + // "Subsystems/Intake/Arm/AbsoluteCurrentPosition (degs)", + // getArmUnfusedPosition().getDegrees()); + DogLog.log("Subsystems/Intake/Arm/TargetPosition (degs)", targetAngleDeg); + + SmartDashboard.putNumber("Arm Angle", getArmPosition().getDegrees()); + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakeVisionDetection.java b/src/main/java/frc/robot/subsystems/IntakeVisionDetection.java new file mode 100644 index 0000000..30328cf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeVisionDetection.java @@ -0,0 +1,87 @@ +package frc.robot.subsystems; + +import dev.doglog.DogLog; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import java.util.List; +import java.util.Optional; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +public class IntakeVisionDetection extends SubsystemBase { + private final PhotonCamera photonCamera; + private PhotonPipelineResult latestVisionResult; + private double latestRawArea; + private double latestRawYaw; + private double latestRawPitch; + + public IntakeVisionDetection(Constants.IntakeVision.IntakeVisionCamera cameraID) { + photonCamera = new PhotonCamera(cameraID.toString()); + } + + @Override + public void periodic() { + if (!cameraConnected()) return; + + if (!validVisionResult(photonCamera.getAllUnreadResults())) return; + + updateVisionResult(); + } + + private boolean cameraConnected() { + boolean cameraConnected = photonCamera.isConnected(); + DogLog.log("Subsystems/IntakeVision/CameraStatus", cameraConnected); + return cameraConnected; + } + + private boolean validVisionResult(List results) { + if (results.isEmpty()) return false; + + latestVisionResult = results.get(results.size() - 1); + + DogLog.log("Subsystems/IntakeVision/ResultExists", latestVisionResult != null); + + return latestVisionResult != null; + } + + private void updateVisionResult() { + Optional target = getLargestTarget(); + target.ifPresentOrElse( + t -> { + DogLog.log("Subsystems/IntakeVision/TargetPresent", true); + latestRawArea = t.getArea(); + latestRawYaw = t.getYaw(); + latestRawPitch = t.getPitch(); + DogLog.log("Subsystems/IntakeVision/Area", latestRawArea); + DogLog.log("Subsystems/IntakeVision/Yaw", latestRawYaw); + DogLog.log("Subsystems/IntakeVision/Pitch", latestRawPitch); + }, + () -> DogLog.log("Subsystems/IntakeVision/TargetPresent", false)); + } + + private Optional getLargestTarget() { + if (latestVisionResult == null) return Optional.empty(); + + List targets = latestVisionResult.getTargets(); + DogLog.log("Subsystems/IntakeVision/TargetsLength", targets.size()); + + if (targets.isEmpty()) return Optional.empty(); + + return targets.stream().max((a, b) -> Double.compare(a.getArea(), b.getArea())); + } + + public double getYaw() { + DogLog.log("Subsystems/IntakeVision/LatestRetrivedYaw", -latestRawYaw); + return -latestRawYaw; + } + + public double getArea() { + return latestRawArea; + } + + public double getPitch() { + DogLog.log("Subsystems/IntakeVision/LatestRetrivedPitch", -latestRawPitch); + return -latestRawPitch; + } +} diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java new file mode 100644 index 0000000..3252e71 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -0,0 +1,169 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.controls.EmptyAnimation; +import com.ctre.phoenix6.controls.FireAnimation; +import com.ctre.phoenix6.controls.RainbowAnimation; +import com.ctre.phoenix6.controls.SingleFadeAnimation; +import com.ctre.phoenix6.controls.SolidColor; +import com.ctre.phoenix6.controls.StrobeAnimation; +import com.ctre.phoenix6.hardware.CANdle; +import com.ctre.phoenix6.signals.AnimationDirectionValue; +import com.ctre.phoenix6.signals.RGBWColor; +import dev.doglog.DogLog; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.BooleanSupplier; + +public class LEDSubsystem extends SubsystemBase { + // left strip is [8, 23] + // middle strip is [24, 53] (center of middle is 38) + // right strip is [51, 76] + private static final int END_OF_STRIP = 76; + + private static CANdle candle = new CANdle(5); + private LEDState currentState = LEDState.NONE; + private BooleanSupplier active, inRange; + private boolean seesTagCached; + + public enum LEDState { + NONE, + ACTIVE, + ACTIVE_IN_RANGE, + FLAME, + RAINBOW + } + + public LEDSubsystem(BooleanSupplier activeSupplier, BooleanSupplier inRangeSupplier) { + active = activeSupplier; + inRange = inRangeSupplier; + } + + public void periodic() { + LEDState computedState = computeState(); + if (computedState != currentState) { + currentState = computedState; + applyState(computedState); + } + if (currentState == LEDState.ACTIVE_IN_RANGE) activeInRangeAnimation(); + + DogLog.log("Subsystems/LEDs/Active", active.getAsBoolean()); + DogLog.log("Subsystems/LEDs/InRange", inRange.getAsBoolean()); + DogLog.log("Subsystems/LEDs/CurrentState", currentState.toString()); + } + + public LEDState computeState() { + if (DriverStation.isDisabled()) return LEDState.FLAME; + if (DriverStation.isAutonomousEnabled()) return LEDState.RAINBOW; + + if (active.getAsBoolean() && inRange.getAsBoolean()) return LEDState.ACTIVE_IN_RANGE; + else if (active.getAsBoolean() && !inRange.getAsBoolean()) return LEDState.ACTIVE; + else if (!active.getAsBoolean()) return LEDState.NONE; + + return LEDState.NONE; + } + + public void applyState(LEDState state) { + clearAll(); + // active in range animation handled in periodic + switch (state) { + case ACTIVE -> candle.setControl(activeAnimation()); + case FLAME -> { + candle.setControl(flame(8, 38, 0, false)); + candle.setControl(flame(39, END_OF_STRIP, 1, true)); + } + case RAINBOW -> candle.setControl(new RainbowAnimation(8, END_OF_STRIP)); + case NONE -> setColor(8, END_OF_STRIP, Color.kBlack); + default -> clearAll(); + } + } + + public void visionStatusIndicators( + VisionSubsystem frontLeft, + VisionSubsystem frontRight, + VisionSubsystem rearLeft, + VisionSubsystem rearRight) { + + if (DriverStation.isDisabled()) { + DogLog.log("Subsystems/LEDs/VisionIndicatorsEnabled", true); + candle.setControl(solidColor(frontLeft.getCameraConnected() ? Color.kGreen : Color.kRed, 3)); + candle.setControl(solidColor(frontRight.getCameraConnected() ? Color.kGreen : Color.kRed, 4)); + candle.setControl(solidColor(rearLeft.getCameraConnected() ? Color.kGreen : Color.kRed, 2)); + candle.setControl(solidColor(rearRight.getCameraConnected() ? Color.kGreen : Color.kRed, 5)); + + boolean seesTag = + frontLeft.seesTags() + || frontRight.seesTags() + || rearLeft.seesTags() + || rearRight.seesTags(); + if (seesTag != seesTagCached) { + clearSlots(2, 5); + seesTagCached = seesTag; + } + + if (seesTag) { + candle.setControl(solidColor(Color.kGreen, 7)); + candle.setControl(solidColor(Color.kGreen, 0)); + candle.setControl(solidColor(Color.kGreen, 6)); + candle.setControl(solidColor(Color.kGreen, 1)); + } else { + candle.setControl(strobe(Color.kRed, 6, 7, 2)); + candle.setControl(strobe(Color.kRed, 6, 0, 3)); + candle.setControl(strobe(Color.kRed, 6, 6, 4)); + candle.setControl(strobe(Color.kRed, 6, 1, 5)); + } + } else { + DogLog.log("Subsystems/LEDs/VisionIndicatorsEnabled", false); + setColor(0, 7, Color.kBlack); + seesTagCached = false; + } + } + + private static void setColor(int start, int end, Color color) { + candle.setControl(new SolidColor(start, end).withColor(new RGBWColor(color))); + } + + // helper methods + public static void clearSlots(int start, int end) { + for (int i = start; i <= end; i++) candle.setControl(new EmptyAnimation(i)); + } + + public void clearAll() { + clearSlots(0, 7); + } + + private SingleFadeAnimation activeAnimation() { + return new SingleFadeAnimation(8, END_OF_STRIP).withColor(new RGBWColor(Color.kRed)); + } + + private SolidColor solidColor(Color color) { + return new SolidColor(8, END_OF_STRIP).withColor(new RGBWColor(color)); + } + + private SolidColor solidColor(Color color, int ledIndex) { + return new SolidColor(ledIndex, ledIndex).withColor(new RGBWColor(color)); + } + + private StrobeAnimation strobe(Color color, int frameRate, int ledIndex, int slot) { + return new StrobeAnimation(ledIndex, ledIndex) + .withColor(new RGBWColor(color)) + .withFrameRate(frameRate) + .withSlot(slot); + } + + private void activeInRangeAnimation() { + boolean red = ((int) (Timer.getFPGATimestamp() * 10)) % 2 == 0; + candle.setControl(solidColor(red ? Color.kRed : Color.kWhite)); + } + + private FireAnimation flame(int startIndex, int endIndex, int slot, boolean backward) { + return new FireAnimation(startIndex, endIndex) + .withSparking(0.5) + .withCooling(0.2) + .withFrameRate(40) + .withDirection( + backward ? AnimationDirectionValue.Backward : AnimationDirectionValue.Forward) + .withSlot(slot); + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..ee6829c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -0,0 +1,88 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Rotations; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MagnetSensorConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.util.LoggedTalonFX; + + +public class ShooterSubsystem extends SubsystemBase { + + private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood; + private final CANcoder hoodEncoder; + + + /* + * make instance variables for necessary closed loop control requests: VelocityVoltage, PositionVoltage. Initialize them with value 0.0. + * make instance variables of type double for the target roller speed and target hood angle + */ + + public ShooterSubsystem() { + // The canbus is a communication system that can connect devices like the roboRIO, pdh, and motors. + CANBus canbus = Constants.Swerve.CAN_BUS; + + // LoggedTalonFX is our version of the existing TalonFX class, which automatically logs some motor information + warmup1 = new LoggedTalonFX("ShooterWarmup1", Constants.Shooter.Rollers.WARMUP_1_ID, canbus); + warmup2 = new LoggedTalonFX("ShooterWarmup2", Constants.Shooter.Rollers.WARMUP_2_ID, canbus); + warmup3 = new LoggedTalonFX("ShooterWarmup3", Constants.Shooter.Rollers.WARMUP_3_ID, canbus); + + shooter = warmup3; // warmup1 and warmup2 will be set as "followers" of the "lead" warmup3, so later we can just reference the `shooter` variable + + // Initialize the hood motor just like the warmup motors. You can find the necessary ID in Constants. + + // Create a variable of type Slot0Configs called rollersSlot0Configs, and initialize it with the pid and feedforward gains found in Constants + + // Create a variable of type CurrentLimitsConfigs called rollersClConfigs, and initialize it with the stator and supply limits found in Constants + + // Create a variable of type MotorOutputConifigs called rollersOutputConfigs, and initialize it to treat clockwise as positive, and neutral mode as coast + + TalonFXConfiguration rollersConfig = new TalonFXConfiguration(); + // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` to the configs you just created. + + warmup1.getConfigurator().apply(rollersConfig); + // I just applied the configuration that you just created to warmup1. Apply it to warmup2 and warmup3 as well. + + Follower follower = new Follower(Constants.Shooter.Rollers.WARMUP_3_ID, MotorAlignmentValue.Aligned); + // use the `setControl` method of warmup1 and 2 to configure them to use this follower configuration. + + hoodEncoder = new CANcoder(Constants.Shooter.Hood.ENCODER_PORT, Constants.Swerve.CAN_BUS); + + // Create a variable of type Slot0Configs called hoodSlot0Configs, and initialize it with the pid and feedforward gains found in Constants + + // Create a variable of type CurrentLimitsConfigs called hoodClConfigs, and initialize it with the stator and supply limits found in Constants + + // Create a variable of type MotorOutputConifigs called hoodOutputConfigs, and initialize it to treat counterclockwise as positive, and neutral mode as coast + + FeedbackConfigs hoodFeedbackConfigs = + new FeedbackConfigs() + .withFeedbackRemoteSensorID(hoodEncoder.getDeviceID()) + .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder) + .withSensorToMechanismRatio(Constants.Shooter.Hood.ENCODER_ROTS_PER_HOOD_ROT) + .withRotorToSensorRatio(Constants.Shooter.Hood.MOTOR_ROTS_PER_ENCODER_ROT); + + // Create a TalonFXConfiguration called hoodConfig + // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the device configs above + + // Apply the created configuration to the hood motor + + MagnetSensorConfigs hoodCANcoderConfig = + new CANcoderConfiguration() + .MagnetSensor.withAbsoluteSensorDiscontinuityPoint(Rotations.of(1)) + .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) + .withMagnetOffset(Rotations.of(Constants.Shooter.Hood.ENCODER_OFFSET)); + + // Apply this config to the `hoodEncoder`. Notice how applying a device configuration is similar between motors and other devices + + } +} diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java new file mode 100644 index 0000000..5fe4655 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -0,0 +1,291 @@ +package frc.robot.subsystems; + +import dev.doglog.DogLog; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +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.kinematics.ChassisSpeeds; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N8; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Constants.Vision.VisionCamera; +import frc.robot.util.VisionUtils; +import java.util.List; +import java.util.Optional; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +public class VisionSubsystem extends SubsystemBase { + private final Constants.Vision.VisionCamera cameraID; + private final PhotonCamera photonCamera; + private final AprilTagFieldLayout fieldLayout; + private final PhotonPoseEstimator poseEstimator; + private PhotonPipelineResult latestVisionResult; + private double lastTagSeenTimestamp = -1.0; + + private final String cameraTitle; + private final String loggingPath; + + private Optional visionEstimate; + + private boolean cameraConnectedStatus = false; + + private final Matrix cameraIntrinsics; + private final Matrix distortionCoeffs; + + // addFilteredPose() vals + private boolean hasValidMeasurement; + private Pose2d latestMeasuredPose; + private double latestFinalTimestamp; + private Matrix latestNoiseVector; + private double latestMinDistance; + private double latestAvgDistance; + private int latestTagCount; + private final CommandSwerveDrivetrain swerve; + private final Transform3d camHeight; + + public VisionSubsystem( + Constants.Vision.VisionCamera cameraID, CommandSwerveDrivetrain drivetrain) { + this.cameraID = cameraID; + photonCamera = new PhotonCamera(cameraID.toString()); + Transform3d robotToCamera = cameraID.getCameraTransform(); + camHeight = new Transform3d(0.0, 0.0, robotToCamera.getZ(), new Rotation3d(0.0, 0.0, 0.0)); + + fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); + + poseEstimator = new PhotonPoseEstimator(fieldLayout, robotToCamera); + latestVisionResult = null; + + cameraTitle = cameraID.getLoggingName(); + loggingPath = "Subsystems/Vision/" + cameraTitle; + + this.swerve = drivetrain; + + cameraIntrinsics = cameraID.getCameraMatrix(); + distortionCoeffs = cameraID.getDistCoeffs(); + } + + public VisionCamera getCameraID() { + return cameraID; + } + + @Override + public void periodic() { + resetMeasurementValues(); + + if (!cameraConnected()) return; + + updateEstimate(photonCamera.getAllUnreadResults()); + } + + private void resetMeasurementValues() { + visionEstimate = Optional.empty(); + latestVisionResult = null; + hasValidMeasurement = false; + } + + private boolean cameraConnected() { + cameraConnectedStatus = photonCamera.isConnected(); + DogLog.log(loggingPath + "/CameraConnected", cameraConnectedStatus); + return cameraConnectedStatus; + } + + private void updateEstimate(List results) { + if (results.isEmpty()) return; + latestVisionResult = results.get(results.size() - 1); + + if (!latestVisionResult.getTargets().isEmpty()) { + lastTagSeenTimestamp = Timer.getFPGATimestamp(); + } + + visionEstimate = poseEstimator.estimateCoprocMultiTagPose(latestVisionResult); + DogLog.log(loggingPath + "/EstimationMethod", "COPROC"); + if (visionEstimate.isEmpty()) { + visionEstimate = + poseEstimator.estimateConstrainedSolvepnpPose( + latestVisionResult, + cameraIntrinsics, + distortionCoeffs, + new Pose3d(swerve.getCurrentState().Pose).plus(camHeight), + false, + Constants.Vision.HDG_PENALTY); + DogLog.log(loggingPath + "/EstimationMethod", "PNP"); + } + if (visionEstimate.isEmpty()) { + visionEstimate = poseEstimator.estimateLowestAmbiguityPose(latestVisionResult); + DogLog.log(loggingPath + "/EstimationMethod", "LOWESTAMBIGUITY"); + } + } + + public void calculateFilteredPose() { + hasValidMeasurement = false; + + if (!resultValid()) return; + if (!tagsValid()) return; + + EstimatedRobotPose estimatedPose = visionEstimate.get(); + latestMeasuredPose = estimatedPose.estimatedPose.toPose2d(); + DogLog.log(loggingPath + "/MeasuredPose", latestMeasuredPose); + + latestMinDistance = getMinDistance(); + latestAvgDistance = getAverageDistance(); + + if (throwOutDistance(latestMinDistance)) return; + + throwOutHeadingChange(latestMeasuredPose); + + ChassisSpeeds fieldSpeeds = + ChassisSpeeds.fromRobotRelativeSpeeds( + swerve.getState().Speeds, swerve.getState().Pose.getRotation()); + + double currentSpeed = Math.hypot(fieldSpeeds.vxMetersPerSecond, fieldSpeeds.vyMetersPerSecond); + + latestNoiseVector = + VisionUtils.computeNoiseVector(latestAvgDistance, currentSpeed, latestTagCount); + latestFinalTimestamp = calculateTimestamp(estimatedPose.timestampSeconds); + hasValidMeasurement = true; + + DogLog.log(loggingPath + "/measuredPoseAvailable", latestMeasuredPose != null); + } + + private boolean resultValid() { + DogLog.log(loggingPath + "/addFilteredPoseRunning", true); + + boolean resultExists = latestVisionResult != null && !latestVisionResult.getTargets().isEmpty(); + DogLog.log(loggingPath + "/HasResultsTargets", resultExists); + + boolean hasEstimate = !visionEstimate.isEmpty(); + DogLog.log(loggingPath + "/HasEstimate", hasEstimate); + + return resultExists && hasEstimate; + } + + private boolean tagsValid() { + List tags = latestVisionResult.getTargets(); + + boolean tagsValid = !tags.isEmpty(); + DogLog.log(loggingPath + "/Tags", tagsValid); + + if (!tagsValid) return false; + + for (PhotonTrackedTarget tag : tags) { + DogLog.log(loggingPath + "/Tags/" + tag.getFiducialId() + "/Area", tag.getArea()); + DogLog.log(loggingPath + "/Tags/" + tag.getFiducialId() + "/Yaw", tag.getYaw()); + } + latestTagCount = tags.size(); + DogLog.log(loggingPath + "/TagCount", latestTagCount); + + return true; + } + + private boolean throwOutDistance(double min) { + boolean thrownOut = Double.isNaN(min) || min > Constants.Vision.MAX_TAG_DISTANCE; + DogLog.log(loggingPath + "/ThrownOutDistance", thrownOut); + return thrownOut; + } + + private void throwOutHeadingChange(Pose2d pose) { + Rotation2d estimatedHeading = pose.getRotation(); + Rotation2d currentHeading = swerve.getCurrentState().Pose.getRotation(); + double rotationDiff = Math.abs(estimatedHeading.relativeTo(currentHeading).getDegrees()); + boolean trueIfThrown = rotationDiff > VisionUtils.getHeadingThreshold(); + DogLog.log(loggingPath + "/ThrownOutHeading", trueIfThrown); + DogLog.log(loggingPath + "/ThrownOutHeadingDiff", rotationDiff); + } + + public boolean getCameraConnected() { + return cameraConnectedStatus; + } + + public boolean seesTags() { + if (!cameraConnectedStatus) return false; + return (Timer.getFPGATimestamp() - lastTagSeenTimestamp) + <= Constants.Vision.TAG_VISIBLE_THRESHOLD_SEC; + } + + public double getMinDistance() { + double minDist = + VisionUtils.visionResultInvalid(latestVisionResult) + ? Double.MAX_VALUE + : latestVisionResult.getTargets().stream() + .mapToDouble(t -> t.getBestCameraToTarget().getTranslation().getNorm()) + .min() + .orElse(Double.NaN); + + DogLog.log(loggingPath + "/ClosestTagDistance", minDist); + return minDist; + } + + public double getAverageDistance() { + double avgDist = + VisionUtils.visionResultInvalid(latestVisionResult) + ? Double.MAX_VALUE + : latestVisionResult.getTargets().stream() + .mapToDouble(t -> t.getBestCameraToTarget().getTranslation().getNorm()) + .average() + .orElse(Double.NaN); + + DogLog.log(loggingPath + "/AverageTagDistance", avgDist); + return avgDist; + } + + public double getMaxDistance() { + double maxDist = + VisionUtils.visionResultInvalid(latestVisionResult) + ? Double.MAX_VALUE + : latestVisionResult.getTargets().stream() + .mapToDouble(t -> t.getBestCameraToTarget().getTranslation().getNorm()) + .max() + .orElse(Double.NaN); + + DogLog.log(loggingPath + "/MaxTagDistance", maxDist); + return maxDist; + } + + public double getPoseAmbiguity() { + double poseAmbiguity = + VisionUtils.visionResultInvalid(latestVisionResult) + ? Double.MAX_VALUE + : latestVisionResult.getTargets().stream() + .mapToDouble(PhotonTrackedTarget::getPoseAmbiguity) + .average() + .orElse(Double.NaN); + + DogLog.log(loggingPath + "/PoseAmbiguity", poseAmbiguity); + return poseAmbiguity; + } + + public boolean hasValidMeasurement() { + return hasValidMeasurement; + } + + private double calculateTimestamp(double timestamp) { + double fpgaTimestamp = Timer.getFPGATimestamp(); + double timestampDiff = Math.abs(timestamp - fpgaTimestamp); + double finalTimestamp = + timestampDiff > Constants.Vision.TIMESTAMP_THRESHOLD + ? fpgaTimestamp + Constants.Vision.TIMESTAMP_FPGA_CORRECTION + : timestamp; + + return finalTimestamp; + } + + public Pose2d getFilteredPose() { + return latestMeasuredPose; + } + + public void addFilteredPose() { + swerve.addVisionMeasurement(latestMeasuredPose, latestFinalTimestamp, latestNoiseVector); + } +} diff --git a/src/main/java/frc/robot/util/CustomController.java b/src/main/java/frc/robot/util/CustomController.java new file mode 100644 index 0000000..d7049df --- /dev/null +++ b/src/main/java/frc/robot/util/CustomController.java @@ -0,0 +1,33 @@ +package frc.robot.util; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +public class CustomController extends GenericHID { + private Trigger visionShootingLockout, intakeVisionLockout; + private Trigger reverseShoot, intakeOverride; + + public CustomController(int port) { + super(port); + visionShootingLockout = new Trigger(() -> this.getRawButton(10)); + intakeVisionLockout = new Trigger(() -> this.getRawButton(11)); + reverseShoot = new Trigger(() -> this.getRawButton(1)); + intakeOverride = new Trigger(() -> this.getRawButton(2)); + } + + public Trigger visionShootingLockout() { + return visionShootingLockout; + } + + public Trigger intakeVisionLockout() { + return intakeVisionLockout; + } + + public Trigger reverseShoot() { + return reverseShoot; + } + + public Trigger intakeOverride() { + return intakeOverride; + } +} diff --git a/src/main/java/frc/robot/util/Elastic.java b/src/main/java/frc/robot/util/Elastic.java new file mode 100644 index 0000000..74d122c --- /dev/null +++ b/src/main/java/frc/robot/util/Elastic.java @@ -0,0 +1,390 @@ +// Copyright (c) 2023-2026 Gold87 and other Elastic contributors +// This software can be modified and/or shared under the terms +// defined by the Elastic license: +// https://github.com/Gold872/elastic_dashboard/blob/main/LICENSE + +package frc.robot.util; + +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PubSubOption; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StringTopic; + +public final class Elastic { + private static final StringTopic notificationTopic = + NetworkTableInstance.getDefault().getStringTopic("/Elastic/RobotNotifications"); + private static final StringPublisher notificationPublisher = + notificationTopic.publish(PubSubOption.sendAll(true), PubSubOption.keepDuplicates(true)); + private static final StringTopic selectedTabTopic = + NetworkTableInstance.getDefault().getStringTopic("/Elastic/SelectedTab"); + private static final StringPublisher selectedTabPublisher = + selectedTabTopic.publish(PubSubOption.keepDuplicates(true)); + private static final ObjectMapper objectMapper = new ObjectMapper(); + + /** + * Represents the possible levels of notifications for the Elastic dashboard. These levels are + * used to indicate the severity or type of notification. + */ + public enum NotificationLevel { + /** Informational Message */ + INFO, + /** Warning message */ + WARNING, + /** Error message */ + ERROR + } + + /** + * Sends an notification to the Elastic dashboard. The notification is serialized as a JSON string + * before being published. + * + * @param notification the {@link Notification} object containing notification details + */ + public static void sendNotification(Notification notification) { + try { + notificationPublisher.set(objectMapper.writeValueAsString(notification)); + } catch (JsonProcessingException e) { + e.printStackTrace(); + } + } + + /** + * Selects the tab of the dashboard with the given name. If no tab matches the name, this will + * have no effect on the widgets or tabs in view. + * + *

If the given name is a number, Elastic will select the tab whose index equals the number + * provided. + * + * @param tabName the name of the tab to select + */ + public static void selectTab(String tabName) { + selectedTabPublisher.set(tabName); + } + + /** + * Selects the tab of the dashboard at the given index. If this index is greater than or equal to + * the number of tabs, this will have no effect. + * + * @param tabIndex the index of the tab to select. + */ + public static void selectTab(int tabIndex) { + selectTab(Integer.toString(tabIndex)); + } + + /** + * Represents an notification object to be sent to the Elastic dashboard. This object holds + * properties such as level, title, description, display time, and dimensions to control how the + * notification is displayed on the dashboard. + */ + public static class Notification { + @JsonProperty("level") + private NotificationLevel level; + + @JsonProperty("title") + private String title; + + @JsonProperty("description") + private String description; + + @JsonProperty("displayTime") + private int displayTimeMillis; + + @JsonProperty("width") + private double width; + + @JsonProperty("height") + private double height; + + /** + * Creates a new Notification with all default parameters. This constructor is intended to be + * used with the chainable decorator methods + * + *

Title and description fields are empty. + */ + public Notification() { + this(NotificationLevel.INFO, "", ""); + } + + /** + * Creates a new Notification with all properties specified. + * + * @param level the level of the notification (e.g., INFO, WARNING, ERROR) + * @param title the title text of the notification + * @param description the descriptive text of the notification + * @param displayTimeMillis the time in milliseconds for which the notification is displayed + * @param width the width of the notification display area + * @param height the height of the notification display area, inferred if below zero + */ + public Notification( + NotificationLevel level, + String title, + String description, + int displayTimeMillis, + double width, + double height) { + this.level = level; + this.title = title; + this.displayTimeMillis = displayTimeMillis; + this.description = description; + this.height = height; + this.width = width; + } + + /** + * Creates a new Notification with default display time and dimensions. + * + * @param level the level of the notification + * @param title the title text of the notification + * @param description the descriptive text of the notification + */ + public Notification(NotificationLevel level, String title, String description) { + this(level, title, description, 3000, 350, -1); + } + + /** + * Creates a new Notification with a specified display time and default dimensions. + * + * @param level the level of the notification + * @param title the title text of the notification + * @param description the descriptive text of the notification + * @param displayTimeMillis the display time in milliseconds + */ + public Notification( + NotificationLevel level, String title, String description, int displayTimeMillis) { + this(level, title, description, displayTimeMillis, 350, -1); + } + + /** + * Creates a new Notification with specified dimensions and default display time. If the height + * is below zero, it is automatically inferred based on screen size. + * + * @param level the level of the notification + * @param title the title text of the notification + * @param description the descriptive text of the notification + * @param width the width of the notification display area + * @param height the height of the notification display area, inferred if below zero + */ + public Notification( + NotificationLevel level, String title, String description, double width, double height) { + this(level, title, description, 3000, width, height); + } + + /** + * Updates the level of this notification + * + * @param level the level to set the notification to + */ + public void setLevel(NotificationLevel level) { + this.level = level; + } + + /** + * @return the level of this notification + */ + public NotificationLevel getLevel() { + return level; + } + + /** + * Updates the title of this notification + * + * @param title the title to set the notification to + */ + public void setTitle(String title) { + this.title = title; + } + + /** + * Gets the title of this notification + * + * @return the title of this notification + */ + public String getTitle() { + return title; + } + + /** + * Updates the description of this notification + * + * @param description the description to set the notification to + */ + public void setDescription(String description) { + this.description = description; + } + + public String getDescription() { + return description; + } + + /** + * Updates the display time of the notification + * + * @param seconds the number of seconds to display the notification for + */ + public void setDisplayTimeSeconds(double seconds) { + setDisplayTimeMillis((int) Math.round(seconds * 1000)); + } + + /** + * Updates the display time of the notification in milliseconds + * + * @param displayTimeMillis the number of milliseconds to display the notification for + */ + public void setDisplayTimeMillis(int displayTimeMillis) { + this.displayTimeMillis = displayTimeMillis; + } + + /** + * Gets the display time of the notification in milliseconds + * + * @return the number of milliseconds the notification is displayed for + */ + public int getDisplayTimeMillis() { + return displayTimeMillis; + } + + /** + * Updates the width of the notification + * + * @param width the width to set the notification to + */ + public void setWidth(double width) { + this.width = width; + } + + /** + * Gets the width of the notification + * + * @return the width of the notification + */ + public double getWidth() { + return width; + } + + /** + * Updates the height of the notification + * + *

If the height is set to -1, the height will be determined automatically by the dashboard + * + * @param height the height to set the notification to + */ + public void setHeight(double height) { + this.height = height; + } + + /** + * Gets the height of the notification + * + * @return the height of the notification + */ + public double getHeight() { + return height; + } + + /** + * Modifies the notification's level and returns itself to allow for method chaining + * + * @param level the level to set the notification to + * @return the current notification + */ + public Notification withLevel(NotificationLevel level) { + this.level = level; + return this; + } + + /** + * Modifies the notification's title and returns itself to allow for method chaining + * + * @param title the title to set the notification to + * @return the current notification + */ + public Notification withTitle(String title) { + setTitle(title); + return this; + } + + /** + * Modifies the notification's description and returns itself to allow for method chaining + * + * @param description the description to set the notification to + * @return the current notification + */ + public Notification withDescription(String description) { + setDescription(description); + return this; + } + + /** + * Modifies the notification's display time and returns itself to allow for method chaining + * + * @param seconds the number of seconds to display the notification for + * @return the current notification + */ + public Notification withDisplaySeconds(double seconds) { + return withDisplayMilliseconds((int) Math.round(seconds * 1000)); + } + + /** + * Modifies the notification's display time and returns itself to allow for method chaining + * + * @param displayTimeMillis the number of milliseconds to display the notification for + * @return the current notification + */ + public Notification withDisplayMilliseconds(int displayTimeMillis) { + setDisplayTimeMillis(displayTimeMillis); + return this; + } + + /** + * Modifies the notification's width and returns itself to allow for method chaining + * + * @param width the width to set the notification to + * @return the current notification + */ + public Notification withWidth(double width) { + setWidth(width); + return this; + } + + /** + * Modifies the notification's height and returns itself to allow for method chaining + * + * @param height the height to set the notification to + * @return the current notification + */ + public Notification withHeight(double height) { + setHeight(height); + return this; + } + + /** + * Modifies the notification's height and returns itself to allow for method chaining + * + *

This will set the height to -1 to have it automatically determined by the dashboard + * + * @return the current notification + */ + public Notification withAutomaticHeight() { + setHeight(-1); + return this; + } + + /** + * Modifies the notification to disable the auto dismiss behavior + * + *

This sets the display time to 0 milliseconds + * + *

The auto dismiss behavior can be re-enabled by setting the display time to a number + * greater than 0 + * + * @return the current notification + */ + public Notification withNoAutoDismiss() { + setDisplayTimeMillis(0); + return this; + } + } +} diff --git a/src/main/java/frc/robot/util/LoggedTalonFX.java b/src/main/java/frc/robot/util/LoggedTalonFX.java new file mode 100644 index 0000000..f3fd2e9 --- /dev/null +++ b/src/main/java/frc/robot/util/LoggedTalonFX.java @@ -0,0 +1,298 @@ +package frc.robot.util; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.StatusSignalCollection; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import dev.doglog.DogLog; +import java.util.ArrayList; +import java.util.Arrays; + +/** + * Team 3501 Version of TalonFX class that automatically sets Current Limits and logs motor + * information. + */ +public class LoggedTalonFX extends TalonFX { + + /** List of all LoggedTalonFX motors on our robot, defined. */ + private static ArrayList motorsOnCanivore = new ArrayList<>(); + + private static ArrayList motorsOnRio = new ArrayList<>(); + + private static StatusSignalCollection signalsOnCanivore = new StatusSignalCollection(); + private static StatusSignalCollection signalsOnRio = new StatusSignalCollection(); + + /** Name of motor instance. */ + private String name; + + private final TalonFXConfiguration motorConfiguration = new TalonFXConfiguration(); + + private String temperature, + closedLoopError, + closedLoopReference, + position, + velocity, + acceleration, + supplyCurrent, + statorCurrent, + torqueCurrent, + motorVoltage, + supplyVoltage, + rotorPosition; + + private StatusSignal deviceTempSignal; + private StatusSignal closedLoopErrorSignal; + private StatusSignal closedLoopReferenceSignal; + private StatusSignal rotorPositionSignal; + private StatusSignal positionSignal; + private StatusSignal velocitySignal; + private StatusSignal accelerationSignal; + private StatusSignal supplyCurrentSignal; + private StatusSignal statorCurrentSignal; + private StatusSignal torqueCurrentSignal; + private StatusSignal motorVoltageSignal; + private StatusSignal supplyVoltageSignal; + + private double deviceTempC; + private double closedLoopErrorVal; + private double closedLoopReferenceVal; + private double rotorPositionVal; + private double positionRotations; + private double velocityRps; + private double accelerationRps2; + private double supplyCurrentA; + private double statorCurrentA; + private double torqueCurrentA; + private double motorVoltageV; + private double supplyVoltageV; + + private BaseStatusSignal[] cachedSignals; + + public double getCachedDeviceTempC() { + return deviceTempC; + } + + public double getCachedClosedLoopError() { + return closedLoopErrorVal; + } + + public double getCachedClosedLoopReference() { + return closedLoopReferenceVal; + } + + public double getCachedRotorPosition() { + return rotorPositionVal; + } + + public double getCachedPositionRotations() { + return positionRotations; + } + + public double getCachedVelocityRps() { + return velocityRps; + } + + public double getCachedAccelerationRps2() { + return accelerationRps2; + } + + public double getCachedSupplyCurrentA() { + return supplyCurrentA; + } + + public double getCachedStatorCurrentA() { + return statorCurrentA; + } + + public double getCachedTorqueCurrentA() { + return torqueCurrentA; + } + + public double getCachedMotorVoltageV() { + return motorVoltageV; + } + + public double getCachedSupplyVoltageV() { + return supplyVoltageV; + } + + private void cacheSignals() { + deviceTempSignal = this.getDeviceTemp(false); + closedLoopErrorSignal = this.getClosedLoopError(false); + closedLoopReferenceSignal = this.getClosedLoopReference(false); + rotorPositionSignal = this.getRotorPosition(false); + positionSignal = this.getPosition(false); + velocitySignal = this.getVelocity(false); + accelerationSignal = this.getAcceleration(false); + supplyCurrentSignal = this.getSupplyCurrent(false); + statorCurrentSignal = this.getStatorCurrent(false); + torqueCurrentSignal = this.getTorqueCurrent(false); + motorVoltageSignal = this.getMotorVoltage(false); + supplyVoltageSignal = this.getSupplyVoltage(false); + + cachedSignals = + new BaseStatusSignal[] { + deviceTempSignal, + closedLoopErrorSignal, + closedLoopReferenceSignal, + rotorPositionSignal, + positionSignal, + velocitySignal, + accelerationSignal, + supplyCurrentSignal, + statorCurrentSignal, + torqueCurrentSignal, + motorVoltageSignal, + supplyVoltageSignal + }; + } + + /** + * @param deviceName Designated name of this LoggedTalonFX + * @param deviceId Motor ID of this LoggedTalonFX + * @param canbus CAN Bus object Associated with this LoggedTalonFX. + */ + public LoggedTalonFX(String deviceName, int deviceId, CANBus canbus) { + super(deviceId, canbus); + name = "Motors/" + deviceName; + init(); + } + + /** + * @param deviceName Designated name of this LoggedTalonFX + * @param deviceId Motor ID of this LoggedTalonFX + */ + public LoggedTalonFX(String deviceName, int deviceId) { + super(deviceId); + name = "Motors/" + deviceName; + init(); + } + + /** + * @param deviceId Motor ID of this LoggedTalonFX + * @param canbus CAN Bus object Associated with this LoggedTalonFX. + */ + public LoggedTalonFX(int deviceId, CANBus canbus) { + super(deviceId, canbus); + name = "Motors/Motor " + deviceId; + init(); + } + + /** + * @param deviceId Motor ID of this LoggedTalonFX + */ + public LoggedTalonFX(int deviceId) { + super(deviceId); + name = "Motors/Motor " + deviceId; + init(); + } + + private static void rebuildSignalCollection() { + signalsOnCanivore = + new StatusSignalCollection( + motorsOnCanivore.stream() + .flatMap(m -> Arrays.stream(m.cachedSignals)) + .toArray(BaseStatusSignal[]::new)); + + signalsOnRio = + new StatusSignalCollection( + motorsOnRio.stream() + .flatMap(m -> Arrays.stream(m.cachedSignals)) + .toArray(BaseStatusSignal[]::new)); + } + + /** Initializes strings that will be outputted through the LoggedTalonFX class. */ + public void init() { + if ("Viper".equals(this.getNetwork().getName())) { + motorsOnCanivore.add(this); + } else { + motorsOnRio.add(this); + } + + this.getConfigurator().apply(new TalonFXConfiguration()); + this.temperature = name + "/temperature(degC)"; + this.closedLoopError = name + "/closedLoopError"; + this.closedLoopReference = name + "/closedLoopReference"; + this.position = name + "/position(rotations)"; + this.velocity = name + "/velocity(rps)"; + this.acceleration = name + "/acceleration(rps2)"; + this.supplyCurrent = name + "/current/supply(A)"; + this.statorCurrent = name + "/current/stator(A)"; + this.torqueCurrent = name + "/current/torque(A)"; + this.motorVoltage = name + "/voltage/motor(V)"; + this.supplyVoltage = name + "/voltage/supply(V)"; + this.rotorPosition = name + "/closedloop/rotorPosition"; + + // Applying current limits + CurrentLimitsConfigs clc = + new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(true) + .withStatorCurrentLimit(80) + .withSupplyCurrentLimitEnable(true) + .withSupplyCurrentLimit(40); + // WITH A HIGH POWER MECHANISM, MAKE SURE TO INCREASE THE CURRENT LIMITS + + motorConfiguration.CurrentLimits = clc; + this.getConfigurator().apply(motorConfiguration); + cacheSignals(); + rebuildSignalCollection(); + } + + public void updateCurrentLimits(double statorCurrentLimit, double supplyCurrentLimit) { + CurrentLimitsConfigs clc = + new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(true) + .withStatorCurrentLimit(statorCurrentLimit) + .withSupplyCurrentLimitEnable(true) + .withSupplyCurrentLimit(supplyCurrentLimit); + + this.getConfigurator().apply(clc); + } + + public void updateCachedValues() { + deviceTempC = deviceTempSignal.getValueAsDouble(); + closedLoopErrorVal = closedLoopErrorSignal.getValueAsDouble(); + closedLoopReferenceVal = closedLoopReferenceSignal.getValueAsDouble(); + rotorPositionVal = rotorPositionSignal.getValueAsDouble(); + positionRotations = positionSignal.getValueAsDouble(); + velocityRps = velocitySignal.getValueAsDouble(); + accelerationRps2 = accelerationSignal.getValueAsDouble(); + supplyCurrentA = supplyCurrentSignal.getValueAsDouble(); + statorCurrentA = statorCurrentSignal.getValueAsDouble(); + torqueCurrentA = torqueCurrentSignal.getValueAsDouble(); + motorVoltageV = motorVoltageSignal.getValueAsDouble(); + supplyVoltageV = supplyVoltageSignal.getValueAsDouble(); + } + + public void logValues() { + DogLog.log(temperature, getCachedDeviceTempC()); + DogLog.log(closedLoopError, getCachedClosedLoopError()); + DogLog.log(closedLoopReference, getCachedClosedLoopReference()); + DogLog.log(rotorPosition, getCachedRotorPosition()); + DogLog.log(position, getCachedPositionRotations()); + DogLog.log(velocity, getCachedVelocityRps()); + DogLog.log(acceleration, getCachedAccelerationRps2()); + DogLog.log(supplyCurrent, getCachedSupplyCurrentA()); + DogLog.log(statorCurrent, getCachedStatorCurrentA()); + DogLog.log(torqueCurrent, getCachedTorqueCurrentA()); + DogLog.log(motorVoltage, getCachedMotorVoltageV()); + DogLog.log(supplyVoltage, getCachedSupplyVoltageV()); + } + + public static void periodic_static() { + signalsOnCanivore.refreshAll(); + for (LoggedTalonFX motor : motorsOnCanivore) { + motor.updateCachedValues(); + motor.logValues(); + } + + signalsOnRio.refreshAll(); + for (LoggedTalonFX motor : motorsOnRio) { + motor.updateCachedValues(); + motor.logValues(); + } + } +} diff --git a/src/main/java/frc/robot/util/MathUtils/MiscMath.java b/src/main/java/frc/robot/util/MathUtils/MiscMath.java new file mode 100644 index 0000000..daf887f --- /dev/null +++ b/src/main/java/frc/robot/util/MathUtils/MiscMath.java @@ -0,0 +1,23 @@ +package frc.robot.util.MathUtils; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; + +public class MiscMath { + public static double clamp(double value, double min, double max) { + return Math.max(min, Math.min(max, value)); + } + + public static Pose3d RotatedPosAroundVertical(Pose3d pose, double r) { + return new Pose3d( + pose.getX() * Math.cos(r) - pose.getY() * Math.sin(r), + pose.getX() * Math.sin(r) + pose.getY() * Math.cos(r), + pose.getZ(), + pose.getRotation()); + } + + public static Rotation2d normalizeAngle(Rotation2d a) { + double radians = a.getRadians(); + return new Rotation2d(radians - (2 * Math.PI) * Math.floor(radians / (2 * Math.PI))); + } +} diff --git a/src/main/java/frc/robot/util/MathUtils/Vector2.java b/src/main/java/frc/robot/util/MathUtils/Vector2.java new file mode 100644 index 0000000..0102246 --- /dev/null +++ b/src/main/java/frc/robot/util/MathUtils/Vector2.java @@ -0,0 +1,65 @@ +package frc.robot.util.MathUtils; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +public class Vector2 { + public float x; + public float y; + + public Vector2(float x, float y) { + this.x = x; + this.y = y; + } + + public Vector2(double x, double y) { + this.x = (float) x; + this.y = (float) y; + } + + public Vector2(Pose2d pose) { + x = (float) pose.getX(); + y = (float) pose.getY(); + } + + public float magnitude() { + return (float) Math.sqrt((Math.pow(x, 2f) + Math.pow(y, 2f))); + } + + public void plus(Vector2 other) { + x += other.x; + y += other.y; + } + + public static Vector2 add(Vector2... a) { + Vector2 newVec = new Vector2(0, 0); + for (Vector2 vec : a) { + newVec.plus(vec); + } + return newVec; + } + + public static Vector2 subtract(Vector2 a, Vector2 b) { + return new Vector2(a.x - b.x, a.y - b.y); + } + + public static Vector2 mult(Vector2 a, float m) { + return new Vector2(a.x * m, a.y * m); + } + + public static Vector2 mult(Vector2 a, double m) { + return new Vector2(a.x * m, a.y * m); + } + + public static Pose2d toPose2d(Vector2 a) { + return new Pose2d(a.x, a.y, new Rotation2d()); + } + + public static Vector2 fromPose2d(Pose2d a) { + return new Vector2(a.getX(), a.getY()); + } + + public static double dist(Vector2 a, Vector2 b) { + return Vector2.subtract(a, b).magnitude(); + } +} diff --git a/src/main/java/frc/robot/util/MathUtils/Vector3.java b/src/main/java/frc/robot/util/MathUtils/Vector3.java new file mode 100644 index 0000000..d3c88ee --- /dev/null +++ b/src/main/java/frc/robot/util/MathUtils/Vector3.java @@ -0,0 +1,78 @@ +package frc.robot.util.MathUtils; + +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.Translation2d; + +public class Vector3 { + public float x; + public float y; + public float z; + + public Vector3(float x, float y, float z) { + this.x = x; + this.y = y; + this.z = z; + } + + public Vector3(double x, double y, double z) { + this.x = (float) x; + this.y = (float) y; + this.z = (float) z; + } + + public Vector3(Pose2d pose) { + x = (float) pose.getX(); + y = (float) pose.getY(); + z = 0; + } + + public Vector3(Pose3d pose) { + x = (float) pose.getX(); + y = (float) pose.getY(); + z = (float) pose.getZ(); + } + + public float magnitude() { + return (float) Math.sqrt((Math.pow(x, 2f) + Math.pow(y, 2f) + Math.pow(z, 2f))); + } + + public void plus(Vector3 other) { + x += other.x; + y += other.y; + z += other.z; + } + + public static Vector3 add(Vector3... a) { + Vector3 newVec = new Vector3(0, 0, 0); + for (Vector3 vec : a) { + newVec.plus(vec); + } + return newVec; + } + + public static Vector3 subtract(Vector3 a, Vector3 b) { + return new Vector3(a.x - b.x, a.y - b.y, a.z - b.z); + } + + public static Vector3 mult(Vector3 a, float m) { + return new Vector3(a.x * m, a.y * m, a.z * m); + } + + public static Vector3 mult(Vector3 a, double m) { + return new Vector3(a.x * m, a.y * m, a.z * m); + } + + public static Pose2d toPose2d(Vector3 a) { + return new Pose2d(a.x, a.y, new Rotation2d()); + } + + public static Vector2 toVector2(Vector3 a) { + return new Vector2(a.x, a.y); + } + + public static Translation2d toTranslation2d(Vector3 a) { + return new Translation2d(a.x, a.y); + } +} diff --git a/src/main/java/frc/robot/util/MiscUtils.java b/src/main/java/frc/robot/util/MiscUtils.java new file mode 100644 index 0000000..cd272de --- /dev/null +++ b/src/main/java/frc/robot/util/MiscUtils.java @@ -0,0 +1,211 @@ +package frc.robot.util; + +import dev.doglog.DogLog; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import java.io.File; +import java.util.Optional; +import java.util.function.BooleanSupplier; + +public class MiscUtils { + public static int shiftIndicatorSum = 0; + + public static Pose2d plus(Pose2d a, Translation2d b) { + return new Pose2d( + a.getX() + b.getX(), a.getY() + b.getY(), new Rotation2d(a.getRotation().getRadians())); + } + + public static Pose2d plusWithRotation(Pose2d a, Pose2d b) { // Transform2d used to be Pose2d + return new Pose2d( + a.getX() + b.getX(), + a.getY() + b.getY(), + new Rotation2d(a.getRotation().getRadians() + b.getRotation().getRadians())); + } + + public static Alliance getSecondAlliance() { + String allianceChar = DriverStation.getGameSpecificMessage(); + if (allianceChar == null || allianceChar.isEmpty()) return null; + return switch (allianceChar.charAt(0)) { + case 'B' -> Alliance.Blue; + case 'R' -> Alliance.Red; + default -> null; + }; + } + + public static String activeFirst() { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isEmpty() || DriverStation.getMatchTime() < 105) return ""; + DogLog.log("Subsystems/LEDs/allianceNotEmpty", true); + Alliance ourAlliance = alliance.get(); + Alliance secondAlliance = getSecondAlliance(); + if (secondAlliance == null) return ""; + DogLog.log("Subsystems/LEDs/ourAlliance", ourAlliance.toString()); + DogLog.log("Subsystems/LEDs/secondAlliance", secondAlliance.toString()); + if (secondAlliance.equals(ourAlliance)) return "LATER"; + else return "NOW"; + } + + public static boolean areWeActive() { + return areWeActive(0.0); + } + + public static boolean areWeActive(double howEarly) { + Optional alliance = DriverStation.getAlliance(); + + if (alliance.isEmpty()) return false; + if (DriverStation.isAutonomousEnabled()) return true; + if (!DriverStation.isTeleopEnabled()) return false; + + // teleop is enabled + double currentMatchTime = DriverStation.getMatchTime(); + double earlyMatchTime = currentMatchTime - howEarly; + + String allianceChar = DriverStation.getGameSpecificMessage(); + + DogLog.log( + "Elastic/AllianceChar", + allianceChar == null || allianceChar.isEmpty() ? "Empty" : allianceChar); + + if (allianceChar == null || allianceChar.isEmpty()) return true; + Alliance secondAlliance = getSecondAlliance(); + if (secondAlliance == null) return true; + boolean redInactiveFirst = getSecondAlliance() == Alliance.Red; + + boolean weAreActiveFirst = + switch (alliance.get()) { + case Red -> !redInactiveFirst; + case Blue -> redInactiveFirst; + }; + + DogLog.log("Subsystems/LEDs/matchTime", currentMatchTime); + + double earlyActiveFirst = weAreActiveFirst ? earlyMatchTime : currentMatchTime; + DogLog.log("Subsystems/LEDs/earlyActiveFirst", earlyActiveFirst); + double earlyActiveSecond = !weAreActiveFirst ? earlyMatchTime : currentMatchTime; + DogLog.log("Subsystems/LEDs/earlyActiveSecond", earlyActiveFirst); + + if (currentMatchTime > 130) return true; + else if (earlyActiveSecond > 105) return weAreActiveFirst; + else if (earlyActiveFirst > 80) return !weAreActiveFirst; + else if (earlyActiveSecond > 55) return weAreActiveFirst; + else if (earlyActiveFirst > 30) return !weAreActiveFirst; + else return true; + } + + public static double countdownTillNextShift(double currentTime) { + // double currentMatchTime = currentTime; + double currentMatchTime = DriverStation.getMatchTime(); + if (DriverStation.isAutonomous()) { + return currentMatchTime; + } else { + if (currentMatchTime > 140) return currentMatchTime - 140; + else if (currentMatchTime > 130) return currentMatchTime - 130; + else if (currentMatchTime > 105) return currentMatchTime - 105; + else if (currentMatchTime > 80) return currentMatchTime - 80; + else if (currentMatchTime > 55) return currentMatchTime - 55; + else if (currentMatchTime > 30) return currentMatchTime - 30; + else return currentMatchTime; + } + } + + public static String currentShiftName(double currentTime) { + // double currentMatchTime = currentTime; + double currentMatchTime = DriverStation.getMatchTime(); + if (DriverStation.isAutonomous()) return "Auto"; + + if (currentMatchTime > 130) return "Transition"; + else if (currentMatchTime > 105) return "ALS 1"; + else if (currentMatchTime > 80) return "ALS 2"; + else if (currentMatchTime > 55) return "ALS 3"; + else if (currentMatchTime > 30) return "ALS 4"; + else return "Endgame"; + } + + public static void shiftSwitchIndicator(double currentTime) { + // double currentTimes = currentTime; + double currentTimes = DriverStation.getMatchTime(); + double timeUntilNextShift = countdownTillNextShift(currentTimes); + boolean isEndgame = currentShiftName(currentTimes).equals("Endgame"); + boolean isActive = areWeActive(); + boolean isTransition = currentShiftName(currentTimes).equals("Transition"); + + if (isTransition || isEndgame) { + shiftIndicatorSum = 0; + SmartDashboard.putString("Elastic/ShiftSwitchIndicator", "#00FF00"); + return; + } + + String color = ""; + if (isActive) { + shiftIndicatorSum = timeUntilNextShift >= 8 ? 0 : shiftIndicatorSum + 1; + if (timeUntilNextShift >= 8) color = "#00FF00"; + else if (timeUntilNextShift < 2) color = "#000000"; + // fast blink + else if (timeUntilNextShift < 5) + color = (shiftIndicatorSum / 8) % 2 == 0 ? "#FF0000" : "#000000"; + // slow blink + else if (timeUntilNextShift < 8) + color = (shiftIndicatorSum / 20) % 2 == 0 ? "#FF0000" : "#000000"; + } else { + // alliance hub inactive + shiftIndicatorSum = + timeUntilNextShift < 2 || timeUntilNextShift >= 8 ? 0 : shiftIndicatorSum + 1; + if (timeUntilNextShift < 2) color = "#00FF00"; + // fast blink + else if (timeUntilNextShift < 5) + color = (shiftIndicatorSum / 8) % 2 == 0 ? "#FFFF00" : "#000000"; + // slow blink + else if (timeUntilNextShift < 8) + color = (shiftIndicatorSum / 20) % 2 == 0 ? "#FFFF00" : "#000000"; + else color = "#000000"; + } + SmartDashboard.putString("Elastic/ShiftSwitchIndicator", color); + } + + public static double get3dDistance(Transform3d transform) { + return Math.hypot(Math.hypot(transform.getX(), transform.getY()), transform.getZ()); + } + + public static double getDistanceToHub(BooleanSupplier redSide, CommandSwerveDrivetrain swerve) { + Pose2d robotPose = swerve.getCurrentState().Pose; + + Translation2d hubTranslation = + redSide.getAsBoolean() + ? new Translation2d( + Constants.Landmarks.RED_HUB.getX(), Constants.Landmarks.RED_HUB.getY()) + : new Translation2d( + Constants.Landmarks.BLUE_HUB.getX(), Constants.Landmarks.BLUE_HUB.getY()); + + return robotPose.getTranslation().getDistance(hubTranslation); + } + + public static double computeShootingSpeed(double distToHubCenter) { + // Constants (meters) + final double a = 20.41232; + final double b = 57.26412; + final double c = -0.182208; + + return (a * Math.sqrt(distToHubCenter - c)) + b; + } + + public static boolean isFlashDriveConnected() { + // Check if /u/logs exists and is writable (indicates USB is actually mounted) + File logsDir = new File("/u/logs"); + DogLog.log("Elastic/LogsDirExists", logsDir.exists()); + DogLog.log("Elastic/LogsDirIsDirectory", logsDir.isDirectory()); + DogLog.log("Elastic/LogsDirCanWrite", logsDir.canWrite()); + return logsDir.exists() && logsDir.isDirectory() && logsDir.canWrite(); + } + + public static File getFlashDriveDirectory() { + File usbDrive = new File("/u"); + return usbDrive.exists() && usbDrive.isDirectory() ? usbDrive : null; + } +} diff --git a/src/main/java/frc/robot/util/Targeting.java b/src/main/java/frc/robot/util/Targeting.java new file mode 100644 index 0000000..6271b46 --- /dev/null +++ b/src/main/java/frc/robot/util/Targeting.java @@ -0,0 +1,276 @@ +package frc.robot.util; + +import dev.doglog.DogLog; +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.math.util.Units; +import frc.robot.Constants; +import frc.robot.Constants.Landmarks; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.util.MathUtils.Vector3; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +public class Targeting { + public static class TargetingInfo { + private double speed; + private double tof; + private Pose2d pos; + + public TargetingInfo(double speed, double tof, Pose2d pos) { + this.speed = speed; + this.tof = tof; + this.pos = pos; + } + + public double getSpeed() { + return speed; + } + + public double getToF() { + return tof; + } + + public Pose2d getPosition() { + return pos; + } + } + + public static Translation2d computeVirtualTarget( + Pose2d target, CommandSwerveDrivetrain drivetrain) { + ChassisSpeeds fieldSpeeds = drivetrain.getFieldSpeeds(); + Pose2d currPose = drivetrain.getPose(); + + // Twist2d twist = + // new Twist2d( + // fieldSpeeds.vxMetersPerSecond * 0.03, + // fieldSpeeds.vyMetersPerSecond * 0.03, + // fieldSpeeds.omegaRadiansPerSecond * 0.03); + // Pose2d lookaheadPose = currPose.exp(twist); + + double initDX = target.getX() - currPose.getX(); + double initDY = target.getY() - currPose.getY(); + double initialDistance = Math.sqrt(initDX * initDX + initDY * initDY); + + if (initialDistance < 1e-6) return target.getTranslation(); + + // double radialVelocity = + // (initDX * fieldSpeeds.vxMetersPerSecond + initDY * fieldSpeeds.vyMetersPerSecond) + // / initialDistance; + + double tof = Constants.Shooter.TIME_OF_FLIGHT_MAP.get(initialDistance); + // initialDistance + // / (initialDistance / Constants.Shooter.TIME_OF_FLIGHT_MAP.get(initialDistance) + // - radialVelocity); + + for (int i = 0; i < Constants.Shooter.TARGETING_CALCULATION_PRECISION; i++) { + double distX = initDX - fieldSpeeds.vxMetersPerSecond * tof; + double distY = initDY - fieldSpeeds.vyMetersPerSecond * tof; + double distance = Math.sqrt(distX * distX + distY * distY); + + if (distance < 1e-6) break; + + double tofTable = Constants.Shooter.TIME_OF_FLIGHT_MAP.get(distance); + double error = tof - tofTable; + + if (Math.abs(error) < 1e-3) break; + + double horizontalVel = distance / tofTable; + double errorDerivative = + 1.0 + + ((distX * fieldSpeeds.vxMetersPerSecond + distY * fieldSpeeds.vyMetersPerSecond) + / (distance * horizontalVel)); + + if (tof < 1e-3) tof = 1e-3; + + double step = error / errorDerivative; + // step = Math.max(-0.05, Math.min(0.05, step)); + step = Math.max(-0.1 * tof, Math.min(0.1 * tof, step)); + tof -= step; + } + + return new Translation2d( + target.getX() - fieldSpeeds.vxMetersPerSecond * tof, + target.getY() - fieldSpeeds.vyMetersPerSecond * tof); + } + + public static boolean pointingAtTarget( + Pose2d targetNoOffset, CommandSwerveDrivetrain drivetrain) { + double desiredRobotHullAngle = + targetAngle(targetNoOffset, drivetrain) + (2 * Math.PI) % (2 * Math.PI); + + double robotHullAngle = + drivetrain.getCurrentState().Pose.getRotation().getRadians() + + (2 * Math.PI) % (2 * Math.PI); + + double diff = Math.abs(desiredRobotHullAngle - robotHullAngle) % (2 * Math.PI); + if (diff > Math.PI) diff = 2 * Math.PI - diff; + DogLog.log("Subsystems/Shooter/Shoot/RotationalErrorRadians", diff); + boolean hullAimed = diff <= Constants.Shooter.ANGULAR_TOLERANCE_FOR_AUTO_AIM_RAD; + DogLog.log("Subsystems/Shooter/Shoot/Pointing", hullAimed); + return hullAimed; + } + + public static boolean pointingAtTarget(double angle, CommandSwerveDrivetrain drivetrain) { + double desiredRobotHullAngle = angle; + + double robotHullAngle = + drivetrain.getCurrentState().Pose.getRotation().getRadians() + + (2 * Math.PI) % (2 * Math.PI); + DogLog.log("PointingAtTarget/DesiredAngle", desiredRobotHullAngle); + DogLog.log("PointingAtTarget/RobotAngle", robotHullAngle); + double diff = Math.abs(desiredRobotHullAngle - robotHullAngle) % (2 * Math.PI); + if (diff > Math.PI) diff = 2 * Math.PI - diff; + boolean hullAimed = diff <= Constants.Shooter.ANGULAR_TOLERANCE_FOR_AUTO_AIM_RAD; + return hullAimed; + } + + // public static TargetingInfo targetingInfo( + // Pose2d target, CommandSwerveDrivetrain drivetrain, int precision) { + // Vector3 relativeVel = + // Vector3.mult( + // new Vector3( + // drivetrain.getFieldSpeeds().vxMetersPerSecond, + // drivetrain.getFieldSpeeds().vyMetersPerSecond, + // 0), + // -1); + + // Vector3 shooterPos = new Vector3(drivetrain.getCurrentState().Pose); + // Vector3 relativePos = Vector3.subtract(new Vector3(target), shooterPos); + + // double correctedSpeed = speedForDist(relativePos.magnitude()); + // double prevTof = 0; + // Vector3 correctedPos = new Vector3(target); + + // for (int i = 0; i < precision; i++) { + // double dist = Vector3.subtract(correctedPos, shooterPos).magnitude(); + // double tof = Constants.Shooter.TOF_FOR_DISTANCE_METERS_CENTER_TO_CENTER_INTERMAP.get(dist); + // correctedPos = Vector3.add(correctedPos, Vector3.mult(relativeVel, tof - prevTof)); + // correctedSpeed = speedForDist(Vector3.subtract(correctedPos, shooterPos).magnitude()); + // prevTof = tof; + // } + // DogLog.log("Subsystems/ShooterSubsystem/Shoot/shootspeed", correctedSpeed); + // DogLog.log("Subsystems/ShooterSubsystem/Shoot/Tof", prevTof); + // DogLog.log("Subsystems/ShooterSubsystem/Shoot/targetPlusLead", + // Vector3.toPose2d(correctedPos)); + + // return new TargetingInfo(correctedSpeed, prevTof, correctedPos); + // } + + public static double newtonTargetingDistance(Pose2d target, CommandSwerveDrivetrain swerve) { + // Load basic stuff in + ChassisSpeeds currSpeeds = swerve.getFieldSpeeds(); + Pose2d currState = swerve.getPose(); + + // initial guess + double initDX = target.getX() - currState.getX(); + double initDY = target.getY() - currState.getY(); + double initialDistance = Math.pow(initDX * initDX + initDY * initDY, 0.5); + + if (initialDistance < 1e-6) return 0; + + double radialVelocity = + (initDX * currSpeeds.vxMetersPerSecond + initDY * currSpeeds.vyMetersPerSecond) + / initialDistance; + + // shit we need + double tof = + initialDistance + / (initialDistance / Constants.Shooter.TIME_OF_FLIGHT_MAP.get(initialDistance) + - radialVelocity); + + double distance = initialDistance; + + for (int i = 0; i < Constants.Shooter.TARGETING_CALCULATION_PRECISION; i++) { + double distX = (initDX) - currSpeeds.vxMetersPerSecond * tof; + double distY = (initDY) - currSpeeds.vyMetersPerSecond * tof; + + distance = Math.pow(distX * distX + distY * distY, 0.5); + if (distance < 1e-6) break; + + double tofTable = Constants.Shooter.TIME_OF_FLIGHT_MAP.get(distance); + double error = tof - tofTable; + + double horizontalVel = distance / tofTable; + double errorDerivative = + 1.0 + - ((distX * currSpeeds.vxMetersPerSecond + distY * currSpeeds.vyMetersPerSecond) + / (distance * horizontalVel)); + + if (Math.abs(error) < 1e-3) break; + + if (tof < 1e-3) tof = 1e-3; + + double step = error / errorDerivative; + step = Math.max(-0.05, Math.min(0.05, step)); + tof -= step; + } + + return distance; + } + + public static TargetingInfo newtonTargetingInfo( + Pose2d target, CommandSwerveDrivetrain drivetrain) { + double distance = newtonTargetingDistance(target, drivetrain); + double timeOfFlight = Constants.Shooter.TIME_OF_FLIGHT_MAP.get(distance); + + Vector3 relativeVel = + Vector3.mult( + new Vector3( + drivetrain.getFieldSpeeds().vxMetersPerSecond, + drivetrain.getFieldSpeeds().vyMetersPerSecond, + 0), + -1); + Vector3 targetPlusOffset = + Vector3.add(new Vector3(target), Vector3.mult(relativeVel, timeOfFlight)); + + return new TargetingInfo( + Constants.Shooter.TIME_OF_FLIGHT_MAP.get(distance), + timeOfFlight, + Vector3.toPose2d(targetPlusOffset)); + } + + public static double shootingSpeed(Pose2d target, CommandSwerveDrivetrain drivetrain) { + return newtonTargetingInfo(target, drivetrain).getSpeed(); + } + + public static Pose2d positionToTarget(Pose2d target, CommandSwerveDrivetrain drivetrain) { + return newtonTargetingInfo(target, drivetrain).getPosition(); + } + + public static double speedForDist(double d) { + return Constants.Shooter.TIME_OF_FLIGHT_MAP.get(d); + } + + public static double targetAngle(Pose2d targetNoOffset, CommandSwerveDrivetrain drivetrain) { + Pose2d target = positionToTarget(targetNoOffset, drivetrain); + return Math.atan2( + Vector3.subtract(new Vector3(target), new Vector3(drivetrain.getCurrentState().Pose)).y, + Vector3.subtract(new Vector3(target), new Vector3(drivetrain.getCurrentState().Pose)).x) + + (Constants.Shooter.SHOOTS_BACKWARDS ? Math.PI : 0); + } + + public static double distMeters(CommandSwerveDrivetrain drivetrain, Pose2d target) { + return Vector3.subtract(new Vector3(drivetrain.getCurrentState().Pose), new Vector3(target)) + .magnitude(); + } + + public static DoubleSupplier amtToRumble(CommandSwerveDrivetrain drivetrain, Pose2d target) { + return () -> + Units.metersToFeet(distMeters(drivetrain, target)) > Constants.Shooter.MAX_DIST_FT + || Units.metersToFeet(distMeters(drivetrain, target)) + < Constants.Shooter.MIN_DIST_FT + ? .5d + : 0d; + } + + public static boolean pointingAtHub(BooleanSupplier redside, CommandSwerveDrivetrain drivetrain) { + Pose2d target = redside.getAsBoolean() ? Landmarks.RED_HUB : Landmarks.BLUE_HUB; + return pointingAtTarget(target, drivetrain); + } + + public static Pose2d getHub(BooleanSupplier redside) { + return (redside.getAsBoolean() ? Landmarks.RED_HUB : Landmarks.BLUE_HUB); + } +} diff --git a/src/main/java/frc/robot/util/VisionCameraConstants.java b/src/main/java/frc/robot/util/VisionCameraConstants.java new file mode 100644 index 0000000..cb909fc --- /dev/null +++ b/src/main/java/frc/robot/util/VisionCameraConstants.java @@ -0,0 +1,242 @@ +package frc.robot.util; + +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +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.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N8; +import edu.wpi.first.math.util.Units; + +public final class VisionCameraConstants { + private static final class FrontRightVals { + private static final double X = Units.inchesToMeters(-2.160666); + private static final double Y = Units.inchesToMeters(-12.316702); // mrd negative + private static final double Z = Units.inchesToMeters(26.967089); + private static final double ROLL = Units.degreesToRadians(0.0); + private static final double PITCH = Units.degreesToRadians(0.0); + private static final double YAW = Units.degreesToRadians(-42.0); // mrd 310 + private static final double FX = 913.9588182724549; // cameramatrix + private static final double CX = 635.3113759543929; + private static final double FY = 913.1110338032502; + private static final double CY = 426.1145158880936; + private static final double K1 = 0.053777333313074126; // distortioncoeffs + private static final double K2 = -0.09163028264269202; + private static final double P1 = 1.374201650684413E-4; + private static final double P2 = 1.705990575776099E-4; + private static final double K3 = 0.03043443776012655; + private static final double K4 = -0.0014003230555085467; + private static final double K5 = 0.003012682052878214; + private static final double K6 = -0.0018355589720653578; + } + + public static final class FrontRight { + public static final Transform3d transform = + new Transform3d( + new Translation3d(FrontRightVals.X, FrontRightVals.Y, FrontRightVals.Z), + new Rotation3d(FrontRightVals.ROLL, FrontRightVals.PITCH, FrontRightVals.YAW)); + public static final Matrix cameraMatrix = + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + FrontRightVals.FX, + 0.0, + FrontRightVals.CX, + 0.0, + FrontRightVals.FY, + FrontRightVals.CY, + 0.0, + 0.0, + 1.0); + public static final Matrix distCoeffs = + MatBuilder.fill( + Nat.N8(), + Nat.N1(), + FrontRightVals.K1, + FrontRightVals.K2, + FrontRightVals.P1, + FrontRightVals.P2, + FrontRightVals.K3, + FrontRightVals.K4, + FrontRightVals.K5, + FrontRightVals.K6); + } + + private static final class FrontLeftVals { + private static final double X = Units.inchesToMeters(-2.160666); + private static final double Y = Units.inchesToMeters(12.316702); // mrd positive + private static final double Z = Units.inchesToMeters(26.967089); + private static final double ROLL = Units.degreesToRadians(0.0); + private static final double PITCH = Units.degreesToRadians(0.0); + private static final double YAW = Units.degreesToRadians(42.0); // mrd 50 + private static final double FX = 912.5173606679848; + private static final double CX = 641.6860955338284; + private static final double FY = 911.7465912506361; + private static final double CY = 352.76061130405736; + private static final double K1 = 0.0510489487465398; + private static final double K2 = -0.08377140729235197; + private static final double P1 = 1.434092037672156E-4; + private static final double P2 = -6.852290091675845E-4; + private static final double K3 = 0.023945103433457356; + private static final double K4 = -0.0027178370025379562; + private static final double K5 = 0.005487814267694603; + private static final double K6 = 7.918351577621345E-4; + } + + public static final class FrontLeft { + public static final Transform3d transform = + new Transform3d( + new Translation3d(FrontLeftVals.X, FrontLeftVals.Y, FrontLeftVals.Z), + new Rotation3d(FrontLeftVals.ROLL, FrontLeftVals.PITCH, FrontLeftVals.YAW)); + public static final Matrix cameraMatrix = + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + FrontLeftVals.FX, + 0.0, + FrontLeftVals.CX, + 0.0, + FrontLeftVals.FY, + FrontLeftVals.CY, + 0.0, + 0.0, + 1.0); + public static final Matrix distCoeffs = + MatBuilder.fill( + Nat.N8(), + Nat.N1(), + FrontLeftVals.K1, + FrontLeftVals.K2, + FrontLeftVals.P1, + FrontLeftVals.P2, + FrontLeftVals.K3, + FrontLeftVals.K4, + FrontLeftVals.K5, + FrontLeftVals.K6); + } + + private static final class RearRightVals { + private static final double X = Units.inchesToMeters(-14.106719); + private static final double Y = Units.inchesToMeters(-11.537644); // mrd negative + private static final double Z = Units.inchesToMeters(23.570693); + private static final double ROLL = Units.degreesToRadians(0.0); + private static final double PITCH = Units.degreesToRadians(330.321995); + private static final double YAW = Units.degreesToRadians(148.35615); // mrd 328.35615 + private static final double FX = 918.2654314246229; + private static final double CX = 633.0888048530759; + private static final double FY = 910.5580953980847; + private static final double CY = 388.6942556055274; + private static final double K1 = 0.04787004424953315; + private static final double K2 = -0.06567381272383281; + private static final double P1 = 3.7779485769898034E-4; + private static final double P2 = -3.4797638844685785E-4; + private static final double K3 = 0.009051433582570044; + private static final double K4 = -0.002113451011579978; + private static final double K5 = 0.005575990805806449; + private static final double K6 = 0.001987724097504375; + } + + public static final class RearRight { + public static final Transform3d transform = + new Transform3d( + new Translation3d(RearRightVals.X, RearRightVals.Y, RearRightVals.Z), + new Rotation3d(RearRightVals.ROLL, RearRightVals.PITCH, RearRightVals.YAW)); + public static final Matrix cameraMatrix = + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + RearRightVals.FX, + 0.0, + RearRightVals.CX, + 0.0, + RearRightVals.FY, + RearRightVals.CY, + 0.0, + 0.0, + 1.0); + public static final Matrix distCoeffs = + MatBuilder.fill( + Nat.N8(), + Nat.N1(), + RearRightVals.K1, + RearRightVals.K2, + RearRightVals.P1, + RearRightVals.P2, + RearRightVals.K3, + RearRightVals.K4, + RearRightVals.K5, + RearRightVals.K6); + } + + private static final class RearLeftVals { + private static final double X = Units.inchesToMeters(-14.106719); + private static final double Y = Units.inchesToMeters(11.537644); // mrd positive + private static final double Z = Units.inchesToMeters(23.570693); + private static final double ROLL = Units.degreesToRadians(0.0); + private static final double PITCH = Units.degreesToRadians(330.321995); + private static final double YAW = Units.degreesToRadians(211.64385); // mrd 31.643850 + private static final double FX = 914.9774783104568; + private static final double CX = 644.1129168082753; + private static final double FY = 915.0443146858144; + private static final double CY = 431.5500633760754; + private static final double K1 = 0.04802570965310978; + private static final double K2 = -0.07484429153153757; + private static final double P1 = -6.419202733122086E-4; + private static final double P2 = 3.908803817378362E-4; + private static final double K3 = 0.01582645075158188; + private static final double K4 = -0.0017780939236932693; + private static final double K5 = 0.003169375124089103; + private static final double K6 = -5.770679995204119E-4; + } + + public static final class RearLeft { + public static final Transform3d transform = + new Transform3d( + new Translation3d(RearLeftVals.X, RearLeftVals.Y, RearLeftVals.Z), + new Rotation3d(RearLeftVals.ROLL, RearLeftVals.PITCH, RearLeftVals.YAW)); + public static final Matrix cameraMatrix = + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + RearLeftVals.FX, + 0.0, + RearLeftVals.CX, + 0.0, + RearLeftVals.FY, + RearLeftVals.CY, + 0.0, + 0.0, + 1.0); + public static final Matrix distCoeffs = + MatBuilder.fill( + Nat.N8(), + Nat.N1(), + RearLeftVals.K1, + RearLeftVals.K2, + RearLeftVals.P1, + RearLeftVals.P2, + RearLeftVals.K3, + RearLeftVals.K4, + RearLeftVals.K5, + RearLeftVals.K6); + } + + private static final class FuelGaugeVals { + private static final double X = Units.inchesToMeters(-3.454827); + private static final double Y = Units.inchesToMeters(-7.056897); + private static final double Z = Units.inchesToMeters(25.105416); + private static final double ROLL = Units.degreesToRadians(286.894287); + private static final double PITCH = Units.degreesToRadians(55.646896); + private static final double YAW = Units.degreesToRadians(23.957651); + } + + public static final class FuelGauge { + public static final Transform3d transform = + new Transform3d( + new Translation3d(FuelGaugeVals.X, FuelGaugeVals.Y, FuelGaugeVals.Z), + new Rotation3d(FuelGaugeVals.ROLL, FuelGaugeVals.PITCH, FuelGaugeVals.YAW)); + } +} diff --git a/src/main/java/frc/robot/util/VisionUtils.java b/src/main/java/frc/robot/util/VisionUtils.java new file mode 100644 index 0000000..ccd6230 --- /dev/null +++ b/src/main/java/frc/robot/util/VisionUtils.java @@ -0,0 +1,285 @@ +package frc.robot.util; + +import dev.doglog.DogLog; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants; +import frc.robot.Constants.FuelGaugeDetection.FuelGauge; +import frc.robot.Constants.IntakeVision.TargetingMode; +import frc.robot.Constants.Vision.CameraSelectionMethod; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.FuelGaugeDetection; +import frc.robot.subsystems.IntakeVisionDetection; +import frc.robot.subsystems.VisionSubsystem; +import org.photonvision.targeting.PhotonPipelineResult; + +public class VisionUtils { + private static VisionSubsystem visionFrontRight, visionFrontLeft, visionRearRight, visionRearLeft; + private static VisionSubsystem preferredVision; + private static CommandSwerveDrivetrain drivetrain; + private static double headingThreshold; + + public static void visionPeriodic( + VisionSubsystem frontRight, + VisionSubsystem frontLeft, + VisionSubsystem rearRight, + VisionSubsystem rearLeft, + CommandSwerveDrivetrain swerve) { + if (!Constants.visionOnRobot + || frontRight == null + || frontLeft == null + || rearRight == null + || rearLeft == null) return; + + visionFrontRight = frontRight; + visionFrontLeft = frontLeft; + visionRearRight = rearRight; + visionRearLeft = rearLeft; + drivetrain = swerve; + + calculateAllCameraPoses(); + + preferredVision = getVisionFallback(); + + if (!Constants.Vision.SKIP_TO_FALLBACK) preferredVision = selectPreferredVision(); + + if (preferredVision == null || !preferredVision.hasValidMeasurement()) return; + + preferredVision.addFilteredPose(); + + preferredVisionLogs(); + } + + private static VisionSubsystem getVisionFallback() { + switch (Constants.Vision.FALLBACK_CAMERA) { + case FRONT_RIGHT_CAM: + return visionFrontRight; + case FRONT_LEFT_CAM: + default: + return visionFrontLeft; + case REAR_RIGHT_CAM: + return visionRearRight; + case REAR_LEFT_CAM: + return visionRearLeft; + } + } + + private static void calculateAllCameraPoses() { + visionFrontRight.calculateFilteredPose(); + visionFrontLeft.calculateFilteredPose(); + visionRearRight.calculateFilteredPose(); + visionRearLeft.calculateFilteredPose(); + } + + private static VisionSubsystem selectPreferredVision() { + CameraSelectionMethod method = Constants.Vision.CAMERA_SELECTION_METHOD; + double frontRightDist = getCameraDistance(visionFrontRight, method); + double frontLeftDist = getCameraDistance(visionFrontLeft, method); + double rearRightDist = getCameraDistance(visionRearRight, method); + double rearLeftDist = getCameraDistance(visionRearLeft, method); + + return compareCameraDistance(frontRightDist, frontLeftDist, rearRightDist, rearLeftDist); + } + + private static double getCameraDistance( + VisionSubsystem vision, Constants.Vision.CameraSelectionMethod method) { + switch (method) { + case MIN: + default: + return vision.getMinDistance(); + case AVG: + return vision.getAverageDistance(); + case MAX: + return vision.getMaxDistance(); + case POSE_AMBIGUITY: + return vision.getPoseAmbiguity(); + } + } + + private static VisionSubsystem compareCameraDistance( + double frontRight, double frontLeft, double rearRight, double rearLeft) { + double bestDistance = Double.MAX_VALUE; + VisionSubsystem bestVision = preferredVision; + + if (frontRight < bestDistance && visionFrontRight.hasValidMeasurement()) { + bestVision = visionFrontRight; + bestDistance = frontRight; + } + + if (frontLeft < bestDistance && visionFrontLeft.hasValidMeasurement()) { + bestVision = visionFrontLeft; + bestDistance = frontLeft; + } + + if (rearRight < bestDistance && visionRearRight.hasValidMeasurement()) { + bestVision = visionRearRight; + bestDistance = rearRight; + } + + if (rearLeft < bestDistance && visionRearLeft.hasValidMeasurement()) { + bestVision = visionRearLeft; + bestDistance = rearLeft; + } + + return bestVision; + } + + private static void preferredVisionLogs() { + DogLog.log("Subsystems/Vision/PreferredCamera", preferredVision.getCameraID().getLoggingName()); + DogLog.log("Subsystems/Vision/CompletePoseEstimate", drivetrain.getState().Pose); + DogLog.log("Subsystems/Vision/RawPoseEstimate", preferredVision.getFilteredPose()); + } + + public static void fuelGaugeLogs(FuelGaugeDetection visionFuelGauge) { + if (Constants.fuelGaugeOnRobot && visionFuelGauge != null) { + FuelGauge gaugeState = visionFuelGauge.getGaugeDefault(); + String gaugeStateHex = getColorOrDefault(gaugeState); + DogLog.log("Elastic/FuelGauge", gaugeState.toString()); + SmartDashboard.putString("Elastic/FuelGaugeHex", gaugeStateHex); + DogLog.log("Elastic/FuelGauge/CameraConnected", true); + } else { + DogLog.log("Elastic/FuelGauge", "N/A"); + SmartDashboard.putString("Elastic/FuelGaugeHex", "#00FFFF"); + DogLog.log("Elastic/FuelGauge/CameraConnected", false); + } + } + + private static String getColorOrDefault(FuelGauge gauge) { + return gauge == null ? "#FFFFFF" : gauge.getColor(); + } + + public static Matrix computeNoiseVector(double distance, double speed, int tagCount) { + double nX = computeNoiseX(distance, speed, tagCount); + double nY = computeNoiseY(distance, speed, tagCount); + double nTH = computeNoiseHeading(distance, speed, tagCount); + return VecBuilder.fill(nX, nY, nTH); + } + + private static double computeNoiseXY( + double baseNoise, + double distanceExponentialCoefficient, + double distanceExponentialBase, + double angleCoefficient, + double speedCoefficient, + double distance, + double robotSpeed, + int tagCount) { + + double maximumRobotSpeed = Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND; + + // Tag count factor (cap at 4 - diminishing returns) + int effectiveTags = Math.min(tagCount, 4); + double tagFactor = 1d / Math.sqrt(effectiveTags); + + double distanceFactor = + baseNoise + distanceExponentialCoefficient * Math.pow(distanceExponentialBase, distance); + + // Speed term (quadratic) + double vNorm = Math.min(robotSpeed, maximumRobotSpeed) / maximumRobotSpeed; + double speedFactor = 1d + speedCoefficient * (vNorm * vNorm); + + DogLog.log("Subsystems/Vision/calibrationFactor", Constants.Vision.CALIBRATION_FACTOR); + DogLog.log("Subsystems/Vision/tagFactor", tagFactor); + DogLog.log("Subsystems/Vision/distanceFactor", distanceFactor); + DogLog.log("Subsystems/Vision/speedFactor", speedFactor); + + double computedStdDevs = + Constants.Vision.CALIBRATION_FACTOR * tagFactor * distanceFactor * speedFactor; + return computedStdDevs; + } + + private static double computeNoiseHeading(double distance, double robotSpeed, int tagCount) { + double baseNoise = Constants.Vision.BASE_NOISE_THETA; + double distanceCoefficient = Constants.Vision.DISTANCE_COEFFICIENT_THETA; + double speedCoefficient = Constants.Vision.SPEED_COEFFICIENT_THETA; + double maximumRobotSpeed = Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND; + + // Tag count factor (cap at 4 - diminishing returns) + int effectiveTags = Math.min(tagCount, 4); + double tagFactor = 1d / Math.sqrt(effectiveTags); + + // Distance term (d^2) + double distanceFactor = baseNoise + distanceCoefficient * distance * distance; + + double vNorm = Math.min(robotSpeed, maximumRobotSpeed) / maximumRobotSpeed; + double speedFactor = 1d + speedCoefficient * (vNorm * vNorm); + + double computedStdDevs = + Constants.Vision.CALIBRATION_FACTOR * tagFactor * distanceFactor * speedFactor; + return computedStdDevs; + } + + private static double computeNoiseX(double distance, double robotSpeed, int tagCount) { + return computeNoiseXY( + Constants.Vision.BASE_NOISE_X, + Constants.Vision.DISTANCE_EXPONENTIAL_COEFFICIENT_X, + Constants.Vision.DISTANCE_EXPONENTIAL_BASE_X, + Constants.Vision.ANGLE_COEFFICIENT_X, + Constants.Vision.SPEED_COEFFICIENT_X, + distance, + robotSpeed, + tagCount); + } + + private static double computeNoiseY(double distance, double robotSpeed, int tagCount) { + return computeNoiseXY( + Constants.Vision.BASE_NOISE_Y, + Constants.Vision.DISTANCE_EXPONENTIAL_COEFFICIENT_Y, + Constants.Vision.DISTANCE_EXPONENTIAL_BASE_Y, + Constants.Vision.ANGLE_COEFFICIENT_Y, + Constants.Vision.SPEED_COEFFICIENT_Y, + distance, + robotSpeed, + tagCount); + } + + public static record IntakeVisionTarget(Pose2d pose, TargetingMode mode) {} + + public static IntakeVisionTarget intakeVisionTargetPose( + Pose2d pose, IntakeVisionDetection vision) { + double degYaw = vision.getYaw(); + double degPitch = vision.getPitch(); + double distance = estimateDistanceFromPitch(degPitch); + DogLog.log("Subsystems/IntakeVision/EstimatedDistance", distance); + Rotation2d heading = pose.getRotation().plus(Rotation2d.fromDegrees(degYaw)); + + if (distance > Constants.IntakeVision.MIN_DETECTION_DIST + && distance < Constants.IntakeVision.MAX_DETECTION_DIST) { + Translation2d poseManipulation = new Translation2d(distance, heading); + return new IntakeVisionTarget( + new Pose2d(pose.getTranslation().plus(poseManipulation), heading), TargetingMode.LOC_SEL); + } else { + return new IntakeVisionTarget( + new Pose2d(pose.getTranslation(), heading), TargetingMode.HDG_SEL); + } + } + + private static double estimateDistanceFromPitch(double degPitch) { + double targetHeight = 0.0; + double pitch = Units.degreesToRadians(degPitch); + double slope = Math.tan(Constants.IntakeVision.INTAKE_PITCH + pitch); + DogLog.log("Subsystems/IntakeVision/CalculatedSlope", slope); + if (Math.abs(slope) < Constants.IntakeVision.MIN_DETECTION_SLOPE) return Double.MAX_VALUE; + return (-(targetHeight - Constants.IntakeVision.INTAKE_Z) / slope) + - Constants.IntakeVision.INTAKE_Y; + } + + public static void setHeadingThreshold(double threshold) { + headingThreshold = threshold; + } + + public static double getHeadingThreshold() { + return headingThreshold; + } + + public static boolean visionResultInvalid(PhotonPipelineResult result) { + return result == null || result.getTargets().isEmpty(); + } +} diff --git a/vendordeps/ChoreoLib2026.json b/vendordeps/ChoreoLib2026.json new file mode 100644 index 0000000..9a106ab --- /dev/null +++ b/vendordeps/ChoreoLib2026.json @@ -0,0 +1,44 @@ +{ + "fileName": "ChoreoLib2026.json", + "name": "ChoreoLib", + "version": "2026.0.3", + "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", + "frcYear": "2026", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/sleipnirgroup-mvn-release/", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://choreo.autos/lib/ChoreoLib2026.json", + "javaDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-java", + "version": "2026.0.3" + }, + { + "groupId": "com.google.code.gson", + "artifactId": "gson", + "version": "2.11.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-cpp", + "version": "2026.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/DogLog.json b/vendordeps/DogLog.json new file mode 100644 index 0000000..7201ed0 --- /dev/null +++ b/vendordeps/DogLog.json @@ -0,0 +1,20 @@ +{ + "javaDependencies": [ + { + "groupId": "com.github.jonahsnider", + "artifactId": "doglog", + "version": "2026.5.0" + } + ], + "fileName": "DogLog.json", + "frcYear": "2026", + "jsonUrl": "https://doglog.dev/vendordep.json", + "name": "DogLog", + "jniDependencies": [], + "mavenUrls": [ + "https://jitpack.io" + ], + "cppDependencies": [], + "version": "2026.5.0", + "uuid": "65592ce1-2251-4a31-8e4b-2df20dacebe4" +} \ No newline at end of file diff --git a/vendordeps/Phoenix6-26.1.3.json b/vendordeps/Phoenix6-26.1.3.json new file mode 100644 index 0000000..d5bc4a2 --- /dev/null +++ b/vendordeps/Phoenix6-26.1.3.json @@ -0,0 +1,449 @@ +{ + "fileName": "Phoenix6-26.1.3.json", + "name": "CTRE-Phoenix (v6)", + "version": "26.1.3", + "frcYear": "2026", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-latest.json", + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2026-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "26.1.3" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "26.1.3", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "26.1.3", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "26.1.3", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "26.1.3", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "26.1.3", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "26.1.3", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "26.1.3", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "26.1.3", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "26.1.3", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "26.1.3", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "26.1.3", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "26.1.3", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "26.1.3", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "26.1.3", + "libName": "CTRE_SimProCANdle", + "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/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..6d1e817 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2026.3.4", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2026", + "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": "v2026.3.4", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2026.3.4", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2026.3.4", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2026.3.4" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2026.3.4" + } + ] +} \ No newline at end of file From ad82dba192e2a8818495bd6bd2307e8164cd837d Mon Sep 17 00:00:00 2001 From: Parneel Bhakhri Date: Mon, 10 Aug 2026 01:06:04 -0700 Subject: [PATCH 02/12] i guess bro --- .../robot/subsystems/ShooterSubsystem.java | 71 +++++++++++++++---- 1 file changed, 59 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index ee6829c..5ae061b 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -3,13 +3,20 @@ import static edu.wpi.first.units.Units.Rotations; import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MagnetSensorConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -28,6 +35,11 @@ public class ShooterSubsystem extends SubsystemBase { * make instance variables of type double for the target roller speed and target hood angle */ + private VelocityVoltage m_velocityReq = new VelocityVoltage(0.0); + private PositionVoltage m_positionReq = new PositionVoltage(0.0); + + private double targetRollerSpeed, targetHoodAngle; + public ShooterSubsystem() { // The canbus is a communication system that can connect devices like the roboRIO, pdh, and motors. CANBus canbus = Constants.Swerve.CAN_BUS; @@ -40,49 +52,84 @@ public ShooterSubsystem() { shooter = warmup3; // warmup1 and warmup2 will be set as "followers" of the "lead" warmup3, so later we can just reference the `shooter` variable // Initialize the hood motor just like the warmup motors. You can find the necessary ID in Constants. + hood = new LoggedTalonFX("ShooterHood", Constants.Shooter.Hood.HOOD_ID, canbus); // Create a variable of type Slot0Configs called rollersSlot0Configs, and initialize it with the pid and feedforward gains found in Constants + Slot0Configs rollersSlot0Configs = new Slot0Configs() + .withKP(Constants.Shooter.Rollers.KP) + .withKI(Constants.Shooter.Rollers.KI) + .withKD(Constants.Shooter.Rollers.KD) + .withKS(Constants.Shooter.Rollers.KS) + .withKV(Constants.Shooter.Rollers.KV); // Create a variable of type CurrentLimitsConfigs called rollersClConfigs, and initialize it with the stator and supply limits found in Constants + CurrentLimitsConfigs rollersClConfigs = new CurrentLimitsConfigs() + .withStatorCurrentLimit(Constants.Shooter.Rollers.STATOR_CURRENT_LIMIT) + .withSupplyCurrentLimit(Constants.Shooter.Rollers.SUPPLY_CURRENT_LIMIT); // Create a variable of type MotorOutputConifigs called rollersOutputConfigs, and initialize it to treat clockwise as positive, and neutral mode as coast - - TalonFXConfiguration rollersConfig = new TalonFXConfiguration(); + MotorOutputConfigs rollersOutputConfigs = new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast); + + TalonFXConfiguration rollersConfig = new TalonFXConfiguration() + .withSlot0(rollersSlot0Configs) + .withCurrentLimits(rollersClConfigs) + .withMotorOutput(rollersOutputConfigs); // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` to the configs you just created. warmup1.getConfigurator().apply(rollersConfig); + warmup2.getConfigurator().apply(rollersConfig); + warmup3.getConfigurator().apply(rollersConfig); // I just applied the configuration that you just created to warmup1. Apply it to warmup2 and warmup3 as well. Follower follower = new Follower(Constants.Shooter.Rollers.WARMUP_3_ID, MotorAlignmentValue.Aligned); // use the `setControl` method of warmup1 and 2 to configure them to use this follower configuration. + warmup1.setControl(follower); + warmup2.setControl(follower); hoodEncoder = new CANcoder(Constants.Shooter.Hood.ENCODER_PORT, Constants.Swerve.CAN_BUS); // Create a variable of type Slot0Configs called hoodSlot0Configs, and initialize it with the pid and feedforward gains found in Constants + Slot0Configs hoodSlot0Configs = new Slot0Configs() + .withKP(Constants.Shooter.Hood.KP) + .withKI(Constants.Shooter.Hood.KI) + .withKD(Constants.Shooter.Hood.KD) + .withKS(Constants.Shooter.Hood.KS) + .withKG(Constants.Shooter.Hood.KG) + .withKV(Constants.Shooter.Hood.KV); // Create a variable of type CurrentLimitsConfigs called hoodClConfigs, and initialize it with the stator and supply limits found in Constants + CurrentLimitsConfigs hoodClConfigs = new CurrentLimitsConfigs() + .withStatorCurrentLimit(Constants.Shooter.Hood.STATOR_CURRENT_LIMIT) + .withSupplyCurrentLimit(Constants.Shooter.Hood.SUPPLY_CURRENT_LIMIT); // Create a variable of type MotorOutputConifigs called hoodOutputConfigs, and initialize it to treat counterclockwise as positive, and neutral mode as coast + MotorOutputConfigs hoodOutputConfigs = new MotorOutputConfigs() + .withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast); - FeedbackConfigs hoodFeedbackConfigs = - new FeedbackConfigs() - .withFeedbackRemoteSensorID(hoodEncoder.getDeviceID()) - .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder) - .withSensorToMechanismRatio(Constants.Shooter.Hood.ENCODER_ROTS_PER_HOOD_ROT) - .withRotorToSensorRatio(Constants.Shooter.Hood.MOTOR_ROTS_PER_ENCODER_ROT); + FeedbackConfigs hoodFeedbackConfigs = new FeedbackConfigs() + .withFeedbackRemoteSensorID(hoodEncoder.getDeviceID()) + .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder) + .withSensorToMechanismRatio(Constants.Shooter.Hood.ENCODER_ROTS_PER_HOOD_ROT) + .withRotorToSensorRatio(Constants.Shooter.Hood.MOTOR_ROTS_PER_ENCODER_ROT); // Create a TalonFXConfiguration called hoodConfig // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the device configs above - // Apply the created configuration to the hood motor + hood.getConfigurator().apply( + new TalonFXConfiguration() + .withSlot0(hoodSlot0Configs) + .withCurrentLimits(hoodClConfigs) + .withMotorOutput(hoodOutputConfigs)); - MagnetSensorConfigs hoodCANcoderConfig = - new CANcoderConfiguration() + MagnetSensorConfigs hoodCANcoderConfig = new CANcoderConfiguration() .MagnetSensor.withAbsoluteSensorDiscontinuityPoint(Rotations.of(1)) .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) .withMagnetOffset(Rotations.of(Constants.Shooter.Hood.ENCODER_OFFSET)); // Apply this config to the `hoodEncoder`. Notice how applying a device configuration is similar between motors and other devices - + hoodEncoder.getConfigurator().apply(hoodCANcoderConfig); } } From ab2a5b8bdd9972f7729e5c1f3f4e831a8b780371 Mon Sep 17 00:00:00 2001 From: Sid Senthil Date: Fri, 14 Aug 2026 16:50:16 -0700 Subject: [PATCH 03/12] Remove all functions depending on the real ShooterSubsystem --- src/main/java/frc/robot/AutoRoutines.java | 787 ------------------ src/main/java/frc/robot/RobotContainer.java | 11 - .../ShootCommandGroups/DriveToAndShoot.java | 154 ---- .../ShootCommandGroups/ShootBasic.java | 21 - .../ShootCommandGroups/ShootBasicRetract.java | 24 - .../ShootCommandGroups/ShootPassing.java | 53 -- .../ShootCommandGroups/ShootWithAim.java | 78 -- .../SwerveCommands/SwerveJoystickCommand.java | 11 +- .../subsystems/CommandSwerveDrivetrain.java | 3 +- .../frc/robot/subsystems/HopperSubsystem.java | 6 +- src/main/java/frc/robot/util/Targeting.java | 276 ------ 11 files changed, 3 insertions(+), 1421 deletions(-) delete mode 100644 src/main/java/frc/robot/AutoRoutines.java delete mode 100644 src/main/java/frc/robot/commandGroups/ShootCommandGroups/DriveToAndShoot.java delete mode 100644 src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootBasic.java delete mode 100644 src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootBasicRetract.java delete mode 100644 src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootPassing.java delete mode 100644 src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootWithAim.java delete mode 100644 src/main/java/frc/robot/util/Targeting.java diff --git a/src/main/java/frc/robot/AutoRoutines.java b/src/main/java/frc/robot/AutoRoutines.java deleted file mode 100644 index 7e236de..0000000 --- a/src/main/java/frc/robot/AutoRoutines.java +++ /dev/null @@ -1,787 +0,0 @@ -package frc.robot; - -import choreo.auto.AutoChooser; -import choreo.auto.AutoFactory; -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; -import com.ctre.phoenix6.swerve.utility.WheelForceCalculator.Feedforwards; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.Swerve.Auto.Depot; -import frc.robot.Constants.Swerve.Auto.Intake; -import frc.robot.Constants.Swerve.Auto.MiscPaths; -import frc.robot.Constants.Swerve.Auto.Outpost; -import frc.robot.Constants.Swerve.Auto.ShootPos; -import frc.robot.commandGroups.IntakeToBumpDTP; -import frc.robot.commandGroups.ShootCommandGroups.ShootWithAim; -import frc.robot.commandGroups.ShootToBumpDTP; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.HopperSubsystem; -import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.ShooterSubsystem; -import java.util.function.BooleanSupplier; - -public class AutoRoutines { - private final AutoFactory autoFactory; - private final AutoChooser autoChooser; - private final IntakeSubsystem intakeSubsystem; - private final ShooterSubsystem lebronShooterSubsystem; - private final HopperSubsystem hopperSubsystem; - private final CommandSwerveDrivetrain swerveSubsystem; - private final BooleanSupplier redSide; - - public AutoRoutines( - IntakeSubsystem intake, - ShooterSubsystem lebron, - HopperSubsystem hopper, - CommandSwerveDrivetrain swerve, - BooleanSupplier redSide) { - this.intakeSubsystem = intake; - this.lebronShooterSubsystem = lebron; - this.hopperSubsystem = hopper; - this.swerveSubsystem = swerve; - this.redSide = redSide; - - autoFactory = swerveSubsystem.createAutoFactory(); - autoChooser = new AutoChooser(); - addCommandstoAutoChooser(); - } - - private AutoTrajectory shoot(AutoRoutine routine, ShootPos type) { - if (type == null) return null; - - AutoTrajectory traj = routine.trajectory(type + ".traj"); - return traj; - } - - // Trajectory loading and specific cmds - private AutoTrajectory intake(AutoRoutine routine, Intake type) { - if (type == null) return null; - - AutoTrajectory traj = routine.trajectory(type + ".traj"); - - if (traj != null) { - traj.atTime("IntakeDown").onTrue(intakeSubsystem.intakeUntilInterruptedCommand()); - traj.atTime("IntakeUp").onTrue(intakeSubsystem.retractIntakeCommand()); - } - return traj; - } - - private AutoTrajectory depot(AutoRoutine routine, Depot type) { - if (type == null) return null; - - AutoTrajectory traj = routine.trajectory(type + ".traj"); - - if (traj != null) { - traj.atTime("IntakeDown").onTrue(intakeSubsystem.intakeUntilInterruptedCommand()); - traj.atTime("IntakeUp").onTrue(intakeSubsystem.retractIntakeCommand()); - } - return traj; - } - - private AutoTrajectory outpost(AutoRoutine routine, Outpost type) { - if (type == null) return null; - - AutoTrajectory traj = routine.trajectory(type + ".traj"); - - if (traj != null) { - traj.atTime("IntakeDown").onTrue(intakeSubsystem.intakeUntilInterruptedCommand()); - traj.atTime("IntakeUp").onTrue(intakeSubsystem.retractIntakeCommand()); - } - return traj; - } - - private AutoTrajectory miscPaths(AutoRoutine routine, MiscPaths type) { - if (type == null) return null; - - AutoTrajectory traj = routine.trajectory(type + ".traj"); - return traj; - } - - public Command returnBasicShoot(BooleanSupplier isRedSide) { - Command shoot = - new ShootWithAim( - () -> 0.0, - () -> 0.0, - lebronShooterSubsystem, - intakeSubsystem, - hopperSubsystem, - swerveSubsystem, - isRedSide, - () -> false) - .withTimeout(3.5); - - return shoot; - } - - public Command returnBasicShootLessTime(BooleanSupplier isRedSide) { - Command shoot = - new ShootWithAim( - () -> 0.0, - () -> 0.0, - lebronShooterSubsystem, - intakeSubsystem, - hopperSubsystem, - swerveSubsystem, - isRedSide, - () -> false) - .withTimeout(2.5); - - return shoot; - } - - public Command returnBasicShootMoreTime(BooleanSupplier isRedSide) { - Command shoot = - new ShootWithAim( - () -> 0.0, - () -> 0.0, - lebronShooterSubsystem, - intakeSubsystem, - hopperSubsystem, - swerveSubsystem, - isRedSide, - () -> false) - .withTimeout(4.2); - - return shoot; - } - - public Command driveForward(double time) { - return Commands.run( - () -> swerveSubsystem.applyFieldSpeeds(new ChassisSpeeds(3, 0, 0), new Feedforwards(4)), - swerveSubsystem) - .withTimeout(time) - .andThen( - () -> - swerveSubsystem.applyFieldSpeeds(new ChassisSpeeds(0, 0, 0), new Feedforwards(4))); - } - - public Command driveBackward(double time) { - return Commands.run( - () -> - swerveSubsystem.applyFieldSpeeds(new ChassisSpeeds(-3, 0, 0), new Feedforwards(4)), - swerveSubsystem) - .withTimeout(time) - .andThen( - () -> - swerveSubsystem.applyFieldSpeeds(new ChassisSpeeds(0, 0, 0), new Feedforwards(4))); - } - - public Command driveForwardSlower(double time) { - return Commands.run( - () -> - swerveSubsystem.applyFieldSpeeds(new ChassisSpeeds(2.7, 0, 0), new Feedforwards(4)), - swerveSubsystem) - .withTimeout(time); - // .andThen( - // () -> - // swerveSubsystem.applyFieldSpeeds(new ChassisSpeeds(0, 0, 0), new Feedforwards(4))); - } - - public Command driveBackwardSlower(double time) { - return Commands.run( - () -> - swerveSubsystem.applyFieldSpeeds( - new ChassisSpeeds(-2.7, 0, 0), new Feedforwards(4)), - swerveSubsystem) - .withTimeout(time); - // .andThen( - // () -> - // swerveSubsystem.applyFieldSpeeds(new ChassisSpeeds(0, 0, 0), new Feedforwards(4))); - } - - public Command driveToBumpAfterIntake(BooleanSupplier isRedSide, BooleanSupplier isLeftSide) { - // return new IntakeToBumpDTP(swerveSubsystem, isRedSide).withTimeout(0.5); - return new IntakeToBumpDTP(swerveSubsystem, isRedSide, isLeftSide); - } - - public Command driveToBumpAfterShoot(BooleanSupplier isRedSide, BooleanSupplier isLeftSide) { - // return new IntakeToBumpDTP(swerveSubsystem, isRedSide).withTimeout(0.5); - return new ShootToBumpDTP(swerveSubsystem, isRedSide, isLeftSide); - } - - // Middle - public AutoRoutine Nike() { - AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); - - AutoTrajectory move = miscPaths(routine, Constants.Swerve.Auto.MiscPaths.Nike); - - routine.active().onTrue(Commands.sequence(move.resetOdometry(), move.cmd())); - - move.done().onTrue(returnBasicShoot(redSide)); - - return routine; - } - - // Sweeps - public AutoRoutine PedriShortRight() { - AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); - - Command forward = - redSide.getAsBoolean() - ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) - : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); - Command backward = - redSide.getAsBoolean() - ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) - : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); - Command forward2 = - redSide.getAsBoolean() - ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) - : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); - Command backward2 = - redSide.getAsBoolean() - ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) - : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); - - AutoTrajectory intake1 = intake(routine, Constants.Swerve.Auto.Intake.RightIntakeSweepShort); - AutoTrajectory intakeToShoot1 = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); - AutoTrajectory shootToBump = - miscPaths(routine, Constants.Swerve.Auto.MiscPaths.RightShootToBump); - AutoTrajectory intake2 = intake(routine, Constants.Swerve.Auto.Intake.RightSecondDip); - AutoTrajectory intakeToShoot2 = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); - - routine.active().onTrue(Commands.sequence(forward, intake1.resetOdometry(), intake1.cmd())); - - intake1 - .done() - .onTrue( - Commands.sequence( - driveToBumpAfterIntake(redSide, () -> false), - backward, - intakeToShoot1.resetOdometry(), - intakeToShoot1.cmd())); - - intakeToShoot1.done().onTrue(Commands.sequence(returnBasicShoot(redSide), shootToBump.cmd())); - - shootToBump.done().onTrue(Commands.sequence(forward2, intake2.resetOdometry(), intake2.cmd())); - - intake2 - .done() - .onTrue( - Commands.sequence( - driveToBumpAfterIntake(redSide, () -> false), - backward2, - intakeToShoot2.resetOdometry(), - intakeToShoot2.cmd())); - - intakeToShoot2.done().onTrue(returnBasicShoot(redSide)); - - return routine; - } - - public AutoRoutine PedriShortLeft() { - AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); - - Command forward = - redSide.getAsBoolean() - ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) - : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); - Command backward = - redSide.getAsBoolean() - ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) - : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); - Command forward2 = - redSide.getAsBoolean() - ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) - : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); - Command backward2 = - redSide.getAsBoolean() - ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) - : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); - - AutoTrajectory intake1 = intake(routine, Constants.Swerve.Auto.Intake.LeftIntakeSweepShort); - AutoTrajectory intakeToShoot1 = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); - AutoTrajectory shootToBump = - miscPaths(routine, Constants.Swerve.Auto.MiscPaths.LeftShootToBump); - AutoTrajectory intake2 = intake(routine, Constants.Swerve.Auto.Intake.LeftSecondDip); - AutoTrajectory intakeToShoot2 = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); - - routine.active().onTrue(Commands.sequence(forward, intake1.resetOdometry(), intake1.cmd())); - - intake1 - .done() - .onTrue( - Commands.sequence( - driveToBumpAfterIntake(redSide, () -> true), - backward, - intakeToShoot1.resetOdometry(), - intakeToShoot1.cmd())); - - intakeToShoot1.done().onTrue(Commands.sequence(returnBasicShoot(redSide), shootToBump.cmd())); - - shootToBump.done().onTrue(Commands.sequence(forward2, intake2.resetOdometry(), intake2.cmd())); - - intake2 - .done() - .onTrue( - Commands.sequence( - driveToBumpAfterIntake(redSide, () -> true), - backward2, - intakeToShoot2.resetOdometry(), - intakeToShoot2.cmd())); - - intakeToShoot2.done().onTrue(returnBasicShoot(redSide)); - - return routine; - } - - public AutoRoutine PedriShortRightWait() { - AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); - - Command forward = - redSide.getAsBoolean() - ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) - : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); - Command backward = - redSide.getAsBoolean() - ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) - : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); - Command forward2 = - redSide.getAsBoolean() - ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) - : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); - Command backward2 = - redSide.getAsBoolean() - ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) - : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); - - AutoTrajectory intake1 = intake(routine, Constants.Swerve.Auto.Intake.RightIntakeSweepShort); - AutoTrajectory intakeToShoot1 = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); - AutoTrajectory shootToBump = - miscPaths(routine, Constants.Swerve.Auto.MiscPaths.RightShootToBump); - AutoTrajectory intake2 = intake(routine, Constants.Swerve.Auto.Intake.RightSecondDip); - AutoTrajectory intakeToShoot2 = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); - - routine - .active() - .onTrue( - Commands.sequence( - Commands.waitSeconds(Constants.Swerve.Auto.WAIT_TIME_FOR_ALLIANCE), - forward, - intake1.resetOdometry(), - intake1.cmd())); - - intake1 - .done() - .onTrue( - Commands.sequence( - driveToBumpAfterIntake(redSide, () -> false), - backward, - intakeToShoot1.resetOdometry(), - intakeToShoot1.cmd())); - - intakeToShoot1.done().onTrue(Commands.sequence(returnBasicShoot(redSide), shootToBump.cmd())); - - shootToBump.done().onTrue(Commands.sequence(forward2, intake2.resetOdometry(), intake2.cmd())); - - intake2 - .done() - .onTrue( - Commands.sequence( - driveToBumpAfterIntake(redSide, () -> false), - backward2, - intakeToShoot2.resetOdometry(), - intakeToShoot2.cmd())); - - intakeToShoot2.done().onTrue(returnBasicShoot(redSide)); - - return routine; - } - - public AutoRoutine PedriShortLeftWait() { - AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); - - Command forward = - redSide.getAsBoolean() - ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) - : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); - Command backward = - redSide.getAsBoolean() - ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) - : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); - Command forward2 = - redSide.getAsBoolean() - ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) - : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); - Command backward2 = - redSide.getAsBoolean() - ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) - : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); - - AutoTrajectory intake1 = intake(routine, Constants.Swerve.Auto.Intake.LeftIntakeSweepShort); - AutoTrajectory intakeToShoot1 = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); - AutoTrajectory shootToBump = - miscPaths(routine, Constants.Swerve.Auto.MiscPaths.LeftShootToBump); - AutoTrajectory intake2 = intake(routine, Constants.Swerve.Auto.Intake.LeftSecondDip); - AutoTrajectory intakeToShoot2 = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); - - routine - .active() - .onTrue( - Commands.sequence( - Commands.waitSeconds(Constants.Swerve.Auto.WAIT_TIME_FOR_ALLIANCE), - forward, - intake1.resetOdometry(), - intake1.cmd())); - - intake1 - .done() - .onTrue( - Commands.sequence( - driveToBumpAfterIntake(redSide, () -> true), - backward, - intakeToShoot1.resetOdometry(), - intakeToShoot1.cmd())); - - intakeToShoot1.done().onTrue(Commands.sequence(returnBasicShoot(redSide), shootToBump.cmd())); - - shootToBump.done().onTrue(Commands.sequence(forward2, intake2.resetOdometry(), intake2.cmd())); - - intake2 - .done() - .onTrue( - Commands.sequence( - driveToBumpAfterIntake(redSide, () -> true), - backward2, - intakeToShoot2.resetOdometry(), - intakeToShoot2.cmd())); - - intakeToShoot2.done().onTrue(returnBasicShoot(redSide)); - - return routine; - } - - public AutoRoutine HubSweepRight() { - AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); - - Command forward = - redSide.getAsBoolean() - ? driveBackwardSlower(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS_SLOWER) - : driveForwardSlower(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS_SLOWER); - Command backward = - redSide.getAsBoolean() - ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) - : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); - Command forward2 = - redSide.getAsBoolean() - ? driveBackwardSlower(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS_SLOWER) - : driveForwardSlower(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS_SLOWER); - Command backward2 = - redSide.getAsBoolean() - ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) - : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); - - AutoTrajectory intake1 = intake(routine, Constants.Swerve.Auto.Intake.HubRight); - AutoTrajectory intakeToShoot1 = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); - AutoTrajectory shootToBump = - miscPaths(routine, Constants.Swerve.Auto.MiscPaths.RightShootToBump); - AutoTrajectory intake2 = intake(routine, Constants.Swerve.Auto.Intake.HubRight); - AutoTrajectory intakeToShoot2 = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); - - routine - .active() - .onTrue( - Commands.sequence( - Commands.waitSeconds(Constants.Swerve.Auto.WAIT_TIME_FOR_ALLIANCE), - forward, - intake1.resetOdometry(), - intake1.cmd())); - - intake1 - .done() - .onTrue( - Commands.sequence( - driveToBumpAfterIntake(redSide, () -> false), - backward, - intakeToShoot1.resetOdometry(), - intakeToShoot1.cmd())); - - intakeToShoot1.done().onTrue(Commands.sequence(returnBasicShoot(redSide), shootToBump.cmd())); - - shootToBump.done().onTrue(Commands.sequence(forward2, intake2.resetOdometry(), intake2.cmd())); - - intake2 - .done() - .onTrue( - Commands.sequence( - driveToBumpAfterIntake(redSide, () -> false), - backward2, - intakeToShoot2.resetOdometry(), - intakeToShoot2.cmd())); - - intakeToShoot2.done().onTrue(returnBasicShoot(redSide)); - - return routine; - } - - public AutoRoutine HubSweepLeft() { - AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); - - Command forward = - redSide.getAsBoolean() - ? driveBackwardSlower(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS_SLOWER) - : driveForwardSlower(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS_SLOWER); - Command backward = - redSide.getAsBoolean() - ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) - : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); - Command forward2 = - redSide.getAsBoolean() - ? driveBackwardSlower(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS_SLOWER) - : driveForwardSlower(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS_SLOWER); - Command backward2 = - redSide.getAsBoolean() - ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) - : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); - - AutoTrajectory intake1 = intake(routine, Constants.Swerve.Auto.Intake.HubLeft); - AutoTrajectory intakeToShoot1 = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); - AutoTrajectory shootToBump = - miscPaths(routine, Constants.Swerve.Auto.MiscPaths.LeftShootToBump); - AutoTrajectory intake2 = intake(routine, Constants.Swerve.Auto.Intake.HubLeft); - AutoTrajectory intakeToShoot2 = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); - - routine - .active() - .onTrue( - Commands.sequence( - Commands.waitSeconds(Constants.Swerve.Auto.WAIT_TIME_FOR_ALLIANCE), - forward, - intake1.resetOdometry(), - intake1.cmd())); - - intake1 - .done() - .onTrue( - Commands.sequence( - driveToBumpAfterIntake(redSide, () -> true), - backward, - intakeToShoot1.resetOdometry(), - intakeToShoot1.cmd())); - - intakeToShoot1.done().onTrue(Commands.sequence(returnBasicShoot(redSide), shootToBump.cmd())); - - shootToBump.done().onTrue(Commands.sequence(forward2, intake2.resetOdometry(), intake2.cmd())); - - intake2 - .done() - .onTrue( - Commands.sequence( - driveToBumpAfterIntake(redSide, () -> true), - backward2, - intakeToShoot2.resetOdometry(), - intakeToShoot2.cmd())); - - intakeToShoot2.done().onTrue(returnBasicShoot(redSide)); - - return routine; - } - - // //Outpost and Depot - public AutoRoutine DrakeOutpostShort() { - AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); - - AutoTrajectory outpostIntake = outpost(routine, Constants.Swerve.Auto.Outpost.OutpostStart); - - routine.active().onTrue(Commands.sequence(outpostIntake.resetOdometry(), outpostIntake.cmd())); - - outpostIntake.done().onTrue(returnBasicShoot(redSide)); - - return routine; - } - - public AutoRoutine DrakeDepotShort() { - AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); - - AutoTrajectory depotIntake = depot(routine, Constants.Swerve.Auto.Depot.DepotStart); - - routine.active().onTrue(Commands.sequence(depotIntake.resetOdometry(), depotIntake.cmd())); - - depotIntake.done().onTrue(returnBasicShoot(redSide)); - - return routine; - } - - public AutoRoutine DrakeOutpostLong() { - AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); - - AutoTrajectory outpostIntake = outpost(routine, Constants.Swerve.Auto.Outpost.OutpostStart); - AutoTrajectory depotIntake = depot(routine, Constants.Swerve.Auto.Depot.DepotSweep); - Command shoot = returnBasicShootLessTime(redSide); - Trigger shootIsFinished = new Trigger(() -> shoot.isFinished()); - - // routine.active().onTrue(Commands.sequence(outpostIntake.resetOdometry(), - // outpostIntake.cmd())); - // outpostIntake.done().onTrue(Commands.sequence(returnBasicShoot(redSide).asProxy(), - // depotIntake.cmd())); - // depotIntake.done().onTrue(returnBasicShoot(redSide)); - routine.active().onTrue(Commands.sequence(outpostIntake.resetOdometry(), outpostIntake.cmd())); - outpostIntake.done().onTrue(shoot); - shootIsFinished.onTrue(depotIntake.cmd()); - depotIntake.done().onTrue(returnBasicShoot(redSide)); - - return routine; - } - - public AutoRoutine DrakeDepotLong() { - AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); - - AutoTrajectory depotIntake = depot(routine, Constants.Swerve.Auto.Depot.DepotStart); - AutoTrajectory outpostIntake = outpost(routine, Constants.Swerve.Auto.Outpost.OutpostSweep); - Command shoot = returnBasicShoot(redSide); - Trigger shootIsFinished = new Trigger(() -> shoot.isFinished()); - - // routine.active().onTrue(Commands.sequence(depotIntake.resetOdometry(), depotIntake.cmd())); - // depotIntake.done().onTrue(Commands.sequence(returnBasicShoot(redSide).asProxy(), - // outpostIntake.cmd())); - // outpostIntake.done().onTrue(returnBasicShoot(redSide)); - routine.active().onTrue(Commands.sequence(depotIntake.resetOdometry(), depotIntake.cmd())); - depotIntake.done().onTrue(shoot); - shootIsFinished.onTrue(outpostIntake.cmd()); - outpostIntake.done().onTrue(returnBasicShoot(redSide)); - - return routine; - } - - public AutoRoutine PedriShortRightHub() { - AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); - - Command forward = - redSide.getAsBoolean() - ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) - : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); - Command backward = - redSide.getAsBoolean() - ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) - : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); - Command forward2 = - redSide.getAsBoolean() - ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) - : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); - Command backward2 = - redSide.getAsBoolean() - ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) - : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); - - AutoTrajectory intake1 = intake(routine, Constants.Swerve.Auto.Intake.RightIntakeSweepShort); - AutoTrajectory intakeToShoot1 = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); - AutoTrajectory shootToBump = - miscPaths(routine, Constants.Swerve.Auto.MiscPaths.RightShootToBump); - AutoTrajectory intake2 = intake(routine, Constants.Swerve.Auto.Intake.HubRight); - AutoTrajectory intakeToShoot2 = shoot(routine, Constants.Swerve.Auto.ShootPos.RightShoot); - - routine.active().onTrue(Commands.sequence(forward, intake1.resetOdometry(), intake1.cmd())); - - intake1 - .done() - .onTrue( - Commands.sequence( - driveToBumpAfterIntake(redSide, () -> false), - backward, - intakeToShoot1.resetOdometry(), - intakeToShoot1.cmd())); - - intakeToShoot1 - .done() - .onTrue(Commands.sequence(returnBasicShootMoreTime(redSide), shootToBump.cmd())); - - shootToBump.done().onTrue(Commands.sequence(forward2, intake2.resetOdometry(), intake2.cmd())); - - intake2 - .done() - .onTrue( - Commands.sequence( - driveToBumpAfterIntake(redSide, () -> false), - backward2, - intakeToShoot2.resetOdometry(), - intakeToShoot2.cmd())); - - intakeToShoot2.done().onTrue(returnBasicShootMoreTime(redSide)); - - return routine; - } - - public AutoRoutine PedriShortLeftHub() { - AutoRoutine routine = autoFactory.newRoutine("CristianoRonaldo.chor"); - - Command forward = - redSide.getAsBoolean() - ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) - : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); - Command backward = - redSide.getAsBoolean() - ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) - : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); - Command forward2 = - redSide.getAsBoolean() - ? driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS) - : driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_FORWARDS); - Command backward2 = - redSide.getAsBoolean() - ? driveForward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS) - : driveBackward(Constants.Swerve.Auto.TIME_FOR_BUMP_BACKWARDS); - - AutoTrajectory intake1 = intake(routine, Constants.Swerve.Auto.Intake.LeftIntakeSweepShort); - AutoTrajectory intakeToShoot1 = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); - AutoTrajectory shootToBump = - miscPaths(routine, Constants.Swerve.Auto.MiscPaths.LeftShootToBump); - AutoTrajectory intake2 = intake(routine, Constants.Swerve.Auto.Intake.HubLeft); - AutoTrajectory intakeToShoot2 = shoot(routine, Constants.Swerve.Auto.ShootPos.LeftShoot); - - routine.active().onTrue(Commands.sequence(forward, intake1.resetOdometry(), intake1.cmd())); - - intake1 - .done() - .onTrue( - Commands.sequence( - driveToBumpAfterIntake(redSide, () -> true), - backward, - intakeToShoot1.resetOdometry(), - intakeToShoot1.cmd())); - - intakeToShoot1 - .done() - .onTrue(Commands.sequence(returnBasicShootMoreTime(redSide), shootToBump.cmd())); - - shootToBump.done().onTrue(Commands.sequence(forward2, intake2.resetOdometry(), intake2.cmd())); - - intake2 - .done() - .onTrue( - Commands.sequence( - driveToBumpAfterIntake(redSide, () -> true), - backward2, - intakeToShoot2.resetOdometry(), - intakeToShoot2.cmd())); - - intakeToShoot2.done().onTrue(returnBasicShootMoreTime(redSide)); - - return routine; - } - - // Add paths to chooser - public void addCommandstoAutoChooser() { - autoChooser.addRoutine("Right", () -> PedriShortRight()); - autoChooser.addRoutine("Left", () -> PedriShortLeft()); - autoChooser.addRoutine("Right Wait", () -> PedriShortRightWait()); - autoChooser.addRoutine("Left Wait", () -> PedriShortLeftWait()); - // autoChooser.addRoutine("Right then Hub", () -> PedriShortRightHub()); - // autoChooser.addRoutine("Left then Hub", () -> PedriShortLeftHub()); - autoChooser.addRoutine("Hub Sweep Left Wait", () -> HubSweepLeft()); - autoChooser.addRoutine("Hub Sweep Right Wait", () -> HubSweepRight()); - - autoChooser.addRoutine("Just Outpost", () -> DrakeOutpostShort()); - autoChooser.addRoutine("Just Depot", () -> DrakeDepotShort()); - autoChooser.addRoutine("Outpost to Depot", () -> DrakeOutpostLong()); - autoChooser.addRoutine("Depot to Outpost", () -> DrakeDepotLong()); - - autoChooser.addRoutine("We are genuinely the worst robot on the field", () -> Nike()); - } - - public AutoChooser getAutoChooser() { - return autoChooser; - } -} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9760900..a5b54f5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,8 +15,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; // * KEEP FOR WIN COMMAND TESTING -import frc.robot.commandGroups.ShootCommandGroups.ShootPassing; -import frc.robot.commandGroups.ShootCommandGroups.ShootWithAim; import frc.robot.commands.SwerveCommands.SwerveJoystickCommand; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -29,7 +27,6 @@ import frc.robot.subsystems.VisionSubsystem; import frc.robot.util.CustomController; import frc.robot.util.MiscUtils; -import frc.robot.util.Targeting; import frc.robot.util.VisionUtils; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; @@ -81,13 +78,6 @@ public class RobotContainer { ? new IntakeVisionDetection(Constants.IntakeVision.IntakeVisionCamera.INTAKE_CAM) : null; - public final LEDSubsystem leds = - new LEDSubsystem( - () -> MiscUtils.areWeActive(2.0), - () -> - Targeting.distMeters(drivetrain, Targeting.getHub(redside)) < 4.47 - && inAllianceSide()); - // * KEEP FOR INTERMAP TESTING // private double hoodAngle = 18.369; // private double shooterSpeed = 58.0; @@ -205,7 +195,6 @@ public static boolean isRedAlliance() { public void visionPeriodic() { VisionUtils.visionPeriodic( visionFrontRight, visionFrontLeft, visionRearRight, visionRearLeft, drivetrain); - leds.visionStatusIndicators(visionFrontLeft, visionFrontRight, visionRearLeft, visionRearRight); } public void doTelemetry() { diff --git a/src/main/java/frc/robot/commandGroups/ShootCommandGroups/DriveToAndShoot.java b/src/main/java/frc/robot/commandGroups/ShootCommandGroups/DriveToAndShoot.java deleted file mode 100644 index 314e831..0000000 --- a/src/main/java/frc/robot/commandGroups/ShootCommandGroups/DriveToAndShoot.java +++ /dev/null @@ -1,154 +0,0 @@ -package frc.robot.commandGroups.ShootCommandGroups; - -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.swerve.utility.LinearPath; -import dev.doglog.DogLog; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import frc.robot.Constants; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.HopperSubsystem; -import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.util.Targeting; -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - -public class DriveToAndShoot extends ParallelCommandGroup { - - public DriveToAndShoot( - Supplier targetPoseSupplier, - ShooterSubsystem shooterSubsystem, - IntakeSubsystem intakeSubsystem, - HopperSubsystem hopperSubsystem, - CommandSwerveDrivetrain drivetrain, - BooleanSupplier redside) { - - DoubleSupplier dist = - () -> - drivetrain - .getPose() - .getTranslation() - .getDistance(drivetrain.getVirtualTarget(redside, () -> false)); - - addCommands( - new DriveToPoseAimed(drivetrain, targetPoseSupplier, redside), - shooterSubsystem.shootAtSpeedHoodCommand( - () -> shooterSubsystem.grabTargetShootingSpeed(dist.getAsDouble()), - () -> shooterSubsystem.grabTargetHoodAngle(dist.getAsDouble())), - Commands.waitUntil(shooterSubsystem::isAtSpeed) - .andThen( - Commands.parallel( - hopperSubsystem.runHopperUntilInterruptedCommand( - () -> Constants.Hopper.TARGET_SURFACE_SPEED_MPS, - () -> - Targeting.pointingAtTarget( - drivetrain - .travelAngleTo( - new Pose2d( - drivetrain.getVirtualTarget(redside, () -> false), - new Rotation2d())) - .getRadians() - + Math.PI, - drivetrain)), - intakeSubsystem.powerRetractRollersCommand()))); - } - - private static final class DriveToPoseAimed extends Command { - - private final CommandSwerveDrivetrain swerve; - private final Supplier targetPoseSupplier; - private final BooleanSupplier redside; - - private LinearPath path = null; - private LinearPath.State pathState = null; - private Pose2d targetPose = null; - - private final PIDController xController = - new PIDController( - Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kPX, - Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kIX, - Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kDX); - private final PIDController yController = - new PIDController( - Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kPY, - Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kIY, - Constants.Swerve.WHICH_SWERVE_ROBOT.SWERVE_DRIVE_TO_POSE_PID_VALUES.kDY); - - private double startTime; - - DriveToPoseAimed( - CommandSwerveDrivetrain swerve, - Supplier targetPoseSupplier, - BooleanSupplier redside) { - this.swerve = swerve; - this.targetPoseSupplier = targetPoseSupplier; - this.redside = redside; - - path = - new LinearPath( - new TrapezoidProfile.Constraints(1.5, 2.0), - new TrapezoidProfile.Constraints( - Constants.Swerve.WHICH_SWERVE_ROBOT - .SWERVE_DRIVE_TO_POSE_PROFILE_VALUES - .maxVelocityAngular, - Constants.Swerve.WHICH_SWERVE_ROBOT - .SWERVE_DRIVE_TO_POSE_PROFILE_VALUES - .maxAccelerationAngular)); - - addRequirements(swerve); - } - - @Override - public void initialize() { - startTime = Utils.getCurrentTimeSeconds(); - targetPose = targetPoseSupplier.get(); - - pathState = - new LinearPath.State(swerve.getCurrentState().Pose, swerve.getCurrentState().Speeds); - - DogLog.log("Subsystems/Swerve/DriveToAndShoot/InitTargetPoseX", targetPose.getX()); - DogLog.log("Subsystems/Swerve/DriveToAndShoot/InitTargetPoseY", targetPose.getY()); - } - - @Override - public void execute() { - if (pathState == null) return; - - double currTime = Utils.getCurrentTimeSeconds() - startTime; - - pathState = path.calculate(currTime, pathState, targetPose); - - double vx = - pathState.speeds.vxMetersPerSecond - + xController.calculate(swerve.getCurrentState().Pose.getX(), pathState.pose.getX()); - double vy = - pathState.speeds.vyMetersPerSecond - + yController.calculate(swerve.getCurrentState().Pose.getY(), pathState.pose.getY()); - - double omega = - swerve.calculateRequiredRotationalRateWithFF( - swerve.getVirtualTarget(redside, () -> false)); - - swerve.applyOneFieldSpeeds(new ChassisSpeeds(vx, vy, omega)); - } - - @Override - public void end(boolean interrupted) {} - - @Override - public boolean isFinished() { - return Math.abs(swerve.getCurrentState().Pose.getX() - targetPose.getX()) - <= Constants.Swerve.TARGET_POS_ERROR - && Math.abs(swerve.getCurrentState().Pose.getY() - targetPose.getY()) - <= Constants.Swerve.TARGET_POS_ERROR; - } - } -} diff --git a/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootBasic.java b/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootBasic.java deleted file mode 100644 index 618b5d3..0000000 --- a/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootBasic.java +++ /dev/null @@ -1,21 +0,0 @@ -package frc.robot.commandGroups.ShootCommandGroups; - -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import frc.robot.subsystems.HopperSubsystem; -import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.ShooterSubsystem; -import java.util.function.DoubleSupplier; - -public class ShootBasic extends ParallelCommandGroup { - public ShootBasic( - DoubleSupplier speed, - ShooterSubsystem shooterSubsystem, - IntakeSubsystem intakeSubsystem, - HopperSubsystem hopperSubsystem) { - addCommands( - shooterSubsystem.shootAtSpeedCommand(speed), - Commands.waitUntil(shooterSubsystem::isAtSpeed) - .andThen(hopperSubsystem.runHopperUntilInterruptedCommand())); - } -} diff --git a/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootBasicRetract.java b/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootBasicRetract.java deleted file mode 100644 index 94d4de0..0000000 --- a/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootBasicRetract.java +++ /dev/null @@ -1,24 +0,0 @@ -package frc.robot.commandGroups.ShootCommandGroups; - -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import frc.robot.subsystems.HopperSubsystem; -import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.ShooterSubsystem; -import java.util.function.DoubleSupplier; - -public class ShootBasicRetract extends ParallelCommandGroup { - public ShootBasicRetract( - DoubleSupplier speed, - ShooterSubsystem shooterSubsystem, - IntakeSubsystem intakeSubsystem, - HopperSubsystem hopperSubsystem) { - addCommands( - shooterSubsystem.shootAtSpeedCommand(speed), - Commands.waitUntil(shooterSubsystem::isAtSpeed) - .andThen( - Commands.parallel( - hopperSubsystem.runHopperUntilInterruptedCommand(), - intakeSubsystem.powerRetractRollersCommand()))); - } -} diff --git a/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootPassing.java b/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootPassing.java deleted file mode 100644 index 41cd6e3..0000000 --- a/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootPassing.java +++ /dev/null @@ -1,53 +0,0 @@ -package frc.robot.commandGroups.ShootCommandGroups; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import frc.robot.Constants; -import frc.robot.commands.SwerveCommands.SwerveJoystickCommandWithPointing; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.HopperSubsystem; -import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.util.Targeting; -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - -public class ShootPassing extends ParallelCommandGroup { - public ShootPassing( - DoubleSupplier translationalX, - DoubleSupplier translationalY, - ShooterSubsystem shooter, - IntakeSubsystem intake, - HopperSubsystem hopper, - CommandSwerveDrivetrain drivetrain, - BooleanSupplier redside) { - Supplier passingTranslation = - () -> - Targeting.computeVirtualTarget( - new Pose2d(drivetrain.getPassingTarget(redside), new Rotation2d()), drivetrain); - DoubleSupplier dist = - () -> drivetrain.getPose().getTranslation().getDistance(passingTranslation.get()); - - addCommands( - shooter.shootAtSpeedHoodCommand( - () -> shooter.grabTargetShootingSpeed(dist.getAsDouble()), - () -> Constants.Shooter.Hood.MAX_HOOD_POSITION), - new SwerveJoystickCommandWithPointing( - translationalX, translationalY, () -> 0.0, () -> false, passingTranslation, drivetrain), - Commands.waitUntil( - () -> - (shooter.isShooterReady() - && Targeting.pointingAtTarget( - new Pose2d(drivetrain.getPassingTarget(redside), new Rotation2d()), - drivetrain))) - .andThen( - Commands.parallel( - hopper.runHopperUntilInterruptedCommand( - () -> Constants.Hopper.TARGET_SURFACE_SPEED_MPS, () -> true), - intake.powerRetractRollersCommand()))); - } -} diff --git a/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootWithAim.java b/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootWithAim.java deleted file mode 100644 index 0a8a7d9..0000000 --- a/src/main/java/frc/robot/commandGroups/ShootCommandGroups/ShootWithAim.java +++ /dev/null @@ -1,78 +0,0 @@ -package frc.robot.commandGroups.ShootCommandGroups; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import frc.robot.Constants; -import frc.robot.commands.SwerveCommands.SwerveJoystickCommand; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.HopperSubsystem; -import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.util.Targeting; -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; - -public class ShootWithAim extends ParallelCommandGroup { - public ShootWithAim( - DoubleSupplier translationalX, - DoubleSupplier translationalY, - ShooterSubsystem shooterSubsystem, - IntakeSubsystem intakeSubsystem, - HopperSubsystem hopperSubsystem, - CommandSwerveDrivetrain drivetrain, - BooleanSupplier redside, - BooleanSupplier manualOverride) { - DoubleSupplier dist = - () -> - (drivetrain - .getPose() - .getTranslation() - .getDistance(drivetrain.getVirtualTarget(redside, () -> false))); - addCommands( - Commands.either( - Commands.parallel( // shoot without aim - shooterSubsystem.shootAtSpeedHoodCommand( - 44.2, Constants.Shooter.Hood.MAX_HOOD_POSITION), - Commands.waitUntil(shooterSubsystem::isShooterReady) - .andThen( - Commands.parallel( - hopperSubsystem.runHopperUntilInterruptedCommand(), - intakeSubsystem.powerRetractRollersCommand()))), - Commands.parallel( // shoot with aim - shooterSubsystem.shootAtSpeedHoodCommand( - () -> shooterSubsystem.grabTargetShootingSpeed(dist.getAsDouble()), - () -> shooterSubsystem.grabTargetHoodAngle(dist.getAsDouble())), - new SwerveJoystickCommand( - translationalX, - translationalY, - () -> 0.0, - () -> 1.0, - () -> true, - () -> true, - () -> false, - redside, - drivetrain, - () -> true, - () -> true), - Commands.waitUntil(shooterSubsystem::isAtSpeed) - .andThen( - Commands.parallel( - hopperSubsystem.runHopperUntilInterruptedCommand( - () -> Constants.Hopper.TARGET_SURFACE_SPEED_MPS, - () -> - Targeting.pointingAtTarget( - drivetrain - .travelAngleTo( - new Pose2d( - drivetrain.getVirtualTarget( - redside, () -> false), - new Rotation2d())) - .getRadians() - + Math.PI, - drivetrain)), - intakeSubsystem.powerRetractThenAgitateArmCommand()))), - manualOverride)); - } -} diff --git a/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java index e83da5a..09fce43 100644 --- a/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java +++ b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.util.Targeting; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; @@ -133,15 +132,7 @@ public void execute() { : robotCentricDrive.withVelocityX(x).withVelocityY(y).withRotationalRate(turn); boolean isPointed = - Targeting.pointingAtTarget( - swerveDrivetrain - .travelAngleTo( - new Pose2d( - swerveDrivetrain.getVirtualTarget(redsideIfPointing, () -> false), - new Rotation2d())) - .getRadians() - + Math.PI, - swerveDrivetrain); + true; boolean intentionallyStationary = (xSpdFunction.getAsDouble() < 0.1) && (ySpdFunction.getAsDouble() < 0.1) diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index dd5f0a8..e7c7952 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -26,7 +26,6 @@ import frc.robot.Constants; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; import frc.robot.util.MathUtils.MiscMath; -import frc.robot.util.Targeting; import java.util.Arrays; import java.util.function.BooleanSupplier; import java.util.function.Supplier; @@ -215,7 +214,7 @@ public Translation2d getVirtualTarget(BooleanSupplier redside, BooleanSupplier o return target; } if (virtualTarget == null || !virtualTargetComputedThisLoop) { - virtualTarget = Targeting.computeVirtualTarget(Targeting.getHub(redside), this); + virtualTarget = getPose().getTranslation(); virtualTargetComputedThisLoop = true; } DogLog.log("VirtualTarget", new Pose2d(virtualTarget, new Rotation2d())); diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java index 8594d71..77b64f8 100644 --- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java @@ -23,17 +23,13 @@ // spotless is clapping out public class HopperSubsystem extends SubsystemBase { - private final CommandSwerveDrivetrain drivetrain; - private final BooleanSupplier redside; private final LoggedTalonFX hopperMotor1, hopperMotor2, hopper; private double targetSurfaceSpeedMps = 0.0; private final VelocityVoltage m_velocityRequest = new VelocityVoltage(0); - public HopperSubsystem(CommandSwerveDrivetrain drivetrain, BooleanSupplier redside) { - this.drivetrain = drivetrain; - this.redside = redside; + public HopperSubsystem() { CurrentLimitsConfigs currentLimitConfigs = new CurrentLimitsConfigs() diff --git a/src/main/java/frc/robot/util/Targeting.java b/src/main/java/frc/robot/util/Targeting.java deleted file mode 100644 index 6271b46..0000000 --- a/src/main/java/frc/robot/util/Targeting.java +++ /dev/null @@ -1,276 +0,0 @@ -package frc.robot.util; - -import dev.doglog.DogLog; -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.math.util.Units; -import frc.robot.Constants; -import frc.robot.Constants.Landmarks; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.util.MathUtils.Vector3; -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; - -public class Targeting { - public static class TargetingInfo { - private double speed; - private double tof; - private Pose2d pos; - - public TargetingInfo(double speed, double tof, Pose2d pos) { - this.speed = speed; - this.tof = tof; - this.pos = pos; - } - - public double getSpeed() { - return speed; - } - - public double getToF() { - return tof; - } - - public Pose2d getPosition() { - return pos; - } - } - - public static Translation2d computeVirtualTarget( - Pose2d target, CommandSwerveDrivetrain drivetrain) { - ChassisSpeeds fieldSpeeds = drivetrain.getFieldSpeeds(); - Pose2d currPose = drivetrain.getPose(); - - // Twist2d twist = - // new Twist2d( - // fieldSpeeds.vxMetersPerSecond * 0.03, - // fieldSpeeds.vyMetersPerSecond * 0.03, - // fieldSpeeds.omegaRadiansPerSecond * 0.03); - // Pose2d lookaheadPose = currPose.exp(twist); - - double initDX = target.getX() - currPose.getX(); - double initDY = target.getY() - currPose.getY(); - double initialDistance = Math.sqrt(initDX * initDX + initDY * initDY); - - if (initialDistance < 1e-6) return target.getTranslation(); - - // double radialVelocity = - // (initDX * fieldSpeeds.vxMetersPerSecond + initDY * fieldSpeeds.vyMetersPerSecond) - // / initialDistance; - - double tof = Constants.Shooter.TIME_OF_FLIGHT_MAP.get(initialDistance); - // initialDistance - // / (initialDistance / Constants.Shooter.TIME_OF_FLIGHT_MAP.get(initialDistance) - // - radialVelocity); - - for (int i = 0; i < Constants.Shooter.TARGETING_CALCULATION_PRECISION; i++) { - double distX = initDX - fieldSpeeds.vxMetersPerSecond * tof; - double distY = initDY - fieldSpeeds.vyMetersPerSecond * tof; - double distance = Math.sqrt(distX * distX + distY * distY); - - if (distance < 1e-6) break; - - double tofTable = Constants.Shooter.TIME_OF_FLIGHT_MAP.get(distance); - double error = tof - tofTable; - - if (Math.abs(error) < 1e-3) break; - - double horizontalVel = distance / tofTable; - double errorDerivative = - 1.0 - + ((distX * fieldSpeeds.vxMetersPerSecond + distY * fieldSpeeds.vyMetersPerSecond) - / (distance * horizontalVel)); - - if (tof < 1e-3) tof = 1e-3; - - double step = error / errorDerivative; - // step = Math.max(-0.05, Math.min(0.05, step)); - step = Math.max(-0.1 * tof, Math.min(0.1 * tof, step)); - tof -= step; - } - - return new Translation2d( - target.getX() - fieldSpeeds.vxMetersPerSecond * tof, - target.getY() - fieldSpeeds.vyMetersPerSecond * tof); - } - - public static boolean pointingAtTarget( - Pose2d targetNoOffset, CommandSwerveDrivetrain drivetrain) { - double desiredRobotHullAngle = - targetAngle(targetNoOffset, drivetrain) + (2 * Math.PI) % (2 * Math.PI); - - double robotHullAngle = - drivetrain.getCurrentState().Pose.getRotation().getRadians() - + (2 * Math.PI) % (2 * Math.PI); - - double diff = Math.abs(desiredRobotHullAngle - robotHullAngle) % (2 * Math.PI); - if (diff > Math.PI) diff = 2 * Math.PI - diff; - DogLog.log("Subsystems/Shooter/Shoot/RotationalErrorRadians", diff); - boolean hullAimed = diff <= Constants.Shooter.ANGULAR_TOLERANCE_FOR_AUTO_AIM_RAD; - DogLog.log("Subsystems/Shooter/Shoot/Pointing", hullAimed); - return hullAimed; - } - - public static boolean pointingAtTarget(double angle, CommandSwerveDrivetrain drivetrain) { - double desiredRobotHullAngle = angle; - - double robotHullAngle = - drivetrain.getCurrentState().Pose.getRotation().getRadians() - + (2 * Math.PI) % (2 * Math.PI); - DogLog.log("PointingAtTarget/DesiredAngle", desiredRobotHullAngle); - DogLog.log("PointingAtTarget/RobotAngle", robotHullAngle); - double diff = Math.abs(desiredRobotHullAngle - robotHullAngle) % (2 * Math.PI); - if (diff > Math.PI) diff = 2 * Math.PI - diff; - boolean hullAimed = diff <= Constants.Shooter.ANGULAR_TOLERANCE_FOR_AUTO_AIM_RAD; - return hullAimed; - } - - // public static TargetingInfo targetingInfo( - // Pose2d target, CommandSwerveDrivetrain drivetrain, int precision) { - // Vector3 relativeVel = - // Vector3.mult( - // new Vector3( - // drivetrain.getFieldSpeeds().vxMetersPerSecond, - // drivetrain.getFieldSpeeds().vyMetersPerSecond, - // 0), - // -1); - - // Vector3 shooterPos = new Vector3(drivetrain.getCurrentState().Pose); - // Vector3 relativePos = Vector3.subtract(new Vector3(target), shooterPos); - - // double correctedSpeed = speedForDist(relativePos.magnitude()); - // double prevTof = 0; - // Vector3 correctedPos = new Vector3(target); - - // for (int i = 0; i < precision; i++) { - // double dist = Vector3.subtract(correctedPos, shooterPos).magnitude(); - // double tof = Constants.Shooter.TOF_FOR_DISTANCE_METERS_CENTER_TO_CENTER_INTERMAP.get(dist); - // correctedPos = Vector3.add(correctedPos, Vector3.mult(relativeVel, tof - prevTof)); - // correctedSpeed = speedForDist(Vector3.subtract(correctedPos, shooterPos).magnitude()); - // prevTof = tof; - // } - // DogLog.log("Subsystems/ShooterSubsystem/Shoot/shootspeed", correctedSpeed); - // DogLog.log("Subsystems/ShooterSubsystem/Shoot/Tof", prevTof); - // DogLog.log("Subsystems/ShooterSubsystem/Shoot/targetPlusLead", - // Vector3.toPose2d(correctedPos)); - - // return new TargetingInfo(correctedSpeed, prevTof, correctedPos); - // } - - public static double newtonTargetingDistance(Pose2d target, CommandSwerveDrivetrain swerve) { - // Load basic stuff in - ChassisSpeeds currSpeeds = swerve.getFieldSpeeds(); - Pose2d currState = swerve.getPose(); - - // initial guess - double initDX = target.getX() - currState.getX(); - double initDY = target.getY() - currState.getY(); - double initialDistance = Math.pow(initDX * initDX + initDY * initDY, 0.5); - - if (initialDistance < 1e-6) return 0; - - double radialVelocity = - (initDX * currSpeeds.vxMetersPerSecond + initDY * currSpeeds.vyMetersPerSecond) - / initialDistance; - - // shit we need - double tof = - initialDistance - / (initialDistance / Constants.Shooter.TIME_OF_FLIGHT_MAP.get(initialDistance) - - radialVelocity); - - double distance = initialDistance; - - for (int i = 0; i < Constants.Shooter.TARGETING_CALCULATION_PRECISION; i++) { - double distX = (initDX) - currSpeeds.vxMetersPerSecond * tof; - double distY = (initDY) - currSpeeds.vyMetersPerSecond * tof; - - distance = Math.pow(distX * distX + distY * distY, 0.5); - if (distance < 1e-6) break; - - double tofTable = Constants.Shooter.TIME_OF_FLIGHT_MAP.get(distance); - double error = tof - tofTable; - - double horizontalVel = distance / tofTable; - double errorDerivative = - 1.0 - - ((distX * currSpeeds.vxMetersPerSecond + distY * currSpeeds.vyMetersPerSecond) - / (distance * horizontalVel)); - - if (Math.abs(error) < 1e-3) break; - - if (tof < 1e-3) tof = 1e-3; - - double step = error / errorDerivative; - step = Math.max(-0.05, Math.min(0.05, step)); - tof -= step; - } - - return distance; - } - - public static TargetingInfo newtonTargetingInfo( - Pose2d target, CommandSwerveDrivetrain drivetrain) { - double distance = newtonTargetingDistance(target, drivetrain); - double timeOfFlight = Constants.Shooter.TIME_OF_FLIGHT_MAP.get(distance); - - Vector3 relativeVel = - Vector3.mult( - new Vector3( - drivetrain.getFieldSpeeds().vxMetersPerSecond, - drivetrain.getFieldSpeeds().vyMetersPerSecond, - 0), - -1); - Vector3 targetPlusOffset = - Vector3.add(new Vector3(target), Vector3.mult(relativeVel, timeOfFlight)); - - return new TargetingInfo( - Constants.Shooter.TIME_OF_FLIGHT_MAP.get(distance), - timeOfFlight, - Vector3.toPose2d(targetPlusOffset)); - } - - public static double shootingSpeed(Pose2d target, CommandSwerveDrivetrain drivetrain) { - return newtonTargetingInfo(target, drivetrain).getSpeed(); - } - - public static Pose2d positionToTarget(Pose2d target, CommandSwerveDrivetrain drivetrain) { - return newtonTargetingInfo(target, drivetrain).getPosition(); - } - - public static double speedForDist(double d) { - return Constants.Shooter.TIME_OF_FLIGHT_MAP.get(d); - } - - public static double targetAngle(Pose2d targetNoOffset, CommandSwerveDrivetrain drivetrain) { - Pose2d target = positionToTarget(targetNoOffset, drivetrain); - return Math.atan2( - Vector3.subtract(new Vector3(target), new Vector3(drivetrain.getCurrentState().Pose)).y, - Vector3.subtract(new Vector3(target), new Vector3(drivetrain.getCurrentState().Pose)).x) - + (Constants.Shooter.SHOOTS_BACKWARDS ? Math.PI : 0); - } - - public static double distMeters(CommandSwerveDrivetrain drivetrain, Pose2d target) { - return Vector3.subtract(new Vector3(drivetrain.getCurrentState().Pose), new Vector3(target)) - .magnitude(); - } - - public static DoubleSupplier amtToRumble(CommandSwerveDrivetrain drivetrain, Pose2d target) { - return () -> - Units.metersToFeet(distMeters(drivetrain, target)) > Constants.Shooter.MAX_DIST_FT - || Units.metersToFeet(distMeters(drivetrain, target)) - < Constants.Shooter.MIN_DIST_FT - ? .5d - : 0d; - } - - public static boolean pointingAtHub(BooleanSupplier redside, CommandSwerveDrivetrain drivetrain) { - Pose2d target = redside.getAsBoolean() ? Landmarks.RED_HUB : Landmarks.BLUE_HUB; - return pointingAtTarget(target, drivetrain); - } - - public static Pose2d getHub(BooleanSupplier redside) { - return (redside.getAsBoolean() ? Landmarks.RED_HUB : Landmarks.BLUE_HUB); - } -} From 8c1ba6a32924144ddfbc6390b56fe8ab0732c2a2 Mon Sep 17 00:00:00 2001 From: Sid Senthil Date: Fri, 14 Aug 2026 16:52:31 -0700 Subject: [PATCH 04/12] fix remaining reference to drivetrain --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a5b54f5..565cfc0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -44,7 +44,7 @@ public class RobotContainer { public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); public final HopperSubsystem hopperSubsystem = - Constants.hopperOnRobot ? new HopperSubsystem(drivetrain, redside) : null; + Constants.hopperOnRobot ? new HopperSubsystem() : null; public final IntakeSubsystem intakeSubsystem = Constants.intakeOnRobot ? new IntakeSubsystem() : null; From 0a98997fbbdac47f38057a2536af2a7a0add5e0b Mon Sep 17 00:00:00 2001 From: Sid Senthil Date: Sun, 16 Aug 2026 11:21:01 -0700 Subject: [PATCH 05/12] rebuild --- src/main/java/frc/robot/Constants.java | 1 - src/main/java/frc/robot/RobotContainer.java | 20 +++++++------------ .../SwerveCommands/SwerveJoystickCommand.java | 5 +---- 3 files changed, 8 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fe1260d..1653a74 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -761,7 +761,6 @@ public static final class Rollers { public static final double WHEEL_ROTS_PER_MOTOR_ROT = 1.0 / MOTOR_ROTS_PER_WHEEL_ROT; public static final double SHOOTER_WHEEL_DIAMETER = 3.0; } - } public static class OI { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 565cfc0..44ec208 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,15 +4,12 @@ package frc.robot; -import choreo.auto.AutoChooser; // * KEEP FOR WIN COMMAND TESTING // 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; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; // * KEEP FOR WIN COMMAND TESTING import frc.robot.commands.SwerveCommands.SwerveJoystickCommand; @@ -22,11 +19,8 @@ import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeVisionDetection; -import frc.robot.subsystems.LEDSubsystem; -import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.VisionSubsystem; import frc.robot.util.CustomController; -import frc.robot.util.MiscUtils; import frc.robot.util.VisionUtils; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; @@ -48,9 +42,8 @@ public class RobotContainer { public final IntakeSubsystem intakeSubsystem = Constants.intakeOnRobot ? new IntakeSubsystem() : null; - -// private final AutoRoutines autoRoutines; -// private final AutoChooser autoChooser; + // private final AutoRoutines autoRoutines; + // private final AutoChooser autoChooser; public final VisionSubsystem visionFrontRight = Constants.visionOnRobot @@ -83,7 +76,8 @@ public class RobotContainer { // private double shooterSpeed = 58.0; public RobotContainer() { - // autoRoutines = new AutoRoutines(intakeSubsystem, lebron, hopperSubsystem, drivetrain, redside); + // autoRoutines = new AutoRoutines(intakeSubsystem, lebron, hopperSubsystem, drivetrain, + // redside); // autoChooser = autoRoutines.getAutoChooser(); // SmartDashboard.putData("Auto Chooser", autoChooser); // SmartDashboard.putData("Elastic/Field2d", field); @@ -213,7 +207,7 @@ public boolean inAllianceSide() { : drivetrain.getPose().getX() < Constants.Landmarks.BLUE_HUB.getX(); } -// public Command getAutonomousCommand() { -// return autoChooser.selectedCommand(); -// } + // public Command getAutonomousCommand() { + // return autoChooser.selectedCommand(); + // } } diff --git a/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java index 09fce43..5eeedd7 100644 --- a/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java +++ b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java @@ -3,8 +3,6 @@ import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; import dev.doglog.DogLog; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -131,8 +129,7 @@ public void execute() { ? fieldCentricDrive.withVelocityX(x).withVelocityY(y).withRotationalRate(turn) : robotCentricDrive.withVelocityX(x).withVelocityY(y).withRotationalRate(turn); - boolean isPointed = - true; + boolean isPointed = true; boolean intentionallyStationary = (xSpdFunction.getAsDouble() < 0.1) && (ySpdFunction.getAsDouble() < 0.1) From 00eac92f09799db0788dc0a543ff22d047d5f91b Mon Sep 17 00:00:00 2001 From: Sid Senthil Date: Sun, 16 Aug 2026 12:37:44 -0700 Subject: [PATCH 06/12] vendordep update --- ...enix6-26.1.3.json => Phoenix6-26.3.0.json} | 62 +++++++++---------- 1 file changed, 31 insertions(+), 31 deletions(-) rename vendordeps/{Phoenix6-26.1.3.json => Phoenix6-26.3.0.json} (92%) diff --git a/vendordeps/Phoenix6-26.1.3.json b/vendordeps/Phoenix6-26.3.0.json similarity index 92% rename from vendordeps/Phoenix6-26.1.3.json rename to vendordeps/Phoenix6-26.3.0.json index d5bc4a2..0567ab3 100644 --- a/vendordeps/Phoenix6-26.1.3.json +++ b/vendordeps/Phoenix6-26.3.0.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-26.1.3.json", + "fileName": "Phoenix6-26.3.0.json", "name": "CTRE-Phoenix (v6)", - "version": "26.1.3", + "version": "26.3.0", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.3" + "version": "26.3.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.3", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.3", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.3", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.3", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.3", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.3", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.3", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.3", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.3", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.3", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.3", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.3", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.3", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.3", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.3", + "version": "26.3.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.3", + "version": "26.3.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.3", + "version": "26.3.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.3", + "version": "26.3.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.3", + "version": "26.3.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.3", + "version": "26.3.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.3", + "version": "26.3.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.3", + "version": "26.3.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.3", + "version": "26.3.0", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.3", + "version": "26.3.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.3", + "version": "26.3.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.3", + "version": "26.3.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.3", + "version": "26.3.0", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.3", + "version": "26.3.0", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, From c73d0ca5854467280afcec1541dcc358df02046b Mon Sep 17 00:00:00 2001 From: Sid Senthil Date: Sun, 16 Aug 2026 12:43:21 -0700 Subject: [PATCH 07/12] rm old constants --- src/main/java/frc/robot/Constants.java | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1653a74..c76a54a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -756,10 +756,6 @@ public static final class Rollers { public static final double STATOR_CURRENT_LIMIT = 120.0; public static final double SUPPLY_CURRENT_LIMIT = 40.0; - - public static final double MOTOR_ROTS_PER_WHEEL_ROT = 15.0 / 12.0; - public static final double WHEEL_ROTS_PER_MOTOR_ROT = 1.0 / MOTOR_ROTS_PER_WHEEL_ROT; - public static final double SHOOTER_WHEEL_DIAMETER = 3.0; } } From 50e9957ad6f0c86b6b5beab7d78d1cf3ac5d2974 Mon Sep 17 00:00:00 2001 From: Sid Senthil Date: Wed, 19 Aug 2026 17:58:25 -0700 Subject: [PATCH 08/12] typo --- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index ee6829c..4eb7f8f 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -45,7 +45,7 @@ public ShooterSubsystem() { // Create a variable of type CurrentLimitsConfigs called rollersClConfigs, and initialize it with the stator and supply limits found in Constants - // Create a variable of type MotorOutputConifigs called rollersOutputConfigs, and initialize it to treat clockwise as positive, and neutral mode as coast + // Create a variable of type MotorOutputConfigs called rollersOutputConfigs, and initialize it to treat clockwise as positive, and neutral mode as coast TalonFXConfiguration rollersConfig = new TalonFXConfiguration(); // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` to the configs you just created. @@ -62,7 +62,7 @@ public ShooterSubsystem() { // Create a variable of type CurrentLimitsConfigs called hoodClConfigs, and initialize it with the stator and supply limits found in Constants - // Create a variable of type MotorOutputConifigs called hoodOutputConfigs, and initialize it to treat counterclockwise as positive, and neutral mode as coast + // Create a variable of type MotorOutputConfigs called hoodOutputConfigs, and initialize it to treat counterclockwise as positive, and neutral mode as coast FeedbackConfigs hoodFeedbackConfigs = new FeedbackConfigs() From 893125ae6477867393064c60c29445d9efd512e3 Mon Sep 17 00:00:00 2001 From: Parneel Bhakhri Date: Sun, 23 Aug 2026 15:03:09 -0700 Subject: [PATCH 09/12] hw 6 --- src/main/java/frc/robot/Constants.java | 17 +- src/main/java/frc/robot/RobotContainer.java | 53 ++--- .../robot/commandGroups/ShootBasicHood.java | 13 ++ .../robot/subsystems/ShooterSubsystem.java | 196 ++++++++++++------ 4 files changed, 171 insertions(+), 108 deletions(-) create mode 100644 src/main/java/frc/robot/commandGroups/ShootBasicHood.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c76a54a..9c63e93 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -705,12 +705,12 @@ public static final class Shooter { public static final class Hood { public static final int HOOD_ID = 12; - public static final double KP = 0.0; + public static final double KP = 2000; public static final double KI = 0.0; - public static final double KD = 0.0; - public static final double KV = 0.0; + public static final double KD = 80.0; public static final double KS = 0.0; - public static final double KG = 0.0; + public static final double KV = 0.124; + public static final double KG = 0.4; public static final double STATOR_CURRENT_LIMIT = 40.0; public static final double SUPPLY_CURRENT_LIMIT = 40.0; @@ -739,6 +739,9 @@ public static final class Hood { public static final double ZERO_MAX_SUPPLY = 0; public static final double ZERO_MAX_STATOR = 0; public static final int MAX_TIMES_EXCEEDED = 10; + + public static final double MIN_HOOD_ANGLE = 3.8; + public static final double MAX_HOOD_ANGLE = 18.369; } public static final class Rollers { @@ -748,14 +751,16 @@ public static final class Rollers { public static final int WARMUP_2_ID = 9; public static final int WARMUP_3_ID = 11; - public static final double KP = 0.0; + public static final double KP = 0.85; public static final double KI = 0.0; public static final double KD = 0.0; - public static final double KV = 0.0; + public static final double KV = 0.124; public static final double KS = 0.0; public static final double STATOR_CURRENT_LIMIT = 120.0; public static final double SUPPLY_CURRENT_LIMIT = 40.0; + + public static final double MOTOR_ROTS_PER_WHEEL_ROT = 15.0 / 12.0; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 44ec208..f8337af 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.commandGroups.ShootBasicHood; // * KEEP FOR WIN COMMAND TESTING import frc.robot.commands.SwerveCommands.SwerveJoystickCommand; import frc.robot.generated.TunerConstants; @@ -19,6 +20,7 @@ import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeVisionDetection; +import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.VisionSubsystem; import frc.robot.util.CustomController; import frc.robot.util.VisionUtils; @@ -41,6 +43,7 @@ public class RobotContainer { Constants.hopperOnRobot ? new HopperSubsystem() : null; public final IntakeSubsystem intakeSubsystem = Constants.intakeOnRobot ? new IntakeSubsystem() : null; + public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); // private final AutoRoutines autoRoutines; // private final AutoChooser autoChooser; @@ -135,49 +138,23 @@ private void configureBindings() { .alongWith( hopperSubsystem.runHopperUntilInterruptedCommand( -Constants.Hopper.TARGET_SURFACE_SPEED_MPS))); - - // * KEEP FOR WIN COMMAND - // joystick - // .a() - // .whileTrue( - // new DriveToAndShoot( - // () -> (new Pose2d(new Translation2d(3.0, 5.0), new Rotation2d())), - // lebron, - // intakeSubsystem, - // hopperSubsystem, - // drivetrain, - // redside)); - - // joystick - // .a() - // .whileTrue( - // new DriveToPose( - // drivetrain, - // () -> - // new Pose2d( - // new Translation2d(2.462480068206787, 2.26101016998291), new - // Rotation2d()))); - - // * KEEP FOR INTERMAP TESTING - // joystick - // .rightTrigger() - // .whileTrue( - // lebron - // .shootAtSpeedHoodCommand(() -> shooterSpeed, () -> hoodAngle) - // .alongWith(Commands.waitUntil(lebron::isAtSpeed).andThen - // - // (hopperSubsystem.runHopperUntilInterruptedCommand().alongWith(Commands.waitSeconds(0.4).andThen(intakeSubsystem.powerRetractRollersCommand()))))); - secondController.intakeOverride().whileTrue(intakeSubsystem.retractIntakeCommand()); // Hopper hopperSubsystem.setDefaultCommand(hopperSubsystem.run(hopperSubsystem::stop)); - // * KEEP FOR INTERMAP TESTING - // joystick.x().onTrue(new InstantCommand(() -> hoodAngle+=0.2)); - // joystick.y().onTrue(new InstantCommand(() -> hoodAngle-=0.2)); - // joystick.a().onTrue(new InstantCommand(() -> shooterSpeed+=0.5)); - // joystick.b().onTrue(new InstantCommand(() -> shooterSpeed-=0.5)); + // Shooter + shooterSubsystem.setDefaultCommand(shooterSubsystem.runOnce(shooterSubsystem::stopShooter)); + joystick + .rightBumper() + .whileTrue( + new ShootBasicHood( + () -> Constants.Shooter.Hood.MIN_HOOD_POSITION, () -> 44.2, shooterSubsystem)); + joystick + .rightTrigger() + .whileTrue( + new ShootBasicHood( + () -> Constants.Shooter.Hood.MAX_HOOD_POSITION, () -> 58.2, shooterSubsystem)); } public static boolean isRedAlliance() { diff --git a/src/main/java/frc/robot/commandGroups/ShootBasicHood.java b/src/main/java/frc/robot/commandGroups/ShootBasicHood.java new file mode 100644 index 0000000..80f7eeb --- /dev/null +++ b/src/main/java/frc/robot/commandGroups/ShootBasicHood.java @@ -0,0 +1,13 @@ +package frc.robot.commandGroups; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.subsystems.ShooterSubsystem; +import java.util.function.DoubleSupplier; + +public class ShootBasicHood extends ParallelCommandGroup { + public ShootBasicHood( + DoubleSupplier hoodAngle, DoubleSupplier shooterVelocity, ShooterSubsystem shooterSubsystem) { + addCommands( + shooterSubsystem.shootWithHood(hoodAngle.getAsDouble(), shooterVelocity.getAsDouble())); + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 5ae061b..ff8a189 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.Rotations; + import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; @@ -18,80 +19,100 @@ import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; - +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.util.LoggedTalonFX; - public class ShooterSubsystem extends SubsystemBase { - - private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood; - private final CANcoder hoodEncoder; + private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood; + private final CANcoder hoodEncoder; + + /* + * make instance variables for necessary closed loop control requests: VelocityVoltage, PositionVoltage. Initialize them with value 0.0. + * make instance variables of type double for the target roller speed and target hood angle + */ + + private VelocityVoltage m_velocityReq = new VelocityVoltage(0.0); + private PositionVoltage m_positionReq = new PositionVoltage(0.0); - /* - * make instance variables for necessary closed loop control requests: VelocityVoltage, PositionVoltage. Initialize them with value 0.0. - * make instance variables of type double for the target roller speed and target hood angle - */ + private double targetShooterSpeed, targetHoodAngle; - private VelocityVoltage m_velocityReq = new VelocityVoltage(0.0); - private PositionVoltage m_positionReq = new PositionVoltage(0.0); + public ShooterSubsystem() { + // The canbus is a communication system that can connect devices like the roboRIO, pdh, and + // motors. + CANBus canbus = Constants.Swerve.CAN_BUS; - private double targetRollerSpeed, targetHoodAngle; + targetShooterSpeed = 0; + targetHoodAngle = 0; - public ShooterSubsystem() { - // The canbus is a communication system that can connect devices like the roboRIO, pdh, and motors. - CANBus canbus = Constants.Swerve.CAN_BUS; - - // LoggedTalonFX is our version of the existing TalonFX class, which automatically logs some motor information - warmup1 = new LoggedTalonFX("ShooterWarmup1", Constants.Shooter.Rollers.WARMUP_1_ID, canbus); - warmup2 = new LoggedTalonFX("ShooterWarmup2", Constants.Shooter.Rollers.WARMUP_2_ID, canbus); - warmup3 = new LoggedTalonFX("ShooterWarmup3", Constants.Shooter.Rollers.WARMUP_3_ID, canbus); + // LoggedTalonFX is our version of the existing TalonFX class, which automatically logs some + // motor information + warmup1 = new LoggedTalonFX("ShooterWarmup1", Constants.Shooter.Rollers.WARMUP_1_ID, canbus); + warmup2 = new LoggedTalonFX("ShooterWarmup2", Constants.Shooter.Rollers.WARMUP_2_ID, canbus); + warmup3 = new LoggedTalonFX("ShooterWarmup3", Constants.Shooter.Rollers.WARMUP_3_ID, canbus); - shooter = warmup3; // warmup1 and warmup2 will be set as "followers" of the "lead" warmup3, so later we can just reference the `shooter` variable + shooter = + warmup3; // warmup1 and warmup2 will be set as "followers" of the "lead" warmup3, so later + // we can just reference the `shooter` variable - // Initialize the hood motor just like the warmup motors. You can find the necessary ID in Constants. - hood = new LoggedTalonFX("ShooterHood", Constants.Shooter.Hood.HOOD_ID, canbus); + // Initialize the hood motor just like the warmup motors. You can find the necessary ID in + // Constants. + hood = new LoggedTalonFX("ShooterHood", Constants.Shooter.Hood.HOOD_ID, canbus); - // Create a variable of type Slot0Configs called rollersSlot0Configs, and initialize it with the pid and feedforward gains found in Constants - Slot0Configs rollersSlot0Configs = new Slot0Configs() + // Create a variable of type Slot0Configs called rollersSlot0Configs, and initialize it with the + // pid and feedforward gains found in Constants + Slot0Configs rollersSlot0Configs = + new Slot0Configs() .withKP(Constants.Shooter.Rollers.KP) .withKI(Constants.Shooter.Rollers.KI) .withKD(Constants.Shooter.Rollers.KD) .withKS(Constants.Shooter.Rollers.KS) .withKV(Constants.Shooter.Rollers.KV); - // Create a variable of type CurrentLimitsConfigs called rollersClConfigs, and initialize it with the stator and supply limits found in Constants - CurrentLimitsConfigs rollersClConfigs = new CurrentLimitsConfigs() + // Create a variable of type CurrentLimitsConfigs called rollersClConfigs, and initialize it + // with the stator and supply limits found in Constants + CurrentLimitsConfigs rollersClConfigs = + new CurrentLimitsConfigs() .withStatorCurrentLimit(Constants.Shooter.Rollers.STATOR_CURRENT_LIMIT) .withSupplyCurrentLimit(Constants.Shooter.Rollers.SUPPLY_CURRENT_LIMIT); - // Create a variable of type MotorOutputConifigs called rollersOutputConfigs, and initialize it to treat clockwise as positive, and neutral mode as coast - MotorOutputConfigs rollersOutputConfigs = new MotorOutputConfigs() + // Create a variable of type MotorOutputConifigs called rollersOutputConfigs, and initialize it + // to treat clockwise as positive, and neutral mode as coast + MotorOutputConfigs rollersOutputConfigs = + new MotorOutputConfigs() .withInverted(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Coast); - TalonFXConfiguration rollersConfig = new TalonFXConfiguration() + TalonFXConfiguration rollersConfig = + new TalonFXConfiguration() .withSlot0(rollersSlot0Configs) .withCurrentLimits(rollersClConfigs) .withMotorOutput(rollersOutputConfigs); - // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` to the configs you just created. - - warmup1.getConfigurator().apply(rollersConfig); - warmup2.getConfigurator().apply(rollersConfig); - warmup3.getConfigurator().apply(rollersConfig); - // I just applied the configuration that you just created to warmup1. Apply it to warmup2 and warmup3 as well. - - Follower follower = new Follower(Constants.Shooter.Rollers.WARMUP_3_ID, MotorAlignmentValue.Aligned); - // use the `setControl` method of warmup1 and 2 to configure them to use this follower configuration. - warmup1.setControl(follower); - warmup2.setControl(follower); - - hoodEncoder = new CANcoder(Constants.Shooter.Hood.ENCODER_PORT, Constants.Swerve.CAN_BUS); - - // Create a variable of type Slot0Configs called hoodSlot0Configs, and initialize it with the pid and feedforward gains found in Constants - Slot0Configs hoodSlot0Configs = new Slot0Configs() + // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` to the configs + // you just created. + + warmup1.getConfigurator().apply(rollersConfig); + warmup2.getConfigurator().apply(rollersConfig); + warmup3.getConfigurator().apply(rollersConfig); + // I just applied the configuration that you just created to warmup1. Apply it to warmup2 and + // warmup3 as well. + + Follower follower = + new Follower(Constants.Shooter.Rollers.WARMUP_3_ID, MotorAlignmentValue.Aligned); + // use the `setControl` method of warmup1 and 2 to configure them to use this follower + // configuration. + warmup1.setControl(follower); + warmup2.setControl(follower); + + hoodEncoder = new CANcoder(Constants.Shooter.Hood.ENCODER_PORT, Constants.Swerve.CAN_BUS); + + // Create a variable of type Slot0Configs called hoodSlot0Configs, and initialize it with the + // pid and feedforward gains found in Constants + Slot0Configs hoodSlot0Configs = + new Slot0Configs() .withKP(Constants.Shooter.Hood.KP) .withKI(Constants.Shooter.Hood.KI) .withKD(Constants.Shooter.Hood.KD) @@ -99,37 +120,84 @@ public ShooterSubsystem() { .withKG(Constants.Shooter.Hood.KG) .withKV(Constants.Shooter.Hood.KV); - // Create a variable of type CurrentLimitsConfigs called hoodClConfigs, and initialize it with the stator and supply limits found in Constants - CurrentLimitsConfigs hoodClConfigs = new CurrentLimitsConfigs() + // Create a variable of type CurrentLimitsConfigs called hoodClConfigs, and initialize it with + // the stator and supply limits found in Constants + CurrentLimitsConfigs hoodClConfigs = + new CurrentLimitsConfigs() .withStatorCurrentLimit(Constants.Shooter.Hood.STATOR_CURRENT_LIMIT) .withSupplyCurrentLimit(Constants.Shooter.Hood.SUPPLY_CURRENT_LIMIT); - // Create a variable of type MotorOutputConifigs called hoodOutputConfigs, and initialize it to treat counterclockwise as positive, and neutral mode as coast - MotorOutputConfigs hoodOutputConfigs = new MotorOutputConfigs() + // Create a variable of type MotorOutputConifigs called hoodOutputConfigs, and initialize it to + // treat counterclockwise as positive, and neutral mode as coast + MotorOutputConfigs hoodOutputConfigs = + new MotorOutputConfigs() .withInverted(InvertedValue.CounterClockwise_Positive) .withNeutralMode(NeutralModeValue.Coast); - FeedbackConfigs hoodFeedbackConfigs = new FeedbackConfigs() - .withFeedbackRemoteSensorID(hoodEncoder.getDeviceID()) - .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder) - .withSensorToMechanismRatio(Constants.Shooter.Hood.ENCODER_ROTS_PER_HOOD_ROT) - .withRotorToSensorRatio(Constants.Shooter.Hood.MOTOR_ROTS_PER_ENCODER_ROT); - - // Create a TalonFXConfiguration called hoodConfig - // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the device configs above - // Apply the created configuration to the hood motor - hood.getConfigurator().apply( + FeedbackConfigs hoodFeedbackConfigs = + new FeedbackConfigs() + .withFeedbackRemoteSensorID(hoodEncoder.getDeviceID()) + .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder) + .withSensorToMechanismRatio(Constants.Shooter.Hood.ENCODER_ROTS_PER_HOOD_ROT) + .withRotorToSensorRatio(Constants.Shooter.Hood.MOTOR_ROTS_PER_ENCODER_ROT); + + // Create a TalonFXConfiguration called hoodConfig + // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the device configs + // above + // Apply the created configuration to the hood motor + hood.getConfigurator() + .apply( new TalonFXConfiguration() .withSlot0(hoodSlot0Configs) .withCurrentLimits(hoodClConfigs) .withMotorOutput(hoodOutputConfigs)); - MagnetSensorConfigs hoodCANcoderConfig = new CANcoderConfiguration() + MagnetSensorConfigs hoodCANcoderConfig = + new CANcoderConfiguration() .MagnetSensor.withAbsoluteSensorDiscontinuityPoint(Rotations.of(1)) .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) .withMagnetOffset(Rotations.of(Constants.Shooter.Hood.ENCODER_OFFSET)); - // Apply this config to the `hoodEncoder`. Notice how applying a device configuration is similar between motors and other devices - hoodEncoder.getConfigurator().apply(hoodCANcoderConfig); - } + // Apply this config to the `hoodEncoder`. Notice how applying a device configuration is similar + // between motors and other devices + hoodEncoder.getConfigurator().apply(hoodCANcoderConfig); + } + + public void setHoodAngle(double degrees) { + targetHoodAngle = degrees; + hood.setControl( + m_positionReq.withPosition( + MathUtil.clamp( + targetHoodAngle, + Constants.Shooter.Hood.MIN_HOOD_ANGLE, + Constants.Shooter.Hood.MAX_HOOD_ANGLE) + / 360.0)); + } + + public void setShooterSpeed(double velocityRps) { + targetShooterSpeed = velocityRps; + shooter.setControl( + m_velocityReq.withVelocity( + targetShooterSpeed * Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT)); + } + + public void stopShooter() { + targetShooterSpeed = 0.0; + shooter.stopMotor(); + } + + public boolean isShooterAtSpeed() { + double currentVelocity = + shooter.getCachedVelocityRps() / Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT; + return Math.abs(currentVelocity - targetShooterSpeed) <= 1.0; + } + + public Command shootWithHood(double hoodAngleDegrees, double shooterVelocityRps) { + return runEnd( + () -> { + setShooterSpeed(shooterVelocityRps); + setHoodAngle(hoodAngleDegrees); + }, + this::stopShooter); + } } From dfc2146f1e755622506ad95413f450c21b51f2cf Mon Sep 17 00:00:00 2001 From: Parneel Bhakhri Date: Mon, 24 Aug 2026 18:49:55 -0700 Subject: [PATCH 10/12] add hopper --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/commandGroups/ShootBasicHood.java | 7 +++++-- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f8337af..642f989 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -149,12 +149,12 @@ private void configureBindings() { .rightBumper() .whileTrue( new ShootBasicHood( - () -> Constants.Shooter.Hood.MIN_HOOD_POSITION, () -> 44.2, shooterSubsystem)); + () -> Constants.Shooter.Hood.MIN_HOOD_POSITION, () -> 44.2, shooterSubsystem, hopperSubsystem)); joystick .rightTrigger() .whileTrue( new ShootBasicHood( - () -> Constants.Shooter.Hood.MAX_HOOD_POSITION, () -> 58.2, shooterSubsystem)); + () -> Constants.Shooter.Hood.MAX_HOOD_POSITION, () -> 58.2, shooterSubsystem, hopperSubsystem)); } public static boolean isRedAlliance() { diff --git a/src/main/java/frc/robot/commandGroups/ShootBasicHood.java b/src/main/java/frc/robot/commandGroups/ShootBasicHood.java index 80f7eeb..bd84bb1 100644 --- a/src/main/java/frc/robot/commandGroups/ShootBasicHood.java +++ b/src/main/java/frc/robot/commandGroups/ShootBasicHood.java @@ -1,13 +1,16 @@ package frc.robot.commandGroups; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.ShooterSubsystem; import java.util.function.DoubleSupplier; public class ShootBasicHood extends ParallelCommandGroup { public ShootBasicHood( - DoubleSupplier hoodAngle, DoubleSupplier shooterVelocity, ShooterSubsystem shooterSubsystem) { + DoubleSupplier hoodAngle, DoubleSupplier shooterVelocity, ShooterSubsystem shooterSubsystem, HopperSubsystem hopperSubsystem) { addCommands( - shooterSubsystem.shootWithHood(hoodAngle.getAsDouble(), shooterVelocity.getAsDouble())); + shooterSubsystem.shootWithHood(hoodAngle.getAsDouble(), shooterVelocity.getAsDouble()), + Commands.waitUntil(shooterSubsystem::isShooterAtSpeed).andThen(hopperSubsystem.runHopperUntilInterruptedCommand())); } } From ef132cbc216dae648373c385de8057000e57c6ff Mon Sep 17 00:00:00 2001 From: Parneel Bhakhri Date: Mon, 24 Aug 2026 18:50:24 -0700 Subject: [PATCH 11/12] spot --- src/main/java/frc/robot/RobotContainer.java | 10 ++++++++-- .../java/frc/robot/commandGroups/ShootBasicHood.java | 8 ++++++-- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 642f989..7b55dfd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -149,12 +149,18 @@ private void configureBindings() { .rightBumper() .whileTrue( new ShootBasicHood( - () -> Constants.Shooter.Hood.MIN_HOOD_POSITION, () -> 44.2, shooterSubsystem, hopperSubsystem)); + () -> Constants.Shooter.Hood.MIN_HOOD_POSITION, + () -> 44.2, + shooterSubsystem, + hopperSubsystem)); joystick .rightTrigger() .whileTrue( new ShootBasicHood( - () -> Constants.Shooter.Hood.MAX_HOOD_POSITION, () -> 58.2, shooterSubsystem, hopperSubsystem)); + () -> Constants.Shooter.Hood.MAX_HOOD_POSITION, + () -> 58.2, + shooterSubsystem, + hopperSubsystem)); } public static boolean isRedAlliance() { diff --git a/src/main/java/frc/robot/commandGroups/ShootBasicHood.java b/src/main/java/frc/robot/commandGroups/ShootBasicHood.java index bd84bb1..cf4df2c 100644 --- a/src/main/java/frc/robot/commandGroups/ShootBasicHood.java +++ b/src/main/java/frc/robot/commandGroups/ShootBasicHood.java @@ -8,9 +8,13 @@ public class ShootBasicHood extends ParallelCommandGroup { public ShootBasicHood( - DoubleSupplier hoodAngle, DoubleSupplier shooterVelocity, ShooterSubsystem shooterSubsystem, HopperSubsystem hopperSubsystem) { + DoubleSupplier hoodAngle, + DoubleSupplier shooterVelocity, + ShooterSubsystem shooterSubsystem, + HopperSubsystem hopperSubsystem) { addCommands( shooterSubsystem.shootWithHood(hoodAngle.getAsDouble(), shooterVelocity.getAsDouble()), - Commands.waitUntil(shooterSubsystem::isShooterAtSpeed).andThen(hopperSubsystem.runHopperUntilInterruptedCommand())); + Commands.waitUntil(shooterSubsystem::isShooterAtSpeed) + .andThen(hopperSubsystem.runHopperUntilInterruptedCommand())); } } From 2733e2157b44bb00dff8d950ff27f84cabb4b8a1 Mon Sep 17 00:00:00 2001 From: Parneel Bhakhri Date: Mon, 24 Aug 2026 19:06:48 -0700 Subject: [PATCH 12/12] feedback --- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index ff8a189..879d0d9 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -150,7 +150,8 @@ public ShooterSubsystem() { new TalonFXConfiguration() .withSlot0(hoodSlot0Configs) .withCurrentLimits(hoodClConfigs) - .withMotorOutput(hoodOutputConfigs)); + .withMotorOutput(hoodOutputConfigs) + .withFeedback(hoodFeedbackConfigs)); MagnetSensorConfigs hoodCANcoderConfig = new CANcoderConfiguration()