From: Kyle-Eldridge <113394349+Kyle-Eldridge@users.noreply.github.com> Date: Tue, 22 Apr 2025 21:59:34 +0000 (-0700) Subject: Copy 2025 code and change CODEOWNERS X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=1a0fb4b2281136e1501ed2f76a68d456f491ac72;p=FRC2026.git Copy 2025 code and change CODEOWNERS --- diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json new file mode 100644 index 0000000..a3dc0c4 --- /dev/null +++ b/.DataLogTool/datalogtool.json @@ -0,0 +1,6 @@ +{ + "download": { + "localDir": "D:\\loggies", + "serverTeam": "972" + } +} diff --git a/.Glass/glass.json b/.Glass/glass.json new file mode 100644 index 0000000..bd13c22 --- /dev/null +++ b/.Glass/glass.json @@ -0,0 +1,8 @@ +{ + "NetworkTables Log": { + "visible": true + }, + "NetworkTables Settings": { + "mode": "Server" + } +} diff --git a/.gitattributes b/.gitattributes index d8c3d83..286213b 100644 --- a/.gitattributes +++ b/.gitattributes @@ -1,4 +1,4 @@ *.gradle text eol=lf *.java text eol=lf *.md text eol=lf -*.xml text eol=lf +*.xml text eol=lf \ No newline at end of file diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS new file mode 100644 index 0000000..affeb5c --- /dev/null +++ b/.github/CODEOWNERS @@ -0,0 +1,2 @@ +# Make the pull reviewers code owners for every file, they will get requested for review when a PR is made +* @Saara21 @maxwtan @Arnav814 \ No newline at end of file diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index 92b3c80..c89f33b 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -1,21 +1,21 @@ ## Overview -This PR will.. +This PR will... -## Design Doc - +## Documentation + ## Tests Ran - + ## Additional Notes - + \ No newline at end of file diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index faa3f6f..849878f 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -18,12 +18,16 @@ jobs: runs-on: ubuntu-latest # This grabs the WPILib docker container - container: wpilib/roborio-cross-ubuntu:2022-18.04 + container: wpilib/roborio-cross-ubuntu:2023-22.04 # Steps represent a sequence of tasks that will be executed as part of the job steps: # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 + + # Declares the repository safe and not under dubious ownership. + - name: Add repository to git safe directories + run: git config --global --add safe.directory $GITHUB_WORKSPACE # Grant execute permission for gradlew - name: Grant execute permission for gradlew diff --git a/.gitignore b/.gitignore index 8965997..10c4de4 100644 --- a/.gitignore +++ b/.gitignore @@ -160,6 +160,22 @@ bin/ # Simulation GUI and other tools window save file *-window.json +*simgui.json +simgui-ds.json +networktables.json +networktables.json.bck + +# Simulation puts log files in root of repo +*.wpilog + +# Don't commit ctre_sim directory +ctre_sim/* # File that has git and version data -src/main/java/frc/robot/util/BuildData.java \ No newline at end of file +src/main/java/frc/robot/util/BuildData.java + +# Log files from simulation +logs/ + +# IntelliJ IDEA files +.idea/ diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 0000000..0c64c43 --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,15 @@ +{ + "robotWidth": 0.76, + "robotLength": 0.91, + "holonomicMode": true, + "pathFolders": [ + "Final Stuff", + "Stuff" + ], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "maxModuleSpeed": 4.5 +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index 4ed293b..612cdd0 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -25,5 +25,36 @@ } }, ], - "java.test.defaultConfig": "WPIlibUnitTests" + "java.test.defaultConfig": "WPIlibUnitTests", + "java.import.gradle.annotationProcessing.enabled": false, + "java.completion.favoriteStaticMembers": [ + "org.junit.Assert.*", + "org.junit.Assume.*", + "org.junit.jupiter.api.Assertions.*", + "org.junit.jupiter.api.Assumptions.*", + "org.junit.jupiter.api.DynamicContainer.*", + "org.junit.jupiter.api.DynamicTest.*", + "org.mockito.Mockito.*", + "org.mockito.ArgumentMatchers.*", + "org.mockito.Answers.*", + "edu.wpi.first.units.Units.*" + ], + "java.completion.filteredTypes": [ + "java.awt.*", + "com.sun.*", + "sun.*", + "jdk.*", + "org.graalvm.*", + "io.micrometer.shaded.*", + "java.beans.*", + "java.util.Base64.*", + "java.util.Timer", + "java.sql.*", + "javax.swing.*", + "javax.management.*", + "javax.smartcardio.*", + "edu.wpi.first.math.proto.*", + "edu.wpi.first.math.**.proto.*", + "edu.wpi.first.math.**.struct.*", + ] } diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 5e8c810..e56da02 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2022", + "projectYear": "2025", "teamNumber": 972 } \ No newline at end of file diff --git a/README.md b/README.md index b0cddad..f1db6d7 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,5 @@ -# Code-Structure-2023 +# FRC2026 -[![CI](https://github.com/iron-claw-972/Code-Structure-2023/actions/workflows/main.yml/badge.svg)](https://github.com/iron-claw-972/Code-Structure-2023/actions/workflows/main.yml) +[![CI](https://github.com/iron-claw-972/FRC2025/actions/workflows/main.yml/badge.svg)](https://github.com/iron-claw-972/FRC2025/actions/workflows/main.yml) - -Code Structure for our future 2023 repository. +Code for the 2025-2026 FRC Season. diff --git a/WPILib-License.md b/WPILib-License.md index 3d5a824..645e542 100644 --- a/WPILib-License.md +++ b/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2021 FIRST and other WPILib contributors +Copyright (c) 2009-2024 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/build.gradle b/build.gradle index 281129d..9e033e6 100644 --- a/build.gradle +++ b/build.gradle @@ -1,13 +1,14 @@ -import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO - plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2022.4.1" - id "com.peterabeles.gversion" version "1.10" + id "edu.wpi.first.GradleRIO" version "2025.3.1" + id "com.peterabeles.gversion" version "1.10.3" + } -sourceCompatibility = JavaVersion.VERSION_11 -targetCompatibility = JavaVersion.VERSION_11 +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} def ROBOT_MAIN_CLASS = "frc.robot.Main" @@ -28,12 +29,22 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + jvmArgs.add("-Dcom.sun.management.jmxremote=true") + jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") + jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") + jvmArgs.add("-XX:+HeapDumpOnOutOfMemoryError") + jvmArgs.add("-XX:HeapDumpPath=/home/lvuser/frc-usercode.hprof") + jvmArgs.add("-Djava.rmi.server.hostname=10.9.72.2") // Replace TE.AM with team number } // Static files artifact frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' + deleteOldFiles = false // Change to true to delete files on roboRIO that no + // longer exist in deploy directory of this project } } } @@ -48,9 +59,47 @@ wpi.java.debugJni = false // Set this to true to enable desktop support. def includeDesktopSupport = true +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 5. +dependencies { + annotationProcessor wpi.java.deps.wpilibAnnotations() + implementation wpi.java.deps.wpilib() + implementation wpi.java.vendor.java() + + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) + roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + + roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + + nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) + nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) + simulationDebug wpi.sim.enableDebug() + + nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) + nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) + simulationRelease wpi.sim.enableRelease() + + implementation "gov.nist.math:jama:1.0.3" + + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" + +} + +// Configuration for AdvantageKit +task(replayWatch, type: JavaExec) { + mainClass = "org.littletonrobotics.junction.ReplayWatch" + classpath = sourceSets.main.runtimeClasspath +} + // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 4. dependencies { + annotationProcessor wpi.java.deps.wpilibAnnotations() implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() @@ -67,6 +116,17 @@ dependencies { nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() + + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" +} + +test { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' } // Simulation configuration (e.g. environment variables). @@ -78,21 +138,24 @@ wpi.sim.addDriverstation() // knows where to look for our Robot Class. jar { from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from sourceSets.main.allSource manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE } +gversion { + srcDir = file("src/main/java") + classPackage = "frc.robot.util" + className = "BuildData" // This will generate GVersion.java +} + // Configure jar and deploy tasks deployArtifact.jarTask = jar wpi.java.configureExecutableTasks(jar) wpi.java.configureTestTasks(test) -project.compileJava.dependsOn(createVersionFile) -gversion { - srcDir = "src/main/java/" - classPackage = "frc.robot.util" - className = "BuildData" - dateFormat = "yyyy-MM-dd HH:mm:ss z" - timeZone = "PST" - indent = " " -} \ No newline at end of file +// Configure string concat to always inline compile +tasks.withType(JavaCompile) { + options.compilerArgs.add '-XDstringConcat=inline' +} + diff --git a/choreo_paths.chor b/choreo_paths.chor new file mode 100644 index 0000000..ba97e56 --- /dev/null +++ b/choreo_paths.chor @@ -0,0 +1,14676 @@ +{ + "version": "v0.3.1", + "robotConfiguration": { + "mass": 73.8410170797483, + "rotationalInertia": 5.8275989259364, + "motorMaxTorque": 0.6789855072463769, + "motorMaxVelocity": 4640, + "gearing": 5.903, + "wheelbase": 0.5270497153931537, + "trackWidth": 0.5270497153931537, + "bumperLength": 0.8732515284441746, + "bumperWidth": 0.8732515284441746, + "wheelRadius": 0.050799972568014815 + }, + "paths": { + "Center 6": { + "waypoints": [ + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 2.6305306911468507, + "y": 4.355533123016357, + "heading": -0.6, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "x": 2.7529065132141115, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 2.6305306911468507, + "y": 6.870196914672851, + "heading": 0.6, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 5.372490882873535, + "y": 6.594394683837891, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 8.173408317565917, + "y": 7.465926647186279, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 5.372490882873535, + "y": 6.594394683837891, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 5.372490882873535, + "y": 6.594394683837891, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 8.160777854919433, + "y": 5.798648357391357, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 5.372490882873535, + "y": 6.594394683837891, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 4.251064518955209e-23, + "angularVelocity": -1.474314261329566e-20, + "velocityX": 3.7387078793459146e-19, + "velocityY": -3.5824936616210366e-19, + "timestamp": 0 + }, + { + "x": 1.3700049879548286, + "y": 5.56309045957489, + "heading": -0.009280219609018163, + "angularVelocity": -0.12291476177728405, + "velocityX": 0.26496229964597995, + "velocityY": -0.25385449119608827, + "timestamp": 0.07550126180802841 + }, + { + "x": 1.4100151408697066, + "y": 5.524757983351538, + "heading": -0.02784117001944216, + "angularVelocity": -0.24583629420151398, + "velocityX": 0.529926943692794, + "velocityY": -0.5077064317258396, + "timestamp": 0.15100252361605682 + }, + { + "x": 1.4700306924160145, + "y": 5.467259673777908, + "heading": -0.05568513031548327, + "angularVelocity": -0.36878801266709876, + "velocityX": 0.794894682673056, + "velocityY": -0.7615542866002325, + "timestamp": 0.22650378542408522 + }, + { + "x": 1.5500518988129126, + "y": 5.390595964752488, + "heading": -0.09281752021598637, + "angularVelocity": -0.4918115142885552, + "velocityX": 1.0598658152278608, + "velocityY": -1.0153963945708073, + "timestamp": 0.30200504723211363 + }, + { + "x": 1.6500789877423618, + "y": 5.29476741780182, + "heading": -0.13924857672911986, + "angularVelocity": -0.6149706031561483, + "velocityX": 1.3248399633873773, + "velocityY": -1.269231065228089, + "timestamp": 0.37750630904014204 + }, + { + "x": 1.7701120892018134, + "y": 5.179774707267526, + "heading": -0.19499506458965388, + "angularVelocity": -0.7383517377799143, + "velocityX": 1.5898158333386678, + "velocityY": -1.5230568043574852, + "timestamp": 0.45300757084817045 + }, + { + "x": 1.9101511510897655, + "y": 5.04561857790655, + "heading": -0.26008157143235316, + "angularVelocity": -0.8620585309976676, + "velocityX": 1.854791013215372, + "velocityY": -1.7768726793213794, + "timestamp": 0.5285088326561989 + }, + { + "x": 2.070195845092578, + "y": 4.892299771059555, + "heading": -0.3345408337552403, + "angularVelocity": -0.9861989129692874, + "velocityX": 2.1197618446397177, + "velocityY": -2.030678735367728, + "timestamp": 0.6040100944642273 + }, + { + "x": 2.210251097690708, + "y": 4.758137158014049, + "heading": -0.40081482224653847, + "angularVelocity": -0.8777865018972559, + "velocityX": 1.855005456123874, + "velocityY": -1.776958554502474, + "timestamp": 0.6795113562722557 + }, + { + "x": 2.330301718519033, + "y": 4.643136516439017, + "heading": -0.4576616163795054, + "angularVelocity": -0.752925087232411, + "velocityX": 1.5900478740814894, + "velocityY": -1.523161849499088, + "timestamp": 0.7550126180802841 + }, + { + "x": 2.4303482300309223, + "y": 4.547297740543023, + "heading": -0.5050607440151612, + "angularVelocity": -0.6277925229405316, + "velocityX": 1.3250972118356075, + "velocityY": -1.2693665456833927, + "timestamp": 0.8305138798883125 + }, + { + "x": 2.510391075160622, + "y": 4.470620833949426, + "heading": -0.5429978618626794, + "angularVelocity": -0.5024699844616926, + "velocityX": 1.060152416170463, + "velocityY": -1.0155711938769658, + "timestamp": 0.9060151416963409 + }, + { + "x": 2.5704305889794776, + "y": 4.4131058554979345, + "heading": -0.5714637486145556, + "angularVelocity": -0.3770253115008103, + "velocityX": 0.7952120584621968, + "velocityY": -0.7617750627496834, + "timestamp": 0.9815164035043693 + }, + { + "x": 2.6104669873911317, + "y": 4.374752877259734, + "heading": -0.5904533440855589, + "angularVelocity": -0.2515136173390948, + "velocityX": 0.5302745603570374, + "velocityY": -0.5079779770531248, + "timestamp": 1.0570176653123977 + }, + { + "x": 2.6305003641950035, + "y": 4.355561956718282, + "heading": -0.5999649881801159, + "angularVelocity": -0.1259799355240121, + "velocityX": 0.2653383046075278, + "velocityY": -0.25418012999898976, + "timestamp": 1.1325189271204261 + }, + { + "x": 2.6305306911468507, + "y": 4.355533123016357, + "heading": -0.6, + "angularVelocity": -0.0004637249636049969, + "velocityX": 0.0004016747683561972, + "velocityY": -0.00038189695422426704, + "timestamp": 1.2080201889284545 + }, + { + "x": 2.610550178329772, + "y": 4.374673695560834, + "heading": -0.5905590496217955, + "angularVelocity": 0.12501971996346353, + "velocityX": -0.2645875697942839, + "velocityY": 0.25346484449015166, + "timestamp": 1.2835358786209563 + }, + { + "x": 2.5705586389053807, + "y": 4.412983573513255, + "heading": -0.5716455307114111, + "angularVelocity": 0.2504581364137668, + "velocityX": -0.5295792117801764, + "velocityY": 0.507310177638816, + "timestamp": 1.359051568313458 + }, + { + "x": 2.5105557750803515, + "y": 4.470462660598173, + "heading": -0.5432668457654932, + "angularVelocity": 0.3757985269216927, + "velocityX": -0.7945747972290169, + "velocityY": 0.7611542358809293, + "timestamp": 1.4345672580059599 + }, + { + "x": 2.430541183538218, + "y": 4.547110911961194, + "heading": -0.5054353006402181, + "angularVelocity": 0.5009759598213868, + "velocityX": -1.0595757235079437, + "velocityY": 1.014997700148553, + "timestamp": 1.5100829476984616 + }, + { + "x": 2.330514381369411, + "y": 4.642928388936716, + "heading": -0.45816894222168364, + "angularVelocity": 0.6259144107800899, + "velocityX": -1.3245830446111697, + "velocityY": 1.2688419766235208, + "timestamp": 1.5855986373909634 + }, + { + "x": 2.210474854671869, + "y": 4.757915322387685, + "heading": -0.401491937913021, + "angularVelocity": 0.7505328301899918, + "velocityX": -1.5895971709500427, + "velocityY": 1.5226893102505303, + "timestamp": 1.6611143270834652 + }, + { + "x": 2.0704221363896043, + "y": 4.892072174009052, + "heading": -0.33543399057621603, + "angularVelocity": 0.8747579159482137, + "velocityX": -1.8546174821756414, + "velocityY": 1.7765427577719441, + "timestamp": 1.736630016775967 + }, + { + "x": 1.9103559157267933, + "y": 5.045399678728609, + "heading": -0.2600283702226508, + "angularVelocity": 0.9985424308592702, + "velocityX": -2.1196419090469347, + "velocityY": 2.0304059374138372, + "timestamp": 1.8121457064684687 + }, + { + "x": 1.7702658106581808, + "y": 5.1796120110222805, + "heading": -0.19499833457123836, + "angularVelocity": 0.8611460203331687, + "velocityX": -1.8551125685146552, + "velocityY": 1.7772774484372842, + "timestamp": 1.8876613961609705 + }, + { + "x": 1.650189127018009, + "y": 5.294652307387748, + "heading": -0.13927528164735775, + "angularVelocity": 0.7379003376753047, + "velocityX": -1.5900892136338995, + "velocityY": 1.5233959569714512, + "timestamp": 1.9631770858534723 + }, + { + "x": 1.5501256774020051, + "y": 5.3905199561224375, + "heading": -0.092847301921564, + "angularVelocity": 0.6148123643556385, + "velocityX": -1.3250683404132293, + "velocityY": 1.269506365168085, + "timestamp": 2.0386927755459743 + }, + { + "x": 1.4700752292390322, + "y": 5.467214496036427, + "heading": -0.055707914242732186, + "angularVelocity": 0.4918102162618465, + "velocityX": -1.0600505469649628, + "velocityY": 1.0156106661581024, + "timestamp": 2.1142084652384763 + }, + { + "x": 1.4100375649931816, + "y": 5.524735600725888, + "heading": -0.027854100312408023, + "angularVelocity": 0.3688480373250149, + "velocityX": -0.795035634188372, + "velocityY": 0.7617106448167867, + "timestamp": 2.1897241549309783 + }, + { + "x": 1.3700125191328987, + "y": 5.563083065206659, + "heading": -0.009284821466752706, + "angularVelocity": 0.24589961266683785, + "velocityX": -0.5300229134271852, + "velocityY": 0.5078079090175974, + "timestamp": 2.2652398446234803 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 1.5518121767006254e-21, + "angularVelocity": 0.12295221701027016, + "velocityX": -0.2650114064294312, + "velocityY": 0.2539039085419965, + "timestamp": 2.3407555343159823 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 1.7406173041226286e-22, + "angularVelocity": -1.5911931901870827e-20, + "velocityX": 3.5223689868208494e-19, + "velocityY": -3.372608251853029e-19, + "timestamp": 2.4162712240084843 + }, + { + "x": 1.3889696841330885, + "y": 5.58225679397583, + "heading": -6.407798473621889e-19, + "angularVelocity": -7.175883313096834e-18, + "velocityX": 0.4362901951704297, + "velocityY": 2.0950088972512994e-17, + "timestamp": 2.505591783041138 + }, + { + "x": 1.466909051129583, + "y": 5.58225679397583, + "heading": -2.0240616740468293e-18, + "angularVelocity": -1.5486712596203492e-17, + "velocityX": 0.8725803761259643, + "velocityY": 4.333390986706464e-17, + "timestamp": 2.594912342073792 + }, + { + "x": 1.5838180988942594, + "y": 5.58225679397583, + "heading": -4.31930166451708e-18, + "angularVelocity": -2.5696659484827644e-17, + "velocityX": 1.3088705336241426, + "velocityY": 6.752712300201592e-17, + "timestamp": 2.6842329011064456 + }, + { + "x": 1.739696823313371, + "y": 5.58225679397583, + "heading": -7.859560105579577e-18, + "angularVelocity": -3.963542637204652e-17, + "velocityX": 1.7451606450663326, + "velocityY": 9.527288452189843e-17, + "timestamp": 2.7735534601390994 + }, + { + "x": 1.9345452126267868, + "y": 5.58225679397583, + "heading": -1.3596874858950657e-17, + "angularVelocity": -6.423285764769935e-17, + "velocityX": 2.1814506248464403, + "velocityY": 1.331744714968596e-16, + "timestamp": 2.862874019171753 + }, + { + "x": 2.1683629455887066, + "y": 5.58225679397583, + "heading": -8.610584120256401e-18, + "angularVelocity": 5.582467007271055e-17, + "velocityX": 2.617737008077176, + "velocityY": 4.485060032849567e-16, + "timestamp": 2.952194578204407 + }, + { + "x": 2.3632110176723393, + "y": 5.58225679397583, + "heading": -5.705566749802383e-18, + "angularVelocity": 3.252350188947776e-17, + "velocityX": 2.1814470732589166, + "velocityY": 4.562389679160296e-16, + "timestamp": 3.041515137237061 + }, + { + "x": 2.5190894141011864, + "y": 5.58225679397583, + "heading": -3.685453907320999e-18, + "angularVelocity": 2.2616437518674518e-17, + "velocityX": 1.7451569730084335, + "velocityY": 4.512114906158933e-16, + "timestamp": 3.1308356962697146 + }, + { + "x": 2.6359981302057838, + "y": 5.58225679397583, + "heading": -2.172184802335044e-18, + "angularVelocity": 1.6942002170479532e-17, + "velocityX": 1.3088668204803557, + "velocityY": 4.421516421317778e-16, + "timestamp": 3.2201562553023684 + }, + { + "x": 2.7139371636966168, + "y": 5.58225679397583, + "heading": -9.80397662414107e-19, + "angularVelocity": 1.3342808787156727e-17, + "velocityX": 0.8725766423197178, + "velocityY": 4.311146572193661e-16, + "timestamp": 3.309476814335022 + }, + { + "x": 2.7529065132141115, + "y": 5.58225679397583, + "heading": 5.080985322169408e-30, + "angularVelocity": 1.0976170246252972e-17, + "velocityX": 0.4362864489377896, + "velocityY": 4.1890359689650406e-16, + "timestamp": 3.398797373367676 + }, + { + "x": 2.744525884733328, + "y": 5.58225679397583, + "heading": 9.653430149203684e-19, + "angularVelocity": 9.143954657969524e-18, + "velocityX": -0.07938327169603715, + "velocityY": 4.0355164701809737e-16, + "timestamp": 3.5043690927202595 + }, + { + "x": 2.681705118537465, + "y": 5.58225679397583, + "heading": 1.7372399265678157e-18, + "angularVelocity": 7.311587955464892e-18, + "velocityX": -0.5950529799183868, + "velocityY": 3.8819956283656593e-16, + "timestamp": 3.609940812072843 + }, + { + "x": 2.564444216810358, + "y": 5.58225679397583, + "heading": 2.315701250129161e-18, + "angularVelocity": 5.4793208552192475e-18, + "velocityX": -1.1107226674549495, + "velocityY": 3.728472544116715e-16, + "timestamp": 3.7155125314254267 + }, + { + "x": 2.3927431839196713, + "y": 5.58225679397583, + "heading": 2.700660449129098e-18, + "angularVelocity": 3.646423505928536e-18, + "velocityX": -1.6263923136199703, + "velocityY": 3.5749449791095625e-16, + "timestamp": 3.8210842507780103 + }, + { + "x": 2.1666020329683717, + "y": 5.58225679397583, + "heading": 2.8921372367337455e-18, + "angularVelocity": 1.8137128843547566e-18, + "velocityX": -2.1420618356706225, + "velocityY": 3.421403973689333e-16, + "timestamp": 3.926655970130594 + }, + { + "x": 1.8944013714726986, + "y": 5.58225679397583, + "heading": 1.9264146661700297e-18, + "angularVelocity": -9.147549897618139e-18, + "velocityX": -2.5783482846063124, + "velocityY": -7.6372503608359e-17, + "timestamp": 4.032227689483178 + }, + { + "x": 1.6766408280374865, + "y": 5.58225679397583, + "heading": 1.154320231308916e-18, + "angularVelocity": -7.313458941346627e-18, + "velocityX": -2.062678762557092, + "velocityY": -6.102339516740112e-17, + "timestamp": 4.137799408835762 + }, + { + "x": 1.5133204157658162, + "y": 5.58225679397583, + "heading": 5.756870468064279e-19, + "angularVelocity": -5.480948762057906e-18, + "velocityX": -1.5470091163924304, + "velocityY": -4.567313800965292e-17, + "timestamp": 4.243371128188346 + }, + { + "x": 1.4044401390253736, + "y": 5.58225679397583, + "heading": 1.9048457565450654e-19, + "angularVelocity": -3.64872783644659e-18, + "velocityX": -1.031339428856028, + "velocityY": -3.032249796435494e-17, + "timestamp": 4.34894284754093 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": -1.3128115287299839e-21, + "angularVelocity": -1.8167496783306206e-18, + "velocityX": -0.5156697206337688, + "velocityY": -1.4971666378034364e-17, + "timestamp": 4.454514566893514 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 2.832404054183828e-22, + "angularVelocity": 1.5118177032219752e-20, + "velocityX": 3.7696639615992387e-19, + "velocityY": 3.7927992176190913e-19, + "timestamp": 4.560086286246098 + }, + { + "x": 1.3700047148687937, + "y": 5.602379495184821, + "heading": 0.009288191856822618, + "angularVelocity": 0.12157329926444245, + "velocityX": 0.2618420490159499, + "velocityY": 0.26338637420506966, + "timestamp": 4.636486218739657 + }, + { + "x": 1.4100143230250328, + "y": 5.642624726402822, + "heading": 0.027864540168642656, + "angularVelocity": 0.24314613515380012, + "velocityX": 0.5236864333566297, + "velocityY": 0.526770507570724, + "timestamp": 4.712886151233216 + }, + { + "x": 1.4700290685888693, + "y": 5.702992212923995, + "heading": 0.05573027757780551, + "angularVelocity": 0.3647351051195263, + "velocityX": 0.7855340129900809, + "velocityY": 0.7901510453070398, + "timestamp": 4.7892860837267754 + }, + { + "x": 1.5500492317268397, + "y": 5.7834815634613825, + "heading": 0.09288926238249703, + "angularVelocity": 0.48637457641503007, + "velocityX": 1.047385259728037, + "velocityY": 1.0535264614818005, + "timestamp": 4.865686016220335 + }, + { + "x": 1.650075082736049, + "y": 5.884092262166466, + "heading": 0.13934967791469124, + "angularVelocity": 0.6081211594802297, + "velocityX": 1.3092400443893395, + "velocityY": 1.3168951257066361, + "timestamp": 4.942085948713894 + }, + { + "x": 1.7701068177219876, + "y": 6.004823674860892, + "heading": 0.19512584033881888, + "angularVelocity": 0.7300551270621883, + "velocityX": 1.571097395878699, + "velocityY": 1.5802554891603338, + "timestamp": 5.018485881207453 + }, + { + "x": 1.9101444770585922, + "y": 6.145675079899509, + "heading": 0.26023970526210166, + "angularVelocity": 0.8522764719559472, + "velocityX": 1.8329552758231984, + "velocityY": 1.843606406988539, + "timestamp": 5.094885813701012 + }, + { + "x": 2.070187850431952, + "y": 6.306645728097026, + "heading": 0.334721545165819, + "angularVelocity": 0.9748940538657789, + "velocityX": 2.094810402965384, + "velocityY": 2.1069475187178845, + "timestamp": 5.171285746194571 + }, + { + "x": 2.2102453512140934, + "y": 6.4475037492072484, + "heading": 0.40094964082687773, + "angularVelocity": 0.8668606568028369, + "velocityX": 1.83321498083718, + "velocityY": 1.8436930048609552, + "timestamp": 5.24768567868813 + }, + { + "x": 2.3302979525685337, + "y": 6.568243321341568, + "heading": 0.4577574427913322, + "angularVelocity": 0.743558274338055, + "velocityX": 1.5713705161265887, + "velocityY": 1.5803622882061914, + "timestamp": 5.324085611181689 + }, + { + "x": 2.4303461105664526, + "y": 6.66886460713537, + "heading": 0.5051246017989356, + "angularVelocity": 0.6199895400640164, + "velocityX": 1.3095320209393337, + "velocityY": 1.3170337002887431, + "timestamp": 5.400485543675249 + }, + { + "x": 2.510390222499523, + "y": 6.7493676560178235, + "heading": 0.5430365397795383, + "angularVelocity": 0.49622999318486116, + "velocityX": 1.0476987259094657, + "velocityY": 1.0537057593505197, + "timestamp": 5.476885476168808 + }, + { + "x": 2.570430594098527, + "y": 6.809752452382176, + "heading": 0.5714836779820867, + "angularVelocity": 0.37234506987223925, + "velocityX": 0.7858694325949297, + "velocityY": 0.7903776141352329, + "timestamp": 5.553285408662367 + }, + { + "x": 2.6104674242959973, + "y": 6.850018954046101, + "heading": 0.5904606458530037, + "angularVelocity": 0.24838985129361124, + "velocityX": 0.5240427431116611, + "velocityY": 0.5270489167947889, + "timestamp": 5.629685341155926 + }, + { + "x": 2.6305007993173484, + "y": 6.870167118249366, + "heading": 0.5999656288193816, + "angularVelocity": 0.1244108817397083, + "velocityX": 0.262217182234288, + "velocityY": 0.26371965976492745, + "timestamp": 5.706085273649485 + }, + { + "x": 2.6305306911468507, + "y": 6.870196914672851, + "heading": 0.6, + "angularVelocity": 0.00044988496058163594, + "velocityX": 0.00039125465856706536, + "velocityY": 0.0003900058876048134, + "timestamp": 5.782485206143044 + }, + { + "x": 2.610549437302289, + "y": 6.850100761877325, + "heading": 0.5905647509563424, + "angularVelocity": -0.1234749074366408, + "velocityX": -0.26148578140433243, + "velocityY": -0.26298941286856325, + "timestamp": 5.858899509579546 + }, + { + "x": 2.570556867464376, + "y": 6.809878732236684, + "heading": 0.5716635252816524, + "angularVelocity": -0.2473519331416332, + "velocityX": -0.523364972778243, + "velocityY": -0.526367811152851, + "timestamp": 5.935313813016047 + }, + { + "x": 2.5105527119270885, + "y": 6.74953087982974, + "heading": 0.5433040446606279, + "angularVelocity": -0.37112790859358535, + "velocityX": -0.7852476936749044, + "velocityY": -0.7897455017317754, + "timestamp": 6.011728116452549 + }, + { + "x": 2.4305366108384567, + "y": 6.669057196782285, + "heading": 0.5054988442861839, + "angularVelocity": -0.4947398415515119, + "velocityX": -1.047135123794239, + "velocityY": -1.0531232953569503, + "timestamp": 6.08814241988905 + }, + { + "x": 2.3305081438358277, + "y": 6.5684575642560175, + "heading": 0.4582658920144913, + "angularVelocity": -0.618116637167829, + "velocityX": -1.3090280550126343, + "velocityY": -1.3165026441661212, + "timestamp": 6.164556723325552 + }, + { + "x": 2.210466881094948, + "y": 6.447731698037168, + "heading": 0.4016287033187306, + "angularVelocity": -0.7411856962463141, + "velocityX": -1.5709266111498505, + "velocityY": -1.5798857123544938, + "timestamp": 6.240971026762053 + }, + { + "x": 2.0704124606025633, + "y": 6.306879099600228, + "heading": 0.335615479546042, + "angularVelocity": -0.863885696838732, + "velocityX": -1.8328299047934917, + "velocityY": -1.8432753045244243, + "timestamp": 6.3173853301985545 + }, + { + "x": 1.9103446925850813, + "y": 6.145899029088003, + "heading": 0.2602568981833521, + "angularVelocity": -0.986184234805082, + "velocityX": -2.0947356819197345, + "velocityY": -2.1066745788763823, + "timestamp": 6.393799633635056 + }, + { + "x": 1.7702572862745904, + "y": 6.004990211385915, + "heading": 0.19517700895499335, + "angularVelocity": -0.8516715628041874, + "velocityX": -1.8332615755229642, + "velocityY": -1.844011021041104, + "timestamp": 6.4702139370715575 + }, + { + "x": 1.6501829922925522, + "y": 5.8842101837041305, + "heading": 0.13940682943388838, + "angularVelocity": -0.7298395328231781, + "velocityX": -1.571358876310599, + "velocityY": -1.5805944993289158, + "timestamp": 6.546628240508059 + }, + { + "x": 1.5501215725938375, + "y": 5.783559506821828, + "heading": 0.09293678090995587, + "angularVelocity": -0.6081328551603944, + "velocityX": -1.3094592922889485, + "velocityY": -1.3171706389490274, + "timestamp": 6.62304254394456 + }, + { + "x": 1.4700727644637088, + "y": 5.703038594475421, + "heading": 0.055762182382246936, + "angularVelocity": -0.48648743567491987, + "velocityX": -1.047563146298221, + "velocityY": -1.0537413641847564, + "timestamp": 6.699456847381062 + }, + { + "x": 1.4100363343772997, + "y": 5.642647734660285, + "heading": 0.027881288300434102, + "angularVelocity": -0.3648648594301607, + "velocityX": -0.7856700563435488, + "velocityY": -0.79030832055309, + "timestamp": 6.775871150817563 + }, + { + "x": 1.3700121102510567, + "y": 5.60238710647987, + "heading": 0.009293825131536333, + "angularVelocity": -0.24324586278985671, + "velocityX": -0.5237792183697898, + "velocityY": -0.5268729330742511, + "timestamp": 6.852285454254065 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": -1.8755644647310815e-21, + "angularVelocity": -0.12162415560405207, + "velocityX": -0.2618895854712081, + "velocityY": -0.2634364457796503, + "timestamp": 6.928699757690566 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": -3.0910168812131e-22, + "angularVelocity": 1.644951644272365e-20, + "velocityX": 3.642271257475659e-19, + "velocityY": 3.6611207460803027e-19, + "timestamp": 7.005114061127068 + }, + { + "x": 1.3751389062520347, + "y": 5.5885822288409, + "heading": 8.044474138243886e-19, + "angularVelocity": 1.104684119406405e-17, + "velocityX": 0.3450801575652578, + "velocityY": 0.08682883964891483, + "timestamp": 7.077963530158855 + }, + { + "x": 1.4254167183820563, + "y": 5.601233098476921, + "heading": 3.3304984787719056e-18, + "angularVelocity": 3.467494133414787e-17, + "velocityX": 0.6901603099959901, + "velocityY": 0.17365767800588378, + "timestamp": 7.150812999190642 + }, + { + "x": 1.5008334359328959, + "y": 5.620209402768863, + "heading": 5.530664896423441e-18, + "angularVelocity": 3.0201543633009046e-17, + "velocityX": 1.0352404561511919, + "velocityY": 0.2604865147838079, + "timestamp": 7.22366246822243 + }, + { + "x": 1.6013890583330925, + "y": 5.645511141572933, + "heading": 9.307328997077923e-18, + "angularVelocity": 5.184202645265804e-17, + "velocityX": 1.3803205944619807, + "velocityY": 0.3473153495879261, + "timestamp": 7.296511937254217 + }, + { + "x": 1.7270835848479098, + "y": 5.677138314704258, + "heading": 1.388759929837292e-17, + "angularVelocity": 6.287307734871695e-17, + "velocityX": 1.7254007226870975, + "velocityY": 0.4341441818542942, + "timestamp": 7.3693614062860044 + }, + { + "x": 1.8779170144977002, + "y": 5.71509092191634, + "heading": 2.114670867599054e-17, + "angularVelocity": 9.964532993924348e-17, + "velocityX": 2.0704808374646535, + "velocityY": 0.520973010736996, + "timestamp": 7.442210875317792 + }, + { + "x": 2.0538893459109575, + "y": 5.759368962864081, + "heading": 2.860167806962463e-17, + "angularVelocity": 1.0233388784665959e-16, + "velocityX": 2.415560933415629, + "velocityY": 0.6078018348825661, + "timestamp": 7.515060344349579 + }, + { + "x": 2.2550005770304224, + "y": 5.809972437029836, + "heading": 3.0384696860296094e-17, + "angularVelocity": 2.447538484936638e-17, + "velocityX": 2.7606410011267433, + "velocityY": 0.6946306519224411, + "timestamp": 7.5879098133813665 + }, + { + "x": 2.4812507044273326, + "y": 5.86690134355086, + "heading": 3.263311662546467e-17, + "angularVelocity": 3.086391424680145e-17, + "velocityX": 3.1057210217714517, + "velocityY": 0.7814594571194984, + "timestamp": 7.660759282413154 + }, + { + "x": 2.7326397212441713, + "y": 5.930155680701671, + "heading": 3.492569144669929e-17, + "angularVelocity": 3.147002787608244e-17, + "velocityX": 3.450800948283466, + "velocityY": 0.8682882386309498, + "timestamp": 7.733608751444941 + }, + { + "x": 3.009167606908465, + "y": 5.999735443305836, + "heading": 3.599237380066186e-17, + "angularVelocity": 1.464228041869e-17, + "velocityX": 3.7958805923984467, + "velocityY": 0.9551169490858475, + "timestamp": 7.8064582204767285 + }, + { + "x": 3.304583058119313, + "y": 6.074067682586061, + "heading": 6.799442288826936e-17, + "angularVelocity": 4.392900801184583e-16, + "velocityX": 4.055148996102434, + "velocityY": 1.0203538923226851, + "timestamp": 7.879307689508516 + }, + { + "x": 3.5999985093303932, + "y": 6.1483999218663445, + "heading": 9.124686962806145e-17, + "angularVelocity": 3.1918484854938582e-16, + "velocityX": 4.055148996105624, + "velocityY": 1.0203538923234874, + "timestamp": 7.952157158540303 + }, + { + "x": 3.8954139605414735, + "y": 6.222732161146628, + "heading": 6.235845734277902e-17, + "angularVelocity": -3.965493869647151e-16, + "velocityX": 4.055148996105624, + "velocityY": 1.0203538923234874, + "timestamp": 8.025006627572091 + }, + { + "x": 4.190829411752554, + "y": 6.297064400426912, + "heading": 1.187079957064157e-17, + "angularVelocity": -6.930408476976731e-16, + "velocityX": 4.055148996105624, + "velocityY": 1.0203538923234874, + "timestamp": 8.097856096603879 + }, + { + "x": 4.486244862963634, + "y": 6.3713966397071955, + "heading": 4.1868203670914044e-17, + "angularVelocity": 4.1177244664893084e-16, + "velocityX": 4.055148996105624, + "velocityY": 1.0203538923234874, + "timestamp": 8.170705565635666 + }, + { + "x": 4.781660314174713, + "y": 6.445728878987481, + "heading": 5.651608381744153e-17, + "angularVelocity": 2.010705135017266e-16, + "velocityX": 4.0551489961056175, + "velocityY": 1.0203538923235147, + "timestamp": 8.243555034667454 + }, + { + "x": 5.077075765372958, + "y": 6.520061118318777, + "heading": 1.7973952569935785e-17, + "angularVelocity": -5.290653694492406e-16, + "velocityX": 4.055148995929428, + "velocityY": 1.0203538930237357, + "timestamp": 8.316404503699241 + }, + { + "x": 5.372490882873535, + "y": 6.594394683837891, + "heading": 4.5271640595913275e-29, + "angularVelocity": -2.467272968321232e-16, + "velocityX": 4.055144415282903, + "velocityY": 1.0203720975190471, + "timestamp": 8.389253972731028 + }, + { + "x": 5.647242858587802, + "y": 6.679885106259968, + "heading": -9.429977726826593e-18, + "angularVelocity": -1.3703765394515705e-16, + "velocityX": 3.9927311876140994, + "velocityY": 1.242357857334978, + "timestamp": 8.458067013931142 + }, + { + "x": 5.921994445476622, + "y": 6.76537677829226, + "heading": -1.6888267565983654e-17, + "angularVelocity": -1.0838483097253208e-16, + "velocityX": 3.9927255371524812, + "velocityY": 1.2423760168319844, + "timestamp": 8.526880055131256 + }, + { + "x": 6.196746032348459, + "y": 6.850868450379129, + "heading": 3.8093668702008635e-18, + "angularVelocity": 3.0078069614698596e-16, + "velocityX": 3.9927255369056973, + "velocityY": 1.2423760176250944, + "timestamp": 8.59569309633137 + }, + { + "x": 6.471497619219933, + "y": 6.936360122466824, + "heading": -1.5257964645179415e-17, + "angularVelocity": -2.7708892359449947e-16, + "velocityX": 3.9927255369004206, + "velocityY": 1.242376017637102, + "timestamp": 8.664506137531484 + }, + { + "x": 6.734791042279063, + "y": 7.0182864892966865, + "heading": -1.5521608741636682e-17, + "angularVelocity": -3.831310052315505e-18, + "velocityX": 3.826214020877989, + "velocityY": 1.190564541270817, + "timestamp": 8.733319178731598 + }, + { + "x": 6.975999483549517, + "y": 7.0933408957400905, + "heading": -1.3888820098781766e-17, + "angularVelocity": 2.372789538662461e-17, + "velocityX": 3.5052722138671455, + "velocityY": 1.0907003256132755, + "timestamp": 8.802132219931712 + }, + { + "x": 7.19512292364644, + "y": 7.161523335765207, + "heading": -1.2272226828080516e-17, + "angularVelocity": 2.349254214730808e-17, + "velocityX": 3.1843301251531018, + "velocityY": 0.990836022300431, + "timestamp": 8.870945261131826 + }, + { + "x": 7.3921613561082085, + "y": 7.2228338073614236, + "heading": -9.91377064737451e-18, + "angularVelocity": 3.427338974634624e-17, + "velocityX": 2.863387942537892, + "velocityY": 0.8909716897691187, + "timestamp": 8.93975830233194 + }, + { + "x": 7.567114777704009, + "y": 7.277272309523435, + "heading": -7.430470042956345e-18, + "angularVelocity": 3.6087645031897434e-17, + "velocityX": 2.5424457129720897, + "velocityY": 0.7911073426285686, + "timestamp": 9.008571343532054 + }, + { + "x": 7.719983186495353, + "y": 7.3248388416480585, + "heading": -5.288724881656081e-18, + "angularVelocity": 3.1124117230174046e-17, + "velocityX": 2.2215034552359305, + "velocityY": 0.6912429867224752, + "timestamp": 9.077384384732168 + }, + { + "x": 7.850766581189917, + "y": 7.3655334033331705, + "heading": -3.488536974859941e-18, + "angularVelocity": 2.6160563105152182e-17, + "velocityX": 1.9005611787195344, + "velocityY": 0.591378624972686, + "timestamp": 9.146197425932282 + }, + { + "x": 7.95946496086461, + "y": 7.399355994291541, + "heading": -1.0573272555421988e-18, + "angularVelocity": 3.5330653563536946e-17, + "velocityX": 1.5796188887886835, + "velocityY": 0.4915142590488284, + "timestamp": 9.215010467132396 + }, + { + "x": 8.046078324827116, + "y": 7.426306614307746, + "heading": 1.2407808813183412e-18, + "angularVelocity": 3.339640418092168e-17, + "velocityX": 1.2586765887969913, + "velocityY": 0.3916498899944195, + "timestamp": 9.28382350833251 + }, + { + "x": 8.110606672538966, + "y": 7.446385263214236, + "heading": 3.1973352292465776e-18, + "angularVelocity": 2.843290041803697e-17, + "velocityX": 0.9377342809802008, + "velocityY": 0.2917855185051375, + "timestamp": 9.352636549532624 + }, + { + "x": 8.153050003569383, + "y": 7.459591940876971, + "heading": 1.989163877112771e-18, + "angularVelocity": -1.7557302091088906e-17, + "velocityX": 0.616791966903332, + "velocityY": 0.191921145067957, + "timestamp": 9.421449590732738 + }, + { + "x": 8.173408317565917, + "y": 7.465926647186279, + "heading": -4.514694690881201e-29, + "angularVelocity": -2.890678630808383e-17, + "velocityX": 0.295849647704581, + "velocityY": 0.09205677003704135, + "timestamp": 9.490262631932852 + }, + { + "x": 8.170410935320641, + "y": 7.464993998545025, + "heading": -1.1479594087562529e-18, + "angularVelocity": -1.587345588598103e-17, + "velocityX": -0.0414464261398678, + "velocityY": -0.012896237403529248, + "timestamp": 9.562582069926373 + }, + { + "x": 8.14302049088619, + "y": 7.456471207485986, + "heading": -2.673164473169197e-18, + "angularVelocity": -2.108983568881684e-17, + "velocityX": -0.3787424957160769, + "velocityY": -0.1178492435160072, + "timestamp": 9.634901507919894 + }, + { + "x": 8.091236984632978, + "y": 7.440358274124418, + "heading": -4.575611442410796e-18, + "angularVelocity": -2.630616362629961e-17, + "velocityX": -0.7160385601703985, + "velocityY": -0.22280224803477408, + "timestamp": 9.707220945913415 + }, + { + "x": 8.015060417013729, + "y": 7.41665519860119, + "heading": -4.947277187842861e-18, + "angularVelocity": -5.139223364033182e-18, + "velocityX": -1.0533346183646357, + "velocityY": -0.3277552506056719, + "timestamp": 9.779540383906935 + }, + { + "x": 7.914490788594349, + "y": 7.385361981092388, + "heading": -5.6961729062268445e-18, + "angularVelocity": -1.0355386313000181e-17, + "velocityX": -1.390630668733768, + "velocityY": -0.4327082507417336, + "timestamp": 9.851859821900456 + }, + { + "x": 7.789528100102434, + "y": 7.346478621824409, + "heading": -8.528749636934408e-18, + "angularVelocity": -3.9167571116071525e-17, + "velocityX": -1.7279267090420518, + "velocityY": -0.5376612477472918, + "timestamp": 9.924179259893977 + }, + { + "x": 7.64017235250811, + "y": 7.3000051210991135, + "heading": -1.201295949426594e-17, + "angularVelocity": -4.817805494591479e-17, + "velocityX": -2.0652227359358726, + "velocityY": -0.642614240578846, + "timestamp": 9.996498697887498 + }, + { + "x": 7.466423547169555, + "y": 7.245941479339108, + "heading": -1.5874361845475978e-17, + "angularVelocity": -5.339369965145435e-17, + "velocityX": -2.402518744049449, + "velocityY": -0.7475672275667952, + "timestamp": 10.068818135881019 + }, + { + "x": 7.268281686124032, + "y": 7.184287697178302, + "heading": -2.0112928949513346e-17, + "angularVelocity": -5.860896076646912e-17, + "velocityX": -2.7398147239926662, + "velocityY": -0.8525202057893394, + "timestamp": 10.14113757387454 + }, + { + "x": 7.045746772766981, + "y": 7.11504377567321, + "heading": -2.557518924430414e-17, + "angularVelocity": -7.552962863536295e-17, + "velocityX": -3.0771106569853086, + "velocityY": -0.9574731694028816, + "timestamp": 10.21345701186806 + }, + { + "x": 6.798818813889274, + "y": 7.03820971693686, + "heading": -2.9820267528270197e-17, + "angularVelocity": -5.869899437443598e-17, + "velocityX": -3.4144064960768983, + "velocityY": -1.062426103798449, + "timestamp": 10.285776449861581 + }, + { + "x": 6.527497829863459, + "y": 6.953785527308316, + "heading": -3.380514122458171e-17, + "angularVelocity": -5.510100474849542e-17, + "velocityX": -3.751702053466217, + "velocityY": -1.1673789505403607, + "timestamp": 10.358095887855102 + }, + { + "x": 6.238746162971835, + "y": 6.863937591938514, + "heading": -5.503987816242723e-18, + "angularVelocity": 3.9133536146805468e-16, + "velocityX": -3.992725536908818, + "velocityY": -1.2423760176047256, + "timestamp": 10.430415325848623 + }, + { + "x": 5.949994496080389, + "y": 6.774089656567438, + "heading": 1.3877748170103676e-17, + "angularVelocity": 2.6800175062346127e-16, + "velocityX": -3.992725536906352, + "velocityY": -1.2423760176223397, + "timestamp": 10.502734763842144 + }, + { + "x": 5.661242829177896, + "y": 6.684241721231868, + "heading": -1.5868862095572502e-17, + "angularVelocity": -4.1132247554688176e-16, + "velocityX": -3.9927255370591226, + "velocityY": -1.2423760171313674, + "timestamp": 10.575054201835664 + }, + { + "x": 5.372490882873535, + "y": 6.594394683837891, + "heading": -6.721600712521686e-29, + "angularVelocity": 2.1942734257626597e-16, + "velocityX": -3.992729400499917, + "velocityY": -1.242363600807112, + "timestamp": 10.647373639829185 + }, + { + "x": 5.061138083926854, + "y": 6.516051319215958, + "heading": 2.3088216109691274e-17, + "angularVelocity": 3.007073899622652e-16, + "velocityX": -4.055146014020917, + "velocityY": -1.020365912387457, + "timestamp": 10.724153316287236 + }, + { + "x": 4.749785061492521, + "y": 6.437708842785697, + "heading": 4.4477316523156246e-17, + "angularVelocity": 2.785776314809067e-16, + "velocityX": -4.055148924786648, + "velocityY": -1.0203543443304697, + "timestamp": 10.800932992745286 + }, + { + "x": 4.438432039050997, + "y": 6.3593663663840205, + "heading": 5.0772646087421463e-17, + "angularVelocity": 8.199213456954237e-17, + "velocityX": -4.0551489248803225, + "velocityY": -1.0203543439581864, + "timestamp": 10.877712669203337 + }, + { + "x": 4.127079016609472, + "y": 6.281023889982345, + "heading": 6.919823627727644e-17, + "angularVelocity": 2.399800447194005e-16, + "velocityX": -4.055148924880325, + "velocityY": -1.0203543439581744, + "timestamp": 10.954492345661388 + }, + { + "x": 3.8157259941679467, + "y": 6.202681413580669, + "heading": 1.0145407846874318e-16, + "angularVelocity": 4.2010911844854016e-16, + "velocityX": -4.055148924880325, + "velocityY": -1.0203543439581744, + "timestamp": 11.031272022119438 + }, + { + "x": 3.5043729717264216, + "y": 6.124338937178994, + "heading": 1.0488814636055141e-16, + "angularVelocity": 4.4726261561856034e-17, + "velocityX": -4.055148924880325, + "velocityY": -1.0203543439581744, + "timestamp": 11.108051698577489 + }, + { + "x": 3.1930199492849143, + "y": 6.0459964607773236, + "heading": 1.0202500821302203e-16, + "angularVelocity": -3.729031274317039e-17, + "velocityX": -4.055148924880093, + "velocityY": -1.020354343958116, + "timestamp": 11.18483137503554 + }, + { + "x": 2.885850005558275, + "y": 5.968706528343063, + "heading": 8.840325766190533e-17, + "angularVelocity": -1.7741349246854113e-16, + "velocityX": -4.000667336680725, + "velocityY": -1.0066457166759242, + "timestamp": 11.26161105149359 + }, + { + "x": 2.606604572932183, + "y": 5.8984429460459165, + "heading": 7.446990987421016e-17, + "angularVelocity": -1.8147182210670663e-16, + "velocityX": -3.636970686880395, + "velocityY": -0.9151325655238358, + "timestamp": 11.33839072795164 + }, + { + "x": 2.35528367155804, + "y": 5.835205718956369, + "heading": 6.231835100914448e-17, + "angularVelocity": -1.5826530438188948e-16, + "velocityX": -3.2732737746225524, + "velocityY": -0.8236193483323324, + "timestamp": 11.415170404409691 + }, + { + "x": 2.1318873081529506, + "y": 5.778994848764575, + "heading": 5.446711019193275e-17, + "angularVelocity": -1.0225675828957241e-16, + "velocityX": -2.909576774879259, + "velocityY": -0.732106109127788, + "timestamp": 11.491950080867742 + }, + { + "x": 1.936415486075461, + "y": 5.72981033631561, + "heading": 4.33435156910529e-17, + "angularVelocity": -1.44876808732947e-16, + "velocityX": -2.545879731393325, + "velocityY": -0.6405928589167442, + "timestamp": 11.568729757325793 + }, + { + "x": 1.7688682073406965, + "y": 5.687652182116517, + "heading": 3.180451468220992e-17, + "angularVelocity": -1.5028717938213945e-16, + "velocityX": -2.182182661661835, + "velocityY": -0.5490796021018084, + "timestamp": 11.645509433783843 + }, + { + "x": 1.6292454732920727, + "y": 5.652520386505328, + "heading": 2.1943940462114387e-17, + "angularVelocity": -1.2842687902434274e-16, + "velocityX": -1.8184855744333217, + "velocityY": -0.45756634088428094, + "timestamp": 11.722289110241894 + }, + { + "x": 1.5175472848891722, + "y": 5.624414949723491, + "heading": 1.6975654570196923e-17, + "angularVelocity": -6.470834628409354e-17, + "velocityX": -1.4547884747069404, + "velocityY": -0.3660530765220469, + "timestamp": 11.799068786699944 + }, + { + "x": 1.4337736428516812, + "y": 5.603335871952093, + "heading": 9.958130236130293e-18, + "angularVelocity": -9.139820142132868e-17, + "velocityX": -1.0910913656071621, + "velocityY": -0.2745398098012838, + "timestamp": 11.875848463157995 + }, + { + "x": 1.3779245477393556, + "y": 5.589283153331981, + "heading": 4.935318887472694e-18, + "angularVelocity": -6.541850109660993e-17, + "velocityX": -0.7273942492169659, + "velocityY": -0.1830265412461097, + "timestamp": 11.952628139616046 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 1.1330935234644406e-28, + "angularVelocity": -6.427897478789963e-17, + "velocityX": -0.36369712699443685, + "velocityY": -0.09151327122340726, + "timestamp": 12.029407816074096 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 1.3072856105106544e-29, + "angularVelocity": -1.4338555853349923e-28, + "velocityX": -1.6135165350121333e-27, + "velocityY": 9.374994749220142e-27, + "timestamp": 12.106187492532147 + }, + { + "x": 1.3749689535834184, + "y": 5.5897009757312865, + "heading": -2.106297966297422e-19, + "angularVelocity": -2.8839388200086933e-18, + "velocityX": 0.34187439625345595, + "velocityY": 0.10192558269393831, + "timestamp": 12.179222951920298 + }, + { + "x": 1.4249068603805064, + "y": 5.604589339131964, + "heading": -1.677218384570217e-18, + "angularVelocity": -2.008050062463534e-17, + "velocityX": 0.6837487874443219, + "velocityY": 0.20385116387852864, + "timestamp": 12.25225841130845 + }, + { + "x": 1.4998137199393493, + "y": 5.62692188404313, + "heading": -4.467707102380705e-18, + "angularVelocity": -3.820731383236851e-17, + "velocityX": 1.0256231724475782, + "velocityY": 0.3057767432183604, + "timestamp": 12.325293870696601 + }, + { + "x": 1.5996895316950535, + "y": 5.656698610296368, + "heading": -4.7388677205012346e-18, + "angularVelocity": -3.712725576261135e-18, + "velocityX": 1.3674975497163229, + "velocityY": 0.40770232025224407, + "timestamp": 12.398329330084753 + }, + { + "x": 1.724534294921327, + "y": 5.693919517675143, + "heading": -4.862675168623981e-18, + "angularVelocity": -1.695169019046009e-18, + "velocityX": 1.7093719170406965, + "velocityY": 0.5096278943213377, + "timestamp": 12.471364789472904 + }, + { + "x": 1.8743480086497812, + "y": 5.738584605890742, + "heading": -6.31034613617928e-18, + "angularVelocity": -1.982148095742636e-17, + "velocityX": 2.05124627110591, + "velocityY": 0.6115534644373783, + "timestamp": 12.544400248861056 + }, + { + "x": 2.0491306715246718, + "y": 5.7906938745389684, + "heading": -8.052005504268843e-18, + "angularVelocity": -2.3846764059595142e-17, + "velocityX": 2.3931206066083033, + "velocityY": 0.7134790290191458, + "timestamp": 12.617435708249207 + }, + { + "x": 2.2488822815123832, + "y": 5.850247323013523, + "heading": -1.0500720583751912e-17, + "angularVelocity": -3.352775624225273e-17, + "velocityX": 2.734994914266474, + "velocityY": 0.815404585299506, + "timestamp": 12.690471167637359 + }, + { + "x": 2.4736028352235584, + "y": 5.917244950303913, + "heading": -1.4273207870070583e-17, + "angularVelocity": -5.165281793044341e-17, + "velocityX": 3.07686917551763, + "velocityY": 0.9173301277441942, + "timestamp": 12.76350662702551 + }, + { + "x": 2.7232923258794886, + "y": 5.99168675438915, + "heading": -1.7375627711302575e-17, + "angularVelocity": -4.247826832535968e-17, + "velocityX": 3.418743343954855, + "velocityY": 1.0192556425175678, + "timestamp": 12.836542086413662 + }, + { + "x": 2.997950733144114, + "y": 6.073572729206291, + "heading": -2.1801449098877115e-17, + "angularVelocity": -6.059825493774254e-17, + "velocityX": 3.760617233951168, + "velocityY": 1.1211810742772599, + "timestamp": 12.909577545801813 + }, + { + "x": 3.290621803248295, + "y": 6.1608289545479265, + "heading": -2.5658304246841473e-17, + "angularVelocity": -5.280798094786635e-17, + "velocityX": 4.007246241154717, + "velocityY": 1.1947104334335177, + "timestamp": 12.982613005189965 + }, + { + "x": 3.5832928733526748, + "y": 6.248085179889621, + "heading": -2.8530592502284846e-17, + "angularVelocity": -3.93273114105059e-17, + "velocityX": 4.007246241157447, + "velocityY": 1.1947104334343266, + "timestamp": 13.055648464578116 + }, + { + "x": 3.875963943459746, + "y": 6.335341405222288, + "heading": -2.8145685781283686e-17, + "angularVelocity": 5.2701348673896014e-18, + "velocityX": 4.007246241194295, + "velocityY": 1.194710433310735, + "timestamp": 13.128683923966268 + }, + { + "x": 4.168635084046373, + "y": 6.422597394155118, + "heading": -2.4589541837216757e-17, + "angularVelocity": 4.869064936312243e-17, + "velocityX": 4.007247206198914, + "velocityY": 1.1947071965290432, + "timestamp": 13.201719383354419 + }, + { + "x": 4.462992171425512, + "y": 6.503984796246935, + "heading": -1.865851348037744e-17, + "angularVelocity": 8.120751764419801e-17, + "velocityX": 4.030331154826551, + "velocityY": 1.1143546268296594, + "timestamp": 13.27475484274257 + }, + { + "x": 4.763214801309505, + "y": 6.5599881562762, + "heading": -1.185788123219857e-17, + "angularVelocity": 9.311411614613915e-17, + "velocityX": 4.110642041538187, + "velocityY": 0.7667968477014759, + "timestamp": 13.347790302130722 + }, + { + "x": 5.067118338085079, + "y": 6.590198145999091, + "heading": -5.110846102885528e-18, + "angularVelocity": 9.238026550270056e-17, + "velocityX": 4.161040942598275, + "velocityY": 0.41363455472140287, + "timestamp": 13.420825761518874 + }, + { + "x": 5.372490882873535, + "y": 6.594394683837891, + "heading": 7.435571865732989e-29, + "angularVelocity": 6.997759918061287e-17, + "velocityX": 4.181154569940233, + "velocityY": 0.05745890932919486, + "timestamp": 13.493861220907025 + }, + { + "x": 5.658666545780349, + "y": 6.575364753181748, + "heading": 4.576818453416559e-19, + "angularVelocity": 6.672831036873803e-18, + "velocityX": 4.172334701892643, + "velocityY": -0.2774493094372273, + "timestamp": 13.562450076632574 + }, + { + "x": 5.942400266786845, + "y": 6.533486061480876, + "heading": -2.3827590815129613e-18, + "angularVelocity": -4.141257201051082e-17, + "velocityX": 4.136732097439099, + "velocityY": -0.6105757452558301, + "timestamp": 13.631038932358123 + }, + { + "x": 6.221870758893712, + "y": 6.469027595137471, + "heading": -4.3182265395820836e-18, + "angularVelocity": -2.821839550453941e-17, + "velocityX": 4.07457580609218, + "velocityY": -0.9397804593989688, + "timestamp": 13.699627788083673 + }, + { + "x": 6.495284408168433, + "y": 6.3824041196974175, + "heading": -2.8846127148492644e-18, + "angularVelocity": 2.0901556230520604e-17, + "velocityX": 3.98626929087078, + "velocityY": -1.2629380461844821, + "timestamp": 13.768216643809222 + }, + { + "x": 6.753350615235624, + "y": 6.291966909385253, + "heading": -3.3860339612335026e-18, + "angularVelocity": -7.310535233140868e-18, + "velocityX": 3.762509293052168, + "velocityY": -1.3185408818312967, + "timestamp": 13.836805499534771 + }, + { + "x": 6.989730454687167, + "y": 6.209128001792428, + "heading": -3.8610537876315765e-18, + "angularVelocity": -6.925612351618996e-18, + "velocityX": 3.4463301209950634, + "velocityY": -1.207760454909701, + "timestamp": 13.90539435526032 + }, + { + "x": 7.204423917011262, + "y": 6.133887431043579, + "heading": -4.30969012277514e-18, + "angularVelocity": -6.540950864334822e-18, + "velocityX": 3.130150810259459, + "velocityY": -1.0969795304636218, + "timestamp": 13.97398321098587 + }, + { + "x": 7.397430999037374, + "y": 6.066245208513786, + "heading": -4.7319640256573494e-18, + "angularVelocity": -6.156596409336524e-18, + "velocityX": 2.813971453298626, + "velocityY": -0.9861984401730881, + "timestamp": 14.042572066711418 + }, + { + "x": 7.568751699180255, + "y": 6.0062013398906515, + "heading": -4.48443110171988e-18, + "angularVelocity": 3.608937943647087e-18, + "velocityX": 2.4977920732254844, + "velocityY": -0.8754172669594278, + "timestamp": 14.111160922436968 + }, + { + "x": 7.718386016488768, + "y": 5.953755828586764, + "heading": -5.202509665797113e-18, + "angularVelocity": -1.0469318324083915e-17, + "velocityX": 2.1816126792850916, + "velocityY": -0.7646360439915042, + "timestamp": 14.179749778162517 + }, + { + "x": 7.846333950328824, + "y": 5.9089086768771955, + "heading": -5.894233408781161e-18, + "angularVelocity": -1.008507483708594e-17, + "velocityX": 1.8654332760999357, + "velocityY": -0.6538547878538727, + "timestamp": 14.248338633888066 + }, + { + "x": 7.952595500247506, + "y": 5.871659886387006, + "heading": -4.927257324037146e-18, + "angularVelocity": 1.4098151580412308e-17, + "velocityX": 1.549253866311417, + "velocityY": -0.5430735080234801, + "timestamp": 14.316927489613615 + }, + { + "x": 8.037170665905128, + "y": 5.842009458334993, + "heading": -3.93393020805799e-18, + "angularVelocity": 1.448233981268987e-17, + "velocityX": 1.2330744515704004, + "velocityY": -0.43229221042344845, + "timestamp": 14.385516345339164 + }, + { + "x": 8.100059447037491, + "y": 5.8199573936691165, + "heading": -2.914254186351223e-18, + "angularVelocity": 1.4866497055858427e-17, + "velocityX": 0.9168950329774561, + "velocityY": -0.32151089900254415, + "timestamp": 14.454105201064714 + }, + { + "x": 8.141261843433236, + "y": 5.8055036931477435, + "heading": -1.470300622541165e-18, + "angularVelocity": 2.1052305779669128e-17, + "velocityX": 0.6007156113029791, + "velocityY": -0.21072957652491553, + "timestamp": 14.522694056790263 + }, + { + "x": 8.160777854919433, + "y": 5.798648357391357, + "heading": 3.515192231280365e-30, + "angularVelocity": 2.1436436094366998e-17, + "velocityX": 0.2845361871072536, + "velocityY": -0.09994824500086058, + "timestamp": 14.591282912515812 + }, + { + "x": 8.15740465483607, + "y": 5.7998119860439035, + "heading": 1.5702668702365359e-18, + "angularVelocity": 2.1839117027368502e-17, + "velocityX": -0.046914134644165766, + "velocityY": 0.016183632732190804, + "timestamp": 14.663184485654664 + }, + { + "x": 8.130199655390193, + "y": 5.809325678962654, + "heading": 3.16948883770298e-18, + "angularVelocity": 2.2241821668858154e-17, + "velocityX": -0.378364453769859, + "velocityY": 0.1323155044240569, + "timestamp": 14.735086058793517 + }, + { + "x": 8.079162856808358, + "y": 5.827189435626364, + "heading": 5.55395968868795e-18, + "angularVelocity": 3.3162985827562857e-17, + "velocityX": -0.7098147697446826, + "velocityY": 0.2484473688664962, + "timestamp": 14.806987631932369 + }, + { + "x": 8.00429425936746, + "y": 5.8534032553979545, + "heading": 7.967390201909252e-18, + "angularVelocity": 3.3565753958663505e-17, + "velocityX": -1.0412650818684488, + "velocityY": 0.3645792244485113, + "timestamp": 14.878889205071221 + }, + { + "x": 7.90559386341362, + "y": 5.887967137481077, + "heading": 1.0409784598773327e-17, + "angularVelocity": 3.396858080066931e-17, + "velocityX": -1.3727153891784034, + "velocityY": 0.4807110689549696, + "timestamp": 14.950790778210074 + }, + { + "x": 7.7830616693918495, + "y": 5.930881080851848, + "heading": 1.2881147946027218e-17, + "angularVelocity": 3.437147811049413e-17, + "velocityX": -1.704165690299189, + "velocityY": 0.59684289922138, + "timestamp": 15.022692351348926 + }, + { + "x": 7.636697677895493, + "y": 5.982145084145087, + "heading": 1.5381486333713155e-17, + "angularVelocity": 3.4774460120026703e-17, + "velocityX": -2.0356159831677796, + "velocityY": 0.7129747105009819, + "timestamp": 15.094593924487778 + }, + { + "x": 7.466501889755232, + "y": 6.04175914544953, + "heading": 1.6910300359972597e-17, + "angularVelocity": 2.1262594954707737e-17, + "velocityX": -2.367066264483349, + "velocityY": 0.8291064951989165, + "timestamp": 15.16649549762663 + }, + { + "x": 7.27247430621708, + "y": 6.109723261898264, + "heading": 2.2846387162938302e-17, + "angularVelocity": 8.255851080616128e-17, + "velocityX": -2.6985165284694865, + "velocityY": 0.9452382400241005, + "timestamp": 15.238397070765483 + }, + { + "x": 7.05461492935771, + "y": 6.186037428713062, + "heading": 2.7078383309752413e-17, + "angularVelocity": 5.88581857401056e-17, + "velocityX": -3.029966763573451, + "velocityY": 1.0613699183941856, + "timestamp": 15.310298643904336 + }, + { + "x": 6.812923763330427, + "y": 6.270701636337379, + "heading": 3.133942989918459e-17, + "angularVelocity": 5.926221643581749e-17, + "velocityX": -3.361416940913657, + "velocityY": 1.1775014638527852, + "timestamp": 15.382200217043188 + }, + { + "x": 6.547400820594944, + "y": 6.3637158561012495, + "heading": 3.5629653523192204e-17, + "angularVelocity": 5.966800776000847e-17, + "velocityX": -3.692866944965416, + "velocityY": 1.293632610572301, + "timestamp": 15.45410179018204 + }, + { + "x": 6.261992928035358, + "y": 6.458260863280659, + "heading": 1.2977811491721515e-17, + "angularVelocity": -3.1503958874068347e-16, + "velocityX": -3.9694248693059904, + "velocityY": 1.3149226512308523, + "timestamp": 15.526003363320893 + }, + { + "x": 5.9696584968278374, + "y": 6.528524677524475, + "heading": 8.30491658549572e-18, + "angularVelocity": -6.499016227646973e-17, + "velocityX": -4.065758486855054, + "velocityY": 0.9772222105367047, + "timestamp": 15.597904936459745 + }, + { + "x": 5.672459000416948, + "y": 6.574009641563384, + "heading": 3.9900100019018806e-18, + "angularVelocity": -6.001129593177065e-17, + "velocityX": -4.133421334703643, + "velocityY": 0.6326004015387804, + "timestamp": 15.669806509598597 + }, + { + "x": 5.372490882873535, + "y": 6.594394683837891, + "heading": 4.637949774599644e-29, + "angularVelocity": -5.549266626130552e-17, + "velocityX": -4.171927044824572, + "velocityY": 0.28351316090317685, + "timestamp": 15.74170808273745 + }, + { + "x": 5.050773644251368, + "y": 6.587292971608332, + "heading": 4.510908674878247e-19, + "angularVelocity": 5.861667091112072e-18, + "velocityX": -4.180530989625617, + "velocityY": -0.0922826771180165, + "timestamp": 15.818664153490314 + }, + { + "x": 4.730994022144736, + "y": 6.551328956326083, + "heading": 3.8554219387702585e-18, + "angularVelocity": 4.423732966002132e-17, + "velocityX": -4.155352774358414, + "velocityY": -0.46733175083411, + "timestamp": 15.895620224243178 + }, + { + "x": 4.415736028009693, + "y": 6.486793579258048, + "heading": 4.369824303813874e-18, + "angularVelocity": 6.684363689090542e-18, + "velocityX": -4.096596812322474, + "velocityY": -0.8386002096609608, + "timestamp": 15.972576294996042 + }, + { + "x": 4.107092112871089, + "y": 6.39573672005434, + "heading": 4.788552904489025e-18, + "angularVelocity": 5.441137996400068e-18, + "velocityX": -4.010650649378687, + "velocityY": -1.1832316581771465, + "timestamp": 16.049532365748906 + }, + { + "x": 3.7984505091445313, + "y": 6.304672026515864, + "heading": 1.0411227800571568e-17, + "angularVelocity": 7.306343529532718e-17, + "velocityX": -4.01062061390491, + "velocityY": -1.183333460863932, + "timestamp": 16.12648843650177 + }, + { + "x": 3.489808905492053, + "y": 6.213607332726315, + "heading": 1.1712240643296965e-17, + "angularVelocity": 1.6905915673051655e-17, + "velocityX": -4.0106206129422866, + "velocityY": -1.1833334641265096, + "timestamp": 16.203444507254634 + }, + { + "x": 3.181167301839591, + "y": 6.12254263893676, + "heading": 1.739759563187972e-17, + "angularVelocity": 7.387792714615859e-17, + "velocityX": -4.010620612942067, + "velocityY": -1.1833334641265585, + "timestamp": 16.280400578007498 + }, + { + "x": 2.8759727989575126, + "y": 6.032495012102541, + "heading": 6.160394566309594e-18, + "angularVelocity": -1.460209825642653e-16, + "velocityX": -3.965827515572878, + "velocityY": -1.1701172623976306, + "timestamp": 16.357356648760362 + }, + { + "x": 2.5985232218869587, + "y": 5.950633524607093, + "heading": -5.014948305455115e-18, + "angularVelocity": -1.452171708155591e-16, + "velocityX": -3.6052981182154618, + "velocityY": -1.0637430769865972, + "timestamp": 16.434312719513226 + }, + { + "x": 2.3488185906123467, + "y": 5.876958182346815, + "heading": -1.021650025995837e-17, + "angularVelocity": -6.759118421281684e-17, + "velocityX": -3.244768461171998, + "velocityY": -0.9573688149552048, + "timestamp": 16.51126879026609 + }, + { + "x": 2.1268589117951118, + "y": 5.8114689872871645, + "heading": -1.3728338188268751e-17, + "angularVelocity": -4.5634319606004684e-17, + "velocityX": -2.884238717567016, + "velocityY": -0.8509945273838397, + "timestamp": 16.588224861018954 + }, + { + "x": 1.9326441887659624, + "y": 5.754165940410867, + "heading": -1.4828016154735985e-17, + "angularVelocity": -1.4289684436291385e-17, + "velocityX": -2.5237089306813805, + "velocityY": -0.7446202270425196, + "timestamp": 16.66518093177182 + }, + { + "x": 1.7661744235233214, + "y": 5.705049042307557, + "heading": -1.1178654192587566e-17, + "angularVelocity": 4.742136554528476e-17, + "velocityX": -2.163179117827389, + "velocityY": -0.638245919039237, + "timestamp": 16.742137002524682 + }, + { + "x": 1.6274496173994693, + "y": 5.664118293370325, + "heading": -6.756440882155054e-18, + "angularVelocity": 5.746412553450026e-17, + "velocityX": -1.802649287661176, + "velocityY": -0.531871605927984, + "timestamp": 16.819093073277546 + }, + { + "x": 1.5164697713460344, + "y": 5.631373693879946, + "heading": -7.46157239431928e-18, + "angularVelocity": -9.162779561300321e-18, + "velocityX": -1.4421194451290982, + "velocityY": -0.4254972891681831, + "timestamp": 16.89604914403041 + }, + { + "x": 1.4332348860767379, + "y": 5.606815244047006, + "heading": -5.695849774050075e-18, + "angularVelocity": 2.2944552690216655e-17, + "velocityX": -1.0815895933226263, + "velocityY": -0.31912296967197257, + "timestamp": 16.973005214783274 + }, + { + "x": 1.3777449621466955, + "y": 5.59044294403529, + "heading": -1.8422092172377636e-18, + "angularVelocity": 5.007584871555877e-17, + "velocityX": -0.7210597343027396, + "velocityY": -0.21274864804744426, + "timestamp": 17.04996128553614 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": -9.960155146068815e-29, + "angularVelocity": 2.3938452146369842e-17, + "velocityX": -0.36052986951212246, + "velocityY": -0.10637432472026213, + "timestamp": 17.126917356289002 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": -4.80920215685678e-29, + "angularVelocity": 4.4408560495863134e-29, + "velocityX": -1.71528985939966e-27, + "velocityY": 5.60383945615098e-27, + "timestamp": 17.203873427041867 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 1.2080201889284545, + "isStopPoint": false, + "x": 2.6305306911468507, + "y": 4.355533123016357, + "heading": -0.6, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 2.4162712240084843, + "isStopPoint": true, + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "timestamp": 3.398797373367676, + "isStopPoint": false, + "x": 2.7529065132141115, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "timestamp": 4.560086286246098, + "isStopPoint": true, + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 5.782485206143044, + "isStopPoint": false, + "x": 2.6305306911468507, + "y": 6.870196914672851, + "heading": 0.6, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 7.005114061127068, + "isStopPoint": true, + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "timestamp": 8.389253972731028, + "isStopPoint": false, + "x": 5.372490882873535, + "y": 6.594394683837891, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 9.490262631932852, + "isStopPoint": false, + "x": 8.173408317565917, + "y": 7.465926647186279, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 10.647373639829185, + "isStopPoint": false, + "x": 5.372490882873535, + "y": 6.594394683837891, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "timestamp": 12.106187492532147, + "isStopPoint": true, + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "timestamp": 13.493861220907025, + "isStopPoint": false, + "x": 5.372490882873535, + "y": 6.594394683837891, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 14.591282912515812, + "isStopPoint": false, + "x": 8.160777854919433, + "y": 5.798648357391357, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 15.74170808273745, + "isStopPoint": false, + "x": 5.372490882873535, + "y": 6.594394683837891, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "timestamp": 17.203873427041867, + "isStopPoint": true, + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint" + }, + { + "scope": [ + 6 + ], + "type": "StopPoint" + }, + { + "scope": [ + 10 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "Source 3": { + "waypoints": [ + { + "x": 0.7043527960777283, + "y": 4.401524543762207, + "heading": -1.03622, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "x": 3.952295303344726, + "y": 1.2853265047073366, + "heading": -0.6, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 6.344450855255133, + "y": 0.6604639410972595, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "x": 8.144450855255126, + "y": 0.6604639410972595, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "x": 3.952295303344726, + "y": 1.2853265047073366, + "heading": -0.6, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "x": 1.9563456773757935, + "y": 2.7954604625701904, + "heading": -1.0362799738795863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 14 + }, + { + "x": 0.7043527960777283, + "y": 4.401524543762207, + "heading": -1.0362799738795863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "x": 3.952295303344726, + "y": 1.5853265047073366, + "heading": -0.6, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "x": 6.352611541748047, + "y": 1.5393844842910767, + "heading": 0.305878931297, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 16 + }, + { + "x": 8.2424898147583, + "y": 2.4419312477111816, + "heading": 0.6435004368303741, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 27 + }, + { + "x": 3.952295303344726, + "y": 1.5853265047073366, + "heading": -0.6, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "x": 1.8480632305145264, + "y": 2.7954604625701904, + "heading": -1.0362799738795863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 14 + }, + { + "x": 0.7043527960777284, + "y": 4.401524543762207, + "heading": -1.0362799738795863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + }, + { + "x": 2.6081254482269287, + "y": 2.3851616382598877, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.7043527960777283, + "y": 4.401524543762207, + "heading": -1.03622, + "angularVelocity": 7.233895438617747e-28, + "velocityX": 4.805672207333151e-25, + "velocityY": 5.304283343813489e-25, + "timestamp": 0 + }, + { + "x": 0.7175301442807854, + "y": 4.387776624576329, + "heading": -1.031337197748148, + "angularVelocity": 0.07304296649540387, + "velocityX": 0.1971229948804499, + "velocityY": -0.20565829797731885, + "timestamp": 0.06684835633229075 + }, + { + "x": 0.7438862242893792, + "y": 4.360280179260933, + "heading": -1.0216329619423168, + "angularVelocity": 0.1451679044671403, + "velocityX": 0.39426668738993087, + "velocityY": -0.41132567536466397, + "timestamp": 0.1336967126645815 + }, + { + "x": 0.783422636003079, + "y": 4.319034521528578, + "heading": -1.0071786936695828, + "angularVelocity": 0.216224737088293, + "velocityX": 0.5914343131665276, + "velocityY": -0.6170033190843506, + "timestamp": 0.20054506899687224 + }, + { + "x": 0.8361412515663075, + "y": 4.264038862538382, + "heading": -0.9880588752449545, + "angularVelocity": 0.28601777924930255, + "velocityX": 0.7886299447838971, + "velocityY": -0.8226927632568858, + "timestamp": 0.267393425329163 + }, + { + "x": 0.9020442909887891, + "y": 4.19529228059345, + "heading": -0.9643753261297147, + "angularVelocity": 0.3542876805753188, + "velocityX": 0.9858587860394051, + "velocityY": -1.0283959953062354, + "timestamp": 0.33424178166145374 + }, + { + "x": 0.9811344267458814, + "y": 4.112793681403445, + "heading": -0.9362533192061244, + "angularVelocity": 0.4206835959256975, + "velocityX": 1.1831276054709545, + "velocityY": -1.234115597097437, + "timestamp": 0.4010901379937445 + }, + { + "x": 1.0734149329571012, + "y": 4.016541746348792, + "heading": -0.9038507047307363, + "angularVelocity": 0.48471819283514717, + "velocityX": 1.380445403212468, + "velocityY": -1.439854924423246, + "timestamp": 0.46793849432603524 + }, + { + "x": 1.1788899063626475, + "y": 3.9065348671821556, + "heading": -0.867372192915153, + "angularVelocity": 0.5456904824144841, + "velocityX": 1.577824485036696, + "velocityY": -1.6456183098924886, + "timestamp": 0.534786850658326 + }, + { + "x": 1.297564608530482, + "y": 3.7827710716251666, + "heading": -0.827093163780206, + "angularVelocity": 0.6025432986673191, + "velocityX": 1.7752822758711535, + "velocityY": -1.8514111991293054, + "timestamp": 0.6016352069906168 + }, + { + "x": 1.4294460217667793, + "y": 3.6452479711516896, + "heading": -0.7834028226957228, + "angularVelocity": 0.6535739019117648, + "velocityX": 1.9728445166361182, + "velocityY": -2.057239818880132, + "timestamp": 0.6684835633229076 + }, + { + "x": 1.5745437873512116, + "y": 3.49396288510523, + "heading": -0.7368919667230378, + "angularVelocity": 0.695766635479982, + "velocityX": 2.170550983530205, + "velocityY": -2.2631085391905352, + "timestamp": 0.7353319196551984 + }, + { + "x": 1.7328717195301693, + "y": 3.3289139649585624, + "heading": -0.6885644312079015, + "angularVelocity": 0.7229427642904057, + "velocityX": 2.3684641009262695, + "velocityY": -2.4690049120466098, + "timestamp": 0.8021802759874892 + }, + { + "x": 1.904448192532791, + "y": 3.150108401693767, + "heading": -0.6405130662040369, + "angularVelocity": 0.7188114658348472, + "velocityX": 2.56665208265925, + "velocityY": -2.674793713341072, + "timestamp": 0.86902863231978 + }, + { + "x": 2.0892282062966747, + "y": 2.957686751038487, + "heading": -0.6000020926775056, + "angularVelocity": 0.6060130083851152, + "velocityX": 2.7641668980667053, + "velocityY": -2.8784799090465154, + "timestamp": 0.9358769886520708 + }, + { + "x": 2.2825884833700547, + "y": 2.7558237851902505, + "heading": -0.6000020755746027, + "angularVelocity": 2.558462724502468e-7, + "velocityX": 2.8925210383965143, + "velocityY": -3.019714723348049, + "timestamp": 1.0027253449843616 + }, + { + "x": 2.4759485486145203, + "y": 2.553960616434876, + "heading": -0.6000020584735544, + "angularVelocity": 2.558185308983695e-7, + "velocityX": 2.8925178695988683, + "velocityY": -3.0197177586827917, + "timestamp": 1.0695737013166524 + }, + { + "x": 2.6693086138453803, + "y": 2.3520974476664738, + "heading": -0.6000020413725061, + "angularVelocity": 2.5581853093723834e-7, + "velocityX": 2.8925178693953844, + "velocityY": -3.0197177588777038, + "timestamp": 1.1364220576489432 + }, + { + "x": 2.862668680951388, + "y": 2.1502342806942307, + "heading": -0.6000020242714578, + "angularVelocity": 2.5581853089079124e-7, + "velocityX": 2.892517897446146, + "velocityY": -3.019717732008527, + "timestamp": 1.203270413981234 + }, + { + "x": 3.0560576886556707, + "y": 1.948398839231791, + "heading": -0.6000020071702018, + "angularVelocity": 2.5582163847929273e-7, + "velocityX": 2.8929508265391246, + "velocityY": -3.0193029797045874, + "timestamp": 1.2701187703135248 + }, + { + "x": 3.2625807834998075, + "y": 1.760024040188964, + "heading": -0.6000019898927613, + "angularVelocity": 2.584572200422514e-7, + "velocityX": 3.0894266691846277, + "velocityY": -2.8179421212161233, + "timestamp": 1.3369671266458156 + }, + { + "x": 3.4814847915941782, + "y": 1.5861911074303183, + "heading": -0.6000019721069275, + "angularVelocity": 2.660623943281706e-7, + "velocityX": 3.274635609681117, + "velocityY": -2.6004069852451583, + "timestamp": 1.4038154829781064 + }, + { + "x": 3.7117480772714337, + "y": 1.4277118597724805, + "heading": -0.6000019534349248, + "angularVelocity": 2.793188025212715e-7, + "velocityX": 3.4445616662982457, + "velocityY": -2.3707276641188693, + "timestamp": 1.4706638393103972 + }, + { + "x": 3.952295303344726, + "y": 1.2853265047073366, + "heading": -0.6, + "angularVelocity": 0.00002922188416015573, + "velocityX": 3.5984015056043734, + "velocityY": -2.12997540818166, + "timestamp": 1.537512195642688 + }, + { + "x": 4.073584345445593, + "y": 1.2180047789733814, + "heading": -0.5929917130855997, + "angularVelocity": 0.2082403637392416, + "velocityX": 3.6039155578478117, + "velocityY": -2.000360548251316, + "timestamp": 1.5711669914207824 + }, + { + "x": 4.1949158538302544, + "y": 1.1550645250202634, + "heading": -0.5791131956918127, + "angularVelocity": 0.41237859487533596, + "velocityX": 3.605177377532477, + "velocityY": -1.8701719174919447, + "timestamp": 1.6048217871988768 + }, + { + "x": 4.3161484807623705, + "y": 1.0965096037793065, + "heading": -0.5584136500132624, + "angularVelocity": 0.6150548591955444, + "velocityX": 3.60223926870563, + "velocityY": -1.739868565153195, + "timestamp": 1.6384765829769712 + }, + { + "x": 4.437139610321772, + "y": 1.0423284939440405, + "heading": -0.5308573563764309, + "angularVelocity": 0.8187924781515704, + "velocityX": 3.5950635492536227, + "velocityY": -1.6099075505468483, + "timestamp": 1.6721313787550656 + }, + { + "x": 4.55774200997228, + "y": 0.992492159013808, + "heading": -0.4963159496655149, + "angularVelocity": 1.0263442672084038, + "velocityX": 3.5835130436003593, + "velocityY": -1.4808093104719087, + "timestamp": 1.70578617453316 + }, + { + "x": 4.67780133943619, + "y": 0.9469498117340087, + "heading": -0.4545524229930145, + "angularVelocity": 1.2409383479214, + "velocityX": 3.5673765562427637, + "velocityY": -1.3532201348089068, + "timestamp": 1.7394409703112543 + }, + { + "x": 4.797163762965293, + "y": 0.9056279106107681, + "heading": -0.40521985466870114, + "angularVelocity": 1.4658406679865637, + "velocityX": 3.5466690784911457, + "velocityY": -1.227816130446904, + "timestamp": 1.7730957660893487 + }, + { + "x": 4.915796573719093, + "y": 0.8685122267311245, + "heading": -0.34817511875559193, + "angularVelocity": 1.6949957530343842, + "velocityX": 3.5249897677589033, + "velocityY": -1.102834916140014, + "timestamp": 1.806750561867443 + }, + { + "x": 5.036950148745924, + "y": 0.8353847117520539, + "heading": -0.2947632712692716, + "angularVelocity": 1.5870501142985936, + "velocityX": 3.5998903640856996, + "velocityY": -0.9843326697775702, + "timestamp": 1.8404053576455375 + }, + { + "x": 5.1614000342462845, + "y": 0.8048309238478079, + "heading": -0.24930473318253552, + "angularVelocity": 1.3507298747694276, + "velocityX": 3.69783511155234, + "velocityY": -0.9078583660321533, + "timestamp": 1.8740601534236319 + }, + { + "x": 5.288723665341134, + "y": 0.7775918945406379, + "heading": -0.21076034358050552, + "angularVelocity": 1.1452866882977288, + "velocityX": 3.783223999764152, + "velocityY": -0.8093654612190252, + "timestamp": 1.9077149492017262 + }, + { + "x": 5.418289324941767, + "y": 0.75437891499721, + "heading": -0.17727721642858435, + "angularVelocity": 0.994899133326939, + "velocityX": 3.8498423955663323, + "velocityY": -0.6897376438259984, + "timestamp": 1.9413697449798206 + }, + { + "x": 5.549779888092408, + "y": 0.7347009595308325, + "heading": -0.14854912765415715, + "angularVelocity": 0.8536105511930082, + "velocityX": 3.907037915714417, + "velocityY": -0.5846998922865582, + "timestamp": 1.975024540757915 + }, + { + "x": 5.680534523642805, + "y": 0.7195857320422079, + "heading": -0.11505529703891493, + "angularVelocity": 0.995217170120021, + "velocityX": 3.8851709697643155, + "velocityY": -0.4491255150763042, + "timestamp": 2.008679336536009 + }, + { + "x": 5.8094562989699625, + "y": 0.7066518872545531, + "heading": -0.0751084136690073, + "angularVelocity": 1.1869596129271087, + "velocityX": 3.8307103741533206, + "velocityY": -0.3843091151981713, + "timestamp": 2.0423341323141035 + }, + { + "x": 5.939054292128191, + "y": 0.6923931933660783, + "heading": -0.03929641038115861, + "angularVelocity": 1.064098071608519, + "velocityX": 3.850803137025198, + "velocityY": -0.4236749491065335, + "timestamp": 2.075988928092198 + }, + { + "x": 6.072255728678552, + "y": 0.6785723607822262, + "heading": -0.015832129828706364, + "angularVelocity": 0.6972046631085195, + "velocityX": 3.9578738622762195, + "velocityY": -0.410664580316609, + "timestamp": 2.1096437238702923 + }, + { + "x": 6.2080495696602656, + "y": 0.6681080409062233, + "heading": 6.6289175254501184e-21, + "angularVelocity": 0.470427154961713, + "velocityX": 4.0349031346700315, + "velocityY": -0.31093101693441927, + "timestamp": 2.1432985196483867 + }, + { + "x": 6.344450855255133, + "y": 0.6604639410972595, + "heading": 6.1943375615915975e-21, + "angularVelocity": -3.3470862706211174e-23, + "velocityX": 4.052952408157215, + "velocityY": -0.2271325566604413, + "timestamp": 2.176953315426481 + }, + { + "x": 6.601107058435761, + "y": 0.6504176052648494, + "heading": 5.751674625392007e-21, + "angularVelocity": -2.9986081284790827e-23, + "velocityX": 3.7733050076657766, + "velocityY": -0.14769909643853868, + "timestamp": 2.2449722544359716 + }, + { + "x": 6.8381422866926505, + "y": 0.6428253473271538, + "heading": 5.309011689192419e-21, + "angularVelocity": -2.9986081284790274e-23, + "velocityX": 3.484841600128703, + "velocityY": -0.11161976426354472, + "timestamp": 2.312991193445462 + }, + { + "x": 7.0554956222161564, + "y": 0.6371380022468355, + "heading": 4.8663487529928285e-21, + "angularVelocity": -2.9986081284790274e-23, + "velocityX": 3.1954825918878096, + "velocityY": -0.08361413987250188, + "timestamp": 2.3810101324549526 + }, + { + "x": 7.253146101633396, + "y": 0.6331249451031679, + "heading": 4.423685816793239e-21, + "angularVelocity": -2.9986081284790257e-23, + "velocityX": 2.9058153845896255, + "velocityY": -0.0589991141012556, + "timestamp": 2.449029071464443 + }, + { + "x": 7.431083352269109, + "y": 0.6306593023093592, + "heading": 3.9810228805936516e-21, + "angularVelocity": -2.998608128479027e-23, + "velocityX": 2.6159956804219875, + "velocityY": -0.03624935686610544, + "timestamp": 2.5170480104739337 + }, + { + "x": 7.589301236096254, + "y": 0.6296608053560406, + "heading": 3.538359944394063e-21, + "angularVelocity": -2.998608128479026e-23, + "velocityX": 2.326085736283958, + "velocityY": -0.014679690213620148, + "timestamp": 2.585066949483424 + }, + { + "x": 7.727795711567633, + "y": 0.6300740984956901, + "heading": 3.0956970081944744e-21, + "angularVelocity": -2.998608128479027e-23, + "velocityX": 2.0361163741771477, + "velocityY": 0.006076147991541318, + "timestamp": 2.6530858884929147 + }, + { + "x": 7.846563921922145, + "y": 0.6318586995087562, + "heading": 2.6530340719948856e-21, + "angularVelocity": -2.998608128479025e-23, + "velocityX": 1.7461050125751612, + "velocityY": 0.02623682519978192, + "timestamp": 2.7211048275024052 + }, + { + "x": 7.94560374326088, + "y": 0.6349837154726694, + "heading": 2.2103711357952968e-21, + "angularVelocity": -2.9986081284790257e-23, + "velocityX": 1.456062425862283, + "velocityY": 0.04594332121935558, + "timestamp": 2.7891237665118958 + }, + { + "x": 8.024913535772157, + "y": 0.6394247965173088, + "heading": 1.7677081995957073e-21, + "angularVelocity": -2.998608128479025e-23, + "velocityX": 1.1659957309862647, + "velocityY": 0.06529183061823228, + "timestamp": 2.8571427055213863 + }, + { + "x": 8.084491995768397, + "y": 0.6451622562559024, + "heading": 1.3250452633961187e-21, + "angularVelocity": -2.9986081284790257e-23, + "velocityX": 0.875909869572164, + "velocityY": 0.08435091493845756, + "timestamp": 2.925161644530877 + }, + { + "x": 8.124338062283094, + "y": 0.6521798491863118, + "heading": 8.823823271965344e-22, + "angularVelocity": -2.99860812847192e-23, + "velocityX": 0.5858084100537775, + "velocityY": 0.1031711613353776, + "timestamp": 2.9931805835403673 + }, + { + "x": 8.144450855255126, + "y": 0.6604639410972595, + "heading": 4.407410385367316e-22, + "angularVelocity": -1.496603606409474e-23, + "velocityX": 0.29569401206363705, + "velocityY": 0.12179096045282119, + "timestamp": 3.061199522549858 + }, + { + "x": 8.145205258312048, + "y": 0.6698373190850176, + "heading": -0.006376457776647946, + "angularVelocity": -0.09515043658591296, + "velocityX": 0.011257312875227653, + "velocityY": 0.13987091878601224, + "timestamp": 3.1282140104013196 + }, + { + "x": 8.126895205856489, + "y": 0.6804219813671147, + "heading": -0.01902497003587162, + "angularVelocity": -0.18874295193092017, + "velocityX": -0.27322528370501487, + "velocityY": 0.15794588038273327, + "timestamp": 3.1952284982527814 + }, + { + "x": 8.089517204789008, + "y": 0.6922175506286075, + "heading": -0.03782482149772114, + "angularVelocity": -0.28053413619334927, + "velocityX": -0.5577600048265854, + "velocityY": 0.17601521163062275, + "timestamp": 3.2622429861042432 + }, + { + "x": 8.03306724564719, + "y": 0.7052235716462792, + "heading": -0.06263444861696359, + "angularVelocity": -0.3702128885060376, + "velocityX": -0.8423545557333496, + "velocityY": 0.19407774997101565, + "timestamp": 3.329257473955705 + }, + { + "x": 7.957540677867716, + "y": 0.7194394630709334, + "heading": -0.09328537538182405, + "angularVelocity": -0.457377617102716, + "velocityX": -1.1270185030269546, + "velocityY": 0.21213161333357952, + "timestamp": 3.396271961807167 + }, + { + "x": 7.86293205227133, + "y": 0.7348644513578727, + "heading": -0.12957394218488386, + "angularVelocity": -0.5415033072175939, + "velocityX": -1.4117637637712936, + "velocityY": 0.2301739337489082, + "timestamp": 3.4632864496586286 + }, + { + "x": 7.749234920592138, + "y": 0.7514974800960112, + "heading": -0.17124964470192677, + "angularVelocity": -0.6218909351276025, + "velocityX": -1.6966052464834631, + "velocityY": 0.24820049024333068, + "timestamp": 3.5303009375100904 + }, + { + "x": 7.616441577963973, + "y": 0.7693370841976203, + "heading": -0.2179978979951545, + "angularVelocity": -0.6975842805341689, + "velocityX": -1.9815617023367158, + "velocityY": 0.2662051844841503, + "timestamp": 3.5973154253615522 + }, + { + "x": 7.464542732509, + "y": 0.7883812084602433, + "heading": -0.2694129359586248, + "angularVelocity": -0.767222724695476, + "velocityX": -2.2666568129515383, + "velocityY": 0.2841792106929851, + "timestamp": 3.664329913213014 + }, + { + "x": 7.2935271024834005, + "y": 0.8086269223991472, + "heading": -0.32495180452365857, + "angularVelocity": -0.8287591287444611, + "velocityX": -2.5519202713994456, + "velocityY": 0.30210950777955076, + "timestamp": 3.731344401064476 + }, + { + "x": 7.10338105268464, + "y": 0.8300699017901093, + "heading": -0.3838485504571199, + "angularVelocity": -0.8788658664975039, + "velocityX": -2.837387196336119, + "velocityY": 0.3199752781591175, + "timestamp": 3.7983588889159376 + }, + { + "x": 6.894088991530415, + "y": 0.8527032772677037, + "heading": -0.4449335451483212, + "angularVelocity": -0.911519234864506, + "velocityX": -3.1230867811468057, + "velocityY": 0.33773854286197624, + "timestamp": 3.8653733767673994 + }, + { + "x": 6.66563899361204, + "y": 0.876514357446011, + "heading": -0.5061812369696652, + "angularVelocity": -0.9139470252626605, + "velocityX": -3.4089643186519996, + "velocityY": 0.3553124248458716, + "timestamp": 3.9323878646188613 + }, + { + "x": 6.4180709439467645, + "y": 0.9014720885867831, + "heading": -0.5632031512454849, + "angularVelocity": -0.8508893539887942, + "velocityX": -3.6942466860898726, + "velocityY": 0.3724229184007376, + "timestamp": 3.999402352470323 + }, + { + "x": 6.152323100168746, + "y": 0.9276310220541838, + "heading": -0.5999998003509488, + "angularVelocity": -0.5490849857276159, + "velocityX": -3.9655282357309254, + "velocityY": 0.39034743539907785, + "timestamp": 4.066416840321785 + }, + { + "x": 5.872690526203783, + "y": 0.9458335250059967, + "heading": -0.5999998233332822, + "angularVelocity": -3.429457439481182e-7, + "velocityX": -4.172718212586672, + "velocityY": 0.27162041426264033, + "timestamp": 4.133431328173247 + }, + { + "x": 5.593039502470092, + "y": 0.9637503473437068, + "heading": -0.5999998459733419, + "angularVelocity": -3.378382864464675e-7, + "velocityX": -4.172993522737036, + "velocityY": 0.2673574463095649, + "timestamp": 4.2004458160247085 + }, + { + "x": 5.313388500530538, + "y": 0.9816675098474326, + "heading": -0.599999868613403, + "angularVelocity": -3.3783830667742366e-7, + "velocityX": -4.1729931975217545, + "velocityY": 0.2673625223166588, + "timestamp": 4.26746030387617 + }, + { + "x": 5.034117072190086, + "y": 1.004758133272553, + "heading": -0.5999998913039959, + "angularVelocity": -3.385923495322561e-7, + "velocityX": -4.167329144698685, + "velocityY": 0.3445616636853435, + "timestamp": 4.334474791727632 + }, + { + "x": 4.75708110726362, + "y": 1.0469100197154555, + "heading": -0.599999914385774, + "angularVelocity": -3.444296746456035e-7, + "velocityX": -4.133971232317993, + "velocityY": 0.6289966213922602, + "timestamp": 4.401489279579094 + }, + { + "x": 4.483580467446654, + "y": 1.1079272876104984, + "heading": -0.5999999383317541, + "angularVelocity": -3.5732542015957227e-7, + "velocityX": -4.081216593390693, + "velocityY": 0.9105086056956437, + "timestamp": 4.468503767430556 + }, + { + "x": 4.214898341412512, + "y": 1.1875239044147397, + "heading": -0.5999999636887432, + "angularVelocity": -3.783807033968074e-7, + "velocityX": -4.009314025195234, + "velocityY": 1.1877523705123063, + "timestamp": 4.5355182552820175 + }, + { + "x": 3.952295303344726, + "y": 1.2853265047073366, + "heading": -0.6, + "angularVelocity": -5.418418901139292e-7, + "velocityX": -3.9186009844595686, + "velocityY": 1.459424721850854, + "timestamp": 4.602532743133479 + }, + { + "x": 3.8160925290954975, + "y": 1.3417239464495747, + "heading": -0.6007169196903561, + "angularVelocity": -0.020304857924182462, + "velocityX": -3.8575840742138063, + "velocityY": 1.5973086766438414, + "timestamp": 4.637840534649855 + }, + { + "x": 3.6823065921813996, + "y": 1.402858783989713, + "heading": -0.6025143699711063, + "angularVelocity": -0.05090803484313769, + "velocityX": -3.7891335359236398, + "velocityY": 1.7314829082919216, + "timestamp": 4.67314832616623 + }, + { + "x": 3.5512210452916233, + "y": 1.4685621311984904, + "heading": -0.605901427445284, + "angularVelocity": -0.09592946283844166, + "velocityX": -3.712652116147769, + "velocityY": 1.860873886102604, + "timestamp": 4.708456117682606 + }, + { + "x": 3.4231385033293678, + "y": 1.5386145338053958, + "heading": -0.6115606852705346, + "angularVelocity": -0.16028354032355382, + "velocityX": -3.6275999279890074, + "velocityY": 1.984049400949251, + "timestamp": 4.743763909198981 + }, + { + "x": 3.2983746783283663, + "y": 1.6127301792840456, + "heading": -0.6203687484454834, + "angularVelocity": -0.2494651406011415, + "velocityX": -3.5336060297947993, + "velocityY": 2.0991300303864997, + "timestamp": 4.779071700715357 + }, + { + "x": 3.1772451571651232, + "y": 1.6905419145686205, + "heading": -0.633385976023098, + "angularVelocity": -0.3686786122427844, + "velocityX": -3.4306739663131984, + "velocityY": 2.20381201833274, + "timestamp": 4.8143794922317324 + }, + { + "x": 3.060043504806466, + "y": 1.7715964442241068, + "heading": -0.6517847535007312, + "angularVelocity": -0.5210968085925122, + "velocityX": -3.319427450009183, + "velocityY": 2.295655609552751, + "timestamp": 4.849687283748108 + }, + { + "x": 2.947017305338435, + "y": 1.8553668515452788, + "heading": -0.6767173903792927, + "angularVelocity": -0.7061511300415926, + "velocityX": -3.2011687679647043, + "velocityY": 2.372575675890688, + "timestamp": 4.8849950752644835 + }, + { + "x": 2.8383532557888453, + "y": 1.9412747030571285, + "heading": -0.7091821281066824, + "angularVelocity": -0.9194780056501994, + "velocityX": -3.077622385393145, + "velocityY": 2.4331131408206836, + "timestamp": 4.920302866780859 + }, + { + "x": 2.7341728601513613, + "y": 2.028705258017072, + "heading": -0.7499487938265991, + "angularVelocity": -1.1546082031499747, + "velocityX": -2.9506347229099466, + "velocityY": 2.4762396968214184, + "timestamp": 4.955610658297235 + }, + { + "x": 2.6345289236594747, + "y": 2.117005920551524, + "heading": -0.7995562186790833, + "angularVelocity": -1.4049993704499368, + "velocityX": -2.8221514915673502, + "velocityY": 2.500883197225741, + "timestamp": 4.99091844981361 + }, + { + "x": 2.5393709341735584, + "y": 2.2053997028238324, + "heading": -0.8584340708283553, + "angularVelocity": -1.667559754394875, + "velocityX": -2.695098883253072, + "velocityY": 2.503520568011431, + "timestamp": 5.026226241329986 + }, + { + "x": 2.4468481236667095, + "y": 2.294669095310899, + "heading": -0.9198458442656634, + "angularVelocity": -1.7393263866086284, + "velocityX": -2.6204643942099364, + "velocityY": 2.528319916176743, + "timestamp": 5.061534032846361 + }, + { + "x": 2.3517474716897926, + "y": 2.3869836058958684, + "heading": -0.9675055780358813, + "angularVelocity": -1.3498361614635055, + "velocityX": -2.6934749496526904, + "velocityY": 2.6145648487290294, + "timestamp": 5.096841824362737 + }, + { + "x": 2.2546678426174425, + "y": 2.482949029345561, + "heading": -1.00231711789936, + "angularVelocity": -0.9859449817849257, + "velocityX": -2.7495242523828574, + "velocityY": 2.7179673190600213, + "timestamp": 5.132149615879112 + }, + { + "x": 2.156133466715771, + "y": 2.5829110015292103, + "heading": -1.0250587051096054, + "angularVelocity": -0.6440954314488436, + "velocityX": -2.790726116527927, + "velocityY": 2.831159013083336, + "timestamp": 5.167457407395488 + }, + { + "x": 2.0565769235323215, + "y": 2.6870555947722727, + "heading": -1.0362799738795865, + "angularVelocity": -0.31781281943892153, + "velocityX": -2.8196763067798187, + "velocityY": 2.949620714588201, + "timestamp": 5.202765198911863 + }, + { + "x": 1.9563456773757937, + "y": 2.7954604625701904, + "heading": -1.0362799738795865, + "angularVelocity": 4.542642020492645e-25, + "velocityX": -2.8387854876180527, + "velocityY": 3.0702817463856076, + "timestamp": 5.238072990428239 + }, + { + "x": 1.7730322630777298, + "y": 3.0210268959304614, + "heading": -1.0362799738795865, + "angularVelocity": 2.002705937424186e-25, + "velocityX": -2.533007078205092, + "velocityY": 3.116855220305981, + "timestamp": 5.310442868852725 + }, + { + "x": 1.606193927798652, + "y": 3.2314395516520142, + "heading": -1.0362799738795865, + "angularVelocity": 2.00271753679935e-25, + "velocityX": -2.3053560253408945, + "velocityY": 2.9074617824749667, + "timestamp": 5.382812747277211 + }, + { + "x": 1.4543761946227032, + "y": 3.425255597367628, + "heading": -1.0362799738795865, + "angularVelocity": 2.002717536795154e-25, + "velocityX": -2.0978027942158572, + "velocityY": 2.6781314261547466, + "timestamp": 5.455182625701697 + }, + { + "x": 1.317018366640586, + "y": 3.601984297874042, + "heading": -1.0362799738795865, + "angularVelocity": 2.0027175368278648e-25, + "velocityX": -1.8979972189043135, + "velocityY": 2.44202013812722, + "timestamp": 5.5275525041261835 + }, + { + "x": 1.1938248299897818, + "y": 3.761379815843054, + "heading": -1.0362799738795865, + "angularVelocity": 2.0027175368332326e-25, + "velocityX": -1.7022764074330956, + "velocityY": 2.2025118936096018, + "timestamp": 5.59992238255067 + }, + { + "x": 1.0846132178875603, + "y": 3.9032947091678634, + "heading": -1.0362799738795865, + "angularVelocity": 2.0027175368296866e-25, + "velocityX": -1.5090755225764045, + "velocityY": 1.960966308281009, + "timestamp": 5.672292260975156 + }, + { + "x": 0.9892598461227126, + "y": 4.027630760526036, + "heading": -1.0362799738795865, + "angularVelocity": 2.0027175368205547e-25, + "velocityX": -1.3175836942208314, + "velocityY": 1.7180635654640377, + "timestamp": 5.744662139399642 + }, + { + "x": 0.9076753302290267, + "y": 4.134317869708134, + "heading": -1.0362799738795865, + "angularVelocity": 2.0027175368262682e-25, + "velocityX": -1.1273269717982857, + "velocityY": 1.4741921847142516, + "timestamp": 5.817032017824128 + }, + { + "x": 0.8397920613678915, + "y": 4.223303498353317, + "heading": -1.0362799738795865, + "angularVelocity": 2.0027175368232324e-25, + "velocityX": -0.93800446178679, + "velocityY": 1.2295948339616842, + "timestamp": 5.889401896248614 + }, + { + "x": 0.7855571157672845, + "y": 4.2945468080907165, + "heading": -1.0362799738795865, + "angularVelocity": 2.0027175368176088e-25, + "velocityX": -0.7494132473526036, + "velocityY": 0.9844331825393212, + "timestamp": 5.9617717746731005 + }, + { + "x": 0.7449279402703854, + "y": 4.348015145319637, + "heading": -1.0362799738795865, + "angularVelocity": 2.0027175368190736e-25, + "velocityX": -0.5614100283351063, + "velocityY": 0.7388203268119757, + "timestamp": 6.034141653097587 + }, + { + "x": 0.7178695768260748, + "y": 4.383681805542776, + "heading": -1.0362799738795865, + "angularVelocity": 2.0027175368211496e-25, + "velocityX": -0.37388985629628485, + "velocityY": 0.49283847091654137, + "timestamp": 6.106511531522073 + }, + { + "x": 0.7043527960777283, + "y": 4.401524543762207, + "heading": -1.0362799738795865, + "angularVelocity": 2.0027175368141127e-25, + "velocityX": -0.18677357268811703, + "velocityY": 0.24654923578529575, + "timestamp": 6.178881409946559 + }, + { + "x": 0.7043527960777283, + "y": 4.401524543762207, + "heading": -1.0362799738795865, + "angularVelocity": 6.759494810993815e-26, + "velocityX": -5.770106844992042e-25, + "velocityY": -3.7765094249209375e-25, + "timestamp": 6.251251288371045 + }, + { + "x": 0.7167356642093481, + "y": 4.3880630372989495, + "heading": -1.0323294869077149, + "angularVelocity": 0.0603254224142757, + "velocityX": 0.18909105537091309, + "velocityY": -0.20556226852806772, + "timestamp": 6.3167375595610284 + }, + { + "x": 0.7415026145057688, + "y": 4.361138808128861, + "heading": -1.0245186649242746, + "angularVelocity": 0.11927419047543625, + "velocityX": 0.3782006494852768, + "velocityY": -0.41114310955310973, + "timestamp": 6.382223830751012 + }, + { + "x": 0.7786550164722235, + "y": 4.320750485584969, + "heading": -1.012951720288187, + "angularVelocity": 0.17663159660641478, + "velocityX": 0.5673311564598186, + "velocityY": -0.6167448811785445, + "timestamp": 6.447710101940995 + }, + { + "x": 0.8281944252622419, + "y": 4.266896512217522, + "heading": -0.9977506488970705, + "angularVelocity": 0.2321260794803288, + "velocityX": 0.7564854112138795, + "velocityY": -0.8223704356476602, + "timestamp": 6.513196373130978 + }, + { + "x": 0.8901226189866902, + "y": 4.199575103915774, + "heading": -0.9790603164668716, + "angularVelocity": 0.2854084083666306, + "velocityX": 0.945666818389875, + "velocityY": -1.0280232341591105, + "timestamp": 6.578682644320962 + }, + { + "x": 0.9644416446349294, + "y": 4.118784201180533, + "heading": -0.9570555981446492, + "angularVelocity": 0.336020327961321, + "velocityX": 1.1348794838635836, + "velocityY": -1.233707481998122, + "timestamp": 6.644168915510945 + }, + { + "x": 1.0511538736294015, + "y": 4.02452141118641, + "heading": -0.9319517389785453, + "angularVelocity": 0.3833453746858561, + "velocityX": 1.324128361850215, + "velocityY": -1.4394282691811102, + "timestamp": 6.709655186700928 + }, + { + "x": 1.150262065510173, + "y": 3.916783943927492, + "heading": -0.9040200178440649, + "angularVelocity": 0.42652789091391524, + "velocityX": 1.5134193790519348, + "velocityY": -1.6451916608041222, + "timestamp": 6.7751414578909115 + }, + { + "x": 1.26176942881864, + "y": 3.7955685577539087, + "heading": -0.8736126776022465, + "angularVelocity": 0.4643315261240514, + "velocityX": 1.7027593918879715, + "velocityY": -1.8510045536403001, + "timestamp": 6.840627729080895 + }, + { + "x": 1.3856796349802256, + "y": 3.66087156928865, + "heading": -0.8412052975881391, + "angularVelocity": 0.4948728859532958, + "velocityX": 1.8921554687715691, + "velocityY": -2.0568736930292832, + "timestamp": 6.906114000270878 + }, + { + "x": 1.521996617397842, + "y": 3.5126891243586433, + "heading": -0.8074753577540704, + "angularVelocity": 0.5150688720115713, + "velocityX": 2.0816116101975704, + "velocityY": -2.2628016870912, + "timestamp": 6.971600271460861 + }, + { + "x": 1.6707234642510467, + "y": 3.3510185124370526, + "heading": -0.7734667417988786, + "angularVelocity": 0.5193243612318167, + "velocityX": 2.2711149092873253, + "velocityY": -2.468771071917722, + "timestamp": 7.037086542650845 + }, + { + "x": 1.8318569074079185, + "y": 3.1758644206376023, + "heading": -0.741004452502463, + "angularVelocity": 0.4957113713535259, + "velocityX": 2.460568302773024, + "velocityY": -2.6746688827541845, + "timestamp": 7.102572813840828 + }, + { + "x": 2.005350740806108, + "y": 2.9872805245573906, + "heading": -0.7141541604245535, + "angularVelocity": 0.4100140623370287, + "velocityX": 2.6493161123018125, + "velocityY": -2.879747047027718, + "timestamp": 7.168059085030811 + }, + { + "x": 2.190424030896748, + "y": 2.7863157366197826, + "heading": -0.7119881829198739, + "angularVelocity": 0.03307529143621359, + "velocityX": 2.8261387727165057, + "velocityY": -3.0688079239476664, + "timestamp": 7.2335453562207945 + }, + { + "x": 2.3763825220142154, + "y": 2.5853069329321596, + "heading": -0.7119881669601573, + "angularVelocity": 2.4371087893295916e-7, + "velocityX": 2.8396561254492445, + "velocityY": -3.0694800610111423, + "timestamp": 7.299031627410778 + }, + { + "x": 2.5753608002161195, + "y": 2.397177338599563, + "heading": -0.7119881508365082, + "angularVelocity": 2.4621418712778765e-7, + "velocityX": 3.0384731728677012, + "velocityY": -2.872809688412548, + "timestamp": 7.364517898600761 + }, + { + "x": 2.7864781410109636, + "y": 2.222779610597699, + "heading": -0.71198813425094, + "angularVelocity": 2.5326786554372286e-7, + "velocityX": 3.2238412259321843, + "velocityY": -2.663118922986393, + "timestamp": 7.430004169790744 + }, + { + "x": 3.0087889994109913, + "y": 2.0628953375517414, + "heading": -0.7119881168686832, + "angularVelocity": 2.654336029118244e-7, + "velocityX": 3.3947704512763814, + "velocityY": -2.4414930051844577, + "timestamp": 7.495490440980728 + }, + { + "x": 3.2412974577495883, + "y": 1.918240900042732, + "heading": -0.7119880410502188, + "angularVelocity": 0.000001157776478274293, + "velocityX": 3.55049163914161, + "velocityY": -2.2089276863748983, + "timestamp": 7.560976712170711 + }, + { + "x": 3.479287995502474, + "y": 1.7906960053033385, + "heading": -0.6997868981239171, + "angularVelocity": 0.18631604311237665, + "velocityX": 3.6342050544066913, + "velocityY": -1.9476585308907035, + "timestamp": 7.626462983360694 + }, + { + "x": 3.7169780040060765, + "y": 1.6799303664576555, + "heading": -0.6633844580774498, + "angularVelocity": 0.5558789557716548, + "velocityX": 3.6296158596972896, + "velocityY": -1.6914329802706074, + "timestamp": 7.691949254550678 + }, + { + "x": 3.952295303344726, + "y": 1.5853265047073366, + "heading": -0.6, + "angularVelocity": 0.9679045229734358, + "velocityX": 3.5933836980268095, + "velocityY": -1.444636563225013, + "timestamp": 7.757435525740661 + }, + { + "x": 4.074112481863887, + "y": 1.5402828586317625, + "heading": -0.5592952549143817, + "angularVelocity": 1.1918777883706093, + "velocityX": 3.5669352311031557, + "velocityY": -1.3189253771711302, + "timestamp": 7.791587303282606 + }, + { + "x": 4.195092381666415, + "y": 1.49959651079917, + "heading": -0.5111868480910893, + "angularVelocity": 1.408664798317049, + "velocityX": 3.5424188288279144, + "velocityY": -1.1913390974341513, + "timestamp": 7.825739080824551 + }, + { + "x": 4.31528397568399, + "y": 1.4633173395591519, + "heading": -0.45584958746005705, + "angularVelocity": 1.6203332480444814, + "velocityX": 3.5193364055489167, + "velocityY": -1.0622923271112426, + "timestamp": 7.859890858366496 + }, + { + "x": 4.434745610407602, + "y": 1.4314952831678345, + "heading": -0.39345071655264674, + "angularVelocity": 1.8271046311066104, + "velocityX": 3.4979624289509004, + "velocityY": -0.931783312076039, + "timestamp": 7.894042635908441 + }, + { + "x": 4.553564809111248, + "y": 1.4041887809787912, + "heading": -0.32420952229913425, + "angularVelocity": 2.0274550619940936, + "velocityX": 3.479151225956343, + "velocityY": -0.799563131245678, + "timestamp": 7.928194413450386 + }, + { + "x": 4.671875409339677, + "y": 1.3814688601488554, + "heading": -0.2484642063556595, + "angularVelocity": 2.2179025923451543, + "velocityX": 3.4642589271706443, + "velocityY": -0.6652632004888026, + "timestamp": 7.962346190992331 + }, + { + "x": 4.789867622790203, + "y": 1.3634163593108408, + "heading": -0.1667309654451272, + "angularVelocity": 2.3932353392191157, + "velocityX": 3.4549362271292985, + "velocityY": -0.5285962294595843, + "timestamp": 7.9964979685342765 + }, + { + "x": 4.9072638759133165, + "y": 1.3502270995004428, + "heading": -0.08189101561832554, + "angularVelocity": 2.484203046901497, + "velocityX": 3.4374858813403817, + "velocityY": -0.38619541235295707, + "timestamp": 8.030649746076222 + }, + { + "x": 5.023862752348719, + "y": 1.3419409683639283, + "heading": 0.004341393754814386, + "angularVelocity": 2.5249757283418135, + "velocityX": 3.414137852479163, + "velocityY": -0.2426266429715844, + "timestamp": 8.064801523618167 + }, + { + "x": 5.139798735192581, + "y": 1.3385933374859216, + "heading": 0.09105898183003379, + "angularVelocity": 2.5391822715146737, + "velocityX": 3.394727630252062, + "velocityY": -0.09802215635467636, + "timestamp": 8.098953301160112 + }, + { + "x": 5.255334333875942, + "y": 1.3402095329646513, + "heading": 0.17703367140947002, + "angularVelocity": 2.5174294214654838, + "velocityX": 3.3830039605247926, + "velocityY": 0.04732390508062842, + "timestamp": 8.133105078702057 + }, + { + "x": 5.370884716650611, + "y": 1.3467609864460404, + "heading": 0.26064984374400246, + "angularVelocity": 2.4483695535857777, + "velocityX": 3.383436854282362, + "velocityY": 0.19183345503298238, + "timestamp": 8.167256856244002 + }, + { + "x": 5.486974245992485, + "y": 1.3581084639316627, + "heading": 0.3400681433878616, + "angularVelocity": 2.3254514218569815, + "velocityX": 3.3992236333612103, + "velocityY": 0.33226608693165577, + "timestamp": 8.201408633785947 + }, + { + "x": 5.604095520081233, + "y": 1.3740075206335363, + "heading": 0.4136873978805543, + "angularVelocity": 2.1556492748371334, + "velocityX": 3.429434205727671, + "velocityY": 0.46554111809689713, + "timestamp": 8.235560411327892 + }, + { + "x": 5.722741961380415, + "y": 1.394099394895898, + "heading": 0.4800808054598774, + "angularVelocity": 1.94406887014239, + "velocityX": 3.474092707281825, + "velocityY": 0.5883112302920394, + "timestamp": 8.269712188869837 + }, + { + "x": 5.843764195041286, + "y": 1.4176581219817168, + "heading": 0.5371133918016127, + "angularVelocity": 1.6699741696223076, + "velocityX": 3.5436584087675294, + "velocityY": 0.6898243307214182, + "timestamp": 8.303863966411782 + }, + { + "x": 5.967478132998898, + "y": 1.4443969152926162, + "heading": 0.5841910806451285, + "angularVelocity": 1.3784842907721413, + "velocityX": 3.6224743442904956, + "velocityY": 0.7829400176333111, + "timestamp": 8.338015743953727 + }, + { + "x": 6.094050696019468, + "y": 1.474151537150211, + "heading": 0.6210751522017017, + "angularVelocity": 1.080004445193888, + "velocityX": 3.7061778955755575, + "velocityY": 0.871246652419496, + "timestamp": 8.372167521495673 + }, + { + "x": 6.223075829158687, + "y": 1.5062918435411075, + "heading": 0.6435004368303741, + "angularVelocity": 0.6566359423350606, + "velocityX": 3.7779917306134942, + "velocityY": 0.9411020071040859, + "timestamp": 8.406319299037618 + }, + { + "x": 6.352611541748047, + "y": 1.5393844842910769, + "heading": 0.6435004368303741, + "angularVelocity": -1.4718933529794898e-27, + "velocityX": 3.7929420344304243, + "velocityY": 0.9689873597157704, + "timestamp": 8.440471076579563 + }, + { + "x": 6.580062653914693, + "y": 1.6130439412495936, + "heading": 0.6435004368303741, + "angularVelocity": 1.0474807729513566e-29, + "velocityX": 3.7848236766813983, + "velocityY": 1.22570540127247, + "timestamp": 8.500566640229724 + }, + { + "x": 6.79440770982544, + "y": 1.694856967245299, + "heading": 0.6435004368303741, + "angularVelocity": -1.650371396897613e-30, + "velocityX": 3.566736758781869, + "velocityY": 1.3613821225135523, + "timestamp": 8.560662203879886 + }, + { + "x": 6.993347493866424, + "y": 1.7776330405952192, + "heading": 0.6435004368303741, + "angularVelocity": 1.3963817165306325e-30, + "velocityX": 3.310390517328132, + "velocityY": 1.3774073878695936, + "timestamp": 8.620757767530048 + }, + { + "x": 7.176977880245271, + "y": 1.8584412636517398, + "heading": 0.6435004368303741, + "angularVelocity": -1.580328410175052e-30, + "velocityX": 3.0556396383571194, + "velocityY": 1.3446620373998812, + "timestamp": 8.68085333118021 + }, + { + "x": 7.345549173736612, + "y": 1.9358621673435095, + "heading": 0.6435004368303741, + "angularVelocity": 2.4054088974712413e-30, + "velocityX": 2.805053871740944, + "velocityY": 1.2882964896121982, + "timestamp": 8.740948894830371 + }, + { + "x": 7.49926861078026, + "y": 2.009079186830037, + "heading": 0.6435004368303741, + "angularVelocity": 2.3808121740418405e-30, + "velocityX": 2.55791655335002, + "velocityY": 1.2183431694351197, + "timestamp": 8.801044458480533 + }, + { + "x": 7.6382952437083365, + "y": 2.0775663187372784, + "heading": 0.6435004368303741, + "angularVelocity": 2.380805137763221e-30, + "velocityX": 2.3134258917580244, + "velocityY": 1.139637067154071, + "timestamp": 8.861140022130694 + }, + { + "x": 7.762751887009844, + "y": 2.1409577892840943, + "heading": 0.6435004368303741, + "angularVelocity": 2.3808050924755408e-30, + "velocityX": 2.070978883333475, + "velocityY": 1.0548444293798613, + "timestamp": 8.921235585780856 + }, + { + "x": 7.872735273758024, + "y": 2.198985004826962, + "heading": 0.6435004368303741, + "angularVelocity": 2.380805092475582e-30, + "velocityX": 1.8301415290558403, + "velocityY": 0.9655823494836527, + "timestamp": 8.981331149431018 + }, + { + "x": 7.968323195692331, + "y": 2.2514425705825416, + "heading": 0.6435004368303741, + "angularVelocity": 2.380807288621953e-30, + "velocityX": 1.5905986420355245, + "velocityY": 0.872902466826909, + "timestamp": 9.04142671308118 + }, + { + "x": 8.049579405713123, + "y": 2.2981684302536656, + "heading": 0.6435004368303741, + "angularVelocity": 2.380808241833061e-30, + "velocityX": 1.3521166136957175, + "velocityY": 0.7775259409019353, + "timestamp": 9.101522276731341 + }, + { + "x": 8.116557022491735, + "y": 2.339031508309391, + "heading": 0.6435004368303741, + "angularVelocity": 2.6392759987141426e-30, + "velocityX": 1.1145184887275905, + "velocityY": 0.6799682967216062, + "timestamp": 9.161617840381503 + }, + { + "x": 8.169300943138342, + "y": 2.3739236285016685, + "heading": 0.6435004368303741, + "angularVelocity": 2.3804976901317442e-30, + "velocityX": 0.8776674590099443, + "velocityY": 0.5806105820954999, + "timestamp": 9.221713404031664 + }, + { + "x": 8.207849590345619, + "y": 2.4027540109969827, + "heading": 0.6435004368303741, + "angularVelocity": 2.3808070805983946e-30, + "velocityX": 0.6414557891774174, + "velocityY": 0.47974227620438903, + "timestamp": 9.281808967681826 + }, + { + "x": 8.232236204057223, + "y": 2.4254453979327417, + "heading": 0.6435004368303741, + "angularVelocity": 2.3808068323507875e-30, + "velocityX": 0.4057972374394851, + "velocityY": 0.3775883868542062, + "timestamp": 9.341904531331988 + }, + { + "x": 8.2424898147583, + "y": 2.4419312477111816, + "heading": 0.6435004368303741, + "angularVelocity": -3.2178293818866526e-30, + "velocityX": 0.1706217577185593, + "velocityY": 0.27432723444296603, + "timestamp": 9.40200009498215 + }, + { + "x": 8.2393017204798, + "y": 2.4522166219660964, + "heading": 0.6319049962965602, + "angularVelocity": -0.1961915039244878, + "velocityX": -0.05394163415594769, + "velocityY": 0.1740255609619532, + "timestamp": 9.461102758970478 + }, + { + "x": 8.222821560618591, + "y": 2.4565750369075374, + "heading": 0.608972533665731, + "angularVelocity": -0.3880106425550933, + "velocityX": -0.27883954375497194, + "velocityY": 0.0737431216687946, + "timestamp": 9.520205422958806 + }, + { + "x": 8.19302770545567, + "y": 2.4550084997233315, + "heading": 0.5749872344457952, + "angularVelocity": -0.5750214444927056, + "velocityX": -0.5041034219507514, + "velocityY": -0.02650535658621403, + "timestamp": 9.579308086947135 + }, + { + "x": 8.149895912141313, + "y": 2.447519382114308, + "heading": 0.5302786937880936, + "angularVelocity": -0.7564555916892447, + "velocityX": -0.7297774821600979, + "velocityY": -0.12671370634836995, + "timestamp": 9.638410750935464 + }, + { + "x": 8.09339864804559, + "y": 2.4341097073401863, + "heading": 0.4752444136142709, + "angularVelocity": -0.9311641212093363, + "velocityX": -0.9559173865137355, + "velocityY": -0.22688782314059242, + "timestamp": 9.697513414923792 + }, + { + "x": 8.023504638887696, + "y": 2.4147802557579743, + "heading": 0.41037330333687494, + "angularVelocity": -1.0976004447144125, + "velocityX": -1.1825864426634942, + "velocityY": -0.32704873651768396, + "timestamp": 9.75661607891212 + }, + { + "x": 7.940178760571984, + "y": 2.3895296850369747, + "heading": 0.33626751416330763, + "angularVelocity": -1.2538485437509457, + "velocityX": -1.4098497883643064, + "velocityY": -0.4272323617423738, + "timestamp": 9.815718742900449 + }, + { + "x": 7.843382276245969, + "y": 2.358354061171314, + "heading": 0.2536606944782709, + "angularVelocity": -1.3976835240683847, + "velocityX": -1.6377685504181283, + "velocityY": -0.5274825492099173, + "timestamp": 9.874821406888778 + }, + { + "x": 7.733073216109231, + "y": 2.321247319783134, + "heading": 0.16343500685588275, + "angularVelocity": -1.526592568487364, + "velocityX": -1.8663974293700505, + "velocityY": -0.6278353441988297, + "timestamp": 9.933924070877106 + }, + { + "x": 7.609206539592904, + "y": 2.2782030815405587, + "heading": 0.06664871856515628, + "angularVelocity": -1.6375960364466748, + "velocityX": -2.095788381735004, + "velocityY": -0.7282960756400965, + "timestamp": 9.993026734865435 + }, + { + "x": 7.471733974955729, + "y": 2.229217918807126, + "heading": -0.03539885417690381, + "angularVelocity": -1.7266154493850245, + "velocityX": -2.325996078016428, + "velocityY": -0.8288148017000726, + "timestamp": 10.052129398853763 + }, + { + "x": 7.320604827398418, + "y": 2.174296101814293, + "heading": -0.14101890697055008, + "angularVelocity": -1.787060779773037, + "velocityX": -2.5570615156561423, + "velocityY": -0.9292612766774592, + "timestamp": 10.111232062842092 + }, + { + "x": 7.155773189909133, + "y": 2.113457738944985, + "heading": -0.24782868572534286, + "angularVelocity": -1.8071905993253574, + "velocityX": -2.7889036866736787, + "velocityY": -1.0293675236250293, + "timestamp": 10.17033472683042 + }, + { + "x": 6.977229587916178, + "y": 2.046760806821276, + "heading": -0.35216325005396515, + "angularVelocity": -1.7653106863207704, + "velocityX": -3.0209061647070943, + "velocityY": -1.1284928228764857, + "timestamp": 10.229437390818749 + }, + { + "x": 6.785127223955647, + "y": 1.9743718586500072, + "heading": -0.44791637926652805, + "angularVelocity": -1.620115283322456, + "velocityX": -3.2503165000898613, + "velocityY": -1.224800089985181, + "timestamp": 10.288540054807077 + }, + { + "x": 6.580482239369349, + "y": 1.8965267551833909, + "heading": -0.523907298024649, + "angularVelocity": -1.285744391710116, + "velocityX": -3.4625340175310977, + "velocityY": -1.3171166613063143, + "timestamp": 10.347642718795406 + }, + { + "x": 6.365568212552411, + "y": 1.8126473637838463, + "heading": -0.5685150089578763, + "angularVelocity": -0.7547495818807144, + "velocityX": -3.636283245360637, + "velocityY": -1.4192150698335522, + "timestamp": 10.406745382783734 + }, + { + "x": 6.1391542252718745, + "y": 1.727320392022171, + "heading": -0.5825906454165231, + "angularVelocity": -0.23815570244729414, + "velocityX": -3.8308592540811275, + "velocityY": -1.4437077113567243, + "timestamp": 10.465848046772063 + }, + { + "x": 5.903469838213687, + "y": 1.6529470737325385, + "heading": -0.5825907170739721, + "angularVelocity": -0.0000012124233368564011, + "velocityX": -3.987711740112587, + "velocityY": -1.2583750591059566, + "timestamp": 10.524950710760391 + }, + { + "x": 5.663724751126659, + "y": 1.5929402727768625, + "heading": -0.5825907737674252, + "angularVelocity": -9.592368472200333e-7, + "velocityX": -4.056417611469635, + "velocityY": -1.0152977362835247, + "timestamp": 10.58405337474872 + }, + { + "x": 5.4207938290604085, + "y": 1.547519030326998, + "heading": -0.5825908335297906, + "angularVelocity": -0.0000010111619577208565, + "velocityX": -4.110321018934508, + "velocityY": -0.7685143001140271, + "timestamp": 10.643156038737049 + }, + { + "x": 5.175563573210271, + "y": 1.5168491316802175, + "heading": -0.582590897808758, + "angularVelocity": -0.0000010875815561180186, + "velocityX": -4.149225082283343, + "velocityY": -0.5189258246098157, + "timestamp": 10.702258702725377 + }, + { + "x": 4.928928882781426, + "y": 1.5010425173093036, + "heading": -0.582590968507317, + "angularVelocity": -0.0000011961991942253073, + "velocityX": -4.172987709615762, + "velocityY": -0.26744334864559327, + "timestamp": 10.761361366713706 + }, + { + "x": 4.6817897858705155, + "y": 1.500156880852723, + "heading": -0.5825910482884903, + "angularVelocity": -0.000001349874404780368, + "velocityX": -4.181522121569922, + "velocityY": -0.014984712986125005, + "timestamp": 10.820464030702034 + }, + { + "x": 4.435048362369104, + "y": 1.5141954560145352, + "heading": -0.5825917399367928, + "angularVelocity": -0.000011702489461728115, + "velocityX": -4.174793602368545, + "velocityY": 0.23752863601181112, + "timestamp": 10.879566694690363 + }, + { + "x": 4.191238550201924, + "y": 1.5428059629228041, + "heading": -0.5872689519449389, + "angularVelocity": -0.07913707593738419, + "velocityX": -4.125191585532014, + "velocityY": 0.48408151134979194, + "timestamp": 10.938669358678691 + }, + { + "x": 3.952295303344726, + "y": 1.5853265047073366, + "heading": -0.6, + "angularVelocity": -0.21540565510845927, + "velocityX": -4.042850706431503, + "velocityY": 0.7194352828652425, + "timestamp": 10.99777202266702 + }, + { + "x": 3.8134740215452583, + "y": 1.615040134332196, + "heading": -0.6109765742860589, + "angularVelocity": -0.31488494666214406, + "velocityX": -3.9823656065913373, + "velocityY": 0.8523947850875385, + "timestamp": 11.03263102244587 + }, + { + "x": 3.6768690053953224, + "y": 1.6493458750794194, + "heading": -0.6252964897864377, + "angularVelocity": -0.41079536392972205, + "velocityX": -3.918787601955746, + "velocityY": 0.9841286601699233, + "timestamp": 11.06749002222472 + }, + { + "x": 3.542588683535768, + "y": 1.688198377556308, + "heading": -0.6428198278038513, + "angularVelocity": -0.5026919340366556, + "velocityX": -3.852099105294062, + "velocityY": 1.1145615973887317, + "timestamp": 11.102349022003569 + }, + { + "x": 3.4107408609461936, + "y": 1.7315503308123263, + "heading": -0.6633872651355344, + "angularVelocity": -0.590018000004748, + "velocityX": -3.782318007574331, + "velocityY": 1.2436373255414277, + "timestamp": 11.137208021782419 + }, + { + "x": 3.2814309719594807, + "y": 1.7793535620510226, + "heading": -0.6868149980921183, + "angularVelocity": -0.6720712902037491, + "velocityX": -3.709512315530314, + "velocityY": 1.3713311208573407, + "timestamp": 11.172067021561269 + }, + { + "x": 3.154759713968678, + "y": 1.8315607122204742, + "heading": -0.7128878595460402, + "angularVelocity": -0.7479520817961316, + "velocityX": -3.633817917737781, + "velocityY": 1.497666327222826, + "timestamp": 11.206926021340118 + }, + { + "x": 3.0308199060377405, + "y": 1.8881276343253246, + "heading": -0.7413498638418092, + "angularVelocity": -0.8164894138195471, + "velocityX": -3.5554608197948028, + "velocityY": 1.6227350888929106, + "timestamp": 11.241785021118968 + }, + { + "x": 2.909692349791607, + "y": 1.9490166364375032, + "heading": -0.7718912775944233, + "angularVelocity": -0.8761414253527852, + "velocityX": -3.474785765930811, + "velocityY": 1.7467225823593937, + "timestamp": 11.276644020897818 + }, + { + "x": 2.791440373924634, + "y": 2.0142005993273084, + "heading": -0.8041312385312992, + "angularVelocity": -0.9248676422562412, + "velocityX": -3.392294002041893, + "velocityY": 1.8699321065819634, + "timestamp": 11.311503020676668 + }, + { + "x": 2.6761026118014377, + "y": 2.0836677838154385, + "heading": -0.8375950488175486, + "angularVelocity": -0.9599762040950126, + "velocityX": -3.3086939629626064, + "velocityY": 1.992804869010546, + "timestamp": 11.346362020455517 + }, + { + "x": 2.563683426803659, + "y": 2.157426720429071, + "heading": -0.8716858609029794, + "angularVelocity": -0.9779630024299947, + "velocityX": -3.2249687515700507, + "velocityY": 2.1159223466412884, + "timestamp": 11.381221020234367 + }, + { + "x": 2.4541405159302982, + "y": 2.2355097870550096, + "heading": -0.9056525586852363, + "angularVelocity": -0.9744025358658038, + "velocityX": -3.1424570862134558, + "velocityY": 2.239968648593113, + "timestamp": 11.416080020013217 + }, + { + "x": 2.3473705926939137, + "y": 2.317972680271922, + "heading": -0.9385621397982856, + "angularVelocity": -0.9440770338171433, + "velocityX": -3.062908399946827, + "velocityY": 2.365612718094827, + "timestamp": 11.450939019792067 + }, + { + "x": 2.2431987205449455, + "y": 2.404885409895606, + "heading": -0.9692995356278491, + "angularVelocity": -0.8817635624821536, + "velocityX": -2.988378117841816, + "velocityY": 2.493265158927951, + "timestamp": 11.485798019570916 + }, + { + "x": 2.141383918660014, + "y": 2.496312439027103, + "heading": -0.996629601543935, + "angularVelocity": -0.7840175016343364, + "velocityX": -2.9207608517415142, + "velocityY": 2.6227668525064347, + "timestamp": 11.520657019349766 + }, + { + "x": 2.041650396130837, + "y": 2.5922897101709004, + "heading": -1.0193236199557545, + "angularVelocity": -0.651023223723952, + "velocityX": -2.861055198425048, + "velocityY": 2.7532996285805456, + "timestamp": 11.555516019128616 + }, + { + "x": 1.943731375644481, + "y": 2.6928135229166212, + "heading": -1.0362799738795865, + "angularVelocity": -0.48642686340414926, + "velocityX": -2.809002584915431, + "velocityY": 2.8837262509956236, + "timestamp": 11.590375018907466 + }, + { + "x": 1.8480632305145264, + "y": 2.7954604625701904, + "heading": -1.0362799738795865, + "angularVelocity": 2.1736687540126838e-29, + "velocityX": -2.7444317317446116, + "velocityY": 2.9446323848869578, + "timestamp": 11.625234018686315 + }, + { + "x": 1.672640445377056, + "y": 3.013300327645549, + "heading": -1.0362799738795865, + "angularVelocity": -2.795107901873647e-30, + "velocityX": -2.460711972616185, + "velocityY": 3.0557100303929148, + "timestamp": 11.696523461371866 + }, + { + "x": 1.5164302211322118, + "y": 3.221006881583978, + "heading": -1.0362799738795865, + "angularVelocity": 8.418621207211218e-30, + "velocityX": -2.1912111858395145, + "velocityY": 2.913566807565, + "timestamp": 11.767812904057417 + }, + { + "x": 1.3764341718321418, + "y": 3.4142598003253495, + "heading": -1.0362799738795865, + "angularVelocity": 4.531432261764684e-30, + "velocityX": -1.9637697255900248, + "velocityY": 2.7108210060469538, + "timestamp": 11.839102346742967 + }, + { + "x": 1.2511473564025137, + "y": 3.591530049792972, + "heading": -1.0362799738795865, + "angularVelocity": 4.531432118047956e-30, + "velocityX": -1.7574385590620065, + "velocityY": 2.4866269504944793, + "timestamp": 11.910391789428518 + }, + { + "x": 1.1397098089961486, + "y": 3.7520669438694583, + "heading": -1.0362799738795865, + "angularVelocity": 4.5314299990799685e-30, + "velocityX": -1.5631704107703956, + "velocityY": 2.251902778712901, + "timestamp": 11.981681232114068 + }, + { + "x": 1.0415712767381338, + "y": 3.895429913411281, + "heading": -1.0362799738795865, + "angularVelocity": 4.5314368609894865e-30, + "velocityX": -1.3766208369855277, + "velocityY": 2.010998601492499, + "timestamp": 12.052970674799619 + }, + { + "x": 0.9563509320037674, + "y": 4.021330695987133, + "heading": -1.0362799738795865, + "angularVelocity": 4.531437736881817e-30, + "velocityX": -1.1954132550911205, + "velocityY": 1.7660508741972465, + "timestamp": 12.12426011748517 + }, + { + "x": 0.8837700445121739, + "y": 4.129566493346561, + "heading": -1.0362799738795865, + "angularVelocity": 4.531436969781661e-30, + "velocityX": -1.0181155127237986, + "velocityY": 1.5182584304501188, + "timestamp": 12.19554956017072 + }, + { + "x": 0.8236159650487832, + "y": 4.219987070970065, + "heading": -1.0362799738795865, + "angularVelocity": 4.531436969781641e-30, + "velocityX": -0.8438006694584916, + "velocityY": 1.2683585986544863, + "timestamp": 12.26683900285627 + }, + { + "x": 0.7757211998785677, + "y": 4.29247675769312, + "heading": -1.0362799738795865, + "angularVelocity": 4.531437053865754e-30, + "velocityX": -0.6718353148231706, + "velocityY": 1.0168362101356112, + "timestamp": 12.338128445541821 + }, + { + "x": 0.7399504493458321, + "y": 4.346943794130791, + "heading": -1.0362799738795865, + "angularVelocity": 4.5314376706268254e-30, + "velocityX": -0.501767852085982, + "velocityY": 0.7640266831362128, + "timestamp": 12.409417888227372 + }, + { + "x": 0.7161921614034822, + "y": 4.383313637078767, + "heading": -1.0362799738795865, + "angularVelocity": 4.531437521414341e-30, + "velocityX": -0.33326516588360594, + "velocityY": 0.5101715145733307, + "timestamp": 12.480707330912923 + }, + { + "x": 0.7043527960777284, + "y": 4.401524543762207, + "heading": -1.0362799738795865, + "angularVelocity": 4.531437376556036e-30, + "velocityX": -0.16607459505576197, + "velocityY": 0.25545026019864153, + "timestamp": 12.551996773598473 + }, + { + "x": 0.7043527960777284, + "y": 4.401524543762207, + "heading": -1.0362799738795865, + "angularVelocity": 1.5152982881175701e-30, + "velocityX": -8.315280281271062e-29, + "velocityY": 1.901615622375989e-28, + "timestamp": 12.623286216284024 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.7043527960777283, + "y": 4.401524543762207, + "heading": -1.03622, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "timestamp": 1.537512195642688, + "isStopPoint": false, + "x": 3.952295303344726, + "y": 1.2853265047073366, + "heading": -0.6, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "timestamp": 2.176953315426481, + "isStopPoint": false, + "x": 6.344450855255133, + "y": 0.6604639410972595, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "timestamp": 3.061199522549858, + "isStopPoint": false, + "x": 8.144450855255126, + "y": 0.6604639410972595, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "timestamp": 4.602532743133479, + "isStopPoint": false, + "x": 3.952295303344726, + "y": 1.2853265047073366, + "heading": -0.6, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "timestamp": 5.238072990428239, + "isStopPoint": false, + "x": 1.9563456773757935, + "y": 2.7954604625701904, + "heading": -1.0362799738795863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 14 + }, + { + "timestamp": 6.251251288371045, + "isStopPoint": true, + "x": 0.7043527960777283, + "y": 4.401524543762207, + "heading": -1.0362799738795863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "timestamp": 7.757435525740661, + "isStopPoint": false, + "x": 3.952295303344726, + "y": 1.5853265047073366, + "heading": -0.6, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "timestamp": 8.440471076579563, + "isStopPoint": false, + "x": 6.352611541748047, + "y": 1.5393844842910767, + "heading": 0.305878931297, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 16 + }, + { + "timestamp": 9.40200009498215, + "isStopPoint": false, + "x": 8.2424898147583, + "y": 2.4419312477111816, + "heading": 0.6435004368303741, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 27 + }, + { + "timestamp": 10.99777202266702, + "isStopPoint": false, + "x": 3.952295303344726, + "y": 1.5853265047073366, + "heading": -0.6, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "timestamp": 11.625234018686315, + "isStopPoint": false, + "x": 1.8480632305145264, + "y": 2.7954604625701904, + "heading": -1.0362799738795863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 14 + }, + { + "timestamp": 12.623286216284024, + "isStopPoint": true, + "x": 0.7043527960777284, + "y": 4.401524543762207, + "heading": -1.0362799738795863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 6 + ], + "type": "StopPoint" + }, + { + "scope": [ + 2, + 3 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 5, + 6 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 8, + 9 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 11, + 12 + ], + "type": "ZeroAngularVelocity" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": true + }, + "Distance Source 4": { + "waypoints": [ + { + "x": 0.6802782416343689, + "y": 4.38139533996582, + "heading": -1.0378726274695866, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "x": 3.517277717590332, + "y": 1.2412056922912598, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "x": 7.967684268951416, + "y": 0.8080758452415466, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 4.870807647705078, + "y": 1.8692435026168823, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 3.1599457263946533, + "y": 3, + "heading": -0.7431853071795862, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 4.870807647705078, + "y": 1.8692435026168823, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 6.3759331703186035, + "y": 1.8584154844284058, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "x": 7.924371719360352, + "y": 2.432312250137329, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "x": 6.3759331703186035, + "y": 1.8584154844284058, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 4.870807647705078, + "y": 1.8692435026168823, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 3.1599459648132324, + "y": 3, + "heading": -0.7431853071795862, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 4.5134758949279785, + "y": 3.504307985305786, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 5.336422443389893, + "y": 4.143174171447754, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 7.924371719360352, + "y": 4.143174171447754, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 4.5134758949279785, + "y": 3.504307985305786, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 3.959945964813233, + "y": 2.5999999999999996, + "heading": -0.7431853071795862, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "x": 3.1599459648132324, + "y": 3, + "heading": -0.7431853071795862, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.6802782416343689, + "y": 4.38139533996582, + "heading": -1.0378726274695866, + "angularVelocity": -3.347916590381317e-25, + "velocityX": -7.79504417865945e-25, + "velocityY": 1.5644415406561792e-24, + "timestamp": 0 + }, + { + "x": 0.6928709180148234, + "y": 4.366129172079192, + "heading": -1.0148952648842788, + "angularVelocity": 0.39308459587811256, + "velocityX": 0.2154288634153295, + "velocityY": -0.2611655455252246, + "timestamp": 0.05845398885188716 + }, + { + "x": 0.7181419130531553, + "y": 4.335559528214801, + "heading": -0.9698560026458198, + "angularVelocity": 0.7705079349260691, + "velocityX": 0.43232284972655155, + "velocityY": -0.5229693381892135, + "timestamp": 0.11690797770377433 + }, + { + "x": 0.7561908960261392, + "y": 4.2896399224059945, + "heading": -0.9039183675220187, + "angularVelocity": 1.128026271926048, + "velocityX": 0.6509219254376942, + "velocityY": -0.7855683882439376, + "timestamp": 0.1753619665556615 + }, + { + "x": 0.8071292746845115, + "y": 4.228302195171338, + "heading": -0.8186581069281054, + "angularVelocity": 1.4585875535363195, + "velocityX": 0.8714269061678875, + "velocityY": -1.0493334747450136, + "timestamp": 0.23381595540754865 + }, + { + "x": 0.871072837229944, + "y": 4.15144639527446, + "heading": -0.7161964133960428, + "angularVelocity": 1.7528605924855503, + "velocityX": 1.0939127303605414, + "velocityY": -1.314808474261995, + "timestamp": 0.2922699442594358 + }, + { + "x": 0.9481336446587337, + "y": 4.0589407507578334, + "heading": -0.5992730958902776, + "angularVelocity": 2.00026242523893, + "velocityX": 1.3183156349526337, + "velocityY": -1.58253775890335, + "timestamp": 0.350723933111323 + }, + { + "x": 1.03842141694975, + "y": 3.9506371946873022, + "heading": -0.47137491438003465, + "angularVelocity": 2.188014608110252, + "velocityX": 1.544595571053174, + "velocityY": -1.852800094531693, + "timestamp": 0.4091779219632102 + }, + { + "x": 1.1420524474939961, + "y": 3.8264005587076344, + "heading": -0.33738265270345347, + "angularVelocity": 2.2922689162598577, + "velocityX": 1.7728649931287002, + "velocityY": -2.1253748190643313, + "timestamp": 0.46763191081509736 + }, + { + "x": 1.2590920368315979, + "y": 3.6861968307243496, + "heading": -0.20574611612623897, + "angularVelocity": 2.2519684141788834, + "velocityX": 2.0022515423910754, + "velocityY": -2.3985314045639927, + "timestamp": 0.5260858996669845 + }, + { + "x": 1.3890539557520782, + "y": 3.530716769695316, + "heading": -0.09362413816745035, + "angularVelocity": 1.9181236415343244, + "velocityX": 2.2233199388630713, + "velocityY": -2.659870850268142, + "timestamp": 0.5845398885188716 + }, + { + "x": 1.526819292676983, + "y": 3.3606093766178162, + "heading": -0.02492102014455636, + "angularVelocity": 1.1753366942498389, + "velocityX": 2.356816696872128, + "velocityY": -2.9101075293342884, + "timestamp": 0.6429938773707587 + }, + { + "x": 1.6725539368788018, + "y": 3.1759976053864527, + "heading": -8.429679741259334e-7, + "angularVelocity": 0.4263212428449645, + "velocityX": 2.493151401029047, + "velocityY": -3.158240778044072, + "timestamp": 0.7014478662226459 + }, + { + "x": 1.8287790494270222, + "y": 2.98801124535308, + "heading": -7.696371008390055e-7, + "angularVelocity": 0.0000012545058896277584, + "velocityX": 2.6726167985570535, + "velocityY": -3.2159714627807388, + "timestamp": 0.759901855074533 + }, + { + "x": 1.9850045217267513, + "y": 2.800025184289596, + "heading": -6.963070170753428e-7, + "angularVelocity": 0.0000012544923828803064, + "velocityX": 2.6726229529960572, + "velocityY": -3.215966348161622, + "timestamp": 0.8183558439264201 + }, + { + "x": 2.1412299940437136, + "y": 2.6120391232404336, + "heading": -6.229769330118699e-7, + "angularVelocity": 0.0000012544923880093, + "velocityX": 2.6726229532908703, + "velocityY": -3.2159663479166176, + "timestamp": 0.8768098327783073 + }, + { + "x": 2.297455466360677, + "y": 2.4240530621912724, + "heading": -5.496468485640347e-7, + "angularVelocity": 0.0000012544923945847663, + "velocityX": 2.6726229532908814, + "velocityY": -3.215966347916608, + "timestamp": 0.9352638216301944 + }, + { + "x": 2.453680938677642, + "y": 2.2360670011421124, + "heading": -4.7631676369736977e-7, + "angularVelocity": 0.0000012544924017498851, + "velocityX": 2.6726229532909214, + "velocityY": -3.2159663479165745, + "timestamp": 0.9937178104820815 + }, + { + "x": 2.6099064110461754, + "y": 2.0480809401358084, + "heading": -4.029866785187995e-7, + "angularVelocity": 0.0000012544924070857982, + "velocityX": 2.6726229541731215, + "velocityY": -3.2159663471834232, + "timestamp": 1.0521717993339688 + }, + { + "x": 2.766132959939523, + "y": 1.8600957737787043, + "heading": -3.2965660566972314e-7, + "angularVelocity": 0.0000012544921961593194, + "velocityX": 2.672641370791651, + "velocityY": -3.2159510419969424, + "timestamp": 1.1106257881858559 + }, + { + "x": 2.933382193240376, + "y": 1.6818463665507355, + "heading": -2.5592792501793225e-7, + "angularVelocity": 0.0000012613113681361818, + "velocityX": 2.8612116398871454, + "velocityY": -3.0493968115610293, + "timestamp": 1.169079777037743 + }, + { + "x": 3.1152217469834365, + "y": 1.5185076592751214, + "heading": -1.793173771215745e-7, + "angularVelocity": 0.000001310612832436061, + "velocityX": 3.1108151439213563, + "velocityY": -2.7943124239046795, + "timestamp": 1.2275337658896301 + }, + { + "x": 3.3103273336088685, + "y": 1.3712698739724987, + "heading": -9.63896176110914e-8, + "angularVelocity": 0.0000014186843556666162, + "velocityX": 3.3377634350976035, + "velocityY": -2.5188663458998373, + "timestamp": 1.2859877547415173 + }, + { + "x": 3.517277717590332, + "y": 1.2412056922912598, + "heading": 2.0248701458039248e-25, + "angularVelocity": 0.0000016489827213558838, + "velocityX": 3.5403979787562827, + "velocityY": -2.2250693962186263, + "timestamp": 1.3444417435934044 + }, + { + "x": 3.8085805078161967, + "y": 1.1011465689731057, + "heading": 1.2027671540774088e-13, + "angularVelocity": 1.5560191716883039e-12, + "velocityX": 3.768582512599393, + "velocityY": -1.8119440684297683, + "timestamp": 1.4217394514854198 + }, + { + "x": 4.113810562883195, + "y": 0.9948051908350006, + "heading": 1.2024608996754215e-13, + "angularVelocity": -3.962011427529581e-16, + "velocityX": 3.9487594573102207, + "velocityY": -1.3757377940192446, + "timestamp": 1.4990371593774352 + }, + { + "x": 4.429079571376881, + "y": 0.9235357844694564, + "heading": 1.2023024929371935e-13, + "angularVelocity": -2.0493070564434177e-16, + "velocityX": 4.078633339737774, + "velocityY": -0.9220118980126479, + "timestamp": 1.5763348672694506 + }, + { + "x": 4.74851734223509, + "y": 0.8742064147498704, + "heading": 1.2026218341744134e-13, + "angularVelocity": 4.1313157391792204e-16, + "velocityX": 4.132564594340397, + "velocityY": -0.6381737707992456, + "timestamp": 1.653632575161466 + }, + { + "x": 5.067955221408222, + "y": 0.8248777464405996, + "heading": 1.2017533765568414e-13, + "angularVelocity": -1.1235231176431118e-15, + "velocityX": 4.132565995609943, + "velocityY": -0.6381646966580525, + "timestamp": 1.7309302830534814 + }, + { + "x": 5.387393100583508, + "y": 0.7755490781452744, + "heading": 1.2011087822013848e-13, + "angularVelocity": -8.339113449011661e-16, + "velocityX": 4.132565995637804, + "velocityY": -0.6381646964776388, + "timestamp": 1.8082279909454968 + }, + { + "x": 5.706830979759032, + "y": 0.7262204098514907, + "heading": 1.2009725083714761e-13, + "angularVelocity": -1.7629737495368953e-16, + "velocityX": 4.132565995640883, + "velocityY": -0.6381646964576962, + "timestamp": 1.8855256988375122 + }, + { + "x": 6.02626887090532, + "y": 0.6768918190768992, + "heading": 1.200782994529034e-13, + "angularVelocity": -2.4517394837846764e-16, + "velocityX": 4.1325661505065865, + "velocityY": -0.6381636935923554, + "timestamp": 1.9628234067295276 + }, + { + "x": 6.346278711766802, + "y": 0.6314211922985453, + "heading": 1.2000563974027956e-13, + "angularVelocity": -9.399982820393656e-16, + "velocityX": 4.139965460664552, + "velocityY": -0.588253235682956, + "timestamp": 2.040121114621543 + }, + { + "x": 6.669374055375815, + "y": 0.6222974986610686, + "heading": 1.1998245156455226e-13, + "angularVelocity": -2.999852952930381e-16, + "velocityX": 4.179882591866463, + "velocityY": -0.11803317182732602, + "timestamp": 2.1174188225135584 + }, + { + "x": 6.957333126631289, + "y": 0.6229872679410849, + "heading": 1.0411186745728865e-13, + "angularVelocity": -2.0531765481888398e-13, + "velocityX": 3.7253248396155745, + "velocityY": 0.008923541186757694, + "timestamp": 2.194716530405574 + }, + { + "x": 7.209511372482262, + "y": 0.6307902356507334, + "heading": 8.529380764861957e-14, + "angularVelocity": -2.4344913092323517e-13, + "velocityX": 3.262428508271763, + "velocityY": 0.10094694813653739, + "timestamp": 2.272014238297589 + }, + { + "x": 7.425794423178197, + "y": 0.6451047906108485, + "heading": 6.637626916620301e-14, + "angularVelocity": -2.4473608595023864e-13, + "velocityX": 2.7980525761266, + "velocityY": 0.18518731474046324, + "timestamp": 2.3493119461896046 + }, + { + "x": 7.606135287145741, + "y": 0.6656663723850257, + "heading": 4.8622136850652897e-14, + "angularVelocity": -2.2968510709725984e-13, + "velocityX": 2.3330687142687285, + "velocityY": 0.266005064508531, + "timestamp": 2.42660965408162 + }, + { + "x": 7.750508424686906, + "y": 0.6923262301695847, + "heading": 3.272151062184158e-14, + "angularVelocity": -2.057063095716186e-13, + "velocityX": 1.8677544454856772, + "velocityY": 0.3448984259895882, + "timestamp": 2.5039073619736354 + }, + { + "x": 7.858897802002306, + "y": 0.7249890399184212, + "heading": 1.9124643365638934e-14, + "angularVelocity": -1.7590259306521183e-13, + "velocityX": 1.4022327475326948, + "velocityY": 0.42255858083743525, + "timestamp": 2.581205069865651 + }, + { + "x": 7.931292420667315, + "y": 0.7635885039265983, + "heading": 8.143934963506789e-15, + "angularVelocity": -1.4205736109888667e-13, + "velocityX": 0.9365687630239243, + "velocityY": 0.49936104265989784, + "timestamp": 2.658502777757666 + }, + { + "x": 7.967684268951416, + "y": 0.8080758452415466, + "heading": -2.356364968162154e-28, + "angularVelocity": -1.053580395279536e-13, + "velocityX": 0.4708011308037708, + "velocityY": 0.5755324773290436, + "timestamp": 2.7358004856496816 + }, + { + "x": 7.968682020134014, + "y": 0.8574283512733433, + "heading": -5.0773941325982076e-15, + "angularVelocity": -6.686084313137058e-14, + "velocityX": 0.013138725015563199, + "velocityY": 0.6498904906252848, + "timestamp": 2.8117402120246786 + }, + { + "x": 7.934920196939559, + "y": 0.9123978556608698, + "heading": -7.250674587273434e-15, + "angularVelocity": -2.861849203858442e-14, + "velocityX": -0.44458710619704805, + "velocityY": 0.7238570246629862, + "timestamp": 2.8876799383996756 + }, + { + "x": 7.866392659944353, + "y": 0.9729462284683079, + "heading": -6.561254231060817e-15, + "angularVelocity": 9.078520415102618e-15, + "velocityX": -0.9023937834172842, + "velocityY": 0.7973214508101477, + "timestamp": 2.9636196647746726 + }, + { + "x": 7.7630913157264, + "y": 1.039022791795512, + "heading": -3.0532129081230325e-15, + "angularVelocity": 4.6195074572890516e-14, + "velocityX": -1.3603070375555597, + "velocityY": 0.8701185332287369, + "timestamp": 3.0395593911496697 + }, + { + "x": 7.6250050093039485, + "y": 1.1105569064458902, + "heading": 3.2228615065427893e-15, + "angularVelocity": 8.264547048371132e-14, + "velocityX": -1.8183671842662383, + "velocityY": 0.9419854148161748, + "timestamp": 3.1154991175246667 + }, + { + "x": 7.452117387393709, + "y": 1.1874432876779466, + "heading": 1.2177403879633131e-14, + "angularVelocity": 1.1791644242793474e-13, + "velocityX": -2.2766426765419903, + "velocityY": 1.0124658713199024, + "timestamp": 3.1914388438996637 + }, + { + "x": 7.244402183717801, + "y": 1.2695082427410065, + "heading": 2.365643857652256e-14, + "angularVelocity": 1.5115981114028605e-13, + "velocityX": -2.735264052048239, + "velocityY": 1.0806590829392273, + "timestamp": 3.2673785702746607 + }, + { + "x": 7.00181043141649, + "y": 1.3564109624551457, + "heading": 3.7383567341129667e-14, + "angularVelocity": 1.8076347413765312e-13, + "velocityX": -3.194530239724227, + "velocityY": 1.1443644040143879, + "timestamp": 3.3433182966496577 + }, + { + "x": 6.724221087183376, + "y": 1.447179537855358, + "heading": 5.2548800981821277e-14, + "angularVelocity": 1.9970092551827854e-13, + "velocityX": -3.6553903665961816, + "velocityY": 1.1952712991351762, + "timestamp": 3.4192580230246548 + }, + { + "x": 6.415496495875919, + "y": 1.5215059011095828, + "heading": 5.2606937821813504e-14, + "angularVelocity": 7.655655711115095e-16, + "velocityX": -4.065389830125926, + "velocityY": 0.9787546887803473, + "timestamp": 3.4951977493996518 + }, + { + "x": 6.102420468620207, + "y": 1.5745971909728977, + "heading": 5.2547023381561494e-14, + "angularVelocity": -7.889736125229508e-16, + "velocityX": -4.122691010364133, + "velocityY": 0.6991240605891275, + "timestamp": 3.571137475774649 + }, + { + "x": 5.7893443139235865, + "y": 1.6276877293196694, + "heading": 5.251469887397708e-14, + "angularVelocity": -4.256600481369481e-16, + "velocityX": -4.1226926885491135, + "velocityY": 0.6991141643651176, + "timestamp": 3.647077202149646 + }, + { + "x": 5.47626834023763, + "y": 1.6807793350795175, + "heading": 5.2545327836571197e-14, + "angularVelocity": 4.03332538271171e-16, + "velocityX": -4.122690304939467, + "velocityY": 0.6991282204215097, + "timestamp": 3.723016928524643 + }, + { + "x": 5.168305647623383, + "y": 1.7582019652150536, + "heading": 5.2570129566972103e-14, + "angularVelocity": 3.265975739589384e-16, + "velocityX": -4.055356890456809, + "velocityY": 1.0195273782422736, + "timestamp": 3.79895665489964 + }, + { + "x": 4.870807647705078, + "y": 1.8692435026168823, + "heading": 8.0561270048309e-28, + "angularVelocity": -6.922612455485455e-13, + "velocityX": -3.917554277839153, + "velocityY": 1.4622325191625807, + "timestamp": 3.874896381274637 + }, + { + "x": 4.665184835706644, + "y": 1.965165570466657, + "heading": -6.025901704154455e-8, + "angularVelocity": -0.0000011105355093099177, + "velocityX": -3.7894981607646425, + "velocityY": 1.7677829427613814, + "timestamp": 3.929157606184941 + }, + { + "x": 4.467801167505074, + "y": 2.0770649526386022, + "heading": -1.1951961632035804e-7, + "angularVelocity": -0.000001092135302451678, + "velocityX": -3.6376559601787903, + "velocityY": 2.062234723910474, + "timestamp": 3.983418831095245 + }, + { + "x": 4.282026375471247, + "y": 2.202504243233706, + "heading": -0.0074900260123963456, + "angularVelocity": -0.13803423172184429, + "velocityX": -3.423711726761048, + "velocityY": 2.31176665846485, + "timestamp": 4.0376800560055495 + }, + { + "x": 4.109553140955502, + "y": 2.3223494763438204, + "heading": -0.05391384347341113, + "angularVelocity": -0.855561545795458, + "velocityX": -3.178572448389246, + "velocityY": 2.2086717229149047, + "timestamp": 4.0919412809158535 + }, + { + "x": 3.9503579748540636, + "y": 2.43525476975937, + "heading": -0.1359907520094868, + "angularVelocity": -1.5126254276742177, + "velocityX": -2.933866059319426, + "velocityY": 2.080773030874018, + "timestamp": 4.146202505826158 + }, + { + "x": 3.8058392953370093, + "y": 2.5380816662907524, + "heading": -0.22641050474906085, + "angularVelocity": -1.6663787610589593, + "velocityX": -2.6633877092887084, + "velocityY": 1.8950345610767008, + "timestamp": 4.200463730736462 + }, + { + "x": 3.6760560842265666, + "y": 2.630611385450664, + "heading": -0.3168870123533691, + "angularVelocity": -1.6674247172611631, + "velocityX": -2.3918223616399943, + "velocityY": 1.7052641054982942, + "timestamp": 4.254724955646766 + }, + { + "x": 3.5609332686690274, + "y": 2.7128111323244637, + "heading": -0.40322451991269215, + "angularVelocity": -1.5911455685352158, + "velocityX": -2.1216405591256335, + "velocityY": 1.5148892604190034, + "timestamp": 4.30898618055707 + }, + { + "x": 3.4603899159226676, + "y": 2.7846878010678795, + "heading": -0.4828129395591552, + "angularVelocity": -1.4667641539243872, + "velocityX": -1.8529502957694302, + "velocityY": 1.3246414702622478, + "timestamp": 4.363247405467374 + }, + { + "x": 3.3743546742713244, + "y": 2.846257792560545, + "heading": -0.5538376442912536, + "angularVelocity": -1.308940313262462, + "velocityX": -1.585574999340014, + "velocityY": 1.1346959379269979, + "timestamp": 4.417508630377678 + }, + { + "x": 3.302766303961918, + "y": 2.8975376071130285, + "heading": -0.6149635904061515, + "angularVelocity": -1.126512462922489, + "velocityX": -1.3193283127637558, + "velocityY": 0.9450544958623966, + "timestamp": 4.471769855287982 + }, + { + "x": 3.245572544711835, + "y": 2.938541171103499, + "heading": -0.6651772993510998, + "angularVelocity": -0.925406844905429, + "velocityX": -1.0540447500886079, + "velocityY": 0.7556697081249255, + "timestamp": 4.526031080198286 + }, + { + "x": 3.202728904985161, + "y": 2.9692797964201003, + "heading": -0.7036896903252039, + "angularVelocity": -0.7097589676194475, + "velocityX": -0.7895811382344551, + "velocityY": 0.5664933913197379, + "timestamp": 4.58029230510859 + }, + { + "x": 3.1741974612408677, + "y": 2.989763077707769, + "heading": -0.7298679174991396, + "angularVelocity": -0.48244814261398083, + "velocityX": -0.5258164332164795, + "velocityY": 0.377493897742418, + "timestamp": 4.634553530018894 + }, + { + "x": 3.1599457263946533, + "y": 3, + "heading": -0.7431853071795862, + "angularVelocity": -0.24543105509432933, + "velocityX": -0.2626504445812457, + "velocityY": 0.18865999264028846, + "timestamp": 4.688814754929198 + }, + { + "x": 3.1599457263946533, + "y": 3, + "heading": -0.7431853071795862, + "angularVelocity": -2.718854590960149e-28, + "velocityX": -1.846408155148528e-27, + "velocityY": 4.7057945380444184e-27, + "timestamp": 4.743075979839502 + }, + { + "x": 3.1707764655908464, + "y": 2.9881322043781373, + "heading": -0.732035938934007, + "angularVelocity": 0.21513876739040086, + "velocityX": 0.2089904853146997, + "velocityY": -0.22900157798098394, + "timestamp": 4.794900057762878 + }, + { + "x": 3.1926755779502765, + "y": 2.9645842050264797, + "heading": -0.7101668692970703, + "angularVelocity": 0.4219866616685564, + "velocityX": 0.42256636754461707, + "velocityY": -0.4543833734287422, + "timestamp": 4.846724135686254 + }, + { + "x": 3.2259183969876806, + "y": 2.9295838122377513, + "heading": -0.6780905954162056, + "angularVelocity": 0.6189453853533228, + "velocityX": 0.6414550990478732, + "velocityY": -0.6753693300723632, + "timestamp": 4.89854821360963 + }, + { + "x": 3.2708261994975825, + "y": 2.8834123046214346, + "heading": -0.6364219103258802, + "angularVelocity": 0.8040410318912782, + "velocityX": 0.8665432036494631, + "velocityY": -0.8909277205970338, + "timestamp": 4.950372291533006 + }, + { + "x": 3.327776902619988, + "y": 2.826423972607863, + "heading": -0.5859053180091909, + "angularVelocity": 0.9747706923291543, + "velocityX": 1.0989236162891207, + "velocityY": -1.0996497052553655, + "timestamp": 5.0021963694563825 + }, + { + "x": 3.3972187070235824, + "y": 2.759075775607749, + "heading": -0.5274547304559486, + "angularVelocity": 1.127865461295109, + "velocityX": 1.3399525314520095, + "velocityY": -1.2995541782661457, + "timestamp": 5.0540204473797585 + }, + { + "x": 3.4796866107524327, + "y": 2.6819736466469406, + "heading": -0.46221377382092116, + "angularVelocity": 1.25889276277117, + "velocityX": 1.591304795635387, + "velocityY": -1.4877665372996447, + "timestamp": 5.105844525303135 + }, + { + "x": 3.575819480010563, + "y": 2.5959468307633444, + "heading": -0.3916508199590152, + "angularVelocity": 1.3615862874827402, + "velocityX": 1.8549846540495372, + "velocityY": -1.659977742600455, + "timestamp": 5.157668603226511 + }, + { + "x": 3.6863681532781767, + "y": 2.502169766316827, + "heading": -0.31770965056129546, + "angularVelocity": 1.426772503449932, + "velocityX": 2.133152729336816, + "velocityY": -1.8095269265604703, + "timestamp": 5.209492681149887 + }, + { + "x": 3.812163930499293, + "y": 2.4023610752274767, + "heading": -0.24304232225652234, + "angularVelocity": 1.4407845020450063, + "velocityX": 2.427361609927921, + "velocityY": -1.925913495980028, + "timestamp": 5.261316759073263 + }, + { + "x": 3.953961837508726, + "y": 2.299083056269231, + "heading": -0.17136133589722868, + "angularVelocity": 1.3831598984795583, + "velocityX": 2.736139506796171, + "velocityY": -1.9928578200840514, + "timestamp": 5.313140836996639 + }, + { + "x": 4.111973704806276, + "y": 2.196045409033803, + "heading": -0.1079080247435874, + "angularVelocity": 1.224398266139137, + "velocityX": 3.0490048955849653, + "velocityY": -1.9882195952964754, + "timestamp": 5.364964914920015 + }, + { + "x": 4.285069061395649, + "y": 2.097867124268194, + "heading": -0.05873651399697375, + "angularVelocity": 0.9488159310680954, + "velocityX": 3.3400566594798016, + "velocityY": -1.8944530939994642, + "timestamp": 5.416788992843391 + }, + { + "x": 4.470523752035761, + "y": 2.009092656335386, + "heading": -0.026966372471160117, + "angularVelocity": 0.6130382401166306, + "velocityX": 3.578542987572568, + "velocityY": -1.7129965739875637, + "timestamp": 5.468613070766767 + }, + { + "x": 4.6664061186047086, + "y": 1.9323261388767596, + "heading": -0.008453101034109357, + "angularVelocity": 0.3572330117368095, + "velocityX": 3.7797559439179373, + "velocityY": -1.4812905609652864, + "timestamp": 5.520437148690143 + }, + { + "x": 4.870807647705078, + "y": 1.8692435026168823, + "heading": 5.290053653169388e-26, + "angularVelocity": 0.16311146040278007, + "velocityX": 3.9441421302774584, + "velocityY": -1.2172457048468346, + "timestamp": 5.572261226613519 + }, + { + "x": 5.017762639572388, + "y": 1.8322127421815186, + "heading": 0.001461003033461962, + "angularVelocity": 0.04008169041354506, + "velocityX": 4.031616878161617, + "velocityY": -1.0159153961722736, + "timestamp": 5.608711860765622 + }, + { + "x": 5.167108391738545, + "y": 1.8029062751901384, + "heading": 0.0007309116487781689, + "angularVelocity": -0.020029593494512152, + "velocityX": 4.097205868708966, + "velocityY": -0.804004310846535, + "timestamp": 5.645162494917725 + }, + { + "x": 5.3178754828089545, + "y": 1.781586355190015, + "heading": 0.00021715295944246574, + "angularVelocity": -0.01409464337964255, + "velocityX": 4.136199398926305, + "velocityY": -0.5848984659953738, + "timestamp": 5.681613129069827 + }, + { + "x": 5.469655961827876, + "y": 1.7683146760745116, + "heading": 0.000005188258876374356, + "angularVelocity": -0.00581511695191908, + "velocityX": 4.1640010537421, + "velocityY": -0.36410008835848723, + "timestamp": 5.71806376322193 + }, + { + "x": 5.621987131567847, + "y": 1.7631363342262816, + "heading": 0.0000016303066679806213, + "angularVelocity": -0.00009761015936093163, + "velocityX": 4.179108903958118, + "velocityY": -0.14206452010194576, + "timestamp": 5.754514397374033 + }, + { + "x": 5.774378851652096, + "y": 1.7660700536906622, + "heading": 0.000001013880704578562, + "angularVelocity": -0.00001691125484483525, + "velocityX": 4.180770064200893, + "velocityY": 0.08048473044772596, + "timestamp": 5.790965031526135 + }, + { + "x": 5.92639870788874, + "y": 1.7771075275794117, + "heading": 6.402152729336533e-7, + "angularVelocity": -0.000010251273821071673, + "velocityX": 4.1705682156938835, + "velocityY": 0.3028060867937732, + "timestamp": 5.827415665678238 + }, + { + "x": 6.077616038227422, + "y": 1.7962174809956766, + "heading": 3.7516129965530495e-7, + "angularVelocity": -0.000007271587434455114, + "velocityX": 4.148551427327075, + "velocityY": 0.524269436205752, + "timestamp": 5.863866299830341 + }, + { + "x": 6.227602479279352, + "y": 1.8233457636897183, + "heading": 1.6965700440858202e-7, + "angularVelocity": -0.000005637879834660427, + "velocityX": 4.114782761420892, + "velocityY": 0.7442472079042579, + "timestamp": 5.9003169339824435 + }, + { + "x": 6.3759331703186035, + "y": 1.8584154844284055, + "heading": -5.246500142800949e-26, + "angularVelocity": -0.000004654432175325993, + "velocityX": 4.0693583113065195, + "velocityY": 0.9621155174515489, + "timestamp": 5.936767568134546 + }, + { + "x": 6.630268920645483, + "y": 1.9443807518692906, + "heading": -2.614202058840289e-14, + "angularVelocity": -4.071729614915087e-13, + "velocityX": 3.961386241119695, + "velocityY": 1.3389451825660532, + "timestamp": 6.000971292499225 + }, + { + "x": 6.86532424088555, + "y": 2.033995146724681, + "heading": -1.479716531157944e-14, + "angularVelocity": 1.7670089062722164e-13, + "velocityX": 3.661085436491938, + "velocityY": 1.3957818762409848, + "timestamp": 6.065175016863903 + }, + { + "x": 7.076902889512762, + "y": 2.114538206034381, + "heading": -5.459326745393024e-15, + "angularVelocity": 1.4544076155375644e-13, + "velocityX": 3.2954264058801392, + "velocityY": 1.2544920112766957, + "timestamp": 6.129378741228582 + }, + { + "x": 7.265004863010539, + "y": 2.1860098840684987, + "heading": 1.931554215731149e-15, + "angularVelocity": 1.1511607830012644e-13, + "velocityX": 2.9297673204961416, + "velocityY": 1.1132014340501033, + "timestamp": 6.19358246559326 + }, + { + "x": 7.429630160202282, + "y": 2.248410165595121, + "heading": 7.392353831711916e-15, + "angularVelocity": 8.50542498899784e-14, + "velocityX": 2.564108216786111, + "velocityY": 0.9719106195800618, + "timestamp": 6.257786189957939 + }, + { + "x": 7.570778780498211, + "y": 2.3017390430021187, + "heading": 1.0931792990306175e-14, + "angularVelocity": 5.5128252973146285e-14, + "velocityX": 2.198449103890015, + "velocityY": 0.8306196865479084, + "timestamp": 6.321989914322617 + }, + { + "x": 7.6884507235438, + "y": 2.345996511723915, + "heading": 1.255617668617468e-14, + "angularVelocity": 2.5300458998949426e-14, + "velocityX": 1.8327899854720402, + "velocityY": 0.689328682404981, + "timestamp": 6.3861936386872955 + }, + { + "x": 7.782645989102363, + "y": 2.3811825687176684, + "heading": 1.2272281762787649e-14, + "angularVelocity": -4.4217827890265095e-15, + "velocityX": 1.467130863367541, + "velocityY": 0.5480376308685124, + "timestamp": 6.450397363051974 + }, + { + "x": 7.853364577004646, + "y": 2.407297211810409, + "heading": 1.0083593884875461e-14, + "angularVelocity": -3.408973388335696e-14, + "velocityX": 1.1014717386268686, + "velocityY": 0.4067465454871274, + "timestamp": 6.5146010874166524 + }, + { + "x": 7.900606487123601, + "y": 2.4243404393726915, + "heading": 5.993744788639443e-15, + "angularVelocity": -6.370111915946951e-14, + "velocityX": 0.7358126119073701, + "velocityY": 0.26545543472644506, + "timestamp": 6.578804811781331 + }, + { + "x": 7.924371719360352, + "y": 2.432312250137329, + "heading": 3.300747525630688e-29, + "angularVelocity": -9.33550950190197e-14, + "velocityX": 0.3701534836478256, + "velocityY": 0.12416430422879107, + "timestamp": 6.643008536146009 + }, + { + "x": 7.922491017378158, + "y": 2.4302702205202578, + "heading": -8.756695320067185e-15, + "angularVelocity": -1.2558246039990905e-13, + "velocityX": -0.02697172547062432, + "velocityY": -0.0292853746930814, + "timestamp": 6.7127371852916164 + }, + { + "x": 7.892909500405094, + "y": 2.4175537778122855, + "heading": -1.9591167293870617e-14, + "angularVelocity": -1.553804943385475e-13, + "velocityX": -0.4242376316697698, + "velocityY": -0.1823704153714205, + "timestamp": 6.7824658344372235 + }, + { + "x": 7.835614963213244, + "y": 2.3941946511690215, + "heading": -3.229380659597313e-14, + "angularVelocity": -1.8217245648308838e-13, + "velocityX": -0.8216785767957127, + "velocityY": -0.33500041847197504, + "timestamp": 6.852194483582831 + }, + { + "x": 7.750591807680805, + "y": 2.3602335487177615, + "heading": -4.660838607004271e-14, + "angularVelocity": -2.0528978618498788e-13, + "velocityX": -1.2193432193830964, + "velocityY": -0.48704661380062597, + "timestamp": 6.921923132728438 + }, + { + "x": 7.6378194003421855, + "y": 2.3157245966907354, + "heading": -6.217925570771978e-14, + "angularVelocity": -2.23306629749304e-13, + "velocityX": -1.6173037728456214, + "velocityY": -0.6383165681882451, + "timestamp": 6.991651781874045 + }, + { + "x": 7.4972691666560305, + "y": 2.2607432816644524, + "heading": -7.852975653361751e-14, + "angularVelocity": -2.3448756036783125e-13, + "velocityX": -2.015674122592836, + "velocityY": -0.7885039463688946, + "timestamp": 7.061380431019652 + }, + { + "x": 7.328898919845841, + "y": 2.195402199778693, + "heading": -9.493163002335684e-14, + "angularVelocity": -2.3522431153784627e-13, + "velocityX": -2.414649485874868, + "velocityY": -0.9370765486839444, + "timestamp": 7.131109080165259 + }, + { + "x": 7.132640115356717, + "y": 2.1198873257690747, + "heading": -1.1019249654185571e-13, + "angularVelocity": -2.1886077968657218e-13, + "velocityX": -2.8146078676972013, + "velocityY": -1.082982030125505, + "timestamp": 7.200837729310866 + }, + { + "x": 6.908362069234848, + "y": 2.034564322766841, + "heading": -1.2195634420509482e-13, + "angularVelocity": -1.6870895689767396e-13, + "velocityX": -3.2164404282884083, + "velocityY": -1.22364342415501, + "timestamp": 7.270566378456473 + }, + { + "x": 6.65571980868215, + "y": 1.940480754591585, + "heading": -1.2356989243287506e-13, + "angularVelocity": -2.314039132481892e-14, + "velocityX": -3.6232203498612474, + "velocityY": -1.3492813833061756, + "timestamp": 7.34029502760208 + }, + { + "x": 6.3759331703186035, + "y": 1.8584154844284055, + "heading": -5.564026809490533e-25, + "angularVelocity": 1.7721538269625782e-12, + "velocityX": -4.012506219348913, + "velocityY": -1.176923275708542, + "timestamp": 7.410023676747687 + }, + { + "x": 6.227730472222822, + "y": 1.8233818050436732, + "heading": 8.669024554559813e-8, + "angularVelocity": 0.0000023803669135590183, + "velocityX": -4.069394391804057, + "velocityY": -0.9619653369627125, + "timestamp": 7.446442535139557 + }, + { + "x": 6.077875008438939, + "y": 1.7962757602738582, + "heading": 1.8100925853170345e-7, + "angularVelocity": 0.0000025898399112686397, + "velocityX": -4.114776530648671, + "velocityY": -0.7442859542205248, + "timestamp": 7.482861393531427 + }, + { + "x": 5.926790527292132, + "y": 1.777174025334229, + "heading": 2.8543132587338983e-7, + "angularVelocity": 0.0000028672526254968387, + "velocityX": -4.148523260150717, + "velocityY": -0.5245012002873043, + "timestamp": 7.519280251923297 + }, + { + "x": 5.774904257799345, + "y": 1.7661306301784576, + "heading": 4.032163201326794e-7, + "angularVelocity": 0.0000032341759039208115, + "velocityX": -4.170539006425706, + "velocityY": -0.30323287558724765, + "timestamp": 7.5556991103151665 + }, + { + "x": 5.622645699142081, + "y": 1.7631768121104108, + "heading": 5.387462462721234e-7, + "angularVelocity": 0.0000037214215965017684, + "velocityX": -4.180761434610339, + "velocityY": -0.0811068275744295, + "timestamp": 7.592117968707036 + }, + { + "x": 5.470445405221861, + "y": 1.7683209297554319, + "heading": 6.980137884656088e-7, + "angularVelocity": 0.00000437321621890928, + "velocityX": -4.179161583884165, + "velocityY": 0.1412487340945748, + "timestamp": 7.628536827098906 + }, + { + "x": 5.3187337669307375, + "y": 1.7815484404811406, + "heading": 8.894300959206496e-7, + "angularVelocity": 0.000005255966713601673, + "velocityX": -4.165743930210383, + "velocityY": 0.363204979776674, + "timestamp": 7.664955685490776 + }, + { + "x": 5.167939795459854, + "y": 1.8028219419315428, + "heading": 0.000001125567179046142, + "angularVelocity": 0.0000064839232626305455, + "velocityX": -4.140546357832767, + "velocityY": 0.5841342202849328, + "timestamp": 7.701374543882646 + }, + { + "x": 5.018489910628665, + "y": 1.8320812776224336, + "heading": 0.0000014279951382221296, + "angularVelocity": 0.000008304158134827807, + "velocityX": -4.103640021416827, + "velocityY": 0.8034116659028133, + "timestamp": 7.737793402274516 + }, + { + "x": 4.870807647705078, + "y": 1.8692435026168823, + "heading": 5.5585714874022985e-25, + "angularVelocity": -0.00003921032128071638, + "velocityX": -4.0551041258491365, + "velocityY": 1.0204115844209116, + "timestamp": 7.774212260666386 + }, + { + "x": 4.65506239891728, + "y": 1.942261032325749, + "heading": -0.00511152183249568, + "angularVelocity": -0.0931448282956492, + "velocityX": -3.9314229328313473, + "velocityY": 1.3305636736338056, + "timestamp": 7.829089401201459 + }, + { + "x": 4.448768572924891, + "y": 2.030799601998699, + "heading": -0.019590181225012573, + "angularVelocity": -0.2638377155103962, + "velocityX": -3.75919415590793, + "velocityY": 1.6133961939281727, + "timestamp": 7.883966541736533 + }, + { + "x": 4.254744422441319, + "y": 2.1321986523330394, + "heading": -0.04966705851292773, + "angularVelocity": -0.5480766124957241, + "velocityX": -3.535609701813157, + "velocityY": 1.8477466089825263, + "timestamp": 7.938843682271607 + }, + { + "x": 4.075334891626867, + "y": 2.2420858584907077, + "heading": -0.10369466963717666, + "angularVelocity": -0.984519430084333, + "velocityX": -3.269294446925206, + "velocityY": 2.002422230572227, + "timestamp": 7.99372082280668 + }, + { + "x": 3.9133815236402523, + "y": 2.3539668793447595, + "heading": -0.1763064959762628, + "angularVelocity": -1.3231707343183676, + "velocityX": -2.9511991041717054, + "velocityY": 2.0387545663488873, + "timestamp": 8.048597963341754 + }, + { + "x": 3.7693508978633568, + "y": 2.462531049535678, + "heading": -0.25707153727623516, + "angularVelocity": -1.4717428880674346, + "velocityX": -2.624601507522793, + "velocityY": 1.9783131761672412, + "timestamp": 8.103475103876828 + }, + { + "x": 3.6424532901295272, + "y": 2.564437617081841, + "heading": -0.33942786854657614, + "angularVelocity": -1.5007402074403637, + "velocityX": -2.312394678303705, + "velocityY": 1.8569948534587097, + "timestamp": 8.158352244411903 + }, + { + "x": 3.5317645016321078, + "y": 2.6576956051105367, + "heading": -0.419267219922559, + "angularVelocity": -1.4548744813872836, + "velocityX": -2.017029083843642, + "velocityY": 1.6993959072829385, + "timestamp": 8.213229384946978 + }, + { + "x": 3.436490735414249, + "y": 2.7410565467131964, + "heading": -0.49379047622142197, + "angularVelocity": -1.3580018122706745, + "velocityX": -1.7361284733297266, + "velocityY": 1.519046743140355, + "timestamp": 8.268106525482052 + }, + { + "x": 3.3559896499896893, + "y": 2.8136894224648494, + "heading": -0.5609733911044363, + "angularVelocity": -1.2242422660502008, + "velocityX": -1.4669329458430604, + "velocityY": 1.323554307740043, + "timestamp": 8.322983666017127 + }, + { + "x": 3.2897444136235245, + "y": 2.87501111637633, + "heading": -0.6192864512733086, + "angularVelocity": -1.0626111273345644, + "velocityX": -1.2071553969512154, + "velocityY": 1.1174360273434412, + "timestamp": 8.377860806552201 + }, + { + "x": 3.2373352351965794, + "y": 2.924594143237033, + "heading": -0.6675358072055315, + "angularVelocity": -0.8792250372700273, + "velocityX": -0.9550275017235731, + "velocityY": 0.9035278875183412, + "timestamp": 8.432737947087276 + }, + { + "x": 3.198416700348633, + "y": 2.962113820712204, + "heading": -0.7047663179682083, + "angularVelocity": -0.6784338688143081, + "velocityX": -0.7091939278992236, + "velocityY": 0.683703216117661, + "timestamp": 8.48761508762235 + }, + { + "x": 3.172701033774837, + "y": 2.987316560934767, + "heading": -0.7301983883543345, + "angularVelocity": -0.46343650813714726, + "velocityX": -0.46860434641925003, + "velocityY": 0.45925753377136663, + "timestamp": 8.542492228157425 + }, + { + "x": 3.1599459648132324, + "y": 3, + "heading": -0.7431853071795862, + "angularVelocity": -0.23665443750574666, + "velocityX": -0.23242954784520237, + "velocityY": 0.23112427035308772, + "timestamp": 8.5973693686925 + }, + { + "x": 3.1599459648132324, + "y": 3, + "heading": -0.7431853071795862, + "angularVelocity": -2.558321101390672e-29, + "velocityX": -1.8146718133161694e-27, + "velocityY": -1.261978589298381e-27, + "timestamp": 8.652246509227574 + }, + { + "x": 3.174254445852066, + "y": 3.0012413490099736, + "heading": -0.7319622185405995, + "angularVelocity": 0.22829900262778544, + "velocityX": 0.29106176163811776, + "velocityY": 0.025251403602526624, + "timestamp": 8.70140611356139 + }, + { + "x": 3.202850559042552, + "y": 3.004179613349041, + "heading": -0.7098801566610144, + "angularVelocity": 0.4491912044213771, + "velocityX": 0.5816994171943644, + "velocityY": 0.059769893978703525, + "timestamp": 8.750565717895206 + }, + { + "x": 3.2456916729749725, + "y": 3.009348914628675, + "heading": -0.6773899559870106, + "angularVelocity": 0.6609125747510244, + "velocityX": 0.8714698686651375, + "velocityY": 0.1051534354209254, + "timestamp": 8.799725322229023 + }, + { + "x": 3.302701027134029, + "y": 3.0173829462286443, + "heading": -0.6350563836414606, + "angularVelocity": 0.861145505933815, + "velocityX": 1.1596788650278267, + "velocityY": 0.1634275073781015, + "timestamp": 8.848884926562839 + }, + { + "x": 3.3737478770473053, + "y": 3.0290415179761236, + "heading": -0.5835964640538578, + "angularVelocity": 1.0467927943066175, + "velocityX": 1.4452282697565253, + "velocityY": 0.2371575586392502, + "timestamp": 8.898044530896655 + }, + { + "x": 3.458615323369708, + "y": 3.045243216249596, + "heading": -0.5239353255211855, + "angularVelocity": 1.2136212107718845, + "velocityX": 1.726365528618052, + "velocityY": 0.32957340672345015, + "timestamp": 8.947204135230471 + }, + { + "x": 3.5569478978283247, + "y": 3.0671028955065203, + "heading": -0.4572868659527135, + "angularVelocity": 1.3557566313166185, + "velocityX": 2.0002718856501422, + "velocityY": 0.44466751824296896, + "timestamp": 8.996363739564288 + }, + { + "x": 3.6681669875857352, + "y": 3.0959670977371183, + "heading": -0.3852650574831731, + "angularVelocity": 1.4650607840632703, + "velocityX": 2.2624081553257263, + "velocityY": 0.5871528589733368, + "timestamp": 9.045523343898104 + }, + { + "x": 3.7913406802540934, + "y": 3.1334264012320054, + "heading": -0.31001862368724636, + "angularVelocity": 1.5306558060347588, + "velocityX": 2.5055875517620856, + "velocityY": 0.761993592147766, + "timestamp": 9.09468294823192 + }, + { + "x": 3.925009228556469, + "y": 3.1812569342680064, + "heading": -0.23433608155223415, + "angularVelocity": 1.539527080427534, + "velocityX": 2.719072907802623, + "velocityY": 0.9729641579539594, + "timestamp": 9.143842552565737 + }, + { + "x": 4.06702482491259, + "y": 3.2412227402695866, + "heading": -0.16161028675015265, + "angularVelocity": 1.479381207143992, + "velocityX": 2.8888677661392754, + "velocityY": 1.219818727473582, + "timestamp": 9.193002156899553 + }, + { + "x": 4.214551884068752, + "y": 3.3147260265586795, + "heading": -0.09566981004789035, + "angularVelocity": 1.3413549111277785, + "velocityX": 3.000981418694643, + "velocityY": 1.4951968650921703, + "timestamp": 9.242161761233369 + }, + { + "x": 4.364378322917566, + "y": 3.402451984218925, + "heading": -0.04058781830439109, + "angularVelocity": 1.1204726419168822, + "velocityX": 3.047755181905567, + "velocityY": 1.7845130946243346, + "timestamp": 9.291321365567185 + }, + { + "x": 4.5134758949279785, + "y": 3.504307985305786, + "heading": 3.3631785914154804e-27, + "angularVelocity": 0.825633543117669, + "velocityX": 3.032928641938876, + "velocityY": 2.0719450953106353, + "timestamp": 9.340480969901002 + }, + { + "x": 4.600523359700349, + "y": 3.569285870650025, + "heading": 0.01816599344926272, + "angularVelocity": 0.6249216900973263, + "velocityX": 2.994487967650624, + "velocityY": 2.235280445392861, + "timestamp": 9.369550201673759 + }, + { + "x": 4.686546893692203, + "y": 3.6390136130428754, + "heading": 0.030283751359764955, + "angularVelocity": 0.4168585535809847, + "velocityX": 2.9592640997300075, + "velocityY": 2.398678538804583, + "timestamp": 9.398619433446516 + }, + { + "x": 4.771736384884454, + "y": 3.7134909076284828, + "heading": 0.035957094180445175, + "angularVelocity": 0.19516658936948508, + "velocityX": 2.930572498723082, + "velocityY": 2.562066144981634, + "timestamp": 9.427688665219273 + }, + { + "x": 4.856660629697274, + "y": 3.792667533709231, + "heading": 0.03406209145937424, + "angularVelocity": -0.06518929484909297, + "velocityX": 2.9214478551306744, + "velocityY": 2.723726127325746, + "timestamp": 9.45675789699203 + }, + { + "x": 4.94653857071501, + "y": 3.871098031264581, + "heading": 0.0279504475909309, + "angularVelocity": -0.21024442325203974, + "velocityX": 3.0918581447331515, + "velocityY": 2.698058833080388, + "timestamp": 9.485827128764788 + }, + { + "x": 5.0396307624325205, + "y": 3.9454940296259062, + "heading": 0.021490360496799883, + "angularVelocity": -0.2222310910942296, + "velocityX": 3.2024304063223243, + "velocityY": 2.5592695033325716, + "timestamp": 9.514896360537545 + }, + { + "x": 5.135720972459086, + "y": 4.015693585929082, + "heading": 0.01454272767997795, + "angularVelocity": -0.2390029730105524, + "velocityX": 3.3055641366008026, + "velocityY": 2.414909236403115, + "timestamp": 9.543965592310302 + }, + { + "x": 5.234684813317334, + "y": 4.081609056683672, + "heading": 0.007288208288849233, + "angularVelocity": -0.24956006570243372, + "velocityX": 3.404418858808358, + "velocityY": 2.2675339778453596, + "timestamp": 9.57303482408306 + }, + { + "x": 5.336422443389893, + "y": 4.143174171447754, + "heading": -2.9790128893571064e-27, + "angularVelocity": -0.25071898513944774, + "velocityX": 3.4998389660885882, + "velocityY": 2.1178789740765374, + "timestamp": 9.602104055855817 + }, + { + "x": 5.601458841609157, + "y": 4.267372818196447, + "heading": -0.014576784799321342, + "angularVelocity": -0.2046562124623809, + "velocityX": 3.7210774646787157, + "velocityY": 1.7437332708461397, + "timestamp": 9.67332976957316 + }, + { + "x": 5.879046341382613, + "y": 4.363262401476311, + "heading": -0.02679043418253227, + "angularVelocity": -0.17147809050647556, + "velocityX": 3.8972933409281296, + "velocityY": 1.3462776050289633, + "timestamp": 9.744555483290503 + }, + { + "x": 6.164615438298519, + "y": 4.429223569887391, + "heading": -0.03945167341125408, + "angularVelocity": -0.1777621952511066, + "velocityX": 4.009353953955124, + "velocityY": 0.926086450643998, + "timestamp": 9.815781197007846 + }, + { + "x": 6.44981818392364, + "y": 4.464212612650885, + "heading": -0.051564894166171, + "angularVelocity": -0.17006808528430892, + "velocityX": 4.004210428230155, + "velocityY": 0.4912417291084996, + "timestamp": 9.887006910725189 + }, + { + "x": 6.723678067297048, + "y": 4.470383566214521, + "heading": -0.06187724212677704, + "angularVelocity": -0.14478405932905375, + "velocityX": 3.844958078766487, + "velocityY": 0.08663940649474564, + "timestamp": 9.958232624442532 + }, + { + "x": 6.976222932545455, + "y": 4.454096398362661, + "heading": -0.06932431769193947, + "angularVelocity": -0.10455599777793567, + "velocityX": 3.5456979237950708, + "velocityY": -0.22866977390349366, + "timestamp": 10.029458338159875 + }, + { + "x": 7.201595271822139, + "y": 4.422975711797285, + "heading": -0.07334279579410785, + "angularVelocity": -0.056418923622381, + "velocityX": 3.164199100497136, + "velocityY": -0.436930498006333, + "timestamp": 10.100684051877218 + }, + { + "x": 7.3972346520908046, + "y": 4.383240926841446, + "heading": -0.07372930856311687, + "angularVelocity": -0.005426590325831984, + "velocityX": 2.746752121643222, + "velocityY": -0.5578713484504427, + "timestamp": 10.171909765594561 + }, + { + "x": 7.562198130742167, + "y": 4.339357119000387, + "heading": -0.07044283334517008, + "angularVelocity": 0.046141695834583545, + "velocityX": 2.316066347976712, + "velocityY": -0.6161231042936308, + "timestamp": 10.243135479311904 + }, + { + "x": 7.696221986430261, + "y": 4.294484349506991, + "heading": -0.06350267472615741, + "angularVelocity": 0.09743894805399134, + "velocityX": 1.8816779600126234, + "velocityY": -0.6300080006424584, + "timestamp": 10.314361193029248 + }, + { + "x": 7.799317237709317, + "y": 4.250909609152329, + "heading": -0.05294727053269687, + "angularVelocity": 0.14819653805575428, + "velocityX": 1.4474442711544644, + "velocityY": -0.6117838359273932, + "timestamp": 10.38558690674659 + }, + { + "x": 7.871602327588269, + "y": 4.210338954621034, + "heading": -0.03881874513325574, + "angularVelocity": 0.1983627072592022, + "velocityX": 1.0148735071411497, + "velocityY": -0.5696068514286532, + "timestamp": 10.456812620463934 + }, + { + "x": 7.913232875268372, + "y": 4.174082836279598, + "heading": -0.021157400982541673, + "angularVelocity": 0.2479630351027785, + "velocityX": 0.5844876170046177, + "velocityY": -0.5090313097502501, + "timestamp": 10.528038334181277 + }, + { + "x": 7.924371719360352, + "y": 4.143174171447754, + "heading": -1.0097437960393402e-29, + "angularVelocity": 0.2970472302531645, + "velocityX": 0.15638796033947933, + "velocityY": -0.4339537397196697, + "timestamp": 10.59926404789862 + }, + { + "x": 7.901061879854102, + "y": 4.117014647255202, + "heading": 0.026867149185379923, + "angularVelocity": 0.3495205221621958, + "velocityX": -0.3032427154636389, + "velocityY": -0.34031487643917635, + "timestamp": 10.676132636384283 + }, + { + "x": 7.842409228188353, + "y": 4.098003471379881, + "heading": 0.0576084810480504, + "angularVelocity": 0.39992059784477624, + "velocityX": -0.7630249601459601, + "velocityY": -0.2473204757606027, + "timestamp": 10.753001224869946 + }, + { + "x": 7.7483989682142225, + "y": 4.086076859183639, + "heading": 0.09201917280173504, + "angularVelocity": 0.4476560898487505, + "velocityX": -1.2229996911113385, + "velocityY": -0.15515586315814983, + "timestamp": 10.829869813355609 + }, + { + "x": 7.619011693227179, + "y": 4.081149603953839, + "heading": 0.12982621784579915, + "angularVelocity": 0.49183998026860576, + "velocityX": -1.6832268880698338, + "velocityY": -0.06409972300609605, + "timestamp": 10.906738401841272 + }, + { + "x": 7.45422085196502, + "y": 4.083102110399606, + "heading": 0.1706474007379839, + "angularVelocity": 0.5310515477957343, + "velocityX": -2.143799496108805, + "velocityY": 0.025400576285222472, + "timestamp": 10.983606990326935 + }, + { + "x": 7.253987975144995, + "y": 4.091754328818402, + "heading": 0.2139089238500665, + "angularVelocity": 0.5627984585686955, + "velocityX": -2.6048725593207736, + "velocityY": 0.1125585702722947, + "timestamp": 11.060475578812598 + }, + { + "x": 7.018252601475449, + "y": 4.106804485403023, + "heading": 0.2586522067646371, + "angularVelocity": 0.5820749905264067, + "velocityX": -3.0667321764795115, + "velocityY": 0.19579072389794971, + "timestamp": 11.137344167298261 + }, + { + "x": 6.746907774405913, + "y": 4.127643106983061, + "heading": 0.30295013129710396, + "angularVelocity": 0.5762812275488699, + "velocityX": -3.5299832144068213, + "velocityY": 0.27109410996826905, + "timestamp": 11.214212755783924 + }, + { + "x": 6.439749090740071, + "y": 4.152402452088509, + "heading": 0.3409535387391942, + "angularVelocity": 0.49439450093686, + "velocityX": -3.9958933774766843, + "velocityY": 0.3220996455537399, + "timestamp": 11.291081344269587 + }, + { + "x": 6.1183816256646795, + "y": 4.146072576447017, + "heading": 0.3409535574726789, + "angularVelocity": 2.4370793128443506e-7, + "velocityX": -4.180738470764694, + "velocityY": -0.08234671360814934, + "timestamp": 11.36794993275525 + }, + { + "x": 5.79974773485045, + "y": 4.103769418408104, + "heading": 0.34095356182725745, + "angularVelocity": 5.664964963804072e-8, + "velocityX": -4.145176815282082, + "velocityY": -0.5503308812129776, + "timestamp": 11.444818521240913 + }, + { + "x": 5.489237994025465, + "y": 4.026346742277124, + "heading": 0.3360287742583908, + "angularVelocity": -0.06406762067427833, + "velocityX": -4.0394880007832965, + "velocityY": -1.0072082453474474, + "timestamp": 11.521687109726576 + }, + { + "x": 5.199567022679866, + "y": 3.922330280425125, + "heading": 0.28775237995316766, + "angularVelocity": -0.6280379964857458, + "velocityX": -3.7683919667605905, + "velocityY": -1.3531725234085639, + "timestamp": 11.59855569821224 + }, + { + "x": 4.938883364377543, + "y": 3.798508366694055, + "heading": 0.21191876629762135, + "angularVelocity": -0.9865357898394373, + "velocityX": -3.391289776980151, + "velocityY": -1.6108259065296096, + "timestamp": 11.675424286697902 + }, + { + "x": 4.709807126039821, + "y": 3.658502172134417, + "heading": 0.11482976825156727, + "angularVelocity": -1.2630516568437093, + "velocityX": -2.9801020527448228, + "velocityY": -1.8213706966370415, + "timestamp": 11.752292875183565 + }, + { + "x": 4.5134758949279785, + "y": 3.504307985305786, + "heading": 1.6717492530612703e-28, + "angularVelocity": -1.4938451520153042, + "velocityX": -2.5541152111627516, + "velocityY": -2.0059453395243487, + "timestamp": 11.829161463669228 + }, + { + "x": 4.400933042470042, + "y": 3.3997056562283166, + "heading": -0.08028643635941543, + "angularVelocity": -1.6227759765270617, + "velocityX": -2.2747533155038484, + "velocityY": -2.1142568335677403, + "timestamp": 11.878636214048043 + }, + { + "x": 4.302937398056285, + "y": 3.291951339386866, + "heading": -0.16507306663801824, + "angularVelocity": -1.7137353827844055, + "velocityX": -1.9807203404449545, + "velocityY": -2.177965851599104, + "timestamp": 11.928110964426859 + }, + { + "x": 4.219869139195696, + "y": 3.1838203852492617, + "heading": -0.2519454962217678, + "angularVelocity": -1.755894247441151, + "velocityX": -1.6790030919722805, + "velocityY": -2.185578569061436, + "timestamp": 11.977585714805674 + }, + { + "x": 4.151481762948963, + "y": 3.0784802030686347, + "heading": -0.3381364984876326, + "angularVelocity": -1.7421210133638232, + "velocityX": -1.382268242348026, + "velocityY": -2.1291705642588132, + "timestamp": 12.027060465184489 + }, + { + "x": 4.096769953274228, + "y": 2.9790758688649013, + "heading": -0.42082279924897953, + "angularVelocity": -1.671282828680057, + "velocityX": -1.1058531726955891, + "velocityY": -2.0091932438793374, + "timestamp": 12.076535215563304 + }, + { + "x": 4.054136857799692, + "y": 2.888285052434389, + "heading": -0.49746057385028863, + "angularVelocity": -1.5490280196365382, + "velocityX": -0.8617142107459757, + "velocityY": -1.8350939769346957, + "timestamp": 12.126009965942119 + }, + { + "x": 4.021745584566135, + "y": 2.808098433899372, + "heading": -0.5658222077410688, + "angularVelocity": -1.3817479293448123, + "velocityX": -0.6547031159438974, + "velocityY": -1.6207584256827539, + "timestamp": 12.175484716320934 + }, + { + "x": 3.997836195630109, + "y": 2.7398606697094916, + "heading": -0.6241866316468845, + "angularVelocity": -1.1796810182756619, + "velocityX": -0.48326446829864, + "velocityY": -1.3792442340264681, + "timestamp": 12.22495946669975 + }, + { + "x": 3.9808795710102305, + "y": 2.684447813014619, + "heading": -0.6715007313655615, + "angularVelocity": -0.9563282150269545, + "velocityX": -0.3427328989039043, + "velocityY": -1.1200229666767407, + "timestamp": 12.274434217078564 + }, + { + "x": 3.9695943024140083, + "y": 2.6424340619644666, + "heading": -0.7072413229342775, + "angularVelocity": -0.722400644673479, + "velocityX": -0.22810157726544558, + "velocityY": -0.8491958166229038, + "timestamp": 12.32390896745738 + }, + { + "x": 3.962913327171321, + "y": 2.61420036575793, + "heading": -0.7311750968478933, + "angularVelocity": -0.48375734552192345, + "velocityX": -0.13503807884896393, + "velocityY": -0.5706687955039338, + "timestamp": 12.373383717836195 + }, + { + "x": 3.959945964813233, + "y": 2.5999999999999996, + "heading": -0.7431853071795862, + "angularVelocity": -0.2427543391272071, + "velocityX": -0.059977308331374074, + "velocityY": -0.2870224841803447, + "timestamp": 12.42285846821501 + }, + { + "x": 3.959945964813233, + "y": 2.5999999999999996, + "heading": -0.7431853071795862, + "angularVelocity": -6.99438774695564e-29, + "velocityX": 3.9418487096305405e-23, + "velocityY": -1.9709900600874517e-23, + "timestamp": 12.472333218593825 + }, + { + "x": 3.909945960323692, + "y": 2.62500000224477, + "heading": -0.7431853071795862, + "angularVelocity": 4.859579089269393e-17, + "velocityX": -0.522546012525329, + "velocityY": 0.2612730062626647, + "timestamp": 12.568018580822319 + }, + { + "x": 3.8099459537390326, + "y": 2.6750000055371, + "heading": -0.7431853071795862, + "angularVelocity": 1.916132911010009e-16, + "velocityX": -1.0450920000267447, + "velocityY": 0.5225460000133729, + "timestamp": 12.663703943050812 + }, + { + "x": 3.6599459506462395, + "y": 2.7500000070834965, + "heading": -0.7431853071795862, + "angularVelocity": 2.752945533306333e-16, + "velocityX": -1.5676379291390343, + "velocityY": 0.7838189645695182, + "timestamp": 12.759389305279306 + }, + { + "x": 3.4599459789802265, + "y": 2.849999992916503, + "heading": -0.7431853071795862, + "angularVelocity": 3.1955367206396795e-16, + "velocityX": -2.090183566305787, + "velocityY": 1.0450917831528945, + "timestamp": 12.8550746675078 + }, + { + "x": 3.309945975887433, + "y": 2.9249999944628997, + "heading": -0.7431853071795862, + "angularVelocity": 2.9470794803491527e-16, + "velocityX": -1.5676379291390345, + "velocityY": 0.783818964569518, + "timestamp": 12.950760029736294 + }, + { + "x": 3.2099459693027734, + "y": 2.9749999977552295, + "heading": -0.7431853071795862, + "angularVelocity": 2.1840033759327932e-16, + "velocityX": -1.0450920000267447, + "velocityY": 0.5225460000133729, + "timestamp": 13.046445391964788 + }, + { + "x": 3.1599459648132324, + "y": 3, + "heading": -0.7431853071795862, + "angularVelocity": 1.4984441245497084e-16, + "velocityX": -0.522546012525329, + "velocityY": 0.26127300626266475, + "timestamp": 13.142130754193282 + }, + { + "x": 3.1599459648132324, + "y": 3, + "heading": -0.7431853071795862, + "angularVelocity": -1.1440552586496935e-29, + "velocityX": -3.941408736271529e-23, + "velocityY": 1.9707570184447103e-23, + "timestamp": 13.237816116421776 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.6802782416343689, + "y": 4.38139533996582, + "heading": -1.0378726274695866, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "timestamp": 1.3444417435934044, + "isStopPoint": false, + "x": 3.517277717590332, + "y": 1.2412056922912598, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "timestamp": 2.7358004856496816, + "isStopPoint": false, + "x": 7.967684268951416, + "y": 0.8080758452415466, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "timestamp": 3.874896381274637, + "isStopPoint": false, + "x": 4.870807647705078, + "y": 1.8692435026168823, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 4.743075979839502, + "isStopPoint": true, + "x": 3.1599457263946533, + "y": 3, + "heading": -0.7431853071795862, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 5.572261226613519, + "isStopPoint": false, + "x": 4.870807647705078, + "y": 1.8692435026168823, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 5.936767568134546, + "isStopPoint": false, + "x": 6.3759331703186035, + "y": 1.8584154844284058, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "timestamp": 6.643008536146009, + "isStopPoint": false, + "x": 7.924371719360352, + "y": 2.432312250137329, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "timestamp": 7.410023676747687, + "isStopPoint": false, + "x": 6.3759331703186035, + "y": 1.8584154844284058, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 7.774212260666386, + "isStopPoint": false, + "x": 4.870807647705078, + "y": 1.8692435026168823, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 8.652246509227574, + "isStopPoint": true, + "x": 3.1599459648132324, + "y": 3, + "heading": -0.7431853071795862, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "timestamp": 9.340480969901002, + "isStopPoint": false, + "x": 4.5134758949279785, + "y": 3.504307985305786, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 9.602104055855817, + "isStopPoint": false, + "x": 5.336422443389893, + "y": 4.143174171447754, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "timestamp": 10.59926404789862, + "isStopPoint": false, + "x": 7.924371719360352, + "y": 4.143174171447754, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 11.829161463669228, + "isStopPoint": false, + "x": 4.5134758949279785, + "y": 3.504307985305786, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 12.472333218593825, + "isStopPoint": true, + "x": 3.959945964813233, + "y": 2.5999999999999996, + "heading": -0.7431853071795862, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "timestamp": 13.237816116421776, + "isStopPoint": true, + "x": 3.1599459648132324, + "y": 3, + "heading": -0.7431853071795862, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint" + }, + { + "scope": [ + 10 + ], + "type": "StopPoint" + }, + { + "scope": [ + 15 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": true + }, + "Distance Center 6": { + "waypoints": [ + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 2.542736291885376, + "y": 6.991001605987549, + "heading": 0.610560190519, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "x": 7.946028232574463, + "y": 7.456615924835205, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 6.018601417541504, + "y": 6.676982402801514, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 7.891887187957764, + "y": 5.810723304748535, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 6.018601417541504, + "y": 6.676982402801514, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 1.990496039390564, + "y": 6.157227039337158, + "heading": -0.6763337151495863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "x": 2.4885952472686768, + "y": 5.583330154418945, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 1.990496039390564, + "y": 4.75, + "heading": -0.6763337151495863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 2.477766990661621, + "y": 4.240628242492676, + "heading": -0.6763337151495863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 1.823684249569662e-28, + "angularVelocity": 1.4442982636716686e-28, + "velocityX": -9.525967417566346e-28, + "velocityY": -1.281458100821258e-28, + "timestamp": 0 + }, + { + "x": 1.3712979426432907, + "y": 5.607416241994932, + "heading": 0.010815420235994461, + "angularVelocity": 0.14684104373961251, + "velocityX": 0.28916233109823775, + "velocityY": 0.3415900192895115, + "timestamp": 0.07365393190185317 + }, + { + "x": 1.413894107001652, + "y": 5.657734961213299, + "heading": 0.03244400597180746, + "angularVelocity": 0.2936514749088203, + "velocityX": 0.5783284511561763, + "velocityY": 0.6831776378947206, + "timestamp": 0.14730786380370633 + }, + { + "x": 1.4777889053988098, + "y": 5.733212662791941, + "heading": 0.06488357959528479, + "angularVelocity": 0.44043234062105985, + "velocityX": 0.8675001693365169, + "velocityY": 1.02476133493076, + "timestamp": 0.22096179570555952 + }, + { + "x": 1.5629828503604866, + "y": 5.8338489085152085, + "heading": 0.10813427799106332, + "angularVelocity": 0.5872150648170668, + "velocityX": 1.1566788460825288, + "velocityY": 1.3663390823095432, + "timestamp": 0.29461572760741267 + }, + { + "x": 1.6694764937636932, + "y": 5.959643075254826, + "heading": 0.16220151122014106, + "angularVelocity": 0.73407124145286, + "velocityX": 1.445865015666968, + "velocityY": 1.707908369470939, + "timestamp": 0.3682696595092658 + }, + { + "x": 1.7972703292010448, + "y": 6.110594337397887, + "heading": 0.22709931173769698, + "angularVelocity": 0.8811179368405597, + "velocityX": 1.7350578867621362, + "velocityY": 2.0494664472795554, + "timestamp": 0.441923591411119 + }, + { + "x": 1.9463646521461748, + "y": 6.286701685728506, + "heading": 0.3028533408965006, + "angularVelocity": 1.0285130366122057, + "velocityX": 2.0242547695051, + "velocityY": 2.391010822956316, + "timestamp": 0.5155775233129721 + }, + { + "x": 2.0954593391167733, + "y": 6.462777598713869, + "heading": 0.3796919635365941, + "angularVelocity": 1.0432385706507032, + "velocityX": 2.024259711881685, + "velocityY": 2.390584025031971, + "timestamp": 0.5892314552148253 + }, + { + "x": 2.2232538317965016, + "y": 6.613699108565928, + "heading": 0.4456074038458184, + "angularVelocity": 0.8949344401200363, + "velocityX": 1.735066810146942, + "velocityY": 2.0490625001957663, + "timestamp": 0.6628853871166785 + }, + { + "x": 2.3297485254769024, + "y": 6.739466694851569, + "heading": 0.5005708250556786, + "angularVelocity": 0.7462387925616956, + "velocityX": 1.4458792752885092, + "velocityY": 1.7075474864428304, + "timestamp": 0.7365393190185318 + }, + { + "x": 2.414943830059909, + "y": 6.840080642329488, + "heading": 0.544560026646676, + "angularVelocity": 0.5972417283793449, + "velocityX": 1.1566973056717835, + "velocityY": 1.3660363388609074, + "timestamp": 0.810193250920385 + }, + { + "x": 2.4788400911601216, + "y": 6.915541090400898, + "heading": 0.5775596586986871, + "angularVelocity": 0.448036258213403, + "velocityX": 0.8675200284671415, + "velocityY": 1.0245270839303438, + "timestamp": 0.8838471828222382 + }, + { + "x": 2.5214375444702517, + "y": 6.9658480830621485, + "heading": 0.5995607651691287, + "angularVelocity": 0.2987091917884169, + "velocityX": 0.578345951264258, + "velocityY": 0.6830184263385521, + "timestamp": 0.9575011147240914 + }, + { + "x": 2.542736291885376, + "y": 6.991001605987549, + "heading": 0.610560190519, + "angularVelocity": 0.14933928258614232, + "velocityX": 0.2891732574916107, + "velocityY": 0.34150957424673456, + "timestamp": 1.0311550466259445 + }, + { + "x": 2.542736291885376, + "y": 6.991001605987549, + "heading": 0.610560190519, + "angularVelocity": -1.9066998701598976e-24, + "velocityX": -9.94898252689215e-24, + "velocityY": 6.590062338164459e-24, + "timestamp": 1.1048089785277977 + }, + { + "x": 2.5860882804994176, + "y": 6.997986329604455, + "heading": 0.6040705470130452, + "angularVelocity": -0.07648715527237229, + "velocityX": 0.5109479868109409, + "velocityY": 0.08232218600769788, + "timestamp": 1.189655165875792 + }, + { + "x": 2.6727971429282147, + "y": 7.011957094513919, + "heading": 0.5914830277795109, + "angularVelocity": -0.1483569224142865, + "velocityX": 1.0219535507607762, + "velocityY": 0.16465990218469131, + "timestamp": 1.274501353223786 + }, + { + "x": 2.8028689447073436, + "y": 7.0329156007946185, + "heading": 0.5733232484637217, + "angularVelocity": -0.21403176599211712, + "velocityX": 1.533030603315665, + "velocityY": 0.2470176555457783, + "timestamp": 1.3593475405717803 + }, + { + "x": 2.976311299631257, + "y": 7.0608641143026825, + "heading": 0.550333584482233, + "angularVelocity": -0.2709569480971165, + "velocityX": 2.044197392306438, + "velocityY": 0.3294021143629483, + "timestamp": 1.4441937279197745 + }, + { + "x": 3.1931336411091458, + "y": 7.095805765761892, + "heading": 0.5236415617438719, + "angularVelocity": -0.3145930721540211, + "velocityX": 2.5554753637732515, + "velocityY": 0.4118234719952363, + "timestamp": 1.5290399152677687 + }, + { + "x": 3.4533461169129187, + "y": 7.137744979228221, + "heading": 0.49516279138937963, + "angularVelocity": -0.3356517392783657, + "velocityX": 3.0668729372189607, + "velocityY": 0.4942969716991197, + "timestamp": 1.613886102615763 + }, + { + "x": 3.7569462055816967, + "y": 7.1866871771577365, + "heading": 0.4688699375076152, + "angularVelocity": -0.3098884546682701, + "velocityX": 3.5782407926424726, + "velocityY": 0.576834380651426, + "timestamp": 1.6987322899637571 + }, + { + "x": 4.1036936295460995, + "y": 7.242619011134652, + "heading": 0.4580775990586799, + "angularVelocity": -0.12719886168450764, + "velocityX": 4.086776728602178, + "velocityY": 0.6592144647291258, + "timestamp": 1.7835784773117513 + }, + { + "x": 4.453970286773956, + "y": 7.299020832288215, + "heading": 0.45807758779565816, + "angularVelocity": -1.3274634993655314e-7, + "velocityX": 4.128372389807061, + "velocityY": 0.664753749302052, + "timestamp": 1.8684246646597455 + }, + { + "x": 4.804246944243858, + "y": 7.355422651938624, + "heading": 0.45807757653265324, + "angularVelocity": -1.3274615245620227e-7, + "velocityX": 4.128372392659827, + "velocityY": 0.6647537315858247, + "timestamp": 1.9532708520077398 + }, + { + "x": 5.1545236017137634, + "y": 7.41182447158901, + "heading": 0.4580775652696484, + "angularVelocity": -1.327461517119493e-7, + "velocityX": 4.128372392659871, + "velocityY": 0.66475373158555, + "timestamp": 2.038117039355734 + }, + { + "x": 5.5048002591836696, + "y": 7.468226291239396, + "heading": 0.45807755400664346, + "angularVelocity": -1.3274615213673127e-7, + "velocityX": 4.128372392659871, + "velocityY": 0.6647537315855488, + "timestamp": 2.122963226703728 + }, + { + "x": 5.855076916654743, + "y": 7.524628110882535, + "heading": 0.4580775427436386, + "angularVelocity": -1.3274615192395048e-7, + "velocityX": 4.128372392673625, + "velocityY": 0.664753731500135, + "timestamp": 2.2078094140517224 + }, + { + "x": 6.205353649404039, + "y": 7.5810294630174075, + "heading": 0.45807753148063285, + "angularVelocity": -1.3274616274715188e-7, + "velocityX": 4.128373279905273, + "velocityY": 0.6647482214320765, + "timestamp": 2.2926556013997166 + }, + { + "x": 6.558504100556125, + "y": 7.6150832124269465, + "heading": 0.45807750773089817, + "angularVelocity": -2.7991516688896703e-7, + "velocityX": 4.162243021052327, + "velocityY": 0.4013586287604083, + "timestamp": 2.377501788747711 + }, + { + "x": 6.882961359930591, + "y": 7.623821022982591, + "heading": 0.38150693231968036, + "angularVelocity": -0.9024633611073862, + "velocityX": 3.8240641037140963, + "velocityY": 0.10298412726321374, + "timestamp": 2.462347976095705 + }, + { + "x": 7.164467598037234, + "y": 7.623239648813218, + "heading": 0.3068146622390825, + "angularVelocity": -0.8803255916998344, + "velocityX": 3.317841931447713, + "velocityY": -0.006852095392192507, + "timestamp": 2.547194163443699 + }, + { + "x": 7.402821150081126, + "y": 7.614460764318352, + "heading": 0.23751214172935955, + "angularVelocity": -0.8168018231093954, + "velocityX": 2.8092429311678027, + "velocityY": -0.10346822608374351, + "timestamp": 2.6320403507916934 + }, + { + "x": 7.597964025430219, + "y": 7.597880541854749, + "heading": 0.1748380247592176, + "angularVelocity": -0.7386792374427614, + "velocityX": 2.299960451360303, + "velocityY": -0.19541505613681964, + "timestamp": 2.7168865381396876 + }, + { + "x": 7.749869224899896, + "y": 7.5737014715455055, + "heading": 0.11942485607561616, + "angularVelocity": -0.6531014582461551, + "velocityX": 1.790359758260474, + "velocityY": -0.2849753308309952, + "timestamp": 2.801732725487682 + }, + { + "x": 7.8585212115542395, + "y": 7.542046503417778, + "heading": 0.07165557432406013, + "angularVelocity": -0.563010351374208, + "velocityX": 1.2805759463145991, + "velocityY": -0.3730865123956133, + "timestamp": 2.886578912835676 + }, + { + "x": 7.923909903080463, + "y": 7.502998223175264, + "heading": 0.03178626375944312, + "angularVelocity": -0.46990102691467095, + "velocityX": 0.7706733038932455, + "velocityY": -0.46022433609607355, + "timestamp": 2.9714251001836702 + }, + { + "x": 7.946028232574463, + "y": 7.456615924835205, + "heading": 4.872348893999645e-24, + "angularVelocity": -0.3746339670994609, + "velocityX": 0.2606873707039118, + "velocityY": -0.5466633185274797, + "timestamp": 3.0562712875316644 + }, + { + "x": 7.922039225710158, + "y": 7.400326502118091, + "heading": -0.024206358094073104, + "angularVelocity": -0.2736076214341235, + "velocityX": -0.271150872146958, + "velocityY": -0.6362466836884642, + "timestamp": 3.1447423541167057 + }, + { + "x": 7.8509861658143585, + "y": 7.336150571120965, + "heading": -0.039787388549924835, + "angularVelocity": -0.17611441861475394, + "velocityX": -0.8031219995239999, + "velocityY": -0.7253889149786428, + "timestamp": 3.233213420701747 + }, + { + "x": 7.732853496113255, + "y": 7.264141046256132, + "heading": -0.04716434286147303, + "angularVelocity": -0.08338267635167741, + "velocityX": -1.3352689671436382, + "velocityY": -0.8139330477678314, + "timestamp": 3.321684487286788 + }, + { + "x": 7.5676196522986485, + "y": 7.1843737716186435, + "heading": -0.04693804499155485, + "angularVelocity": 0.002557874327203416, + "velocityX": -1.8676596789502777, + "velocityY": -0.9016199048624852, + "timestamp": 3.4101555538718293 + }, + { + "x": 7.355252804098637, + "y": 7.096966555657978, + "heading": -0.040037308652199996, + "angularVelocity": 0.07799992252519808, + "velocityX": -2.400410172470089, + "velocityY": -0.9879751576934634, + "timestamp": 3.4986266204568706 + }, + { + "x": 7.09570163072694, + "y": 7.002127255801257, + "heading": -0.028090499343208248, + "angularVelocity": 0.13503634318128263, + "velocityX": -2.933740751528163, + "velocityY": -1.071980970926346, + "timestamp": 3.587097687041912 + }, + { + "x": 6.78887318109941, + "y": 6.900320277003001, + "heading": -0.014696086966441679, + "angularVelocity": 0.1513987893871656, + "velocityX": -3.4681219688088203, + "velocityY": -1.1507375544116039, + "timestamp": 3.675568753626953 + }, + { + "x": 6.434688295787984, + "y": 6.793487938945866, + "heading": -0.014695897490859833, + "angularVelocity": 0.000002141667204427998, + "velocityX": -4.003397935425274, + "velocityY": -1.2075398452944421, + "timestamp": 3.7640398202119942 + }, + { + "x": 6.07023453147431, + "y": 6.729977275374465, + "heading": -0.014695885109107147, + "angularVelocity": 1.3995256487596975e-7, + "velocityX": -4.119468413589764, + "velocityY": -0.7178693105316132, + "timestamp": 3.8525108867970355 + }, + { + "x": 5.70577998818262, + "y": 6.666471082103607, + "heading": -0.014695872727371504, + "angularVelocity": 1.39952372261179e-7, + "velocityX": -4.119477218479835, + "velocityY": -0.7178187821417759, + "timestamp": 3.9409819533820767 + }, + { + "x": 5.341325440621852, + "y": 6.602964913334049, + "heading": -0.014695860345028063, + "angularVelocity": 1.399592422510824e-7, + "velocityX": -4.119477266733788, + "velocityY": -0.7178185052003817, + "timestamp": 4.029453019967118 + }, + { + "x": 5.012545454341452, + "y": 6.545672600238492, + "heading": 0.062448454910025174, + "angularVelocity": 0.8719722529952728, + "velocityX": -3.716243049521347, + "velocityY": -0.6475824843875421, + "timestamp": 4.11792408655216 + }, + { + "x": 4.730734074716532, + "y": 6.496564334374803, + "heading": 0.12856972259022606, + "angularVelocity": 0.7473773091301318, + "velocityX": -3.18535076497619, + "velocityY": -0.5550771315329972, + "timestamp": 4.206395153137201 + }, + { + "x": 4.495891252721745, + "y": 6.455640399436969, + "heading": 0.183666717807647, + "angularVelocity": 0.6227685201970513, + "velocityX": -2.6544590345709054, + "velocityY": -0.4625685720483159, + "timestamp": 4.294866219722243 + }, + { + "x": 4.30801697716886, + "y": 6.422901026579304, + "heading": 0.22774087166689647, + "angularVelocity": 0.4981759072259397, + "velocityX": -2.123567430627664, + "velocityY": -0.3700573997962927, + "timestamp": 4.383337286307285 + }, + { + "x": 4.167111258222206, + "y": 6.398346386946046, + "heading": 0.26079453276814246, + "angularVelocity": 0.3736098407887256, + "velocityX": -1.5926757117956818, + "velocityY": -0.2775442930786397, + "timestamp": 4.471808352892326 + }, + { + "x": 4.073174110202746, + "y": 6.38197659167395, + "heading": 0.28282976197594306, + "angularVelocity": 0.249067068572296, + "velocityX": -1.061783831091989, + "velocityY": -0.18502993016773903, + "timestamp": 4.560279419477368 + }, + { + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "angularVelocity": 0.12453576635098247, + "velocityX": -0.5308918758680198, + "velocityY": -0.09251495826569413, + "timestamp": 4.64875048606241 + }, + { + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "angularVelocity": -1.7260583733230727e-24, + "velocityX": -2.8721435656913086e-23, + "velocityY": 9.699536718146445e-24, + "timestamp": 4.737221552647451 + }, + { + "x": 4.048121646310105, + "y": 6.381836946527732, + "heading": 0.2879220023652063, + "angularVelocity": -0.09569026109228784, + "velocityX": 0.35391656262382837, + "velocityY": 0.12992033412904042, + "timestamp": 4.799146053846991 + }, + { + "x": 4.092109802394514, + "y": 6.39750532464796, + "heading": 0.27630914546149715, + "angularVelocity": -0.1875325061769808, + "velocityX": 0.7103513994027256, + "velocityY": 0.25302388903770934, + "timestamp": 4.861070555046531 + }, + { + "x": 4.158348380070515, + "y": 6.420279601349659, + "heading": 0.2593011930834734, + "angularVelocity": -0.27465626768989004, + "velocityX": 1.0696667133831064, + "velocityY": 0.36777489136832514, + "timestamp": 4.922995056246071 + }, + { + "x": 4.247041342694878, + "y": 6.449512566443521, + "heading": 0.23726388208727434, + "angularVelocity": -0.35587385557112505, + "velocityX": 1.4322757697888755, + "velocityY": 0.4720742925270236, + "timestamp": 4.984919557445611 + }, + { + "x": 4.358419967210315, + "y": 6.484373638974516, + "heading": 0.21066668095834623, + "angularVelocity": -0.42951013918099495, + "velocityX": 1.7986196474403566, + "velocityY": 0.5629608935994745, + "timestamp": 5.046844058645151 + }, + { + "x": 4.492739232760998, + "y": 6.523763810645006, + "heading": 0.1801310666057291, + "angularVelocity": -0.4931103805619993, + "velocityX": 2.1690811060046427, + "velocityY": 0.6360999427926483, + "timestamp": 5.108768559844691 + }, + { + "x": 4.650257629888207, + "y": 6.566174217155756, + "heading": 0.14651122621435186, + "angularVelocity": -0.5429166120053787, + "velocityX": 2.543716849969224, + "velocityY": 0.6848727997677386, + "timestamp": 5.170693061044231 + }, + { + "x": 4.831166826127315, + "y": 6.609441866820435, + "heading": 0.11103492926681961, + "angularVelocity": -0.5728959662221038, + "velocityX": 2.9214477748663974, + "velocityY": 0.6987161596224655, + "timestamp": 5.232617562243771 + }, + { + "x": 5.035367402593747, + "y": 6.650325238947526, + "heading": 0.07555553837017441, + "angularVelocity": -0.572945929468525, + "velocityX": 3.297573214331347, + "velocityY": 0.6602131843638516, + "timestamp": 5.294542063443311 + }, + { + "x": 5.261782084531844, + "y": 6.683862940799087, + "heading": 0.04296427542789473, + "angularVelocity": -0.5263064265509467, + "velocityX": 3.656302070298779, + "velocityY": 0.5415901816228108, + "timestamp": 5.356466564642851 + }, + { + "x": 5.5066279243005445, + "y": 6.7031170566524425, + "heading": 0.017406971953643092, + "angularVelocity": -0.41271714715792507, + "velocityX": 3.953941251456059, + "velocityY": 0.31092888082073744, + "timestamp": 5.418391065842391 + }, + { + "x": 5.761751882534451, + "y": 6.701608504256703, + "heading": 0.0031721000803410847, + "angularVelocity": -0.22987463116469584, + "velocityX": 4.119919471160824, + "velocityY": -0.024361155383045233, + "timestamp": 5.480315567041931 + }, + { + "x": 6.018601417541504, + "y": 6.676982402801514, + "heading": 1.3856044746201851e-28, + "angularVelocity": -0.051225282705460874, + "velocityX": 4.1477852874325745, + "velocityY": -0.39767944800776983, + "timestamp": 5.542240068241471 + }, + { + "x": 6.298785976787896, + "y": 6.621524323291068, + "heading": -1.1062883462477051e-7, + "angularVelocity": -0.0000016196318539939486, + "velocityX": 4.1019670747856205, + "velocityY": -0.8119191749701038, + "timestamp": 5.610544994667785 + }, + { + "x": 6.570847588785804, + "y": 6.538762513850569, + "heading": -1.7625665609767657e-7, + "angularVelocity": -9.608065612032315e-7, + "velocityX": 3.983045238931686, + "velocityY": -1.2116521277536396, + "timestamp": 5.678849921094098 + }, + { + "x": 6.822289575216137, + "y": 6.436346015436509, + "heading": -1.9920369427970006e-7, + "angularVelocity": -3.359499729024442e-7, + "velocityX": 3.681169127699538, + "velocityY": -1.4994013429550492, + "timestamp": 5.747154847520412 + }, + { + "x": 7.046075124873892, + "y": 6.327103284773838, + "heading": -1.9303612449853094e-7, + "angularVelocity": 9.029465521528115e-8, + "velocityX": 3.2762724647565618, + "velocityY": -1.5993389697962725, + "timestamp": 5.815459773946726 + }, + { + "x": 7.24149070801809, + "y": 6.220433729890437, + "heading": -1.707914860870973e-7, + "angularVelocity": 3.256666769918985e-7, + "velocityX": 2.860929560549486, + "velocityY": -1.5616670782666768, + "timestamp": 5.88376470037304 + }, + { + "x": 7.409535890143074, + "y": 6.121659591172554, + "heading": -1.4083076615117047e-7, + "angularVelocity": 4.3863190407280574e-7, + "velocityX": 2.4602205275232807, + "velocityY": -1.446076350355775, + "timestamp": 5.952069626799354 + }, + { + "x": 7.551301991175276, + "y": 6.033880291623994, + "heading": -1.0837809038008853e-7, + "angularVelocity": 4.7511471674142645e-7, + "velocityX": 2.0754886718915797, + "velocityY": -1.285109349224681, + "timestamp": 6.020374553225667 + }, + { + "x": 7.6676940976057, + "y": 5.959048466238134, + "heading": -7.68763569270851e-8, + "angularVelocity": 4.6119270016324603e-7, + "velocityX": 1.704007492870772, + "velocityY": -1.0955553179109054, + "timestamp": 6.088679479651981 + }, + { + "x": 7.75943039247277, + "y": 5.898484356691668, + "heading": -4.872749406160536e-8, + "angularVelocity": 4.1210589540487035e-7, + "velocityX": 1.3430406804702926, + "velocityY": -0.8866726415670821, + "timestamp": 6.156984406078295 + }, + { + "x": 7.827081854833763, + "y": 5.853132007538663, + "heading": -2.56890633614765e-8, + "angularVelocity": 3.372879806112141e-7, + "velocityX": 0.9904331341894409, + "velocityY": -0.6639689335136235, + "timestamp": 6.225289332504609 + }, + { + "x": 7.871108582498892, + "y": 5.823696591560291, + "heading": -9.097335168428086e-9, + "angularVelocity": 2.4290675740566603e-7, + "velocityX": 0.6445615267974086, + "velocityY": -0.43094133203006635, + "timestamp": 6.293594258930923 + }, + { + "x": 7.891887187957764, + "y": 5.810723304748535, + "heading": 7.7887817466186635e-28, + "angularVelocity": 1.3318710149321612e-7, + "velocityX": 0.30420361379478605, + "velocityY": -0.18993193449599038, + "timestamp": 6.361899185357236 + }, + { + "x": 7.888143888902488, + "y": 5.815954260813035, + "heading": 2.765818870905722e-10, + "angularVelocity": 3.82019160949285e-9, + "velocityX": -0.051703022902955066, + "velocityY": 0.07225077056721455, + "timestamp": 6.434299189169424 + }, + { + "x": 7.858277690270956, + "y": 5.839675291870993, + "heading": -8.025005255471161e-9, + "angularVelocity": -1.1466279979897161e-7, + "velocityX": -0.41251653396327165, + "velocityY": 0.3276385332726272, + "timestamp": 6.506699192981612 + }, + { + "x": 7.801862071405289, + "y": 5.881268564071655, + "heading": -2.3925602238115484e-8, + "angularVelocity": -2.1962149372107496e-7, + "velocityX": -0.7792212140211278, + "velocityY": 0.5744926797042514, + "timestamp": 6.5790991967938 + }, + { + "x": 7.718377433983251, + "y": 5.939936873772295, + "heading": -4.617816836254086e-8, + "angularVelocity": -3.0735586951280266e-7, + "velocityX": -1.1531026661076393, + "velocityY": 0.8103357266780017, + "timestamp": 6.651499200605988 + }, + { + "x": 7.6071812503843494, + "y": 6.014616392636198, + "heading": -7.314569126794996e-8, + "angularVelocity": -3.7247957852827054e-7, + "velocityX": -1.5358588086176677, + "velocityY": 1.03148501286863, + "timestamp": 6.723899204418176 + }, + { + "x": 7.467468982650147, + "y": 6.1038259312718015, + "heading": -1.0259692735622503e-7, + "angularVelocity": -4.06785007424503e-7, + "velocityX": -1.929727353283392, + "velocityY": 1.232175883125927, + "timestamp": 6.796299208230364 + }, + { + "x": 7.298232339066578, + "y": 6.205388592850735, + "heading": -1.3134543452628007e-7, + "angularVelocity": -3.9707880740767756e-7, + "velocityX": -2.33752257834935, + "velocityY": 1.402799119215454, + "timestamp": 6.868699212042552 + }, + { + "x": 7.098262109849036, + "y": 6.315880698175479, + "heading": -1.545804802746847e-7, + "angularVelocity": -3.209260293504718e-7, + "velocityX": -2.7620195951409254, + "velocityY": 1.52613397108887, + "timestamp": 6.94109921585474 + }, + { + "x": 6.866440435797629, + "y": 6.429499058364546, + "heading": -1.6464439215056105e-7, + "angularVelocity": -1.3900430035864317e-7, + "velocityX": -3.201956655316912, + "velocityY": 1.5693142846207861, + "timestamp": 7.013499219666928 + }, + { + "x": 6.603388966204105, + "y": 6.536118308127826, + "heading": -1.4954692300634435e-7, + "angularVelocity": 2.085285683600351e-7, + "velocityX": -3.63330739976067, + "velocityY": 1.47264149377475, + "timestamp": 7.085899223479116 + }, + { + "x": 6.316248920025813, + "y": 6.6216655877567385, + "heading": -9.596874319673351e-8, + "angularVelocity": 7.400300689016137e-7, + "velocityX": -3.966022528440158, + "velocityY": 1.181592197851674, + "timestamp": 7.1582992272913035 + }, + { + "x": 6.018601417541504, + "y": 6.676982402801514, + "heading": 3.619661729671041e-26, + "angularVelocity": 0.000001325535057231274, + "velocityX": -4.111153132759927, + "velocityY": 0.7640443664653904, + "timestamp": 7.230699231103491 + }, + { + "x": 5.740842709923353, + "y": 6.700963706149043, + "heading": 0.0018237295021199065, + "angularVelocity": 0.027302725220575197, + "velocityX": -4.158275480494951, + "velocityY": 0.35901976415240183, + "timestamp": 7.297495841602489 + }, + { + "x": 5.465613780315343, + "y": 6.698113446403688, + "heading": 0.01705683549279003, + "angularVelocity": 0.2280520804405013, + "velocityX": -4.120402630491888, + "velocityY": -0.04267072421882417, + "timestamp": 7.364292452101486 + }, + { + "x": 5.204358543644784, + "y": 6.6722541835752125, + "heading": 0.046262668986232006, + "angularVelocity": 0.4372352620179803, + "velocityX": -3.911204995566067, + "velocityY": -0.3871343565982821, + "timestamp": 7.431089062600483 + }, + { + "x": 4.966474467989701, + "y": 6.632584406687531, + "heading": 0.08307800660337901, + "angularVelocity": 0.5511557748532727, + "velocityX": -3.561319562145362, + "velocityY": -0.593889069989206, + "timestamp": 7.49788567309948 + }, + { + "x": 4.755212132064066, + "y": 6.587176716637979, + "heading": 0.12225145158563697, + "angularVelocity": 0.5864585746135442, + "velocityX": -3.162770301478206, + "velocityY": -0.6797903323288379, + "timestamp": 7.5646822835984775 + }, + { + "x": 4.571183536225408, + "y": 6.5411456783481245, + "heading": 0.16051492442030083, + "angularVelocity": 0.572835545828158, + "velocityX": -2.7550588939152947, + "velocityY": -0.689122366329426, + "timestamp": 7.631478894097475 + }, + { + "x": 4.414262802682262, + "y": 6.497671625821495, + "heading": 0.19585610790995653, + "angularVelocity": 0.5290864794731811, + "velocityX": -2.3492319800493826, + "velocityY": -0.6508421939655534, + "timestamp": 7.698275504596472 + }, + { + "x": 4.284155725885966, + "y": 6.458840625879828, + "heading": 0.22695656960626623, + "angularVelocity": 0.46559939889130514, + "velocityX": -1.9478095643528162, + "velocityY": -0.581331891717008, + "timestamp": 7.765072115095469 + }, + { + "x": 4.180556238781294, + "y": 6.426101360577794, + "heading": 0.2528995134208406, + "angularVelocity": 0.3883871295380163, + "velocityX": -1.5509692232995544, + "velocityY": -0.4901336318932807, + "timestamp": 7.831868725594466 + }, + { + "x": 4.103186927937392, + "y": 6.4005094673645, + "heading": 0.27301572769159327, + "angularVelocity": 0.3011562131742396, + "velocityX": -1.1582819886506504, + "velocityY": -0.38313161434557874, + "timestamp": 7.8986653360934636 + }, + { + "x": 4.05180613335198, + "y": 6.382864598687039, + "heading": 0.2867974869909083, + "angularVelocity": 0.20632423107040648, + "velocityX": -0.7692126022799183, + "velocityY": -0.26415814433767054, + "timestamp": 7.965461946592461 + }, + { + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "angularVelocity": 0.1055455809722198, + "velocityX": -0.383261866992415, + "velocityY": -0.13582880894924632, + "timestamp": 8.032258557091458 + }, + { + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "angularVelocity": -2.6570810296558003e-27, + "velocityX": -7.92943357698847e-27, + "velocityY": 1.86287757068553e-26, + "timestamp": 8.099055167590455 + }, + { + "x": 4.001912054553803, + "y": 6.377610355104089, + "heading": 0.27793057177759545, + "angularVelocity": -0.248523754884058, + "velocityX": -0.37931188575122854, + "velocityY": 0.05962352837962387, + "timestamp": 8.16310136825578 + }, + { + "x": 3.9532989917689947, + "y": 6.385094713412846, + "heading": 0.24611473380028503, + "angularVelocity": -0.49676386181851123, + "velocityX": -0.7590311724943191, + "velocityY": 0.11685873995658545, + "timestamp": 8.227147568921106 + }, + { + "x": 3.8803348270353966, + "y": 6.396044749749356, + "heading": 0.19840169352590004, + "angularVelocity": -0.7449784652131087, + "velocityX": -1.1392426713158736, + "velocityY": 0.17097089636479054, + "timestamp": 8.291193769586432 + }, + { + "x": 3.7829788345403452, + "y": 6.410188068322517, + "heading": 0.13480855568773095, + "angularVelocity": -0.992925999943011, + "velocityX": -1.520090051926524, + "velocityY": 0.22082993879789267, + "timestamp": 8.355239970251757 + }, + { + "x": 3.6611743486827018, + "y": 6.427129765366101, + "heading": 0.0554387772249063, + "angularVelocity": -1.239258186095577, + "velocityX": -1.9018221938586934, + "velocityY": 0.264523060971458, + "timestamp": 8.419286170917083 + }, + { + "x": 3.5148377265715247, + "y": 6.446239524609827, + "heading": -0.0393712537936742, + "angularVelocity": -1.4803381002100633, + "velocityX": -2.284860313195802, + "velocityY": 0.29837459591997334, + "timestamp": 8.483332371582408 + }, + { + "x": 3.343848310096575, + "y": 6.466345188072757, + "heading": -0.14873635590657566, + "angularVelocity": -1.707597031155534, + "velocityX": -2.6697823555289077, + "velocityY": 0.3139243741871967, + "timestamp": 8.547378572247734 + }, + { + "x": 3.14815023920896, + "y": 6.484576821653784, + "heading": -0.2703054170409716, + "angularVelocity": -1.8981463361059787, + "velocityX": -3.0555765815093188, + "velocityY": 0.2846637800780145, + "timestamp": 8.61142477291306 + }, + { + "x": 2.932193940008419, + "y": 6.488086466497611, + "heading": -0.39300827574348024, + "angularVelocity": -1.9158491437094674, + "velocityX": -3.3718830618700366, + "velocityY": 0.05479864234519086, + "timestamp": 8.675470973578385 + }, + { + "x": 2.7340316065376515, + "y": 6.475211847713753, + "heading": -0.4935227085605528, + "angularVelocity": -1.5694050821579926, + "velocityX": -3.094052908872265, + "velocityY": -0.20102080451476154, + "timestamp": 8.73951717424371 + }, + { + "x": 2.5578914607417174, + "y": 6.452000594790772, + "heading": -0.5736763053972732, + "angularVelocity": -1.251496513517863, + "velocityX": -2.750204445636945, + "velocityY": -0.36241420539950464, + "timestamp": 8.803563374909036 + }, + { + "x": 2.4046623857559206, + "y": 6.420343190075613, + "heading": -0.6349490103430218, + "angularVelocity": -0.9566953903468849, + "velocityX": -2.392477202301163, + "velocityY": -0.4942901278498211, + "timestamp": 8.867609575574361 + }, + { + "x": 2.274706473608676, + "y": 6.381144596576831, + "heading": -0.6781364741434692, + "angularVelocity": -0.6743173420406988, + "velocityX": -2.0290963522775014, + "velocityY": -0.6120362034215643, + "timestamp": 8.931655776239687 + }, + { + "x": 2.1682152530987344, + "y": 6.3349342062757374, + "heading": -0.7036879484930157, + "angularVelocity": -0.3989537877986885, + "velocityX": -1.6627250235562485, + "velocityY": -0.7215164962332046, + "timestamp": 8.995701976905012 + }, + { + "x": 2.085304191805873, + "y": 6.282058814568287, + "heading": -0.7118496672173015, + "angularVelocity": -0.12743486170139653, + "velocityX": -1.2945508153733418, + "velocityY": -0.8255820198258502, + "timestamp": 9.059748177570338 + }, + { + "x": 2.026047892095963, + "y": 6.222762660032223, + "heading": -0.7027306858009142, + "angularVelocity": 0.14238130164876692, + "velocityX": -0.925211786091023, + "velocityY": -0.9258340685330668, + "timestamp": 9.123794378235663 + }, + { + "x": 1.990496039390564, + "y": 6.157227039337158, + "heading": -0.6763337151495863, + "angularVelocity": 0.41215513765236195, + "velocityX": -0.5550969821172682, + "velocityY": -1.0232554002308782, + "timestamp": 9.187840578900989 + }, + { + "x": 1.978661907650031, + "y": 6.085722412992096, + "heading": -0.632671818562567, + "angularVelocity": 0.6828637108843474, + "velocityX": -0.18508355676508165, + "velocityY": -1.1183186784859962, + "timestamp": 9.251779979825479 + }, + { + "x": 1.9906295592630834, + "y": 6.008657807318728, + "heading": -0.5719280710449405, + "angularVelocity": 0.9500205919877586, + "velocityX": 0.18717178203132864, + "velocityY": -1.2052756916565248, + "timestamp": 9.315719380749968 + }, + { + "x": 2.026610081409329, + "y": 5.926878777279561, + "heading": -0.49455650916344973, + "angularVelocity": 1.2100764280363472, + "velocityX": 0.5627284839396147, + "velocityY": -1.2790083869528952, + "timestamp": 9.379658781674458 + }, + { + "x": 2.086926247462202, + "y": 5.8419986579151795, + "heading": -0.40162648863024836, + "angularVelocity": 1.4534077452953975, + "velocityX": 0.9433332996676658, + "velocityY": -1.3275088308165748, + "timestamp": 9.443598182598947 + }, + { + "x": 2.171907866002122, + "y": 5.758194669745824, + "heading": -0.29634456613068483, + "angularVelocity": 1.6465891293523014, + "velocityX": 1.3290962585070243, + "velocityY": -1.3106783447709351, + "timestamp": 9.507537583523437 + }, + { + "x": 2.2727395248667923, + "y": 5.693604569796404, + "heading": -0.19652914952300654, + "angularVelocity": 1.5610940228476111, + "velocityX": 1.5769878573580702, + "velocityY": -1.0101768082828597, + "timestamp": 9.571476984447926 + }, + { + "x": 2.3578316910702357, + "y": 5.647359218392216, + "heading": -0.11735526130423977, + "angularVelocity": 1.238264467198687, + "velocityX": 1.3308252028187528, + "velocityY": -0.7232684500563616, + "timestamp": 9.635416385372416 + }, + { + "x": 2.4228249155353856, + "y": 5.614614683745836, + "heading": -0.05848104065944221, + "angularVelocity": 0.9207815493036267, + "velocityX": 1.0164815985984024, + "velocityY": -0.5121182584280018, + "timestamp": 9.699355786296906 + }, + { + "x": 2.4665798931428133, + "y": 5.593575115113347, + "heading": -0.019455306739924745, + "angularVelocity": 0.6103550135792704, + "velocityX": 0.6843194802388106, + "velocityY": -0.3290548289205331, + "timestamp": 9.763295187221395 + }, + { + "x": 2.4885952472686768, + "y": 5.583330154418945, + "heading": -2.950378951534863e-27, + "angularVelocity": 0.30427727596166787, + "velocityX": 0.34431592738666283, + "velocityY": -0.16022922558346586, + "timestamp": 9.827234588145885 + }, + { + "x": 2.4885952472686768, + "y": 5.583330154418945, + "heading": -1.2597633775469736e-27, + "angularVelocity": 2.52578953066849e-28, + "velocityX": 5.932691751344987e-28, + "velocityY": -9.915062724222499e-27, + "timestamp": 9.891173989070374 + }, + { + "x": 2.4743714672940076, + "y": 5.57364847936169, + "heading": -0.01214464618202118, + "angularVelocity": -0.22627917912193768, + "velocityX": -0.26501762245193394, + "velocityY": -0.18038907446510918, + "timestamp": 9.944845061367648 + }, + { + "x": 2.4462340281188424, + "y": 5.553844302753565, + "heading": -0.03641057081599083, + "angularVelocity": -0.45212297044421884, + "velocityX": -0.5242570712826018, + "velocityY": -0.36899163293093734, + "timestamp": 9.998516133664921 + }, + { + "x": 2.4046457242182946, + "y": 5.523311043473541, + "heading": -0.07278723106010666, + "angularVelocity": -0.6777703274239201, + "velocityX": -0.7748737284434789, + "velocityY": -0.568896017409645, + "timestamp": 10.052187205962195 + }, + { + "x": 2.3503574105372382, + "y": 5.481175509134353, + "heading": -0.12134175658440335, + "angularVelocity": -0.9046684451423458, + "velocityX": -1.0115004481439094, + "velocityY": -0.7850697318996741, + "timestamp": 10.105858278259468 + }, + { + "x": 2.284734671597692, + "y": 5.426115387645732, + "heading": -0.18232155955050056, + "angularVelocity": -1.136176348934157, + "velocityX": -1.2226835822484632, + "velocityY": -1.0258807795688225, + "timestamp": 10.159529350556742 + }, + { + "x": 2.210690375930078, + "y": 5.356070422302002, + "heading": -0.2559264629038059, + "angularVelocity": -1.3714073560077662, + "velocityX": -1.379594118364116, + "velocityY": -1.3050785524791577, + "timestamp": 10.213200422854015 + }, + { + "x": 2.135682836612729, + "y": 5.268691707848941, + "heading": -0.33902159970532053, + "angularVelocity": -1.5482294883408945, + "velocityX": -1.3975412844725235, + "velocityY": -1.6280411535116714, + "timestamp": 10.266871495151289 + }, + { + "x": 2.0724663596543236, + "y": 5.168263556709831, + "heading": -0.4225586144389228, + "angularVelocity": -1.556462562754615, + "velocityX": -1.1778500829694967, + "velocityY": -1.8711783990984618, + "timestamp": 10.320542567448562 + }, + { + "x": 2.0260137993243106, + "y": 5.062857319928912, + "heading": -0.4997989805362741, + "angularVelocity": -1.4391433372065379, + "velocityX": -0.8655046068899471, + "velocityY": -1.9639301446614508, + "timestamp": 10.374213639745836 + }, + { + "x": 1.996941498368396, + "y": 4.956716720914925, + "heading": -0.5683761952809107, + "angularVelocity": -1.2777314074293329, + "velocityX": -0.5416754261008351, + "velocityY": -1.9776127897370002, + "timestamp": 10.42788471204311 + }, + { + "x": 1.9851616081186387, + "y": 4.852009928485605, + "heading": -0.6273903061238789, + "angularVelocity": -1.0995515520185801, + "velocityX": -0.2194830426437353, + "velocityY": -1.950898089931401, + "timestamp": 10.481555784340383 + }, + { + "x": 1.990496039390564, + "y": 4.75, + "heading": -0.6763337151495863, + "angularVelocity": -0.9119141267500614, + "velocityX": 0.09939118120053472, + "velocityY": -1.9006500917401496, + "timestamp": 10.535226856637657 + }, + { + "x": 2.0404464839460696, + "y": 4.59880499645476, + "heading": -0.7273828368780748, + "angularVelocity": -0.6071201159183436, + "velocityX": 0.5940537008649031, + "velocityY": -1.7981411818772612, + "timestamp": 10.619310912477696 + }, + { + "x": 2.1298849994259657, + "y": 4.462832545778162, + "heading": -0.7498828617388094, + "angularVelocity": -0.26758967126346256, + "velocityX": 1.0636798449642342, + "velocityY": -1.617101474449241, + "timestamp": 10.703394968317735 + }, + { + "x": 2.249114137990773, + "y": 4.356577462238403, + "heading": -0.739452965709272, + "angularVelocity": 0.12404130515991267, + "velocityX": 1.4179755885185839, + "velocityY": -1.2636769537128287, + "timestamp": 10.787479024157774 + }, + { + "x": 2.359728475062173, + "y": 4.292347233042172, + "heading": -0.7129681413379042, + "angularVelocity": 0.3149803385049905, + "velocityX": 1.315520950628657, + "velocityY": -0.7638811966732779, + "timestamp": 10.871563079997813 + }, + { + "x": 2.437807047108505, + "y": 4.256472630548062, + "heading": -0.689567924920021, + "angularVelocity": 0.2782955244499613, + "velocityX": 0.9285776151767543, + "velocityY": -0.42665166583255143, + "timestamp": 10.955647135837852 + }, + { + "x": 2.477766990661621, + "y": 4.240628242492676, + "heading": -0.6763337151495863, + "angularVelocity": 0.1573926190669421, + "velocityX": 0.47523805974745176, + "velocityY": -0.1884351069545121, + "timestamp": 11.039731191677891 + }, + { + "x": 2.477766990661621, + "y": 4.240628242492676, + "heading": -0.6763337151495863, + "angularVelocity": 1.021747484041298e-28, + "velocityX": -7.166324056649742e-28, + "velocityY": 5.068526190074515e-27, + "timestamp": 11.12381524751793 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "timestamp": 1.1048089785277977, + "isStopPoint": true, + "x": 2.542736291885376, + "y": 6.991001605987549, + "heading": 0.610560190519, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "timestamp": 3.0562712875316644, + "isStopPoint": false, + "x": 7.946028232574463, + "y": 7.456615924835205, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "timestamp": 4.737221552647451, + "isStopPoint": true, + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 5.542240068241471, + "isStopPoint": false, + "x": 6.018601417541504, + "y": 6.676982402801514, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "timestamp": 6.361899185357236, + "isStopPoint": false, + "x": 7.891887187957764, + "y": 5.810723304748535, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "timestamp": 7.230699231103491, + "isStopPoint": false, + "x": 6.018601417541504, + "y": 6.676982402801514, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 8.099055167590455, + "isStopPoint": true, + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 9.187840578900989, + "isStopPoint": false, + "x": 1.990496039390564, + "y": 6.157227039337158, + "heading": -0.6763337151495863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "timestamp": 9.891173989070374, + "isStopPoint": true, + "x": 2.4885952472686768, + "y": 5.583330154418945, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "timestamp": 10.535226856637657, + "isStopPoint": false, + "x": 1.990496039390564, + "y": 4.75, + "heading": -0.6763337151495863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "timestamp": 11.12381524751793, + "isStopPoint": true, + "x": 2.477766990661621, + "y": 4.240628242492676, + "heading": -0.6763337151495863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 7 + ], + "type": "StopPoint" + }, + { + "scope": [ + 9 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "Distance Center 7": { + "waypoints": [ + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 2.6293623447418213, + "y": 6.7744364738464355, + "heading": 0.610560190519, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "x": 7.946028232574463, + "y": 7.456615924835205, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 6.018601417541504, + "y": 6.676982402801514, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 7.891887187957764, + "y": 5.810723304748535, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 6.018601417541504, + "y": 6.676982402801514, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 6.007773399353027, + "y": 6.4279327392578125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "x": 8.010997772216797, + "y": 4.30559778213501, + "heading": -0.6499245399095859, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "x": 6.007773399353027, + "y": 6.4279327392578125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 2.9650378227233887, + "y": 6.406276702880859, + "heading": -1.265712572039586, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "x": 2.60770583152771, + "y": 5.854036331176758, + "heading": -1.374993138249586, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 1.990496039390564, + "y": 4.75, + "heading": -0.6763337151495863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 2.477766990661621, + "y": 4.240628242492676, + "heading": -0.6763337151495863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 3.125039836628047e-22, + "angularVelocity": -2.4778681964275978e-21, + "velocityX": -1.6966574578954054e-22, + "velocityY": -5.560554740883435e-23, + "timestamp": 0 + }, + { + "x": 1.3761104747158472, + "y": 5.6065902507064385, + "heading": 0.012329157086594333, + "angularVelocity": 0.16082740054460393, + "velocityX": 0.3405974752403179, + "velocityY": 0.31741720579612304, + "timestamp": 0.07666079937152849 + }, + { + "x": 1.4283317379990463, + "y": 5.655256776007888, + "heading": 0.036989563809549715, + "angularVelocity": 0.32168209730557745, + "velocityX": 0.6811990445092346, + "velocityY": 0.6348293482512852, + "timestamp": 0.15332159874305698 + }, + { + "x": 1.5066641753369279, + "y": 5.728255747741762, + "heading": 0.07398796382390593, + "angularVelocity": 0.48262476151660494, + "velocityX": 1.0218056422585864, + "velocityY": 0.9522333752364306, + "timestamp": 0.22998239811458548 + }, + { + "x": 1.61110814877825, + "y": 5.825586301435188, + "heading": 0.12333895950365491, + "angularVelocity": 0.6437579060527999, + "velocityX": 1.3624169627444864, + "velocityY": 1.269626125625491, + "timestamp": 0.30664319748611396 + }, + { + "x": 1.7416638579543184, + "y": 5.94724735627152, + "heading": 0.18506856130544258, + "angularVelocity": 0.8052303433808673, + "velocityX": 1.7030308873162687, + "velocityY": 1.5870047773271128, + "timestamp": 0.38330399685764244 + }, + { + "x": 1.8983311632556052, + "y": 6.093237705653962, + "heading": 0.2592169764309428, + "angularVelocity": 0.9672272626084648, + "velocityX": 2.0436429907548357, + "velocityY": 1.9043676896051687, + "timestamp": 0.4599647962291709 + }, + { + "x": 2.0811093927308018, + "y": 6.263556191912837, + "heading": 0.3458391806253537, + "angularVelocity": 1.129941311655328, + "velocityX": 2.384246328940312, + "velocityY": 2.2217155006882288, + "timestamp": 0.5366255956006994 + }, + { + "x": 2.237755248322454, + "y": 6.4095214814608905, + "heading": 0.4214192098887476, + "angularVelocity": 0.9859019196643553, + "velocityX": 2.0433631905204215, + "velocityY": 1.9040407971830313, + "timestamp": 0.6132863949722279 + }, + { + "x": 2.3682919967752616, + "y": 6.5311594209661115, + "heading": 0.48444328648392837, + "angularVelocity": 0.8221160894728113, + "velocityX": 1.7027835546062429, + "velocityY": 1.586703249932421, + "timestamp": 0.6899471943437564 + }, + { + "x": 2.4727205283209286, + "y": 6.628470028391072, + "heading": 0.5348829907239893, + "angularVelocity": 0.657959539341745, + "velocityX": 1.362215531298668, + "velocityY": 1.2693659369941386, + "timestamp": 0.7666079937152849 + }, + { + "x": 2.5510415142038663, + "y": 6.70145317297353, + "heading": 0.5727208811580081, + "angularVelocity": 0.493575474613063, + "velocityX": 1.0216562640230678, + "velocityY": 0.9520269183308826, + "timestamp": 0.8432687930868134 + }, + { + "x": 2.60325538804063, + "y": 6.750108693952396, + "heading": 0.5979476766685491, + "angularVelocity": 0.3290703425655907, + "velocityX": 0.6811026530484638, + "velocityY": 0.634685802623348, + "timestamp": 0.9199295924583418 + }, + { + "x": 2.6293623447418213, + "y": 6.7744364738464355, + "heading": 0.610560190519, + "angularVelocity": 0.1645236412071015, + "velocityX": 0.34055158458062124, + "velocityY": 0.3173431544346181, + "timestamp": 0.9965903918298703 + }, + { + "x": 2.6293623447418213, + "y": 6.7744364738464355, + "heading": 0.610560190519, + "angularVelocity": 2.9094056410031188e-21, + "velocityX": -2.0988135133774157e-22, + "velocityY": 3.818316675059001e-22, + "timestamp": 1.073251191201399 + }, + { + "x": 2.6720197676214497, + "y": 6.782417422598798, + "heading": 0.6038766101713076, + "angularVelocity": -0.07923422323582437, + "velocityX": 0.5057061621584873, + "velocityY": 0.09461459908935298, + "timestamp": 1.1576033823978742 + }, + { + "x": 2.7573390707361796, + "y": 6.798381015719943, + "heading": 0.5908586177943566, + "angularVelocity": -0.15432903629769548, + "velocityX": 1.0114651665183407, + "velocityY": 0.1892492998073053, + "timestamp": 1.2419555735943495 + }, + { + "x": 2.885325839743734, + "y": 6.8223294638474, + "heading": 0.5719744160417135, + "angularVelocity": -0.22387328040663998, + "velocityX": 1.5172903891665839, + "velocityY": 0.2839102077582811, + "timestamp": 1.3263077647908248 + }, + { + "x": 3.0559871968712695, + "y": 6.854265761931121, + "heading": 0.5478849059076967, + "angularVelocity": -0.2855825058285316, + "velocityX": 2.023200046220811, + "velocityY": 0.37860662101039444, + "timestamp": 1.4106599559873 + }, + { + "x": 3.269332249645403, + "y": 6.894194168881417, + "heading": 0.5195930329458099, + "angularVelocity": -0.3354017549584274, + "velocityX": 2.5292176735184513, + "velocityY": 0.47335352388527485, + "timestamp": 1.4950121471837754 + }, + { + "x": 3.525371770387385, + "y": 6.942121092388337, + "heading": 0.4888001874449035, + "angularVelocity": -0.36505092593484567, + "velocityX": 3.0353630072941193, + "velocityY": 0.568176390288274, + "timestamp": 1.5793643383802507 + }, + { + "x": 3.824109398072704, + "y": 6.998056361458239, + "heading": 0.4590231888113175, + "angularVelocity": -0.35300800383748954, + "velocityX": 3.5415514813301257, + "velocityY": 0.663115780117865, + "timestamp": 1.663716529576726 + }, + { + "x": 4.165377594624907, + "y": 7.062001251881552, + "heading": 0.4419288520213311, + "angularVelocity": -0.2026543299885333, + "velocityX": 4.045753781989037, + "velocityY": 0.7580702945151749, + "timestamp": 1.7480687207732013 + }, + { + "x": 4.512094110834224, + "y": 7.126817088416808, + "heading": 0.4419288475732292, + "angularVelocity": -5.2732499850975034e-8, + "velocityX": 4.110343919836483, + "velocityY": 0.7683954099578318, + "timestamp": 1.8324209119696766 + }, + { + "x": 4.858810627216134, + "y": 7.191632924028826, + "heading": 0.44192884312513186, + "angularVelocity": -5.27324452565932e-8, + "velocityX": 4.110343921882592, + "velocityY": 0.7683953990127874, + "timestamp": 1.916773103166152 + }, + { + "x": 5.205527143597466, + "y": 7.256448759643943, + "heading": 0.4419288386770344, + "angularVelocity": -5.273244693132246e-8, + "velocityX": 4.110343921875726, + "velocityY": 0.7683953990495135, + "timestamp": 2.001125294362627 + }, + { + "x": 5.55224365998293, + "y": 7.321264595236958, + "heading": 0.44192883422893686, + "angularVelocity": -5.273244840969362e-8, + "velocityX": 4.110343921924708, + "velocityY": 0.768395398787497, + "timestamp": 2.0854774855591027 + }, + { + "x": 5.898960176356647, + "y": 7.386080430892807, + "heading": 0.44192882978083925, + "angularVelocity": -5.273244895227244e-8, + "velocityX": 4.110343921785453, + "velocityY": 0.7683953995324063, + "timestamp": 2.1698296767555783 + }, + { + "x": 6.24567671797852, + "y": 7.450896131489656, + "heading": 0.44192882533274175, + "angularVelocity": -5.2732447593359085e-8, + "velocityX": 4.110344221103768, + "velocityY": 0.7683937984002956, + "timestamp": 2.2541818679520538 + }, + { + "x": 6.595261801688144, + "y": 7.49783974677019, + "heading": 0.44192881540719375, + "angularVelocity": -1.1766793324652698e-7, + "velocityX": 4.144350949880616, + "velocityY": 0.5565192156205201, + "timestamp": 2.3385340591485293 + }, + { + "x": 6.9128163797305655, + "y": 7.523763714384209, + "heading": 0.36817463115775373, + "angularVelocity": -0.8743600279173521, + "velocityX": 3.764627492636967, + "velocityY": 0.30733010306319974, + "timestamp": 2.422886250345005 + }, + { + "x": 7.188041974628758, + "y": 7.539922522639208, + "heading": 0.2960994016568734, + "angularVelocity": -0.8544559243636105, + "velocityX": 3.262815002127576, + "velocityY": 0.19156358626608547, + "timestamp": 2.5072384415414803 + }, + { + "x": 7.420776182221607, + "y": 7.5471672706134445, + "heading": 0.22917449817448735, + "angularVelocity": -0.7933985179650258, + "velocityX": 2.75907720109792, + "velocityY": 0.08588689720415285, + "timestamp": 2.591590632737956 + }, + { + "x": 7.610971318020134, + "y": 7.545802222186919, + "heading": 0.168638193399793, + "angularVelocity": -0.7176613187639852, + "velocityX": 2.2547740977530752, + "velocityY": -0.016182726342533954, + "timestamp": 2.6759428239344314 + }, + { + "x": 7.758604986098777, + "y": 7.535983661526973, + "heading": 0.1151255004194216, + "angularVelocity": -0.6343960034864801, + "velocityX": 1.7502054894433117, + "velocityY": -0.11639959224149259, + "timestamp": 2.760295015130907 + }, + { + "x": 7.863664232769146, + "y": 7.517806741993987, + "heading": 0.06902173017336893, + "angularVelocity": -0.5465628052111479, + "velocityX": 1.2454833144246713, + "velocityY": -0.21548840966854907, + "timestamp": 2.8446472063273824 + }, + { + "x": 7.926140623551973, + "y": 7.491335488819979, + "heading": 0.03058497546670338, + "angularVelocity": -0.4556699021266401, + "velocityX": 0.7406611481769909, + "velocityY": -0.31381820434695645, + "timestamp": 2.928999397523858 + }, + { + "x": 7.946028232574463, + "y": 7.456615924835205, + "heading": -1.1630164830490874e-22, + "angularVelocity": -0.3625866149163102, + "velocityX": 0.23576873037201065, + "velocityY": -0.41160239576827296, + "timestamp": 3.0133515887203335 + }, + { + "x": 7.920037662049596, + "y": 7.411169444194395, + "heading": -0.0232873594647728, + "angularVelocity": -0.2632383110672307, + "velocityX": -0.29379517669185795, + "velocityY": -0.5137231134312821, + "timestamp": 3.101816521390507 + }, + { + "x": 7.8471886878468275, + "y": 7.356716657006662, + "heading": -0.03810345649711681, + "angularVelocity": -0.16747988819008414, + "velocityX": -0.8234785468539669, + "velocityY": -0.6155296290198013, + "timestamp": 3.1902814540606808 + }, + { + "x": 7.727467357765122, + "y": 7.293295235469457, + "heading": -0.044875887681678404, + "angularVelocity": -0.076554980376365, + "velocityX": -1.3533196314981237, + "velocityY": -0.7169103013242628, + "timestamp": 3.2787463867308544 + }, + { + "x": 7.560854391886669, + "y": 7.220959149263091, + "heading": -0.04421375783355388, + "angularVelocity": 0.007484658927997671, + "velocityX": -1.8833786546771185, + "velocityY": -0.8176809049983522, + "timestamp": 3.367211319401028 + }, + { + "x": 7.34732146541913, + "y": 7.139792163137446, + "heading": -0.03705684189341484, + "angularVelocity": 0.08090116302718922, + "velocityX": -2.413757858875676, + "velocityY": -0.9175046391349556, + "timestamp": 3.4556762520712017 + }, + { + "x": 7.086823282403413, + "y": 7.049941858740973, + "heading": -0.02504724603475039, + "angularVelocity": 0.13575543999383538, + "velocityX": -2.9446490847049964, + "velocityY": -1.0156601229943323, + "timestamp": 3.5441411847413753 + }, + { + "x": 6.779279315725815, + "y": 6.9517369192823635, + "heading": -0.011789177436815698, + "angularVelocity": 0.14986806859804128, + "velocityX": -3.476450582110584, + "velocityY": -1.1101002000956572, + "timestamp": 3.632606117411549 + }, + { + "x": 6.424632928888111, + "y": 6.846536629082463, + "heading": -0.011789088874469935, + "angularVelocity": 0.000001001101149226084, + "velocityX": -4.008892293627146, + "velocityY": -1.1891750439931048, + "timestamp": 3.7210710500817226 + }, + { + "x": 6.061695217745179, + "y": 6.775000553604086, + "heading": -0.011789083969654859, + "angularVelocity": 5.544360832077928e-8, + "velocityX": -4.102616711370633, + "velocityY": -0.808637652447976, + "timestamp": 3.809535982751896 + }, + { + "x": 5.698757430246908, + "y": 6.703464865514997, + "heading": -0.011789079064840387, + "angularVelocity": 5.544360147801105e-8, + "velocityX": -4.102617574484819, + "velocityY": -0.8086332734327347, + "timestamp": 3.89800091542207 + }, + { + "x": 5.33581964060784, + "y": 6.631929188287501, + "heading": -0.011789074159930215, + "angularVelocity": 5.5444683287550804e-8, + "velocityX": -4.102617598684197, + "velocityY": -0.8086331506542177, + "timestamp": 3.9864658480922435 + }, + { + "x": 5.008416015419383, + "y": 6.567395761473697, + "heading": 0.06462490200896195, + "angularVelocity": 0.8637770228547912, + "velocityX": -3.7009424560252184, + "velocityY": -0.7294803134526322, + "timestamp": 4.0749307807624175 + }, + { + "x": 4.727784426794771, + "y": 6.512080858011152, + "heading": 0.13012220422079274, + "angularVelocity": 0.7403758781575775, + "velocityX": -3.172235372301678, + "velocityY": -0.625274917336753, + "timestamp": 4.163395713432592 + }, + { + "x": 4.493924797623965, + "y": 6.465984740956769, + "heading": 0.18470070193798652, + "angularVelocity": 0.6169506500466159, + "velocityX": -2.6435291602234257, + "velocityY": -0.5210665476482677, + "timestamp": 4.251860646102766 + }, + { + "x": 4.30683709599071, + "y": 6.429107629337118, + "heading": 0.22836093103558394, + "angularVelocity": 0.4935314794210844, + "velocityX": -2.1148233089237682, + "velocityY": -0.4168557021022203, + "timestamp": 4.34032557877294 + }, + { + "x": 4.166521316526833, + "y": 6.401449688021447, + "heading": 0.26110453763292085, + "angularVelocity": 0.3701309163871272, + "velocityX": -1.5861175183054803, + "velocityY": -0.3126429928883621, + "timestamp": 4.428790511443114 + }, + { + "x": 4.072977463595493, + "y": 6.383011025435591, + "heading": 0.2829331362314755, + "angularVelocity": 0.24674860353919897, + "velocityX": -1.0574116783663996, + "velocityY": -0.20842905803818979, + "timestamp": 4.517255444113288 + }, + { + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "angularVelocity": 0.12337586761318343, + "velocityX": -0.5287058100920702, + "velocityY": -0.10421452338466086, + "timestamp": 4.605720376783462 + }, + { + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "angularVelocity": -3.545196144958051e-23, + "velocityX": -1.2710664244245501e-21, + "velocityY": 5.161280003041427e-22, + "timestamp": 4.694185309453636 + }, + { + "x": 4.048121639777245, + "y": 6.381836957223538, + "heading": 0.2879219902009138, + "angularVelocity": -0.09569046695813048, + "velocityX": 0.35391649199799335, + "velocityY": 0.12992051965342163, + "timestamp": 4.7561098045517625 + }, + { + "x": 4.09210978411728, + "y": 6.3975053544995015, + "heading": 0.2763091104232377, + "angularVelocity": -0.18753289403933296, + "velocityX": 0.7103512797372235, + "velocityY": 0.25302422330831614, + "timestamp": 4.818034299649889 + }, + { + "x": 4.1583483462814845, + "y": 6.420279656151659, + "heading": 0.2593011262084734, + "angularVelocity": -0.27465680887366445, + "velocityX": 1.0696665682819408, + "velocityY": 0.3677753305226003, + "timestamp": 4.879958794748015 + }, + { + "x": 4.247041291177504, + "y": 6.4495126487830605, + "heading": 0.2372637765315283, + "angularVelocity": -0.35587451527904157, + "velocityX": 1.4322756246211616, + "velocityY": 0.4720747837358832, + "timestamp": 4.941883289846142 + }, + { + "x": 4.358419897352032, + "y": 6.484373747552581, + "heading": 0.21066653247243172, + "angularVelocity": -0.4295108747669278, + "velocityX": 1.7986195284763546, + "velocityY": 0.5629613727859919, + "timestamp": 5.003807784944268 + }, + { + "x": 4.492739145472355, + "y": 6.523763939464936, + "heading": 0.18013087413356532, + "angularVelocity": -0.49311113946878693, + "velocityX": 2.16908103824632, + "velocityY": 0.6361003323472623, + "timestamp": 5.065732280042394 + }, + { + "x": 4.650257527226494, + "y": 6.56617435469435, + "heading": 0.14651099259101713, + "angularVelocity": -0.5429173300367439, + "velocityX": 2.5437168523462796, + "velocityY": 0.6848730080432686, + "timestamp": 5.127656775140521 + }, + { + "x": 4.831166710355601, + "y": 6.609441995709787, + "heading": 0.11103466181531912, + "angularVelocity": -0.5728965689503284, + "velocityX": 2.921447851006871, + "velocityY": 0.6987160887928849, + "timestamp": 5.189581270238647 + }, + { + "x": 5.035367274609871, + "y": 6.650325338062944, + "heading": 0.0755552485953242, + "angularVelocity": -0.5729463464138673, + "velocityX": 3.297573342030341, + "velocityY": 0.6602127686042919, + "timestamp": 5.251505765336773 + }, + { + "x": 5.261781944494072, + "y": 6.683862994996324, + "heading": 0.042963975482345464, + "angularVelocity": -0.5263066426514128, + "velocityX": 3.6563022358991017, + "velocityY": 0.541589509615474, + "timestamp": 5.3134302604349 + }, + { + "x": 5.506627789251162, + "y": 6.703117076456781, + "heading": 0.017406668728306682, + "angularVelocity": -0.4127172407871934, + "velocityX": 3.9539417215934254, + "velocityY": 0.31092835605597996, + "timestamp": 5.375354755533026 + }, + { + "x": 5.761751812356917, + "y": 6.701608512470805, + "heading": 0.003171834984879961, + "angularVelocity": -0.22987403806635848, + "velocityX": 4.119920924691961, + "velocityY": -0.024361344950586963, + "timestamp": 5.4372792506311525 + }, + { + "x": 6.018601417541504, + "y": 6.676982402801514, + "heading": -1.3559620024912911e-24, + "angularVelocity": -0.051221006806011604, + "velocityX": 4.147786829389234, + "velocityY": -0.39767961983813094, + "timestamp": 5.499203745729279 + }, + { + "x": 6.298786009014622, + "y": 6.621524313440803, + "heading": -4.414364457925229e-8, + "angularVelocity": -6.462732937493416e-7, + "velocityX": 4.101968029940426, + "velocityY": -0.8119194148513419, + "timestamp": 5.567508664107004 + }, + { + "x": 6.570847718915352, + "y": 6.538762468325385, + "heading": -7.033434259435762e-8, + "angularVelocity": -3.8343795201205294e-7, + "velocityX": 3.983047141586983, + "velocityY": -1.2116527928156882, + "timestamp": 5.6358135824847295 + }, + { + "x": 6.822289711502508, + "y": 6.436346018958693, + "heading": -7.949311143160773e-8, + "angularVelocity": -1.3408652048454663e-7, + "velocityX": 3.6811696516008747, + "velocityY": -1.499400801569386, + "timestamp": 5.704118500862455 + }, + { + "x": 7.0460752580417605, + "y": 6.327103349848984, + "heading": -7.703336636195349e-8, + "angularVelocity": 3.6011243817786175e-8, + "velocityX": 3.2762728051547096, + "velocityY": -1.5993382571017543, + "timestamp": 5.77242341924018 + }, + { + "x": 7.241490832586327, + "y": 6.220433828516969, + "heading": -6.8157674853432e-8, + "angularVelocity": 1.2994220210379365e-7, + "velocityX": 2.8609297717613993, + "velocityY": -1.5616667710827985, + "timestamp": 5.8407283376179056 + }, + { + "x": 7.409535997448797, + "y": 6.121659695713664, + "heading": -5.6202483858229566e-8, + "angularVelocity": 1.750267957146284e-7, + "velocityX": 2.460220564691751, + "velocityY": -1.4460764341607861, + "timestamp": 5.909033255995631 + }, + { + "x": 7.551302074325524, + "y": 6.033880383414205, + "heading": -4.325249063148238e-8, + "angularVelocity": 1.895909333370981e-7, + "velocityX": 2.0754885628112913, + "velocityY": -1.2851096873294061, + "timestamp": 5.977338174373356 + }, + { + "x": 7.667694153636149, + "y": 5.959048535338257, + "heading": -3.068159533861856e-8, + "angularVelocity": 1.840408508117524e-7, + "velocityX": 1.7040072966192434, + "velocityY": -1.095555779191907, + "timestamp": 6.045643092751082 + }, + { + "x": 7.7594304227528665, + "y": 5.89848440024899, + "heading": -1.9448297882684093e-8, + "angularVelocity": 1.644581052540686e-7, + "velocityX": 1.343040461733904, + "velocityY": -0.8866731199991837, + "timestamp": 6.113948011128807 + }, + { + "x": 7.827081864823745, + "y": 5.853132028276085, + "heading": -1.0253962002990125e-8, + "angularVelocity": 1.3460723031465246e-7, + "velocityX": 0.9904329538433378, + "velocityY": -0.6639693458398959, + "timestamp": 6.182252929506532 + }, + { + "x": 7.8711085813221855, + "y": 5.823696596603824, + "heading": -3.6318337075530827e-9, + "angularVelocity": 9.69495089477567e-8, + "velocityX": 0.6445614392652327, + "velocityY": -0.430941612571493, + "timestamp": 6.250557847884258 + }, + { + "x": 7.891887187957764, + "y": 5.810723304748535, + "heading": -2.0933118512576364e-26, + "angularVelocity": 5.317089594440484e-8, + "velocityX": 0.30420366686733796, + "velocityY": -0.18993203071478146, + "timestamp": 6.318862766261983 + }, + { + "x": 7.888143904798224, + "y": 5.815954272618714, + "heading": 1.1238376512921276e-10, + "angularVelocity": 1.5522619182937645e-9, + "velocityX": -0.05170280503827788, + "velocityY": 0.0722509359902741, + "timestamp": 6.391262767708095 + }, + { + "x": 7.858277731902267, + "y": 5.8396753308857665, + "heading": -3.1980355240507115e-9, + "angularVelocity": -4.572402241792647e-8, + "velocityX": -0.41251619198083334, + "velocityY": 0.32763891979626797, + "timestamp": 6.463662769154206 + }, + { + "x": 7.801862145221396, + "y": 5.881268642056604, + "heading": -9.54060326150597e-9, + "angularVelocity": -8.760452501062605e-8, + "velocityX": -0.7792207949451817, + "velocityY": 0.5744932367411117, + "timestamp": 6.536062770600318 + }, + { + "x": 7.718377542515441, + "y": 5.939936997849849, + "heading": -1.8417788083637822e-8, + "angularVelocity": -1.226130475803823e-7, + "velocityX": -1.1531022242878466, + "velocityY": 0.8103363897984549, + "timestamp": 6.60846277204643 + }, + { + "x": 7.607181391684359, + "y": 6.014616563892487, + "heading": -2.9176471451729494e-8, + "angularVelocity": -1.4860059603865473e-7, + "velocityX": -1.5358584062162697, + "velocityY": 1.0314856982181417, + "timestamp": 6.680862773492541 + }, + { + "x": 7.4674691498308405, + "y": 6.103826142786331, + "heading": -4.092645392613916e-8, + "angularVelocity": -1.6229257237177443e-7, + "velocityX": -1.9297270588800863, + "velocityY": 1.2321764794472392, + "timestamp": 6.753262774938653 + }, + { + "x": 7.298232520371032, + "y": 6.205388827036514, + "heading": -5.239643950278259e-8, + "angularVelocity": -1.5842521198263621e-7, + "velocityX": -2.337522459661463, + "velocityY": 1.402799478198597, + "timestamp": 6.825662776384765 + }, + { + "x": 7.0982622904373915, + "y": 6.315880924025181, + "heading": -6.166708665777315e-8, + "angularVelocity": -1.2804761008037847e-7, + "velocityX": -2.762019695296288, + "velocityY": 1.526133905824668, + "timestamp": 6.8980627778308765 + }, + { + "x": 6.866440603568656, + "y": 6.429499233364628, + "heading": -6.568320426031161e-8, + "angularVelocity": -5.547123649614453e-8, + "velocityX": -3.2019569369937573, + "velocityY": 1.5693136335641493, + "timestamp": 6.970462779276988 + }, + { + "x": 6.60338911605519, + "y": 6.536118404749743, + "heading": -5.966101479819827e-8, + "angularVelocity": 8.317941079871027e-8, + "velocityX": -3.6333077660123805, + "velocityY": 1.472640459330275, + "timestamp": 7.0428627807231 + }, + { + "x": 6.316249006337029, + "y": 6.621665638178002, + "heading": -3.828640775690033e-8, + "angularVelocity": 2.9522937312656454e-7, + "velocityX": -3.9660235356747005, + "velocityY": 1.1815915983362697, + "timestamp": 7.115262782169212 + }, + { + "x": 6.018601417541504, + "y": 6.676982402801514, + "heading": 7.9356951670816615e-25, + "angularVelocity": 5.288177761349547e-7, + "velocityX": -4.11115445925878, + "velocityY": 0.7640436950085439, + "timestamp": 7.187662783615323 + }, + { + "x": 5.740842572720578, + "y": 6.700963658984785, + "heading": 0.0018231984576357557, + "angularVelocity": 0.027294778663288, + "velocityX": -4.158278085090587, + "velocityY": 0.35901910559907296, + "timestamp": 7.2544593852704224 + }, + { + "x": 5.465613553779653, + "y": 6.698113364745584, + "heading": 0.01705607864209539, + "angularVelocity": 0.2280487301302209, + "velocityX": -4.12040451342204, + "velocityY": -0.04267124626965841, + "timestamp": 7.321255986925522 + }, + { + "x": 5.204358328316174, + "y": 6.672254128794052, + "heading": 0.046261831986706164, + "angularVelocity": 0.43723412001426465, + "velocityX": -3.911205345632041, + "velocityY": -0.3871340054851043, + "timestamp": 7.388052588580621 + }, + { + "x": 4.966474282232876, + "y": 6.6325844049513245, + "heading": 0.0830771823140007, + "angularVelocity": 0.5511560381078753, + "velocityX": -3.5613195909516313, + "velocityY": -0.593888354493849, + "timestamp": 7.45484919023572 + }, + { + "x": 4.755211974738486, + "y": 6.587176755241801, + "heading": 0.12225070699122965, + "angularVelocity": 0.5864598453600871, + "velocityX": -3.162770294591211, + "velocityY": -0.6797898184099774, + "timestamp": 7.521645791890819 + }, + { + "x": 4.571183406962543, + "y": 6.54114573684605, + "heading": 0.16051429938187217, + "angularVelocity": 0.5728374115230364, + "velocityX": -2.7550588385643326, + "velocityY": -0.6891221597384368, + "timestamp": 7.588442393545918 + }, + { + "x": 4.414262701798846, + "y": 6.497671687179865, + "heading": 0.1958556202852531, + "angularVelocity": 0.5290886067208012, + "velocityX": -2.3492318662250016, + "velocityY": -0.6508422373141216, + "timestamp": 7.655238995201017 + }, + { + "x": 4.284155652887569, + "y": 6.458840678668644, + "heading": 0.2269562205060937, + "angularVelocity": 0.46560153436288354, + "velocityX": -1.9478094047819534, + "velocityY": -0.5813320969788546, + "timestamp": 7.7220355968561165 + }, + { + "x": 4.180556191608258, + "y": 6.426101398719019, + "heading": 0.25289929121670346, + "angularVelocity": 0.38838908069852646, + "velocityX": -1.5509690420216515, + "velocityY": -0.49013391607366485, + "timestamp": 7.788832198511216 + }, + { + "x": 4.103186902723614, + "y": 6.4005094893734436, + "heading": 0.27301561090145554, + "angularVelocity": 0.301157831181616, + "velocityX": -1.1582818132595463, + "velocityY": -0.38313190658586416, + "timestamp": 7.855628800166315 + }, + { + "x": 4.051806124428388, + "y": 6.382864606916464, + "heading": 0.28679744635493554, + "angularVelocity": 0.20632539847822937, + "velocityX": -0.7692124602465237, + "velocityY": -0.2641583856030227, + "timestamp": 7.922425401821414 + }, + { + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "angularVelocity": 0.10554620330039294, + "velocityX": -0.3832617841429427, + "velocityY": -0.13582895013430246, + "timestamp": 7.989222003476513 + }, + { + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "angularVelocity": 2.005442344245764e-24, + "velocityX": 1.8885981121979073e-24, + "velocityY": 6.519144438430986e-24, + "timestamp": 8.056018605131612 + }, + { + "x": 4.0487237842181845, + "y": 6.381905764315973, + "heading": 0.289763925680258, + "angularVelocity": -0.0651795729135273, + "velocityX": 0.35941624407112255, + "velocityY": 0.12950958254097514, + "timestamp": 8.118670877771688 + }, + { + "x": 4.09402733620905, + "y": 6.397352444809068, + "heading": 0.28163512088907877, + "angularVelocity": -0.1297447713968433, + "velocityX": 0.7230951102304722, + "velocityY": 0.24654621200788543, + "timestamp": 8.181323150411764 + }, + { + "x": 4.162408374331436, + "y": 6.419155700762566, + "heading": 0.2695155327794676, + "angularVelocity": -0.19344211468968972, + "velocityX": 1.0914374729105671, + "velocityY": 0.3480042308880483, + "timestamp": 8.24397542305184 + }, + { + "x": 4.254175470208227, + "y": 6.446070164240337, + "heading": 0.25348421107280095, + "angularVelocity": -0.255877736451212, + "velocityX": 1.4647049821157165, + "velocityY": 0.4295847914789773, + "timestamp": 8.306627695691915 + }, + { + "x": 4.369627429708199, + "y": 6.476469024717609, + "heading": 0.23366000099411213, + "angularVelocity": -0.3164164561527553, + "velocityX": 1.8427417655416987, + "velocityY": 0.4851996455405065, + "timestamp": 8.369279968331991 + }, + { + "x": 4.508985987745745, + "y": 6.508175804754511, + "heading": 0.2102282566283648, + "angularVelocity": -0.3739967183689891, + "velocityX": 2.224317684980605, + "velocityY": 0.5060754973574803, + "timestamp": 8.431932240972067 + }, + { + "x": 4.6722331784478675, + "y": 6.538223031668598, + "heading": 0.18348837802396292, + "angularVelocity": -0.426798222596284, + "velocityX": 2.605606849091409, + "velocityY": 0.47958718252252885, + "timestamp": 8.494584513612143 + }, + { + "x": 4.858743027816154, + "y": 6.562565877888605, + "heading": 0.15393434470481, + "angularVelocity": -0.4717152638490063, + "velocityX": 2.9769047715116295, + "velocityY": 0.3885389179711166, + "timestamp": 8.557236786252219 + }, + { + "x": 5.066570002168237, + "y": 6.575971754133761, + "heading": 0.1223656565639417, + "angularVelocity": -0.5038713970078006, + "velocityX": 3.3171498110851902, + "velocityY": 0.21397270490362202, + "timestamp": 8.619889058892294 + }, + { + "x": 5.291589628159925, + "y": 6.572681851877193, + "heading": 0.08994651870042891, + "angularVelocity": -0.5174455210867492, + "velocityX": 3.591563665764864, + "velocityY": -0.05251050150194843, + "timestamp": 8.68254133153237 + }, + { + "x": 5.527619341276251, + "y": 6.548104466256015, + "heading": 0.05802485635108468, + "angularVelocity": -0.5095052582166943, + "velocityX": 3.767296909918484, + "velocityY": -0.3922824278437735, + "timestamp": 8.745193604172446 + }, + { + "x": 5.768129964554371, + "y": 6.499988313354724, + "heading": 0.027769937254171585, + "angularVelocity": -0.48290218091084397, + "velocityX": 3.8388172231166338, + "velocityY": -0.7679873510368684, + "timestamp": 8.807845876812522 + }, + { + "x": 6.007773399353027, + "y": 6.4279327392578125, + "heading": 4.2309997245679596e-24, + "angularVelocity": -0.44323910504738345, + "velocityX": 3.8249759298495123, + "velocityY": -1.1500871566280768, + "timestamp": 8.870498149452597 + }, + { + "x": 6.229030281346825, + "y": 6.339362395858419, + "heading": -0.023466940923758877, + "angularVelocity": -0.39811204237675923, + "velocityX": 3.753579534147267, + "velocityY": -1.5025784749406275, + "timestamp": 8.929443718603581 + }, + { + "x": 6.443849172872896, + "y": 6.230687798127142, + "heading": -0.039879946371075364, + "angularVelocity": -0.27844341285900365, + "velocityX": 3.6443602906917305, + "velocityY": -1.8436431999310363, + "timestamp": 8.988389287754565 + }, + { + "x": 6.650509698951262, + "y": 6.102546806166999, + "heading": -0.049142702906474676, + "angularVelocity": -0.15714084483048257, + "velocityX": 3.505955223691552, + "velocityY": -2.173886753589912, + "timestamp": 9.047334856905549 + }, + { + "x": 6.8468725839470075, + "y": 5.955927867022489, + "heading": -0.05303207582244352, + "angularVelocity": -0.06598244740001617, + "velocityX": 3.331257765834417, + "velocityY": -2.487361497332514, + "timestamp": 9.106280426056532 + }, + { + "x": 7.030112126915743, + "y": 5.792643403875283, + "heading": -0.05586334395844049, + "angularVelocity": -0.0480319076866473, + "velocityX": 3.108622846602461, + "velocityY": -2.7700888378728914, + "timestamp": 9.165225995207516 + }, + { + "x": 7.196822404953559, + "y": 5.61624162712754, + "heading": -0.06598691122862967, + "angularVelocity": -0.17174432982839563, + "velocityX": 2.8282071144448753, + "velocityY": -2.992621486034079, + "timestamp": 9.2241715643585 + }, + { + "x": 7.344318297823079, + "y": 5.433198235403824, + "heading": -0.09660333760407158, + "angularVelocity": -0.5194016584524004, + "velocityX": 2.5022388449880117, + "velocityY": -3.1052951792672387, + "timestamp": 9.283117133509483 + }, + { + "x": 7.472388333611968, + "y": 5.252274949850564, + "heading": -0.15660945149917227, + "angularVelocity": -1.0179919332257161, + "velocityX": 2.1726829960848693, + "velocityY": -3.0693279946087215, + "timestamp": 9.342062702660467 + }, + { + "x": 7.582263019092128, + "y": 5.081723782489176, + "heading": -0.22798352839802205, + "angularVelocity": -1.2108471921957489, + "velocityX": 1.8640024528175283, + "velocityY": -2.8933670472251234, + "timestamp": 9.40100827181145 + }, + { + "x": 7.676466487781464, + "y": 4.9254488732019475, + "heading": -0.3020784286066942, + "angularVelocity": -1.2570054251047096, + "velocityX": 1.5981433387815942, + "velocityY": -2.6511731337590945, + "timestamp": 9.459953840962434 + }, + { + "x": 7.7568938758586405, + "y": 4.785294368443394, + "heading": -0.37402343822514994, + "angularVelocity": -1.2205329536164857, + "velocityX": 1.364434837691845, + "velocityY": -2.3776936379994624, + "timestamp": 9.518899410113418 + }, + { + "x": 7.824872010815036, + "y": 4.662247591209576, + "heading": -0.4407895129961546, + "angularVelocity": -1.1326733414005847, + "velocityX": 1.1532357043202854, + "velocityY": -2.0874644015845227, + "timestamp": 9.577844979264402 + }, + { + "x": 7.88135001784302, + "y": 4.556897034423127, + "heading": -0.5003478187596111, + "angularVelocity": -1.0103949562502774, + "velocityX": 0.9581382933689164, + "velocityY": -1.7872514983543952, + "timestamp": 9.636790548415386 + }, + { + "x": 7.927030718663331, + "y": 4.469623470627648, + "heading": -0.5512507357361617, + "angularVelocity": -0.863557985947457, + "velocityX": 0.7749641148311569, + "velocityY": -1.4805788637299317, + "timestamp": 9.69573611756637 + }, + { + "x": 7.962451814207847, + "y": 4.400689098568436, + "heading": -0.5924140389845107, + "angularVelocity": -0.6983273525260651, + "velocityX": 0.6009119269641297, + "velocityY": -1.1694580789039828, + "timestamp": 9.754681686717353 + }, + { + "x": 7.988036352165696, + "y": 4.350283491457437, + "heading": -0.6229985575776359, + "angularVelocity": -0.5188603491941064, + "velocityX": 0.434036660029808, + "velocityY": -0.8551212217815544, + "timestamp": 9.813627255868337 + }, + { + "x": 8.004125055669116, + "y": 4.31854932275225, + "heading": -0.6423430672717187, + "angularVelocity": -0.3281758064721266, + "velocityX": 0.27294169409427244, + "velocityY": -0.5383639374810789, + "timestamp": 9.87257282501932 + }, + { + "x": 8.010997772216797, + "y": 4.30559778213501, + "heading": -0.6499245399095859, + "angularVelocity": -0.12861819381958664, + "velocityX": 0.1165942859941986, + "velocityY": -0.21972034206788113, + "timestamp": 9.931518394170304 + }, + { + "x": 8.008640625334808, + "y": 4.3120713019161, + "heading": -0.6449603194163791, + "angularVelocity": 0.08240933446076017, + "velocityX": -0.03913019295512813, + "velocityY": 0.10746469813501351, + "timestamp": 9.991756964542093 + }, + { + "x": 7.996662965285712, + "y": 4.338162984806749, + "heading": -0.6277276549089263, + "angularVelocity": 0.2860735970507537, + "velocityX": -0.19883705697477932, + "velocityY": 0.43313914539493714, + "timestamp": 10.051995534913882 + }, + { + "x": 7.9747782083324354, + "y": 4.383759514989304, + "heading": -0.598776455290747, + "angularVelocity": 0.4806090091364049, + "velocityX": -0.36330140005325046, + "velocityY": 0.75693247534156, + "timestamp": 10.11223410528567 + }, + { + "x": 7.942638409560382, + "y": 4.448715501539106, + "heading": -0.5587851834199348, + "angularVelocity": 0.6638814902808625, + "velocityX": -0.533541858209585, + "velocityY": 1.0783122200427058, + "timestamp": 10.17247267565746 + }, + { + "x": 7.89981288541696, + "y": 4.532838211183571, + "heading": -0.5085984817650532, + "angularVelocity": 0.8331323493424854, + "velocityX": -0.7109319474068704, + "velocityY": 1.3964924652969615, + "timestamp": 10.232711246029249 + }, + { + "x": 7.845756103945311, + "y": 4.635862370094525, + "heading": -0.4492930873299135, + "angularVelocity": 0.9845086639525084, + "velocityX": -0.8973782269069894, + "velocityY": 1.710268990035694, + "timestamp": 10.292949816401038 + }, + { + "x": 7.779757952213243, + "y": 4.757405734926412, + "heading": -0.382295753262504, + "angularVelocity": 1.1121999352558545, + "velocityX": -1.0956128494539268, + "velocityY": 2.0177000231201787, + "timestamp": 10.353188386772826 + }, + { + "x": 7.700864385195838, + "y": 4.896884050165335, + "heading": -0.3095988880574394, + "angularVelocity": 1.2068159114066472, + "velocityX": -1.3096852486783623, + "velocityY": 2.315432029313937, + "timestamp": 10.413426957144615 + }, + { + "x": 7.607749192737627, + "y": 5.0533313100207735, + "heading": -0.23416819513379072, + "angularVelocity": 1.252199254698346, + "velocityX": -1.5457736112180178, + "velocityY": 2.597127702232213, + "timestamp": 10.473665527516404 + }, + { + "x": 7.4985230728620555, + "y": 5.224975559172228, + "heading": -0.16073992185561656, + "angularVelocity": 1.2189577678384218, + "velocityX": -1.8132256327039729, + "velocityY": 2.8494077480935314, + "timestamp": 10.533904097888193 + }, + { + "x": 7.370615754860522, + "y": 5.408140956613431, + "heading": -0.09743106837998239, + "angularVelocity": 1.0509687246044361, + "velocityX": -2.123345843237925, + "velocityY": 3.0406664087596154, + "timestamp": 10.594142668259982 + }, + { + "x": 7.222231012592685, + "y": 5.5947592821558745, + "heading": -0.061240832593330456, + "angularVelocity": 0.600781784217115, + "velocityX": -2.4632845924465627, + "velocityY": 3.097987292703733, + "timestamp": 10.654381238631771 + }, + { + "x": 7.053607883062888, + "y": 5.775645840558194, + "heading": -0.04890249083117564, + "angularVelocity": 0.20482461130805693, + "velocityX": -2.7992551697203822, + "velocityY": 3.0028361776499053, + "timestamp": 10.71461980900356 + }, + { + "x": 6.867274277323916, + "y": 5.943523141012965, + "heading": -0.04597480359183944, + "angularVelocity": 0.04860153920099139, + "velocityX": -3.0932607561721848, + "velocityY": 2.7868739151450646, + "timestamp": 10.774858379375349 + }, + { + "x": 6.666800620063063, + "y": 6.094343858364721, + "heading": -0.04319414895342738, + "angularVelocity": 0.04616070104668807, + "velocityX": -3.3279949378535707, + "velocityY": 2.503723385546804, + "timestamp": 10.835096949747138 + }, + { + "x": 6.455252510472756, + "y": 6.226027819661062, + "heading": -0.03567675833355503, + "angularVelocity": 0.12479364256945982, + "velocityX": -3.511838150949524, + "velocityY": 2.18604061290953, + "timestamp": 10.895335520118927 + }, + { + "x": 6.234971447573829, + "y": 6.337451095987177, + "heading": -0.021378533882876995, + "angularVelocity": 0.23735995662629603, + "velocityX": -3.6568109359063894, + "velocityY": 1.8496998789714996, + "timestamp": 10.955574090490716 + }, + { + "x": 6.007773399353027, + "y": 6.4279327392578125, + "heading": 1.0451301322490283e-23, + "angularVelocity": 0.3548977631927474, + "velocityX": -3.771637454517041, + "velocityY": 1.5020549576822213, + "timestamp": 11.015812660862505 + }, + { + "x": 5.747502918390015, + "y": 6.502130719218711, + "heading": 0.02823606248628203, + "angularVelocity": 0.41875457589826204, + "velocityX": -3.8599381527595638, + "velocityY": 1.1003922252307357, + "timestamp": 11.08324132698684 + }, + { + "x": 5.486616061564071, + "y": 6.54859640899773, + "heading": 0.06008791313844042, + "angularVelocity": 0.47237847762588864, + "velocityX": -3.869079307380576, + "velocityY": 0.6891088382697167, + "timestamp": 11.150669993111176 + }, + { + "x": 5.232615422686886, + "y": 6.568179274186433, + "heading": 0.09435854776952027, + "angularVelocity": 0.5082502235456623, + "velocityX": -3.76695333715803, + "velocityY": 0.29042344027083444, + "timestamp": 11.218098659235512 + }, + { + "x": 4.994185583794339, + "y": 6.564780827927549, + "heading": 0.12929833940251856, + "angularVelocity": 0.5181741481964114, + "velocityX": -3.5360307803344355, + "velocityY": -0.05040061526083558, + "timestamp": 11.285527325359848 + }, + { + "x": 4.7782894744816735, + "y": 6.54517331085893, + "heading": 0.1630448352516845, + "angularVelocity": 0.5004769898152576, + "velocityX": -3.201844582162713, + "velocityY": -0.29078903967139774, + "timestamp": 11.352955991484183 + }, + { + "x": 4.588583133594499, + "y": 6.516394405425367, + "heading": 0.19417829885666568, + "angularVelocity": 0.4617244473970761, + "velocityX": -2.813437545054831, + "velocityY": -0.42680520152208584, + "timestamp": 11.420384657608519 + }, + { + "x": 4.426380144437834, + "y": 6.483997287524405, + "heading": 0.22180064905808416, + "angularVelocity": 0.4096529234388779, + "velocityX": -2.4055494269687476, + "velocityY": -0.4804650568235182, + "timestamp": 11.487813323732855 + }, + { + "x": 4.29190849535161, + "y": 6.4519615961956065, + "heading": 0.24536762088418218, + "angularVelocity": 0.3495096845404179, + "velocityX": -1.994280130623705, + "velocityY": -0.47510492450978464, + "timestamp": 11.55524198985719 + }, + { + "x": 4.184982062888447, + "y": 6.423114091223226, + "heading": 0.26454171695590006, + "angularVelocity": 0.2843611949309722, + "velocityX": -1.5857711357658024, + "velocityY": -0.42782256613509534, + "timestamp": 11.622670655981526 + }, + { + "x": 4.105281875933628, + "y": 6.39950678354966, + "heading": 0.2791044377060579, + "angularVelocity": 0.21597225018962463, + "velocityX": -1.1819926380844537, + "velocityY": -0.3501078848280095, + "timestamp": 11.690099322105862 + }, + { + "x": 4.052465191081701, + "y": 6.382673090712085, + "heading": 0.2889078281274289, + "angularVelocity": 0.1453890605413106, + "velocityX": -0.7832971922436318, + "velocityY": -0.24965187367837952, + "timestamp": 11.757527988230198 + }, + { + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "angularVelocity": 0.07325884092771977, + "velocityX": -0.38944343537672876, + "velocityY": -0.13171543471725672, + "timestamp": 11.824956654354533 + }, + { + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "angularVelocity": 8.278440610410196e-25, + "velocityX": 6.433403533567858e-24, + "velocityY": -1.0701732524530557e-23, + "timestamp": 11.89238532047887 + }, + { + "x": 4.018087490415642, + "y": 6.380018770347843, + "heading": 0.2795341228567479, + "angularVelocity": -0.3355412595403682, + "velocityX": -0.1903063381197778, + "velocityY": 0.1459774304073072, + "timestamp": 11.935043118258669 + }, + { + "x": 4.001589705706272, + "y": 6.3921613257192265, + "heading": 0.25120714606109396, + "angularVelocity": -0.66405155141573, + "velocityX": -0.3867472201572967, + "velocityY": 0.28465031022144105, + "timestamp": 11.977700916038469 + }, + { + "x": 3.97639313263677, + "y": 6.409754098035882, + "heading": 0.20896838468257176, + "angularVelocity": -0.9901767924485971, + "velocityX": -0.5906674601339826, + "velocityY": 0.4124163279002243, + "timestamp": 12.020358713818268 + }, + { + "x": 3.9421738742779104, + "y": 6.432121957225729, + "heading": 0.15224609434940017, + "angularVelocity": -1.3297050782127584, + "velocityX": -0.8021806126865689, + "velocityY": 0.524355694715196, + "timestamp": 12.063016511598068 + }, + { + "x": 3.8986904655937336, + "y": 6.458382475722753, + "heading": 0.07940140875468289, + "angularVelocity": -1.7076522789747404, + "velocityX": -1.0193542786395084, + "velocityY": 0.6156088655251506, + "timestamp": 12.105674309377868 + }, + { + "x": 3.8458035667226933, + "y": 6.4875155211783255, + "heading": -0.011842050030088008, + "angularVelocity": -2.1389631798568893, + "velocityX": -1.2397944015779776, + "velocityY": 0.6829477134745302, + "timestamp": 12.148332107157668 + }, + { + "x": 3.7833094863246672, + "y": 6.518292557296895, + "heading": -0.12260101088673928, + "angularVelocity": -2.59645285554573, + "velocityX": -1.4650095328554629, + "velocityY": 0.7214867555386046, + "timestamp": 12.190989904937467 + }, + { + "x": 3.7106930068716886, + "y": 6.548467697277574, + "heading": -0.24923492157262775, + "angularVelocity": -2.9685993482264874, + "velocityX": -1.702302585516201, + "velocityY": 0.7073768818644702, + "timestamp": 12.233647702717267 + }, + { + "x": 3.6287066452771013, + "y": 6.573438773987302, + "heading": -0.38557434807314456, + "angularVelocity": -3.196119668537644, + "velocityX": -1.921954856127452, + "velocityY": 0.5853812903945306, + "timestamp": 12.276305500497067 + }, + { + "x": 3.5407648103952, + "y": 6.589073554075392, + "heading": -0.5249239622169254, + "angularVelocity": -3.266685609583233, + "velocityX": -2.0615652813551293, + "velocityY": 0.36651634406438516, + "timestamp": 12.318963298276866 + }, + { + "x": 3.4508628456235098, + "y": 6.593818697188294, + "heading": -0.660906842350814, + "angularVelocity": -3.1877613756771255, + "velocityX": -2.107515377042373, + "velocityY": 0.11123741402210238, + "timestamp": 12.361621096056666 + }, + { + "x": 3.3616615472274485, + "y": 6.587670279666556, + "heading": -0.7900084359706764, + "angularVelocity": -3.0264476916105565, + "velocityX": -2.091090094629868, + "velocityY": -0.14413349590798102, + "timestamp": 12.404278893836466 + }, + { + "x": 3.274774187468561, + "y": 6.57100680311711, + "heading": -0.9098799975312181, + "angularVelocity": -2.8100738387697106, + "velocityX": -2.036845882373094, + "velocityY": -0.39063143004857287, + "timestamp": 12.446936691616266 + }, + { + "x": 3.191172742303364, + "y": 6.544245662152579, + "heading": -1.018505234653352, + "angularVelocity": -2.546433308227988, + "velocityX": -1.959816247354131, + "velocityY": -0.6273446440595214, + "timestamp": 12.489594489396065 + }, + { + "x": 3.111446083115606, + "y": 6.507722354665327, + "heading": -1.1144658869059407, + "angularVelocity": -2.2495453878782246, + "velocityX": -1.8689820698037456, + "velocityY": -0.856192991391337, + "timestamp": 12.532252287175865 + }, + { + "x": 3.035976307427136, + "y": 6.461676426153115, + "heading": -1.19697800039683, + "angularVelocity": -1.9342797281008164, + "velocityX": -1.7691906196856966, + "velocityY": -1.079425823852935, + "timestamp": 12.574910084955665 + }, + { + "x": 2.9650378227233887, + "y": 6.406276702880859, + "heading": -1.265712572039586, + "angularVelocity": -1.6113014553063978, + "velocityX": -1.662966406984595, + "velocityY": -1.2987009680675718, + "timestamp": 12.617567882735464 + }, + { + "x": 2.9126493602591794, + "y": 6.357513140307781, + "heading": -1.310830078079544, + "angularVelocity": -1.357475138846794, + "velocityX": -1.576240390915871, + "velocityY": -1.4671760406243946, + "timestamp": 12.650804223312786 + }, + { + "x": 2.8630250654829577, + "y": 6.303097416119462, + "heading": -1.3474650887415063, + "angularVelocity": -1.1022576500783743, + "velocityX": -1.4930733622967893, + "velocityY": -1.6372357258082417, + "timestamp": 12.684040563890107 + }, + { + "x": 2.816011074730038, + "y": 6.242970636362372, + "heading": -1.3754971781027352, + "angularVelocity": -0.843416840551841, + "velocityX": -1.4145357141092718, + "velocityY": -1.809067385659103, + "timestamp": 12.717276904467429 + }, + { + "x": 2.771398801296672, + "y": 6.177065835385166, + "heading": -1.39470215946782, + "angularVelocity": -0.5778308030153397, + "velocityX": -1.3422739284302563, + "velocityY": -1.982913877774391, + "timestamp": 12.75051324504475 + }, + { + "x": 2.7288922086629652, + "y": 6.105306533827174, + "heading": -1.404713958079907, + "angularVelocity": -0.3012304735774339, + "velocityX": -1.278919155820414, + "velocityY": -2.1590614463421836, + "timestamp": 12.783749585622072 + }, + { + "x": 2.6880462952478212, + "y": 6.027607098500584, + "heading": -1.4049934257695869, + "angularVelocity": -0.008408497591055405, + "velocityX": -1.228953389742779, + "velocityY": -2.3377855075780865, + "timestamp": 12.816985926199393 + }, + { + "x": 2.6481321260568067, + "y": 5.943876209208477, + "heading": -1.394939336924496, + "angularVelocity": 0.30250288300244654, + "velocityX": -1.2009194904643212, + "velocityY": -2.5192571696428567, + "timestamp": 12.850222266776715 + }, + { + "x": 2.60770583152771, + "y": 5.854036331176758, + "heading": -1.374993138249586, + "angularVelocity": 0.6001322145711994, + "velocityX": -1.2163280862719847, + "velocityY": -2.7030616629624156, + "timestamp": 12.883458607354036 + }, + { + "x": 2.5335974902090035, + "y": 5.699627153541733, + "heading": -1.326359964050082, + "angularVelocity": 0.9256228980196083, + "velocityX": -1.4104853073634442, + "velocityY": -2.9388308050189744, + "timestamp": 12.935999630597472 + }, + { + "x": 2.448082198863997, + "y": 5.538609196121231, + "heading": -1.2379059045506466, + "angularVelocity": 1.6835237313443032, + "velocityX": -1.627590900709989, + "velocityY": -3.0646140383385263, + "timestamp": 12.988540653840907 + }, + { + "x": 2.3666499769197493, + "y": 5.393860387223226, + "heading": -1.1439329584994455, + "angularVelocity": 1.7885633025417567, + "velocityX": -1.5498788740172182, + "velocityY": -2.7549674513826976, + "timestamp": 13.041081677084343 + }, + { + "x": 2.2923433899929266, + "y": 5.264310203060086, + "heading": -1.0550447251034136, + "angularVelocity": 1.6917872532514617, + "velocityX": -1.4142584658571056, + "velocityY": -2.465695872783135, + "timestamp": 13.093622700327778 + }, + { + "x": 2.2258000185216607, + "y": 5.149549363728709, + "heading": -0.9737081275164423, + "angularVelocity": 1.5480588798227184, + "velocityX": -1.2665031505563715, + "velocityY": -2.1842140150119014, + "timestamp": 13.146163723571213 + }, + { + "x": 2.1672914591200874, + "y": 5.049375589010017, + "heading": -0.9010867791693077, + "angularVelocity": 1.382183746415878, + "velocityX": -1.1135786056257322, + "velocityY": -1.9065821054638286, + "timestamp": 13.198704746814649 + }, + { + "x": 2.1169661488037037, + "y": 4.963668228608927, + "heading": -0.8378922046243602, + "angularVelocity": 1.2027663460635734, + "velocityX": -0.9578288965407921, + "velocityY": -1.6312465024517704, + "timestamp": 13.251245770058084 + }, + { + "x": 2.07491635973485, + "y": 4.8923475085199595, + "heading": -0.7846102526515801, + "angularVelocity": 1.0141019090152126, + "velocityX": -0.8003229947393125, + "velocityY": -1.3574292178993321, + "timestamp": 13.30378679330152 + }, + { + "x": 2.0412044621816885, + "y": 4.835357214512788, + "heading": -0.7415891809457651, + "angularVelocity": 0.8188091713876277, + "velocityX": -0.6416300154065587, + "velocityY": -1.0846818445678552, + "timestamp": 13.356327816544955 + }, + { + "x": 2.0158754197230326, + "y": 4.7926558969979665, + "heading": -0.7090843787352074, + "angularVelocity": 0.6186556752036474, + "velocityX": -0.4820812556561803, + "velocityY": -0.8127233707835632, + "timestamp": 13.40886883978839 + }, + { + "x": 1.9989634783303936, + "y": 4.76421184486368, + "heading": -0.6872854235406406, + "angularVelocity": 0.4148939980397224, + "velocityX": -0.3218807009959066, + "velocityY": -0.5413684465659858, + "timestamp": 13.461409863031825 + }, + { + "x": 1.990496039390564, + "y": 4.75, + "heading": -0.6763337151495863, + "angularVelocity": 0.20844109450081444, + "velocityX": -0.1611586226746669, + "velocityY": -0.27049044701382907, + "timestamp": 13.51395088627526 + }, + { + "x": 1.990496039390564, + "y": 4.75, + "heading": -0.6763337151495863, + "angularVelocity": -2.917607263748552e-24, + "velocityX": -1.8295999549572517e-24, + "velocityY": 1.1040980204698261e-23, + "timestamp": 13.566491909518696 + }, + { + "x": 2.0311019529427297, + "y": 4.707552352551894, + "heading": -0.6763337151495863, + "angularVelocity": -5.2261319194150544e-17, + "velocityX": 0.41398186246377605, + "velocityY": -0.4327585469834974, + "timestamp": 13.664578123514891 + }, + { + "x": 2.1123137791008166, + "y": 4.622657058644844, + "heading": -0.6763337151495863, + "angularVelocity": -1.825824738863161e-16, + "velocityX": 0.8279637152804861, + "velocityY": -0.8655170838823737, + "timestamp": 13.762664337511087 + }, + { + "x": 2.2341315150260925, + "y": 4.495314121246338, + "heading": -0.6763337151495863, + "angularVelocity": -9.603465419020808e-17, + "velocityX": 1.2419455391559997, + "velocityY": -1.2982755905273877, + "timestamp": 13.860750551507282 + }, + { + "x": 2.3559492509513684, + "y": 4.367971183847831, + "heading": -0.6763337151495863, + "angularVelocity": -2.328687477765739e-16, + "velocityX": 1.2419455391559997, + "velocityY": -1.2982755905273877, + "timestamp": 13.958836765503477 + }, + { + "x": 2.4371610771094554, + "y": 4.283075889940782, + "heading": -0.6763337151495863, + "angularVelocity": -1.6799417860231906e-16, + "velocityX": 0.8279637152804862, + "velocityY": -0.8655170838823736, + "timestamp": 14.056922979499673 + }, + { + "x": 2.477766990661621, + "y": 4.240628242492676, + "heading": -0.6763337151495863, + "angularVelocity": -1.0297253977874022e-16, + "velocityX": 0.4139818624637761, + "velocityY": -0.4327585469834973, + "timestamp": 14.155009193495868 + }, + { + "x": 2.477766990661621, + "y": 4.240628242492676, + "heading": -0.6763337151495863, + "angularVelocity": -1.0483209862050802e-28, + "velocityX": -1.5321576073165576e-26, + "velocityY": -1.2874611866638058e-26, + "timestamp": 14.253095407492063 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "timestamp": 1.073251191201399, + "isStopPoint": true, + "x": 2.6293623447418213, + "y": 6.7744364738464355, + "heading": 0.610560190519, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "timestamp": 3.0133515887203335, + "isStopPoint": false, + "x": 7.946028232574463, + "y": 7.456615924835205, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "timestamp": 4.694185309453636, + "isStopPoint": true, + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 5.499203745729279, + "isStopPoint": false, + "x": 6.018601417541504, + "y": 6.676982402801514, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "timestamp": 6.318862766261983, + "isStopPoint": false, + "x": 7.891887187957764, + "y": 5.810723304748535, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "timestamp": 7.187662783615323, + "isStopPoint": false, + "x": 6.018601417541504, + "y": 6.676982402801514, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 8.056018605131612, + "isStopPoint": true, + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 8.870498149452597, + "isStopPoint": false, + "x": 6.007773399353027, + "y": 6.4279327392578125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "timestamp": 9.931518394170304, + "isStopPoint": false, + "x": 8.010997772216797, + "y": 4.30559778213501, + "heading": -0.6499245399095859, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "timestamp": 11.015812660862505, + "isStopPoint": false, + "x": 6.007773399353027, + "y": 6.4279327392578125, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 11.89238532047887, + "isStopPoint": true, + "x": 4.026205539703369, + "y": 6.373791694641113, + "heading": 0.293847574053, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 12.617567882735464, + "isStopPoint": false, + "x": 2.9650378227233887, + "y": 6.406276702880859, + "heading": -1.265712572039586, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "timestamp": 12.883458607354036, + "isStopPoint": false, + "x": 2.60770583152771, + "y": 5.854036331176758, + "heading": -1.374993138249586, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 13.566491909518696, + "isStopPoint": true, + "x": 1.990496039390564, + "y": 4.75, + "heading": -0.6763337151495863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "timestamp": 14.253095407492063, + "isStopPoint": true, + "x": 2.477766990661621, + "y": 4.240628242492676, + "heading": -0.6763337151495863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 7 + ], + "type": "StopPoint" + }, + { + "scope": [ + 11 + ], + "type": "StopPoint" + }, + { + "scope": [ + 14 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "Distance Center 6 Wing First": { + "waypoints": [ + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 2.629361867904663, + "y": 4.316425800323486, + "heading": -0.4973420499689195, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 2.510251045227051, + "y": 5.334280490875244, + "heading": 0.5713370823927012, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 3.0191783905029297, + "y": 5.832379341125488, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "x": 2.445281744003296, + "y": 6.449589252471924, + "heading": 0.5224037961561394, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 + }, + { + "x": 2.7051594257354736, + "y": 6.882719039916992, + "heading": 0.5224037961561394, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 6.776577949523926, + "y": 6.9043755531311035, + "heading": 0.5224037961561394, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 8.021825790405273, + "y": 7.337504863739014, + "heading": 0.5224037961561394, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "x": 3.733842372894287, + "y": 6.697348880767819, + "heading": 0.5224037961561394, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 5.7778338432312015, + "y": 6.801184844970702, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 8.03265380859375, + "y": 5.864864349365234, + "heading": -0.302884711661745, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 5.802036285400391, + "y": 6.762963199615477, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 3.733842372894287, + "y": 5.897348880767822, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.35, + "y": 5.58225679397583, + "heading": -8.23399117094977e-21, + "angularVelocity": 3.616025777443299e-20, + "velocityX": -8.860914587038282e-20, + "velocityY": 8.140056679610733e-20, + "timestamp": 0 + }, + { + "x": 1.376109320489114, + "y": 5.556422100544237, + "heading": -0.010099266800899679, + "angularVelocity": -0.12998961247130109, + "velocityX": 0.33605810393746355, + "velocityY": -0.33252332606842316, + "timestamp": 0.07769287567596511 + }, + { + "x": 1.4283280795481978, + "y": 5.50475284033716, + "heading": -0.03029817312262219, + "angularVelocity": -0.2599840222926798, + "velocityX": 0.672117727716421, + "velocityY": -0.6650450219216428, + "timestamp": 0.15538575135193022 + }, + { + "x": 1.5066564357182666, + "y": 5.42724921578407, + "heading": -0.0605984277415001, + "angularVelocity": -0.39000042610408264, + "velocityX": 1.0081793921073796, + "velocityY": -0.9975641122660442, + "timestamp": 0.23307862702789534 + }, + { + "x": 1.6110945641629029, + "y": 5.323911514336438, + "heading": -0.10100431936436054, + "angularVelocity": -0.5200720306889132, + "velocityX": 1.3442433110626257, + "velocityY": -1.3300795027670849, + "timestamp": 0.31077150270386045 + }, + { + "x": 1.74164261984874, + "y": 5.194740113776595, + "heading": -0.15152427748693714, + "angularVelocity": -0.6502521329404897, + "velocityX": 1.6803092246233249, + "velocityY": -1.6625900307588073, + "timestamp": 0.38846437837982556 + }, + { + "x": 1.8983006855277498, + "y": 5.039735479549906, + "heading": -0.2121726212286143, + "angularVelocity": -0.7806165393417044, + "velocityX": 2.0163762033006187, + "velocityY": -1.9950945679134022, + "timestamp": 0.46615725405579067 + }, + { + "x": 2.0810686976676913, + "y": 4.8588981712823704, + "heading": -0.282971977534701, + "angularVelocity": -0.9112721815237043, + "velocityX": 2.3524423642421857, + "velocityY": -2.327591902013722, + "timestamp": 0.5438501297317557 + }, + { + "x": 2.2377244681281647, + "y": 4.703905740759624, + "heading": -0.3441858501541506, + "angularVelocity": -0.7878955706924178, + "velocityX": 2.0163466610997847, + "velocityY": -1.9949374916842595, + "timestamp": 0.6215430054077209 + }, + { + "x": 2.3682705543010854, + "y": 4.574745653480951, + "heading": -0.3952223440590563, + "angularVelocity": -0.6569005647025403, + "velocityX": 1.6802838746423994, + "velocityY": -1.6624444153330409, + "timestamp": 0.6992358810836861 + }, + { + "x": 2.4727071860687984, + "y": 4.471417690375359, + "heading": -0.43606485774485665, + "angularVelocity": -0.5256918775428373, + "velocityX": 1.344224047045041, + "velocityY": -1.3299541586868706, + "timestamp": 0.7769287567596512 + }, + { + "x": 2.5510345452184433, + "y": 4.393921749583127, + "heading": -0.4667026260025969, + "angularVelocity": -0.39434462930065384, + "velocityX": 1.0081665592650493, + "velocityY": -0.9974652130968322, + "timestamp": 0.8546216324356164 + }, + { + "x": 2.6032527543826127, + "y": 4.34225778871809, + "heading": -0.4871292250327995, + "angularVelocity": -0.2629146990953991, + "velocityX": 0.6721106499128248, + "velocityY": -0.6649768130673013, + "timestamp": 0.9323145081115816 + }, + { + "x": 2.629361867904663, + "y": 4.316425800323486, + "heading": -0.4973420499689195, + "angularVelocity": -0.1314512411500224, + "velocityX": 0.3360554400244373, + "velocityY": -0.33248850901518073, + "timestamp": 1.0100073837875467 + }, + { + "x": 2.629361867904663, + "y": 4.316425800323486, + "heading": -0.4973420499689195, + "angularVelocity": -4.651997800205279e-20, + "velocityX": 8.78848662911863e-20, + "velocityY": -7.782125385278791e-20, + "timestamp": 1.087700259463512 + }, + { + "x": 2.6233052396026286, + "y": 4.326393541424155, + "heading": -0.47896226252358143, + "angularVelocity": 0.4000703563411541, + "velocityX": -0.13183381201916095, + "velocityY": 0.21696647721963866, + "timestamp": 1.133641647406949 + }, + { + "x": 2.6115515837229393, + "y": 4.3466736424263335, + "heading": -0.44330520584356, + "angularVelocity": 0.7761423473736202, + "velocityX": -0.25584024353292506, + "velocityY": 0.4414342254341193, + "timestamp": 1.1795830353503862 + }, + { + "x": 2.5946459289354515, + "y": 4.3776713184474, + "heading": -0.39137970233868447, + "angularVelocity": 1.1302554369669013, + "velocityX": -0.36798310944156426, + "velocityY": 0.6747222364990407, + "timestamp": 1.2255244232938234 + }, + { + "x": 2.573337356420698, + "y": 4.419766771585406, + "heading": -0.32351956642109647, + "angularVelocity": 1.4771024332381333, + "velocityX": -0.46382082624470733, + "velocityY": 0.9162860553937363, + "timestamp": 1.2714658112372605 + }, + { + "x": 2.548515626258525, + "y": 4.473258670814533, + "heading": -0.23928333849781427, + "angularVelocity": 1.8335586209757855, + "velocityX": -0.5402912552997571, + "velocityY": 1.1643509615988623, + "timestamp": 1.3174071991806977 + }, + { + "x": 2.521204362807651, + "y": 4.538447030993977, + "heading": -0.13846900348834276, + "angularVelocity": 2.19441204374569, + "velocityX": -0.594480590888976, + "velocityY": 1.4189462508120771, + "timestamp": 1.3633485871241349 + }, + { + "x": 2.4928952875938295, + "y": 4.615794515250989, + "heading": -0.02317453113785117, + "angularVelocity": 2.509599241808752, + "velocityX": -0.6161998250613465, + "velocityY": 1.6836122659646693, + "timestamp": 1.409289975067572 + }, + { + "x": 2.4664488301334497, + "y": 4.705652347771511, + "heading": 0.1004658369360928, + "angularVelocity": 2.691263229273123, + "velocityX": -0.57565647544085, + "velocityY": 1.9559233306393553, + "timestamp": 1.4552313630110092 + }, + { + "x": 2.4465017409069274, + "y": 4.806636359026755, + "heading": 0.223493186130365, + "angularVelocity": 2.6779197299337794, + "velocityX": -0.4341856029922523, + "velocityY": 2.1981053637207353, + "timestamp": 1.5011727509544464 + }, + { + "x": 2.437142466873225, + "y": 4.914004319219758, + "heading": 0.3340193354303189, + "angularVelocity": 2.405807796578398, + "velocityX": -0.20372205657403003, + "velocityY": 2.337063919905811, + "timestamp": 1.5471141388978835 + }, + { + "x": 2.4390218774995867, + "y": 5.023216279374713, + "heading": 0.42300384320052303, + "angularVelocity": 1.9369137884941887, + "velocityX": 0.040908877822231356, + "velocityY": 2.377202018568005, + "timestamp": 1.5930555268413207 + }, + { + "x": 2.4523438963207767, + "y": 5.131375051574103, + "heading": 0.4925726441961787, + "angularVelocity": 1.514294715721443, + "velocityX": 0.2899785883176208, + "velocityY": 2.3542774182737958, + "timestamp": 1.6389969147847578 + }, + { + "x": 2.4764740733075925, + "y": 5.235554065862188, + "heading": 0.5432225068747625, + "angularVelocity": 1.1024887350147914, + "velocityX": 0.5252383105300292, + "velocityY": 2.2676505641568747, + "timestamp": 1.684938302728195 + }, + { + "x": 2.510251045227051, + "y": 5.334280490875244, + "heading": 0.5713370823927012, + "angularVelocity": 0.6119661763931314, + "velocityX": 0.7352187957630835, + "velocityY": 2.148964788234253, + "timestamp": 1.7308796906716322 + }, + { + "x": 2.5650236180534773, + "y": 5.445977034086529, + "heading": 0.5690314952034347, + "angularVelocity": -0.041009635460105165, + "velocityX": 0.9742434618308231, + "velocityY": 1.9867539777170269, + "timestamp": 1.7871003120792386 + }, + { + "x": 2.631436378611181, + "y": 5.546741566981676, + "heading": 0.5272160744085959, + "angularVelocity": -0.7437737212413915, + "velocityX": 1.1812882692313675, + "velocityY": 1.792305569954328, + "timestamp": 1.843320933486845 + }, + { + "x": 2.707043830480548, + "y": 5.633772829267234, + "heading": 0.4487228756397609, + "angularVelocity": -1.3961638417289928, + "velocityX": 1.3448348662175675, + "velocityY": 1.5480309556625385, + "timestamp": 1.8995415548944514 + }, + { + "x": 2.7862462006186286, + "y": 5.702826720944244, + "heading": 0.3492973403296841, + "angularVelocity": -1.768488729237435, + "velocityX": 1.4087779208958542, + "velocityY": 1.228266247296711, + "timestamp": 1.9557621763020578 + }, + { + "x": 2.8600121003633747, + "y": 5.75337861080909, + "heading": 0.24738242695849674, + "angularVelocity": -1.8127674653805728, + "velocityX": 1.3120790538748128, + "velocityY": 0.8991698881863764, + "timestamp": 2.0119827977096643 + }, + { + "x": 2.9223194382842412, + "y": 5.7887086014295965, + "heading": 0.1556886936267466, + "angularVelocity": -1.6309626438839835, + "velocityX": 1.1082648387880867, + "velocityY": 0.6284169355646319, + "timestamp": 2.0682034191172707 + }, + { + "x": 2.9703569068277655, + "y": 5.812107898641809, + "heading": 0.08110867474881293, + "angularVelocity": -1.326559846025525, + "velocityX": 0.854445705878034, + "velocityY": 0.416204884015149, + "timestamp": 2.124424040524877 + }, + { + "x": 3.00283548948403, + "y": 5.82606180657968, + "heading": 0.028064925087883746, + "angularVelocity": -0.943492767117523, + "velocityX": 0.5776987490193435, + "velocityY": 0.2481991053905678, + "timestamp": 2.1806446619324835 + }, + { + "x": 3.0191783905029297, + "y": 5.832379341125488, + "heading": -3.6672255294112824e-21, + "angularVelocity": -0.4991927229763187, + "velocityX": 0.2906922870953705, + "velocityY": 0.11237041476304822, + "timestamp": 2.23686528334009 + }, + { + "x": 3.0191783905029297, + "y": 5.832379341125488, + "heading": -2.1726460551285392e-21, + "angularVelocity": 3.7108122990500546e-22, + "velocityX": 2.1269801588032454e-22, + "velocityY": 1.2368753944018978e-21, + "timestamp": 2.2930859047476964 + }, + { + "x": 3.002861357001422, + "y": 5.839481859090172, + "heading": 0.011550492706860791, + "angularVelocity": 0.21199493252787568, + "velocityX": -0.2994788624170613, + "velocityY": 0.1303578864481573, + "timestamp": 2.3475706632706013 + }, + { + "x": 2.970454672747075, + "y": 5.854191308674451, + "heading": 0.034599552401506535, + "angularVelocity": 0.42303683304306455, + "velocityX": -0.5947843971947421, + "velocityY": 0.26997365837814763, + "timestamp": 2.402055421793506 + }, + { + "x": 2.922320531881221, + "y": 5.877237889129693, + "heading": 0.06905281952724152, + "angularVelocity": 0.6323468812154363, + "velocityX": -0.8834423088361375, + "velocityY": 0.4229913296863304, + "timestamp": 2.456540180316411 + }, + { + "x": 2.8591099926637993, + "y": 5.909758325766861, + "heading": 0.11477832447476202, + "angularVelocity": 0.8392347912911837, + "velocityX": -1.1601508555984257, + "velocityY": 0.5968721807493298, + "timestamp": 2.511024938839316 + }, + { + "x": 2.782225597166992, + "y": 5.953695739861746, + "heading": 0.171638299210357, + "angularVelocity": 1.043594140399681, + "velocityX": -1.4111174864524112, + "velocityY": 0.8064166068830917, + "timestamp": 2.565509697362221 + }, + { + "x": 2.695621918494965, + "y": 6.012601850588778, + "heading": 0.23913912308030816, + "angularVelocity": 1.238893696143934, + "velocityX": -1.5895028448298905, + "velocityY": 1.0811484224944983, + "timestamp": 2.619994455885126 + }, + { + "x": 2.6121558310066813, + "y": 6.089315201605884, + "heading": 0.31084169357843383, + "angularVelocity": 1.316011531334634, + "velocityX": -1.5319162597223552, + "velocityY": 1.4079781776927003, + "timestamp": 2.6744792144080307 + }, + { + "x": 2.5439162846102246, + "y": 6.175738160019639, + "heading": 0.3773551147962972, + "angularVelocity": 1.2207711481349772, + "velocityX": -1.2524520296399875, + "velocityY": 1.5861859491847345, + "timestamp": 2.7289639729309356 + }, + { + "x": 2.4931765430661446, + "y": 6.266142072193765, + "heading": 0.43533450902171794, + "angularVelocity": 1.0641396933244534, + "velocityX": -0.9312648696561509, + "velocityY": 1.6592514058059242, + "timestamp": 2.7834487314538405 + }, + { + "x": 2.460287773937943, + "y": 6.357897509213886, + "heading": 0.48380582267307914, + "angularVelocity": 0.8896306960961281, + "velocityX": -0.6036324656624045, + "velocityY": 1.684056963958949, + "timestamp": 2.8379334899767454 + }, + { + "x": 2.445281744003296, + "y": 6.449589252471924, + "heading": 0.5224037961561394, + "angularVelocity": 0.7084178131547396, + "velocityX": -0.275417021961089, + "velocityY": 1.6828879441485027, + "timestamp": 2.8924182484996503 + }, + { + "x": 2.4640368595524906, + "y": 6.587540353459253, + "heading": 0.5579085954415691, + "angularVelocity": 0.42638133020810537, + "velocityX": 0.22523239891554875, + "velocityY": 1.6566710733888894, + "timestamp": 2.975688309287961 + }, + { + "x": 2.52273710591792, + "y": 6.71371975523591, + "heading": 0.5680417649597526, + "angularVelocity": 0.12169043017686823, + "velocityX": 0.704938195189473, + "velocityY": 1.5153033465104557, + "timestamp": 3.0589583700762715 + }, + { + "x": 2.605078049581534, + "y": 6.805600488381497, + "heading": 0.5525667893518443, + "angularVelocity": -0.1858408107476787, + "velocityX": 0.9888421226560808, + "velocityY": 1.1034065818586072, + "timestamp": 3.142228430864582 + }, + { + "x": 2.670460197759116, + "y": 6.858702767349351, + "heading": 0.5337797940272959, + "angularVelocity": -0.22561524690499246, + "velocityX": 0.785181943649555, + "velocityY": 0.6377115432021956, + "timestamp": 3.2254984916528926 + }, + { + "x": 2.7051594257354736, + "y": 6.882719039916992, + "heading": 0.5224037961561394, + "angularVelocity": -0.13661570273230195, + "velocityX": 0.41670712916338404, + "velocityY": 0.28841425525909853, + "timestamp": 3.308768552441203 + }, + { + "x": 2.7051594257354736, + "y": 6.882719039916992, + "heading": 0.5224037961561394, + "angularVelocity": 6.900030919446935e-23, + "velocityX": -5.778219378902074e-20, + "velocityY": -2.504372507974792e-19, + "timestamp": 3.3920386132295137 + }, + { + "x": 2.7398288490574805, + "y": 6.881986068889246, + "heading": 0.5224037961561394, + "angularVelocity": 5.395309096135092e-17, + "velocityX": 0.4600360054616196, + "velocityY": -0.009725949595165711, + "timestamp": 3.4674010259763555 + }, + { + "x": 2.809167695484875, + "y": 6.880520128147862, + "heading": 0.5224037961561394, + "angularVelocity": 1.1380978370170907e-16, + "velocityX": 0.9200720080488729, + "velocityY": -0.019451881753150334, + "timestamp": 3.542763438723197 + }, + { + "x": 2.9131759647111264, + "y": 6.878321218057033, + "heading": 0.5224037961561394, + "angularVelocity": 1.7975067781041165e-16, + "velocityX": 1.380108006568699, + "velocityY": -0.029177809078592246, + "timestamp": 3.618125851470039 + }, + { + "x": 3.0518536562841723, + "y": 6.875389337051742, + "heading": 0.5224037961561394, + "angularVelocity": 2.5102040552411234e-16, + "velocityX": 1.8401439990900241, + "velocityY": -0.03890375717055886, + "timestamp": 3.6934882642168807 + }, + { + "x": 3.2252007695055918, + "y": 6.8717244798411565, + "heading": 0.5224037961561394, + "angularVelocity": 3.2835275620118203e-16, + "velocityX": 2.3001799823438382, + "velocityY": -0.04862977546772752, + "timestamp": 3.7688506769637224 + }, + { + "x": 3.433217303234589, + "y": 6.8673266340826435, + "heading": 0.5224037961561394, + "angularVelocity": 3.215116027866161e-16, + "velocityX": 2.7602159504601937, + "velocityY": -0.05835595754194546, + "timestamp": 3.844213089710564 + }, + { + "x": 3.6759032554627358, + "y": 6.8621957741400355, + "heading": 0.5224037961561394, + "angularVelocity": 4.116551365644819e-16, + "velocityX": 3.220251891926296, + "velocityY": -0.06808247978795974, + "timestamp": 3.919575502457406 + }, + { + "x": 3.9532586221839736, + "y": 6.85633184925441, + "heading": 0.5224037961561394, + "angularVelocity": 5.073242166542338e-16, + "velocityX": 3.680287780235135, + "velocityY": -0.07780967556497664, + "timestamp": 3.9949379152042477 + }, + { + "x": 4.265283392553291, + "y": 6.849734762645486, + "heading": 0.5224037961561394, + "angularVelocity": 5.186061419797297e-16, + "velocityX": 4.140323524639203, + "velocityY": -0.08753815554028055, + "timestamp": 4.070300327951089 + }, + { + "x": 4.580344671204565, + "y": 6.84307401284669, + "heading": 0.5224037961561394, + "angularVelocity": 2.9918936261002046e-16, + "velocityX": 4.180615603558647, + "velocityY": -0.08838291604556786, + "timestamp": 4.14566274069793 + }, + { + "x": 4.895405891408077, + "y": 6.836410498973634, + "heading": 0.5224037961561394, + "angularVelocity": -6.759041033244585e-16, + "velocityX": 4.180614828002794, + "velocityY": -0.08841959313908884, + "timestamp": 4.221025153444772 + }, + { + "x": 5.2104670226750835, + "y": 6.829742781371814, + "heading": 0.5224037961561394, + "angularVelocity": 1.2122501066067013e-16, + "velocityX": 4.180613647885222, + "velocityY": -0.08847537331665184, + "timestamp": 4.296387566191613 + }, + { + "x": 5.525528609681589, + "y": 6.8230966331486265, + "heading": 0.5224037961561394, + "angularVelocity": -5.738963451554141e-16, + "velocityX": 4.18061969519029, + "velocityY": -0.0881891646106545, + "timestamp": 4.371749978938454 + }, + { + "x": 5.840589424991367, + "y": 6.8164140027083135, + "heading": 0.5224037961561394, + "angularVelocity": 2.6947897759150035e-16, + "velocityX": 4.180609455381084, + "velocityY": -0.08867325496545927, + "timestamp": 4.4471123916852955 + }, + { + "x": 6.155679411271227, + "y": 6.811288045591518, + "heading": 0.5224037961561394, + "angularVelocity": -2.0026083836087547e-16, + "velocityX": 4.18099653123254, + "velocityY": -0.06801742314188503, + "timestamp": 4.522474804432137 + }, + { + "x": 6.4694252556395035, + "y": 6.840809560208346, + "heading": 0.5224037961561394, + "angularVelocity": -1.0762473033828276e-16, + "velocityX": 4.163160824245574, + "velocityY": 0.3917273019906568, + "timestamp": 4.597837217178978 + }, + { + "x": 6.776577949523926, + "y": 6.9043755531311035, + "heading": 0.5224037961561394, + "angularVelocity": -3.677462960461617e-16, + "velocityX": 4.0756748979921005, + "velocityY": 0.843470778148919, + "timestamp": 4.673199629925819 + }, + { + "x": 7.013543953390312, + "y": 6.961987444002269, + "heading": 0.5224037961561394, + "angularVelocity": -4.893185861598382e-16, + "velocityX": 3.6868185744404176, + "velocityY": 0.8963504718263726, + "timestamp": 4.737473474456494 + }, + { + "x": 7.225303695322754, + "y": 7.018671437407212, + "heading": 0.5224037961561394, + "angularVelocity": -4.901113542018955e-16, + "velocityX": 3.2946487560952082, + "velocityY": 0.8819138456528895, + "timestamp": 4.801747318987169 + }, + { + "x": 7.411968501618956, + "y": 7.072813587578339, + "heading": 0.5224037961561394, + "angularVelocity": -4.1553599222426735e-16, + "velocityX": 2.9042109999677495, + "velocityY": 0.8423667600167797, + "timestamp": 4.866021163517844 + }, + { + "x": 7.573636696281331, + "y": 7.123581388206941, + "heading": 0.5224037961561394, + "angularVelocity": -4.213277119698898e-16, + "velocityX": 2.5153030107794323, + "velocityY": 0.7898671846893066, + "timestamp": 4.930295008048518 + }, + { + "x": 7.710381873366203, + "y": 7.1704684590736765, + "heading": 0.5224037961561394, + "angularVelocity": -4.2912506340016083e-16, + "velocityX": 2.12754002943782, + "velocityY": 0.729489129040025, + "timestamp": 4.994568852579193 + }, + { + "x": 7.822259372652904, + "y": 7.213134689148531, + "heading": 0.5224037961561394, + "angularVelocity": -4.3865920867652994e-16, + "velocityX": 1.7406380480835721, + "velocityY": 0.6638194803251983, + "timestamp": 5.058842697109868 + }, + { + "x": 7.909311852563935, + "y": 7.25133601958431, + "heading": 0.5224037961561394, + "angularVelocity": -4.515086254769384e-16, + "velocityX": 1.3543997647361083, + "velocityY": 0.5943526595417427, + "timestamp": 5.123116541640543 + }, + { + "x": 7.971573038400992, + "y": 7.284888837484052, + "heading": 0.5224037961561394, + "angularVelocity": -4.833939499970947e-16, + "velocityX": 0.9686861940760827, + "velocityY": 0.5220291106708211, + "timestamp": 5.187390386171217 + }, + { + "x": 8.009070195209654, + "y": 7.3136500159409, + "heading": 0.5224037961561394, + "angularVelocity": -5.006358797639765e-16, + "velocityX": 0.5833968246720782, + "velocityY": 0.4474787320855215, + "timestamp": 5.251664230701892 + }, + { + "x": 8.021825790405273, + "y": 7.337504863739014, + "heading": 0.5224037961561394, + "angularVelocity": -4.469977199106365e-16, + "velocityX": 0.19845701293833062, + "velocityY": 0.3711439384449641, + "timestamp": 5.315938075232567 + }, + { + "x": 7.983783192652999, + "y": 7.362254574962456, + "heading": 0.5224037961561394, + "angularVelocity": -4.2160773073279116e-16, + "velocityX": -0.3881238839919007, + "velocityY": 0.25250520772193735, + "timestamp": 5.41395471198525 + }, + { + "x": 7.888245908562877, + "y": 7.375375716980563, + "heading": 0.5224037961561394, + "angularVelocity": -4.616597613928156e-16, + "velocityX": -0.9747047772224879, + "velocityY": 0.1338664787204887, + "timestamp": 5.511971348737933 + }, + { + "x": 7.7352139386258125, + "y": 7.376868290112506, + "heading": 0.5224037961561394, + "angularVelocity": -5.113708650512922e-16, + "velocityX": -1.561285665444697, + "velocityY": 0.015227752975333887, + "timestamp": 5.609987985490616 + }, + { + "x": 7.524687283587215, + "y": 7.36673229479228, + "heading": 0.5224037961561394, + "angularVelocity": -5.254329700677736e-16, + "velocityX": -2.147866546061973, + "velocityY": -0.10341096834204828, + "timestamp": 5.7080046222432985 + }, + { + "x": 7.256665944815729, + "y": 7.344967731118788, + "heading": 0.5224037961561394, + "angularVelocity": -3.7838755078538407e-16, + "velocityX": -2.7344474127158738, + "velocityY": -0.22204968865039343, + "timestamp": 5.806021258995981 + }, + { + "x": 6.931149925685574, + "y": 7.311574596140671, + "heading": 0.5224037961561394, + "angularVelocity": -3.394615825938055e-16, + "velocityX": -3.321028244944788, + "velocityY": -0.3406884390695356, + "timestamp": 5.904037895748664 + }, + { + "x": 6.548139239864235, + "y": 7.26655286347746, + "heading": 0.5224037961561394, + "angularVelocity": -4.742302930350804e-16, + "velocityX": -3.9076089377332557, + "velocityY": -0.45932745863144137, + "timestamp": 6.002054532501347 + }, + { + "x": 6.146412026065515, + "y": 7.185302300694055, + "heading": 0.5224037961561394, + "angularVelocity": -2.3651181209873e-16, + "velocityX": -4.09856150045591, + "velocityY": -0.828946651050856, + "timestamp": 6.10007116925403 + }, + { + "x": 5.744684992223911, + "y": 7.1040508481538485, + "heading": 0.5224037961561394, + "angularVelocity": -2.6117867815425296e-16, + "velocityX": -4.098559664470492, + "velocityY": -0.8289557286608469, + "timestamp": 6.198087806006713 + }, + { + "x": 5.34295787439086, + "y": 7.022799810888984, + "heading": 0.5224037961561394, + "angularVelocity": -3.2687293678006866e-16, + "velocityX": -4.098560521380605, + "velocityY": -0.8289514918766033, + "timestamp": 6.296104442759396 + }, + { + "x": 4.941230796975172, + "y": 6.941548573791956, + "heading": 0.5224037961561394, + "angularVelocity": -1.6274431602051954e-16, + "velocityX": -4.098560109028538, + "velocityY": -0.8289535306342138, + "timestamp": 6.394121079512079 + }, + { + "x": 4.596262688532624, + "y": 6.871777230633822, + "heading": 0.5224037961561394, + "angularVelocity": 5.785170797428673e-16, + "velocityX": -3.5194852615987555, + "velocityY": -0.7118316386858059, + "timestamp": 6.492137716264762 + }, + { + "x": 4.308789253876432, + "y": 6.813634450082659, + "heading": 0.5224037961561394, + "angularVelocity": 3.9995305722921473e-16, + "velocityX": -2.9329044964228768, + "velocityY": -0.5931929770031786, + "timestamp": 6.590154353017445 + }, + { + "x": 4.0788105028099615, + "y": 6.767120224861494, + "heading": 0.5224037961561394, + "angularVelocity": 3.6861351582812966e-16, + "velocityX": -2.3463236312296227, + "velocityY": -0.4745543895627656, + "timestamp": 6.688170989770128 + }, + { + "x": 3.906326438276487, + "y": 6.732234554149373, + "heading": 0.5224037961561394, + "angularVelocity": 2.21916201762866e-16, + "velocityX": -1.7597427360080535, + "velocityY": -0.3559158104980193, + "timestamp": 6.7861876265228105 + }, + { + "x": 3.79133706145364, + "y": 6.708977438989503, + "heading": 0.5224037961561394, + "angularVelocity": 1.8561143927305445e-16, + "velocityX": -1.1731618287718801, + "velocityY": -0.23727722079009275, + "timestamp": 6.884204263275493 + }, + { + "x": 3.733842372894287, + "y": 6.697348880767819, + "heading": 0.5224037961561394, + "angularVelocity": 5.508639517425219e-17, + "velocityX": -0.5865809158951689, + "velocityY": -0.11863861694240564, + "timestamp": 6.982220900028176 + }, + { + "x": 3.733842372894287, + "y": 6.697348880767819, + "heading": 0.5224037961561394, + "angularVelocity": -1.6835319404035148e-20, + "velocityX": 3.846585110796379e-19, + "velocityY": 3.193403425909451e-20, + "timestamp": 7.080237536780859 + }, + { + "x": 3.751331338841791, + "y": 6.699587280336841, + "heading": 0.513975805961829, + "angularVelocity": -0.15604839616653143, + "velocityX": 0.3238168322219289, + "velocityY": 0.04144507227375776, + "timestamp": 7.134246359693184 + }, + { + "x": 3.7863260736878988, + "y": 6.704004062694685, + "heading": 0.4973370967850019, + "angularVelocity": -0.308073908661883, + "velocityX": 0.6479447793727443, + "velocityY": 0.08177890425448403, + "timestamp": 7.188255182605508 + }, + { + "x": 3.838845900665831, + "y": 6.71052756994018, + "heading": 0.4727433220581201, + "angularVelocity": -0.4553658717355572, + "velocityX": 0.9724305057192445, + "velocityY": 0.12078595484453863, + "timestamp": 7.242264005517833 + }, + { + "x": 3.9089133882112788, + "y": 6.719070782800993, + "heading": 0.4405046382901893, + "angularVelocity": -0.5969151340377453, + "velocityX": 1.2973340977860786, + "velocityY": 0.15818180067878418, + "timestamp": 7.2962728284301575 + }, + { + "x": 3.9965552657179173, + "y": 6.729525850718403, + "heading": 0.40100871332347776, + "angularVelocity": -0.7312865349949899, + "velocityX": 1.622732597763012, + "velocityY": 0.19358074021316565, + "timestamp": 7.350281651342482 + }, + { + "x": 4.101803555150174, + "y": 6.741755679434534, + "heading": 0.354754655561301, + "angularVelocity": -0.8564166976433442, + "velocityX": 1.9487240002085033, + "velocityY": 0.22644131193126646, + "timestamp": 7.404290474254807 + }, + { + "x": 4.224696877453653, + "y": 6.755580236078661, + "heading": 0.30240542478108134, + "angularVelocity": -0.9692718329596139, + "velocityX": 2.2754304885144183, + "velocityY": 0.2559684862336096, + "timestamp": 7.458299297167131 + }, + { + "x": 4.365281618112227, + "y": 6.770752614262487, + "heading": 0.24487468968297588, + "angularVelocity": -1.0652099415589684, + "velocityX": 2.602995826937264, + "velocityY": 0.28092406695210137, + "timestamp": 7.512308120079456 + }, + { + "x": 4.5236114777315315, + "y": 6.786913235329627, + "heading": 0.18348645255620313, + "angularVelocity": -1.1366334946130574, + "velocityX": 2.9315554585652097, + "velocityY": 0.2992218714593103, + "timestamp": 7.56631694299178 + }, + { + "x": 4.69973843079224, + "y": 6.803490840028783, + "heading": 0.12031512412262027, + "angularVelocity": -1.169648309798064, + "velocityX": 3.2610774233429534, + "velocityY": 0.3069425291135703, + "timestamp": 7.620325765904105 + }, + { + "x": 4.89365495499135, + "y": 6.819447149799665, + "heading": 0.059092961452105006, + "angularVelocity": -1.1335585441271512, + "velocityX": 3.590460108969724, + "velocityY": 0.2954389470176964, + "timestamp": 7.674334588816429 + }, + { + "x": 5.104830717151077, + "y": 6.832438699160781, + "heading": 0.008851181851826708, + "angularVelocity": -0.9302513347094954, + "velocityX": 3.9100234141844274, + "velocityY": 0.24054494544725308, + "timestamp": 7.728343411728754 + }, + { + "x": 5.328140039231948, + "y": 6.839579388098938, + "heading": 4.9993181720020666e-8, + "angularVelocity": -0.16388307282707454, + "velocityX": 4.134682261884916, + "velocityY": 0.1322133783539373, + "timestamp": 7.7823522346410785 + }, + { + "x": 5.5537444848612765, + "y": 6.829254816135378, + "heading": 1.8581535238630143e-8, + "angularVelocity": -5.816021306799029e-7, + "velocityX": 4.177177606621186, + "velocityY": -0.19116454325104273, + "timestamp": 7.836361057553403 + }, + { + "x": 5.7778338432312015, + "y": 6.801184844970702, + "heading": 7.562116538308675e-20, + "angularVelocity": -3.4404629163368187e-7, + "velocityX": 4.149125018586378, + "velocityY": -0.5197293636679055, + "timestamp": 7.890369880465728 + }, + { + "x": 6.023879514181097, + "y": 6.7480895192913355, + "heading": -8.873359130944661e-9, + "angularVelocity": -1.474096821803706e-7, + "velocityX": 4.087461537509469, + "velocityY": -0.8820521031648062, + "timestamp": 7.950565106449386 + }, + { + "x": 6.264312553803963, + "y": 6.673594285856147, + "heading": -1.6744083268388366e-8, + "angularVelocity": -1.3075329494712452e-7, + "velocityX": 3.9942210647757475, + "velocityY": -1.237560491182664, + "timestamp": 8.010760332433046 + }, + { + "x": 6.497275551280226, + "y": 6.578274673466115, + "heading": -2.399265114525153e-8, + "angularVelocity": -1.204176536996037e-7, + "velocityX": 3.8701241447204757, + "velocityY": -1.5835078418994266, + "timestamp": 8.070955558416705 + }, + { + "x": 6.720968856148328, + "y": 6.462867168278596, + "heading": -3.0899254081227276e-8, + "angularVelocity": -1.1473672244136315e-7, + "velocityX": 3.7161303278241413, + "velocityY": -1.9172202330272603, + "timestamp": 8.131150784400363 + }, + { + "x": 6.933377464225438, + "y": 6.349413240298023, + "heading": -0.03079337544261274, + "angularVelocity": -0.5115579190900974, + "velocityX": 3.528662026034608, + "velocityY": -1.88476621071864, + "timestamp": 8.191346010384022 + }, + { + "x": 7.127357335376337, + "y": 6.247833133054634, + "heading": -0.07153202857540313, + "angularVelocity": -0.676775482890449, + "velocityX": 3.2225125494762574, + "velocityY": -1.6875110207404995, + "timestamp": 8.25154123636768 + }, + { + "x": 7.302666826825636, + "y": 6.158120559909072, + "heading": -0.11291500541703978, + "angularVelocity": -0.6874793833795204, + "velocityX": 2.9123487549808393, + "velocityY": -1.490360268269716, + "timestamp": 8.31173646235134 + }, + { + "x": 7.459290055702994, + "y": 6.080235640166216, + "heading": -0.15221505882089248, + "angularVelocity": -0.6528765821814755, + "velocityX": 2.601921104505488, + "velocityY": -1.2938720383573232, + "timestamp": 8.371931688334998 + }, + { + "x": 7.597228420785523, + "y": 6.0141535807269655, + "heading": -0.18812935690621163, + "angularVelocity": -0.5966303390084257, + "velocityX": 2.2915166913730753, + "velocityY": -1.097795686607935, + "timestamp": 8.432126914318657 + }, + { + "x": 7.7164854369458595, + "y": 5.959858078865012, + "heading": -0.21989239705905567, + "angularVelocity": -0.5276670970795383, + "velocityX": 1.981170669459914, + "velocityY": -0.9019901657432539, + "timestamp": 8.492322140302315 + }, + { + "x": 7.817064511998192, + "y": 5.917337690294524, + "heading": -0.24699919876355111, + "angularVelocity": -0.4503148092151716, + "velocityX": 1.6708812602454004, + "velocityY": -0.706374764371373, + "timestamp": 8.552517366285974 + }, + { + "x": 7.898968592472621, + "y": 5.886583965823989, + "heading": -0.26909119151069666, + "angularVelocity": -0.36700572821410926, + "velocityX": 1.360640800595448, + "velocityY": -0.5108997261491135, + "timestamp": 8.612712592269633 + }, + { + "x": 7.9622001639044475, + "y": 5.867590422826931, + "heading": -0.2859005975102692, + "angularVelocity": -0.27924815838611156, + "velocityX": 1.0504416321817922, + "velocityY": -0.31553238129239974, + "timestamp": 8.672907818253291 + }, + { + "x": 8.006761315879022, + "y": 5.860351935159993, + "heading": -0.29722007092856884, + "angularVelocity": -0.18804603244404322, + "velocityX": 0.7402771772411256, + "velocityY": -0.12025019507199955, + "timestamp": 8.73310304423695 + }, + { + "x": 8.03265380859375, + "y": 5.864864349365234, + "heading": -0.302884711661745, + "angularVelocity": -0.09410448487582672, + "velocityX": 0.4301419637789344, + "velocityY": 0.07496299135859619, + "timestamp": 8.793298270220609 + }, + { + "x": 8.038999044372272, + "y": 5.883116303873676, + "heading": -0.30232446533447555, + "angularVelocity": 0.008705413942196032, + "velocityX": 0.09859574498610282, + "velocityY": 0.28360885474165515, + "timestamp": 8.857654351503994 + }, + { + "x": 8.023741230476212, + "y": 5.914373930337575, + "heading": -0.29538057133457873, + "angularVelocity": 0.10789802395394477, + "velocityX": -0.23708425982112472, + "velocityY": 0.48569810094960236, + "timestamp": 8.92201043278738 + }, + { + "x": 7.986572081097167, + "y": 5.958123497607152, + "heading": -0.2823371032309085, + "angularVelocity": 0.20267654343704639, + "velocityX": -0.5775545781815625, + "velocityY": 0.6798047114915214, + "timestamp": 8.986366514070765 + }, + { + "x": 7.927131820563721, + "y": 6.013727378429472, + "heading": -0.26354639848302325, + "angularVelocity": 0.2919802507107623, + "velocityX": -0.9236152877566656, + "velocityY": 0.8640035209333707, + "timestamp": 9.05072259535415 + }, + { + "x": 7.8449983231200475, + "y": 6.080375877201426, + "heading": -0.23945459436001962, + "angularVelocity": 0.3743516330168969, + "velocityX": -1.2762352182695424, + "velocityY": 1.0356208371120963, + "timestamp": 9.115078676637536 + }, + { + "x": 7.73967594274609, + "y": 6.157012398666797, + "heading": -0.21064125546050952, + "angularVelocity": 0.4477174235117473, + "velocityX": -1.6365567678084803, + "velocityY": 1.1908201981396094, + "timestamp": 9.179434757920921 + }, + { + "x": 7.610589170044395, + "y": 6.24221277947117, + "heading": -0.1778829463049905, + "angularVelocity": 0.5090165296309906, + "velocityX": -2.005820897224518, + "velocityY": 1.3238901298107526, + "timestamp": 9.243790839204307 + }, + { + "x": 7.457098123742525, + "y": 6.333983891334329, + "heading": -0.1422582406491948, + "angularVelocity": 0.5535561666492045, + "velocityX": -2.3850278519288124, + "velocityY": 1.4259897438293916, + "timestamp": 9.308146920487692 + }, + { + "x": 7.278589562677723, + "y": 6.429422311009697, + "heading": -0.10532315073359272, + "angularVelocity": 0.5739176341853679, + "velocityX": -2.7737636833224153, + "velocityY": 1.4829743789885954, + "timestamp": 9.372503001771078 + }, + { + "x": 7.0748043390181765, + "y": 6.524168352293141, + "heading": -0.06939014472798437, + "angularVelocity": 0.5583467061547885, + "velocityX": -3.166526295505748, + "velocityY": 1.4722158247367187, + "timestamp": 9.436859083054463 + }, + { + "x": 6.846786882119966, + "y": 6.6118039531436965, + "heading": -0.037838987740982835, + "angularVelocity": 0.4902591388072404, + "velocityX": -3.543059992950117, + "velocityY": 1.3617299111899137, + "timestamp": 9.501215164337848 + }, + { + "x": 6.5986490653165255, + "y": 6.684332838455106, + "heading": -0.014752410202795902, + "angularVelocity": 0.35873187238556964, + "velocityX": -3.855701152946053, + "velocityY": 1.12699350030396, + "timestamp": 9.565571245621234 + }, + { + "x": 6.337948603586263, + "y": 6.7352009376864705, + "heading": -0.00346493782708955, + "angularVelocity": 0.1753909211159554, + "velocityX": -4.050906402804312, + "velocityY": 0.7904163556412387, + "timestamp": 9.62992732690462 + }, + { + "x": 6.071046358370447, + "y": 6.761675318964879, + "heading": -0.0003587026360581851, + "angularVelocity": 0.04826638181018736, + "velocityX": -4.14727310757996, + "velocityY": 0.41137342035837426, + "timestamp": 9.694283408188005 + }, + { + "x": 5.802036285400391, + "y": 6.762963199615477, + "heading": 3.48426610312228e-20, + "angularVelocity": 0.0055737178042065665, + "velocityX": -4.180025688411564, + "velocityY": 0.020011794144629762, + "timestamp": 9.75863948947139 + }, + { + "x": 5.504490144089293, + "y": 6.7332005959035195, + "heading": 1.0813799079954769e-8, + "angularVelocity": 1.512165560586682e-7, + "velocityX": -4.160785901890718, + "velocityY": -0.4161903138202568, + "timestamp": 9.83015149425296 + }, + { + "x": 5.212483549504086, + "y": 6.672709068505996, + "heading": 1.7927215293047143e-8, + "angularVelocity": 9.947163745193156e-8, + "velocityX": -4.08332272989868, + "velocityY": -0.8458933235376672, + "timestamp": 9.901663499034528 + }, + { + "x": 4.936047112225373, + "y": 6.585152370785529, + "heading": 2.0960766953807052e-8, + "angularVelocity": 4.2420173648314827e-8, + "velocityX": -3.86559484834855, + "velocityY": -1.2243636294060563, + "timestamp": 9.973175503816098 + }, + { + "x": 4.685471386719961, + "y": 6.480097764592836, + "heading": 2.0521148140147226e-8, + "angularVelocity": -6.147482719554469e-9, + "velocityX": -3.5039672887200504, + "velocityY": -1.469048539662375, + "timestamp": 10.044687508597667 + }, + { + "x": 4.465637952492511, + "y": 6.36957828843063, + "heading": 1.806553462983808e-8, + "angularVelocity": -3.433847950126585e-8, + "velocityX": -3.0740773510534587, + "velocityY": -1.545467456824671, + "timestamp": 10.116199513379236 + }, + { + "x": 4.276834572623958, + "y": 6.2625357492286335, + "heading": 1.4753077547904654e-8, + "angularVelocity": -4.632029394280808e-8, + "velocityX": -2.640163430535123, + "velocityY": -1.4968471311768274, + "timestamp": 10.187711518160805 + }, + { + "x": 4.117926572195921, + "y": 6.164505408559498, + "heading": 1.1239712389074229e-8, + "angularVelocity": -4.912972541552041e-8, + "velocityX": -2.2221164252549563, + "velocityY": -1.3708235556892094, + "timestamp": 10.259223522942374 + }, + { + "x": 3.9876518132215875, + "y": 6.078928620338262, + "heading": 7.894590787280817e-9, + "angularVelocity": -4.6777063683539706e-8, + "velocityX": -1.8217187362073228, + "velocityY": -1.1966772359777518, + "timestamp": 10.330735527723943 + }, + { + "x": 3.8849194916806877, + "y": 6.008061273344668, + "heading": 4.950126193053713e-9, + "angularVelocity": -4.1174409852415885e-8, + "velocityX": -1.4365744864053387, + "velocityY": -0.9909853207171924, + "timestamp": 10.402247532505513 + }, + { + "x": 3.808836355920425, + "y": 5.953465817607232, + "heading": 2.5714210248063034e-9, + "angularVelocity": -3.3263018922920817e-8, + "velocityX": -1.0639211694967192, + "velocityY": -0.763444625894576, + "timestamp": 10.473759537287082 + }, + { + "x": 3.758675804955618, + "y": 5.916276486245191, + "heading": 8.864982180144823e-10, + "angularVelocity": -2.3561398005174934e-8, + "velocityX": -0.7014283981832174, + "velocityY": -0.5200431938055047, + "timestamp": 10.545271542068651 + }, + { + "x": 3.733842372894287, + "y": 5.897348880767822, + "heading": 2.776883269501392e-22, + "angularVelocity": -1.2396495116174843e-8, + "velocityX": -0.34726242310201455, + "velocityY": -0.26467731586021115, + "timestamp": 10.61678354685022 + }, + { + "x": 3.733842372894287, + "y": 5.897348880767822, + "heading": 1.3627894416387573e-22, + "angularVelocity": -7.174234051514954e-23, + "velocityX": 3.367103597351517e-20, + "velocityY": -6.161027541818125e-20, + "timestamp": 10.68829555163179 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "timestamp": 1.087700259463512, + "isStopPoint": true, + "x": 2.629361867904663, + "y": 4.316425800323486, + "heading": -0.4973420499689195, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "timestamp": 1.7308796906716322, + "isStopPoint": false, + "x": 2.510251045227051, + "y": 5.334280490875244, + "heading": 0.5713370823927012, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 2.2930859047476964, + "isStopPoint": true, + "x": 3.0191783905029297, + "y": 5.832379341125488, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "timestamp": 2.8924182484996503, + "isStopPoint": false, + "x": 2.445281744003296, + "y": 6.449589252471924, + "heading": 0.5224037961561394, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 + }, + { + "timestamp": 3.3920386132295137, + "isStopPoint": true, + "x": 2.7051594257354736, + "y": 6.882719039916992, + "heading": 0.5224037961561394, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 4.673199629925819, + "isStopPoint": false, + "x": 6.776577949523926, + "y": 6.9043755531311035, + "heading": 0.5224037961561394, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 5.315938075232567, + "isStopPoint": false, + "x": 8.021825790405273, + "y": 7.337504863739014, + "heading": 0.5224037961561394, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "timestamp": 7.080237536780859, + "isStopPoint": true, + "x": 3.733842372894287, + "y": 6.697348880767819, + "heading": 0.5224037961561394, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "timestamp": 7.890369880465728, + "isStopPoint": false, + "x": 5.7778338432312015, + "y": 6.801184844970702, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "timestamp": 8.793298270220609, + "isStopPoint": false, + "x": 8.03265380859375, + "y": 5.864864349365234, + "heading": -0.302884711661745, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "timestamp": 9.75863948947139, + "isStopPoint": false, + "x": 5.802036285400391, + "y": 6.762963199615477, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 10.68829555163179, + "isStopPoint": true, + "x": 3.733842372894287, + "y": 5.897348880767822, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 5 + ], + "type": "StopPoint" + }, + { + "scope": [ + 8 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "Distance Center 6 Wing First Subwoofer": { + "waypoints": [ + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 2.629361867904663, + "y": 4.2189717292785645, + "heading": -0.44684263169879895, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 2.6500000000000012, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 2.629361867904663, + "y": 6.918971729278555, + "heading": 0.44684263169879895, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 8.08679485321045, + "y": 7.44578742980957, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 4.056788444519045, + "y": 6.276337623596191, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 6.321792125701904, + "y": 6.503730297088623, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 7.90271520614624, + "y": 5.908177375793457, + "heading": -0.34555536227067424, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 6.321792125701904, + "y": 6.503730297088623, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 4.056788444519045, + "y": 6.276337623596191, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.35, + "y": 5.58225679397583, + "heading": -1.3105569434022965e-22, + "angularVelocity": -4.761453764625247e-22, + "velocityX": 1.1722115722360089e-20, + "velocityY": -1.2492293356601218e-20, + "timestamp": 0 + }, + { + "x": 1.376106022093243, + "y": 5.554437182407712, + "heading": -0.009091989168136132, + "angularVelocity": -0.11487456261093582, + "velocityX": 0.32984177763681827, + "velocityY": -0.3514924679072023, + "timestamp": 0.07914710586476341 + }, + { + "x": 1.428318135425877, + "y": 5.498798018189392, + "heading": -0.027275691769830347, + "angularVelocity": -0.22974564139798406, + "velocityX": 0.6596844289145241, + "velocityY": -0.7029841914041539, + "timestamp": 0.15829421172952682 + }, + { + "x": 1.5066364383487854, + "y": 5.415339395500696, + "heading": -0.05455120903188366, + "angularVelocity": -0.34461799915537367, + "velocityX": 0.9895283228262182, + "velocityY": -1.05447472496721, + "timestamp": 0.2374413175942902 + }, + { + "x": 1.6110610489670512, + "y": 5.304061450304077, + "heading": -0.09091971001300213, + "angularVelocity": -0.4595051276197058, + "velocityX": 1.3193737089602908, + "velocityY": -1.4059635406853233, + "timestamp": 0.31658842345905364 + }, + { + "x": 1.7415920897935755, + "y": 5.164964366004136, + "heading": -0.1363843221081841, + "angularVelocity": -0.5744317697840544, + "velocityX": 1.6492206430082152, + "velocityY": -1.7574500391411858, + "timestamp": 0.39573552932381706 + }, + { + "x": 1.8982296650833075, + "y": 4.998048375309143, + "heading": -0.19095112736368186, + "angularVelocity": -0.6894352567829158, + "velocityX": 1.9790688942862253, + "velocityY": -2.108933597397716, + "timestamp": 0.4748826351885805 + }, + { + "x": 2.080973829240046, + "y": 4.803313756359354, + "heading": -0.25463009738549497, + "angularVelocity": -0.8045647320398506, + "velocityX": 2.308917832939953, + "velocityY": -2.4604136414353124, + "timestamp": 0.5540297410533439 + }, + { + "x": 2.237634329672691, + "y": 4.636382002307393, + "heading": -0.3095162598785737, + "angularVelocity": -0.6934702399208541, + "velocityX": 1.979358546607204, + "velocityY": -2.109132762696247, + "timestamp": 0.6331768469181073 + }, + { + "x": 2.368188294580038, + "y": 4.497268491668379, + "heading": -0.35527028560397067, + "angularVelocity": -0.5780884244027288, + "velocityX": 1.6495102819100074, + "velocityY": -1.7576575810202477, + "timestamp": 0.7123239527828706 + }, + { + "x": 2.47263578406431, + "y": 4.385973104721263, + "heading": -0.39188377719785317, + "angularVelocity": -0.46260051070525543, + "velocityX": 1.3196627765864057, + "velocityY": -1.4061839119838944, + "timestamp": 0.791471058647634 + }, + { + "x": 2.5509768566083473, + "y": 4.302495763635934, + "heading": -0.41935051793107386, + "angularVelocity": -0.34703405049519326, + "velocityX": 0.9898160101760007, + "velocityY": -1.0547112263076903, + "timestamp": 0.8706181645123974 + }, + { + "x": 2.603211554320217, + "y": 4.246836425701493, + "heading": -0.437666590123682, + "angularVelocity": -0.2314180915712106, + "velocityX": 0.6599697757884107, + "velocityY": -0.7032390802709192, + "timestamp": 0.9497652703771607 + }, + { + "x": 2.6293398948866087, + "y": 4.218995077070589, + "heading": -0.44683039365411925, + "angularVelocity": -0.11578191559013186, + "velocityX": 0.33012376486686545, + "velocityY": -0.3517671091912785, + "timestamp": 1.028912376241924 + }, + { + "x": 2.629361867904663, + "y": 4.2189717292785645, + "heading": -0.44684263169879895, + "angularVelocity": -0.00015462403262836384, + "velocityX": 0.00027762250829401467, + "velocityY": -0.0002949923660397582, + "timestamp": 1.1080594821066876 + }, + { + "x": 2.6032711516040385, + "y": 4.24677311445227, + "heading": -0.4377041053238575, + "angularVelocity": 0.11544864826739264, + "velocityX": -0.32960871432123445, + "velocityY": 0.35121990204749487, + "timestamp": 1.1872161159182462 + }, + { + "x": 2.5510677157973753, + "y": 4.302399261550015, + "heading": -0.4194176623258515, + "angularVelocity": 0.23101592522919745, + "velocityX": -0.6594953990961658, + "velocityY": 0.7027351267888616, + "timestamp": 1.2663727497298047 + }, + { + "x": 2.472751515756506, + "y": 4.38585023239311, + "heading": -0.3919884490131159, + "angularVelocity": 0.34651818795167477, + "velocityX": -0.9893826489300932, + "velocityY": 1.0542511325299573, + "timestamp": 1.3455293835413633 + }, + { + "x": 2.36832250186272, + "y": 4.497126133012537, + "heading": -0.35542377520119517, + "angularVelocity": 0.46192810445890287, + "velocityX": -1.3192705256061201, + "velocityY": 1.4057684777795487, + "timestamp": 1.4246860173529219 + }, + { + "x": 2.2377806374279694, + "y": 4.636227121930257, + "heading": -0.30973274156337427, + "angularVelocity": 0.5772230505227592, + "velocityX": -1.6491588657688505, + "velocityY": 1.7572878256655662, + "timestamp": 1.5038426511644805 + }, + { + "x": 2.081125922237724, + "y": 4.803153414756678, + "heading": -0.2549254844011435, + "angularVelocity": 0.6923899428657577, + "velocityX": -1.9790472086417852, + "velocityY": 2.1088098973967053, + "timestamp": 1.582999284976039 + }, + { + "x": 1.8983584218600063, + "y": 4.997905279612994, + "heading": -0.19101192332640735, + "angularVelocity": 0.8074315189664284, + "velocityX": -2.3089347231821, + "velocityY": 2.4603353563511114, + "timestamp": 1.6621559187875976 + }, + { + "x": 1.7416842694507195, + "y": 5.164862529299444, + "heading": -0.13643643316957763, + "angularVelocity": 0.6894619885776455, + "velocityX": -1.9792927625278673, + "velocityY": 2.1092009809804435, + "timestamp": 1.7413125525991562 + }, + { + "x": 1.6111226785905355, + "y": 5.303993796816868, + "heading": -0.09095798078133475, + "angularVelocity": 0.574537473340636, + "velocityX": -1.649408073251331, + "velocityY": 1.757670340664581, + "timestamp": 1.8204691864107148 + }, + { + "x": 1.506673537960871, + "y": 5.415298936436349, + "heading": -0.054575132238388, + "angularVelocity": 0.4596310731146078, + "velocityX": -1.319524790282489, + "velocityY": 1.4061378593290832, + "timestamp": 1.8996258202222733 + }, + { + "x": 1.428336751560515, + "y": 5.498777850544818, + "heading": -0.027287678660803165, + "angularVelocity": 0.34472731170637844, + "velocityX": -0.9896427201142192, + "velocityY": 1.054604144830112, + "timestamp": 1.978782454033832 + }, + { + "x": 1.3761122507008792, + "y": 5.55443047925558, + "heading": -0.009095885873172527, + "angularVelocity": 0.2298201920882369, + "velocityX": -0.6597615177012465, + "velocityY": 0.7030696737717484, + "timestamp": 2.0579390878453907 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 3.968181320363232e-22, + "angularVelocity": 0.11490996313494475, + "velocityX": -0.32988076227498664, + "velocityY": 0.3515348414953293, + "timestamp": 2.1370957216569493 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 1.7752085844076005e-22, + "angularVelocity": -5.239816034386896e-22, + "velocityX": 2.9480236062895993e-20, + "velocityY": -1.2346716393304613e-20, + "timestamp": 2.216252355468508 + }, + { + "x": 1.4019931563006072, + "y": 5.58225679397583, + "heading": 6.677169422600811e-24, + "angularVelocity": -1.8513645761030444e-21, + "velocityX": 0.563430207687053, + "velocityY": 1.0793744639684502e-16, + "timestamp": 2.3085320302023935 + }, + { + "x": 1.5059794683382701, + "y": 5.58225679397583, + "heading": -1.5919770654572408e-22, + "angularVelocity": -1.7975194292009434e-21, + "velocityX": 1.1268604092671195, + "velocityY": 2.1588928379491377e-16, + "timestamp": 2.400811704936279 + }, + { + "x": 1.661958935173697, + "y": 5.58225679397583, + "heading": -1.0739397610061377e-22, + "angularVelocity": 5.613814545355455e-22, + "velocityX": 1.6902906006684286, + "velocityY": 3.2384452847940683e-16, + "timestamp": 2.493091379670165 + }, + { + "x": 1.8699315549281479, + "y": 5.58225679397583, + "heading": 5.877231323874057e-22, + "angularVelocity": 7.532725689717569e-21, + "velocityX": 2.253720771710548, + "velocityY": 4.3180658827193484e-16, + "timestamp": 2.5853710544040505 + }, + { + "x": 2.1298973219641684, + "y": 5.58225679397583, + "heading": 3.2033613274405696e-21, + "angularVelocity": 2.834468774110536e-20, + "velocityX": 2.817150881661696, + "velocityY": 5.397890976977118e-16, + "timestamp": 2.677650729137936 + }, + { + "x": 2.3379041602219917, + "y": 5.58225679397583, + "heading": 1.310690511155894e-21, + "angularVelocity": -2.0510155439556265e-20, + "velocityX": 2.2540915847142853, + "velocityY": 5.581506402100161e-16, + "timestamp": 2.7699304038718218 + }, + { + "x": 2.4939178511908273, + "y": 5.58225679397583, + "heading": 3.997113631994639e-23, + "angularVelocity": -1.377030243024098e-20, + "velocityX": 1.6906614746827489, + "velocityY": 4.50249833557945e-16, + "timestamp": 2.8622100786057074 + }, + { + "x": 2.597938389238784, + "y": 5.58225679397583, + "heading": -6.030574852797699e-22, + "angularVelocity": -6.968254669755187e-21, + "velocityX": 1.127231303620532, + "velocityY": 3.4232862627505307e-16, + "timestamp": 2.954489753339593 + }, + { + "x": 2.6499657724881525, + "y": 5.58225679397583, + "heading": -6.166353516647707e-22, + "angularVelocity": -1.4713443371706887e-22, + "velocityX": 0.5638011122102904, + "velocityY": 2.344006170727818e-16, + "timestamp": 3.0467694280734787 + }, + { + "x": 2.6500000000000012, + "y": 5.58225679397583, + "heading": 7.150164834326212e-28, + "angularVelocity": 6.6822560720639164e-21, + "velocityX": 0.00037091062519919635, + "velocityY": 1.2646920658154274e-16, + "timestamp": 3.1390491028073644 + }, + { + "x": 2.5980273838116625, + "y": 5.58225679397583, + "heading": 1.2475447557817004e-21, + "angularVelocity": 1.3517380299097123e-20, + "velocityX": -0.5631334791901095, + "velocityY": 1.852154451951177e-17, + "timestamp": 3.2313409272307405 + }, + { + "x": 2.494047919980239, + "y": 5.58225679397583, + "heading": 3.2324491441975756e-21, + "angularVelocity": 2.1506824119623174e-20, + "velocityX": -1.1266378628992357, + "velocityY": -8.942818834696392e-17, + "timestamp": 3.3236327516541166 + }, + { + "x": 2.3380616094450217, + "y": 5.58225679397583, + "heading": 6.132702376489969e-21, + "angularVelocity": 3.142480817163132e-20, + "velocityX": -1.6901422364309449, + "velocityY": -1.9738137273676251e-16, + "timestamp": 3.415924576077493 + }, + { + "x": 2.1300684540847508, + "y": 5.58225679397583, + "heading": 1.0303595448942247e-20, + "angularVelocity": 4.519243978206457e-20, + "velocityX": -2.253646589606145, + "velocityY": -3.05341460883141e-16, + "timestamp": 3.508216400500869 + }, + { + "x": 1.8700684595368808, + "y": 5.58225679397583, + "heading": 1.681410681468819e-20, + "angularVelocity": 7.054266238709e-20, + "velocityX": -2.8171508816984203, + "velocityY": -4.133222662368775e-16, + "timestamp": 3.600508224924245 + }, + { + "x": 1.6620410811667898, + "y": 5.58225679397583, + "heading": 7.817959688103422e-21, + "angularVelocity": -9.747502065767468e-20, + "velocityX": -2.2540174026227233, + "velocityY": -4.316713876129951e-16, + "timestamp": 3.692800049347621 + }, + { + "x": 1.5060205419917154, + "y": 5.58225679397583, + "heading": 3.3227072373995285e-21, + "angularVelocity": -4.870694497734212e-20, + "velocityX": -1.6905131104500808, + "velocityY": -3.237733503311185e-16, + "timestamp": 3.7850918737709973 + }, + { + "x": 1.4020068476435492, + "y": 5.58225679397583, + "heading": 9.77517677073401e-22, + "angularVelocity": -2.5410592286057495e-20, + "velocityX": -1.1270087572547884, + "velocityY": -2.158546447821665e-16, + "timestamp": 3.8773836981943735 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 4.719104857778951e-25, + "angularVelocity": -1.0586486204388865e-20, + "velocityX": -0.5635043837141495, + "velocityY": -1.0792904871510697e-16, + "timestamp": 3.9696755226177496 + }, + { + "x": 1.35, + "y": 5.58225679397583, + "heading": 3.507762761410347e-25, + "angularVelocity": -1.3163866439134166e-24, + "velocityX": 1.7883460029357865e-20, + "velocityY": -7.181910053514036e-24, + "timestamp": 4.061967347041126 + }, + { + "x": 1.3761091905494292, + "y": 5.609537510382012, + "heading": 0.009092277255100973, + "angularVelocity": 0.11546531077977794, + "velocityX": 0.331567737808145, + "velocityY": 0.346445264454243, + "timestamp": 4.140712012375655 + }, + { + "x": 1.4283276412673929, + "y": 5.664098878978288, + "heading": 0.02727672011448041, + "angularVelocity": 0.23092920367528352, + "velocityX": 0.6631363597283135, + "velocityY": 0.6928897134108606, + "timestamp": 4.219456677710184 + }, + { + "x": 1.5066554501194507, + "y": 5.745940797463734, + "heading": 0.0545537157896761, + "angularVelocity": 0.3463980138758046, + "velocityX": 0.9947062257398623, + "velocityY": 1.0393328632201235, + "timestamp": 4.298201343044713 + }, + { + "x": 1.6110927331941791, + "y": 5.85506311882236, + "heading": 0.09092486765955961, + "angularVelocity": 0.4618871858222897, + "velocityX": 1.3262775659919341, + "velocityY": 1.3857741460331916, + "timestamp": 4.376946008379242 + }, + { + "x": 1.7416396083512349, + "y": 5.991465645233681, + "heading": 0.13639392962694408, + "angularVelocity": 0.5774240296053013, + "velocityX": 1.657850402975963, + "velocityY": 1.7322129166699285, + "timestamp": 4.455690673713772 + }, + { + "x": 1.8982961713119855, + "y": 6.155148124011134, + "heading": 0.19096791231543717, + "angularVelocity": 0.6930499032111946, + "velocityX": 1.9894244555517604, + "velocityY": 2.0786484783710053, + "timestamp": 4.534435339048301 + }, + { + "x": 2.081062461891351, + "y": 6.346110236484044, + "heading": 0.25465858240789885, + "angularVelocity": 0.8088252051346765, + "velocityX": 2.3209990137481022, + "velocityY": 2.425079993186225, + "timestamp": 4.61318000438283 + }, + { + "x": 2.2377196372080883, + "y": 6.509785226593951, + "heading": 0.30954671204597756, + "angularVelocity": 0.6970393410766176, + "velocityX": 1.9894322320275923, + "velocityY": 2.078553377737655, + "timestamp": 4.691924669717359 + }, + { + "x": 2.3682671510821507, + "y": 6.646180841197734, + "heading": 0.3553020329707184, + "angularVelocity": 0.5810593102448243, + "velocityX": 1.657858514217577, + "velocityY": 1.7321251417392771, + "timestamp": 4.770669335051888 + }, + { + "x": 2.472705075371397, + "y": 6.755297231195074, + "heading": 0.39191475956413135, + "angularVelocity": 0.46495500917899774, + "velocityX": 1.3262857089501277, + "velocityY": 1.3856988220571407, + "timestamp": 4.8494140003864175 + }, + { + "x": 2.5510334762030915, + "y": 6.837134483375156, + "heading": 0.4193781488302824, + "angularVelocity": 0.3487650769671707, + "velocityX": 0.9947137434508653, + "velocityY": 1.039273604534536, + "timestamp": 4.928158665720947 + }, + { + "x": 2.603252400434301, + "y": 6.891692644090699, + "heading": 0.437688018711337, + "angularVelocity": 0.23252203566132756, + "velocityX": 0.6631423730022795, + "velocityY": 0.6928489756577794, + "timestamp": 5.006903331055476 + }, + { + "x": 2.629361867904663, + "y": 6.918971729278555, + "heading": 0.44684263169879895, + "angularVelocity": 0.11625692926080337, + "velocityX": 0.3315712545026574, + "velocityY": 0.3464245491674547, + "timestamp": 5.085647996390005 + }, + { + "x": 2.629361867904663, + "y": 6.918971729278555, + "heading": 0.44684263169879895, + "angularVelocity": 1.7104653429303553e-24, + "velocityX": 8.676311589571829e-24, + "velocityY": 9.986643357418398e-25, + "timestamp": 5.164392661724534 + }, + { + "x": 2.647240076574808, + "y": 6.9229927793221195, + "heading": 0.43944656124636106, + "angularVelocity": -0.13452107233246685, + "velocityX": 0.32517210553324816, + "velocityY": 0.073135588315622, + "timestamp": 5.219373419193956 + }, + { + "x": 2.6830022691087487, + "y": 6.931034450131843, + "heading": 0.4248111822038071, + "angularVelocity": -0.26619093144894534, + "velocityX": 0.6504492513372576, + "velocityY": 0.14626336885583588, + "timestamp": 5.274354176663378 + }, + { + "x": 2.736655222701382, + "y": 6.94309618304354, + "heading": 0.40312445923850565, + "angularVelocity": -0.3944420550655893, + "velocityX": 0.9758496619926198, + "velocityY": 0.21938098831042505, + "timestamp": 5.3293349341328 + }, + { + "x": 2.8082070403706108, + "y": 6.959177291873429, + "heading": 0.3746181200720642, + "angularVelocity": -0.518478472805611, + "velocityX": 1.3013974518088953, + "velocityY": 0.29248612732978096, + "timestamp": 5.384315691602222 + }, + { + "x": 2.897667587057907, + "y": 6.979276960170721, + "heading": 0.3395861567651547, + "angularVelocity": -0.6371677095644406, + "velocityX": 1.6271246669719133, + "velocityY": 0.365576416593952, + "timestamp": 5.439296449071644 + }, + { + "x": 3.005049084237076, + "y": 7.003394214511703, + "heading": 0.29841332172862295, + "angularVelocity": -0.748858999613284, + "velocityX": 1.9530741685196018, + "velocityY": 0.4386490010508486, + "timestamp": 5.494277206541065 + }, + { + "x": 3.1303669283648046, + "y": 7.03152781229272, + "heading": 0.251621978066968, + "angularVelocity": -0.8510494546692724, + "velocityX": 2.279303703617138, + "velocityY": 0.5116989848068959, + "timestamp": 5.549257964010487 + }, + { + "x": 3.2736407888898387, + "y": 7.063675864480488, + "heading": 0.1999566397529945, + "angularVelocity": -0.9396985544025636, + "velocityX": 2.605890990219942, + "velocityY": 0.5847146104825557, + "timestamp": 5.604238721479909 + }, + { + "x": 3.4348957619080127, + "y": 7.099834631071478, + "heading": 0.14455793072354875, + "angularVelocity": -1.0076017788633853, + "velocityX": 2.932934729170615, + "velocityY": 0.6576622122948987, + "timestamp": 5.659219478949331 + }, + { + "x": 3.6141609000051536, + "y": 7.139994322888153, + "heading": 0.08739671753641093, + "angularVelocity": -1.0396585245106666, + "velocityX": 3.260506881827549, + "velocityY": 0.730431766768768, + "timestamp": 5.714200236418753 + }, + { + "x": 3.8114358161798583, + "y": 7.184118841743803, + "heading": 0.03277975478683384, + "angularVelocity": -0.9933832355793388, + "velocityX": 3.5880719956326845, + "velocityY": 0.8025447608682011, + "timestamp": 5.769180993888175 + }, + { + "x": 4.025725584987085, + "y": 7.231688135484132, + "heading": 1.1487831117686327e-8, + "angularVelocity": -0.5962039231131655, + "velocityX": 3.8975412247895487, + "velocityY": 0.8651989519566878, + "timestamp": 5.824161751357597 + }, + { + "x": 4.250000000000002, + "y": 7.282256793975824, + "heading": -1.3446001300471768e-25, + "angularVelocity": -2.0894275827457326e-7, + "velocityX": 4.079143782943505, + "velocityY": 0.9197519426649702, + "timestamp": 5.879142508827019 + }, + { + "x": 4.555578615051821, + "y": 7.331690581992348, + "heading": 1.1852293345406531e-17, + "angularVelocity": 1.6010580396806402e-16, + "velocityX": 4.127885452839213, + "velocityY": 0.6677725612361392, + "timestamp": 5.953170390303632 + }, + { + "x": 4.861157269791372, + "y": 7.381124124675592, + "heading": 2.2033445333656944e-17, + "angularVelocity": 1.3753131637930226e-16, + "velocityX": 4.127885988957933, + "velocityY": 0.667769247170231, + "timestamp": 6.027198271780246 + }, + { + "x": 5.166735924531274, + "y": 7.430557667356667, + "heading": -3.355740357093402e-17, + "angularVelocity": -7.5094474940707e-16, + "velocityX": 4.127885988962678, + "velocityY": 0.6677692471409031, + "timestamp": 6.101226153256859 + }, + { + "x": 5.472314579271176, + "y": 7.47999121003774, + "heading": -3.9827748951312203e-17, + "angularVelocity": -8.470248310915055e-17, + "velocityX": 4.12788598896268, + "velocityY": 0.6677692471408927, + "timestamp": 6.175254034733473 + }, + { + "x": 5.77789323401108, + "y": 7.529424752718813, + "heading": -2.9096619403201524e-17, + "angularVelocity": 1.4496064636808375e-16, + "velocityX": 4.12788598896268, + "velocityY": 0.6677692471408905, + "timestamp": 6.249281916210086 + }, + { + "x": 6.083471888751526, + "y": 7.578858295396524, + "heading": -1.7014104780220546e-17, + "angularVelocity": 1.632157287494049e-16, + "velocityX": 4.127885988970029, + "velocityY": 0.6677692470954667, + "timestamp": 6.3233097976867 + }, + { + "x": 6.389050604827326, + "y": 7.628291458921789, + "heading": -2.8399965998049225e-17, + "angularVelocity": -1.5380503927329875e-16, + "velocityX": 4.127886817513971, + "velocityY": 0.6677641253435272, + "timestamp": 6.397337679163313 + }, + { + "x": 6.6974520045085875, + "y": 7.654947886751572, + "heading": 7.456276940362482e-19, + "angularVelocity": 3.937110330692362e-16, + "velocityX": 4.166016824062293, + "velocityY": 0.36008632555834863, + "timestamp": 6.471365560639927 + }, + { + "x": 6.983237000162054, + "y": 7.656945566167564, + "heading": 6.6435514939088325e-18, + "angularVelocity": 7.96716545476509e-17, + "velocityX": 3.860504852401468, + "velocityY": 0.02698550027564136, + "timestamp": 6.5453934421165405 + }, + { + "x": 7.236389344029046, + "y": 7.6515490284862535, + "heading": 7.190524978488895e-18, + "angularVelocity": 7.388749666939387e-18, + "velocityX": 3.4196891605896758, + "velocityY": -0.07289871834323541, + "timestamp": 6.619421323593154 + }, + { + "x": 7.456702532389806, + "y": 7.639737294256418, + "heading": 8.941088425266997e-18, + "angularVelocity": 2.3647353022640597e-17, + "velocityX": 2.976083929003971, + "velocityY": -0.15955791242746284, + "timestamp": 6.693449205069768 + }, + { + "x": 7.6441107685462475, + "y": 7.621856856041679, + "heading": 1.0945616358890774e-17, + "angularVelocity": 2.7078012954548576e-17, + "velocityX": 2.53158988773232, + "velocityY": -0.24153653810000633, + "timestamp": 6.767477086546381 + }, + { + "x": 7.79858185865481, + "y": 7.598084937291478, + "heading": 9.68583076902552e-18, + "angularVelocity": -1.7017717713022894e-17, + "velocityX": 2.0866609583763616, + "velocityY": -0.3211211543006168, + "timestamp": 6.841504968022995 + }, + { + "x": 7.920096730975915, + "y": 7.568529170908052, + "heading": 8.319598760311882e-18, + "angularVelocity": -1.8455641056660778e-17, + "velocityX": 1.6414743998785648, + "velocityY": -0.3992518196372258, + "timestamp": 6.915532849499608 + }, + { + "x": 8.0086427775389, + "y": 7.533261859454375, + "heading": 6.577363737030138e-18, + "angularVelocity": -2.3534849148015073e-17, + "velocityX": 1.1961175275690947, + "velocityY": -0.476405791307416, + "timestamp": 6.989560730976222 + }, + { + "x": 8.064211046315053, + "y": 7.49233491761698, + "heading": 4.5104421550156944e-18, + "angularVelocity": -2.792085280315562e-17, + "velocityX": 0.7506397274614434, + "velocityY": -0.5528584773876515, + "timestamp": 7.063588612452835 + }, + { + "x": 8.08679485321045, + "y": 7.44578742980957, + "heading": -4.5362375322406465e-30, + "angularVelocity": -6.092896439943622e-17, + "velocityX": 0.30507163577996627, + "velocityY": -0.6287831946415446, + "timestamp": 7.137616493929449 + }, + { + "x": 8.056348015754137, + "y": 7.3720119192300455, + "heading": -3.562838906898219e-18, + "angularVelocity": -3.5333147474799246e-17, + "velocityX": -0.30194533800340373, + "velocityY": -0.7316415542426236, + "timestamp": 7.2384520877413605 + }, + { + "x": 7.964689737823336, + "y": 7.2878795305099695, + "heading": -7.250663421617498e-18, + "angularVelocity": -3.6572646376797225e-17, + "velocityX": -0.9089873373659229, + "velocityY": -0.8343520927442343, + "timestamp": 7.339287681553272 + }, + { + "x": 7.811816492885734, + "y": 7.1934111312196, + "heading": -9.984265294683441e-18, + "angularVelocity": -2.710949348016387e-17, + "velocityX": -1.516064309818574, + "velocityY": -0.9368556847751761, + "timestamp": 7.440123275365184 + }, + { + "x": 7.597723004977704, + "y": 7.088638022017575, + "heading": -1.433836387576785e-17, + "angularVelocity": -4.318017494077071e-17, + "velocityX": -2.1231936047045, + "velocityY": -1.039048864009848, + "timestamp": 7.5409588691770955 + }, + { + "x": 7.322400517715837, + "y": 6.973612368749597, + "heading": -2.1940207343875766e-17, + "angularVelocity": -7.538849309728229e-17, + "velocityX": -2.730409737809707, + "velocityY": -1.140724707612031, + "timestamp": 7.641794462989007 + }, + { + "x": 6.985831656638794, + "y": 6.848438495527462, + "heading": -2.9554201439643655e-17, + "angularVelocity": -7.550899248834429e-17, + "velocityX": -3.337798175759688, + "velocityY": -1.2413659551172047, + "timestamp": 7.742630056800919 + }, + { + "x": 6.587965404213338, + "y": 6.713429306112849, + "heading": -2.3579747883783766e-17, + "angularVelocity": 5.924945081446257e-17, + "velocityX": -3.945692561374678, + "velocityY": -1.3389040943860189, + "timestamp": 7.84346565061283 + }, + { + "x": 6.1730951719461284, + "y": 6.638125788985514, + "heading": 1.9461532946570554e-17, + "angularVelocity": 4.2684610863345274e-16, + "velocityX": -4.1143232918434265, + "velocityY": -0.7467949984784092, + "timestamp": 7.944301244424742 + }, + { + "x": 5.757475578384333, + "y": 6.567074520342679, + "heading": 3.073583715260816e-17, + "angularVelocity": 1.1180877485527298e-16, + "velocityX": -4.1217548075042965, + "velocityY": -0.704624884496314, + "timestamp": 8.045136838236653 + }, + { + "x": 5.341855982130269, + "y": 6.496023267448575, + "heading": 4.9911384157422675e-17, + "angularVelocity": 1.901664509492479e-16, + "velocityX": -4.12175483420387, + "velocityY": -0.7046247283140503, + "timestamp": 8.145972432048564 + }, + { + "x": 4.974693840732125, + "y": 6.433255942720096, + "heading": 4.3755003466781107e-17, + "angularVelocity": -6.105364641788702e-17, + "velocityX": -3.6411958071374304, + "velocityY": -0.6224719105196076, + "timestamp": 8.246808025860476 + }, + { + "x": 4.668725379751608, + "y": 6.380949837101729, + "heading": 4.297922243446681e-17, + "angularVelocity": -7.693523718012518e-18, + "velocityX": -3.034329936621781, + "velocityY": -0.5187266087403004, + "timestamp": 8.347643619672388 + }, + { + "x": 4.423950607483419, + "y": 6.339104952011474, + "heading": 3.647497975951615e-17, + "angularVelocity": -6.450343999932714e-17, + "velocityX": -2.4274639838465006, + "velocityY": -0.41498129289849484, + "timestamp": 8.4484792134843 + }, + { + "x": 4.240369526692457, + "y": 6.307721287922, + "heading": 2.807259348278656e-17, + "angularVelocity": -8.332758264591161e-17, + "velocityX": -1.8205980036513219, + "velocityY": -0.3112359723691862, + "timestamp": 8.549314807296211 + }, + { + "x": 4.1179821387611755, + "y": 6.286798845069638, + "heading": 1.6554566502834792e-17, + "angularVelocity": -1.1422580603516648e-16, + "velocityX": -1.2137320097461906, + "velocityY": -0.20749064949612547, + "timestamp": 8.650150401108123 + }, + { + "x": 4.056788444519045, + "y": 6.276337623596191, + "heading": -1.081005308476118e-27, + "angularVelocity": -1.6417383860510177e-16, + "velocityX": -0.6068660076150864, + "velocityY": -0.10374532521681326, + "timestamp": 8.750985994920034 + }, + { + "x": 4.056788444519045, + "y": 6.276337623596191, + "heading": -8.68734990058906e-28, + "angularVelocity": -7.610530353701758e-29, + "velocityX": -1.0690268508350096e-22, + "velocityY": -4.0812108972482127e-23, + "timestamp": 8.851821588731946 + }, + { + "x": 4.0828323692232145, + "y": 6.281987827191528, + "heading": 5.917096277980191e-10, + "angularVelocity": 8.956304425960182e-9, + "velocityX": 0.39420909706230056, + "velocityY": 0.08552327204275713, + "timestamp": 8.917887858887182 + }, + { + "x": 4.134945642418738, + "y": 6.293169746044328, + "heading": 1.7586803299010682e-9, + "angularVelocity": 1.766363833407003e-8, + "velocityX": 0.788803016623651, + "velocityY": 0.16925306705715784, + "timestamp": 8.983954129042418 + }, + { + "x": 4.213159185837416, + "y": 6.309735593389541, + "heading": 3.480401916232622e-9, + "angularVelocity": 2.6060523505958926e-8, + "velocityX": 1.183864977315949, + "velocityY": 0.25074591476540037, + "timestamp": 9.050020399197654 + }, + { + "x": 4.317511365530878, + "y": 6.33149590597647, + "heading": 5.730589257299653e-9, + "angularVelocity": 3.405954862866893e-8, + "velocityX": 1.57950765872307, + "velocityY": 0.3293709866744319, + "timestamp": 9.11608666935289 + }, + { + "x": 4.448050927235057, + "y": 6.358199069694473, + "heading": 8.474350324606633e-9, + "angularVelocity": 4.153043695156349e-8, + "velocityX": 1.975888171035693, + "velocityY": 0.4041875476738608, + "timestamp": 9.182152939508127 + }, + { + "x": 4.604841555340528, + "y": 6.389494861933726, + "heading": 1.1663148228371095e-8, + "angularVelocity": 4.826665553045075e-8, + "velocityX": 2.3732326304642357, + "velocityY": 0.4737030282732438, + "timestamp": 9.248219209663363 + }, + { + "x": 4.7879690180112195, + "y": 6.42486273284508, + "heading": 1.5224903472771874e-8, + "angularVelocity": 5.3911856020201134e-8, + "velocityX": 2.7718753040000252, + "velocityY": 0.5353393014052362, + "timestamp": 9.314285479818599 + }, + { + "x": 4.9975511936263155, + "y": 6.463449108142169, + "heading": 1.9041584229582254e-8, + "angularVelocity": 5.777048935625297e-8, + "velocityX": 3.1723022220361643, + "velocityY": 0.5840556036601274, + "timestamp": 9.380351749973835 + }, + { + "x": 5.233736795312115, + "y": 6.503606531218817, + "heading": 2.2885938036083167e-8, + "angularVelocity": 5.8189357405342333e-8, + "velocityX": 3.574980109075873, + "velocityY": 0.6078354806815993, + "timestamp": 9.446418020129071 + }, + { + "x": 5.496423817757033, + "y": 6.540955963494301, + "heading": 2.615742950768295e-8, + "angularVelocity": 4.9518331576957005e-8, + "velocityX": 3.9761140113961715, + "velocityY": 0.565332842125389, + "timestamp": 9.512484290284307 + }, + { + "x": 5.772317890968588, + "y": 6.555158418696577, + "heading": 2.6157432845131755e-8, + "angularVelocity": 5.051668270867061e-14, + "velocityX": 4.176020116820391, + "velocityY": 0.21497286238354563, + "timestamp": 9.578550560439544 + }, + { + "x": 6.0482967296543855, + "y": 6.542711256144356, + "heading": 2.61574082135951e-8, + "angularVelocity": -3.7283074403740065e-13, + "velocityX": 4.1773031538988095, + "velocityY": -0.1884041966191396, + "timestamp": 9.64461683059478 + }, + { + "x": 6.321792125701904, + "y": 6.503730297088623, + "heading": 1.1564358585154957e-28, + "angularVelocity": -3.9592681942136435e-7, + "velocityX": 4.139712979178139, + "velocityY": -0.5900281484657701, + "timestamp": 9.710683100750016 + }, + { + "x": 6.547355460734675, + "y": 6.4527161195833775, + "heading": -0.007269533201866749, + "angularVelocity": -0.1301585532473417, + "velocityX": 4.038635843353985, + "velocityY": -0.9133917343524579, + "timestamp": 9.76653446833592 + }, + { + "x": 6.760403627607179, + "y": 6.388539344746577, + "heading": -0.033469969753863994, + "angularVelocity": -0.46911002692456527, + "velocityX": 3.814555955945363, + "velocityY": -1.1490636238779843, + "timestamp": 9.822385835921823 + }, + { + "x": 6.955177537271062, + "y": 6.320001202518741, + "heading": -0.07019350986549461, + "angularVelocity": -0.6575226659427229, + "velocityX": 3.4873615111448264, + "velocityY": -1.227152443893485, + "timestamp": 9.878237203507727 + }, + { + "x": 7.130957377141255, + "y": 6.252260176669711, + "heading": -0.11078396487947455, + "angularVelocity": -0.7267584800237601, + "velocityX": 3.1472790634862915, + "velocityY": -1.212880342541995, + "timestamp": 9.93408857109363 + }, + { + "x": 7.288010376164827, + "y": 6.187988784317964, + "heading": -0.15176321891304065, + "angularVelocity": -0.7337197960378785, + "velocityX": 2.811981260476969, + "velocityY": -1.1507577187414648, + "timestamp": 9.989939938679534 + }, + { + "x": 7.426691779416035, + "y": 6.128716395109355, + "heading": -0.19110286527521025, + "angularVelocity": -0.7043631707256224, + "velocityX": 2.48304400134704, + "velocityY": -1.0612522444941654, + "timestamp": 10.045791306265437 + }, + { + "x": 7.547307972917185, + "y": 6.075411733859649, + "heading": -0.22748863294074106, + "angularVelocity": -0.6514749636088404, + "velocityX": 2.1595924811622984, + "velocityY": -0.9544020774015809, + "timestamp": 10.101642673851341 + }, + { + "x": 7.650107021696187, + "y": 6.028737089255784, + "heading": -0.26000406925250097, + "angularVelocity": -0.5821779791112268, + "velocityX": 1.8405824820831433, + "velocityY": -0.835693853549357, + "timestamp": 10.157494041437245 + }, + { + "x": 7.735288937949401, + "y": 5.989171589665674, + "heading": -0.2879751600685273, + "angularVelocity": -0.500812997515315, + "velocityX": 1.5251536342811545, + "velocityY": -0.708406997004223, + "timestamp": 10.213345409023148 + }, + { + "x": 7.803016656542128, + "y": 5.957077010153221, + "heading": -0.3108860017100809, + "angularVelocity": -0.41021093362333316, + "velocityX": 1.2126420805820985, + "velocityY": -0.574642679305731, + "timestamp": 10.269196776609052 + }, + { + "x": 7.853424723852741, + "y": 5.932735740203229, + "heading": -0.3283294175539037, + "angularVelocity": -0.3123185088886745, + "velocityX": 0.9025395346511673, + "velocityY": -0.4358222726158591, + "timestamp": 10.325048144194955 + }, + { + "x": 7.886625758394815, + "y": 5.916374096245245, + "heading": -0.3399762524437299, + "angularVelocity": -0.20853267150374424, + "velocityX": 0.5944533854253174, + "velocityY": -0.2929497461779804, + "timestamp": 10.380899511780859 + }, + { + "x": 7.90271520614624, + "y": 5.908177375793457, + "heading": -0.34555536227067424, + "angularVelocity": -0.0998920898107501, + "velocityX": 0.28807616441402983, + "velocityY": -0.14675952991089913, + "timestamp": 10.436750879366762 + }, + { + "x": 7.900517279512601, + "y": 5.908892154044228, + "heading": -0.34435114666329186, + "angularVelocity": 0.020228619667652558, + "velocityX": -0.03692114738981263, + "velocityY": 0.012006967268082137, + "timestamp": 10.496281169973198 + }, + { + "x": 7.878897174609603, + "y": 5.918918511140869, + "heading": -0.33622484089703686, + "angularVelocity": 0.1365070736842074, + "velocityX": -0.3631782187312475, + "velocityY": 0.168424460799739, + "timestamp": 10.555811460579633 + }, + { + "x": 7.83776699530333, + "y": 5.938088764329109, + "heading": -0.3214499320108964, + "angularVelocity": 0.24819144565948495, + "velocityX": -0.6909117843585054, + "velocityY": 0.3220251907550373, + "timestamp": 10.615341751186069 + }, + { + "x": 7.777022436898006, + "y": 5.966198255304121, + "heading": -0.3003583171650395, + "angularVelocity": 0.3543005523909993, + "velocityX": -1.0203974780992662, + "velocityY": 0.4721880355137041, + "timestamp": 10.674872041792504 + }, + { + "x": 7.69653792964951, + "y": 6.002991760583309, + "heading": -0.27336046232249744, + "angularVelocity": 0.45351458169470893, + "velocityX": -1.3519925138715216, + "velocityY": 0.6180635925739989, + "timestamp": 10.73440233239894 + }, + { + "x": 7.596159781373443, + "y": 6.048142382340488, + "heading": -0.24097637486036755, + "angularVelocity": 0.543993438167913, + "velocityX": -1.6861692972352893, + "velocityY": 0.7584478640575794, + "timestamp": 10.793932623005375 + }, + { + "x": 7.4756963505533935, + "y": 6.101216994676396, + "heading": -0.20388575895135275, + "angularVelocity": 0.6230545077333302, + "velocityX": -2.0235653075583584, + "velocityY": 0.8915564126302368, + "timestamp": 10.85346291361181 + }, + { + "x": 7.334904005956902, + "y": 6.161615825528181, + "heading": -0.16301475379345282, + "angularVelocity": 0.6865581327009607, + "velocityX": -2.3650538769799136, + "velocityY": 1.0145898875429862, + "timestamp": 10.912993204218246 + }, + { + "x": 7.173468437929862, + "y": 6.2284574380514455, + "heading": -0.11969881454856235, + "angularVelocity": 0.7276285535251157, + "velocityX": -2.7118222737113333, + "velocityY": 1.1228168356369232, + "timestamp": 10.972523494824681 + }, + { + "x": 6.990989423393413, + "y": 6.3003338750456575, + "heading": -0.07602350478150598, + "angularVelocity": 0.7336653210010509, + "velocityX": -3.0653136861509287, + "velocityY": 1.2073926779460713, + "timestamp": 11.032053785431117 + }, + { + "x": 6.7870402492258135, + "y": 6.374708055282701, + "heading": -0.03565897410889504, + "angularVelocity": 0.6780502877008852, + "velocityX": -3.425973098568256, + "velocityY": 1.2493501959992728, + "timestamp": 11.091584076037552 + }, + { + "x": 6.561897153690842, + "y": 6.4462208450798215, + "heading": -0.0063889390098301095, + "angularVelocity": 0.4916830541374962, + "velocityX": -3.781992213399873, + "velocityY": 1.2012840701535195, + "timestamp": 11.151114366643988 + }, + { + "x": 6.321792125701904, + "y": 6.503730297088623, + "heading": -2.3188011153160923e-28, + "angularVelocity": 0.10732248985761594, + "velocityX": -4.0333253129286994, + "velocityY": 0.9660536077172232, + "timestamp": 11.210644657250423 + }, + { + "x": 6.026159037797862, + "y": 6.542710336847052, + "heading": 7.2737432191713975e-9, + "angularVelocity": 1.0199984022231118e-7, + "velocityX": -4.145668443609958, + "velocityY": 0.5466178427552496, + "timestamp": 11.281955976986984 + }, + { + "x": 5.728075284165582, + "y": 6.550738026435919, + "heading": 7.273746219720962e-9, + "angularVelocity": 4.207676391895168e-14, + "velocityX": -4.180034175969023, + "velocityY": 0.11257244457854902, + "timestamp": 11.353267296723544 + }, + { + "x": 5.430772694641847, + "y": 6.527726353002143, + "heading": 7.273745820396832e-9, + "angularVelocity": -5.5997299201229045e-15, + "velocityX": -4.1690798967405485, + "velocityY": -0.3226931365004294, + "timestamp": 11.424578616460105 + }, + { + "x": 5.155259555560304, + "y": 6.482595213861262, + "heading": 6.079879033454249e-9, + "angularVelocity": -1.6741616777714582e-8, + "velocityX": -3.863526016617648, + "velocityY": -0.6328748269924842, + "timestamp": 11.495889936196665 + }, + { + "x": 4.910749929550947, + "y": 6.439148378340905, + "heading": 4.8501339645893866e-9, + "angularVelocity": -1.7244738611033904e-8, + "velocityX": -3.428763160079285, + "velocityY": -0.6092558051212502, + "timestamp": 11.567201255933226 + }, + { + "x": 4.69702658726096, + "y": 6.39973389360689, + "heading": 3.7031081606903725e-9, + "angularVelocity": -1.6084764776985806e-8, + "velocityX": -2.997046514908499, + "velocityY": -0.5527100729536375, + "timestamp": 11.638512575669786 + }, + { + "x": 4.513967146494363, + "y": 6.3651959253494725, + "heading": 2.681611531111107e-9, + "angularVelocity": -1.4324466765625498e-8, + "velocityX": -2.5670460376116413, + "velocityY": -0.48432658917276944, + "timestamp": 11.709823895406346 + }, + { + "x": 4.36149950762511, + "y": 6.335968042251882, + "heading": 1.8076673867467656e-9, + "angularVelocity": -1.2255335444539114e-8, + "velocityX": -2.138056614749246, + "velocityY": -0.4098631634579714, + "timestamp": 11.781135215142907 + }, + { + "x": 4.239576710769736, + "y": 6.31231402870262, + "heading": 1.0946867890651993e-9, + "angularVelocity": -9.998140552095568e-9, + "velocityX": -1.7097257112304496, + "velocityY": -0.33170068422020343, + "timestamp": 11.852446534879467 + }, + { + "x": 4.148165858183431, + "y": 6.294411248448443, + "heading": 5.516922206255535e-10, + "angularVelocity": -7.614423214232743e-9, + "velocityX": -1.2818561334160725, + "velocityY": -0.25105102977076493, + "timestamp": 11.923757854616028 + }, + { + "x": 4.087242656413975, + "y": 6.282387124354374, + "heading": 1.851682282397909e-10, + "angularVelocity": -5.139772952453801e-9, + "velocityX": -0.8543272231465969, + "velocityY": -0.16861452204907645, + "timestamp": 11.995069174352588 + }, + { + "x": 4.056788444519045, + "y": 6.276337623596191, + "heading": 6.063226678000312e-28, + "angularVelocity": -2.5966176046641755e-9, + "velocityX": -0.4270599956281157, + "velocityY": -0.08483226478671645, + "timestamp": 12.066380494089149 + }, + { + "x": 4.056788444519045, + "y": 6.276337623596191, + "heading": 2.9642655774633174e-28, + "angularVelocity": -1.8888378589965254e-28, + "velocityX": -6.1162792213078806e-27, + "velocityY": -8.775569155171093e-28, + "timestamp": 12.13769181382571 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "timestamp": 1.1080594821066876, + "isStopPoint": false, + "x": 2.629361867904663, + "y": 4.2189717292785645, + "heading": -0.44684263169879895, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "timestamp": 2.216252355468508, + "isStopPoint": true, + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 3.1390491028073644, + "isStopPoint": false, + "x": 2.6500000000000012, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 4.061967347041126, + "isStopPoint": true, + "x": 1.35, + "y": 5.58225679397583, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "timestamp": 5.164392661724534, + "isStopPoint": true, + "x": 2.629361867904663, + "y": 6.918971729278555, + "heading": 0.44684263169879895, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 5.879142508827019, + "isStopPoint": false, + "x": 4.250000000000002, + "y": 7.282256793975824, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 7.137616493929449, + "isStopPoint": false, + "x": 8.08679485321045, + "y": 7.44578742980957, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 8.851821588731946, + "isStopPoint": true, + "x": 4.056788444519045, + "y": 6.276337623596191, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 9.710683100750016, + "isStopPoint": false, + "x": 6.321792125701904, + "y": 6.503730297088623, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 10.436750879366762, + "isStopPoint": false, + "x": 7.90271520614624, + "y": 5.908177375793457, + "heading": -0.34555536227067424, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 11.210644657250423, + "isStopPoint": false, + "x": 6.321792125701904, + "y": 6.503730297088623, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 12.13769181382571, + "isStopPoint": true, + "x": 4.056788444519045, + "y": 6.276337623596191, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint" + }, + { + "scope": [ + 5 + ], + "type": "StopPoint" + }, + { + "scope": [ + 7 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + } + }, + "splitTrajectoriesAtStopPoints": true, + "usesObstacles": true +} \ No newline at end of file diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar index 7454180..a4b76b9 100644 Binary files a/gradle/wrapper/gradle-wrapper.jar and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index f959987..34bd9ce 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,7 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.3.3-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip +networkTimeout=10000 +validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME zipStorePath=permwrapper/dists diff --git a/gradlew b/gradlew index c53aefa..f5feea6 100644 --- a/gradlew +++ b/gradlew @@ -1,7 +1,7 @@ #!/bin/sh # -# Copyright © 2015-2021 the original authors. +# Copyright © 2015-2021 the original authors. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -15,6 +15,8 @@ # See the License for the specific language governing permissions and # limitations under the License. # +# SPDX-License-Identifier: Apache-2.0 +# ############################################################################## # @@ -32,10 +34,10 @@ # Busybox and similar reduced shells will NOT work, because this script # requires all of these POSIX shell features: # * functions; -# * expansions «$var», «${var}», «${var:-default}», «${var+SET}», -# «${var#prefix}», «${var%suffix}», and «$( cmd )»; -# * compound commands having a testable exit status, especially «case»; -# * various built-in commands including «command», «set», and «ulimit». +# * expansions «$var», «${var}», «${var:-default}», «${var+SET}», +# «${var#prefix}», «${var%suffix}», and «$( cmd )»; +# * compound commands having a testable exit status, especially «case»; +# * various built-in commands including «command», «set», and «ulimit». # # Important for patching: # @@ -55,7 +57,7 @@ # Darwin, MinGW, and NonStop. # # (3) This script is generated from the Groovy template -# https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt # within the Gradle project. # # You can find Gradle at https://github.com/gradle/gradle/. @@ -80,13 +82,12 @@ do esac done -APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit - -APP_NAME="Gradle" +# This is normally unused +# shellcheck disable=SC2034 APP_BASE_NAME=${0##*/} - -# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. -DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s +' "$PWD" ) || exit # Use the maximum available, or set MAX_FD != -1 to use that value. MAX_FD=maximum @@ -133,22 +134,29 @@ location of your Java installation." fi else JAVACMD=java - which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + if ! command -v java >/dev/null 2>&1 + then + die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. Please set the JAVA_HOME variable in your environment to match the location of your Java installation." + fi fi # Increase the maximum file descriptors if we can. if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then case $MAX_FD in #( max*) + # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 MAX_FD=$( ulimit -H -n ) || warn "Could not query maximum file descriptor limit" esac case $MAX_FD in #( '' | soft) :;; #( *) + # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 ulimit -n "$MAX_FD" || warn "Could not set maximum file descriptor limit to $MAX_FD" esac @@ -193,11 +201,15 @@ if "$cygwin" || "$msys" ; then done fi -# Collect all arguments for the java command; -# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of -# shell script including quotes and variable substitutions, so put them in -# double quotes to make sure that they get re-expanded; and -# * put everything else in single quotes, so that it's not re-expanded. + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Collect all arguments for the java command: +# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# and any embedded shellness will be escaped. +# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be +# treated as '${Hostname}' itself on the command line. set -- \ "-Dorg.gradle.appname=$APP_BASE_NAME" \ @@ -205,6 +217,12 @@ set -- \ org.gradle.wrapper.GradleWrapperMain \ "$@" +# Stop when "xargs" is not available. +if ! command -v xargs >/dev/null 2>&1 +then + die "xargs is not available" +fi + # Use "xargs" to parse quoted args. # # With -n1 it outputs one arg per line, with the quotes and backslashes removed. diff --git a/gradlew.bat b/gradlew.bat index 107acd3..9d21a21 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -13,8 +13,10 @@ @rem See the License for the specific language governing permissions and @rem limitations under the License. @rem +@rem SPDX-License-Identifier: Apache-2.0 +@rem -@if "%DEBUG%" == "" @echo off +@if "%DEBUG%"=="" @echo off @rem ########################################################################## @rem @rem Gradle startup script for Windows @@ -25,7 +27,8 @@ if "%OS%"=="Windows_NT" setlocal set DIRNAME=%~dp0 -if "%DIRNAME%" == "" set DIRNAME=. +if "%DIRNAME%"=="" set DIRNAME=. +@rem This is normally unused set APP_BASE_NAME=%~n0 set APP_HOME=%DIRNAME% @@ -40,13 +43,13 @@ if defined JAVA_HOME goto findJavaFromJavaHome set JAVA_EXE=java.exe %JAVA_EXE% -version >NUL 2>&1 -if "%ERRORLEVEL%" == "0" goto execute +if %ERRORLEVEL% equ 0 goto execute -echo. -echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. +echo. 1>&2 +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 goto fail @@ -56,11 +59,11 @@ set JAVA_EXE=%JAVA_HOME%/bin/java.exe if exist "%JAVA_EXE%" goto execute -echo. -echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. +echo. 1>&2 +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 goto fail @@ -75,13 +78,15 @@ set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar :end @rem End local scope for the variables with windows NT shell -if "%ERRORLEVEL%"=="0" goto mainEnd +if %ERRORLEVEL% equ 0 goto mainEnd :fail rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of rem the _cmd.exe /c_ return code! -if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 -exit /b 1 +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% :mainEnd if "%OS%"=="Windows_NT" endlocal diff --git a/settings.gradle b/settings.gradle index c363694..4d2ffed 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2022' + String frcYear = '2025' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') @@ -25,3 +25,7 @@ pluginManagement { } } } + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); +rootProject.name = '2025RobotCode' \ No newline at end of file diff --git a/src/main/deploy/example.txt b/src/main/deploy/example.txt deleted file mode 100644 index bb82515..0000000 --- a/src/main/deploy/example.txt +++ /dev/null @@ -1,3 +0,0 @@ -Files placed in this directory will be deployed to the RoboRIO into the -'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function -to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path deleted file mode 100644 index 1926665..0000000 --- a/src/main/deploy/pathplanner/New Path.path +++ /dev/null @@ -1,74 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.0, - "y": 3.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.0, - "y": 3.0 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 5.334010668021336, - "y": 5.955334987593052 - }, - "prevControl": { - "x": 5.334010668021336, - "y": 4.955334987593052 - }, - "nextControl": { - "x": 5.334010668021336, - "y": 4.955334987593052 - }, - "holonomicAngle": 0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 5.0, - "y": 3.0 - }, - "prevControl": { - "x": 4.0, - "y": 3.0 - }, - "nextControl": null, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Align Right Side.auto b/src/main/deploy/pathplanner/autos/Align Right Side.auto new file mode 100644 index 0000000..4e318b1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Align Right Side.auto @@ -0,0 +1,88 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "AP1" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Station R P2" + } + }, + { + "type": "path", + "data": { + "pathName": "Station R P3" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Lollipop J, L, A.auto b/src/main/deploy/pathplanner/autos/Left Lollipop J, L, A.auto new file mode 100644 index 0000000..02ddf6a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Lollipop J, L, A.auto @@ -0,0 +1,118 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "P 1 - Align After" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 11/20 Right" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Official Lollipop 1" + } + }, + { + "type": "path", + "data": { + "pathName": "Lollipop 2" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 6/19 Right" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Lollipop 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Lollipop 4" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 6/19 Right" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Side Alt.auto b/src/main/deploy/pathplanner/autos/Left Side Alt.auto new file mode 100644 index 0000000..8d405d2 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Side Alt.auto @@ -0,0 +1,158 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "P 1" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 11/20 Right" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Gronud P2" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 6/19 Left" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "GroundB #6" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 6/19 Right" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Side Sweep.auto b/src/main/deploy/pathplanner/autos/Left Side Sweep.auto new file mode 100644 index 0000000..9054c73 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Side Sweep.auto @@ -0,0 +1,99 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "P 1 - Align After" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 11/20 Right" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Sweep P2" + } + }, + { + "type": "named", + "data": { + "name": "Drive To Left Station" + } + }, + { + "type": "path", + "data": { + "pathName": "LSweep P3" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 6/19 Right" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "GrondP4" + } + }, + { + "type": "named", + "data": { + "name": "Drive To Left Station" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Side.auto b/src/main/deploy/pathplanner/autos/Left Side.auto new file mode 100644 index 0000000..2cf0435 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Side.auto @@ -0,0 +1,124 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "P 1" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 11/20 Right" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Gronud P2" + } + }, + { + "type": "path", + "data": { + "pathName": "P 5" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 6/19 Left" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "GroundB #6" + } + }, + { + "type": "path", + "data": { + "pathName": "P 3" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 6/19 Right" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "3d intake peice" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Side.auto b/src/main/deploy/pathplanner/autos/Right Side.auto new file mode 100644 index 0000000..206b69e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Side.auto @@ -0,0 +1,118 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "P 1 Mirrored" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 9/22 Left" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Gronud P2 Mirrored" + } + }, + { + "type": "path", + "data": { + "pathName": "P 5 Mirrored" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 8/17 Right" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "GroundB #6 Mirrored" + } + }, + { + "type": "path", + "data": { + "pathName": "P 3 Mirrored" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 8/17 Left" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Station Left Side - Align After.auto b/src/main/deploy/pathplanner/autos/Station Left Side - Align After.auto new file mode 100644 index 0000000..2f6ff4b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Station Left Side - Align After.auto @@ -0,0 +1,144 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "P 1 - Align After" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 11/20 Right" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Station P2" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Station Intake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "StationB Alt Opt Left - Align After" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 6/19 Left" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": " Station P4" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Station Intake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": " StationB Alt Opt - Align After" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 6/19 Right" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Station Left Side.auto b/src/main/deploy/pathplanner/autos/Station Left Side.auto new file mode 100644 index 0000000..65980d3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Station Left Side.auto @@ -0,0 +1,144 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "P 1" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 11/20 Right" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Station P2" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Station Intake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "StationP 3" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 6/19 Left" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": " Station P4" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Station Intake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "StationB Alt Opt" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive To 6/19 Right" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Station Right Side.auto b/src/main/deploy/pathplanner/autos/Station Right Side.auto new file mode 100644 index 0000000..bfc5082 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Station Right Side.auto @@ -0,0 +1,142 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "AP1" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive to 9/22 Left" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Station R P2" + } + }, + { + "type": "named", + "data": { + "name": "Drive to Right Station Intake" + } + }, + { + "type": "named", + "data": { + "name": "Station Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "Station R P3" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive to 8/17 Right" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Station R P4" + } + }, + { + "type": "named", + "data": { + "name": "Drive to Right Station Intake" + } + }, + { + "type": "named", + "data": { + "name": "Station Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "Station R P5" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Drive to 8/17 Left" + } + }, + { + "type": "named", + "data": { + "name": "OuttakeCoral" + } + }, + { + "type": "named", + "data": { + "name": "Lower Elevator" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..23e0db9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3d intake peice.path b/src/main/deploy/pathplanner/paths/3d intake peice.path new file mode 100644 index 0000000..69fa8f5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3d intake peice.path @@ -0,0 +1,80 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.66, + "y": 5.12 + }, + "prevControl": null, + "nextControl": { + "x": 3.909326741052695, + "y": 5.101664891709005 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.722, + "y": 7.129 + }, + "prevControl": { + "x": 1.5210513367419152, + "y": 7.294369927869808 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6049966239027695, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "IntakeCoral", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": 1.0, + "command": { + "type": "named", + "data": { + "name": "IntakeCoral" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -145.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": -150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4th peice.path b/src/main/deploy/pathplanner/paths/4th peice.path new file mode 100644 index 0000000..90f5af4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/4th peice.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.9300204918032788, + "y": 7.243698770491803 + }, + "prevControl": null, + "nextControl": { + "x": 2.930020491803279, + "y": 7.243698770491803 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6202868852459016, + "y": 5.589395491803279 + }, + "prevControl": { + "x": 2.6202868852459016, + "y": 5.589395491803279 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -145.66978280449666 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -141.34019174590986 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/AP1.path b/src/main/deploy/pathplanner/paths/AP1.path new file mode 100644 index 0000000..a693998 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/AP1.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.18063524590164, + "y": 0.48 + }, + "prevControl": null, + "nextControl": { + "x": 6.483621180258631, + "y": 1.147118232765553 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.81183, + "y": 2.768 + }, + "prevControl": { + "x": 5.174076079601306, + "y": 2.4481272403373495 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "MoveElevator", + "waypointRelativePos": 0.4514601420678732, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center to G.path b/src/main/deploy/pathplanner/paths/Center to G.path new file mode 100644 index 0000000..056673f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center to G.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.229, + "y": 3.8608 + }, + "prevControl": null, + "nextControl": { + "x": 6.903565973322253, + "y": 3.8608 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.74, + "y": 3.8608 + }, + "prevControl": { + "x": 6.308787430236012, + "y": 3.8608 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "MoveElevator", + "waypointRelativePos": 0.7829518547750566, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center to H.path b/src/main/deploy/pathplanner/paths/Center to H.path new file mode 100644 index 0000000..5734ea0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center to H.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.229, + "y": 4.191 + }, + "prevControl": null, + "nextControl": { + "x": 6.903565973322253, + "y": 4.191 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.74, + "y": 4.191 + }, + "prevControl": { + "x": 6.308787430236012, + "y": 4.191 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/GrondP4.path b/src/main/deploy/pathplanner/paths/GrondP4.path new file mode 100644 index 0000000..2863bb1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/GrondP4.path @@ -0,0 +1,82 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.721, + "y": 5.077 + }, + "prevControl": null, + "nextControl": { + "x": 3.7080822290916022, + "y": 4.827333960669942 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.260710227277713, + "y": 7.240838068181818 + }, + "prevControl": { + "x": 3.769261363646335, + "y": 7.031434659090908 + }, + "nextControl": { + "x": 2.5266509333291625, + "y": 7.543097777451788 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.8076988636413492, + "y": 6.572741477272728 + }, + "prevControl": { + "x": 2.1270340909140755, + "y": 7.342904545454545 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.4, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeCoral" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -53.616 + }, + "reversed": false, + "folder": "Ground LSide", + "idealStartingState": { + "velocity": 0, + "rotation": -145.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Gronud P2 Mirrored.path b/src/main/deploy/pathplanner/paths/Gronud P2 Mirrored.path new file mode 100644 index 0000000..9b881a0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Gronud P2 Mirrored.path @@ -0,0 +1,101 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.0, + "y": 2.81 + }, + "prevControl": null, + "nextControl": { + "x": 4.967147352430555, + "y": 2.1109704137731478 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6396306818181814, + "y": 1.826 + }, + "prevControl": { + "x": 3.976501550755575, + "y": 1.7392387033253078 + }, + "nextControl": { + "x": 2.8861082175925916, + "y": 2.020070167824074 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.722, + "y": 0.921 + }, + "prevControl": { + "x": 2.7759913917824064, + "y": 2.1777043547453694 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.25, + "rotationDegrees": -39.29328640235812 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.6961512491559774, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.55, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeCoral" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -36.0 + }, + "reversed": false, + "folder": "Ground LSide", + "idealStartingState": { + "velocity": 0.0, + "rotation": 29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Gronud P2.path b/src/main/deploy/pathplanner/paths/Gronud P2.path new file mode 100644 index 0000000..4345b4a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Gronud P2.path @@ -0,0 +1,101 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.0, + "y": 5.24 + }, + "prevControl": null, + "nextControl": { + "x": 4.856164772727273, + "y": 5.775014204545455 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6396306818181814, + "y": 6.223735795454545 + }, + "prevControl": { + "x": 3.9487500000000004, + "y": 6.383281249999999 + }, + "nextControl": { + "x": 3.061864563544763, + "y": 5.925533927958591 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.722, + "y": 7.129 + }, + "prevControl": { + "x": 2.562698863636364, + "y": 6.044247159090909 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.25, + "rotationDegrees": -145.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.6961512491559774, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.55, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeCoral" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -144.0 + }, + "reversed": false, + "folder": "Ground LSide", + "idealStartingState": { + "velocity": 0.0, + "rotation": 150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/GroundB #2.path b/src/main/deploy/pathplanner/paths/GroundB #2.path new file mode 100644 index 0000000..c1fc6d2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/GroundB #2.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.0073368, + "y": 5.1909832 + }, + "prevControl": null, + "nextControl": { + "x": 3.6414852850066137, + "y": 5.481452904381226 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.885, + "y": 7.011 + }, + "prevControl": { + "x": 2.279280354370602, + "y": 6.697959071416107 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.6463314097279476, + "endWaypointRelativePos": 1.0, + "command": { + "type": "named", + "data": { + "name": "IntakeCoral" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -144.0 + }, + "reversed": false, + "folder": "Ground BSide", + "idealStartingState": { + "velocity": 0, + "rotation": -150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/GroundB #3.path b/src/main/deploy/pathplanner/paths/GroundB #3.path new file mode 100644 index 0000000..049b70a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/GroundB #3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.885, + "y": 7.011 + }, + "prevControl": null, + "nextControl": { + "x": 2.1167904466171583, + "y": 6.862521975540034 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.7213752, + "y": 5.0258832 + }, + "prevControl": { + "x": 3.3903399058878056, + "y": 5.426007692748621 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -150.0 + }, + "reversed": false, + "folder": "Ground BSide", + "idealStartingState": { + "velocity": 0, + "rotation": -144.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/GroundB #5.path b/src/main/deploy/pathplanner/paths/GroundB #5.path new file mode 100644 index 0000000..b81a9be --- /dev/null +++ b/src/main/deploy/pathplanner/paths/GroundB #5.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.722, + "y": 7.129 + }, + "prevControl": null, + "nextControl": { + "x": 1.8649319141897391, + "y": 6.267760789902279 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.1273, + "y": 4.191 + }, + "prevControl": { + "x": 2.1273, + "y": 4.191 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "MoveElevator", + "waypointRelativePos": 0.6806256151574805, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.5, + "rotation": -90.89200324814249 + }, + "reversed": false, + "folder": "Ground BSide", + "idealStartingState": { + "velocity": 0, + "rotation": -142.86256094570606 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/GroundB #6 Mirrored.path b/src/main/deploy/pathplanner/paths/GroundB #6 Mirrored.path new file mode 100644 index 0000000..302abdd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/GroundB #6 Mirrored.path @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.98, + "y": 2.81 + }, + "prevControl": null, + "nextControl": { + "x": 3.661208032083275, + "y": 2.7146988994001573 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.722, + "y": 0.921 + }, + "prevControl": { + "x": 1.2995983363740153, + "y": 0.5281644353113872 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3291032261499149, + "rotationDegrees": -32.65592295916657 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.5509790681971605, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.11073598919649108, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeCoral" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -35.0 + }, + "reversed": false, + "folder": "Ground LSide", + "idealStartingState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/GroundB #6.path b/src/main/deploy/pathplanner/paths/GroundB #6.path new file mode 100644 index 0000000..2a7adb3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/GroundB #6.path @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.98, + "y": 5.24 + }, + "prevControl": null, + "nextControl": { + "x": 3.316855850468241, + "y": 5.760793783082248 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.722, + "y": 7.129 + }, + "prevControl": { + "x": 2.338563708774429, + "y": 6.649638232899023 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3291032261499149, + "rotationDegrees": -136.13455862983048 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.5509790681971605, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.11073598919649108, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeCoral" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -142.62124693823438 + }, + "reversed": false, + "folder": "Ground LSide", + "idealStartingState": { + "velocity": 0, + "rotation": -150.82019924107797 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/GrounddB #4.path b/src/main/deploy/pathplanner/paths/GrounddB #4.path new file mode 100644 index 0000000..0cc0a83 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/GrounddB #4.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.7213752, + "y": 5.0258832 + }, + "prevControl": null, + "nextControl": { + "x": 3.2108522727272732, + "y": 5.555639204545454 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.722, + "y": 7.129 + }, + "prevControl": { + "x": 2.061030502628888, + "y": 6.716198757492258 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.3779527559055112, + "endWaypointRelativePos": 1.0, + "command": { + "type": "named", + "data": { + "name": "IntakeCoral" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -144.0 + }, + "reversed": false, + "folder": "Ground BSide", + "idealStartingState": { + "velocity": 0, + "rotation": -150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LSweep P3.path b/src/main/deploy/pathplanner/paths/LSweep P3.path new file mode 100644 index 0000000..94ffe3f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LSweep P3.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.788, + "y": 6.603 + }, + "prevControl": null, + "nextControl": { + "x": 2.9316477272727273, + "y": 6.26396875 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.721, + "y": 5.025 + }, + "prevControl": { + "x": 3.5675633667260422, + "y": 5.222375782631895 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Elevator", + "waypointRelativePos": 0.16647919010123857, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -150.0 + }, + "reversed": false, + "folder": "Ground LSide", + "idealStartingState": { + "velocity": 0, + "rotation": -56.976000000000006 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Lollipop 2.path b/src/main/deploy/pathplanner/paths/Lollipop 2.path new file mode 100644 index 0000000..d258477 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Lollipop 2.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.239, + "y": 5.843 + }, + "prevControl": null, + "nextControl": { + "x": 2.189204371946254, + "y": 5.904804878359121 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6657752058351885, + "y": 5.122 + }, + "prevControl": { + "x": 3.1011126071083357, + "y": 5.555954762962963 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Elevator", + "waypointRelativePos": 0.4830139640748033, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.5, + "rotation": -150.68614751980144 + }, + "reversed": false, + "folder": "Lollipop Path", + "idealStartingState": { + "velocity": 0, + "rotation": -89.56435434461116 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Lollipop 3.path b/src/main/deploy/pathplanner/paths/Lollipop 3.path new file mode 100644 index 0000000..f776ef5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Lollipop 3.path @@ -0,0 +1,80 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6657752058351885, + "y": 5.122185271626888 + }, + "prevControl": null, + "nextControl": { + "x": 3.232096205428022, + "y": 5.337027431134217 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.224, + "y": 4.023 + }, + "prevControl": { + "x": 2.2404897088951454, + "y": 4.642129432338781 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6121951664719626, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.6682840182086613, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeCoral" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -48.652092538649285 + }, + "reversed": false, + "folder": "Lollipop Path", + "idealStartingState": { + "velocity": 0, + "rotation": -149.20719589802195 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Lollipop 4.path b/src/main/deploy/pathplanner/paths/Lollipop 4.path new file mode 100644 index 0000000..1774ff5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Lollipop 4.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.224, + "y": 4.023 + }, + "prevControl": null, + "nextControl": { + "x": 1.9751816342630288, + "y": 4.332334397903094 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.923609664351851, + "y": 4.208198061342593 + }, + "prevControl": { + "x": 2.412264463822535, + "y": 4.184931100328749 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Elevator", + "waypointRelativePos": 0.402936988901869, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.5, + "rotation": -89.04406269082419 + }, + "reversed": false, + "folder": "Lollipop Path", + "idealStartingState": { + "velocity": 0, + "rotation": -43.54279799705628 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Lollipop 5.path b/src/main/deploy/pathplanner/paths/Lollipop 5.path new file mode 100644 index 0000000..b9155fd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Lollipop 5.path @@ -0,0 +1,80 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.239, + "y": 4.191 + }, + "prevControl": null, + "nextControl": { + "x": 3.0626564776043614, + "y": 3.9678578908798716 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.197960792824073, + "y": 2.2002983940972216 + }, + "prevControl": { + "x": 0.9319482350521202, + "y": 1.9434278342202775 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6121951664719626, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.6682840182086613, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeCoral" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -48.652 + }, + "reversed": false, + "folder": "Lollipop Path", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Lollipop 6.path b/src/main/deploy/pathplanner/paths/Lollipop 6.path new file mode 100644 index 0000000..98904f0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Lollipop 6.path @@ -0,0 +1,80 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.198, + "y": 2.2 + }, + "prevControl": null, + "nextControl": { + "x": 1.0216564776043615, + "y": 1.976857890879872 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.23, + "y": 4.191 + }, + "prevControl": { + "x": 2.9639874422280474, + "y": 3.9341294401230558 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6121951664719626, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.6682840182086613, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeCoral" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Lollipop Path", + "idealStartingState": { + "velocity": 0, + "rotation": -36.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Official Lollipop 1.path b/src/main/deploy/pathplanner/paths/Official Lollipop 1.path new file mode 100644 index 0000000..81b32a1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Official Lollipop 1.path @@ -0,0 +1,80 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.026909205835189, + "y": 5.287285271626888 + }, + "prevControl": null, + "nextControl": { + "x": 3.9877799051430065, + "y": 5.857346791883403 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.239, + "y": 5.843 + }, + "prevControl": { + "x": 1.9968631781709456, + "y": 5.892619841156129 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.5537930782710281, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.40029835137795283, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeCoral" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -89.24717948106847 + }, + "reversed": false, + "folder": "Lollipop Path", + "idealStartingState": { + "velocity": 0, + "rotation": 150.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/P 1 - Align After.path b/src/main/deploy/pathplanner/paths/P 1 - Align After.path new file mode 100644 index 0000000..bf6da05 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/P 1 - Align After.path @@ -0,0 +1,88 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.065, + "y": 7.315 + }, + "prevControl": null, + "nextControl": { + "x": 6.311723981584821, + "y": 6.540661185128348 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.9713092, + "y": 5.1909832 + }, + "prevControl": { + "x": 5.454483001977311, + "y": 5.695001290538291 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "backdrive", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "backdrive" + } + } + }, + { + "name": "lower intake", + "waypointRelativePos": 0.14206787687450353, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "lower intake" + } + } + }, + { + "name": "MoveElevator", + "waypointRelativePos": 0.4625098658247798, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.5, + "rotation": 150.0 + }, + "reversed": false, + "folder": "Ground LSide", + "idealStartingState": { + "velocity": 0, + "rotation": 150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/P 1 Mirrored.path b/src/main/deploy/pathplanner/paths/P 1 Mirrored.path new file mode 100644 index 0000000..772e6f8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/P 1 Mirrored.path @@ -0,0 +1,88 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.065, + "y": 0.735 + }, + "prevControl": null, + "nextControl": { + "x": 6.163789350689934, + "y": 1.5557182436227601 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.094774590163934, + "y": 2.712 + }, + "prevControl": { + "x": 5.687902753152638, + "y": 2.1760455531838283 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "backdrive", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "backdrive" + } + } + }, + { + "name": "lower intake", + "waypointRelativePos": 0.14206787687450353, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "lower intake" + } + } + }, + { + "name": "MoveElevator", + "waypointRelativePos": 0.4625098658247798, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 29.999999999999996 + }, + "reversed": false, + "folder": "Ground LSide", + "idealStartingState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/P 1.path b/src/main/deploy/pathplanner/paths/P 1.path new file mode 100644 index 0000000..9a24b89 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/P 1.path @@ -0,0 +1,88 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.065, + "y": 7.315 + }, + "prevControl": null, + "nextControl": { + "x": 6.407825499545448, + "y": 6.60471106063858 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.094774590163934, + "y": 5.33765368852459 + }, + "prevControl": { + "x": 5.760558198478665, + "y": 6.031726466482178 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "backdrive", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "backdrive" + } + } + }, + { + "name": "lower intake", + "waypointRelativePos": 0.14206787687450353, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "lower intake" + } + } + }, + { + "name": "MoveElevator", + "waypointRelativePos": 0.4625098658247798, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 150.0 + }, + "reversed": false, + "folder": "Ground LSide", + "idealStartingState": { + "velocity": 0, + "rotation": 150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/P 3 Mirrored.path b/src/main/deploy/pathplanner/paths/P 3 Mirrored.path new file mode 100644 index 0000000..023d3dc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/P 3 Mirrored.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.722, + "y": 0.921 + }, + "prevControl": null, + "nextControl": { + "x": 1.9852053519873245, + "y": 0.9368665670159784 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5843237704918027, + "y": 2.796 + }, + "prevControl": { + "x": 3.3462031912644794, + "y": 2.7198515282593014 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Elevator", + "waypointRelativePos": 0.16647919010123857, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -29.999999999999996 + }, + "reversed": false, + "folder": "Ground LSide", + "idealStartingState": { + "velocity": 0, + "rotation": -35.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/P 3.path b/src/main/deploy/pathplanner/paths/P 3.path new file mode 100644 index 0000000..2a95b2f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/P 3.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.722, + "y": 7.1288 + }, + "prevControl": null, + "nextControl": { + "x": 2.513128546999364, + "y": 6.385951337740788 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5843237704918027, + "y": 5.25373975409836 + }, + "prevControl": { + "x": 3.1610940855407414, + "y": 5.679961652038816 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Elevator", + "waypointRelativePos": 0.16647919010123857, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -150.0 + }, + "reversed": false, + "folder": "Ground LSide", + "idealStartingState": { + "velocity": 0.0, + "rotation": -145.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/P 5 Mirrored.path b/src/main/deploy/pathplanner/paths/P 5 Mirrored.path new file mode 100644 index 0000000..ac07b82 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/P 5 Mirrored.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.722, + "y": 0.921 + }, + "prevControl": null, + "nextControl": { + "x": 2.0053161494766774, + "y": 1.1670302077595403 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.8720286885245896, + "y": 2.7 + }, + "prevControl": { + "x": 3.5346593444490164, + "y": 2.4590517448283564 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "MoveElevator", + "waypointRelativePos": 0.39370078740157555, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -29.999999999999996 + }, + "reversed": false, + "folder": "Ground LSide", + "idealStartingState": { + "velocity": 0, + "rotation": -35.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/P 5.path b/src/main/deploy/pathplanner/paths/P 5.path new file mode 100644 index 0000000..8631720 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/P 5.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.722, + "y": 7.129 + }, + "prevControl": null, + "nextControl": { + "x": 2.58836769386574, + "y": 6.410185185185185 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.8720286885245896, + "y": 5.3496413934426235 + }, + "prevControl": { + "x": 3.2181991167653305, + "y": 5.822776882447252 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "MoveElevator", + "waypointRelativePos": 0.39370078740157555, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -151.83 + }, + "reversed": false, + "folder": "Ground LSide", + "idealStartingState": { + "velocity": 0, + "rotation": -145.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Station P2.path b/src/main/deploy/pathplanner/paths/Station P2.path new file mode 100644 index 0000000..922df85 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Station P2.path @@ -0,0 +1,71 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.9713092, + "y": 5.1909832 + }, + "prevControl": null, + "nextControl": { + "x": 4.461440347544805, + "y": 7.189854101642059 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.65, + "y": 7.4 + }, + "prevControl": { + "x": 3.1784221311513603, + "y": 5.374715163937142 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4991119005328578, + "rotationDegrees": 61.586 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Station", + "waypointRelativePos": 0.6590141796083744, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Station Setpoint" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 36.0 + }, + "reversed": false, + "folder": "Station Left", + "idealStartingState": { + "velocity": 0, + "rotation": 150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Station P4.path b/src/main/deploy/pathplanner/paths/Station P4.path new file mode 100644 index 0000000..df31b0a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Station P4.path @@ -0,0 +1,71 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.9517367941648103, + "y": 5.287285271626888 + }, + "prevControl": null, + "nextControl": { + "x": 3.4418679417096154, + "y": 7.286156173268947 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.356, + "y": 7.331 + }, + "prevControl": { + "x": 2.8844221311513616, + "y": 5.305715163937142 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8063943161634158, + "rotationDegrees": 61.586 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "MoveElevator", + "waypointRelativePos": 0.6861642294713134, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Station Setpoint" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 36.0 + }, + "reversed": false, + "folder": "Station Left", + "idealStartingState": { + "velocity": 0, + "rotation": -150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Station R P2.path b/src/main/deploy/pathplanner/paths/Station R P2.path new file mode 100644 index 0000000..34fb495 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Station R P2.path @@ -0,0 +1,71 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.812, + "y": 2.768 + }, + "prevControl": null, + "nextControl": { + "x": 3.6667827868852463, + "y": 1.1836495901639348 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.525653409090909, + "y": 0.7293892045454539 + }, + "prevControl": { + "x": 3.111195051053366, + "y": 2.9091389109644723 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6287744227353493, + "rotationDegrees": 131.817128535173 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Station", + "waypointRelativePos": 0.6634844868735067, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Station Setpoint" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 145.0 + }, + "reversed": false, + "folder": "Station RSide", + "idealStartingState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Station R P3.path b/src/main/deploy/pathplanner/paths/Station R P3.path new file mode 100644 index 0000000..ce2a120 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Station R P3.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.526, + "y": 0.729 + }, + "prevControl": null, + "nextControl": { + "x": 3.0456919758517573, + "y": 2.2265382848833575 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.007, + "y": 2.861 + }, + "prevControl": { + "x": 3.745218130977533, + "y": 2.6169235964968895 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Elevator", + "waypointRelativePos": 0.16647919010123857, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "reversed": false, + "folder": "Station RSide", + "idealStartingState": { + "velocity": 0, + "rotation": 144.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Station R P4.path b/src/main/deploy/pathplanner/paths/Station R P4.path new file mode 100644 index 0000000..dca9db2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Station R P4.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.007, + "y": 2.861 + }, + "prevControl": null, + "nextControl": { + "x": 2.966570514239714, + "y": 1.77435496905575 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.526, + "y": 0.729 + }, + "prevControl": { + "x": 2.4772300707441195, + "y": 1.765652915018721 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Station", + "waypointRelativePos": 0.6587112171837709, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Station Setpoint" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 144.0 + }, + "reversed": false, + "folder": "Station RSide", + "idealStartingState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Station R P5.path b/src/main/deploy/pathplanner/paths/Station R P5.path new file mode 100644 index 0000000..1c32f18 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Station R P5.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.526, + "y": 0.729 + }, + "prevControl": null, + "nextControl": { + "x": 2.9585445192827198, + "y": 1.9643408880242017 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.721, + "y": 3.026 + }, + "prevControl": { + "x": 3.1963255727771056, + "y": 2.590703696539521 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "MoveElevator", + "waypointRelativePos": 0.39370078740157555, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "reversed": false, + "folder": "Station RSide", + "idealStartingState": { + "velocity": 0, + "rotation": 144.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StationB #5.path b/src/main/deploy/pathplanner/paths/StationB #5.path new file mode 100644 index 0000000..0100ceb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/StationB #5.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.722, + "y": 7.403 + }, + "prevControl": null, + "nextControl": { + "x": 1.8649319141897391, + "y": 6.541760789902279 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.2385, + "y": 4.191 + }, + "prevControl": { + "x": 2.2385, + "y": 4.191 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "MoveElevator", + "waypointRelativePos": 0.6806256151574805, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.89200324814249 + }, + "reversed": false, + "folder": "Station Left", + "idealStartingState": { + "velocity": 0, + "rotation": 36.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StationB Alt Opt - Align After.path b/src/main/deploy/pathplanner/paths/StationB Alt Opt - Align After.path new file mode 100644 index 0000000..da5d433 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/StationB Alt Opt - Align After.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.356, + "y": 7.331 + }, + "prevControl": null, + "nextControl": { + "x": 2.334068287037036, + "y": 6.521408420138889 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.666, + "y": 5.122185271626888 + }, + "prevControl": { + "x": 2.836426070601852, + "y": 5.99030882486763 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "MoveElevator", + "waypointRelativePos": 0.6806256151574805, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -149.28470582855732 + }, + "reversed": false, + "folder": "Station Left", + "idealStartingState": { + "velocity": 0, + "rotation": 36.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StationB Alt Opt Left - Align After.path b/src/main/deploy/pathplanner/paths/StationB Alt Opt Left - Align After.path new file mode 100644 index 0000000..d148283 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/StationB Alt Opt Left - Align After.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.356, + "y": 7.331 + }, + "prevControl": null, + "nextControl": { + "x": 2.320208548409597, + "y": 6.4855863560267855 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.9517367941648103, + "y": 5.287285271626888 + }, + "prevControl": { + "x": 3.2517059326171873, + "y": 5.978196498325892 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "MoveElevator", + "waypointRelativePos": 0.6806256151574805, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -149.28470582855732 + }, + "reversed": false, + "folder": "Station Left", + "idealStartingState": { + "velocity": 0, + "rotation": 36.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StationB Alt Opt.path b/src/main/deploy/pathplanner/paths/StationB Alt Opt.path new file mode 100644 index 0000000..c01326c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/StationB Alt Opt.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.65, + "y": 7.4 + }, + "prevControl": null, + "nextControl": { + "x": 2.334068287037036, + "y": 6.521408420138889 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5380135672433033, + "y": 5.395889718191963 + }, + "prevControl": { + "x": 2.708439637845155, + "y": 6.264013271432705 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "MoveElevator", + "waypointRelativePos": 0.6806256151574805, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -149.28470582855732 + }, + "reversed": false, + "folder": "Station Left", + "idealStartingState": { + "velocity": 0, + "rotation": 36.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StationP 3.path b/src/main/deploy/pathplanner/paths/StationP 3.path new file mode 100644 index 0000000..3b53624 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/StationP 3.path @@ -0,0 +1,71 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.65, + "y": 7.4 + }, + "prevControl": null, + "nextControl": { + "x": 2.9294370334836404, + "y": 6.130971138637706 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.8229949951171873, + "y": 5.580079432896205 + }, + "prevControl": { + "x": 3.6027199899253537, + "y": 5.6983119245714535 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3836589698046169, + "rotationDegrees": 150.75582343932 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "MoveElevator", + "waypointRelativePos": 0.45374746792707615, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -151.83 + }, + "reversed": false, + "folder": "Station Left", + "idealStartingState": { + "velocity": 0, + "rotation": 36.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sweep P2.path b/src/main/deploy/pathplanner/paths/Sweep P2.path new file mode 100644 index 0000000..996f28a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Sweep P2.path @@ -0,0 +1,71 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.9713092, + "y": 5.191 + }, + "prevControl": null, + "nextControl": { + "x": 4.267840909084701, + "y": 7.908934659084283 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7777840909028838, + "y": 6.602656249993373 + }, + "prevControl": { + "x": 2.183778409084702, + "y": 7.44026988635701 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5275310834813515, + "rotationDegrees": -77.19714447023723 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.3, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeCoral" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -56.976000000000006 + }, + "reversed": false, + "folder": "Ground LSide", + "idealStartingState": { + "velocity": 0, + "rotation": 150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sweep P2.path b/src/main/deploy/pathplanner/paths/Sweep P2.path new file mode 100644 index 0000000..1d79a3e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Sweep P2.path @@ -0,0 +1,87 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.157, + "y": 2.996 + }, + "prevControl": null, + "nextControl": { + "x": 4.011782786885246, + "y": 1.411649590163935 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.831931818176833, + "y": 0.5299573863636362 + }, + "prevControl": { + "x": 3.285943759901909, + "y": 0.6018426104701067 + }, + "nextControl": { + "x": 1.6353409090859239, + "y": 0.34049715909090883 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.688, + "y": 1.368 + }, + "prevControl": { + "x": 1.3261818181768328, + "y": 0.709875 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2984014209591429, + "rotationDegrees": -36.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.55, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeCoral" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -127.476 + }, + "reversed": false, + "folder": "Sweep Right Side", + "idealStartingState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sweep P3.path b/src/main/deploy/pathplanner/paths/Sweep P3.path new file mode 100644 index 0000000..f55b8e1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Sweep P3.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.688, + "y": 1.368 + }, + "prevControl": null, + "nextControl": { + "x": 2.2076919758517572, + "y": 2.8655382848833577 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0114204545454544, + "y": 2.1852414772727276 + }, + "prevControl": { + "x": 2.749638585522988, + "y": 1.9411650737696169 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Elevator", + "waypointRelativePos": 0.16647919010123857, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "reversed": false, + "folder": "Sweep Right Side", + "idealStartingState": { + "velocity": 0, + "rotation": -127.476 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sweep P4.path b/src/main/deploy/pathplanner/paths/Sweep P4.path new file mode 100644 index 0000000..4dd6a62 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Sweep P4.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.721, + "y": 3.0259168 + }, + "prevControl": null, + "nextControl": { + "x": 2.6805705142397143, + "y": 1.93927176905575 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.722, + "y": 0.923 + }, + "prevControl": { + "x": 2.6732300707441192, + "y": 1.959652915018721 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.19856459330143417, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeCoral" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -36.0 + }, + "reversed": false, + "folder": "Sweep Right Side", + "idealStartingState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sweep RP5.path b/src/main/deploy/pathplanner/paths/Sweep RP5.path new file mode 100644 index 0000000..f528baf --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Sweep RP5.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.722, + "y": 0.923 + }, + "prevControl": null, + "nextControl": { + "x": 3.1545445192827195, + "y": 2.1583408880242017 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.00733, + "y": 2.86082 + }, + "prevControl": { + "x": 3.482655572777105, + "y": 2.4255236965395213 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "MoveElevator", + "waypointRelativePos": 0.39370078740157555, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "reversed": false, + "folder": "Sweep Right Side", + "idealStartingState": { + "velocity": 0, + "rotation": -36.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepGroundB #5.path b/src/main/deploy/pathplanner/paths/SweepGroundB #5.path new file mode 100644 index 0000000..a6493ce --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SweepGroundB #5.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.808, + "y": 6.57 + }, + "prevControl": null, + "nextControl": { + "x": 3.599744318181818, + "y": 6.393252840909091 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.2385, + "y": 4.191 + }, + "prevControl": { + "x": 2.2385, + "y": 4.191 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "MoveElevator", + "waypointRelativePos": 0.6806256151574805, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.89200324814249 + }, + "reversed": false, + "folder": "Ground BSide", + "idealStartingState": { + "velocity": 0, + "rotation": -53.616 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..c606f42 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,41 @@ +{ + "robotWidth": 0.832, + "robotLength": 0.832, + "holonomicMode": true, + "pathFolders": [ + "Ground BSide", + "Ground LSide", + "Station RSide", + "Lollipop Path", + "Station Left", + "Sweep Right Side" + ], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 58.9670081, + "robotMOI": 7.504, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.05, + "driveGearing": 5.903, + "maxDriveSpeed": 5.41, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 52.0, + "wheelCOF": 0.9, + "flModuleX": 0.2635, + "flModuleY": 0.2635, + "frModuleX": 0.2635, + "frModuleY": -0.2635, + "blModuleX": -0.2635, + "blModuleY": 0.2635, + "brModuleX": -0.2635, + "brModuleY": -0.2635, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [ + "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.0,\"y\":-0.55},\"size\":{\"width\":0.241,\"length\":0.5969},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}" + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 8776e5d..e9c0192 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -12,14 +12,15 @@ import edu.wpi.first.wpilibj.RobotBase; * call. */ public final class Main { - private Main() {} + private Main() { + } - /** - * Main initialization function. Do not perform any initialization here. - * - *

If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 477e691..a7d87ff 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,15 +4,26 @@ package frc.robot; +import java.util.Optional; + +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; + +import au.grapplerobotics.CanBridge; +import edu.wpi.first.net.PortForwarder; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.controls.Driver; -import frc.robot.controls.Operator; -import frc.robot.subsystems.Drivetrain; -import frc.robot.util.PathGroupLoader; -import frc.robot.util.ShuffleboardManager; +import frc.robot.constants.Constants; +import frc.robot.constants.VisionConstants; +import frc.robot.constants.swerve.DriveConstants; +import frc.robot.util.BuildData; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -20,119 +31,191 @@ import frc.robot.util.ShuffleboardManager; * the package after creating this project, you must also update the build.gradle file in the * project. */ -public class Robot extends TimedRobot { - private Command m_autoCommand; - public static ShuffleboardManager shuffleboard; - public static Drivetrain drive; - - private static boolean isTestMode = false; - - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ - @Override - public void robotInit() { - - // This is really annoying so it's disabled - DriverStation.silenceJoystickConnectionWarning(true); - - // load paths before auto starts - PathGroupLoader.loadPathGroups(); - - // make subsystems - shuffleboard = new ShuffleboardManager(); - drive = new Drivetrain(); - - shuffleboard.setup(); - - Driver.configureControls(); - Operator.configureControls(); - } - - /** - * This function is called every robot packet, no matter the mode. Use this for items like - * diagnostics that you want ran during disabled, autonomous, teleoperated and test. - * - *

This runs after the mode specific periodic functions, but before LiveWindow and - * SmartDashboard integrated updating. - */ - @Override - public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic - // block in order for anything in the Command-based framework to work. - CommandScheduler.getInstance().run(); - } - - /** This function is called once each time the robot enters Disabled mode. */ - @Override - public void disabledInit() { - CommandScheduler.getInstance().cancelAll(); - isTestMode = false; - } - - @Override - public void disabledPeriodic() { - m_autoCommand = getAutonomousCommand(); - } - - /** This autonomous runs the autonomous command selected by your {@link Robot} class. */ - @Override - public void autonomousInit() { - isTestMode = false; - if (m_autoCommand != null) { - m_autoCommand.schedule(); +public class Robot extends LoggedRobot { + private Command autoCommand; + private RobotContainer robotContainer; + + public Robot(){ + CanBridge.runTCP(); + PortForwarder.add(5800,"10.9.72.12",5800); + PortForwarder.add(1182,"10.9.72.12",1182); + + // Set up data receivers & replay source + switch (Constants.CURRENT_MODE) { + case REAL: + // Running on a real robot, log to a USB stick ("/U/logs") + Logger.addDataReceiver(new WPILOGWriter()); + Logger.addDataReceiver(new NT4Publisher()); + break; + + case SIM: + // Running a physics simulator, log to NT + Logger.addDataReceiver(new NT4Publisher()); + break; + + case REPLAY: + // Replaying a log, set up replay source + setUseTiming(false); // Run as fast as possible + String logPath = LogFileUtil.findReplayLog(); + Logger.setReplaySource(new WPILOGReader(logPath)); + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); + break; + } + Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may be added. + } + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // To Set the Robot Identity + // SimGUI: Persistent Values, Preferences, RobotId, then restart Simulation + // changes networktables.json, networktables.json.bck (both Untracked) + // Uncomment the next line, set the desired RobotId, deploy, and then comment the line out + // RobotId.setRobotId(RobotId.SwerveCompetition); + DriveConstants.update(RobotId.getRobotId()); + RobotController.setBrownoutVoltage(6.0); + // obtain this robot's identity + RobotId robotId = RobotId.getRobotId(); + + // Record metadata + Logger.recordMetadata("ProjectName", BuildData.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildData.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildData.GIT_SHA); + Logger.recordMetadata("GitDate", BuildData.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildData.GIT_BRANCH); + switch (BuildData.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; + } + + robotContainer = new RobotContainer(robotId); + + } + + /** + * This function is called every robot packet, no matter the mode. Use this for items like + * diagnostics that you want ran during disabled, autonomous, teleoperated and test. + * + *

This runs after the mode-specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + + CommandScheduler.getInstance().run(); + + robotContainer.logComponents(); + } + + /** + * This function is called once each time the robot enters Disabled mode. + */ + @Override + public void disabledInit() { + CommandScheduler.getInstance().cancelAll(); + } + + /** + * This function is called periodically when the robot is disabled + */ + @Override + public void disabledPeriodic() { } - } - - /** This function is called periodically during autonomous. */ - @Override - public void autonomousPeriodic() {} - - @Override - public void teleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. - if (m_autoCommand != null) { - m_autoCommand.cancel(); + + /** + * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. + */ + @Override + public void autonomousInit() { + // Disable vision if the constant is false. + robotContainer.setVisionEnabled(VisionConstants.ENABLED_AUTO); + + // Get the autonomous command. + // This access is fast (about 14 microseconds) because the value is already resident in the Network Tables. + // There was a problem last year because the operation also installed about over a dozen items (taking more than 20 ms). + autoCommand = robotContainer.getAutoCommand(); + + // If there is an autonomous command, then schedule it + if (autoCommand != null) { + autoCommand.schedule(); + } } - isTestMode = false; - } - /** This function is called periodically during operator control. */ - @Override - public void teleopPeriodic() {} + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + } - @Override - public void testInit() { - // Cancels all running commands at the start of test mode. - CommandScheduler.getInstance().cancelAll(); - // it may be needed to disable LiveWindow (we don't use it anyway) - //LiveWindow.setEnabled(false) + /** + * This function is called once each time the robot enters Teleop mode. + */ + @Override + public void teleopInit() { + robotContainer.setVisionEnabled(true); + + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (autoCommand != null) { + autoCommand.cancel(); + } + } - isTestMode = true; + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + } - } + /** + * This function is called once each time the robot enters Test mode. + */ + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } - /** This function is called periodically during test mode. */ - @Override - public void testPeriodic() {} + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - return shuffleboard.getAutonomousCommand(); - } + @Override + public void simulationPeriodic() { + } - public static boolean isTestMode() { - return isTestMode; - } + /** + * Gets the set Alliance; defaults to red if not set. + * This method replaces {@link edu.first.wpilibj.DriverStation.getAlliance}. + * The .get() is not necessary, so DriverStation.getAlliance().get() becomes Robot.getAlliance() + */ + public static Alliance getAlliance() { + Optional dsAlliance = DriverStation.getAlliance(); + if (dsAlliance.isPresent()) + return dsAlliance.get(); + else + return Alliance.Red; // default to Red alliance + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java new file mode 100644 index 0000000..06b0fd4 --- /dev/null +++ b/src/main/java/frc/robot/RobotContainer.java @@ -0,0 +1,435 @@ +package frc.robot; + +import java.io.IOException; +import java.util.function.BooleanSupplier; + +import org.json.simple.parser.ParseException; +import org.littletonrobotics.junction.Logger; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.commands.PathPlannerAuto; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.DoNothing; +import frc.robot.commands.drive_comm.DefaultDriveCommand; +import frc.robot.commands.drive_comm.DriveToPose; +import frc.robot.commands.gpm.IntakeCoral; +import frc.robot.commands.gpm.MoveArm; +import frc.robot.commands.gpm.MoveElevator; +import frc.robot.commands.gpm.OuttakeCoral; +import frc.robot.commands.gpm.StationIntake; +import frc.robot.constants.ArmConstants; +import frc.robot.constants.AutoConstants; +import frc.robot.constants.Constants; +import frc.robot.constants.ElevatorConstants; +import frc.robot.constants.FieldConstants; +import frc.robot.constants.IntakeConstants; +import frc.robot.constants.VisionConstants; +import frc.robot.controls.BaseDriverConfig; +import frc.robot.controls.Operator; +import frc.robot.controls.PS5ControllerDriverConfig; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.climb.Climb; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.drivetrain.GyroIOPigeon2; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.outtake.Outtake; +import frc.robot.subsystems.outtake.OuttakeAlpha; +import frc.robot.subsystems.outtake.OuttakeComp; +import frc.robot.util.PathGroupLoader; +import frc.robot.util.Vision.DetectedObject; +import frc.robot.util.Vision.Vision; + +/** + * This class is where the bulk of the robot should be declared. Since + * Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in + * the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of + * the robot (including + * subsystems, commands, and trigger mappings) should be declared here. + */ +public class RobotContainer { + // The robot's subsystems are defined here... + private Drivetrain drive = null; + private Vision vision = null; + private Intake intake = null; + private Indexer indexer = null; + private Outtake outtake = null; + private Elevator elevator = null; + private Climb climb = null; + private Arm arm = null; + private Command auto = new DoNothing(); + + // Dashboard inputs + // private final LoggedDashboardChooser autoChooser; + + // Controllers are defined here + private BaseDriverConfig driver = null; + @SuppressWarnings("unused") + private Operator operator = null; + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + *

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

The name of a robot in the RoboRIO's persistent memory. + * At deploy time, that name is used to set the corresponding RobotId. + *

Note that the RobotId is determined at Deploy time. + */ +public enum RobotId { + Default, + SwerveCompetition, Vertigo, Vivace, Phil, BetaBot, + ClassBot1, ClassBot2, ClassBot3, ClassBot4, + TestBed1, TestBed2; + + /** The key used to access the RobotId name in the RoboRIO's persistent memory. */ + public static final String ROBOT_ID_KEY = "RobotId"; + + /** + * Is this robot a classbot? + * @return true if a classbot + * @deprecated this method is not needed.... + */ + @Deprecated + public boolean isClassBot() { + return this == ClassBot1 || this == ClassBot2 || this == ClassBot3 || this == ClassBot4; + } + + /** + * Whether this robot is a swerve bot + * @return true if a swerve bot + * @deprecated this method is not needed.... + */ + @Deprecated + public boolean isSwerveBot() { + return this == SwerveCompetition || this == Phil || this == Vertigo || this == Vivace || this == BetaBot; + } + + /** + * Determine the Robot Identity from the RoboRIO's onboard Preferences (flash memory). + * @returns the RobotId + */ + public static RobotId getRobotId() { + // assume a default identity + RobotId robotId = RobotId.Default; + + // check whether Preferences has an entry for the RobotId + if (!Preferences.containsKey(ROBOT_ID_KEY)) { + // There is no such key. Set it to the default identity. + // This step guarantees persistent memory will have a key. + setRobotId(RobotId.Default); + } + + // Remove the "Default" key if present. + // This key was the result of a programming error in 2023. + if (Preferences.containsKey("Default")) { + Preferences.remove("Default"); + } + + // get the RobotId string from the RoboRIO's Preferences + String strId = Preferences.getString(ROBOT_ID_KEY, RobotId.Default.name()); + + // match that string to a RobotId by looking at all possible RobotId enums + for (RobotId rid : RobotId.values()) { + // does the preference string match the RobotId enum? + if (strId.equals(rid.name())) { + // yes, this instance is the desired RobotId + robotId = rid; + break; + } + } + + // report the RobotId to the SmartDashboard. + //SmartDashboard.putString("RobotID", robotId.name()); + + // return the robot identity + return robotId; + } + + /** + * Set the RobotId in the RoboRIO's preferences (flash memory). + *

+ * Calling it after the robot has been constructed (robotInit()) does not affect the robot. + */ + static void setRobotId(RobotId robotId) { + // Set the robot identity in the RoboRIO Preferences + Preferences.setString(ROBOT_ID_KEY, robotId.name()); + } +} diff --git a/src/main/java/frc/robot/commands/DoNothing.java b/src/main/java/frc/robot/commands/DoNothing.java index 4648cef..c6c0c08 100644 --- a/src/main/java/frc/robot/commands/DoNothing.java +++ b/src/main/java/frc/robot/commands/DoNothing.java @@ -2,4 +2,8 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.InstantCommand; -public class DoNothing extends InstantCommand {} +/** + * Does nothing. Can be used to more clearly mark commands intended not to do anything. + */ +public class DoNothing extends InstantCommand { +} diff --git a/src/main/java/frc/robot/commands/SupplierCommand.java b/src/main/java/frc/robot/commands/SupplierCommand.java new file mode 100644 index 0000000..b173bd1 --- /dev/null +++ b/src/main/java/frc/robot/commands/SupplierCommand.java @@ -0,0 +1,50 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; + +import java.util.function.Supplier; + +/** + * Runs the given command when this command is initialized, and ends when it ends. + * Useful for commands that are not created yet because the constructor parameters are not available until initialization. + */ +public class SupplierCommand extends Command { + + private final Supplier commandSupplier; + private Command command; + + /** + * Runs the given command when this command is initialized, and ends when it ends. + * Useful for commands that are not created yet because the constructor parameters are not available until initialization. + * + * @param commandSupplier A Supplier to the command to run + * @param Subsystem all subsystems that may be required to run the command supplied + */ + public SupplierCommand(Supplier commandSupplier, Subsystem... Subsystem) { + addRequirements(Subsystem); + this.commandSupplier = commandSupplier; + } + + @Override + public final void initialize() { + command = commandSupplier.get(); + command.initialize(); + } + + @Override + public final void execute() { + command.execute(); + } + + @Override + public final void end(boolean interrupted) { + command.end(interrupted); + } + + @Override + public final boolean isFinished() { + return command.isFinished(); + } + +} diff --git a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java new file mode 100644 index 0000000..98e5d01 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java @@ -0,0 +1,37 @@ +package frc.robot.commands.auto_comm; + +import choreo.auto.AutoFactory; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.DoNothing; +import frc.robot.subsystems.drivetrain.Drivetrain; + +public class ChoreoPathCommand extends SequentialCommandGroup { + private static AutoFactory factory; + + public ChoreoPathCommand(String pathName, boolean resetOdemetry, Drivetrain drive) { + if(factory == null){ + factory = new AutoFactory( + drive::getPose, + drive::resetOdometry, + sample -> drive.setChassisSpeeds(sample.getChassisSpeeds(), false), + true, + drive, + (trajectory, bool)->{ + if(bool){ + + }else{ + + } + } + ); + } + + var command = factory.trajectoryCmd(pathName); + + addCommands( + resetOdemetry ? new InstantCommand(()->factory.resetOdometry(pathName)) : new DoNothing(), + command + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto_comm/FollowPathCommand.java b/src/main/java/frc/robot/commands/auto_comm/FollowPathCommand.java new file mode 100644 index 0000000..a624ab2 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto_comm/FollowPathCommand.java @@ -0,0 +1,48 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.auto_comm; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.RobotContainer; +import frc.robot.commands.SupplierCommand; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.util.PathGroupLoader; + +/** Add your docs here. */ +public class FollowPathCommand extends SequentialCommandGroup { + Drivetrain drive; + PathPlannerPath path; + + + public FollowPathCommand(String name, Drivetrain drive){ + this(name, false, drive); + } + + public FollowPathCommand(String pathName, boolean resetOdemetry, Drivetrain drive){ + this.drive = drive; + this.path = PathGroupLoader.getPathGroup(pathName); + addCommands( + new InstantCommand(()->resetOdemetry(resetOdemetry)), + new SupplierCommand(()->AutoBuilder.followPath(path), drive) // "problem" (254) + // or pp's interaction with the drivetrain + // or pp config + ); + } + + public void resetOdemetry(boolean resetOdemetry){ + if (resetOdemetry){ + if(RobotContainer.getAllianceColorBooleanSupplier().getAsBoolean()){ + drive.resetOdometry(new Pose2d(path.getAllPathPoints().get(0).flip().position, path.getIdealStartingState().flip().rotation())); + }else{ + drive.resetOdometry(new Pose2d(path.getAllPathPoints().get(0).position, path.getIdealStartingState().rotation())); + } + } + } +} diff --git a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java new file mode 100644 index 0000000..0ea7bf8 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java @@ -0,0 +1,71 @@ +package frc.robot.commands.drive_comm; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Robot; +import frc.robot.constants.swerve.DriveConstants; +import frc.robot.controls.BaseDriverConfig; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.util.Vision.DriverAssist; + + +/** + * Default drive command. Drives robot using driver controls. + */ +public class DefaultDriveCommand extends Command { + private final Drivetrain swerve; + private final BaseDriverConfig driver; + + public DefaultDriveCommand( + Drivetrain swerve, + BaseDriverConfig driver) { + this.swerve = swerve; + this.driver = driver; + + addRequirements(swerve); + } + + @Override + public void initialize() { + swerve.setStateDeadband(true); + } + + @Override + public void execute() { + double forwardTranslation = driver.getForwardTranslation(); + double sideTranslation = driver.getSideTranslation(); + double rotation = -driver.getRotation(); + + double slowFactor = driver.getIsSlowMode() ? DriveConstants.SLOW_DRIVE_FACTOR : 1; + + forwardTranslation *= slowFactor; + sideTranslation *= slowFactor; + rotation *= driver.getIsSlowMode() ? DriveConstants.SLOW_ROT_FACTOR : 1; + + int allianceReversal = Robot.getAlliance() == Alliance.Red ? 1 : -1; + forwardTranslation *= allianceReversal; + sideTranslation *= allianceReversal; + + ChassisSpeeds driverInput = new ChassisSpeeds(forwardTranslation, sideTranslation, rotation); + ChassisSpeeds corrected = DriverAssist.calculate(swerve, driverInput, swerve.getDesiredPose(), true); + + // If the driver is pressing the align button or a command set the drivetrain to + // align, then align to speaker + if (driver.getIsAlign() || swerve.getIsAlign()) { + swerve.driveHeading( + forwardTranslation, + sideTranslation, + swerve.getAlignAngle(), + true); + } else { + swerve.drive( + corrected.vxMetersPerSecond, + corrected.vyMetersPerSecond, + corrected.omegaRadiansPerSecond, + true, + false); + } + } +} + diff --git a/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java b/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java new file mode 100644 index 0000000..bab477d --- /dev/null +++ b/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java @@ -0,0 +1,207 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.commands.drive_comm; + +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.Constants; +import frc.robot.constants.VisionConstants; +import frc.robot.constants.swerve.DriveConstants; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.util.GeomUtil; + +public class DriveToPose extends Command { + protected static boolean updateTarget = false; + private static final double drivekP = 5.0; + private static final double drivekD = 0.0; + private static final double thetakP = 7.0; + private static final double thetakD = 0.0; + private static final double driveMaxVelocity = DriveConstants.MAX_SPEED; + private static final double driveMaxAcceleration = 2.6; + private static final double thetaMaxVelocity = 5.0; + private static final double thetaMaxAcceleration = 5.0; + private static final double driveTolerance = 0.015; + private static final double thetaTolerance = Units.degreesToRadians(1.0); + private static final double ffMinRadius = 0.05; + private static final double ffMaxRadius = 0.1; + + private final Drivetrain drive; + private final Supplier target; + private Pose2d targetPose; + + private final ProfiledPIDController driveController = + new ProfiledPIDController( + drivekP, 0.0, drivekD, new TrapezoidProfile.Constraints(driveMaxVelocity, driveMaxAcceleration), Constants.LOOP_TIME); + private final ProfiledPIDController thetaController = + new ProfiledPIDController( + thetakP, 0.0, thetakD, new TrapezoidProfile.Constraints(thetaMaxVelocity, thetaMaxAcceleration), Constants.LOOP_TIME); + + private Translation2d lastSetpointTranslation = new Translation2d(); + private double driveErrorAbs = 0.0; + private double thetaErrorAbs = 0.0; + private boolean running = false; + private Supplier robot; + + private Supplier linearFF = () -> Translation2d.kZero; + private DoubleSupplier omegaFF = () -> 0.0; + + private Debouncer debouncer = new Debouncer(0.2); + + public DriveToPose(Drivetrain drive, Supplier target) { + this.drive = drive; + this.target = target; + robot = drive::getPose; + + // Set tolerance + driveController.setTolerance(driveTolerance); + thetaController.setTolerance(thetaTolerance); + + // Enable continuous input for theta controller + thetaController.enableContinuousInput(-Math.PI, Math.PI); + + addRequirements(drive); + } + + public DriveToPose( + Drivetrain drive, + Supplier target, + Supplier linearFF, + DoubleSupplier omegaFF) { + this(drive, target); + this.linearFF = linearFF; + this.omegaFF = omegaFF; + } + + @Override + public void initialize() { + drive.setVisionEnabled(VisionConstants.ENABLED_GO_TO_POSE); + + targetPose = target.get(); + Pose2d currentPose = robot.get(); + ChassisSpeeds fieldVelocity = ChassisSpeeds.fromRobotRelativeSpeeds(drive.getChassisSpeeds(), currentPose.getRotation()); + Translation2d linearFieldVelocity = + new Translation2d(fieldVelocity.vxMetersPerSecond, fieldVelocity.vyMetersPerSecond); + + thetaController.reset( + currentPose.getRotation().getRadians(), fieldVelocity.omegaRadiansPerSecond); + lastSetpointTranslation = currentPose.getTranslation(); + + if(targetPose != null){ + driveController.reset( + currentPose.getTranslation().getDistance(target.get().getTranslation()), + -linearFieldVelocity + .rotateBy( + targetPose + .getTranslation() + .minus(currentPose.getTranslation()) + .getAngle() + .unaryMinus()) + .getX()); + } + } + + @Override + public void execute() { + running = true; + + // Get current pose and target pose + Pose2d currentPose = robot.get(); + if(updateTarget){ + targetPose = target.get(); + } + if(targetPose == null){ + return; + } + + // Calculate drive speed + double currentDistance = currentPose.getTranslation().getDistance(targetPose.getTranslation()); + double ffScaler = + MathUtil.clamp( + (currentDistance - ffMinRadius) / (ffMaxRadius - ffMinRadius), + 0.0, + 1.0); + driveErrorAbs = currentDistance; + driveController.reset( + lastSetpointTranslation.getDistance(targetPose.getTranslation()), + driveController.getSetpoint().velocity); + double driveVelocityScalar = + driveController.getSetpoint().velocity * ffScaler + + driveController.calculate(driveErrorAbs, 0.0); + if (currentDistance < driveController.getPositionTolerance()) driveVelocityScalar = 0.0; + lastSetpointTranslation = + new Pose2d( + targetPose.getTranslation(), + currentPose.getTranslation().minus(targetPose.getTranslation()).getAngle()) + .transformBy(GeomUtil.toTransform2d(driveController.getSetpoint().position, 0.0)) + .getTranslation(); + + // Calculate theta speed + double thetaVelocity = + thetaController.getSetpoint().velocity * ffScaler + + thetaController.calculate( + currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians()); + thetaErrorAbs = + Math.abs(currentPose.getRotation().minus(targetPose.getRotation()).getRadians()); + if (thetaErrorAbs < thetaController.getPositionTolerance()) thetaVelocity = 0.0; + + Translation2d driveVelocity = + new Pose2d( + new Translation2d(), + currentPose.getTranslation().minus(targetPose.getTranslation()).getAngle()) + .transformBy(GeomUtil.toTransform2d(driveVelocityScalar, 0.0)) + .getTranslation(); + + // Scale feedback velocities by input ff + final double linearS = linearFF.get().getNorm() * 3.0; + final double thetaS = Math.abs(omegaFF.getAsDouble()) * 3.0; + driveVelocity = + driveVelocity.interpolate(linearFF.get().times(DriveConstants.MAX_SPEED), linearS); + thetaVelocity = + MathUtil.interpolate( + thetaVelocity, omegaFF.getAsDouble() * DriveConstants.MAX_ANGULAR_SPEED, thetaS); + + // Command speeds + drive.drive(driveVelocity.getX(), driveVelocity.getY(), thetaVelocity, true, false); + } + + @Override + public void end(boolean interrupted) { + drive.stop(); + drive.setVisionEnabled(true); + running = false; + } + + /** Checks if the robot is stopped at the final pose. */ + public boolean atGoal() { + return running && (driveController.atGoal() && thetaController.atGoal() || targetPose == null); + } + + /** Checks if the robot pose is within the allowed drive and theta tolerances. */ + public boolean withinTolerance(double driveTolerance, Rotation2d thetaTolerance) { + return running + && (Math.abs(driveErrorAbs) < driveTolerance + && Math.abs(thetaErrorAbs) < thetaTolerance.getRadians() + || targetPose == null); + } + + @Override + public boolean isFinished(){ + return debouncer.calculate(withinTolerance(driveTolerance, new Rotation2d(thetaTolerance))); + } +} diff --git a/src/main/java/frc/robot/commands/drive_comm/GoToPose.java b/src/main/java/frc/robot/commands/drive_comm/GoToPose.java new file mode 100644 index 0000000..e2afa7a --- /dev/null +++ b/src/main/java/frc/robot/commands/drive_comm/GoToPose.java @@ -0,0 +1,89 @@ +package frc.robot.commands.drive_comm; + +import java.util.function.Supplier; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.DoNothing; +import frc.robot.commands.SupplierCommand; +import frc.robot.constants.AutoConstants; +import frc.robot.constants.VisionConstants; +import frc.robot.constants.swerve.DriveConstants; +import frc.robot.subsystems.drivetrain.Drivetrain; + +/** +* Moves the robot to a pose using PathPlanner +*/ +public class GoToPose extends SequentialCommandGroup { + + private Drivetrain drive; + private Supplier poseSupplier; + private double maxSpeed; + private double maxAccel; + + /** + * Uses PathPlanner to go to a pose + * @param poseSupplier The supplier for the pose to use + * @param drive The drivetrain + */ + public GoToPose(Supplier poseSupplier, Drivetrain drive) { + this(poseSupplier, AutoConstants.MAX_AUTO_SPEED, AutoConstants.MAX_AUTO_ACCEL, drive); + } + public GoToPose(Pose2d pose, Drivetrain drive){ + this(()->pose, drive); + } + + /** + * Uses PathPlanner to go to a pose + * @param poseSupplier The supplier for the pose to use + * @param maxSpeed The maximum speed to use + * @param maxAccel The maximum acceleration to use + * @param drive The drivetrain + */ + public GoToPose(Supplier poseSupplier, double maxSpeed, double maxAccel, Drivetrain drive) { + this.poseSupplier = poseSupplier; + this.maxSpeed = maxSpeed; + this.maxAccel = maxAccel; + this.drive = drive; + addCommands( + new InstantCommand(()->drive.setVisionEnabled(VisionConstants.ENABLED_GO_TO_POSE)), + new SupplierCommand(() -> createCommand(), drive).handleInterrupt(()->drive.setVisionEnabled(true)), + new InstantCommand(()->drive.setVisionEnabled(true)) + ); + } + + /** + * Creates the PathPlanner command and schedules it + */ + public Command createCommand() { + Pose2d pose = poseSupplier.get(); + if(pose==null){ + return new DoNothing(); + } + Command command = AutoBuilder.pathfindToPose( + pose, + new PathConstraints(maxSpeed, maxAccel, DriveConstants.MAX_ANGULAR_SPEED, DriveConstants.MAX_ANGULAR_ACCEL), + 0 + ); + + // get the distance to the pose. + double dist = drive.getPose().minus(pose).getTranslation().getNorm(); + + // if greater than 3m or less than 2 cm, don't run it. If the path is too small pathplanner makes weird paths. + if (dist > 3) { + command = new DoNothing(); + DriverStation.reportWarning("Alignment Path too long, doing nothing, GoToPose.java", false); + } else if (dist < 0.02) { + command = new DoNothing(); + DriverStation.reportWarning("Alignment Path too short, doing nothing, GoToPose.java", false); + } + + return command; + } +} diff --git a/src/main/java/frc/robot/commands/drive_comm/GoToPosePID.java b/src/main/java/frc/robot/commands/drive_comm/GoToPosePID.java new file mode 100644 index 0000000..cb49203 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive_comm/GoToPosePID.java @@ -0,0 +1,61 @@ +package frc.robot.commands.drive_comm; + +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.VisionConstants; +import frc.robot.subsystems.drivetrain.Drivetrain; + +/** + * Runs the chassis PIDs to move the robot to a specific pose. + */ +public class GoToPosePID extends Command { + + private Drivetrain drive; + + private Supplier poseSupplier; + private Pose2d pose; + + /** + * Runs the chassis PIDs to move the robot to a specific pose. + * @param pose The pose supplier to go to + * @param drive The drivetrain + */ + public GoToPosePID(Supplier pose, Drivetrain drive) { + this.drive = drive; + this.poseSupplier = pose; + + addRequirements(drive); + } + + public GoToPosePID(Pose2d pose, Drivetrain drive){ + this(()->pose, drive); + } + + @Override + public void initialize() { + pose = poseSupplier.get(); + drive.setVisionEnabled(VisionConstants.ENABLED_GO_TO_POSE); + } + + @Override + public void execute() { + if(pose == null) { + return; + } + + drive.driveWithPID(pose.getX(), pose.getY(), pose.getRotation().getRadians()); + } + + @Override + public void end(boolean interrupted) { + drive.stop(); + drive.setVisionEnabled(true); + } + + @Override + public boolean isFinished() { + return pose == null || drive.getXController().atSetpoint() && drive.getYController().atSetpoint() && drive.getRotationController().atSetpoint(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/drive_comm/SetFormationX.java b/src/main/java/frc/robot/commands/drive_comm/SetFormationX.java new file mode 100644 index 0000000..22dc905 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive_comm/SetFormationX.java @@ -0,0 +1,27 @@ +package frc.robot.commands.drive_comm; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.drivetrain.Drivetrain; + +/** + * Sets the robot's wheels to an X formation to prevent being pushed around by other bots. + */ +public class SetFormationX extends SequentialCommandGroup { + public SetFormationX(Drivetrain drive) { + addRequirements(drive); + addCommands( + new InstantCommand(() -> drive.setStateDeadband(false), drive), + new RunCommand(() -> drive.setModuleStates(new SwerveModuleState[]{ + new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(45))), + new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(-45))), + new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(-45))), + new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(45))) + }, false), drive) + ); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/drive_comm/SimplePresetSteerAngles.java b/src/main/java/frc/robot/commands/drive_comm/SimplePresetSteerAngles.java new file mode 100644 index 0000000..74c2e43 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive_comm/SimplePresetSteerAngles.java @@ -0,0 +1,51 @@ +package frc.robot.commands.drive_comm; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.drivetrain.Drivetrain; + +/** + * Attempts to set all four modules to a constant angle. Determines if the modules are able to reach the angle requested in a certain time. + */ +public class SimplePresetSteerAngles extends InstantCommand { + + /** + * sets the angle of module steer to 0 to remove initial turn time and drift + * + * @param drive drivetrain to be used + */ + public SimplePresetSteerAngles(Drivetrain drive) { + this(drive, new Rotation2d()); + } + + /** + * sets the angle of module steer to a angle to remove initial turn time and drift + * + * @param angle angle to set module steer to in radians + * @param drive drivetrain to be used + */ + public SimplePresetSteerAngles(Drivetrain drive, double angle) { + this(drive, new Rotation2d(angle)); + } + + /** + * sets the angle of module steer to a angle to remove initial turn time and drift + * + * @param rotation rotation to set module steer to + * @param drive drivetrain to be used + */ + public SimplePresetSteerAngles(Drivetrain drive, Rotation2d rotation) { + super(() -> { + drive.setStateDeadband(false); + drive.setModuleStates(new SwerveModuleState[]{ + new SwerveModuleState(0, rotation), + new SwerveModuleState(0, rotation), + new SwerveModuleState(0, rotation), + new SwerveModuleState(0, rotation) + }, true); + drive.setStateDeadband(true); + }, drive); + drive.setStateDeadband(true); + } +} diff --git a/src/main/java/frc/robot/commands/drive_comm/SysIDDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/SysIDDriveCommand.java new file mode 100644 index 0000000..17bb229 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive_comm/SysIDDriveCommand.java @@ -0,0 +1,60 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.drive_comm; + + +import com.ctre.phoenix6.SignalLogger; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.util.SysId; + +/** + * A command to run all 4 SysId routines on the drivetrain +*/ +public class SysIDDriveCommand extends SequentialCommandGroup { + + private Config config = new Config(); + private SysId sysId; + public SysIDDriveCommand(Drivetrain drive) { + config = new Config( + Units.Volts.of(0.5).per(Units.Seconds), + Units.Volts.of(3), + Units.Seconds.of(5), + (x)->SignalLogger.writeString("state", x.toString()) + ); + Rotation2d[] angles = { + Rotation2d.fromDegrees(0),//-45-180 + Rotation2d.fromDegrees(0),//45 + Rotation2d.fromDegrees(0),//45+180 + Rotation2d.fromDegrees(0),//-45 + }; + sysId = new SysId( + "Drivetrain", + x ->{ + drive.setAngleMotors(angles); + drive.setDriveVoltages(x); + }, + drive, + config + ); + addCommands( + sysId.runQuasisStatic(Direction.kForward), + new WaitCommand(0.5), + sysId.runQuasisStatic(Direction.kReverse), + new WaitCommand(0.5), + sysId.runDynamic(Direction.kForward), + new WaitCommand(0.5), + sysId.runDynamic(Direction.kReverse) + ); + } + + + +} diff --git a/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java b/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java new file mode 100644 index 0000000..393712a --- /dev/null +++ b/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java @@ -0,0 +1,51 @@ +package frc.robot.commands.drive_comm; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.Trajectory.State; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.constants.swerve.DriveConstants; +import frc.robot.subsystems.drivetrain.Drivetrain; + +/** + * Sets all module angles to a given trajectory's initial angle. + */ +public class TrajectoryPresetSteerAngles extends InstantCommand { + /* + * make sure to add wait command after called to give time to correct + */ + public TrajectoryPresetSteerAngles(Drivetrain drive, Trajectory trajectory) { + super( + () -> { + + // 0.01 is the time between trajectory samples, in seconds + // Can be replaced for any small number, but it should be the same as the time between all uses + double time = 0.01; + + drive.setStateDeadband(false); + + Pose2d initialPose = trajectory.getInitialPose(); + State sample = trajectory.sample(time); + Pose2d nextPose = sample.poseMeters; + + double xVelocity = sample.velocityMetersPerSecond * nextPose.getRotation().getCos(); + double yVelocity = sample.velocityMetersPerSecond * nextPose.getRotation().getSin(); + double angularVelo = (nextPose.getRotation().getRadians() - initialPose.getRotation().getRadians()) / time; + + ChassisSpeeds chassisSpeeds = new ChassisSpeeds(xVelocity, yVelocity, angularVelo); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds, initialPose.getRotation()); + + SwerveModuleState[] swerveModuleStates = DriveConstants.KINEMATICS.toSwerveModuleStates(chassisSpeeds); + for (SwerveModuleState swerveModuleState : swerveModuleStates) { + swerveModuleState.speedMetersPerSecond = 0; + } + drive.setModuleStates(swerveModuleStates, true); + drive.setStateDeadband(true); + }, + drive + ); + + } +} diff --git a/src/main/java/frc/robot/commands/gpm/IntakeAlgae.java b/src/main/java/frc/robot/commands/gpm/IntakeAlgae.java new file mode 100644 index 0000000..0b38ce3 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/IntakeAlgae.java @@ -0,0 +1,26 @@ +package frc.robot.commands.gpm; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.IntakeConstants; +import frc.robot.subsystems.intake.Intake; + +public class IntakeAlgae extends Command { + private final Intake intake; + + public IntakeAlgae(Intake intake) { + this.intake = intake; + addRequirements(intake); + } + + @Override + public void initialize() { + intake.setAngle(IntakeConstants.ALGAE_SETPOINT); + intake.setSpeed(IntakeConstants.ALGAE_INTAKE_POWER); + } + + @Override + public void end(boolean interrupted) { + intake.deactivate(); + intake.setSpeed(-0.05); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/gpm/IntakeAlgaeArm.java b/src/main/java/frc/robot/commands/gpm/IntakeAlgaeArm.java new file mode 100644 index 0000000..5ddef9b --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/IntakeAlgaeArm.java @@ -0,0 +1,23 @@ +package frc.robot.commands.gpm; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.outtake.Outtake; + +public class IntakeAlgaeArm extends Command { + private final Outtake outtake; + + public IntakeAlgaeArm(Outtake outtake) { + this.outtake = outtake; + addRequirements(outtake); + } + + @Override + public void initialize() { + outtake.intakeAlgaeReef(); + } + + @Override + public void end(boolean interrupted) { + outtake.setMotor(-0.1); + } +} diff --git a/src/main/java/frc/robot/commands/gpm/IntakeCoral.java b/src/main/java/frc/robot/commands/gpm/IntakeCoral.java new file mode 100644 index 0000000..2b02329 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/IntakeCoral.java @@ -0,0 +1,28 @@ +package frc.robot.commands.gpm; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.constants.ArmConstants; +import frc.robot.constants.ElevatorConstants; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.outtake.Outtake; + +/** + * Intake a coral. + */ +public class IntakeCoral extends SequentialCommandGroup { + public IntakeCoral(Intake intake, Indexer indexer, Elevator elevator, Outtake outtake, Arm arm) { + addCommands( + new ParallelCommandGroup( + new InstantCommand(() -> { + intake.unstow(); + elevator.setSetpoint(ElevatorConstants.INTAKE_SETPOINT); + arm.setSetpoint(ArmConstants.INTAKE_SETPOINT);}), + new IntakeCoralHelper(intake, indexer, outtake, arm, elevator))); + } + +} diff --git a/src/main/java/frc/robot/commands/gpm/IntakeCoralHelper.java b/src/main/java/frc/robot/commands/gpm/IntakeCoralHelper.java new file mode 100644 index 0000000..3f5159a --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/IntakeCoralHelper.java @@ -0,0 +1,106 @@ +package frc.robot.commands.gpm; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.ArmConstants; +import frc.robot.constants.ElevatorConstants; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.outtake.Outtake; + +/** + * All the intake code except the elevator stuff. + * Don't call this directly; use {@link frc.robot.commands.gpm.IntakeCoral} + * instead. + */ +public class IntakeCoralHelper extends Command { + private Intake intake; + private Indexer indexer; + private Outtake outtake; + private Arm arm; + private Elevator elevator; + + private enum Phase { + Acquiring, Intaking, Indexing, Detected, InOuttake, Done + }; + + private Phase phase; + + public IntakeCoralHelper(Intake intake, Indexer indexer, Outtake outtake, Arm arm, Elevator elevator) { + this.intake = intake; + this.indexer = indexer; + this.outtake = outtake; + this.arm = arm; + this.elevator = elevator; + addRequirements(intake, indexer, arm, elevator); + if(outtake != null){ + addRequirements(outtake); + } + } + + @Override + public void initialize() { + intake.activate(); + intake.unstow(); + indexer.run(); + phase = Phase.Acquiring; + if(outtake != null) { + outtake.setMotor(0.7); + } + } + + @Override + public void execute() { + if(outtake != null && outtake.coralLoaded()){ + phase = Phase.InOuttake; + } + switch (phase) { + case Acquiring: + case Intaking: + case Indexing: + if (!indexer.isIndexerClear()) { + phase = Phase.Detected; + intake.setSpeed(0.1); + indexer.slow(); + } + break; + case Detected: + if(indexer.isIndexerClear()){ + phase = Phase.InOuttake; + intake.deactivate(); + intake.stow(); + } + break; + case InOuttake: + if(outtake == null || outtake.coralLoaded()){ + phase = Phase.Done; + elevator.setSetpoint(ElevatorConstants.INTAKE_STOW_SETPOINT); + arm.setSetpoint(ArmConstants.STOW_SETPOINT); + indexer.stop(); + if(outtake != null){ + outtake.setMotor(0.1); + } + } + break; + case Done: + break; + } + } + + @Override + public boolean isFinished() { + return phase == Phase.Done && elevator.getPosition() > ElevatorConstants.SAFE_SETPOINT-0.025; + } + + @Override + public void end(boolean interrupted) { + // in case it's interrupted + intake.deactivate(); + intake.stow(); + indexer.stop(); + if(outtake != null){ + outtake.setMotor(0.02); + } + } +} diff --git a/src/main/java/frc/robot/commands/gpm/MoveArm.java b/src/main/java/frc/robot/commands/gpm/MoveArm.java new file mode 100644 index 0000000..1a70774 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/MoveArm.java @@ -0,0 +1,25 @@ +package frc.robot.commands.gpm; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.arm.Arm; + +public class MoveArm extends Command { + private Arm arm; + private double setpoint; + + public MoveArm(Arm arm, double setpoint) { + this.arm = arm; + this.setpoint = setpoint; + addRequirements(arm); + } + + @Override + public void initialize() { + arm.setSetpoint(setpoint); + } + + @Override + public boolean isFinished() { + return arm.atSetpoint(); + } +} diff --git a/src/main/java/frc/robot/commands/gpm/MoveElevator.java b/src/main/java/frc/robot/commands/gpm/MoveElevator.java new file mode 100644 index 0000000..200f6a8 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/MoveElevator.java @@ -0,0 +1,47 @@ +package frc.robot.commands.gpm; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.Elevator; + +/** + * Moves the elevator to a position + */ +public class MoveElevator extends Command { + private Elevator elevator; + private double setpoint; + + /** + * Creates a command to move the elevator to a position + * + * @param elevator The elevator subsystem + * @param setpoint The setpoint to move to + */ + public MoveElevator(Elevator elevator, double setpoint) { + this.elevator = elevator; + this.setpoint = setpoint; + if(elevator != null){ + addRequirements(elevator); + } + } + + /** + * Sets the elevator setpoint + */ + @Override + public void initialize() { + if(elevator != null){ + elevator.setSetpoint(setpoint); + } + } + + /** + * Returns whether the elevator is at the setpoint + * + * @return True if the elevator is within about 1 inch of the setpoint, false + * otherwise + */ + @Override + public boolean isFinished() { + return elevator == null || elevator.atSetpoint(); + } +} diff --git a/src/main/java/frc/robot/commands/gpm/NetSetpoint.java b/src/main/java/frc/robot/commands/gpm/NetSetpoint.java new file mode 100644 index 0000000..57e92ac --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/NetSetpoint.java @@ -0,0 +1,42 @@ +package frc.robot.commands.gpm; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.constants.ArmConstants; +import frc.robot.constants.ElevatorConstants; +import frc.robot.constants.FieldConstants; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.elevator.Elevator; + +public class NetSetpoint extends SequentialCommandGroup { + public NetSetpoint(Elevator elevator, Arm arm, Drivetrain drive){ + this(true, elevator, arm, drive); + } + public NetSetpoint(boolean chooseClosestSide, Elevator elevator, Arm arm, Drivetrain drive){ + addCommands( + // new InstantCommand(()->{ + // if(chooseClosestSide){ + // drive.setAlignAngle( + // useSetpoint1(drive, chooseClosestSide) == (drive.getPose().getX() < FieldConstants.FIELD_LENGTH/2) + // ? Math.PI / 2 : -Math.PI / 2 + // ); + // drive.setIsAlign(false); + // } + // }), + new ParallelCommandGroup( + new MoveArm(arm, ArmConstants.ALGAE_STOW_SETPOINT), + new MoveElevator(elevator, ElevatorConstants.NET_SETPOINT)), + new ConditionalCommand( + new MoveArm(arm, ArmConstants.ALGAE_NET_SETPOINT_1), + new MoveArm(arm, ArmConstants.ALGAE_NET_SETPOINT_2), + ()->useSetpoint1(drive, chooseClosestSide)) + ); + } + private static boolean useSetpoint1(Drivetrain drive, boolean chooseClosestSide){ + boolean positive = MathUtil.angleModulus(drive.getYaw().getRadians()) > 0; + return !chooseClosestSide || positive == (drive.getPose().getX() < FieldConstants.FIELD_LENGTH/2); + } +} diff --git a/src/main/java/frc/robot/commands/gpm/OutakeMotors.java b/src/main/java/frc/robot/commands/gpm/OutakeMotors.java new file mode 100644 index 0000000..29b2953 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/OutakeMotors.java @@ -0,0 +1,27 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.gpm; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.outtake.Outtake; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class OutakeMotors extends SequentialCommandGroup { + /** Creates a new OutakeMotors. */ + public OutakeMotors(Intake intake, Outtake outtake) { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + new InstantCommand(()->outtake.outtake(), outtake), + new WaitCommand(0.5), + new InstantCommand(()->outtake.stop(), outtake) + ); + } +} diff --git a/src/main/java/frc/robot/commands/gpm/OuttakeAlgae.java b/src/main/java/frc/robot/commands/gpm/OuttakeAlgae.java new file mode 100644 index 0000000..1bae55f --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/OuttakeAlgae.java @@ -0,0 +1,16 @@ +package frc.robot.commands.gpm; + +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import frc.robot.constants.IntakeConstants; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.outtake.Outtake; + +public class OuttakeAlgae extends ConditionalCommand { + public OuttakeAlgae(Outtake outtake, Intake intake){ + super( + new OuttakeAlgaeArm(outtake), + new OuttakeAlgaeIntake(intake), + ()-> intake.isAtSetpoint(IntakeConstants.STOW_SETPOINT) + ); + } +} diff --git a/src/main/java/frc/robot/commands/gpm/OuttakeAlgaeArm.java b/src/main/java/frc/robot/commands/gpm/OuttakeAlgaeArm.java new file mode 100644 index 0000000..6a2ec85 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/OuttakeAlgaeArm.java @@ -0,0 +1,34 @@ +package frc.robot.commands.gpm; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.outtake.Outtake; + +public class OuttakeAlgaeArm extends Command{ + private final Outtake outtake; + + private final Timer timer = new Timer(); + private final double EJECTION_TIME = 0.25; + + public OuttakeAlgaeArm(Outtake outtake) { + this.outtake = outtake; + addRequirements(outtake); + } + + @Override + public void initialize() { + outtake.outtakeAlgae(); + timer.restart(); + } + + @Override + public boolean isFinished() { + return timer.hasElapsed(EJECTION_TIME); + } + + @Override + public void end(boolean interrupted) { + outtake.stop(); + timer.stop(); + } +} diff --git a/src/main/java/frc/robot/commands/gpm/OuttakeAlgaeIntake.java b/src/main/java/frc/robot/commands/gpm/OuttakeAlgaeIntake.java new file mode 100644 index 0000000..dba0acf --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/OuttakeAlgaeIntake.java @@ -0,0 +1,38 @@ +package frc.robot.commands.gpm; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.IntakeConstants; +import frc.robot.subsystems.intake.Intake; + +public class OuttakeAlgaeIntake extends Command { + private Intake intake; + + private final Timer timer = new Timer(); + private final double EJECTION_TIME = 1; + + public OuttakeAlgaeIntake(Intake intake) { + this.intake = intake; + addRequirements(intake); + } + + @Override + public void initialize() { + intake.setSpeed(IntakeConstants.ALGAE_OUTTAKE_POWER); + intake.setAngle(50); + timer.restart(); + } + + @Override + public boolean isFinished() { + return timer.hasElapsed(EJECTION_TIME); + } + + @Override + public void end(boolean interrupted) { + intake.deactivate(); + intake.stow(); + timer.stop(); + } + +} diff --git a/src/main/java/frc/robot/commands/gpm/OuttakeCoral.java b/src/main/java/frc/robot/commands/gpm/OuttakeCoral.java new file mode 100644 index 0000000..1ea8e57 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/OuttakeCoral.java @@ -0,0 +1,24 @@ +package frc.robot.commands.gpm; + +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.constants.ElevatorConstants; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.outtake.Outtake; + +public class OuttakeCoral extends SequentialCommandGroup { + public OuttakeCoral(Outtake outtake, Elevator elevator, Arm arm){ + BooleanSupplier l4Supplier = () -> elevator.getSetpoint() > ElevatorConstants.L3_SETPOINT + 0.001; + BooleanSupplier l1Supplier = () -> elevator.getSetpoint() < ElevatorConstants.L1_SETPOINT + 0.001; + addCommands( + new ConditionalCommand( + new ScoreL4(outtake, arm), + new OuttakeCoralBasic(outtake, l4Supplier, l1Supplier), + l4Supplier) + //new InstantCommand(()->elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT), elevator) + ); + } +} diff --git a/src/main/java/frc/robot/commands/gpm/OuttakeCoralBasic.java b/src/main/java/frc/robot/commands/gpm/OuttakeCoralBasic.java new file mode 100644 index 0000000..7279dc4 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/OuttakeCoralBasic.java @@ -0,0 +1,54 @@ +package frc.robot.commands.gpm; + + +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.outtake.Outtake; + + +/** + * Command to eject coral. + * Wants coral to be present. + */ +public class OuttakeCoralBasic extends Command { + public static final double L1_SPEED = -0.2; + public static final double L4_SPEED = -0.8; + public static final double OUTTAKE_SPEED = -0.45; + + private Outtake outtake; + + // counter to measure time in ticks (every 20 milliseconds); + private Timer timer = new Timer(); + + private BooleanSupplier l4Supplier; + private BooleanSupplier l1Supplier; + + public OuttakeCoralBasic(Outtake outtake, BooleanSupplier l4Supplier, BooleanSupplier l1Supplier){ + this.outtake = outtake; + this.l4Supplier = l4Supplier; + this.l1Supplier = l1Supplier; + addRequirements(outtake); + } + + + @Override + public void initialize(){ + timer.restart(); + boolean l4 = l4Supplier.getAsBoolean(); + boolean l1 = !l4 && l1Supplier.getAsBoolean(); + outtake.setMotor(l4 ? L4_SPEED : l1 ? L1_SPEED : OUTTAKE_SPEED); + } + + public boolean isFinished(){ + return timer.hasElapsed(0.25); + } + + + public void end(boolean interrupted){ + outtake.stop(); + } +} + + diff --git a/src/main/java/frc/robot/commands/gpm/RemoveAlgae.java b/src/main/java/frc/robot/commands/gpm/RemoveAlgae.java new file mode 100644 index 0000000..c63db70 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/RemoveAlgae.java @@ -0,0 +1,22 @@ +package frc.robot.commands.gpm; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.outtake.Outtake; + +public class RemoveAlgae extends Command { + private Outtake outtake; + public RemoveAlgae(Outtake outtake){ + this.outtake = outtake; + addRequirements(outtake); + } + + @Override + public void initialize(){ + outtake.removeAlgae(); + } + + @Override + public void end(boolean interrupted){ + outtake.stop(); + } +} diff --git a/src/main/java/frc/robot/commands/gpm/ResetClimb.java b/src/main/java/frc/robot/commands/gpm/ResetClimb.java new file mode 100644 index 0000000..4479bc8 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/ResetClimb.java @@ -0,0 +1,23 @@ +package frc.robot.commands.gpm; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.climb.Climb; + +public class ResetClimb extends Command { + private Climb climb; + + public ResetClimb(Climb climb){ + this.climb = climb; + addRequirements(climb); + } + + @Override + public void initialize(){ + climb.reset(true); + } + @Override + public void end(boolean interrupted){ + climb.reset(false); + } + +} diff --git a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java new file mode 100644 index 0000000..254d08d --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java @@ -0,0 +1,45 @@ +package frc.robot.commands.gpm; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.outtake.Outtake; + +public class ReverseMotors extends Command { + private Intake intake; + private Indexer indexer; + private Outtake outtake; + + public ReverseMotors(Intake intake, Indexer indexer, Outtake outtake) { + this.intake = intake; + this.indexer = indexer; + this.outtake = outtake; + addRequirements(intake, indexer); + if(outtake != null){ + addRequirements(outtake); + } + } + + @Override + public void initialize() { + intake.setSpeed(-.5); + indexer.reverse(); + if(outtake != null){ + outtake.reverse(); + } + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + intake.deactivate(); + indexer.stop(); + if(outtake != null){ + outtake.stop();; + } + } +} diff --git a/src/main/java/frc/robot/commands/gpm/RunIntakeAndIndexer.java b/src/main/java/frc/robot/commands/gpm/RunIntakeAndIndexer.java new file mode 100644 index 0000000..4e2cd3c --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/RunIntakeAndIndexer.java @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.gpm; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.intake.Intake; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class RunIntakeAndIndexer extends SequentialCommandGroup { + /** Creates a new RunIntakeAndIndexer. */ + public RunIntakeAndIndexer(Intake intake, Indexer indexer ) { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + new InstantCommand(() -> intake.activate()), + new InstantCommand(() -> indexer.run()), + new WaitCommand(3), + new InstantCommand(() -> intake.deactivate()), + new InstantCommand(() -> indexer.stop()) + ); + } +} diff --git a/src/main/java/frc/robot/commands/gpm/ScoreL4.java b/src/main/java/frc/robot/commands/gpm/ScoreL4.java new file mode 100644 index 0000000..7b0cb59 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/ScoreL4.java @@ -0,0 +1,22 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.gpm; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.outtake.Outtake; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class ScoreL4 extends SequentialCommandGroup { + /** Creates a new ScoreL4. */ + public ScoreL4(Outtake outtake, Arm arm) { + addCommands( + new OuttakeCoralBasic(outtake, ()->true, ()->false) + ); + } +} + diff --git a/src/main/java/frc/robot/commands/gpm/StationIntake.java b/src/main/java/frc/robot/commands/gpm/StationIntake.java new file mode 100644 index 0000000..1224d33 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/StationIntake.java @@ -0,0 +1,30 @@ +package frc.robot.commands.gpm; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.outtake.Outtake; + +public class StationIntake extends Command { + private Outtake outtake; + + public StationIntake(Outtake outtake) { + this.outtake = outtake; + addRequirements(outtake); + } + + @Override + public void initialize() { + outtake.setMotor(0.7); + } + + @Override + public boolean isFinished() { + return outtake.coralLoaded(); + } + + @Override + public void end(boolean interrupted) { + outtake.setMotor(0.02); + } + + +} diff --git a/src/main/java/frc/robot/commands/test_comm/PoseTransform.java b/src/main/java/frc/robot/commands/test_comm/PoseTransform.java new file mode 100644 index 0000000..94b969b --- /dev/null +++ b/src/main/java/frc/robot/commands/test_comm/PoseTransform.java @@ -0,0 +1,59 @@ +package frc.robot.commands.test_comm; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.TestConstants; +import frc.robot.subsystems.drivetrain.Drivetrain; + +/** + * Tests the odometry of the robot by driving a certain distance and calculating the error. + */ +public class PoseTransform extends Command { + + private final Drivetrain drive; + + private double startTime; + private Pose2d finalPose; + private final Transform2d distanceToMove; + private Pose2d error; + + public PoseTransform(Drivetrain drive, Transform2d poseTransform) { + this.drive = drive; + // finalPose is position after robot moves from current position-- startPose-- by the values that are inputted-- distanceToMove + distanceToMove = poseTransform; + + addRequirements(drive); + } + + @Override + public void initialize() { + startTime = Timer.getFPGATimestamp(); + finalPose = drive.getPose().transformBy(distanceToMove); + } + + @Override + public void execute() { + drive.driveWithPID(finalPose.getX(), finalPose.getY(), finalPose.getRotation().getRadians()); + } + + @Override + public boolean isFinished() { + double errorMarginMeters = TestConstants.POSE_TRANSFORM_TRANSLATION_ERROR; + double errorMarginRadians = Units.degreesToRadians(10); + error = drive.getPose().relativeTo(finalPose); + // if robot thinks its precision is < 0.1 to the target we inputted, it will stop, so then we can see how off it is + return Math.abs(error.getX()) < errorMarginMeters && Math.abs(error.getY()) < errorMarginMeters && Math.abs(error.getRotation().getRadians()) < errorMarginRadians; + } + + @Override + public void end(boolean interrupted) { + drive.stop(); + System.out.println(Timer.getFPGATimestamp() - startTime); + System.out.println(error.getX()); + System.out.println(error.getY()); + System.out.println(error.getRotation().getRadians()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/test_comm/PoseTransformTest.java b/src/main/java/frc/robot/commands/test_comm/PoseTransformTest.java new file mode 100644 index 0000000..3e1a535 --- /dev/null +++ b/src/main/java/frc/robot/commands/test_comm/PoseTransformTest.java @@ -0,0 +1,67 @@ +package frc.robot.commands.test_comm; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.TestConstants; +import frc.robot.subsystems.drivetrain.Drivetrain; + +/** + * Tests the odometry of the robot by driving a certain distance and calculating the error. + */ +public class PoseTransformTest extends Command { + + private final Drivetrain drive; + + private double startTime; + private Pose2d finalPose; + private Pose2d error; + private double x; + private double y; + private double rotation; + + public PoseTransformTest(Drivetrain drive, double x, double y, double rotation) { + this.drive = drive; + // finalPose is position after robot moves from current position-- startPose-- by the values that are inputted-- distanceToMove + this.x =x; + this.y =y; + this.rotation = rotation; + addRequirements(drive); + } + + @Override + public void initialize() { + startTime = Timer.getFPGATimestamp(); + finalPose = drive.getPose().transformBy(new Transform2d( + new Translation2d(x, y), + Rotation2d.fromDegrees(rotation) + )); + } + + @Override + public void execute() { + drive.driveWithPID(finalPose.getX(), finalPose.getY(), finalPose.getRotation().getRadians()); + } + + @Override + public boolean isFinished() { + double errorMarginMeters = TestConstants.POSE_TRANSFORM_TRANSLATION_ERROR; + double errorMarginRadians = Units.degreesToRadians(10); + error = drive.getPose().relativeTo(finalPose); + // if robot thinks its precision is < 0.1 to the target we inputted, it will stop, so then we can see how off it is + return Math.abs(error.getX()) < errorMarginMeters && Math.abs(error.getY()) < errorMarginMeters && Math.abs(error.getRotation().getRadians()) < errorMarginRadians; + } + + @Override + public void end(boolean interrupted) { + drive.stop(); + System.out.println(Timer.getFPGATimestamp() - startTime); + System.out.println(error.getX()); + System.out.println(error.getY()); + System.out.println(error.getRotation().getRadians()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/vision/AcquireGamePiece.java b/src/main/java/frc/robot/commands/vision/AcquireGamePiece.java new file mode 100644 index 0000000..ba4f07a --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/AcquireGamePiece.java @@ -0,0 +1,28 @@ +package frc.robot.commands.vision; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.gpm.IntakeCoral; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.outtake.Outtake; +import frc.robot.util.Vision.DetectedObject; + +public class AcquireGamePiece extends SequentialCommandGroup { + /** + * Intakes a game piece + * + * @param gamePiece The supplier for the game piece to intake + * @param drive The drivetrain + * @param intake The intake + * @param index The indexer + * @param arm The arm + */ + public AcquireGamePiece(Supplier gamePiece, Drivetrain drive, Intake intake, Indexer indexer, Elevator elevator, Outtake outtake, Arm arm){ + addCommands(new IntakeCoral(intake, indexer, elevator, outtake, arm).deadlineFor(new DriveToCoral(gamePiece, drive))); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/vision/AimAtTag.java b/src/main/java/frc/robot/commands/vision/AimAtTag.java new file mode 100644 index 0000000..50586e0 --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/AimAtTag.java @@ -0,0 +1,88 @@ +package frc.robot.commands.vision; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.FieldConstants; +import frc.robot.subsystems.drivetrain.Drivetrain; + +/** + * Aims the robot at the closest April tag + */ +public class AimAtTag extends Command { + Drivetrain m_drive; + PIDController m_pid; + + /** + * Aims the robot at the closest April tag + * @param drive The drivetrain + */ + public AimAtTag(Drivetrain drive){ + m_drive = drive; + // Copy drive PID and changetolerance + m_pid = new PIDController( + m_drive.getRotationController().getP(), + m_drive.getRotationController().getI(), + m_drive.getRotationController().getD() + ); + m_pid.setTolerance(Units.degreesToRadians(1)); + addRequirements(drive); + } + + /** + * Gets the closest tag and sets the setpoint to aim at it + */ + @Override + public void initialize(){ + double dist = Double.POSITIVE_INFINITY; + Translation2d closest = new Translation2d(); + Translation2d driveTranslation = m_drive.getPose().getTranslation(); + for(AprilTag tag : FieldConstants.APRIL_TAGS){ + Translation2d translation = tag.pose.toPose2d().getTranslation(); + double dist2 = driveTranslation.getDistance(translation); + if(dist2 < dist){ + dist = dist2; + closest = translation; + } + } + m_pid.reset(); + m_pid.setSetpoint(Math.atan2(closest.getY() - driveTranslation.getY(), closest.getX() - driveTranslation.getX())); + } + + /** + * Runs the PID + */ + @Override + public void execute() { + double angle = m_drive.getPose().getRotation().getRadians(); + // If the distance between the angles is more than 180 degrees, use an identical angle ±360 degrees + if(angle - m_pid.getSetpoint() > Math.PI){ + angle -= 2*Math.PI; + }else if(angle - m_pid.getSetpoint() < -Math.PI){ + angle += 2*Math.PI; + } + double speed = m_pid.calculate(angle); + m_drive.drive(0, 0, speed, true, false); + } + + /** + * Stops the drivetrain + * @param interrupted If the command is interrupted + */ + @Override + public void end(boolean interrupted) { + m_drive.stop(); + } + + /** + * Returns if the command is finished + * @return If the PID is at the setpoint + */ + @Override + public boolean isFinished() { + return m_pid.atSetpoint(); + } +} + diff --git a/src/main/java/frc/robot/commands/vision/CalculateStdDevs.java b/src/main/java/frc/robot/commands/vision/CalculateStdDevs.java new file mode 100644 index 0000000..eea25aa --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/CalculateStdDevs.java @@ -0,0 +1,124 @@ +package frc.robot.commands.vision; + +import java.util.ArrayList; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.util.MathUtils; +import frc.robot.util.Vision.Vision; + +/** + * Calculates standard deviations for vision + */ +public class CalculateStdDevs extends Command { + private final Vision m_vision; + private ArrayList m_poses; + private int m_arrayLength; + private Timer m_endTimer; + private Drivetrain m_drive; + + /** + * Constructor for CalculateStdDevs + * @param posesToUse the amount of poses to take the standard deviation of. More poses will take more time. + * @param vision The vision + */ + public CalculateStdDevs(int posesToUse, Vision vision, Drivetrain drive) { + m_vision = vision; + m_drive = drive; + m_arrayLength = posesToUse; + m_endTimer = new Timer(); + } + + /** + * Resets the pose array + */ + @Override + public void initialize() { + // create the ArrayList of poses to store + // an ArrayList prevents issues if the command ends early, and makes checking if the command has finished easy + m_poses = new ArrayList(); + + m_drive.setVisionEnabled(false); + } + + /** + * Adds a pose to the array + */ + @Override + public void execute() { + Pose2d pose = m_vision.getPose2d(m_drive.getPose()); + // If the pose exists, add it to the first open spot in the array + if (pose != null) { + // if we see a pose, reset the timer (it will be started the next time it doesn't get a pose) + m_endTimer.stop(); + m_endTimer.reset(); + // add the pose to our data + m_poses.add(pose); + if(m_poses.size()%10==0){ + System.out.printf("%.0f%% done\n", ((double)m_poses.size())/m_arrayLength * 100); + } + } else { + m_endTimer.start(); + // If kStdDevCommandEndTime seconds have passed since it saw an April tag, stop the command + // Prevents it from running forever + if (m_endTimer.hasElapsed(10)) { + cancel(); + } + } + } + + /** + * Calculates the standard deviation + */ + @Override + public void end(boolean interrupted) { + m_drive.setVisionEnabled(true); + + // If the array is empty, don't try to calculate std devs + if (m_poses.size() == 0) { + System.out.println("There are no poses in the array\nTry again where the robot can see an April tag."); + return; + } + + // create arrays of the poses by X, Y, and Rotation for calculations + double[] xArray = new double[m_poses.size()]; + double[] yArray = new double[m_poses.size()]; + double[] rotArray = new double[m_poses.size()]; + + // copy the values into the arrays + for (int i = 0; i < m_poses.size(); i++) { + xArray[i] = m_poses.get(i).getX(); + yArray[i] = m_poses.get(i).getY(); + rotArray[i] = m_poses.get(i).getRotation().getRadians(); + } + + // Calculate the standard deviations + double stdDevX = MathUtils.stdDev(xArray); + double stdDevY = MathUtils.stdDev(yArray); + double stdDevRot = MathUtils.stdDev(rotArray); + + // Find distance to tag + double distance; + try{ + distance = m_vision.getEstimatedPoses(m_drive.getPose()).get(0).targetsUsed.get(0).getBestCameraToTarget().getTranslation().getNorm(); + }catch(Exception e){ + System.out.println("Could not see a target"); + distance = -1; + } + + // Print and log values + System.out.printf("Standard deviation values:\nX: %.5f\nY: %.5f\nRotation: %.5f\nDistance: %.5f\n", + stdDevX, stdDevY, stdDevRot, distance); + } + + /** + * Returns if the command is finished + * @return If the array is full + */ + @Override + public boolean isFinished() { + return m_poses.size() >= m_arrayLength; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/vision/DriveToCoral.java b/src/main/java/frc/robot/commands/vision/DriveToCoral.java new file mode 100644 index 0000000..db4c0b4 --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/DriveToCoral.java @@ -0,0 +1,59 @@ +package frc.robot.commands.vision; + +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.commands.drive_comm.DriveToPose; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.util.Vision.DetectedObject; + +/** + * Moves toward the detected object + *

Only works with the front camera + */ +public class DriveToCoral extends DriveToPose { + private static final boolean constantUpdate = false; + + private static Pose2d tempPose; + + private Supplier objectSupplier; + + /** + * Moves toward the detected object + * @param detectedObject The supplier for the detected object to use + * @param drive The drivetrain + */ + public DriveToCoral(Supplier detectedObject, Drivetrain drive) { + super(drive, + constantUpdate + ? () -> getPose(detectedObject) + : () -> tempPose); + objectSupplier = detectedObject; + updateTarget = constantUpdate; + } + + public static Pose2d getPose(Supplier supplier){ + DetectedObject object = supplier.get(); + if(object == null) return null; + return new Pose2d(object.pose.toPose2d().getTranslation(), new Rotation2d(object.getAngle()+Math.PI/2)); + } + + @Override + public void initialize(){ + // Set the static variable so the super class has access to it + if(constantUpdate){ + tempPose = getPose(objectSupplier); + } + super.initialize(); + } + + @Override + public void execute(){ + // Set the static variable so the super class has access to it + if(constantUpdate){ + tempPose = getPose(objectSupplier); + } + super.execute(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/vision/GoToPose2.java b/src/main/java/frc/robot/commands/vision/GoToPose2.java new file mode 100644 index 0000000..cec124a --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/GoToPose2.java @@ -0,0 +1,78 @@ +package frc.robot.commands.vision; + +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.Constants; +import frc.robot.subsystems.drivetrain.Drivetrain; + +public class GoToPose2 extends Command { + private static final double MIN_ACCEL = 2; + private final Supplier poseSupplier; + private final Drivetrain drive; + private Pose2d pose; + private double vx; + private double vy; + private Pose2d error; + + public GoToPose2(Supplier poseSupplier, Drivetrain drive){ + this.poseSupplier = poseSupplier; + this.drive = drive; + addRequirements(drive); + } + + @Override + public void initialize(){ + pose = poseSupplier.get(); + ChassisSpeeds v = drive.getChassisSpeeds(); + vx = v.vxMetersPerSecond; + vy = v.vyMetersPerSecond; + error = null; + } + + @Override + public void execute(){ + if(pose == null){ + return; + } + Pose2d drivePose = drive.getPose(); + error = drivePose.relativeTo(pose); + double ax = calcAccel(vx, error.getX()); + double ay = calcAccel(vy, error.getY()); + if(Math.abs(ax) < MIN_ACCEL && Math.abs(error.getX()) > 0.01){ + ax = -Math.signum(error.getX())*MIN_ACCEL; + } + if(Math.abs(ay) < MIN_ACCEL && Math.abs(error.getY()) > 0.01){ + ay = -Math.signum(error.getY())*MIN_ACCEL; + } + vx += ax*Constants.LOOP_TIME; + vy += ay*Constants.LOOP_TIME; + Translation2d v = new Translation2d(vx, vy).rotateBy(pose.getRotation()); + drive.driveHeading(v.getX(), v.getY(), pose.getRotation().getRadians(), true); + } + + @Override + public void end(boolean interrupted){ + drive.stop(); + } + + @Override + public boolean isFinished(){ + return pose == null || error != null && error.getTranslation().getNorm() < 0.01; + } + + private double calcAccel(double v, double x){ + if(Math.abs(x) < 0.001 || Math.abs(Math.signum(v) - Math.signum(x)) < 0.5){ + return 0; + } + double a = v*v/2/x; + double a2 = -v/Constants.LOOP_TIME; + if(Math.abs(a2) < Math.abs(a)){ + return a2; + } + return a; + } +} diff --git a/src/main/java/frc/robot/commands/vision/ReturnData.java b/src/main/java/frc/robot/commands/vision/ReturnData.java new file mode 100644 index 0000000..ae65672 --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/ReturnData.java @@ -0,0 +1,84 @@ +package frc.robot.commands.vision; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.VisionConstants; +import frc.robot.util.Vision.DetectedObject; +import frc.robot.util.Vision.Vision; + +/** + * Adds data from object detection vision to SmartDashboard + */ +public class ReturnData extends Command{ + private final Vision m_vision; + private final Timer timer = new Timer(); + + /** + * Adds data from object detection vision to Smartdashboard + * @param vision The vision + */ + public ReturnData(Vision vision){ + m_vision = vision; + } + + @Override + public void initialize(){ + timer.reset(); + timer.start(); + } + + /** + * Adds the data to SmartDashboard + */ + @Override + public void execute() { + if(timer.advanceIfElapsed(2)){ + double[] xOffset = m_vision.getHorizontalOffset(); + double[] yOffset = m_vision.getVerticalOffset(); + // long[] objectClass = m_vision.getDetectedObjectClass(); + + // //put the offsets and area on SmartDashboard for testing + // SmartDashboard.putNumberArray("Object X offsets degrees", xOffset); + // SmartDashboard.putNumberArray("Object Y offsets degrees", yOffset); + // SmartDashboard.putNumberArray("Object Distances", m_vision.getDistance()); + + DetectedObject bestGamePiece = m_vision.getBestGamePiece(Math.PI, false); + if(bestGamePiece!=null){ + // SmartDashboard.putString("Vision best game piece", bestGamePiece.toString()); + System.out.println("\n\nBest game piece: "+bestGamePiece); + } + + if ((xOffset.length != 0) == (yOffset.length != 0)) { + for (int i = 0; i < xOffset.length; i++) { + System.out.printf("\nx: %.2f, y: %.2f, type: %s\n", xOffset[i], yOffset[i], DetectedObject.getType(0)); + DetectedObject object = new DetectedObject(Units.degreesToRadians(xOffset[i]), Units.degreesToRadians(yOffset[i]), 0, VisionConstants.OBJECT_DETECTION_CAMERAS.get(0)); + System.out.printf("Object: %s\nDistance: %.2f, Angle: %.2f\n", object, object.getDistance(), Units.radiansToDegrees(object.getAngle())); + } + }else { + System.out.println("One of the arrays is empty!"); + } + } + } + + /** + * Does nothing + * @param interrupted If the command is interrupted + */ + @Override + public void end(boolean interrupted) { + + } + + /** + * Returns if the command is finished + * @retrun Always false (command never finishes) + */ + @Override + public boolean isFinished() { + return false; + } + + +} + diff --git a/src/main/java/frc/robot/commands/vision/TestVisionDistance.java b/src/main/java/frc/robot/commands/vision/TestVisionDistance.java new file mode 100644 index 0000000..1810c13 --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/TestVisionDistance.java @@ -0,0 +1,113 @@ +package frc.robot.commands.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.util.Vision.Vision; + +/** + * Gathers data on the distance limits of the camera used for vision. + */ +public class TestVisionDistance extends Command { + private final Drivetrain m_drive; + private final Vision m_vision; + private Translation2d m_visionStartTranslation, m_driveStartTranslation; + private Pose2d m_currentPose = null; + private double m_driveDistance; + private double m_visionDistance; + + private double m_speed; + + private final Timer m_endTimer = new Timer(); + private final Timer m_printTimer = new Timer(); + + // How many seconds of not seeing april tag before ending the command + private static final double kEndDelay = 0.25; + + // How many seconds between each data print + private static final double kPrintDelay = 1; + + /** + * Constructor for TestVisionDistance + * @param speed What speed to move at, negative if backward + * @param drive The drivetrain + * @param vision The vision + */ + public TestVisionDistance(double speed, Drivetrain drive, Vision vision){ + addRequirements(drive); + m_drive = drive; + m_speed = speed; + m_vision = vision; + } + + /** + * Starts the timers and disables vision for odometry + */ + @Override + public void initialize() { + + m_endTimer.reset(); + m_printTimer.restart(); + + m_drive.setVisionEnabled(false); + + m_currentPose = m_vision.getPose2d(m_drive.getPose()); + m_visionStartTranslation = m_currentPose.getTranslation(); + m_driveStartTranslation = m_drive.getPose().getTranslation(); + m_driveDistance = 0; + m_visionDistance = 0; + } + + /** + * Drives the robot, finds the pose from the drivetrain and vision, and someimes prints the distances + */ + @Override + public void execute() { + m_drive.drive(m_speed, 0, 0, false, false); + Pose2d newestPose = m_vision.getPose2d(m_currentPose, m_drive.getPose()); + + // If the camera can see the apriltag + if (newestPose != null) { + //update current pose + m_currentPose = newestPose; + // reset the timer + m_endTimer.reset(); + m_driveDistance = m_drive.getPose().getTranslation().getDistance(m_driveStartTranslation); + m_visionDistance = m_currentPose.getTranslation().getDistance(m_visionStartTranslation); + SmartDashboard.putNumber("Vision test drive distance", m_driveDistance); + SmartDashboard.putNumber("Vision test vision distnace", m_visionDistance); + SmartDashboard.putNumber("Vision test error", m_visionDistance - m_driveDistance); + SmartDashboard.putNumber("Vision test % error", (m_visionDistance-m_driveDistance) / m_driveDistance * 100); + // If kPrintDelay seconds have passed, print the data + if (m_printTimer.advanceIfElapsed(kPrintDelay)) { + System.out.printf("\nDrive dist: %.2f\nVision dist: %.2f\nError: %.2f\n %% error: %.2f\n", + m_driveDistance, m_visionDistance, + m_visionDistance-m_driveDistance, (m_visionDistance-m_driveDistance) / m_driveDistance * 100 + ); + } + } else { + m_endTimer.start(); + } + } + + /** + * Re-enables vision and stops the robot + */ + @Override + public void end(boolean interrupted) { + m_drive.setVisionEnabled(true); + m_drive.stop(); + } + + /** + * Returns if the command is finished + * @return If the end delay has elapsed + */ + @Override + public boolean isFinished() { + return m_endTimer.hasElapsed(kEndDelay); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java new file mode 100644 index 0000000..bc0be45 --- /dev/null +++ b/src/main/java/frc/robot/constants/ArmConstants.java @@ -0,0 +1,52 @@ +package frc.robot.constants; + +import edu.wpi.first.math.system.plant.DCMotor; + +public class ArmConstants { + // Degrees + public static final double START_ANGLE = -86.5; + public static final double MIN_ANGLE = -86.5; + public static final double MAX_ANGLE = 180; + public static final double OFFSET = 0 - START_ANGLE; + + public static final double GEAR_RATIO = 29.36; + public static final double ENCODER_GEAR_RATIO = 50.0/24.0; + public static final DCMotor MOTOR = DCMotor.getKrakenX60(1); + + public static final double MASS = 2.46; // kilograms + public static final double LENGTH = 0.138*2; // meters + public static final double MOI = 0.0261057394; // kg*m^2 + public static final double CENTER_OF_MASS_LENGTH = 0.138; // meters + + public static final double MAX_VELOCITY = 21; // rad/s + public static final double MAX_ACCELERATION = 120; // rad/s^2 + + public static final double INTAKE_SETPOINT = START_ANGLE; + public static final double STATION_INTAKE_SETPOINT = 75.5; + + public static final double TOLERANCE = 3.0; + + //Dunk L4 = 6.4 + public static final double L1_SETPOINT = 50; + + //4 in offset + // public static final double L4_SETPOINT = 11; + // Original L4: 7.5 degrees + public static final double L4_SETPOINT_RIGHT = 7.5; + public static final double L4_SETPOINT_LEFT = 6; + public static final double L2_L3_SETPOINT = 21.25; + + //touching reef + public static final double L4_SETPOINT_ALT = 4.5; + public static final double L2_L3_SETPOINT_ALT = 12.23; + + public static final double ALGAE_SETPOINT = -19.37; + public static final double ALGAE_NET_SETPOINT_1 = 85.0; + public static final double ALGAE_NET_SETPOINT_2 = 25; + public static final double ALGAE_STOW_SETPOINT = 50; + public static final double ALGAE_LOLI_SETPOINT = -19; + + public static final double PROCESSOR_SETPOINT = -65.0; + + public static final double STOW_SETPOINT = -14.0; +} diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 956250b..939cb07 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -1,10 +1,39 @@ package frc.robot.constants; +import com.pathplanner.lib.config.ModuleConfig; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; + +import edu.wpi.first.math.system.plant.DCMotor; +import frc.robot.constants.swerve.DriveConstants; + +/** + * Container class for auto constants. + */ public class AutoConstants { - // Pathplanner output folder should be src/main/deploy/pathplanner - public final String kTrajectoryDirectory = "pathplanner/"; + // Pathplanner output folder should be src/main/deploy/pathplanner + public static final String TRAJECTORY_DIRECTORY = "pathplanner/paths/"; + + public static final double MAX_AUTO_SPEED = 5.2; // m/s + public static final double MAX_AUTO_ACCEL = 4.8; // m/s^2 + + public static RobotConfig CONFIG; + public static final PPHolonomicDriveController AUTO_CONTROLLER = new PPHolonomicDriveController( + new PIDConstants(4.0, 0.0, 0), // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants + ); + - public final double kMaxAutoSpeed = 1.0; // m/s - public final double kMaxAutoAccel = 1.0; // m/s^2 + + static { + try{ + CONFIG = RobotConfig.fromGUISettings(); + }catch(Exception e){ + e.printStackTrace(); + // Although these values are probably wrong and auto might not work correctly, at least it won't cause NullPointerExceptions + CONFIG = new RobotConfig(50, 0.5, new ModuleConfig(DriveConstants.WHEEL_RADIUS, MAX_AUTO_SPEED, DriveConstants.COSF, DCMotor.getKrakenX60(1).withReduction(DriveConstants.DRIVE_GEAR_RATIO), DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT, 1), DriveConstants.MODULE_LOCATIONS); + } + } } diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index e19f2d7..bc90b34 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -1,18 +1,79 @@ package frc.robot.constants; -public final class Constants { - public static final double kGravitationalAccel = 9.8; - public static final double kMaxVoltage = 12.0; - public static final double kLoopTime = 0.02; - - public static final double kCancoderResolution = 4096; - - // CAN bus names - // public static final String kCanivoreCAN = ""; // replace this if using Canivore - public static final String kRioCAN = "rio"; - - public static final OIConstants oi = new OIConstants(); - public static final DriveConstants drive = new DriveConstants(); - public static final AutoConstants auto = new AutoConstants(); - public static final FalconConstants falcon = new FalconConstants(); +import edu.wpi.first.wpilibj.RobotBase; + +public class Constants { + + // constants: + + public static final double GRAVITY_ACCELERATION = 9.8; + public static final double ROBOT_VOLTAGE = 12.0; + public static final double LOOP_TIME = 0.02; + + // CAN bus names + public static final String CANIVORE_CAN = "CANivore"; + public static final String RIO_CAN = "rio"; + + // Logging + public static final boolean USE_TELEMETRY = true; + + public static enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + + /** Replaying from a log file. */ + REPLAY + } + + // Kraken Speed + public static double MAX_RPM = 5800.0; // Rotations per minute + + /* + * Talon Stator / Supply Limits explanation + * Supply current is current that's being drawn at the input bus voltage. Stator + * current is current that's being drawn by the motor. + * Supply limiting (supported by Talon FX and SRX) is useful for preventing + * breakers from tripping in the PDP. + * Stator limiting (supported by Talon FX) is useful for limiting + * acceleration/heat. + */ + + // These are the default values + + // Stator + public static final boolean TALONFX_STATOR_LIMIT_ENABLE = false; // enabled? + public static final double TALONFX_STATOR_CURRENT_LIMIT = 100; // Limit(amp) + public static final double TALONFX_STATOR_TRIGGER_THRESHOLD = 100; // Trigger Threshold(amp) + public static final double TALONFX_STATOR_TRIGGER_DURATION = 0; // Trigger Threshold Time(s) + + // Supply + public static final boolean TALONFX_SUPPLY_LIMIT_ENABLE = false; // enabled? + public static final double TALONFX_SUPPLY_CURRENT_LIMIT = 40; // Limit(amp), current to hold after trigger hit + public static final double TALONFX_SUPPLY_TRIGGER_THRESHOLD = 55; // (amp), amps to activate trigger + public static final double TALONFX_SUPPLY_TRIGGER_DURATION = 3; // (s), how long after trigger before reducing + + // OIConstants: + + public static final int DRIVER_JOY = 0; + public static final int OPERATOR_JOY = 1; + public static final int TEST_JOY = 2; + public static final int MANUAL_JOY = 3; + public static final double DEFAULT_DEADBAND = 0.00005; + + public static final double TRANSLATIONAL_DEADBAND = 0.01; + + public static final double ROTATION_DEADBAND = 0.01; + + public static final double HEADING_DEADBAND = 0.05; + public static final double HEADING_SLEWRATE = 10; + + //Modes + public static final Mode SIM_MODE = Mode.REPLAY; + public static final Mode CURRENT_MODE = RobotBase.isReal() ? Mode.REAL : SIM_MODE; + + // Enables 3D logs of mechanisms + public static final boolean LOG_MECHANISMS = true; } diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java deleted file mode 100644 index d19eb8f..0000000 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ /dev/null @@ -1,9 +0,0 @@ -package frc.robot.constants; - -public class DriveConstants { - - public final int kLeftMotor1 = -1; - public final int kLeftMotor2 = -1; - public final int kRightMotor1 = -1; - public final int kRightMotor2 = -1; -} diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java new file mode 100644 index 0000000..be34322 --- /dev/null +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -0,0 +1,59 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.constants; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; + +/** Stores constants for the elevator. */ +public class ElevatorConstants { + public static final double ANGLE = Units.degreesToRadians(3.5); // radians, from vertical + public static final DCMotor MOTOR = DCMotor.getKrakenX60(1); + public static final int NUMBER_OF_STAGES = 3; + public static final double GEARING = 8.333/NUMBER_OF_STAGES; + public static final double MIN_HEIGHT = 0.0; // meters + public static final double MAX_HEIGHT = Units.inchesToMeters(66);//Units.inchesToMeters(48); // meters + public static final double START_HEIGHT = MIN_HEIGHT; // meters + public static final double CARRIAGE_MASS = 3; // kilograms 2.49475803 + public static final double DRUM_RADIUS = Units.inchesToMeters(1.281/2); // meters + public static final double SPRING_FORCE = 0; //Newtons + + public static final double BOTTOM_LIMIT_SWITCH_HEIGHT = 0;//0.015; // meters + public static final double TOP_LIMIT_SWITCH_HEIGHT = MAX_HEIGHT; // meters + public static final double SIM_LIMIT_SWITCH_TRIGGER_DISTANCE = 0.01; // meters + + public static final double STOW_SETPOINT = 0; + public static final double INTAKE_SETPOINT = 0.036; + public static final double SAFE_SETPOINT = 0.225; + public static final double INTAKE_STOW_SETPOINT = 0.58; + + //4 inch offset + public static final double L1_SETPOINT = 0.0; + public static final double L2_SETPOINT = 0.523; + public static final double L3_SETPOINT = 0.9192; + // public static final double L4_SETPOINT = 1.675; + public static final double L4_SETPOINT = 1.675; + + //touching reef + public static final double L1_SETPOINT_ALT = 0.27; + public static final double L2_SETPOINT_ALT = 0.588; + public static final double L3_SETPOINT_ALT = 0.98-0.0254-0.01; + public static final double L4_SETPOINT_ALT = 1.675; + //Dunk L4 = 1.5 + + public static final double BOTTOM_ALGAE_SETPOINT = 0.405; + public static final double TOP_ALGAE_SETPOINT = 0.799; + + public static final double NET_SETPOINT = MAX_HEIGHT; + public static final double STATION_INTAKE_SETPOINT = 0.233; + + + public static final double CENTER_OF_MASS_HEIGHT_STOWED = Units.inchesToMeters(9.44); + public static final double CENTER_OF_MASS_HEIGHT_EXTENDED = Units.inchesToMeters(10+14.767); + + + // The x distance from the center of the robot to the outtake. + public static final double OUTTAKE_X = Units.inchesToMeters(-7.25); +} diff --git a/src/main/java/frc/robot/constants/FalconConstants.java b/src/main/java/frc/robot/constants/FalconConstants.java deleted file mode 100644 index 5f2edbb..0000000 --- a/src/main/java/frc/robot/constants/FalconConstants.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc.robot.constants; - -public class FalconConstants { - - // Stored in hex though not really a hex. 21.0 = 0x2100, ex. 1.2 = 0x0102 - public final int kFirmwareVersion = 0x2100; - public final boolean kBreakOnWrongFirmware = true; - - public final double kResolution = 2048; - public final double kMaxRpm = 6380.0; // Rotations per minute - - /* - * Talon Stator / Supply Limits explanation - * Supply current is current that’s being drawn at the input bus voltage. Stator - * current is current that’s being drawn by the motor. - * Supply limiting (supported by Talon FX and SRX) is useful for preventing - * breakers from tripping in the PDP. - * Stator limiting (supported by Talon FX) is useful for limiting - * acceleration/heat. - */ - - // These are the default values - - // Stator - public final boolean kStatorLimitEnable = false; // enabled? - public final double kStatorCurrentLimit = 100; // Limit(amp) - public final double kStatorTriggerThreshold = 100; // Trigger Threshold(amp) - public final double kStatorTriggerDuration = 0; // Trigger Threshold Time(s) - - // Supply - public final boolean kSupplyLimitEnable = false; // enabled? - public final double kSupplyCurrentLimit = 40; // Limit(amp), current to hold after trigger hit - public final double kSupplyTriggerThreshold = 55; // (amp), amps to activate trigger - public final double kSupplyTriggerDuration = 3; // (s), how long after trigger before reducing - -} diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java new file mode 100644 index 0000000..03c90e0 --- /dev/null +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -0,0 +1,46 @@ +package frc.robot.constants; + +import java.util.ArrayList; +import java.util.List; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; + +public class FieldConstants { + /** Width of the field [meters] */ + public static final double FIELD_LENGTH = Units.inchesToMeters(57*12 + 6+7.0/8.0); + /** Height of the field [meters] */ + public static final double FIELD_WIDTH = Units.inchesToMeters(26*12 + 5); + + /** + * ArrayList of April Tags to use when the field layout is not available. + *

+ * Obtain AprilTag ID i with APRIL_TAGS.get(i-1). + */ + public static final ArrayList APRIL_TAGS = new ArrayList(List.of( + new AprilTag(1 , new Pose3d(Units.inchesToMeters(657.37), Units.inchesToMeters(25.80), Units.inchesToMeters(58.50), new Rotation3d(0, 0, Units.degreesToRadians(126)))), + new AprilTag(2 , new Pose3d(Units.inchesToMeters(657.37), Units.inchesToMeters(291.20), Units.inchesToMeters(58.50), new Rotation3d(0, 0, Units.degreesToRadians(234)))), + new AprilTag(3 , new Pose3d(Units.inchesToMeters(455.15), Units.inchesToMeters(317.15), Units.inchesToMeters(51.25), new Rotation3d(0, 0, Units.degreesToRadians(270)))), + new AprilTag(4 , new Pose3d(Units.inchesToMeters(365.20), Units.inchesToMeters(241.64), Units.inchesToMeters(73.54), new Rotation3d(0, Units.degreesToRadians(30), 0))), + new AprilTag(5 , new Pose3d(Units.inchesToMeters(365.20), Units.inchesToMeters(75.39), Units.inchesToMeters(73.54), new Rotation3d(0, Units.degreesToRadians(30), 0))), + new AprilTag(6 , new Pose3d(Units.inchesToMeters(530.49), Units.inchesToMeters(130.17), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(300)))), + new AprilTag(7 , new Pose3d(Units.inchesToMeters(546.87), Units.inchesToMeters(158.50), Units.inchesToMeters(12.13), new Rotation3d(0, 0, 0))), + new AprilTag(8 , new Pose3d(Units.inchesToMeters(530.49), Units.inchesToMeters(186.83), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(60)))), + new AprilTag(9 , new Pose3d(Units.inchesToMeters(497.77), Units.inchesToMeters(186.83), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(120)))), + new AprilTag(10, new Pose3d(Units.inchesToMeters(481.39), Units.inchesToMeters(158.50), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(180)))), + new AprilTag(11, new Pose3d(Units.inchesToMeters(497.77), Units.inchesToMeters(130.17), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(240)))), + new AprilTag(12, new Pose3d(Units.inchesToMeters(33.51), Units.inchesToMeters(25.80), Units.inchesToMeters(58.50), new Rotation3d(0, 0, Units.degreesToRadians(54)))), + new AprilTag(13, new Pose3d(Units.inchesToMeters(33.51), Units.inchesToMeters(291.20), Units.inchesToMeters(58.50), new Rotation3d(0, 0, Units.degreesToRadians(306)))), + new AprilTag(14, new Pose3d(Units.inchesToMeters(325.68), Units.inchesToMeters(241.64), Units.inchesToMeters(73.54), new Rotation3d(0, Units.degreesToRadians(30), Units.degreesToRadians(180)))), + new AprilTag(15, new Pose3d(Units.inchesToMeters(325.68), Units.inchesToMeters(75.39), Units.inchesToMeters(73.54), new Rotation3d(0, Units.degreesToRadians(30), Units.degreesToRadians(180)))), + new AprilTag(16, new Pose3d(Units.inchesToMeters(235.73), Units.inchesToMeters(-0.15), Units.inchesToMeters(51.25), new Rotation3d(0, 0, Units.degreesToRadians(90)))), + new AprilTag(17, new Pose3d(Units.inchesToMeters(160.39), Units.inchesToMeters(130.17), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(240)))), + new AprilTag(18, new Pose3d(Units.inchesToMeters(144.00), Units.inchesToMeters(158.50), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(180)))), + new AprilTag(19, new Pose3d(Units.inchesToMeters(160.39), Units.inchesToMeters(186.83), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(120)))), + new AprilTag(20, new Pose3d(Units.inchesToMeters(193.10), Units.inchesToMeters(186.83), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(60)))), + new AprilTag(21, new Pose3d(Units.inchesToMeters(209.49), Units.inchesToMeters(158.50), Units.inchesToMeters(12.13), new Rotation3d(0, 0, 0))), + new AprilTag(22, new Pose3d(Units.inchesToMeters(193.10), Units.inchesToMeters(130.17), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(300)))) + )); +} diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java new file mode 100644 index 0000000..7302cf0 --- /dev/null +++ b/src/main/java/frc/robot/constants/IdConstants.java @@ -0,0 +1,50 @@ +package frc.robot.constants; + +import edu.wpi.first.wpilibj.I2C; + +public class IdConstants { + // Drivetrain + public static final int DRIVE_FRONT_LEFT_ID = 1; + public static final int STEER_FRONT_LEFT_ID = 2; + public static final int ENCODER_FRONT_LEFT_ID = 3; + public static final int DRIVE_FRONT_RIGHT_ID = 10; + public static final int STEER_FRONT_RIGHT_ID = 11; + public static final int ENCODER_FRONT_RIGHT_ID = 12; + public static final int DRIVE_BACK_LEFT_ID = 7; + public static final int STEER_BACK_LEFT_ID = 8; + public static final int ENCODER_BACK_LEFT_ID = 9; + public static final int DRIVE_BACK_RIGHT_ID = 4; + public static final int STEER_BACK_RIGHT_ID = 5; + public static final int ENCODER_BACK_RIGHT_ID = 6; + public static final int PIGEON = 13; + + // LEDs + public static final int CANDLE_ID = 1; + + // Elevator + public static final int ELEVATOR_RIGHT_MOTOR = 50; + public static final int ELEVATOR_BOTTOM_LIMIT_SWITCH = 29; + public static final int ELEVATOR_TOP_LIMIT_SWITCH = 30; + + // Indexer + public static final int INDEXER_MOTOR = 56; + public static final int INDEXER_SENSOR = 24; + + // Climb + public static final int CLIMB_MOTOR = 31; + + // Intake + public static final int INTAKE_ROLLER = 51; + public static final int INTAKE_PIVOT = 55; //55 + public static final int INTAKE_LASER_CAN = 25; + + // Outtake + public static final int OUTTAKE_MOTOR_ALPHA = 14; + public static final int OUTTAKE_MOTOR_COMP = 30; + public static final int OUTTAKE_DIO_EJECTING = 3; + public static final I2C.Port i2cPort = I2C.Port.kMXP; + + //Arm + public static final int ARM_MOTOR = 29; + public static final int ARM_ABSOLUTE_ENCODER = 5; +} diff --git a/src/main/java/frc/robot/constants/IndexerConstants.java b/src/main/java/frc/robot/constants/IndexerConstants.java new file mode 100644 index 0000000..d0fdd1b --- /dev/null +++ b/src/main/java/frc/robot/constants/IndexerConstants.java @@ -0,0 +1,16 @@ +package frc.robot.constants; + +public class IndexerConstants { + public static final double SPEED = 1; + + public static final double MOMENT_OF_INERTIA = 0.000326; + public static final double GEAR_RATIO = 1.0; + public static final int MEASUREMENT_THRESHOLD = 15; // in millimeters + + // this stuff is for sim, all in meters + public static final double WHEEL_CIRCUMFERENCE = Math.PI * 0.1; + public static final double START_SIM_POS_AT = -1.0; // start coral here + public static final double START_SIM_SENSOR_POS_AT = -0.5; // activate the sensor here + public static final double END_SIM_SENSOR_POS_AT = 0.5; // deactivate the sensor here +} + diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java new file mode 100644 index 0000000..1939452 --- /dev/null +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -0,0 +1,23 @@ +package frc.robot.constants; + +/** + * Storage class for intake constants + */ +public class IntakeConstants { + public static final double PIVOT_GEAR_RATIO = 75; + public static final double MOMENT_OFiNERTIA = 0.600984136; + public static final double CENTER_OF_MASS_DIST = 0.265; + public static final double MASS = 6.45688739; + public static final double ARM_LENGTH = CENTER_OF_MASS_DIST*2; + public static final double DETECT_CORAL_DIST = 0.49; + public static final double INTAKE_MOTOR_POWER = 1; + public static final double INTAKE_SETPOINT = -30.41; + public static final double STOW_SETPOINT = 90; + public static final double STATION_SETPOINT = 70; + public static final double INTAKE_SAFE_POINT = 30; + + public static final double ALGAE_SETPOINT = 25; + public static final double ALGAE_INTAKE_POWER = -0.5; + public static final double ALGAE_OUTTAKE_POWER = 0.5; + +} diff --git a/src/main/java/frc/robot/constants/OIConstants.java b/src/main/java/frc/robot/constants/OIConstants.java deleted file mode 100644 index 5ea0f3d..0000000 --- a/src/main/java/frc/robot/constants/OIConstants.java +++ /dev/null @@ -1,9 +0,0 @@ -package frc.robot.constants; - -// OI stands for Operator Input -public class OIConstants { - public final int kDriverJoy = 0; - public final int kOperatorJoy = 1; - - public final double kDeadband = 0.05; -} diff --git a/src/main/java/frc/robot/constants/TestConstants.java b/src/main/java/frc/robot/constants/TestConstants.java new file mode 100644 index 0000000..51667d6 --- /dev/null +++ b/src/main/java/frc/robot/constants/TestConstants.java @@ -0,0 +1,8 @@ +package frc.robot.constants; + +/** + * Container class for test constants. + */ +public class TestConstants { + public static final double POSE_TRANSFORM_TRANSLATION_ERROR = 0.6; +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java new file mode 100644 index 0000000..c599707 --- /dev/null +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -0,0 +1,391 @@ +package frc.robot.constants; + +import java.util.ArrayList; +import java.util.List; + +import org.photonvision.PhotonPoseEstimator.PoseStrategy; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import frc.robot.constants.swerve.DriveConstants; + +/** + * Container class for vision constants. + */ +public class VisionConstants { + /** + * If April tag vision is enabled on the robot + */ + public static final boolean ENABLED = true; + + /** + * If object detection should be enabled + */ + public static final boolean OBJECT_DETECTION_ENABLED = false; + + /** If odometry should be updated using vision during auto */ + public static final boolean ENABLED_AUTO = true; + + /** + * If odometry should be updated using vision while running the GoToPose, + * GoToPosePID, and DriveToPose commands in teleop + */ + public static final boolean ENABLED_GO_TO_POSE = true; + + /** If vision should be simulated */ + public static final boolean ENABLED_SIM = false; + + /** If vision should only return values if it can see 2 good targets */ + public static final boolean ONLY_USE_2_TAGS = false; + + /** PoseStrategy to use in pose estimation */ + public static final PoseStrategy POSE_STRATEGY = PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR; + + /** Fallback PoseStrategy if MultiTag doesn't work */ + public static final PoseStrategy MULTITAG_FALLBACK_STRATEGY = PoseStrategy.LOWEST_AMBIGUITY; + + /** + * Any April tags we always want to ignore. To ignore a tag, put its id in this + * array. + */ + public static final int[] TAGS_TO_IGNORE = {12, 13, 16, 1, 2, 3}; + + /** + * If multiple cameras return different poses, they will be ignored if the + * difference between them is greater than this + */ + public static final double MAX_POSE_DIFFERENCE = 0.2; + + /** + * The maximum distance to the tag to use + */ + public static final double MAX_DISTANCE = 2; + + /** If vision should use manual calculations */ + public static final boolean USE_MANUAL_CALCULATIONS = true; + + //

    did not work + /** + * Which version of driver assist to use. This would be an enum, except there is + * no short and descriptive name for all of these. + *

    + * The options are: + *

    + * 0: Disable driver assist + *

    + * 1: Completely remove the component of the driver's input that is not toward + * the object + *

    + * 2: Interpolate between the next achievable driver speed and a speed + * calculated using trapezoid profiles + *

    + * 3-5: Add a speed perpendicular to the driver input; there are 3 similar but + * different calculations for this + */ + public static final int DRIVER_ASSIST_MODE = 5; + + /** + * The number to multiply the distance to the April tag by. + *

    + * Only affects manual calculations. + *

    + * To find this, set it to 1 and measure the actual distance and the calculated + * distance. + *

    + * This should not be needed, and it is only here because it improved the + * accuracy of vision in the 2023 fall semester + */ + public static final double DISTANCE_SCALE = 1; + + /** + * The standard deviations to use for vision + */ + public static final Matrix VISION_STD_DEVS = VecBuilder.fill( + 0.3, // x in meters (default=0.9) + 0.3, // y in meters (default=0.9) + 0.9 // heading in radians. The gyroscope is very accurate, so as long as it is reset + // correctly it is unnecessary to correct it with vision + ); + + /** + * The standard deviations to use for vision when the wheels slip + */ + public static final Matrix VISION_STD_DEVS_2 = VecBuilder.fill( + 0.01, // x in meters (default=0.9) + 0.01, // y in meters (default=0.9) + 0.9 // heading in radians. The gyroscope is very accurate, so as long as it is reset + // correctly it is unnecessary to correct it with vision + ); + + /** + * The highest ambiguity to use. Ambiguities higher than this will be ignored. + *

    + * Only affects calculations using PhotonVision, not manual calculations. + */ + public static final double HIGHEST_AMBIGUITY = 0.01; + + /** + * The camera poses + *

    + * Everything is in meters and radians + *

    + * 0 for all numbers is center of the robot, on the ground, looking straight + * toward the front + *

    + * + X: Front of Robot + *

    + * + Y: Left of Robot + *

    + * + Z: Top of Robot + *

    + * + Pitch: Down + *

    + * + Yaw: Counterclockwise + */ + public static final ArrayList> APRIL_TAG_CAMERAS = new ArrayList>( + List.of( + new Pair( + "CameraFront", + new Transform3d( + new Translation3d(Units.inchesToMeters(10.485), Units.inchesToMeters(10.217), + Units.inchesToMeters(11.012)), + new Rotation3d(0, Units.degreesToRadians(-11), + Math.PI/2 + Units.degreesToRadians(20)))), + new Pair( + "CameraBack", + new Transform3d( + new Translation3d(Units.inchesToMeters(-9.538), Units.inchesToMeters(7.474), + Units.inchesToMeters(8.719)), + new Rotation3d(0, Units.degreesToRadians(-19.5), + Math.PI/2-Units.degreesToRadians(25)))))); + + /** + * The transformations from the robot to object detection cameras + */ + public static final ArrayList OBJECT_DETECTION_CAMERAS = new ArrayList<>(List.of( + new Transform3d( + new Translation3d(Units.inchesToMeters(10), 0, Units.inchesToMeters(24)), + new Rotation3d(0, Units.degreesToRadians(20), 0)))); + + // Poses to potentially align to + public static final Pose2d RED_PROCESSOR_POSE = new Pose2d( + FieldConstants.APRIL_TAGS.get(2).pose.getX(), + FieldConstants.APRIL_TAGS.get(2).pose.getY() - DriveConstants.ROBOT_WIDTH_WITH_BUMPERS / 2, + new Rotation2d(Math.PI / 2)); + + public static final Pose2d BLUE_PROCESSOR_POSE = new Pose2d( + FieldConstants.APRIL_TAGS.get(15).pose.getX(), + FieldConstants.APRIL_TAGS.get(15).pose.getY() + DriveConstants.ROBOT_WIDTH_WITH_BUMPERS / 2, + new Rotation2d(-Math.PI / 2)); + + public static final Pose2d BLUE_CAGE_LEFT_POSE = new Pose2d( + FieldConstants.FIELD_LENGTH / 2, + FieldConstants.FIELD_WIDTH / 2 + Units.inchesToMeters(1019.0 / 8), + new Rotation2d()); + + public static final Pose2d BLUE_CAGE_MIDDLE_POSE = new Pose2d( + FieldConstants.FIELD_LENGTH / 2, + FieldConstants.FIELD_WIDTH / 2 + Units.inchesToMeters(675.0 / 8), + new Rotation2d()); + + public static final Pose2d BLUE_CAGE_RIGHT_POSE = new Pose2d( + FieldConstants.FIELD_LENGTH / 2, + FieldConstants.FIELD_WIDTH / 2 + Units.inchesToMeters(41.5), + new Rotation2d()); + + public static final Pose2d RED_CAGE_RIGHT_POSE = new Pose2d( + FieldConstants.FIELD_LENGTH / 2, + FieldConstants.FIELD_WIDTH / 2 + Units.inchesToMeters(-41.5), + new Rotation2d(Math.PI)); + + public static final Pose2d RED_CAGE_MIDDLE_POSE = new Pose2d( + FieldConstants.FIELD_LENGTH / 2, + FieldConstants.FIELD_WIDTH / 2 - Units.inchesToMeters(-675.0 / 8), + new Rotation2d(Math.PI)); + + public static final Pose2d RED_CAGE_LEFT_POSE = new Pose2d( + FieldConstants.FIELD_LENGTH / 2, + FieldConstants.FIELD_WIDTH / 2 - Units.inchesToMeters(-1019.0 / 8), + new Rotation2d(Math.PI)); + + /** + * The distance between the AprilTag and algae setpoint in the directin parallel to the face of the reef + * Positive is to the left + */ + public static final double ALGAE_X_OFFSET = 0.02; + + /** + * Stores all of the alignment poses for both reefs. + * Each branch is in the format (RED or BLUE)_BRANCH_(tag ID)_(LEFT or RIGHT) + * Each algae setpoint is in the form (RED or BLUE)_ALGAE_(tag ID) + * Left and right refer to the position of the branch when looking directly at the AprilTag + */ + public enum REEF { + RED_BRANCH_6_LEFT(5, 0.1651, .0587), + RED_BRANCH_6_RIGHT(5, -0.1651, .0587), + RED_BRANCH_7_LEFT(6, 0.1651, .0587), + RED_BRANCH_7_RIGHT(6, -0.1651, .0587), + RED_BRANCH_8_LEFT(7, 0.1651, .0587), + RED_BRANCH_8_RIGHT(7, -0.1651, .0587), + RED_BRANCH_9_LEFT(8, 0.1651, .0587), + RED_BRANCH_9_RIGHT(8, -0.1651, .0587), + RED_BRANCH_10_LEFT(9, 0.1651, .0587), + RED_BRANCH_10_RIGHT(9, -0.1651, .0587), + RED_BRANCH_11_LEFT(10, 0.1651, .0587), + RED_BRANCH_11_RIGHT(10, -0.1651, .0587), + + BLUE_BRANCH_17_LEFT(16, 0.1651, .0587), + BLUE_BRANCH_17_RIGHT(16, -0.1651, .0587), + BLUE_BRANCH_18_LEFT(17, 0.1651, .0587), + BLUE_BRANCH_18_RIGHT(17, -0.1651, .0587), + BLUE_BRANCH_19_LEFT(18, 0.1651, .0587), + BLUE_BRANCH_19_RIGHT(18, -0.1651, .0587), + BLUE_BRANCH_20_LEFT(19, 0.1651, .0587), + BLUE_BRANCH_20_RIGHT(19, -0.1651, .0587), + BLUE_BRANCH_21_LEFT(20, 0.1651, .0587), + BLUE_BRANCH_21_RIGHT(20, -0.1651, .0587), + BLUE_BRANCH_22_LEFT(21, 0.1651, .0587), + BLUE_BRANCH_22_RIGHT(21, -0.1651, .0587), + + RED_ALGAE_6(5, ALGAE_X_OFFSET, 0, true), + RED_ALGAE_7(6, ALGAE_X_OFFSET, 0, true), + RED_ALGAE_8(7, ALGAE_X_OFFSET, 0, true), + RED_ALGAE_9(8, ALGAE_X_OFFSET, 0, true), + RED_ALGAE_10(9, ALGAE_X_OFFSET, 0, true), + RED_ALGAE_11(10, ALGAE_X_OFFSET, 0, true), + BLUE_ALGAE_17(16, ALGAE_X_OFFSET, 0, true), + BLUE_ALGAE_18(17, ALGAE_X_OFFSET, 0, true), + BLUE_ALGAE_19(18, ALGAE_X_OFFSET, 0, true), + BLUE_ALGAE_20(19, ALGAE_X_OFFSET, 0, true), + BLUE_ALGAE_21(20, ALGAE_X_OFFSET, 0, true), + BLUE_ALGAE_22(21, ALGAE_X_OFFSET, 0, true); + + /** + * The pose to align to for scoring on this branch or intaking this algae + */ + public final Pose2d pose; + /** + * The pose to align to for scoring on L4, null for algae + */ + public final Pose2d l4Pose; + /** + * The pose to align to for scoring on L1, null for algae + */ + public final Pose2d l1Pose; + /** + * The ID of the AprilTag on the smae side of hte reef ast his branch + */ + public final int aprilTagId; + private final int aprilTagIndex; + /** + * The horizontal (parallel to the closest face of the reef) distance, in meters, between the AprilTag and branch + */ + public final double xOffset; + /** + * The y (normal to the closest face of the reef) distance, in meters, between the AprilTag and branch + */ + public final double yOffset; + + public final boolean isAlgae; + + private REEF(int aprilTagIndex, double xOffset, double yOffset) { + this(aprilTagIndex, xOffset, yOffset, false); + } + private REEF(int aprilTagIndex, double xOffset, double yOffset, boolean isAlgae) { + this.aprilTagIndex = aprilTagIndex; + aprilTagId = aprilTagIndex+1; + this.xOffset = xOffset; + this.yOffset = yOffset; + this.isAlgae = isAlgae; + pose = getPose(); + if(isAlgae){ + l4Pose = null; + l1Pose = null; + }else{ + l4Pose = pose.transformBy(new Transform2d(0, Units.inchesToMeters(2), new Rotation2d())); + l1Pose = getL1Pose(); + } + } + + /** + * Calculates the Pose2d to align to the branch based on the AprilTag's pose and + * offsets. + * + * @return The calculated Pose2d for this reef branch. + */ + private Pose2d getPose() { + Pose3d basePose3d = FieldConstants.APRIL_TAGS.get(aprilTagIndex).pose; + double adjustedYOffset = DriveConstants.ROBOT_WIDTH_WITH_BUMPERS / 2.0 + (isAlgae ? 0 : Units.inchesToMeters(4.5)); + + // Apply both X and Y offsets to calculate the reef branch pose + Transform3d transform = new Transform3d(adjustedYOffset, -xOffset, 0, new Rotation3d(0, 0, 0)); + + Pose3d branchPose3d = basePose3d.plus(transform); + + // Convert the calculated branch Pose3d to Pose2d + return branchPose3d.toPose2d().transformBy(new Transform2d(0, 0, new Rotation2d(Math.PI/2))); + } + /** + * Calculates the Pose2d to align to for scoring on L1 near this branch based on the AprilTag's pose and + * offsets. + * + * @return The calculated Pose2d for this reef branch. + */ + private Pose2d getL1Pose() { + Pose3d basePose3d = FieldConstants.APRIL_TAGS.get(aprilTagIndex).pose; + double adjustedYOffset = DriveConstants.ROBOT_WIDTH_WITH_BUMPERS / 2.0; + + // Apply both X and Y offsets to calculate the reef branch pose + Transform3d transform = new Transform3d(adjustedYOffset, -Math.signum(xOffset)*Units.inchesToMeters(37.04/2-1), 0, new Rotation3d(0, 0, 0)); + + Pose3d branchPose3d = basePose3d.plus(transform); + + // Convert the calculated branch Pose3d to Pose2d + return branchPose3d.toPose2d().transformBy(new Transform2d(0, 0, new Rotation2d(Math.PI/2))); + } + + /** + * Finds the appropriate reef branch based on the AprilTag ID and whether the + * pose is left or right. + * + * @param aprilTagId The ID of the AprilTag. + * @param isLeftPose True if the pose is left, false if it's right. + * @return The matching REEF, or null if no match is found. + */ + public static REEF fromAprilTagIdAndPose(int aprilTagId, boolean isLeftPose) { + for (REEF branch : values()) { + if (!branch.isAlgae && branch.aprilTagId == aprilTagId && + ((isLeftPose && branch.xOffset > 0) || (!isLeftPose && branch.xOffset < 0))) { + return branch; + } + } + return null; + } + + /** + * Finds the appropriate reef algae position based on the AprilTag ID + * + * @param aprilTagId The ID of the AprilTag. + * @return The matching REEF, or null if no match is found. + */ + public static REEF fromAprilTagIdAlgae(int aprilTagId) { + for (REEF branch : values()) { + if (branch.isAlgae && branch.aprilTagId == aprilTagId) { + return branch; + } + } + return null; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/swerve/DriveConstants.java b/src/main/java/frc/robot/constants/swerve/DriveConstants.java new file mode 100644 index 0000000..3e57260 --- /dev/null +++ b/src/main/java/frc/robot/constants/swerve/DriveConstants.java @@ -0,0 +1,240 @@ +package frc.robot.constants.swerve; + +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.util.Units; +import frc.robot.RobotId; +import frc.robot.constants.Constants; +import frc.robot.util.SwerveStuff.ModuleLimits; +import lib.COTSFalconSwerveConstants; + +/** + * Global constants are, by default, for the competition robot. + * Global constants get changed in the update method if the RobotId detected is not the competition robot. + */ +public class DriveConstants { + /** + * The robot's width with its bumpers on. + *

    + * The frame width is 26.5 inches, and each bumper is 3.25 inches. + */ + public static final double ROBOT_WIDTH_WITH_BUMPERS = 0.832; + + public static double ROBOT_MASS = 25; + + /** Radius of the drive wheels [meters]. */ + public static final double WHEEL_RADIUS = Units.inchesToMeters(1.95); + + public static double WHEEL_MOI = 0.000326 * ROBOT_MASS; + + + /** Distance between the left and right wheels [meters]. */ + public static double TRACK_WIDTH = Units.inchesToMeters(20.75);//22.75 swerve bot, 20.75 comp bot + + // Mk4i gear ratios + // https://www.swervedrivespecialties.com/products/mk4i-swerve-module + // standard gear ratios + // https://www.swervedrivespecialties.com/products/kit-adapter-16t-drive-pinion-gear-mk4i + // changes 14-tooth pinion to 16-tooth pinion -- (50.0 / 14.0) becomes (50.0 / 16.0). + /** Drive gear ratio for an Mk4i with L2-Plus gearing */ + public static double DRIVE_GEAR_RATIO = (50.0 / 16.0) * (17.0 / 27.0) * (45.0 / 15.0); + // all MK4i modules have the same steering gear ratio + public static final double STEER_GEAR_RATIO = 150.0 / 7.0; + + /** Theoretical maximum speed of the robot based on maximum motor RPM, gear ratio, and wheel radius */ + public static final double MAX_SPEED = 4.5; + + // Need to convert tangential velocity (the m/s of the edge of the robot) to angular velocity (the radians/s of the robot) + // To do so, divide by the radius. The radius is the diagonal of the square chassis, diagonal = sqrt(2) * side_length. + public static final double MAX_ANGULAR_SPEED = MAX_SPEED / ((TRACK_WIDTH / 2) * Math.sqrt(2)); + + public static final double COSF = 0.9; + + // The maximum acceleration of the robot, limited by friction + public static final double MAX_LINEAR_ACCEL = COSF * Constants.GRAVITY_ACCELERATION; + // The maximum amount a drive motor can accelerate, independant of friction + // This does nothing if greater than LINEAR_ACCEL + public static final double MAX_DRIVE_ACCEL = MAX_LINEAR_ACCEL; + // The maximum angular acceleration of the robot + public static final double MAX_ANGULAR_ACCEL = MAX_LINEAR_ACCEL / TRACK_WIDTH * Math.sqrt(2); + + /** + * If this is false, Drivetrain will use the previous setpoint to calculate the new setpoint. + *

    If this is true, Drivetrain will use the actual current setpoint instead. + */ + public static final boolean USE_ACTUAL_SPEED = false; + + /** + * Disables the deadband and optimization for the modules. + * SwerveSetpointGenerator adds its own optimization and deadband, and the controllers also have a deadband. + * Setting this to true fixes bugs caused by using hte actual current state. + */ + public static final boolean DISABLE_DEADBAND_AND_OPTIMIZATION = false; + + public static final Rotation2d STARTING_HEADING = new Rotation2d(); + + public static final Translation2d[] MODULE_LOCATIONS = { + new Translation2d(DriveConstants.TRACK_WIDTH / 2, DriveConstants.TRACK_WIDTH / 2), + new Translation2d(DriveConstants.TRACK_WIDTH / 2, -DriveConstants.TRACK_WIDTH / 2), + new Translation2d(-DriveConstants.TRACK_WIDTH / 2, DriveConstants.TRACK_WIDTH / 2), + new Translation2d(-DriveConstants.TRACK_WIDTH / 2, -DriveConstants.TRACK_WIDTH / 2) + }; + + public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics(MODULE_LOCATIONS); + + public static double STEER_OFFSET_FRONT_LEFT = 302.646; + public static double STEER_OFFSET_FRONT_RIGHT = 103.039+180; + public static double STEER_OFFSET_BACK_LEFT = 165.49+90; + public static double STEER_OFFSET_BACK_RIGHT = 73.563; + + // Heading PID. + public static final double HEADING_P = 5.5; + public static final double HEADING_D = 0; + + public static final double HEADING_TOLERANCE = Units.degreesToRadians(1.5); + + // Translational PID + // TODO: Tune this better (low priority since we aren't using it in 2025) + public static final double TRANSLATIONAL_P = 1; + public static final double TRANSLATIONAL_D = 0.001; + + //The PIDs for PathPlanner Command + public static final double PATH_PLANNER_HEADING_P = 3.5; + public static final double PATH_PLANNER_HEADING_D = 0; + + public static final double PATH_PLANNER_TRANSLATIONAL_P = 6; + public static final double PATH_PLANNER_TRANSLATIONAL_D = 0; + + // CAN + public static String DRIVE_MOTOR_CAN = Constants.CANIVORE_CAN; + public static String STEER_MOTOR_CAN = Constants.CANIVORE_CAN; + public static String STEER_ENCODER_CAN = Constants.CANIVORE_CAN; + public static String PIGEON_CAN = Constants.CANIVORE_CAN; + + + public static COTSFalconSwerveConstants MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO); + + /* Swerve Current Limiting */ + public static final int STEER_CONTINUOUS_CURRENT_LIMIT = 15; + public static final int STEER_PEAK_CURRENT_LIMIT = 15; + public static final double STEER_PEAK_CURRENT_DURATION = 0.01; + public static final boolean STEER_ENABLE_CURRENT_LIMIT = true; + + public static final int DRIVE_CONTINUOUS_CURRENT_LIMIT = 60; + public static final int DRIVE_PEAK_CURRENT_LIMIT = 60; + public static final double DRIVE_PEAK_CURRENT_DURATION = 0.01; + public static final boolean DRIVE_ENABLE_CURRENT_LIMIT = true; + + /* Motor inversions */ + public static final InvertedValue INVERT_DRIVE_MOTOR = InvertedValue.CounterClockwise_Positive; + public static final InvertedValue INVERT_STEER_MOTOR = InvertedValue.Clockwise_Positive; + + /* Neutral Modes */ + public static final NeutralModeValue DRIVE_NEUTRAL_MODE = NeutralModeValue.Brake; + public static final NeutralModeValue STEER_NEUTRAL_MODE = NeutralModeValue.Brake; + + /* Drive Motor PID Values */ + public static final double[] P_VALUES = { + 0.1, + 0.1, + 0.1, + 0.1 + }; + public static final double[] I_VALUES = { + 0, + 0, + 0, + 0 + }; + public static final double[] D_VALUES = { + 0, + 0, + 0, + 0 + }; + /* Drive Motor Characterization Values + * Divide SYSID values by 12 to convert from volts to percent output for CTRE */ + public static final double[] S_VALUES = { + 0.11, + 0.11, + 0.11, + 0.11 + }; + public static final double[] V_VALUES = { + 0.11079, + 0.10718, + 0.11009, + 0.1164 + }; + public static final double[] A_VALUES = { + 0.005482, + 0.0049593, + 0.010156, + 0.0065708 + }; + /* Ramp values for drive motors in open loop driving. */ + // Open loop prevents throttle from changing too quickly. + // It will limit it to time given (in seconds) to go from zero to full throttle. + // A small open loop ramp (0.25) helps with tread wear, tipping, etc + public static final double OPEN_LOOP_RAMP = 0.25; + + public static final double WHEEL_CIRCUMFERENCE = 2*Math.PI*WHEEL_RADIUS; + + public static final boolean INVERT_GYRO = false; // Make sure gyro is CCW+ CW- + + public static final double SLOW_DRIVE_FACTOR = 0.2; + public static final double SLOW_ROT_FACTOR = 0.1; + + public static final ModuleLimits MODULE_LIMITS = new ModuleLimits(MAX_SPEED, MAX_DRIVE_ACCEL, COSF, Units.rotationsPerMinuteToRadiansPerSecond(Constants.MAX_RPM / STEER_GEAR_RATIO)); + + /** + * Updates the constants if the RobotId is not the competition robot. + */ + public static void update(RobotId robotId) { + if(robotId == RobotId.BetaBot) { + STEER_OFFSET_FRONT_LEFT = 193.884-180; + STEER_OFFSET_FRONT_RIGHT = 110.914; + STEER_OFFSET_BACK_LEFT = 128.054+180; + STEER_OFFSET_BACK_RIGHT = 107.43; + } else if (robotId == RobotId.Vivace) { + STEER_OFFSET_FRONT_LEFT = 100.184+180; + STEER_OFFSET_FRONT_RIGHT = 224.293; + STEER_OFFSET_BACK_LEFT = 304.795-180; + STEER_OFFSET_BACK_RIGHT = 201.177-180; + + ROBOT_MASS = 50; + WHEEL_MOI = 0.000326 * ROBOT_MASS; + + } else if (robotId == RobotId.Vertigo) { + STEER_OFFSET_FRONT_LEFT = 4.185; + STEER_OFFSET_FRONT_RIGHT = 101.519+90; + STEER_OFFSET_BACK_LEFT = 38.997+180; + STEER_OFFSET_BACK_RIGHT = 242.847-90; + + DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + + ROBOT_MASS = 20; + + WHEEL_MOI = 0.000326 * ROBOT_MASS; + + // Falcon Speed + Constants.MAX_RPM = 6080.0; + } else if (robotId == RobotId.Phil) { + ROBOT_MASS = 30; + WHEEL_MOI = 0.000326 * ROBOT_MASS; + + STEER_OFFSET_FRONT_LEFT = 121.463+180; + STEER_OFFSET_FRONT_RIGHT = 284.242; + STEER_OFFSET_BACK_LEFT = 157.676; + STEER_OFFSET_BACK_RIGHT = 77.199; + + DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + } + + MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO); + } +} diff --git a/src/main/java/frc/robot/constants/swerve/ModuleConstants.java b/src/main/java/frc/robot/constants/swerve/ModuleConstants.java new file mode 100644 index 0000000..7074482 --- /dev/null +++ b/src/main/java/frc/robot/constants/swerve/ModuleConstants.java @@ -0,0 +1,145 @@ +package frc.robot.constants.swerve; + +import frc.robot.constants.IdConstants; + +/** + * Container class for module constants, defined using constants from {@link DriveConstants} + * . + * + * @see DriveConstants + */ +public enum ModuleConstants { + + FRONT_LEFT( + IdConstants.DRIVE_FRONT_LEFT_ID, + IdConstants.STEER_FRONT_LEFT_ID, + IdConstants.ENCODER_FRONT_LEFT_ID, + DriveConstants.STEER_OFFSET_FRONT_LEFT, + ModuleType.FRONT_LEFT, + DriveConstants.S_VALUES[0], + DriveConstants.V_VALUES[0], + DriveConstants.A_VALUES[0], + DriveConstants.P_VALUES[0], + DriveConstants.I_VALUES[0], + DriveConstants.D_VALUES[0] + ), + FRONT_RIGHT( + IdConstants.DRIVE_FRONT_RIGHT_ID, + IdConstants.STEER_FRONT_RIGHT_ID, + IdConstants.ENCODER_FRONT_RIGHT_ID, + DriveConstants.STEER_OFFSET_FRONT_RIGHT, + ModuleType.FRONT_RIGHT, + DriveConstants.S_VALUES[1], + DriveConstants.V_VALUES[1], + DriveConstants.A_VALUES[1], + DriveConstants.P_VALUES[1], + DriveConstants.I_VALUES[1], + DriveConstants.D_VALUES[1] + ), + BACK_LEFT( + IdConstants.DRIVE_BACK_LEFT_ID, + IdConstants.STEER_BACK_LEFT_ID, + IdConstants.ENCODER_BACK_LEFT_ID, + DriveConstants.STEER_OFFSET_BACK_LEFT, + ModuleType.BACK_LEFT, + DriveConstants.S_VALUES[2], + DriveConstants.V_VALUES[2], + DriveConstants.A_VALUES[2], + DriveConstants.P_VALUES[2], + DriveConstants.I_VALUES[2], + DriveConstants.D_VALUES[2] + ), + BACK_RIGHT( + IdConstants.DRIVE_BACK_RIGHT_ID, + IdConstants.STEER_BACK_RIGHT_ID, + IdConstants.ENCODER_BACK_RIGHT_ID, + DriveConstants.STEER_OFFSET_BACK_RIGHT, + ModuleType.BACK_RIGHT, + DriveConstants.S_VALUES[3], + DriveConstants.V_VALUES[3], + DriveConstants.A_VALUES[3], + DriveConstants.P_VALUES[3], + DriveConstants.I_VALUES[3], + DriveConstants.D_VALUES[3] + ), + + NONE(0, 0, 0, 0.0, ModuleType.NONE,0,0,0,0,0,0); + + private final int drivePort; + private final int steerPort; + private final int encoderPort; + private final double steerOffset; + private final double ks; + private final double kv; + private final double ka; + private final double driveP; + private final double driveI; + private final double driveD; + private final ModuleType type; + + ModuleConstants( + int drivePort, + int steerPort, + int encoderPort, + double steerOffset, + ModuleType type, + double ks, + double kv, + double ka, + double driveP, + double driveI, + double driveD + + ) { + this.drivePort = drivePort; + this.steerPort = steerPort; + this.encoderPort = encoderPort; + this.steerOffset = steerOffset; + this.type = type; + this.ks =ks; + this.kv= kv; + this.ka = ka; + this.driveP =driveP; + this.driveI = driveI; + this.driveD = driveD; + } + + public int getDrivePort() { + return drivePort; + } + + public int getSteerPort() { + return steerPort; + } + + public int getEncoderPort() { + return encoderPort; + } + + public double getSteerOffset() { + return steerOffset; + } + + public ModuleType getType() { + return type; + } + public double getDriveS(){ + return ks; + } + public double getDriveV(){ + return kv; + } + public double getDriveA(){ + return ka; + } + public double getDriveP(){ + return driveP; + } + public double getDriveI(){ + return driveI; + } + public double getDriveD(){ + return driveD; + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/swerve/ModuleType.java b/src/main/java/frc/robot/constants/swerve/ModuleType.java new file mode 100644 index 0000000..da71218 --- /dev/null +++ b/src/main/java/frc/robot/constants/swerve/ModuleType.java @@ -0,0 +1,31 @@ +package frc.robot.constants.swerve; + +/** + * Represents the type for a module on the robot. + *

    + * IDs: + * 0 - FRONT_LEFT + * 1 - FRONT_RIGHT + * 2 - BACK_LEFT + * 3 - BACK_RIGHT + */ +public enum ModuleType { + FRONT_LEFT, + FRONT_RIGHT, + BACK_LEFT, + BACK_RIGHT, + NONE; + + public final byte id; + + ModuleType() { + this.id = id(); + } + + private byte id() { + if (this == NONE) + return -1; + // This is a trick that relies on the order the enums are defined. + return (byte) this.ordinal(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/controls/BaseDriverConfig.java b/src/main/java/frc/robot/controls/BaseDriverConfig.java new file mode 100644 index 0000000..b9bd052 --- /dev/null +++ b/src/main/java/frc/robot/controls/BaseDriverConfig.java @@ -0,0 +1,79 @@ +package frc.robot.controls; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.constants.Constants; +import frc.robot.constants.swerve.DriveConstants; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.util.DynamicSlewRateLimiter; +import frc.robot.util.MathUtils; + +/** + * Abstract class for different controller types. + */ +public abstract class BaseDriverConfig { + + private final Drivetrain drive; + + private double previousHeading = 0; + + private final DynamicSlewRateLimiter headingLimiter = new DynamicSlewRateLimiter(Constants.HEADING_SLEWRATE); + + /** + * @param drive the drivetrain instance + * @param controllerTab the shuffleboard controller tab + * @param shuffleboardUpdates whether to update the shuffleboard + */ + public BaseDriverConfig(Drivetrain drive) { + headingLimiter.setContinuousLimits(-Math.PI, Math.PI); + headingLimiter.enableContinuous(true); + this.drive = drive; + } + + public double getForwardTranslation() { + double forward = getRawForwardTranslation(); + return forward * DriveConstants.MAX_SPEED * Math.min(1,RobotController.getBatteryVoltage()/12) * MathUtil.applyDeadband(Math.sqrt(forward*forward + Math.pow(getRawSideTranslation(), 2)), Constants.TRANSLATIONAL_DEADBAND); + } + + public double getSideTranslation() { + double side = getRawSideTranslation(); + return side * DriveConstants.MAX_SPEED * Math.min(1,RobotController.getBatteryVoltage()/12) * MathUtil.applyDeadband(Math.sqrt(side*side + Math.pow(getRawForwardTranslation(), 2)), Constants.TRANSLATIONAL_DEADBAND); + } + + public double getRotation() { + return MathUtils.expoMS(MathUtil.applyDeadband(getRawRotation(), Constants.ROTATION_DEADBAND), 2) + * DriveConstants.MAX_ANGULAR_SPEED * Math.min(1, RobotController.getBatteryVoltage()/12); + } + + public double getHeading() { + if (getRawHeadingMagnitude() <= Constants.HEADING_DEADBAND) + return headingLimiter.calculate(previousHeading, 1e-6); + previousHeading = headingLimiter.calculate(getRawHeadingAngle(), + MathUtils.expoMS(getRawHeadingMagnitude(), 2)); + return previousHeading; + } + + protected Drivetrain getDrivetrain() { + return drive; + } + + /** + * Configures the controls for the controller. + */ + public abstract void configureControls(); + + public abstract double getRawSideTranslation(); + + public abstract double getRawForwardTranslation(); + + public abstract double getRawRotation(); + + public abstract double getRawHeadingAngle(); + + public abstract double getRawHeadingMagnitude(); + + public abstract boolean getIsSlowMode(); + + public abstract boolean getIsAlign(); + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/controls/Driver.java b/src/main/java/frc/robot/controls/Driver.java deleted file mode 100644 index 8433b48..0000000 --- a/src/main/java/frc/robot/controls/Driver.java +++ /dev/null @@ -1,31 +0,0 @@ -package frc.robot.controls; - -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Robot; -import frc.robot.commands.DoNothing; -import frc.robot.constants.Constants; -import frc.robot.util.TestType; -import lib.controllers.GameController; -import lib.controllers.GameController.Button; - -public class Driver { - private static GameController driver = new GameController(Constants.oi.kDriverJoy); - - public static void configureControls() { - - // example button binding implementation - driver.get(Button.A).whenPressed(new DoNothing()); - - // example test type implementation - // tests drivetrain, when in TEST_DRIVE test mode and - driver.get(Button.A).and(Robot.shuffleboard.isTestTypeTrigger(TestType.TEST_DRIVE)).whenActive( - new SequentialCommandGroup( - new RunCommand(() -> Robot.drive.tankDrive(0.5, 0.5), Robot.drive).withTimeout(1), - new RunCommand(() -> Robot.drive.tankDrive(-0.5, -0.5), Robot.drive).withTimeout(1), - new RunCommand(() -> Robot.drive.tankDrive(0.5, -0.5), Robot.drive).withTimeout(1), - new RunCommand(() -> Robot.drive.tankDrive(-0.5, 0.5), Robot.drive).withTimeout(1) - ) - ); - } -} diff --git a/src/main/java/frc/robot/controls/Ex3DProDriverConfig.java b/src/main/java/frc/robot/controls/Ex3DProDriverConfig.java new file mode 100644 index 0000000..84a80fa --- /dev/null +++ b/src/main/java/frc/robot/controls/Ex3DProDriverConfig.java @@ -0,0 +1,66 @@ +package frc.robot.controls; + +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.commands.drive_comm.SetFormationX; +import frc.robot.constants.Constants; +import frc.robot.constants.swerve.DriveConstants; +import frc.robot.subsystems.drivetrain.Drivetrain; +import lib.controllers.Ex3DProController; +import lib.controllers.Ex3DProController.Ex3DProAxis; +import lib.controllers.Ex3DProController.Ex3DProButton; + +/** + * Driver controls for the Ex3D Pro controller. + */ +public class Ex3DProDriverConfig extends BaseDriverConfig { + + private final Ex3DProController kDriver = new Ex3DProController(Constants.DRIVER_JOY); + private final BooleanSupplier slowModeSupplier = kDriver.get(Ex3DProButton.B11); + + public Ex3DProDriverConfig(Drivetrain drive) { + super(drive); + } + + @Override + public void configureControls() { + kDriver.get(Ex3DProButton.B1).whileTrue(new SetFormationX(super.getDrivetrain())); + kDriver.get(Ex3DProButton.B2).onTrue(new InstantCommand(() -> super.getDrivetrain().setYaw(DriveConstants.STARTING_HEADING))); + } + + @Override + public double getRawSideTranslation() { + return -kDriver.get(Ex3DProAxis.X); + } + + @Override + public double getRawForwardTranslation() { + return -kDriver.get(Ex3DProAxis.Y); + } + + @Override + public double getRawRotation() { + return kDriver.get(Ex3DProAxis.Z); + } + + @Override + public double getRawHeadingAngle() { + return kDriver.get(Ex3DProAxis.Z) * Math.PI; + } + + @Override + public double getRawHeadingMagnitude() { + return kDriver.get(Ex3DProAxis.SLIDER); + } + + @Override + public boolean getIsSlowMode() { + return slowModeSupplier.getAsBoolean(); + } + + @Override + public boolean getIsAlign() { + return false; + } +} diff --git a/src/main/java/frc/robot/controls/GameControllerDriverConfig.java b/src/main/java/frc/robot/controls/GameControllerDriverConfig.java new file mode 100644 index 0000000..3cf0260 --- /dev/null +++ b/src/main/java/frc/robot/controls/GameControllerDriverConfig.java @@ -0,0 +1,322 @@ +package frc.robot.controls; + +import java.util.function.BooleanSupplier; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Robot; +import frc.robot.commands.drive_comm.DriveToPose; +import frc.robot.commands.gpm.IntakeAlgae; +import frc.robot.commands.gpm.IntakeAlgaeArm; +import frc.robot.commands.gpm.IntakeCoral; +import frc.robot.commands.gpm.MoveArm; +import frc.robot.commands.gpm.MoveElevator; +import frc.robot.commands.gpm.NetSetpoint; +import frc.robot.commands.gpm.OuttakeAlgae; +import frc.robot.commands.gpm.OuttakeCoral; +import frc.robot.commands.gpm.ResetClimb; +import frc.robot.commands.gpm.ReverseMotors; +import frc.robot.commands.gpm.StationIntake; +import frc.robot.constants.ArmConstants; +import frc.robot.constants.Constants; +import frc.robot.constants.ElevatorConstants; +import frc.robot.constants.FieldConstants; +import frc.robot.constants.VisionConstants; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.climb.Climb; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.outtake.Outtake; +import lib.controllers.GameController; +import lib.controllers.GameController.Axis; +import lib.controllers.GameController.Button; +import lib.controllers.GameController.DPad; + +/** + * Driver controls for the generic game controller. + */ +public class GameControllerDriverConfig extends BaseDriverConfig { + public static final boolean singleAlignmentButton = true; + + private final GameController driver = new GameController(Constants.DRIVER_JOY); + private final BooleanSupplier slowModeSupplier = driver.get(Button.RIGHT_JOY); + private final Elevator elevator; + private final Intake intake; + private final Indexer indexer; + private final Outtake outtake; + private final Climb climb; + private final Arm arm; + private int alignmentDirection = 0; + private Pose2d alignmentPose = null; + + public GameControllerDriverConfig(Drivetrain drive, Elevator elevator, Intake intake, Indexer indexer, + Outtake outtake, Climb climb, Arm arm) { + super(drive); + this.elevator = elevator; + this.intake = intake; + this.indexer = indexer; + this.outtake = outtake; + this.climb = climb; + this.arm = arm; + } + + @Override + public void configureControls() { + Trigger menu = driver.get(Button.LEFT_JOY); + + // Elevator setpoints + if (elevator != null && arm != null) { + driver.get(Button.BACK).and(menu.negate()).onTrue( + new SequentialCommandGroup( + new MoveElevator(elevator, ElevatorConstants.L1_SETPOINT), + new MoveArm(arm, ArmConstants.L1_SETPOINT))); + + driver.get(driver.LEFT_TRIGGER_BUTTON).onTrue( + new SequentialCommandGroup( + new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT), + new MoveArm(arm, ArmConstants.L4_SETPOINT_RIGHT))); + + Command l2Coral = new SequentialCommandGroup( + new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT), + new MoveArm(arm, ArmConstants.L2_L3_SETPOINT)); + Command l3Coral = new SequentialCommandGroup( + new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT), + new MoveArm(arm, ArmConstants.L2_L3_SETPOINT)); + Command l2Algae = new ParallelCommandGroup( + new MoveElevator(elevator, ElevatorConstants.BOTTOM_ALGAE_SETPOINT), + new MoveArm(arm, ArmConstants.ALGAE_SETPOINT)).andThen(new IntakeAlgaeArm(outtake)); + Command l3Algae = new ParallelCommandGroup( + new MoveElevator(elevator, ElevatorConstants.TOP_ALGAE_SETPOINT), + new MoveArm(arm, ArmConstants.ALGAE_SETPOINT)).andThen(new IntakeAlgaeArm(outtake)); + driver.get(Button.RB).whileTrue(new ConditionalCommand(l2Algae, new InstantCommand(l2Coral::schedule), menu)); + driver.get(Button.LB).whileTrue(new ConditionalCommand(l3Algae, new InstantCommand(l3Coral::schedule), menu)); + + // Processor setpoint + driver.get(Button.Y).and(menu.negate()).onTrue( + new ParallelCommandGroup( + new MoveElevator(elevator, ElevatorConstants.SAFE_SETPOINT + 0.001), + new MoveArm(arm, ArmConstants.PROCESSOR_SETPOINT))); + driver.get(DPad.UP).onTrue(new NetSetpoint(elevator, arm, getDrivetrain())); + } + // Intake/outtake + Trigger r3 = driver.get(Button.RIGHT_JOY); + + if (intake != null && indexer != null && elevator != null && outtake != null && arm != null) {// && elevator != null){ + boolean toggle = true; + Command intakeCoral = new IntakeCoral(intake, indexer, elevator, outtake, arm); + Command intakeAlgae = new IntakeAlgae(intake); + driver.get(Button.A).onTrue(new InstantCommand(() -> { + if (r3.getAsBoolean()) + return; + if (menu.getAsBoolean()) { + intakeAlgae.schedule(); + } else { + if (toggle) { + if (intakeCoral.isScheduled()) { + intakeCoral.cancel(); + } else { + intakeCoral.schedule(); + } + } else { + intakeCoral.schedule(); + } + } + })).onFalse(new InstantCommand(() -> { + if (!toggle) { + intakeCoral.cancel(); + } + intakeAlgae.cancel(); + })); + // On true, run the command to start intaking + // On false, run the command to finish intaking if it has a coral + Command startIntake = new StationIntake(outtake); + // Command finishIntake = new FinishStationIntake(intake, indexer, elevator, + // outtake); + driver.get(Button.A).and(r3).and(menu.negate()).onTrue(startIntake) + .onFalse(new InstantCommand(() -> { + if (!startIntake.isScheduled()) { + // finishIntake.schedule(); + } else { + startIntake.cancel(); + } + })); + } + + if (intake != null && outtake != null && arm != null && elevator != null) { + driver.get(DPad.DOWN).and(menu).onTrue(new SequentialCommandGroup( + new OuttakeAlgae(outtake, intake), + new InstantCommand(() -> { + arm.setSetpoint(ArmConstants.START_ANGLE); + elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT); + getDrivetrain().setIsAlign(false); + }, elevator))); + } + + if (outtake != null && elevator != null && arm != null) { + driver.get(DPad.DOWN).and(menu.negate()) + .onTrue(new OuttakeCoral(outtake, elevator, arm) + .alongWith(new InstantCommand(() -> getDrivetrain().setDesiredPose(() -> null))) + .andThen(new SequentialCommandGroup(new MoveArm(arm, ArmConstants.INTAKE_SETPOINT), + new MoveElevator(elevator, ElevatorConstants.STOW_SETPOINT)))); + } + driver.get(DPad.DOWN).and(menu.negate()).onTrue(new InstantCommand(() -> { + }, getDrivetrain())); + if (intake != null && indexer != null && outtake != null) { + driver.get(Button.B).and(menu.negate()).whileTrue(new ReverseMotors(intake, indexer, outtake)); + } + + // Climb + if (climb != null) { + driver.get(Button.X).and(menu.negate()) + .toggleOnTrue(new StartEndCommand(() -> climb.extend(), () -> climb.climb(), climb)); + if (intake != null) { + driver.get(Button.X).and(menu.negate()).onTrue(new InstantCommand(() -> intake.setAngle(65), intake)); + } + driver.get(Button.X).and(menu).whileTrue(new ResetClimb(climb)); + driver.get(driver.RIGHT_TRIGGER_BUTTON).and(menu).onTrue(new InstantCommand(() -> climb.stow(), climb)); + } + + // Alignment + if (singleAlignmentButton) { + driver.get(DPad.LEFT).toggleOnTrue(new InstantCommand(() -> { + setAlignmentDirection(); + setAlignmentPose(false, true); + }).andThen(new DriveToPose(getDrivetrain(), () -> alignmentPose))); + driver.get(DPad.RIGHT).toggleOnTrue(new InstantCommand(() -> { + setAlignmentDirection(); + setAlignmentPose(false, false); + }).andThen(new DriveToPose(getDrivetrain(), () -> alignmentPose))); + } else { + driver.get(DPad.LEFT).onTrue(new InstantCommand(() -> setAlignmentPose(false, true)) + .andThen(new DriveToPose(getDrivetrain(), () -> alignmentPose))); + driver.get(DPad.RIGHT).onTrue(new InstantCommand(() -> setAlignmentPose(false, false)) + .andThen(new DriveToPose(getDrivetrain(), () -> alignmentPose))); + } + + // Reset yaw to be away from driver + driver.get(Button.START).onTrue(new InstantCommand(() -> super.getDrivetrain().setYaw( + new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)))); + + // Cancel commands + driver.get(driver.RIGHT_TRIGGER_BUTTON).and(menu.negate()).onTrue(new InstantCommand(() -> { + if (elevator != null) { + if (outtake != null && outtake.coralLoaded()) { + elevator.setSetpoint(ElevatorConstants.INTAKE_STOW_SETPOINT); + } else { + elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT); + } + } + if (outtake != null) { + outtake.stop(); + } + if (intake != null) { + intake.stow(); + intake.deactivate(); + } + if (indexer != null) { + indexer.stop(); + } + if (climb != null) { + climb.stow(); + } + if (arm != null) { + if (outtake != null && outtake.coralLoaded()) { + arm.setSetpoint(ArmConstants.STOW_SETPOINT); + } else { + arm.setSetpoint(ArmConstants.START_ANGLE); + } + } + getDrivetrain().setIsAlign(false); + getDrivetrain().setDesiredPose(() -> null); + CommandScheduler.getInstance().cancelAll(); + })); + } + + private void setAlignmentDirection() { + Translation2d drivePose = getDrivetrain().getPose().getTranslation(); + int closestDirection = 0; + double closestDist = 20; + boolean isRed = Robot.getAlliance() == Alliance.Red; + int start = isRed ? 5 : 16; + for (int i = 0; i < 6; i++) { + double dist = FieldConstants.APRIL_TAGS.get(start + i).pose.toPose2d().getTranslation().getDistance(drivePose); + if (dist < closestDist) { + closestDist = dist; + closestDirection = i; + } + } + if (isRed) { + closestDirection = (8 - closestDirection) % 6; + } + alignmentDirection = closestDirection; + } + + /** + * Sets the drivetrain's alignment pose to the selected position + * + * @param isAlgae True for algae, false for branches + * @param isLeft True for left branch, false for right, ignored for algae + */ + private void setAlignmentPose(boolean isAlgae, boolean isLeft) { + int id = Robot.getAlliance() == Alliance.Blue ? alignmentDirection + 17 + : (8 - alignmentDirection) % 6 + 6; + if (isAlgae) { + alignmentPose = VisionConstants.REEF.fromAprilTagIdAlgae(id).pose; + } else { + alignmentPose = VisionConstants.REEF.fromAprilTagIdAndPose(id, isLeft).pose; + } + } + + @Override + public double getRawForwardTranslation() { + return driver.get(Axis.LEFT_Y); + } + + @Override + public double getRawSideTranslation() { + return driver.get(Axis.LEFT_X); + } + + @Override + public double getRawRotation() { + return driver.get(Axis.RIGHT_X); + } + + @Override + public double getRawHeadingAngle() { + return Math.atan2(driver.get(Axis.RIGHT_X), -driver.get(Axis.RIGHT_Y)) - Math.PI / 2; + } + + @Override + public double getRawHeadingMagnitude() { + return Math.hypot(driver.get(Axis.RIGHT_X), driver.get(Axis.RIGHT_Y)); + } + + @Override + public boolean getIsSlowMode() { + return slowModeSupplier.getAsBoolean(); + } + + @Override + public boolean getIsAlign() { + return false; + // return kDriver.LEFT_TRIGGER_BUTTON.getAsBoolean(); + } + + public GameController getGameController() { + return driver; + } + +} diff --git a/src/main/java/frc/robot/controls/MadCatzDriverConfig.java b/src/main/java/frc/robot/controls/MadCatzDriverConfig.java new file mode 100644 index 0000000..43233a2 --- /dev/null +++ b/src/main/java/frc/robot/controls/MadCatzDriverConfig.java @@ -0,0 +1,67 @@ +package frc.robot.controls; + +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.commands.drive_comm.SetFormationX; +import frc.robot.constants.Constants; +import frc.robot.constants.swerve.DriveConstants; +import frc.robot.subsystems.drivetrain.Drivetrain; +import lib.controllers.MadCatzController; +import lib.controllers.MadCatzController.MadCatzAxis; +import lib.controllers.MadCatzController.MadCatzButton; + +/** + * Driver controls for the MadCatz controller. + */ +public class MadCatzDriverConfig extends BaseDriverConfig { + + private final MadCatzController kDriver = new MadCatzController(Constants.DRIVER_JOY); + private final BooleanSupplier slowModeSupplier = kDriver.get(MadCatzButton.B6); + + public MadCatzDriverConfig(Drivetrain drive) { + super(drive); + } + + @Override + public void configureControls() { + kDriver.get(MadCatzButton.B1).whileTrue(new SetFormationX(super.getDrivetrain())); + kDriver.get(MadCatzButton.B2).onTrue(new InstantCommand(() -> super.getDrivetrain().setYaw(DriveConstants.STARTING_HEADING))); + } + + @Override + public double getRawSideTranslation() { + return kDriver.get(MadCatzAxis.X); + } + + @Override + public double getRawForwardTranslation() { + return -kDriver.get(MadCatzAxis.Y); + } + + @Override + public double getRawRotation() { + return kDriver.get(MadCatzAxis.ZROTATE); + } + + @Override + public double getRawHeadingAngle() { + return kDriver.get(MadCatzAxis.ZROTATE) * Math.PI; + } + + @Override + public double getRawHeadingMagnitude() { + return kDriver.get(MadCatzAxis.SLIDER); + } + + @Override + public boolean getIsSlowMode() { + return slowModeSupplier.getAsBoolean(); + } + + @Override + public boolean getIsAlign() { + return false; + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/controls/Operator.java b/src/main/java/frc/robot/controls/Operator.java index 0da08c5..31ae3d1 100644 --- a/src/main/java/frc/robot/controls/Operator.java +++ b/src/main/java/frc/robot/controls/Operator.java @@ -1,14 +1,167 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + package frc.robot.controls; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Robot; import frc.robot.commands.DoNothing; +import frc.robot.commands.gpm.IntakeAlgae; +import frc.robot.commands.gpm.IntakeCoral; +import frc.robot.commands.gpm.MoveElevator; +import frc.robot.commands.gpm.OuttakeAlgaeIntake; +import frc.robot.commands.gpm.OuttakeCoral; +import frc.robot.commands.gpm.RemoveAlgae; +import frc.robot.commands.gpm.ReverseMotors; +import frc.robot.commands.gpm.StationIntake; import frc.robot.constants.Constants; +import frc.robot.constants.ElevatorConstants; +import frc.robot.constants.IntakeConstants; +import frc.robot.constants.VisionConstants; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.climb.Climb; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.outtake.Outtake; import lib.controllers.GameController; import lib.controllers.GameController.Button; +import lib.controllers.GameController.DPad; +/** + * Controls for the operator, which are almost a duplicate of most of the driver's controls + */ public class Operator { - private static GameController operator = new GameController(Constants.oi.kOperatorJoy); - public static void configureControls() { - operator.get(Button.A).whenPressed(new DoNothing()); - } + private final GameController driver = new GameController(Constants.OPERATOR_JOY); + + private final Drivetrain drive; + private final Elevator elevator; + private final Intake intake; + private final Indexer indexer; + private final Outtake outtake; + private final Climb climb; + private final Arm arm; + private int alignmentDirection = 0; + + public Operator(Drivetrain drive, Elevator elevator, Intake intake, Indexer indexer, Outtake outtake, Climb climb, Arm arm) { + this.drive = drive; + this.elevator = elevator; + this.intake = intake; + this.indexer = indexer; + this.outtake = outtake; + this.climb = climb; + this.arm = arm; + } + + public void configureControls() { + // TODO: Update to match new driver controls, except the ones that move the drivetrain + Trigger menu = driver.get(Button.LEFT_JOY); + + // Elevator setpoints + if(elevator != null && outtake != null) { + driver.get(Button.BACK).onTrue(new MoveElevator(elevator, ElevatorConstants.L1_SETPOINT).deadlineFor(new RemoveAlgae(outtake))); + driver.get(Button.LB).onTrue(new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT).deadlineFor(new RemoveAlgae(outtake))); + driver.get(Button.RB).and(menu.negate()).onTrue(new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT).deadlineFor(new RemoveAlgae(outtake))); + driver.get(driver.LEFT_TRIGGER_BUTTON).onTrue(new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT).deadlineFor(new RemoveAlgae(outtake))); + driver.get(Button.Y).and(menu.negate()).onTrue(new MoveElevator(elevator, ElevatorConstants.STOW_SETPOINT)); + } + + // Intake/outtake + Trigger r3 = driver.get(Button.RIGHT_JOY); + if(intake != null && indexer != null){// && elevator != null){ + driver.get(Button.A).and(menu.negate()).and(r3.negate()).whileTrue(new IntakeCoral(intake, indexer, elevator, outtake, arm)); + // On true, run the command to start intaking + // On false, run the command to finish intaking if it has a coral + Command startIntake = new StationIntake(outtake); + Command finishIntake = new DoNothing(); + driver.get(Button.A).and(r3).and(menu.negate()).onTrue(startIntake) + .onFalse(new InstantCommand(()->{ + if(!startIntake.isScheduled()){ + finishIntake.schedule(); + }else{ + startIntake.cancel(); + } + })); + } + if(intake != null){ + driver.get(Button.A).and(menu).whileTrue(new IntakeAlgae(intake)); + driver.get(DPad.DOWN).and(menu).onTrue(new OuttakeAlgaeIntake(intake)); + } + if(outtake != null && elevator != null){ + driver.get(DPad.DOWN).and(menu.negate()).onTrue(new OuttakeCoral(outtake, elevator, arm)); + } + if(intake != null && indexer != null){ + driver.get(Button.B).and(menu.negate()).whileTrue(new ReverseMotors(intake, indexer, outtake)); + } + + // Climb + if(climb != null){ + driver.get(Button.X).and(menu.negate()).onTrue(new InstantCommand(()->climb.extend(), climb)) + .onFalse(new InstantCommand(()->climb.climb(), climb)); + if(intake != null){ + driver.get(Button.X).and(menu.negate()).onTrue(new InstantCommand(()->intake.setAngle(IntakeConstants.ALGAE_SETPOINT), intake)); + } + } + + // Alignment + driver.get(Button.B).and(menu).onTrue(new InstantCommand(()->alignmentDirection = 0)); + driver.get(Button.Y).and(menu).onTrue(new InstantCommand(()->alignmentDirection = 2)); + driver.get(Button.X).and(menu).onTrue(new InstantCommand(()->alignmentDirection = 3)); + driver.get(Button.RB).onTrue(new InstantCommand(()->alignmentDirection = 4)); + driver.get(DPad.UP).onTrue(new InstantCommand(()->alignmentDirection = 5)); + driver.get(DPad.LEFT).onTrue(new InstantCommand(()->setAlignmentPose(true))); + driver.get(DPad.RIGHT).onTrue(new InstantCommand(()->setAlignmentPose(false))); + + // Cancel commands + driver.get(Button.START).onTrue(new InstantCommand(()->{ + if(elevator != null){ + elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT); + } + if(outtake != null){ + outtake.stop(); + } + if(intake != null){ + intake.stow(); + intake.deactivate(); + } + if(indexer != null){ + indexer.stop(); + } + if(climb != null){ + climb.stow(); + } + drive.setDesiredPose(()->null); + CommandScheduler.getInstance().cancelAll(); + })); + } + + /** + * Sets the drivetrain's alignmetn pose to the selected position + * @param isLeft True for left branch, false for right + */ + private void setAlignmentPose(boolean isLeft){ + Pose2d pose = VisionConstants.REEF.fromAprilTagIdAndPose( + Robot.getAlliance() == Alliance.Blue ? alignmentDirection + 17 + : (8-alignmentDirection) % 6 + 6, + isLeft).pose; + drive.setDesiredPose(pose); + } + + public Trigger getRightTrigger(){ + return new Trigger(driver.RIGHT_TRIGGER_BUTTON); + } + public Trigger getLeftTrigger(){ + return new Trigger(driver.LEFT_TRIGGER_BUTTON); + } + public GameController getGameController(){ + return driver; + } } diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java new file mode 100644 index 0000000..eaea0f9 --- /dev/null +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -0,0 +1,486 @@ +package frc.robot.controls; + +import java.util.function.BooleanSupplier; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Robot; +import frc.robot.commands.DoNothing; +import frc.robot.commands.drive_comm.DriveToPose; +import frc.robot.commands.gpm.IntakeAlgae; +import frc.robot.commands.gpm.IntakeAlgaeArm; +import frc.robot.commands.gpm.IntakeCoral; +import frc.robot.commands.gpm.MoveArm; +import frc.robot.commands.gpm.MoveElevator; +import frc.robot.commands.gpm.OuttakeAlgae; +import frc.robot.commands.gpm.NetSetpoint; +import frc.robot.commands.gpm.OuttakeCoral; +import frc.robot.commands.gpm.ResetClimb; +import frc.robot.commands.gpm.ReverseMotors; +import frc.robot.commands.gpm.StationIntake; +import frc.robot.constants.ArmConstants; +import frc.robot.constants.Constants; +import frc.robot.constants.ElevatorConstants; +import frc.robot.constants.FieldConstants; +import frc.robot.constants.VisionConstants; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.climb.Climb; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.outtake.Outtake; +import lib.controllers.PS5Controller; +import lib.controllers.PS5Controller.DPad; +import lib.controllers.PS5Controller.PS5Axis; +import lib.controllers.PS5Controller.PS5Button; + +/** + * Driver controls for the PS5 controller + */ +public class PS5ControllerDriverConfig extends BaseDriverConfig { + private final boolean autoOuttake = true; + + private final PS5Controller driver = new PS5Controller(Constants.DRIVER_JOY); + private final Elevator elevator; + private final Intake intake; + private final Indexer indexer; + private final Outtake outtake; + private final Climb climb; + private final Arm arm; + private final BooleanSupplier slowModeSupplier = ()->false; + private Pose2d alignmentPose = null; + // 0 == not selected, -1 == left, 1 == right + private byte selectedDirection = 0; + + public PS5ControllerDriverConfig(Drivetrain drive, Elevator elevator, Intake intake, Indexer indexer, Outtake outtake, Climb climb, Arm arm) { + super(drive); + this.elevator = elevator; + this.intake = intake; + this.indexer = indexer; + this.outtake = outtake; + this.climb = climb; + this.arm = arm; + } + + public void configureControls() { + Trigger menu = driver.get(PS5Button.LEFT_JOY); + + // Elevator setpoints + if(elevator != null && arm != null && outtake != null) { + driver.get(PS5Button.OPTIONS).and(menu.negate()).onTrue( + new SequentialCommandGroup( + new InstantCommand(()->setAlignmentPose(false, true)), + new ConditionalCommand( + new ParallelCommandGroup( + new MoveElevator(elevator, ElevatorConstants.L1_SETPOINT), + new MoveArm(arm, ArmConstants.L1_SETPOINT), + new DriveToPose(getDrivetrain(), ()->alignmentPose) + ), + // This is instant so it doesn't requre the drivetrain for more than 1 frame + new InstantCommand(()->{ + elevator.setSetpoint(ElevatorConstants.L1_SETPOINT); + arm.setSetpoint(ArmConstants.L1_SETPOINT); + }), + ()->selectedDirection != 0 + ), + new ConditionalCommand( + new SequentialCommandGroup( + new OuttakeCoral(outtake, elevator, arm), + new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT), + new InstantCommand(()->{ + elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT); + arm.setSetpoint(ArmConstants.INTAKE_SETPOINT); + alignmentPose = null; + selectedDirection = 0; + }, elevator, arm) + ), + new DoNothing(), + () -> selectedDirection != 0 && autoOuttake + ) + ) + ); + + driver.get(PS5Button.LEFT_TRIGGER).onTrue( + new SequentialCommandGroup( + new InstantCommand(()->setAlignmentPose(true)), + new ConditionalCommand( + new ParallelCommandGroup( + new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT), + new ConditionalCommand( + new MoveArm(arm, ArmConstants.L4_SETPOINT_RIGHT), + new MoveArm(arm, ArmConstants.L4_SETPOINT_LEFT), + () -> selectedDirection >= 0 + ), + new DriveToPose(getDrivetrain(), ()->alignmentPose) + ), + // This is instant so it doesn't requre the drivetrain for more than 1 frame + new InstantCommand(()->{ + elevator.setSetpoint(ElevatorConstants.L4_SETPOINT); + arm.setSetpoint(ArmConstants.L4_SETPOINT_RIGHT); + }), + ()->selectedDirection != 0 + ), + new ConditionalCommand( + new WaitCommand(0.5), + new DoNothing(), + () -> selectedDirection < 0 + ), + new ConditionalCommand( + new OuttakeCoral(outtake, elevator, arm) + .andThen(new InstantCommand(()->{ + elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT); + arm.setSetpoint(ArmConstants.INTAKE_SETPOINT); + alignmentPose = null; + selectedDirection = 0; + }, elevator, arm)), + new DoNothing(), + () -> selectedDirection != 0 && autoOuttake + ) + ) + ); + + Command l2Coral = new SequentialCommandGroup( + new SequentialCommandGroup( + new InstantCommand(()->setAlignmentPose(false)), + new ConditionalCommand( + new ParallelCommandGroup( + new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT), + new MoveArm(arm, ArmConstants.L2_L3_SETPOINT), + new DriveToPose(getDrivetrain(), ()->alignmentPose) + ), + // This is instant so it doesn't requre the drivetrain for more than 1 frame + new InstantCommand(()->{ + elevator.setSetpoint(ElevatorConstants.L2_SETPOINT); + arm.setSetpoint(ArmConstants.L2_L3_SETPOINT); + }), + ()->selectedDirection != 0 + ), + new ConditionalCommand( + new OuttakeCoral(outtake, elevator, arm) + .andThen(new InstantCommand(()->{ + elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT); + arm.setSetpoint(ArmConstants.INTAKE_SETPOINT); + alignmentPose = null; + selectedDirection = 0; + }, elevator, arm)), + new DoNothing(), + () -> selectedDirection != 0 && autoOuttake + ) + ) + ); + Command l3Coral = new SequentialCommandGroup( + new SequentialCommandGroup( + new InstantCommand(()->setAlignmentPose(false)), + new ConditionalCommand( + new ParallelCommandGroup( + new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT), + new MoveArm(arm, ArmConstants.L2_L3_SETPOINT), + new DriveToPose(getDrivetrain(), ()->alignmentPose) + ), + // This is instant so it doesn't requre the drivetrain for more than 1 frame + new InstantCommand(()->{ + elevator.setSetpoint(ElevatorConstants.L3_SETPOINT); + arm.setSetpoint(ArmConstants.L2_L3_SETPOINT); + }), + ()->selectedDirection != 0 + ), + new ConditionalCommand( + new OuttakeCoral(outtake, elevator, arm) + .andThen(new InstantCommand(()->{ + elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT); + arm.setSetpoint(ArmConstants.INTAKE_SETPOINT); + alignmentPose = null; + selectedDirection = 0; + }, elevator, arm)), + new DoNothing(), + () -> selectedDirection != 0 && autoOuttake + ) + ) + ); + Command l2Algae = new ParallelCommandGroup( + new MoveElevator(elevator, ElevatorConstants.BOTTOM_ALGAE_SETPOINT), + new MoveArm(arm, ArmConstants.ALGAE_SETPOINT)).andThen(new IntakeAlgaeArm(outtake)); + Command l3Algae = new ParallelCommandGroup( + new MoveElevator(elevator, ElevatorConstants.TOP_ALGAE_SETPOINT), + new MoveArm(arm, ArmConstants.ALGAE_SETPOINT)).andThen(new IntakeAlgaeArm(outtake)); + driver.get(PS5Button.RB).whileTrue(new ConditionalCommand(l2Algae, new InstantCommand(l2Coral::schedule), menu)); + driver.get(PS5Button.LB).whileTrue(new ConditionalCommand(l3Algae, new InstantCommand(l3Coral::schedule), menu)); + + //Processor setpoint + driver.get(PS5Button.TRIANGLE).and(menu.negate()).onTrue( + new ParallelCommandGroup( + new MoveElevator(elevator, ElevatorConstants.SAFE_SETPOINT + 0.001), + new MoveArm(arm, ArmConstants.PROCESSOR_SETPOINT) + ) + ); + driver.get(DPad.UP).onTrue(new NetSetpoint(elevator, arm, getDrivetrain())); + } + + // Intake/outtake + Trigger r3 = driver.get(PS5Button.RIGHT_JOY); + + if(intake != null && indexer != null && elevator != null && outtake != null && arm != null){ + boolean toggle = true; + Command intakeCoral = new IntakeCoral(intake, indexer, elevator, outtake, arm); + Command intakeAlgae = new IntakeAlgae(intake); + driver.get(PS5Button.CROSS).onTrue(new InstantCommand(()->{ + if(r3.getAsBoolean()) return; + if(menu.getAsBoolean()){ + intakeAlgae.schedule(); + }else{ + if(toggle){ + if(intakeCoral.isScheduled()){ + intakeCoral.cancel(); + }else{ + intakeCoral.schedule(); + } + }else{ + intakeCoral.schedule(); + } + } + })).onFalse(new InstantCommand(()->{ + if(!toggle){ + intakeCoral.cancel(); + } + intakeAlgae.cancel(); + })); + // On true, run the command to start intaking + // On false, run the command to finish intaking if it has a coral + Command startIntake = new StationIntake(outtake); + // Command finishIn6take = new FinishStationIntake(intake, indexer, elevator, outtake); + // driver.get(PS5Button.CROSS).and(r3).and(menu.negate()).onTrue(startIntake) + // .onFalse(new InstantCommand(()->{ + // if(!startIntake.isScheduled()){ + // // finishIntake.schedule(); + // }else{ + // startIntake.cancel(); + // } + // })); + driver.get(PS5Button.CROSS).and(r3).onTrue( + new SequentialCommandGroup( + new MoveElevator(elevator, ElevatorConstants.STATION_INTAKE_SETPOINT), + new MoveArm(arm, ArmConstants.STATION_INTAKE_SETPOINT)). + andThen(startIntake)); + } + + if(intake != null && outtake != null && arm != null && elevator != null){ + Command algae = new SequentialCommandGroup( + new OuttakeAlgae(outtake, intake), + // Only move the arm and elevator in sequence when scoring in the net + new ConditionalCommand( + new SequentialCommandGroup( + new MoveArm(arm, ArmConstants.ALGAE_STOW_SETPOINT), + new InstantCommand(()-> elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT)), + new WaitCommand(0.25), + new InstantCommand(()-> arm.setSetpoint(ArmConstants.INTAKE_SETPOINT)) + ), + new InstantCommand(()->{ + elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT); + arm.setSetpoint(ArmConstants.INTAKE_SETPOINT); + }), + // 1 meter is in between L3 and net setpoints + () -> elevator.getSetpoint() > 1 + ) + ); + Command coral = new OuttakeCoral(outtake, elevator, arm).alongWith(new InstantCommand(()->getDrivetrain().setDesiredPose(()->null))) + .andThen( + new ConditionalCommand( + new SequentialCommandGroup(new MoveArm(arm, ArmConstants.INTAKE_SETPOINT), new MoveElevator(elevator, ElevatorConstants.STOW_SETPOINT), new InstantCommand(()->selectedDirection = 0)), + new DoNothing(), + ()->!arm.canMoveElevator() + )); + Command cancelAlign = new InstantCommand(()->{}, getDrivetrain()); + + driver.get(DPad.DOWN).onTrue(new InstantCommand(()->{ + if(menu.getAsBoolean()){ + algae.schedule(); + }else{ + coral.schedule(); + } + cancelAlign.schedule(); + })); + } + if(intake != null && indexer != null && outtake != null){ + driver.get(PS5Button.CIRCLE).and(menu.negate()).whileTrue(new ReverseMotors(intake, indexer, outtake)); + } + + // Climb + if(climb != null){ + driver.get(PS5Button.SQUARE).and(menu.negate()).toggleOnTrue(new StartEndCommand(()->climb.extend(), ()->climb.climb(), climb)); + if(intake != null){ + driver.get(PS5Button.SQUARE).and(menu.negate()).onTrue(new InstantCommand(()->intake.setAngle(65), intake)); + } + driver.get(PS5Button.PS).and(menu).whileTrue(new ResetClimb(climb)); + driver.get(PS5Button.RIGHT_TRIGGER).and(menu).onTrue(new InstantCommand(()->climb.stow(), climb)); + } + + // Alignment + driver.get(DPad.LEFT).toggleOnTrue(new InstantCommand(()->{ + selectedDirection = -1; + })); + driver.get(DPad.RIGHT).toggleOnTrue(new InstantCommand(()->{ + selectedDirection = 1; + })); + driver.get(PS5Button.TOUCHPAD).toggleOnTrue(new InstantCommand(()->{ + setAlignmentPose(true, false, false, false); + }).andThen(new DriveToPose(getDrivetrain(), ()->alignmentPose))); + + // Reset the yaw. Mainly useful for testing/driver practice + driver.get(PS5Button.CREATE).and(menu.negate()).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( + new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI) + ))); + driver.get(PS5Button.CREATE).and(menu).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( + new Rotation2d(Robot.getAlliance() == Alliance.Blue ? Math.PI*5/6 : -Math.PI/6) + ))); + + // Cancel commands + driver.get(PS5Button.RIGHT_TRIGGER).and(menu.negate()).onTrue(new InstantCommand(()->{ + if(elevator != null){ + if(outtake != null && outtake.coralLoaded()){ + elevator.setSetpoint(ElevatorConstants.INTAKE_STOW_SETPOINT); + }else{ + elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT); + } + } + if(outtake != null){ + outtake.stop(); + } + if(intake != null){ + intake.stow(); + intake.deactivate(); + } + if(indexer != null){ + indexer.stop(); + } + if(climb != null){ + climb.stow(); + } + if(arm != null){ + if(outtake != null && outtake.coralLoaded()){ + arm.setSetpoint(ArmConstants.STOW_SETPOINT); + }else{ + arm.setSetpoint(ArmConstants.START_ANGLE); + } + } + getDrivetrain().setIsAlign(false); + getDrivetrain().setDesiredPose(()->null); + alignmentPose = null; + selectedDirection = 0; + CommandScheduler.getInstance().cancelAll(); + })); + + driver.get(PS5Button.MUTE).and(menu).onTrue(new FunctionalCommand( + ()->getDrivetrain().setStateDeadband(false), + getDrivetrain()::alignWheels, + interrupted->getDrivetrain().setStateDeadband(true), + ()->false, getDrivetrain()).withTimeout(2)); + } + + /** + * Sets the drivetrain's alignmetn pose to the nearest reef branch or algae location + * @param isAlgae True for algae, false for branches + * @param isLeft True for left branch, false for right, ignored for algae + * @param l4 If the robot should align to the L4 scoring pose + */ + private void setAlignmentPose(boolean isAlgae, boolean isLeft, boolean l4, boolean l1){ + Translation2d drivePose = getDrivetrain().getPose().getTranslation(); + int closestId = 0; + double closestDist = 20; + boolean isRed = Robot.getAlliance() == Alliance.Red; + int start = isRed ? 5 : 16; + for(int i = 0; i < 6; i++){ + double dist = FieldConstants.APRIL_TAGS.get(start+i).pose.toPose2d().getTranslation().getDistance(drivePose); + if(dist < closestDist){ + closestDist = dist; + closestId = start+i+1; + } + } + if(isAlgae){ + alignmentPose = VisionConstants.REEF.fromAprilTagIdAlgae(closestId).pose; + }else{ + if(l4){ + alignmentPose = VisionConstants.REEF.fromAprilTagIdAndPose(closestId, isLeft).l4Pose; + }else if(l1){ + alignmentPose = VisionConstants.REEF.fromAprilTagIdAndPose(closestId, isLeft).l1Pose; + }else{ + alignmentPose = VisionConstants.REEF.fromAprilTagIdAndPose(closestId, isLeft).pose; + } + } + } + + /** + * Sets the drivetrain's alignmetn pose to the nearest reef branch with the selected direction + * @param l4 If the robot should align to the L4 scoring pose + * @param l1 If the robot should align to the L1 scoring pose + */ + private void setAlignmentPose(boolean l4, boolean l1){ + if(selectedDirection == 0){ + alignmentPose = null; + return; + } + setAlignmentPose(false, selectedDirection < 0, l4, l1); + } + /** + * Sets the drivetrain's alignmetn pose to the nearest reef branch with the selected direction + * @param l4 If the robot should align to the L4 scoring pose + */ + private void setAlignmentPose(boolean l4){ + setAlignmentPose(l4, false); + } + + @Override + public double getRawSideTranslation() { + return driver.get(PS5Axis.LEFT_X); + } + + @Override + public double getRawForwardTranslation() { + return driver.get(PS5Axis.LEFT_Y); + } + + @Override + public double getRawRotation() { + return driver.get(PS5Axis.RIGHT_X); + } + + @Override + public double getRawHeadingAngle() { + return Math.atan2(driver.get(PS5Axis.RIGHT_X), -driver.get(PS5Axis.RIGHT_Y)) - Math.PI / 2; + } + + @Override + public double getRawHeadingMagnitude() { + return Math.hypot(driver.get(PS5Axis.RIGHT_X), driver.get(PS5Axis.RIGHT_Y)); + } + + @Override + public boolean getIsSlowMode() { + return slowModeSupplier.getAsBoolean(); + } + + @Override + public boolean getIsAlign() { + return false; + } + + public void startRumble(){ + driver.rumbleOn(); + } + + public void endRumble(){ + driver.rumbleOff(); + } +} diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java deleted file mode 100644 index b3646ea..0000000 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ /dev/null @@ -1,64 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.subsystems; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.Constants; -import frc.robot.util.MotorFactory; - -public class Drivetrain extends SubsystemBase { - - private final WPI_TalonFX m_leftMotor1; - private final WPI_TalonFX m_leftMotor2; - private final WPI_TalonFX m_rightMotor1; - private final WPI_TalonFX m_rightMotor2; - - public Drivetrain() { - - m_leftMotor1 = MotorFactory.createTalonFX(Constants.drive.kLeftMotor1, Constants.kRioCAN); - m_leftMotor2 = MotorFactory.createTalonFX(Constants.drive.kLeftMotor2, Constants.kRioCAN); - m_rightMotor1 = MotorFactory.createTalonFX(Constants.drive.kRightMotor1, Constants.kRioCAN); - m_rightMotor2 = MotorFactory.createTalonFX(Constants.drive.kRightMotor2, Constants.kRioCAN); - - SupplyCurrentLimitConfiguration supplyCurrentLimit = new SupplyCurrentLimitConfiguration(true, 40, 45, 1); - - m_leftMotor1.configSupplyCurrentLimit(supplyCurrentLimit); - m_leftMotor2.configSupplyCurrentLimit(supplyCurrentLimit); - m_rightMotor1.configSupplyCurrentLimit(supplyCurrentLimit); - m_rightMotor2.configSupplyCurrentLimit(supplyCurrentLimit); - - m_leftMotor2.follow(m_leftMotor1); - m_rightMotor2.follow(m_rightMotor1); - } - - /** - * Drives the robot using tank drive controls Tank drive is slightly easier to code but less - * intuitive to control, so this is here as an example for now - * - * @param leftPower the commanded power to the left motors - * @param rightPower the commanded power to the right motors - */ - public void tankDrive(double leftPower, double rightPower) { - m_leftMotor1.set(ControlMode.PercentOutput, leftPower); - m_rightMotor1.set(ControlMode.PercentOutput, rightPower); - } - - /** - * Drives the robot using arcade controls. - * - * @param forward the commanded forward movement - * @param turn the commanded turn rotation - */ - public void arcadeDrive(double throttle, double turn) { - m_leftMotor1.set(ControlMode.PercentOutput, throttle + turn); - m_rightMotor1.set(ControlMode.PercentOutput, throttle - turn); - } -} diff --git a/src/main/java/frc/robot/subsystems/LED/LED.java b/src/main/java/frc/robot/subsystems/LED/LED.java new file mode 100644 index 0000000..a521639 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LED/LED.java @@ -0,0 +1,90 @@ +package frc.robot.subsystems.LED; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.IdConstants; + +import com.ctre.phoenix.led.CANdle; +import com.ctre.phoenix.led.CANdle.LEDStripType; +import com.ctre.phoenix.led.CANdle.VBatOutputMode; +import com.ctre.phoenix.led.Animation; + +public class LED extends SubsystemBase { + + private CANdle candle; + public static final int stripLength = 67; + + // Constructor + public LED() { + this.candle = new CANdle(IdConstants.CANDLE_ID, "rio"); + + candle.configStatusLedState(false); + candle.configLOSBehavior(false); + + System.out.println("Strip type ec: " + candle.configLEDType(LEDStripType.GRB)); + + candle.configBrightnessScalar(1); + candle.configVBatOutput(VBatOutputMode.On); + candle.configV5Enabled(true); // Turns off LEDs + } + + @Override + public void periodic() { + } + + /** + * Sets the color of all the LEDs. + * + * @param red Red value (0-255) + * @param green Green value (0-255) + * @param blue Blue value (0-255) + */ + public void setLEDs(int red, int green, int blue) { + candle.setLEDs(red, green, blue); + } + + /** + * Sets an animation for the LEDs. + * + * @param animation The animation object (e.g., RainbowAnimation, StrobeAnimation, etc.) + */ + public void animate(Animation animation) { + candle.animate(animation); + } + + /** + * Sets the color of a specific section of LEDs. + * + * @param r Red value (0-255) + * @param g Green value (0-255) + * @param b Blue value (0-255) + * @param start Start index of the section + * @param end End index of the section + */ + public void setSection(int r, int g, int b, int start, int end) { + candle.setLEDs(r, g, b, 0, start, end); + } + + /** + * Creates an alternating pattern of two colors across the LEDs. + * + * @param r1 Red value of the first color (0-255) + * @param g1 Green value of the first color (0-255) + * @param b1 Blue value of the first color (0-255) + * @param r2 Red value of the second color (0-255) + * @param g2 Green value of the second color (0-255) + * @param b2 Blue value of the second color (0-255) + * @param size Size of each color block + * @param offset Offset for the starting position of the pattern + * @param total Total number of LEDs + */ + public void alternate(int r1, int g1, int b1, int r2, int g2, int b2, int size, int offset, int total) { + for (int i = -offset; i < total; i += size) { + boolean color2 = ((i - offset) / size) % 2 == 0; + if (color2) { + setSection(r2, g2, b2, i, i + size - 1); + } else { + setSection(r1, g1, b1, i, i + size - 1); + } + } + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java new file mode 100644 index 0000000..0a075db --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -0,0 +1,168 @@ +package frc.robot.subsystems.arm; + +import java.util.function.BooleanSupplier; + +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.sim.TalonFXSimState; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.ArmConstants; +import frc.robot.constants.Constants; +import frc.robot.constants.IdConstants; +import frc.robot.util.PhoenixUtil; + +public class Arm extends SubsystemBase implements ArmIO { + //motor + private TalonFX motor = new TalonFX(IdConstants.ARM_MOTOR); + private TalonFXSimState encoderSim; + + // Mechism2d display + private Mechanism2d simulationMechanism; + private MechanismLigament2d simLigament; + private SingleJointedArmSim armSim; + + private final DutyCycleEncoder absoluteEncoder = new DutyCycleEncoder(IdConstants.ARM_ABSOLUTE_ENCODER); + + private double setpoint = ArmConstants.START_ANGLE; + + private MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0); + + private final ArmFeedforward feedforward = new ArmFeedforward(0, ArmConstants.MASS*ArmConstants.CENTER_OF_MASS_LENGTH/ArmConstants.GEAR_RATIO/ArmConstants.MOTOR.KtNMPerAmp*ArmConstants.MOTOR.rOhms, 0); + + private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); + + private BooleanSupplier elevatorStowed; + + public Arm() { + if (RobotBase.isSimulation()) { + encoderSim = motor.getSimState(); + encoderSim.setRawRotorPosition(Units.degreesToRotations(ArmConstants.START_ANGLE)*ArmConstants.GEAR_RATIO); + armSim = new SingleJointedArmSim( + ArmConstants.MOTOR, + ArmConstants.GEAR_RATIO, + ArmConstants.MOI, + ArmConstants.LENGTH, + Units.degreesToRadians(ArmConstants.MIN_ANGLE), //min angle + Units.degreesToRadians(ArmConstants.MAX_ANGLE), //max angle + true, + Units.degreesToRadians(ArmConstants.START_ANGLE)); + simulationMechanism = new Mechanism2d(3, 3); + MechanismRoot2d root = simulationMechanism.getRoot("Arm", 1.5, 1.5); + simLigament = root.append( + new MechanismLigament2d("angle", 1, ArmConstants.START_ANGLE, 4, new Color8Bit(Color.kAliceBlue)) + ); + SmartDashboard.putData("Arm Display", simulationMechanism); + } + + // resetAbsolute(); + motor.setPosition(Units.degreesToRotations(ArmConstants.START_ANGLE)*ArmConstants.GEAR_RATIO); + + var talonFXConfigs = new TalonFXConfiguration(); + + var slot0Configs = talonFXConfigs.Slot0; + slot0Configs.kS = 0.1; // Static friction compensation (should be >0 if friction exists) + slot0Configs.kG = ArmConstants.MASS * 9.81 * ArmConstants.CENTER_OF_MASS_LENGTH / ArmConstants.GEAR_RATIO; // Gravity compensation + slot0Configs.kV = 0.12; // Velocity gain: 1 rps -> 0.12V + slot0Configs.kA = 0; // Acceleration gain: 1 rps² -> 0V (should be tuned if acceleration matters) + slot0Configs.kP = 0.85; // If position error is 2.5 rotations, apply 12V (0.5 * 2.5 * 12V) + slot0Configs.kI = 0.; // Integral term (usually left at 0 for MotionMagic) + slot0Configs.kD = 0; // Derivative term (used to dampen oscillations) + + // set Motion Magic settings + var motionMagicConfigs = talonFXConfigs.MotionMagic; + motionMagicConfigs.MotionMagicCruiseVelocity = ArmConstants.MAX_VELOCITY * ArmConstants.GEAR_RATIO/Math.PI/2; + motionMagicConfigs.MotionMagicAcceleration = ArmConstants.MAX_ACCELERATION * ArmConstants.GEAR_RATIO/Math.PI/2; + talonFXConfigs.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + motor.getConfigurator().apply(talonFXConfigs); + updateInputs(); + PhoenixUtil.tryUntilOk(100, ()->motor.setNeutralMode(NeutralModeValue.Brake)); + + } + + public void setElevatorStowed(BooleanSupplier elevatorStowed){ + this.elevatorStowed = elevatorStowed; + } + + @Override + public void periodic() { + double setpoint2 = setpoint; + if(elevatorStowed == null || elevatorStowed.getAsBoolean() && Math.abs(setpoint2-ArmConstants.L1_SETPOINT) > 0.0001){ + setpoint2 = ArmConstants.START_ANGLE; + } + double setpointRotations = Units.degreesToRotations(setpoint2) * ArmConstants.GEAR_RATIO; + motor.setControl(voltageRequest.withPosition(setpointRotations).withFeedForward(feedforward.calculate(Units.degreesToRadians(getAngle()), 0))); + updateInputs(); + Logger.recordOutput("Arm/Atsetpoint",atSetpoint()); + } + + @Override + public void simulationPeriodic() { + armSim.setInputVoltage(getAppliedVoltage()); + armSim.update(Constants.LOOP_TIME); + + double armRotations = Units.radiansToRotations(armSim.getAngleRads()); + encoderSim.setRawRotorPosition(armRotations * ArmConstants.GEAR_RATIO); + simLigament.setAngle(getAngle()); + } + + public void setSetpoint(double setpoint) { + + this.setpoint = MathUtil.clamp(setpoint, ArmConstants.MIN_ANGLE, ArmConstants.MAX_ANGLE); + } + + public double getAppliedVoltage() { + return motor.getMotorVoltage().getValueAsDouble(); + } + + /** + * Gets the angle of the arm + * @return The angle in degrees + */ + public double getAngle() { + return inputs.measuredAngle; + } + + public void resetAbsolute(){ + if(RobotBase.isSimulation()){ + motor.setPosition(Units.degreesToRotations(ArmConstants.START_ANGLE)*ArmConstants.GEAR_RATIO); + }else{ + double absolutePosition = absoluteEncoder.get() / ArmConstants.ENCODER_GEAR_RATIO; + motor.setPosition(MathUtil.inputModulus(absolutePosition - Units.degreesToRotations(ArmConstants.OFFSET), -0.5, 0.5)*ArmConstants.GEAR_RATIO); + } + } + + public boolean atSetpoint() { + return Math.abs(getAngle() - setpoint) < ArmConstants.TOLERANCE; + } + + public boolean canMoveElevator() { + return Math.abs(getAngle() - ArmConstants.START_ANGLE) < 5 || Math.abs(getAngle() - ArmConstants.L1_SETPOINT) < 5; + } + + @Override + public void updateInputs(){ + inputs.measuredAngle = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / ArmConstants.GEAR_RATIO; + inputs.currentAmps = motor.getStatorCurrent().getValueAsDouble(); + + Logger.processInputs("Arm", inputs); + Logger.recordOutput("Arm/setpointDeg", setpoint); + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java new file mode 100644 index 0000000..8eab615 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -0,0 +1,13 @@ +package frc.robot.subsystems.arm; + +import org.littletonrobotics.junction.AutoLog; + +public interface ArmIO { + @AutoLog + public static class ArmIOInputs{ + public double measuredAngle = 0.0; + public double currentAmps = 0.0; + } + + public void updateInputs(); +} diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java new file mode 100644 index 0000000..9dceaeb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/Climb.java @@ -0,0 +1,185 @@ +package frc.robot.subsystems.climb; + +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.sim.TalonFXSimState; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.Constants; +import frc.robot.constants.IdConstants; +import frc.robot.util.ClimbArmSim; + +public class Climb extends SubsystemBase { + + private static final double startingPosition = 0; + private static final double extendPosition = 2; + private static final double climbPosition = -0.83; + + //Motors + private final PIDController pid = new PIDController(2.5, 0, 0.0); + + private TalonFX motor = new TalonFX(IdConstants.CLIMB_MOTOR, Constants.CANIVORE_CAN); + private final DCMotor climbGearBox = DCMotor.getKrakenX60(1); + private TalonFXSimState encoderSim; + + //Mechism2d display + private final Mechanism2d simulationMechanism = new Mechanism2d(3, 3); + private final MechanismRoot2d mechanismRoot = simulationMechanism.getRoot("Climb", 1.5, 1.5); + private final MechanismLigament2d simLigament = mechanismRoot.append( + new MechanismLigament2d("angle", 1, startingPosition, 4, new Color8Bit(Color.kAntiqueWhite)) + ); + + private final double versaPlanetaryGearRatio = 1.0; + private final double climbGearRatio = 60.0/1.0; + private final double totalGearRatio = versaPlanetaryGearRatio * climbGearRatio; + + private ClimbArmSim climbSim; + + private double power; + + private boolean resetting = false; + + private final ClimbIOInputsAutoLogged inputs = new ClimbIOInputsAutoLogged(); + + public Climb() { + if (RobotBase.isSimulation()) { + encoderSim = motor.getSimState(); + encoderSim.setRawRotorPosition(Units.degreesToRotations(startingPosition)*totalGearRatio); + + climbSim = new ClimbArmSim( + climbGearBox, + totalGearRatio, + 0.1, + 0.127, + 0, //min angle + Units.degreesToRadians(90), //max angle + true, + Units.degreesToRadians(startingPosition), + 60 + ); + + climbSim.setIsClimbing(true); + SmartDashboard.putData("Climb Display", simulationMechanism); + } + + pid.setIZone(1); + + pid.setSetpoint(Units.degreesToRadians(startingPosition)); + + motor.setPosition(Units.degreesToRotations(startingPosition)*totalGearRatio); + motor.setNeutralMode(NeutralModeValue.Brake); + //SmartDashboard.putData("Climb PID", pid); + } + + @Override + public void periodic() { + + inputs.measuredPositionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble() / totalGearRatio); + inputs.currentAmps = motor.getStatorCurrent().getValueAsDouble(); + Logger.processInputs("Climb", inputs); + + double motorPosition = motor.getPosition().getValueAsDouble(); + double currentPosition = Units.rotationsToRadians(motorPosition/totalGearRatio); + power = pid.calculate(currentPosition); + + if(resetting){ + power = -0.1; + } + + Logger.recordOutput("Climb/Motor Power", power); + Logger.recordOutput("Climb/setpointDeg", motorPosition/totalGearRatio); + + motor.set(MathUtil.clamp(power, -1, 1)); + } + + + @Override + public void simulationPeriodic() { + climbSim.setInput(power * Constants.ROBOT_VOLTAGE); + climbSim.update(Constants.LOOP_TIME); + + double climbRotations = Units.radiansToRotations(climbSim.getAngleRads()); + encoderSim.setRawRotorPosition(climbRotations * totalGearRatio); + + simLigament.setAngle(Units.radiansToDegrees(getAngle())); + } + + /** + * Sets the motor to an angle from 0-90 deg + * @param angle in degrees + */ + public void setAngle(double angle) { + pid.reset(); + pid.setSetpoint(Units.degreesToRadians(angle)); + } + + /** + * Gets the current position of the motor in degrees + * @return The angle in degrees + */ + public double getAngle() { + return inputs.measuredPositionDeg; + } + + /** + * Turns the motor to 90 degrees (extended positiion) + */ + public void extend(){ + double extendAngle = Units.rotationsToDegrees(extendPosition); + setAngle(extendAngle); + } + + /** + * Turns the motor to 0 degrees (climb position) + */ + public void climb(){ + setAngle(Units.rotationsToDegrees(climbPosition)); + } + + /** + * Turns the motor to 0 degrees (climb position) + */ + public void stow(){ + setAngle(startingPosition); + } + + public void reset(boolean resetting){ + this.resetting = resetting; + if(!resetting){ + motor.setPosition(climbPosition*totalGearRatio); + pid.setSetpoint(Units.degreesToRadians(startingPosition)); + pid.reset(); + } + } + + public double getCurrent(){ + return motor.getStatorCurrent().getValueAsDouble(); + } + + //not working + public Mechanism2d getMech2d() { + return simulationMechanism; + } + + /** + * Gets the estimated angle of the climb + * This is slightly inaccurate since it assumes the climb rotates exactly 90 degrees and the motor position is proportional to the climb position + * Used only for 3D robot display + */ + public double getEstimatedClimbAngle(){ + return Units.degreesToRotations(getAngle())/(extendPosition-climbPosition)*Math.PI/2; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java new file mode 100644 index 0000000..6de5982 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java @@ -0,0 +1,11 @@ +package frc.robot.subsystems.climb; + +import org.littletonrobotics.junction.AutoLog; + +public interface ClimbIO { + @AutoLog + public static class ClimbIOInputs{ + public double measuredPositionDeg = 0.0; + public double currentAmps = 0.0; + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java new file mode 100644 index 0000000..0175034 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -0,0 +1,693 @@ +package frc.robot.subsystems.drivetrain; + +import java.util.Arrays; +import java.util.Optional; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.Supplier; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import com.pathplanner.lib.util.PathPlannerLogging; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.Constants; +import frc.robot.constants.FieldConstants; +import frc.robot.constants.VisionConstants; +import frc.robot.constants.swerve.DriveConstants; +import frc.robot.constants.swerve.ModuleConstants; +import frc.robot.util.EqualsUtil; +import frc.robot.util.PhoenixOdometryThread; +import frc.robot.util.SwerveModulePose; +import frc.robot.util.SwerveStuff.SwerveSetpoint; +import frc.robot.util.SwerveStuff.SwerveSetpointGenerator; +import frc.robot.util.Vision.Vision; + +/** + * Represents a swerve drive style drivetrain. + *

    + * Module IDs are: + * 1: Front left + * 2: Front right + * 3: Back left + * 4: Back right + */ +public class Drivetrain extends SubsystemBase { + + protected final Module[] modules; + + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + + public static Lock odometryLock = new ReentrantLock(); + + private SwerveSetpoint currentSetpoint = + new SwerveSetpoint( + new ChassisSpeeds(), + new SwerveModuleState[] { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }); + // Odometry + private final SwerveDrivePoseEstimator poseEstimator; + + // Vision + private final Vision vision; + + + // PID Controllers for chassis movement + private final PIDController xController; + private final PIDController yController; + private final PIDController rotationController; + + // If vision is enabled for drivetrain odometry updating + // DO NOT CHANGE THIS HERE TO DISABLE VISION, change VisionConstants.ENABLED instead + private boolean visionEnabled = true; + + // Disables vision for the first few seconds after deploying + private Timer visionEnableTimer = new Timer(); + + // If the robot should algin to the angle + private boolean isAlign = false; + // Angle to align to, can be null + private Double alignAngle = null; + // used for drift control + private double currentHeading = 0; + // used for drift control + private boolean drive_turning = false; + + private SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(); + + // The pose supplier to drive to + private Supplier desiredPoSupplier = ()->null; + + private SwerveModulePose modulePoses; + + // The previous pose to reset to if the current pose gets too far off the field + private Pose2d prevPose = new Pose2d(); + + private SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];; + + private boolean slipped = false; + + private double previousAngularVelocity = 0; + + private double centerOfMassHeight = 0; + + private Rotation2d rawGyroRotation = new Rotation2d(); + + + /** + * Creates a new Swerve Style Drivetrain. + */ + public Drivetrain(Vision vision, GyroIO gyroIO) { + this.vision = vision; + + modules = new Module[4]; + this.gyroIO = gyroIO; + ModuleConstants[] constants = Arrays.copyOfRange(ModuleConstants.values(), 0, 4); + + if(RobotBase.isReal()){ + Arrays.stream(constants).forEach(moduleConstants -> { + modules[moduleConstants.ordinal()] = new Module(moduleConstants); + }); + }else{ + Arrays.stream(constants).forEach(moduleConstants -> { + modules[moduleConstants.ordinal()] = new ModuleSim(moduleConstants); + }); + } + + /* + * By pausing init for a second before setting module offsets, we avoid a bug + * with inverting motors. + * See https://github.com/Team364/BaseFalconSwerve/issues/8 for more info. + */ + Timer.delay(1.0); + resetModulesToAbsolute(); + gyroIO.updateInputs(gyroInputs); + poseEstimator = new SwerveDrivePoseEstimator( + DriveConstants.KINEMATICS, + gyroInputs.yawPosition, + updateModulePositions(), + new Pose2d(), + // Defaults, except trust pigeon more + VecBuilder.fill(0.1, 0.1, 0), + VisionConstants.VISION_STD_DEVS + ); + poseEstimator.setVisionMeasurementStdDevs(VisionConstants.VISION_STD_DEVS); + + // initialize PID controllers + xController = new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D); + yController = new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D); + rotationController = new PIDController(DriveConstants.HEADING_P, 0, DriveConstants.HEADING_D); + rotationController.enableContinuousInput(-Math.PI, Math.PI); + rotationController.setTolerance(Units.degreesToRadians(0.25), Units.degreesToRadians(0.25)); + + PhoenixOdometryThread.getInstance().start(); + + modulePoses = new SwerveModulePose(this, DriveConstants.MODULE_LOCATIONS); + + + PathPlannerLogging.setLogActivePathCallback( + (activePath) -> { + Logger.recordOutput( + "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + }); + PathPlannerLogging.setLogTargetPoseCallback( + (targetPose) -> { + Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + }); + + //PPLibTelemetry.enableCompetitionMode(); + } + + public void close() { + // close each of the modules + for (int i = 0; i < modules.length; i++) { + modules[i].close(); + } + } + + @Override + public void periodic() { + odometryLock.lock(); // Prevents odometry updates while reading data + gyroIO.updateInputs(gyroInputs); + Logger.processInputs("Drive/Gyro", gyroInputs); + for (var module : modules) { + module.periodic(); + } + odometryLock.unlock(); + // Update odometry + double[] sampleTimestamps = + gyroInputs.odometryYawTimestamps; // All signals are sampled together + int sampleCount = sampleTimestamps.length; + for (int i = 0; i < sampleCount; i++) { + // Read wheel positions and deltas from each module + SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; + } + // Use the real gyro angle + rawGyroRotation = gyroInputs.odometryYawPositions[i]; + // Apply update + poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); + } + Logger.recordOutput("Odometry/module poses", modulePoses.getModulePoses()); + updateOdometryVision(); + } + + // DRIVE + /** + * Method to drive the robot using joystick info. + * + * @param xSpeed speed of the robot in the x direction (forward) in m/s + * @param ySpeed speed of the robot in the y direction (sideways) in m/s + * @param rot angular rate of the robot in rad/s + * @param fieldRelative whether the provided x and y speeds are relative to the field + * @param isOpenLoop whether to use velocity control for the drive motors + */ + public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean isOpenLoop) { + // rot = headingControl(rot, xSpeed, ySpeed); + ChassisSpeeds speeds = ChassisSpeeds.discretize(xSpeed, ySpeed, rot, Constants.LOOP_TIME); + if(fieldRelative){ + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw()); + } + setChassisSpeeds(speeds, isOpenLoop); + } + + /** + * Drives the robot using the provided x speed, y speed, and positional heading. + * + * @param xSpeed speed of the robot in the x direction (forward) + * @param ySpeed speed of the robot in the y direction (sideways) + * @param heading target heading of the robot in radians + * @param fieldRelative whether the provided x and y speeds are relative to the field + */ + public void driveHeading(double xSpeed, double ySpeed, double heading, boolean fieldRelative) { + double rot = rotationController.calculate(getYaw().getRadians(), heading); + ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, rot); + if(fieldRelative){ + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw()); + } + setChassisSpeeds(speeds, false); + } + + /** + * Runs the PID controllers with the provided x, y, and rot values. Then, calls {@link #drive(double, double, double, boolean, boolean)} using the PID outputs. + * This is based on the odometry of the chassis. + * + * @param x the position to move to in the x, in meters + * @param y the position to move to in the y, in meters + * @param rot the angle to move to, in radians + */ + public void driveWithPID(double x, double y, double rot) { + Pose2d pose = getPose(); + double xSpeed = xController.calculate(pose.getX(), x); + double ySpeed = yController.calculate(pose.getY(), y); + double rotRadians = rotationController.calculate(pose.getRotation().getRadians(), rot); + drive(xSpeed, ySpeed, rotRadians, true, false); + } + + /** + * Updates odometry using vision + */ + public void updateOdometryVision() { + // Start the timer if it hasn't started yet + visionEnableTimer.start(); + + // Update the swerve module poses + modulePoses.update(); + + if(modulePoses.slipped()){ + slipped = true; + } + + Pose2d pose2 = getPose(); + + // Even if vision is disabled, it should still update inputs + // This prevents it from storing a lot of unread results, and it could be useful for replays + vision.updateInputs(); + + if(VisionConstants.ENABLED){ + if(vision != null && visionEnabled && visionEnableTimer.hasElapsed(5)){ + vision.updateOdometry(poseEstimator, time->getPoseAt(time).getRotation().getRadians(), slipped); + + if(vision.canSeeTag()){ + slipped = false; + modulePoses.reset(); + } + } + } + + Pose2d pose3 = getPose(); + + // Reset the pose to a position on the field if it is too far off the field + // This uses nearField() instead of onField() so we don't reset the odometry when the wheels slip near the edge of the field + // This is meant for poses that are caused by errors + if(!Vision.nearField(prevPose)){ + // If the pose at the beginning of the method is off the field, reset to a position in the middle of the field + // Use the rotation of the pose after updating odometry so the yaw is right + prevPose = new Pose2d(FieldConstants.FIELD_LENGTH/2, FieldConstants.FIELD_WIDTH/2, pose2.getRotation()); + resetOdometry(prevPose); + }else if(!Vision.nearField(pose2)){ + // if the drivetrain pose is off the field, reset our odometry to the pose before(this is the right pose) + // Keep the rotation from pose2 so yaw is correct for driver + prevPose = new Pose2d(prevPose.getTranslation(), pose2.getRotation()); + resetOdometry(prevPose); + }else if(!Vision.nearField(pose3)){ + //if our vision+drivetrain odometry isn't near the field, reset our odometry to the pose before(this is the right pose) + resetOdometry(pose2); + prevPose = pose2; + }else{ + // Set the previous pose to the current pose if we need to return to that + prevPose = pose3; + } + + // if (Robot.isSimulation()) { + // pigeon.getSimState().addYaw( + // +Units.radiansToDegrees(currentSetpoint.chassisSpeeds().omegaRadiansPerSecond * Constants.LOOP_TIME)); + // } + } + + /** + * Stops all swerve modules. + */ + public void stop() { + Arrays.stream(modules).forEach(Module::stop); + } + + + // GETTERS AND SETTERS + + /** + * Sets the desired states for all swerve modules. + * + * @param swerveModuleStates an array of module states to set swerve modules to. Order of the array matters here! + */ + public void setModuleStates(SwerveModuleState[] swerveModuleStates, boolean isOpenLoop) { + // makes sure speeds of modules don't exceed maximum allowed + SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, DriveConstants.MAX_SPEED); + + for (int i = 0; i < 4; i++) { + modules[i].setDesiredState(swerveModuleStates[i], isOpenLoop); + } + } + + /** + * Sets the chassis speeds of the robot. + * + * @param chassisSpeeds the target chassis speeds + * @param isOpenLoop if open loop control should be used for the drive velocity + */ + public void setChassisSpeeds(ChassisSpeeds chassisSpeeds, boolean isOpenLoop) { + + if(DriveConstants.USE_ACTUAL_SPEED){ + SwerveSetpoint currentState = new SwerveSetpoint(getChassisSpeeds(), getModuleStates()); + currentSetpoint = setpointGenerator.generateSetpoint( + DriveConstants.MODULE_LIMITS, + centerOfMassHeight, + currentState, chassisSpeeds, + Constants.LOOP_TIME); + }else{ + currentSetpoint = setpointGenerator.generateSetpoint( + DriveConstants.MODULE_LIMITS, + centerOfMassHeight, + currentSetpoint, chassisSpeeds, + Constants.LOOP_TIME); + } + + SwerveModuleState[] swerveModuleStates = currentSetpoint.moduleStates(); + setModuleStates(swerveModuleStates, isOpenLoop); + } + + public void setDriveVoltages(Voltage voltage){ + for (int i = 0; i module.setStateDeadband(stateDeadBand)); + } + public void setOptimized(boolean optimized) { + Arrays.stream(modules).forEach(module -> module.setOptimize(optimized)); + } + + public void setVisionEnabled(boolean enabled){ + visionEnabled = enabled; + } + + public void setIsAlign(boolean isAlign){ + this.isAlign = isAlign; + } + public boolean getIsAlign(){ + return isAlign; + } + + /** + * Calculates chassis speed of drivetrain using the current SwerveModuleStates + * @return ChassisSpeeds object + * This is often used as an input for other methods + */ + public ChassisSpeeds getChassisSpeeds() { + return DriveConstants.KINEMATICS.toChassisSpeeds(getModuleStates()); + } + + /** + * Gets the state of each module + * @return An array of 4 SwerveModuleStates + */ + public SwerveModuleState[] getModuleStates(){ + return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); + } + + public SwerveSetpoint getCurrSetpoint(){ + return currentSetpoint; + } + + /** + * @return the yaw of the robot, aka heading, the direction it is facing + */ + public Rotation2d getYaw() { + return getPose().getRotation(); + } + + /** + * @return an array of modules + */ + public Module[] getModules(){ + return modules; + } + + /** + * Resets the yaw of the robot. + * + * @param rotation the new yaw angle as Rotation2d + */ + public void setYaw(Rotation2d rotation) { + resetOdometry(new Pose2d(getPose().getTranslation(), rotation)); + } + + /** + * Resets the odometry to the given pose. + * + * @param pose the pose to reset to. + */ + public void resetOdometry(Pose2d pose) { + // NOTE: must use pigeon yaw for odometer! + currentHeading = pose.getRotation().getRadians(); + poseEstimator.resetPosition(gyroInputs.yawPosition, getModulePositions(), pose); + modulePoses.reset(); + } + + /** + * @return the pose of the robot as estimated by the odometry + */ + @AutoLogOutput(key = "Odometry/Robot") + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** + * Sets the angle to align to + * @param newAngle The new angle in radians, can be set to null + */ + public void setAlignAngle(Double newAngle){ + alignAngle = newAngle; + } + + /** + * Returns whether or not the robot is at the input align angle + * @return true if it within tolerance the align angle, false otherwise + */ + public boolean atAlignAngle(){ + if(alignAngle == null){ + return false; + } + double diff = Math.abs(alignAngle - getYaw().getRadians()); + return diff < DriveConstants.HEADING_TOLERANCE || diff > 2*Math.PI - DriveConstants.HEADING_TOLERANCE; + } + + /** + * Gets the angle to align to + * @return The angle in radians + */ + public double getAlignAngle(){ + if(alignAngle != null){ + return alignAngle; + } + return 0; + } + + /** + * Sets vision to only use certain April tags + * @param ids An array of the tags to only use + */ + public void onlyUseTags(int[] ids){ + if(vision != null){ + vision.onlyUse(ids); + } + } + /** + * Returns if vision has seen an April tag in the last frame + * @return true if vision saw a tag last frame or if vision is disabled + */ + public boolean canSeeTag(){ + // if no vision system, then return true + if (vision == null) return true; + + return vision.canSeeTag() || !visionEnabled || !VisionConstants.ENABLED; + } + + /** + * Gets the pose at a previous time + * @param timestamp The timestamp of the pose to get + * @return The pose, null if there are no poses yet, or the current pose if timestamp < 0 + */ + public Pose2d getPoseAt(double timestamp){ + if(timestamp < 0){ + return getPose(); + } + Optional pose = poseEstimator.sampleAt(timestamp); + if(pose.isPresent()){ + return pose.get(); + }else{ + return null; + } + } + + /** + * Uses pigeon and rotational input to return a rotation that accounts for drift + * @return A rotation + */ + public double headingControl(double rot, double xSpeed, double ySpeed){ + if((!EqualsUtil.epsilonEquals(getAngularRate(0), 0, 0.0004)&&EqualsUtil.epsilonEquals(Math.hypot(xSpeed, ySpeed),0,0.1))||!EqualsUtil.epsilonEquals(rot, 0, 0.0004)){ + drive_turning = true; + currentHeading = getYaw().getRadians(); + } + else{ + drive_turning = false; + } + if (!drive_turning){ + rotationController.setSetpoint(currentHeading); + double output = rotationController.calculate(getYaw().getRadians()); + rot = Math.abs(output) > Math.abs(rot) ? output : rot; + } + return rot; + } + + /** + * Resets the swerve modules from the absolute encoders + */ + public void resetModulesToAbsolute() { + Arrays.stream(modules).forEach(Module::resetToAbsolute); + } + + // getters for the PID Controllers + public PIDController getXController() { + return xController; + } + public PIDController getYController() { + return yController; + } + public PIDController getRotationController() { + return rotationController; + } + + /** + * Set the desired pose to drive to + * This will enable driver assist to go to the pose + * @param supplier The supplier for the desired pose, use ()->null to not use a desired pose + */ + public void setDesiredPose(Supplier supplier){ + desiredPoSupplier = supplier; + } + + /** + * Set the desired pose to drive to + * This will enable driver assist to go to the pose + * @param pose The Pose2d to drive to + */ + public void setDesiredPose(Pose2d pose){ + setDesiredPose(()->pose); + } + + /** + * Gets the current desired pose, or null if there is no desired pose + * @return The Pose2d if it exists, null otherwise + */ + public Pose2d getDesiredPose(){ + return desiredPoSupplier.get(); + } + + public boolean atSetpoint(){ + Pose2d pose = getDesiredPose(); + return pose != null && getPose().getTranslation().getDistance(pose.getTranslation()) < 0.025; + } + + public SwerveModulePose getSwerveModulePose(){ + return modulePoses; + } + + public double getAcceleration() { + double accelX = gyroInputs.accelerationX; + double accelY = gyroInputs.accelerationY; + + double angularVelocity = getAngularRate(3); + double angularAccel = (angularVelocity - previousAngularVelocity) / Constants.LOOP_TIME; + previousAngularVelocity = angularVelocity; + + double pigeonOffsetX = 0.082677; + double pigeonOffsetY = 0.030603444; + + double totalX = accelX + Math.pow(angularVelocity, 2) * pigeonOffsetX + angularAccel * pigeonOffsetY; + double totalY = accelY + Math.pow(angularVelocity, 2) * pigeonOffsetY - angularAccel * pigeonOffsetX; + + return Math.hypot(totalX, totalY); + } + + @AutoLogOutput(key = "Drivetrain/AccelerationFaults") + public boolean accelerationOverMax() { + return getAcceleration() > DriveConstants.MAX_LINEAR_ACCEL; + } + + public void setCenterOfMass(double height){ + centerOfMassHeight = height; + } + + public void alignWheels(){ + SwerveModuleState state = new SwerveModuleState(0, new Rotation2d(1.465)); + setModuleStates(new SwerveModuleState[]{ + state, state, state, state + }, false); + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java b/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java new file mode 100644 index 0000000..ca66aa9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java @@ -0,0 +1,32 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drivetrain; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface GyroIO { + @AutoLog + public static class GyroIOInputs { + public boolean connected = false; + public Rotation2d yawPosition = new Rotation2d(); + public double yawVelocityRadPerSec = 0.0; + public double accelerationX = 0.0; + public double accelerationY = 0.0; + public double[] odometryYawTimestamps = new double[] {}; + public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; + } + + public default void updateInputs(GyroIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java new file mode 100644 index 0000000..1520fdf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java @@ -0,0 +1,72 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drivetrain; + +import java.util.Queue; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.LinearAcceleration; +import frc.robot.constants.IdConstants; +import frc.robot.constants.swerve.DriveConstants; +import frc.robot.util.PhoenixOdometryThread; + +/** IO implementation for Pigeon 2. */ +public class GyroIOPigeon2 implements GyroIO { + private final Pigeon2 pigeon = + new Pigeon2( + IdConstants.PIGEON, + DriveConstants.PIGEON_CAN); + private final StatusSignal yaw = pigeon.getYaw(); + private final StatusSignal accelrationx = pigeon.getAccelerationX(); + private final StatusSignal accelrationy = pigeon.getAccelerationY(); + private final Queue yawPositionQueue; + private final Queue yawTimestampQueue; + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + + public GyroIOPigeon2() { + pigeon.getConfigurator().apply(new Pigeon2Configuration()); + pigeon.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(250); + yawVelocity.setUpdateFrequency(50.0); + pigeon.optimizeBusUtilization(); + yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(pigeon.getYaw()); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity, accelrationx, accelrationy).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + inputs.accelerationX = accelrationx.getValueAsDouble(); + inputs.accelerationY = accelrationy.getValueAsDouble(); + + inputs.odometryYawTimestamps = + yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryYawPositions = + yawPositionQueue.stream() + .map((Double value) -> Rotation2d.fromDegrees(value)) + .toArray(Rotation2d[]::new); + yawTimestampQueue.clear(); + yawPositionQueue.clear(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Module.java b/src/main/java/frc/robot/subsystems/drivetrain/Module.java new file mode 100644 index 0000000..6fa9d46 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/Module.java @@ -0,0 +1,422 @@ +package frc.robot.subsystems.drivetrain; + +import java.util.Queue; + +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MagnetSensorConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; +import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.SensorDirectionValue; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import frc.robot.constants.swerve.DriveConstants; +import frc.robot.constants.swerve.ModuleConstants; +import frc.robot.constants.swerve.ModuleType; +import frc.robot.util.PhoenixOdometryThread; +import lib.CTREModuleState; + + +public class Module implements ModuleIO{ + private final ModuleType type; + + // Degrees + private final double angleOffset; + + private final TalonFX angleMotor; + private final TalonFX driveMotor; + private final CANcoder CANcoder; + private SwerveModuleState desiredState; + + protected boolean stateDeadband = true; + + private boolean optimizeStates = true; + + // Inputs from drive motor + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + // Inputs from turn motor + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnPosition; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + // Timestamp inputs from Phoenix thread + protected final Queue timestampQueue; + protected final Queue drivePositionQueue; + protected final Queue turnPositionQueue; + + private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; + + // Connection debouncers + private final Debouncer driveConnectedDebounce = new Debouncer(0.5); + private final Debouncer turnConnectedDebounce = new Debouncer(0.5); + private final Debouncer turnEncoderConnectedDebounce = new Debouncer(0.5); + + private final Alert driveDisconnectedAlert; + private final Alert turnDisconnectedAlert; + private final Alert turnEncoderDisconnectedAlert; + + protected final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + + private ModuleConstants moduleConstants; + private final MotionMagicVelocityVoltage velocityRequest = + new MotionMagicVelocityVoltage(0.0).withUpdateFreqHz(0); + + + public Module(ModuleConstants moduleConstants) { + this.moduleConstants = moduleConstants; + + type = moduleConstants.getType(); + angleOffset = moduleConstants.getSteerOffset(); + + /* Angle Encoder Config */ + CANcoder = new CANcoder(moduleConstants.getEncoderPort(), DriveConstants.STEER_ENCODER_CAN); + /* Angle Motor Config */ + angleMotor = new TalonFX(moduleConstants.getSteerPort(), DriveConstants.STEER_ENCODER_CAN); + driveMotor = new TalonFX(moduleConstants.getDrivePort(), DriveConstants.DRIVE_MOTOR_CAN); + // Create drive status signals + drivePosition = driveMotor.getPosition(); + driveVelocity = driveMotor.getVelocity(); + driveAppliedVolts = driveMotor.getMotorVoltage(); + driveCurrent = driveMotor.getStatorCurrent(); + + // Create turn status signals + turnAbsolutePosition = CANcoder.getAbsolutePosition(); + turnPosition = angleMotor.getPosition(); + turnVelocity = angleMotor.getVelocity(); + turnAppliedVolts = angleMotor.getMotorVoltage(); + turnCurrent = angleMotor.getStatorCurrent(); + + // Create timestamp queue + timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + drivePositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(driveMotor.getPosition()); + turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(angleMotor.getPosition()); + updateInputs(); + + configCANcoder(); + configAngleMotor(); + configDriveMotor(); + + driveDisconnectedAlert = + new Alert( + "Disconnected drive motor on module " + Integer.toString(moduleConstants.ordinal()) + ".", + AlertType.kError); + turnDisconnectedAlert = + new Alert( + "Disconnected turn motor on module " + Integer.toString(moduleConstants.ordinal()) + ".", AlertType.kError); + turnEncoderDisconnectedAlert = + new Alert( + "Disconnected turn encoder on module " + Integer.toString(moduleConstants.ordinal()) + ".", + AlertType.kError); + + + // Configure periodic frames + BaseStatusSignal.setUpdateFrequencyForAll( + 250, drivePosition, turnPosition); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + ParentDevice.optimizeBusUtilizationForAll(driveMotor, angleMotor); + + setDesiredState(new SwerveModuleState(0, getAngle()), false); + } + + public void close() { + angleMotor.close(); + driveMotor.close(); + CANcoder.close(); + } + + @Override + public void updateInputs() { + // Refresh all signals + var driveStatus = + BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); + var turnStatus = + BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); + var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); + + // Update drive inputs + inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()/DriveConstants.DRIVE_GEAR_RATIO); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()/DriveConstants.DRIVE_GEAR_RATIO); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); + + // Update turn inputs + inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK()); + inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK()); + inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); + inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()/DriveConstants.MODULE_CONSTANTS.angleGearRatio); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()/DriveConstants.MODULE_CONSTANTS.angleGearRatio); + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); + + // Update odometry inputs + inputs.odometryTimestamps = + timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryDrivePositionsRad = + drivePositionQueue.stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value)) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream() + .map((Double value) -> Rotation2d.fromRotations(value)) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + + } + + public void periodic() { + updateInputs(); + Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs); + + // Calculate positions for odometry + int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together + odometryPositions = new SwerveModulePosition[sampleCount]; + for (int i = 0; i < sampleCount; i++) { + double positionMeters = inputs.odometryDrivePositionsRad[i]/DriveConstants.DRIVE_GEAR_RATIO * DriveConstants.WHEEL_RADIUS; + Rotation2d angle = inputs.odometryTurnPositions[i].div(DriveConstants.MODULE_CONSTANTS.angleGearRatio); + odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + } + // Update alerts + driveDisconnectedAlert.set(!inputs.driveConnected); + turnDisconnectedAlert.set(!inputs.turnConnected); + turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected); + Logger.recordOutput("Angle "+ moduleConstants.ordinal(), MathUtil.inputModulus(getAngle().getDegrees(), 0, 360)); + } + + public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) { + // Separate if here and in setAngle() to avoid warning + if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){ + /* + * This is a custom optimize function, since default WPILib optimize assumes + * continuous controller which CTRE and Rev onboard is not + */ + desiredState = optimizeStates ? CTREModuleState.optimize(wantedState, getState().angle) : wantedState; + }else{ + desiredState = wantedState; + } + setAngle(); + setSpeed(isOpenLoop); + } + + public void setSpeed(boolean isOpenLoop) { + if(desiredState == null){ + return; + } + if (isOpenLoop) { + double percentOutput = desiredState.speedMetersPerSecond / DriveConstants.MAX_SPEED; + driveMotor.set(percentOutput); + } else { + double velocity = desiredState.speedMetersPerSecond/DriveConstants.WHEEL_RADIUS/2/Math.PI*DriveConstants.DRIVE_GEAR_RATIO; + Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity); + + driveMotor.setControl( + velocityRequest + .withVelocity(velocity)); + ///.withFeedForward(feedforward)); + } + } + + private void setAngle() { + if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){ + // Prevent rotating module if desired speed < 1%. Prevents jittering and unnecessary movement. + if (stateDeadband && (Math.abs(desiredState.speedMetersPerSecond) <= (DriveConstants.MAX_SPEED * 0.01))) { + stop(); + return; + } + } + if(desiredState == null){ + return; + } + angleMotor.setControl(new PositionDutyCycle(desiredState.angle.getRotations()*DriveConstants.MODULE_CONSTANTS.angleGearRatio)); + } + + public void setDriveVoltage(Voltage voltage){ + driveMotor.setVoltage(voltage.baseUnitMagnitude()); + } + public void setAngle(Rotation2d angle){ + angleMotor.setControl(new PositionDutyCycle(angle.getRotations()*DriveConstants.MODULE_CONSTANTS.angleGearRatio)); + } + + public void setOptimize(boolean enable) { + optimizeStates = enable; + } + + public byte getModuleIndex() { + return type.id; + } + + public Rotation2d getAngle() { + return inputs.turnPosition; + } + + public Rotation2d getCANcoder() { + return inputs.turnAbsolutePosition; + } + + public void resetToAbsolute() { + // Sensor ticks + double absolutePosition = getCANcoder().getRotations() - Units.degreesToRotations(angleOffset); + angleMotor.setPosition(absolutePosition*DriveConstants.MODULE_CONSTANTS.angleGearRatio); + } + + private void configCANcoder() { + CANcoder.getConfigurator().apply(new CANcoderConfiguration()); + CANcoder.getConfigurator().apply(new MagnetSensorConfigs().withAbsoluteSensorDiscontinuityPoint(1). + withSensorDirection(DriveConstants.MODULE_CONSTANTS.canCoderInvert?SensorDirectionValue.Clockwise_Positive:SensorDirectionValue.CounterClockwise_Positive)); + } + + private void configAngleMotor() { + angleMotor.getConfigurator().apply(new TalonFXConfiguration()); + CurrentLimitsConfigs config = new CurrentLimitsConfigs(); + config.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT; + config.SupplyCurrentLimit = DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT; + config.SupplyCurrentLowerLimit = DriveConstants.STEER_PEAK_CURRENT_LIMIT; + config.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION; + angleMotor.getConfigurator().apply(config); + angleMotor.getConfigurator().apply(new Slot0Configs() + .withKP(DriveConstants.MODULE_CONSTANTS.angleKP) + .withKI(DriveConstants.MODULE_CONSTANTS.angleKI) + .withKD(DriveConstants.MODULE_CONSTANTS.angleKD)); + angleMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_STEER_MOTOR)); + angleMotor.setNeutralMode(DriveConstants.STEER_NEUTRAL_MODE); + angleMotor.setPosition(0); + + resetToAbsolute(); + } + + /** + * @return Speed in RPM + */ + public double getDriveVelocity() { + return inputs.driveVelocityRadPerSec*60/DriveConstants.MODULE_CONSTANTS.driveGearRatio/2/Math.PI; + } + + public double getDriveVoltage(){ + return inputs.driveAppliedVolts; + } + + public double getDriveStatorCurrent(){ + return inputs.driveCurrentAmps; + } + private void configDriveMotor() { + var talonFXConfigs = new TalonFXConfiguration(); + // set Motion Magic settings + var motionMagicConfigs = talonFXConfigs.MotionMagic; + motionMagicConfigs.MotionMagicCruiseVelocity = DriveConstants.MAX_SPEED/DriveConstants.WHEEL_CIRCUMFERENCE * DriveConstants.DRIVE_GEAR_RATIO; + motionMagicConfigs.MotionMagicAcceleration = DriveConstants.MAX_DRIVE_ACCEL/DriveConstants.WHEEL_CIRCUMFERENCE * DriveConstants.DRIVE_GEAR_RATIO; + var slot0Configs = talonFXConfigs.Slot0; + slot0Configs.kS = 0; // Add 0.25 V output to overcome static friction + slot0Configs.kV = 0.11; // A velocity target of 1 rps results in 0.12 V output + slot0Configs.kA = 0.006; // An acceleration of 1 rps/s requires 0.01 V output + slot0Configs.kP = moduleConstants.getDriveP(); // A position error of 2.5 rotations results in 12 V output + slot0Configs.kI = moduleConstants.getDriveI(); // no output for integrated error + slot0Configs.kD = moduleConstants.getDriveD(); // A velocity error of 1 rps results in 0.1 V output + driveMotor.getConfigurator().apply(talonFXConfigs); + CurrentLimitsConfigs config = new CurrentLimitsConfigs(); + config.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT; + config.SupplyCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT; + config.SupplyCurrentLowerLimit = DriveConstants.DRIVE_PEAK_CURRENT_LIMIT; + config.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION; + config.StatorCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT; + config.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT; + driveMotor.getConfigurator().apply(config); + driveMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_DRIVE_MOTOR)); + driveMotor.getConfigurator().apply(new OpenLoopRampsConfigs().withDutyCycleOpenLoopRampPeriod(DriveConstants.OPEN_LOOP_RAMP)); + driveMotor.getConfigurator().apply(new ClosedLoopRampsConfigs().withDutyCycleClosedLoopRampPeriod(DriveConstants.OPEN_LOOP_RAMP)); + driveMotor.setNeutralMode(DriveConstants.DRIVE_NEUTRAL_MODE); + + } + + public SwerveModuleState getState() { + return new SwerveModuleState( + inputs.driveVelocityRadPerSec*DriveConstants.WHEEL_RADIUS, + getAngle()); + } + + public SwerveModulePosition getPosition() { + return new SwerveModulePosition( + inputs.drivePositionRad*DriveConstants.WHEEL_RADIUS, + getAngle()); + } + + public SwerveModuleState getDesiredState() { + return desiredState; + } + + + public double getDriveVelocityError() { + return getDesiredState().speedMetersPerSecond - getState().speedMetersPerSecond; + } + + public void stop() { + driveMotor.set(0); + angleMotor.set(0); + } + + public ModuleType getModuleType(){ + return type; + } + + public void setStateDeadband(boolean enabled) { + stateDeadband = enabled; + } + + public double getDesiredVelocity() { + return getDesiredState().speedMetersPerSecond; + } + + public Rotation2d getDesiredAngle() { + return getDesiredState().angle; + } + + /** Returns the module positions received this cycle. */ + public SwerveModulePosition[] getOdometryPositions() { + return odometryPositions; + } + + /** Returns the timestamps of the samples received this cycle. */ + public double[] getOdometryTimestamps() { + return inputs.odometryTimestamps; + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drivetrain/ModuleIO.java b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIO.java new file mode 100644 index 0000000..fece5cf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIO.java @@ -0,0 +1,43 @@ +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drivetrain; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface ModuleIO { + @AutoLog + public static class ModuleIOInputs { + public boolean driveConnected = false; + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double driveCurrentAmps = 0.0; + + public boolean turnConnected = false; + public boolean turnEncoderConnected = false; + public Rotation2d turnAbsolutePosition = new Rotation2d(); + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double turnCurrentAmps = 0.0; + + public double[] odometryTimestamps = new double[] {}; + public double[] odometryDrivePositionsRad = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs() {} + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drivetrain/ModuleSim.java b/src/main/java/frc/robot/subsystems/drivetrain/ModuleSim.java new file mode 100644 index 0000000..24a2b77 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/ModuleSim.java @@ -0,0 +1,165 @@ +package frc.robot.subsystems.drivetrain; + +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.constants.Constants; +import frc.robot.constants.swerve.DriveConstants; +import frc.robot.constants.swerve.ModuleConstants; +import lib.CTREModuleState; + + +/** + * Swerve module for drivetrain to be used inside of simulation. + * TODO: improve this simulation to be more realistic + */ +public class ModuleSim extends Module { + + private double currentSteerPositionRad = 0; + private double currentDrivePositionMeters = 0; + private double currentSpeed = 0; + + private SwerveModuleState desiredState; + + + protected boolean stateDeadband = true; + + public ModuleSim(ModuleConstants ignored) { + super(ignored); + } + + /** + * Updates the simulation + */ + @Override + public void periodic() { + currentDrivePositionMeters += currentSpeed * Constants.LOOP_TIME; + super.periodic(); + } + + @Override + public void updateInputs(){ + // Update drive inputs + inputs.driveConnected = true; + inputs.drivePositionRad = currentDrivePositionMeters / DriveConstants.WHEEL_RADIUS; + inputs.driveVelocityRadPerSec = currentSpeed / DriveConstants.WHEEL_RADIUS; + inputs.driveAppliedVolts = currentSpeed / DriveConstants.MAX_SPEED * Constants.ROBOT_VOLTAGE; + inputs.driveCurrentAmps = 0; // This simulation currently isn't good enough to calculate this + + // Update turn inputs + inputs.turnConnected = true; + inputs.turnEncoderConnected = true; + inputs.turnAbsolutePosition = new Rotation2d(currentSteerPositionRad); + inputs.turnPosition = new Rotation2d(currentSteerPositionRad); + inputs.turnVelocityRadPerSec = 0; // Simulated modules currently teleport + inputs.turnAppliedVolts = 0; + inputs.turnCurrentAmps = 0; + + // Update odometry inputs + // Simulate as only getting one value per frame + inputs.odometryTimestamps = + new double[]{Timer.getFPGATimestamp()}; + inputs.odometryDrivePositionsRad = + new double[]{inputs.drivePositionRad*DriveConstants.DRIVE_GEAR_RATIO}; + inputs.odometryTurnPositions = + new Rotation2d[]{inputs.turnPosition}; + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + /** + * Sets the desired state for the module. + * + * @param desiredState Desired state with speed and angle. + * @param isOpenLoop whether to use closed/open loop control for drive velocity + */ + public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) { + if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){ + // If the module isn't moving, don't rotate it + if (Math.abs(desiredState.speedMetersPerSecond) < 0.001) { + currentSpeed = 0; + return; + } + // Optimize the reference state to avoid spinning further than 90 degrees + desiredState = CTREModuleState.optimize(desiredState, new Rotation2d(currentSteerPositionRad)); + } + + currentSpeed = desiredState.speedMetersPerSecond; + currentSteerPositionRad = desiredState.angle.getRadians(); + } + + public void resetToAbsolute() { + // does nothing when robot does not have a swerve drivetrain + } + + public SwerveModuleState getDesiredState() { + return desiredState; + } + + public double getDesiredVelocity() { + return getDesiredState().speedMetersPerSecond; + } + + public Rotation2d getDesiredAngle() { + return getDesiredState().angle; + } + + /** + * Sets current speed to zero + */ + public void stop() { + currentSpeed = 0; + } + + public SwerveModuleState getState() { + return new SwerveModuleState( + currentSpeed, + getAngle() + ); + } + + public SwerveModulePosition getPosition() { + return new SwerveModulePosition( + currentDrivePositionMeters, + new Rotation2d(currentSteerPositionRad) + ); + } + + /** + * Gets the simulated angle of the module. + */ + public Rotation2d getAngle() { + return new Rotation2d(currentSteerPositionRad); + } + + /** + * Sets state deadband + */ + public void setStateDeadband(boolean enabled) { + stateDeadband = enabled; + } + + public TalonFX getDriveMotor(){ + return null; + } + + public double getDriveVoltage(){ + return 0; + } + + public double getDriveStatorCurrent(){ + return 0; + } + + public double getSteerVelocity() { + return 0; + } + public double getDriveVelocity() { + return 0; + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java new file mode 100644 index 0000000..9fc85ae --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -0,0 +1,183 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.elevator; + +import java.util.function.BooleanSupplier; + +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.Constants; +import frc.robot.constants.ElevatorConstants; +import frc.robot.constants.IdConstants; +import frc.robot.util.AngledElevatorSim; +import frc.robot.util.PhoenixUtil; + +public class Elevator extends SubsystemBase { + private TalonFX rightMotor = new TalonFX(IdConstants.ELEVATOR_RIGHT_MOTOR, Constants.CANIVORE_CAN); + + private double setpoint = ElevatorConstants.INTAKE_SETPOINT; + + private MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0); + + private double maxVelocity = 3.6; // m/s 3.68 + private double maxAcceleration = 14; // m/s 8 + + // Sim variables + private AngledElevatorSim sim; + private Mechanism2d mechanism; + private MechanismLigament2d ligament; + private double voltage; + + private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); + + private BooleanSupplier armStowed; + + public Elevator() { + // This increases both the time and memory efficiency of the code when running + // on a real robot; do not remove this if statement + if (RobotBase.isSimulation()) { + sim = new AngledElevatorSim(ElevatorConstants.MOTOR, ElevatorConstants.GEARING, ElevatorConstants.CARRIAGE_MASS, + ElevatorConstants.DRUM_RADIUS, ElevatorConstants.MIN_HEIGHT, ElevatorConstants.MAX_HEIGHT, true, + ElevatorConstants.START_HEIGHT, ElevatorConstants.ANGLE, ElevatorConstants.SPRING_FORCE); + double width = ElevatorConstants.MAX_HEIGHT * Math.sin(ElevatorConstants.ANGLE); + double height = ElevatorConstants.MAX_HEIGHT * Math.cos(ElevatorConstants.ANGLE); + double size = Math.max(width, height); + mechanism = new Mechanism2d(size, size); + ligament = mechanism.getRoot("base", size / 2 - width / 2, size / 2 - height / 2).append(new MechanismLigament2d( + "elevator", ElevatorConstants.START_HEIGHT, 90 - Units.radiansToDegrees(Math.abs(ElevatorConstants.ANGLE)))); + SmartDashboard.putData("elevator", mechanism); + } + Timer.delay(1.0); + + //m_lastProfiledReference = new ExponentialProfile.State(getPosition(),0); + resetEncoder(ElevatorConstants.START_HEIGHT); + + var talonFXConfigs = new TalonFXConfiguration(); + + // set slot 0 gains + var slot0Configs = talonFXConfigs.Slot0; + slot0Configs.kS = 0.15; // Add 0.25 V output to overcome static friction + slot0Configs.kV = 0.12; // A velocity target of 1 rps results in 0.12 V output + slot0Configs.kA = 0; // An acceleration of 1 rps/s requires 0.01 V output + slot0Configs.kP = 0.75; // A position error of 2.5 rotations results in 12 V output + slot0Configs.kI = 0; // no output for integrated error + slot0Configs.kD = 0; // A velocity error of 1 rps results in 0.1 V output + + // set Motion Magic settings + var motionMagicConfigs = talonFXConfigs.MotionMagic; + motionMagicConfigs.MotionMagicCruiseVelocity = ElevatorConstants.GEARING * maxVelocity/ElevatorConstants.DRUM_RADIUS/Math.PI/2; // Target cruise velocity + motionMagicConfigs.MotionMagicAcceleration = ElevatorConstants.GEARING * maxAcceleration/ElevatorConstants.DRUM_RADIUS/Math.PI/2; // Target acceleration + rightMotor.getConfigurator().apply(talonFXConfigs); + rightMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)); + updateInputs(); + PhoenixUtil.tryUntilOk(100, ()-> rightMotor.setNeutralMode(NeutralModeValue.Brake)); + } + + public void setArmStowed(BooleanSupplier armStowed){ + this.armStowed = armStowed; + } + + @Override + public void periodic() { + double setpoint2 = setpoint; + if(setpoint2 < ElevatorConstants.SAFE_SETPOINT && (armStowed == null || !armStowed.getAsBoolean())){ + setpoint2 = ElevatorConstants.SAFE_SETPOINT; + } + double setpointRotations = ElevatorConstants.GEARING * setpoint2 / ElevatorConstants.DRUM_RADIUS/Math.PI/2; + rightMotor.setControl(voltageRequest.withPosition(setpointRotations).withFeedForward(0.4)); + updateInputs(); + Logger.processInputs("Elevator", inputs); + Logger.recordOutput("Elevator/Setpoint", getSetpoint()); + Logger.recordOutput("Elevator/AtSetpoint", atSetpoint()); + } + + @Override + public void simulationPeriodic() { + sim.setInputVoltage(0); + sim.update(Constants.LOOP_TIME); + ligament.setLength(sim.getPositionMeters()); + rightMotor.getSimState().setRawRotorPosition( + sim.getPositionMeters() / (2 * Math.PI * ElevatorConstants.DRUM_RADIUS) * ElevatorConstants.GEARING); + } + + public void resetEncoder(double height) { + // Without the if statement, this causes loop overruns in simulation, and this + // code does nothing anyway on sim (it sets the position to itself) + if (RobotBase.isReal()) { + rightMotor.setPosition(height / (2 * Math.PI * ElevatorConstants.DRUM_RADIUS) * ElevatorConstants.GEARING); + } + } + + public void updateInputs(){ + inputs.measuredPosition = rightMotor.getPosition().getValueAsDouble() / ElevatorConstants.GEARING + * (2 * Math.PI * ElevatorConstants.DRUM_RADIUS); + inputs.velocity = rightMotor.getVelocity().getValueAsDouble()/ ElevatorConstants.GEARING + * (2 * Math.PI * ElevatorConstants.DRUM_RADIUS); + inputs.currentAmps = rightMotor.getStatorCurrent().getValueAsDouble(); + } + + /** + * Get the position of the elevator in meters. + */ + public double getPosition() { + return inputs.measuredPosition; + } + + /** + * Get the velocity of the elevator in m/s. + */ + public double getVelocity(){ + return inputs.velocity; + } + + public double getVoltage(){ + return voltage; + } + + /** + * Method to set the setpoint of the elevator. Clamped between min and max height. + * @param setpoint The setpoint in meters. + */ + public void setSetpoint(double setpoint) { + this.setpoint = MathUtil.clamp(setpoint, ElevatorConstants.MIN_HEIGHT, ElevatorConstants.MAX_HEIGHT); + } + + /** + * Get the velocity of the elevator in meters. + */ + public double getSetpoint() { + return setpoint; + } + + public Mechanism2d getMechanism2d() { + return mechanism; + } + + public boolean atSetpoint(){ + return Math.abs(getPosition() - setpoint) < (0.025+0.0125); + } + + /** + * Get the COM at the current elevater height. + */ + public double getCenterOfMassHeight(){ + return (getPosition()-ElevatorConstants.MIN_HEIGHT)/(ElevatorConstants.MAX_HEIGHT-ElevatorConstants.MIN_HEIGHT)*(ElevatorConstants.CENTER_OF_MASS_HEIGHT_EXTENDED-ElevatorConstants.CENTER_OF_MASS_HEIGHT_STOWED)+ElevatorConstants.CENTER_OF_MASS_HEIGHT_STOWED; + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java new file mode 100644 index 0000000..c0f96a9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -0,0 +1,12 @@ +package frc.robot.subsystems.elevator; + +import org.littletonrobotics.junction.AutoLog; + +public interface ElevatorIO { + @AutoLog + public static class ElevatorIOInputs { + public double measuredPosition = 0.0; + public double velocity = 0.0; + public double currentAmps = 0.0; + } +} diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java new file mode 100644 index 0000000..4a6eaf8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -0,0 +1,142 @@ +package frc.robot.subsystems.indexer; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.hardware.TalonFX; + +import au.grapplerobotics.ConfigurationFailedException; +import au.grapplerobotics.LaserCan; +import au.grapplerobotics.interfaces.LaserCanInterface; +import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode; +import au.grapplerobotics.interfaces.LaserCanInterface.RegionOfInterest; +import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget; +import au.grapplerobotics.simulation.MockLaserCan; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; +import frc.robot.constants.Constants; +import frc.robot.constants.IdConstants; +import frc.robot.constants.IndexerConstants; + +public class Indexer extends SubsystemBase { + private TalonFX motor; + private MockLaserCan simSensor; + private LaserCanInterface sensor; + + private FlywheelSim flywheelSim; + + // where the coral is for simulation + // in meters + private double simCoralPos; + + private final IndexerIOInputsAutoLogged inputs = new IndexerIOInputsAutoLogged(); + + public Indexer() { + motor = new TalonFX(IdConstants.INDEXER_MOTOR); + if (Robot.isSimulation()) { + flywheelSim = new FlywheelSim(LinearSystemId.createFlywheelSystem(DCMotor.getNEO(1), + IndexerConstants.MOMENT_OF_INERTIA, IndexerConstants.GEAR_RATIO), DCMotor.getNEO(1)); + + // have both interfaces availible + simSensor = new MockLaserCan(); + sensor = simSensor; + } else { + sensor = new LaserCan(IdConstants.INDEXER_SENSOR); + try { + sensor.setRangingMode(RangingMode.SHORT); + sensor.setTimingBudget(TimingBudget.TIMING_BUDGET_20MS); + sensor.setRegionOfInterest(new RegionOfInterest(-4, -4, 8, 8)); + } catch (ConfigurationFailedException e) { + DriverStation.reportError("Indexer LaserCan configuration error", true); + } + } + simCoralPos = IndexerConstants.START_SIM_POS_AT; // initialize it anyway, it's easier + } + + /** Runs the indexer. */ + public void run() { + motor.set(IndexerConstants.SPEED); + simCoralPos = IndexerConstants.START_SIM_POS_AT; + } + + public void slow(){ + motor.set(0.6); + } + + /** Reverses the indexer. */ + public void reverse() { + motor.set(-IndexerConstants.SPEED); + simCoralPos = IndexerConstants.END_SIM_SENSOR_POS_AT; + } + + /** Stops the indexer */ + public void stop() { + motor.stopMotor(); + } + + /** + * @return the motor velocity in rotations per minute + */ + public double getMotor() { + return inputs.velocity; + } + + /** + * Gets the LaserCAN's distance reading. + * If the distance is null, return 314,159 + * + * @return the distance, in millimeters + */ + public int getSensorValue() { + return inputs.sensorDistance; + } + + /** + * Checks whether a coral is in the indexer. + * True means nothing is there, false means something is there + * + * @return the sensor's state + */ + @AutoLogOutput(key = "Intake/isIndexerClear") + public boolean isIndexerClear() { + return true; + } + + @Override + public void periodic() { + //SmartDashboard.putBoolean("Indexer has coral ", isIndexerClear()); + + if (Robot.isReal()) { + inputs.velocity = motor.getVelocity().getValueAsDouble() / IndexerConstants.GEAR_RATIO; + } else { + inputs.velocity = flywheelSim.getAngularVelocityRPM(); + } + var measurement = sensor.getMeasurement(); + inputs.sensorDistance = (measurement == null || measurement.status > 0) ? 314159 : measurement.distance_mm; + Logger.processInputs("Indexer", inputs); + Logger.recordOutput("Indexer/indexer coral",isIndexerClear()); + } + + @Override + public void simulationPeriodic() { + flywheelSim.setInput(motor.get() * Constants.ROBOT_VOLTAGE); + flywheelSim.update(Constants.LOOP_TIME); + + // pretend we have a fake coral + simCoralPos += flywheelSim.getAngularVelocityRPM() / 60. * Constants.LOOP_TIME + * IndexerConstants.WHEEL_CIRCUMFERENCE; + + // toggle the sensor (values are backwards because that's how the sensor works) + simSensor.setMeasurementPartialSim(0, // 0 == valid measurement + (simCoralPos < IndexerConstants.START_SIM_SENSOR_POS_AT + || simCoralPos > IndexerConstants.END_SIM_SENSOR_POS_AT) + ? IndexerConstants.MEASUREMENT_THRESHOLD * 2 + : 0, + 1000 // IDK what this is exactly, but 1000 seems good + ); + } +} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java new file mode 100644 index 0000000..3678c55 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java @@ -0,0 +1,11 @@ +package frc.robot.subsystems.indexer; + +import org.littletonrobotics.junction.AutoLog; + +public interface IndexerIO { + @AutoLog + public class IndexerIOInputs{ + public double velocity = 0.0; + public int sensorDistance = 0; + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java new file mode 100644 index 0000000..27e0309 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -0,0 +1,242 @@ +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import au.grapplerobotics.LaserCan; +import au.grapplerobotics.interfaces.LaserCanInterface.Measurement; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.Constants; +import frc.robot.constants.IdConstants; +import frc.robot.constants.IntakeConstants; + + +public class Intake extends SubsystemBase { + private final TalonFX rollerMotor = new TalonFX(IdConstants.INTAKE_ROLLER); + private final TalonFX pivotMotor = new TalonFX(IdConstants.INTAKE_PIVOT, Constants.CANIVORE_CAN); + + private SingleJointedArmSim stowArmSim; + private Mechanism2d stowMechanism2d; + private MechanismLigament2d stowWheelLigament; + + private final double positionTolerance = 5; + + private final PIDController stowPID = new PIDController(0.015, 0, 0); + private double power; + + private LaserCan laserCan; + private boolean hasCoral = false; + private boolean isMoving = false; + private Timer laserCanSimTimer; + private DCMotor dcMotor = DCMotor.getKrakenX60(1); + private ArmFeedforward feedforward = new ArmFeedforward(0, + Constants.GRAVITY_ACCELERATION * IntakeConstants.CENTER_OF_MASS_DIST * IntakeConstants.MASS + / IntakeConstants.PIVOT_GEAR_RATIO * dcMotor.rOhms / dcMotor.KtNMPerAmp / Constants.ROBOT_VOLTAGE, + 0); + private double startPosition = 90; + + private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + + public Intake() { + if (RobotBase.isSimulation()) { + stowMechanism2d = new Mechanism2d(10, 10); + stowWheelLigament = stowMechanism2d.getRoot("Root", 5, 5) + .append(new MechanismLigament2d("Intake", 4, startPosition)); + SmartDashboard.putData("Intake pivot", stowMechanism2d); + stowArmSim = new SingleJointedArmSim( + dcMotor, + IntakeConstants.PIVOT_GEAR_RATIO, + IntakeConstants.MOMENT_OFiNERTIA, + IntakeConstants.ARM_LENGTH, + Math.toRadians(0), + Math.toRadians(90), + true, + Units.degreesToRadians(startPosition)); + laserCanSimTimer = new Timer(); + } else { + // laserCan = new LaserCan(IdConstants.INTAKE_LASER_CAN); + // try { + // laserCan.setRangingMode(RangingMode.SHORT); + // laserCan.setTimingBudget(TimingBudget.TIMING_BUDGET_20MS); + // laserCan.setRegionOfInterest(new RegionOfInterest(-4, -4, 8, 8)); + // } catch (ConfigurationFailedException e) { + // DriverStation.reportError("Intake LaserCan configuration error", true); + // } + } + rollerMotor.getConfigurator().apply(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake).withInverted(InvertedValue.CounterClockwise_Positive)); + pivotMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive)); + pivotMotor.setPosition(Units.degreesToRotations(startPosition) * IntakeConstants.PIVOT_GEAR_RATIO); + pivotMotor.setNeutralMode(NeutralModeValue.Coast); + stowPID.setTolerance(positionTolerance); + + setAngle(startPosition); + + rollerMotor.getConfigurator().apply(new CurrentLimitsConfigs().withStatorCurrentLimit(40).withStatorCurrentLimitEnable(true)); + } + + /** + * publishes stuff to smartdashboard + */ + @SuppressWarnings("unused") + private void publish() { + SmartDashboard.putNumber("Stow Motor Position", getPivotAngle()); + SmartDashboard.putNumber("Target Angle", stowPID.getSetpoint()); + SmartDashboard.putNumber("Roller Motor Power", power); + + SmartDashboard.putBoolean("Has Coral", hasCoral()); + SmartDashboard.putBoolean("Stow Arm Sim - Is Stowed", isAtSetpoint(90)); + SmartDashboard.putBoolean("Stow Arm Sim - Is Unstowed", isAtSetpoint(0)); + SmartDashboard.putBoolean("Is Roller Active", rollerMotor.get() > 0); + } + + @Override + public void periodic() { + //publish(); + // SmartDashboard.putNumber("angle", getPivotAngle()); + // SmartDashboard.putBoolean("Intake has coral", hasCoral()); + + double position = getPivotAngle(); + power = stowPID.calculate(position) + feedforward.calculate(Units.degreesToRadians(position), 0); + power = MathUtil.clamp(power, -1, 1); + pivotMotor.set(power); + if (laserCan != null) { + Measurement measurement = laserCan.getMeasurement(); + hasCoral = measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT + && measurement.distance_mm <= 1000 * IntakeConstants.DETECT_CORAL_DIST; + } + if(RobotBase.isSimulation()) { + inputs.measuredPivotPosition = Units.radiansToDegrees(stowArmSim.getAngleRads()); + } else { + inputs.measuredPivotPosition = Units.rotationsToDegrees(pivotMotor.getPosition().getValueAsDouble()) / IntakeConstants.PIVOT_GEAR_RATIO; + } + inputs.rollerVelocity = rollerMotor.getVelocity().getValueAsDouble(); + Logger.processInputs("Intake", inputs); + + Logger.recordOutput("Intake/PivotSetpoint", stowPID.getSetpoint()); + } + + @Override + public void simulationPeriodic() { + stowArmSim.setInputVoltage(power * Constants.ROBOT_VOLTAGE); + stowArmSim.update(Constants.LOOP_TIME); + stowWheelLigament.setAngle(Units.radiansToDegrees(stowArmSim.getAngleRads())); + if (!isMoving) { + laserCanSimTimer.reset(); + laserCanSimTimer.start(); + hasCoral = false; + } else { + if (isAtSetpoint()) { + laserCanSimTimer.start(); + } + hasCoral = laserCanSimTimer.hasElapsed(0.5) && !laserCanSimTimer.hasElapsed(1); + } + } + + /** + * Gets the rotation of the intake. + * + * @return the rotation of the intake (in degrees). + */ + public double getPivotAngle() { + return inputs.measuredPivotPosition; + } + + public PIDController getPID() { + return stowPID; + } + + /** + * Checks if intake has coral. + * + * @return Boolean (True if has Coral, False otherwise) + */ + @AutoLogOutput(key = "Intake/HasCoral") + public boolean hasCoral() { + return hasCoral; + } + + /** + * Checks if motor is at current setpoint. + * + * @return Boolean (True if at setpoint, False otherwise) + */ + public boolean isAtSetpoint(double setpoint) { + return Math.abs(getPivotAngle() - setpoint) < positionTolerance; + } + + /** + * Returns whether or not the intake is at its setpoint + * + * @return True if it is at the setpoint, false otherwise + */ + @AutoLogOutput(key = "Intake/AtSetPoint") + public boolean isAtSetpoint() { + return stowPID.atSetpoint(); + } + + /** + * Sets the desired angle of the intake, mostly to stow or unstow. + * + * @param angle desired angle of the intake in degrees + */ + public void setAngle(double angle) { + stowPID.setSetpoint(angle); + } + + /** + * Sets the speed of the roller motor. + * + * @param power The desired speed of the roller, between 0 and 1. + */ + public void setSpeed(double power) { + rollerMotor.set(power); + isMoving = Math.abs(power) < 0.01; + + } + + /** + * Moves the intake up, doesn't stop it. + */ + public void stow() { + stowPID.setSetpoint(IntakeConstants.STOW_SETPOINT); + } + + /** + * Moves the intake down. + */ + public void unstow() { + stowPID.setSetpoint(IntakeConstants.INTAKE_SETPOINT); + } + + /** + * Stops the motor. + */ + public void deactivate() { + rollerMotor.set(0); + } + + /** + * Starts the motor. + */ + public void activate(){ + rollerMotor.set(IntakeConstants.INTAKE_MOTOR_POWER); + } +} + diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java new file mode 100644 index 0000000..e0e9305 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,11 @@ +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeIO { + @AutoLog + public static class IntakeIOInputs { + public double rollerVelocity = 0.0; + public double measuredPivotPosition = 0.0; + } +} diff --git a/src/main/java/frc/robot/subsystems/outtake/Outtake.java b/src/main/java/frc/robot/subsystems/outtake/Outtake.java new file mode 100644 index 0000000..09bb2e2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/outtake/Outtake.java @@ -0,0 +1,51 @@ +package frc.robot.subsystems.outtake; + +import edu.wpi.first.wpilibj.simulation.DIOSim; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** + * Abstract class for the outtake. All commands should use this subsystem + */ +public abstract class Outtake extends SubsystemBase { + /** Coral detected before the rollers */ + protected DIOSim dioInputLoaded; + /** Coral detected after the rollers */ + protected DIOSim dioInputEjecting; + + protected abstract double getMotorSpeed(); + + public void simulationPeriodic(){} + + /** Set the motor power to move the coral */ + public abstract void setMotor(double power); + + /** stop the coral motor */ + public void stop() { + setMotor(0); + } + + /** start spinning the rollers to eject the coral */ + public abstract void outtake(); + + public abstract boolean coralLoaded(); + + /** + * Coral is at the ejecting beam break sensor. + * @return coral is interrupting the beam breaker. + */ + public abstract boolean coralEjecting(); + + public abstract void reverse(); + + public void removeAlgae(){ + setMotor(-0.6); + } + + public void intakeAlgaeReef() { + setMotor(-0.6); + } + + public void outtakeAlgae() { + setMotor(0.9); + } +} diff --git a/src/main/java/frc/robot/subsystems/outtake/OuttakeAlpha.java b/src/main/java/frc/robot/subsystems/outtake/OuttakeAlpha.java new file mode 100644 index 0000000..edd057c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/outtake/OuttakeAlpha.java @@ -0,0 +1,107 @@ +package frc.robot.subsystems.outtake; + + +import com.revrobotics.ColorSensorV3; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; + + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.DIOSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.constants.IdConstants; + + +public class OuttakeAlpha extends Outtake { + + private SparkFlex motor = new SparkFlex(IdConstants.OUTTAKE_MOTOR_ALPHA, MotorType.kBrushless); + private double power; + + + /** Coral detected before the rollers */ + private final ColorSensorV3 colorSensor = new ColorSensorV3(IdConstants.i2cPort); + /** Coral detected after the rollers */ + private DigitalInput digitalInputEjecting = new DigitalInput(IdConstants.OUTTAKE_DIO_EJECTING); + + + + public OuttakeAlpha(){ + motor.configure(new SparkFlexConfig() + .inverted(true) + .idleMode(IdleMode.kBrake), + ResetMode.kResetSafeParameters, + PersistMode.kNoPersistParameters + ); + if (RobotBase.isSimulation()){ + // object that will control the ejecting sensor + dioInputEjecting = new DIOSim(digitalInputEjecting); + // assume coral is loaded + dioInputLoaded.setValue(false); + // we are not ejecting + dioInputEjecting.setValue(true); + } + } + + @Override + protected double getMotorSpeed(){ + return motor.get(); + } + + @Override + public void periodic(){ + motor.set(power); + // SmartDashboard.putBoolean("Coral loaded", coralLoaded()); + // SmartDashboard.putBoolean("Coral ejected", coralEjecting()); + + } + + /** Set the motor power to move the coral */ + public void setMotor(double power){ + this.power = power; + } + + + /** start spinning the rollers to eject the coral */ + public void outtake(){ + // assumes the coral is present + // if the coral is not present, we should not bother to spin the rollers + setMotor(SmartDashboard.getNumber("wheel speed", 0.2)); + // this starts the motor... what needs to be done later? + } + + /** + * Coral is at the ejecting beam break sensor. + * @return coral is interrupting the beam breaker. + */ + public boolean coralEjecting() { + return !digitalInputEjecting.get(); + } + + + public void reverse(){ + setMotor(-0.2); + } + + + public boolean isSimulation(){ + return RobotBase.isSimulation(); + } + + public int getProximity() { + return colorSensor.getProximity(); // Returns 0 (far) to ~2047 (very close) + } + + // coral detection from color sensor + public boolean coralLoaded() { + //this is about 1/2inch away -- might have to change based on placement + if (getProximity() > 800) { + return true; + } + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/outtake/OuttakeComp.java b/src/main/java/frc/robot/subsystems/outtake/OuttakeComp.java new file mode 100644 index 0000000..b2daa19 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/outtake/OuttakeComp.java @@ -0,0 +1,87 @@ +package frc.robot.subsystems.outtake; + +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.ColorSensorV3; + +import edu.wpi.first.hal.I2CJNI; +import frc.robot.constants.IdConstants; + +public class OuttakeComp extends Outtake { + + private TalonFX motor = new TalonFX(IdConstants.OUTTAKE_MOTOR_COMP); + private double power; + + /** Coral detected before the rollers */ + private ColorSensorV3 colorSensor = new ColorSensorV3(IdConstants.i2cPort); + + OuttakeIOIntakesAutoLogged inputs = new OuttakeIOIntakesAutoLogged(); + + public OuttakeComp(){ + motor.getConfigurator().apply(new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + ); + } + + @Override + protected double getMotorSpeed() { + return power; + } + + @Override + public void periodic(){ + motor.set(power); + // SmartDashboard.putBoolean("Coral loaded", coralLoaded()); + // SmartDashboard.putBoolean("Coral ejected", coralEjecting()); + + inputs.motorVelocity = motor.getVelocity().getValueAsDouble(); + Logger.processInputs("Outtake", inputs); + //Logger.recordOutput("Outtake/Sensor", getProximity()); + //Logger.recordOutput("Outtake/SensorConnected", colorSensor.isConnected()); + } + + /** Set the motor power to move the coral */ + public void setMotor(double power){ + this.power = power; + } + + /** start spinning the rollers to eject the coral */ + public void outtake(){ + setMotor(-0.3); + } + + /** + * Coral is in the outtake. + * @return The same thing as coralLoaded(), for compatibility with previous code + */ + public boolean coralEjecting() { + return coralLoaded(); + } + + + public void reverse(){ + setMotor(0.2); + } + + public int getProximity() { + inputs.proximity = colorSensor.getProximity(); + if (inputs.proximity > 0){ + return inputs.proximity; + } + else{ + I2CJNI.i2CClose(1); + colorSensor = new ColorSensorV3(IdConstants.i2cPort); + return inputs.proximity = colorSensor.getProximity(); // Returns 0 (far) to ~2047 (very close) + } + } + + // coral detection from color sensor + public boolean coralLoaded() { + return getProximity() > 2000; + } +} diff --git a/src/main/java/frc/robot/subsystems/outtake/OuttakeIO.java b/src/main/java/frc/robot/subsystems/outtake/OuttakeIO.java new file mode 100644 index 0000000..006d7c8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/outtake/OuttakeIO.java @@ -0,0 +1,11 @@ +package frc.robot.subsystems.outtake; + +import org.littletonrobotics.junction.AutoLog; + +public interface OuttakeIO { + @AutoLog + public static class OuttakeIOIntakes { + public double motorVelocity = 0.0; + public int proximity = 0; + } +} diff --git a/src/main/java/frc/robot/util/AngledElevatorSim.java b/src/main/java/frc/robot/util/AngledElevatorSim.java new file mode 100644 index 0000000..0334792 --- /dev/null +++ b/src/main/java/frc/robot/util/AngledElevatorSim.java @@ -0,0 +1,95 @@ +package frc.robot.util; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.NumericalIntegration; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; + +public class AngledElevatorSim extends ElevatorSim { + private double angle; + private boolean simulateGravity; + private double minHeight; + private double maxHeight; + private double springAccel; + + /** + * Creates a simulated angled elevator mechanism. + * + * @param gearbox The type of and number of motors in the elevator gearbox. + * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). + * @param carriageMassKg The mass of the elevator carriage. + * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + * @param simulateGravity Whether gravity should be simulated or not. + * @param startingHeightMeters The starting height of the elevator. + * @param angleRads The angle of the elevator from vertical in radians. + * @param springForceNewtons The force of the constant force spring in Newtons. Up is positive. + * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no + * noise is desired. If present must have 1 element for position. + */ + public AngledElevatorSim( + DCMotor gearbox, + double gearing, + double carriageMassKg, + double drumRadiusMeters, + double minHeightMeters, + double maxHeightMeters, + boolean simulateGravity, + double startingHeightMeters, + double angleRads, + double springForceNewtons, + double... measurementStdDevs) { + super(gearbox, gearing, carriageMassKg, drumRadiusMeters, minHeightMeters, maxHeightMeters, simulateGravity, startingHeightMeters, measurementStdDevs); + angle = angleRads; + this.simulateGravity = simulateGravity; + minHeight = minHeightMeters; + maxHeight = maxHeightMeters; + springAccel = springForceNewtons/carriageMassKg; + } + + // Copied from ElevatorSim with one difference + /** + * Creates a simulated elevator mechanism. + * + * @param gearbox The type of and number of motors in the elevator gearbox. + * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). + * @param carriageMassKg The mass of the elevator carriage. + * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + * @param simulateGravity Whether gravity should be simulated or not. + * @param startingHeightMeters The starting height of the elevator. + * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no + * noise is desired. If present must have 1 element for position. + */ + @Override + protected Matrix updateX(Matrix currentXhat, Matrix u, double dtSeconds) { + // Calculate updated x-hat from Runge-Kutta. + var updatedXhat = + NumericalIntegration.rkdp( + (x, _u) -> { + Matrix xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u)); + if (simulateGravity) { + // This is the only line that is different + xdot = xdot.plus(VecBuilder.fill(0, springAccel-9.8*Math.cos(angle))); + } + return xdot; + }, + currentXhat, + u, + dtSeconds); + + // We check for collisions after updating x-hat. + if (wouldHitLowerLimit(updatedXhat.get(0, 0))) { + return VecBuilder.fill(minHeight, 0); + } + if (wouldHitUpperLimit(updatedXhat.get(0, 0))) { + return VecBuilder.fill(maxHeight, 0); + } + return updatedXhat; + } +} diff --git a/src/main/java/frc/robot/util/ClimbArmSim.java b/src/main/java/frc/robot/util/ClimbArmSim.java new file mode 100644 index 0000000..30cde9d --- /dev/null +++ b/src/main/java/frc/robot/util/ClimbArmSim.java @@ -0,0 +1,166 @@ +package frc.robot.util; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.NumericalIntegration; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; + +public class ClimbArmSim extends SingleJointedArmSim { + private boolean m_simulateGravity; + private double m_armLenMeters; + private double m_minAngle; + private double m_maxAngle; + private double mass; + private double momentOfInertia; + private boolean isClimbing; + + /** + * Creates a simulated arm mechanism. + * + * @param plant The linear system that represents the arm. This system can be created with {@link + * edu.wpi.first.math.system.plant.LinearSystemId#createSingleJointedArmSystem(DCMotor, + * double, double)}. + * @param gearbox The type of and number of motors in the arm gearbox. + * @param gearing The gearing of the arm (numbers greater than 1 represent reductions). + * @param armLengthMeters The length of the arm. + * @param minAngleRads The minimum angle that the arm is capable of. + * @param maxAngleRads The maximum angle that the arm is capable of. + * @param simulateGravity Whether gravity should be simulated or not. + * @param startingAngleRads The initial position of the Arm simulation in radians. + * @param robotMassKilograms The mass of the robot in kilograms, including battery and bumpers + * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no + * noise is desired. If present must have 1 element for position. + */ + public ClimbArmSim( + LinearSystem plant, + DCMotor gearbox, + double gearing, + double armLengthMeters, + double minAngleRads, + double maxAngleRads, + boolean simulateGravity, + double startingAngleRads, + double robotMasKilograms, + double armMassKilograms, + double... measurementStdDevs) { + super(plant, gearbox, gearing, armLengthMeters, minAngleRads, maxAngleRads, simulateGravity, startingAngleRads, measurementStdDevs); + m_armLenMeters = armLengthMeters; + m_minAngle = minAngleRads; + m_maxAngle = maxAngleRads; + m_simulateGravity = simulateGravity; + mass = robotMasKilograms; + momentOfInertia = 1.0/3.0 * armMassKilograms * armLengthMeters * armLengthMeters; + isClimbing = false; + } + + /** + * Creates a simulated arm mechanism. + * + * @param gearbox The type of and number of motors in the arm gearbox. + * @param gearing The gearing of the arm (numbers greater than 1 represent reductions). + * @param jKgMetersSquared The moment of inertia of the arm; can be calculated from CAD software. + * @param armLengthMeters The length of the arm. + * @param minAngleRads The minimum angle that the arm is capable of. + * @param maxAngleRads The maximum angle that the arm is capable of. + * @param simulateGravity Whether gravity should be simulated or not. + * @param startingAngleRads The initial position of the Arm simulation in radians. + * @param robotMassKilograms The mass of the robot in kilograms, including battery and bumpers + * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no + * noise is desired. If present must have 1 element for position. + */ + public ClimbArmSim( + DCMotor gearbox, + double gearing, + double jKgMetersSquared, + double armLengthMeters, + double minAngleRads, + double maxAngleRads, + boolean simulateGravity, + double startingAngleRads, + double robotMassKilograms, + double... measurementStdDevs) { + this( + LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing), + gearbox, + gearing, + armLengthMeters, + minAngleRads, + maxAngleRads, + simulateGravity, + startingAngleRads, + robotMassKilograms, + 1, + measurementStdDevs); + momentOfInertia = jKgMetersSquared; + } + + public void setIsClimbing(boolean climbing){ + isClimbing = climbing; + } + + /** + * Updates the state of the arm. + * + * @param currentXhat The current state estimate. + * @param u The system inputs (voltage). + * @param dtSeconds The time difference between controller updates. + */ + @Override + protected Matrix updateX(Matrix currentXhat, Matrix u, double dtSeconds) { + // The torque on the arm is given by τ = F⋅r, where F is the force applied by + // gravity and r the distance from pivot to center of mass. Recall from + // dynamics that the sum of torques for a rigid body is τ = J⋅α, were τ is + // torque on the arm, J is the mass-moment of inertia about the pivot axis, + // and α is the angular acceleration in rad/s². Rearranging yields: α = F⋅r/J + // + // We substitute in F = m⋅g⋅cos(θ), where θ is the angle from horizontal: + // + // α = (m⋅g⋅cos(θ))⋅r/J + // + // Multiply RHS by cos(θ) to account for the arm angle. Further, we know the + // arm mass-moment of inertia J of our arm is given by J=1/3 mL², modeled as a + // rod rotating about it's end, where L is the overall rod length. The mass + // distribution is assumed to be uniform. Substitute r=L/2 to find: + // + // α = (m⋅g⋅cos(θ))⋅r/(1/3 mL²) + // α = (m⋅g⋅cos(θ))⋅(L/2)/(1/3 mL²) + // α = 3/2⋅g⋅cos(θ)/L + // + // Adding the torque from the robot weight, which is in the opposite direction as the arm's mass + // α = 3/2⋅g⋅cos(θ)/L - m⋅g⋅cos(θ)⋅L/J + // + // This acceleration is next added to the linear system dynamics ẋ=Ax+Bu + // + // f(x, u) = Ax + Bu + [0 α]ᵀ + // f(x, u) = Ax + Bu + [0 3/2⋅g⋅cos(θ)/L - m⋅g⋅cos(θ)⋅L/J]ᵀ + + Matrix updatedXhat = + NumericalIntegration.rkdp( + (Matrix x, Matrix _u) -> { + Matrix xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u)); + if (m_simulateGravity) { + double alphaGrav = 3.0 / 2.0 * -9.8 * Math.cos(x.get(0, 0)) / m_armLenMeters + (isClimbing ? mass * 9.8 * Math.cos(x.get(0, 0)) * m_armLenMeters / momentOfInertia : 0); + xdot = xdot.plus(VecBuilder.fill(0, alphaGrav)); + } + return xdot; + }, + currentXhat, + u, + dtSeconds); + + // We check for collision after updating xhat + if (wouldHitLowerLimit(updatedXhat.get(0, 0))) { + return VecBuilder.fill(m_minAngle, 0); + } + if (wouldHitUpperLimit(updatedXhat.get(0, 0))) { + return VecBuilder.fill(m_maxAngle, 0); + } + return updatedXhat; + } + +} diff --git a/src/main/java/frc/robot/util/ConversionUtils.java b/src/main/java/frc/robot/util/ConversionUtils.java new file mode 100644 index 0000000..7c6da16 --- /dev/null +++ b/src/main/java/frc/robot/util/ConversionUtils.java @@ -0,0 +1,146 @@ +package frc.robot.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.constants.FieldConstants; + +public class ConversionUtils { + + /** + * @param positionCounts CANCoder Position Counts + * @param gearRatio Gear Ratio between CANCoder and Mechanism + * @return Degrees of Rotation of Mechanism + */ + public static double CANcoderToDegrees(double positionCounts, double gearRatio) { + return positionCounts * (360.0 / (gearRatio * 4096.0)); + } + + /** + * @param degrees Degrees of rotation of Mechanism + * @param gearRatio Gear Ratio between CANCoder and Mechanism + * @return CANCoder Position Counts + */ + public static double degreesToCANcoder(double degrees, double gearRatio) { + return degrees / (360.0 / (gearRatio * 4096.0)); + } + + /** + * @param positionCounts CANCoder Position Counts + * @param gearRatio Gear Ratio between CANCoder and Mechanism + * @return Radians of Rotation of Mechanism + */ + public static double CANcoderToRadians(double positionCounts, double gearRatio) { + return Math.toRadians(CANcoderToDegrees(positionCounts, gearRatio)); + } + + /** + * @param radians Radians of rotation of Mechanism + * @param gearRatio Gear Ratio between CANCoder and Mechanism + * @return CANCoder Position Counts + */ + public static double radiansToCANcoder(double radians, double gearRatio) { + return degreesToCANcoder(Math.toDegrees(radians), gearRatio); + } + + /** + * @param positionCounts Falcon Position Counts + * @param gearRatio Gear Ratio between Falcon and Mechanism + * @return Degrees of Rotation of Mechanism + */ + public static double falconToDegrees(double positionCounts, double gearRatio) { + return positionCounts * (360.0 / (gearRatio * 2048.0)); + } + + /** + * @param degrees Degrees of rotation of Mechanism + * @param gearRatio Gear Ratio between Falcon and Mechanism + * @return Falcon Position Counts + */ + public static double degreesToFalcon(double degrees, double gearRatio) { + return degrees / (360.0 / (gearRatio * 2048.0)); + } + + /** + * @param velocityCounts Falcon Velocity Counts + * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM) + * @return RPM of Mechanism + */ + public static double falconToRPM(double velocityCounts, double gearRatio) { + double motorRPM = velocityCounts * (600.0 / 2048.0); + return motorRPM / gearRatio; + } + + /** + * @param RPM RPM of mechanism + * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM) + * @return RPM of Mechanism + */ + public static double RPMToFalcon(double RPM, double gearRatio) { + double motorRPM = RPM * gearRatio; + return motorRPM * (2048.0 / 600.0); + } + + /** + * @param velocitycounts Falcon Velocity Counts + * @param circumference Circumference of Wheel + * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon MPS) + * @return Falcon Velocity Counts + */ + public static double falconToMPS(double velocitycounts, double circumference, double gearRatio) { + double wheelRPM = falconToRPM(velocitycounts, gearRatio); + return (wheelRPM * circumference) / 60; + } + + /** + * @param velocity Velocity MPS + * @param circumference Circumference of Wheel + * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon MPS) + * @return Falcon Velocity Counts + */ + public static double MPSToFalcon(double velocity, double circumference, double gearRatio) { + double wheelRPM = ((velocity * 60) / circumference); + return RPMToFalcon(wheelRPM, gearRatio); + } + + /** + * @param positionCounts Falcon Position Counts + * @param circumference Circumference of Wheel + * @param gearRatio Gear Ratio between Falcon and Wheel + * @return Meters + */ + public static double falconToMeters(double positionCounts, double circumference, double gearRatio) { + return positionCounts * (circumference / (gearRatio * 2048.0)); + } + + /** + * @param meters Meters + * @param circumference Circumference of Wheel + * @param gearRatio Gear Ratio between Falcon and Wheel + * @return Falcon Position Counts + */ + public static double MetersToFalcon(double meters, double circumference, double gearRatio) { + return meters / (circumference / (gearRatio * 2048.0)); + } + + + /** + * Converts between an absolute coordinate system and the pathplanner coordinate system. + *

    + * Absolute coordinate system always has the origin right of the blue driver station from blue driver perspective, + * bottom left if looking down at the field. Positive X goes toward red alliance (forward from blue driver perspective) + * and positive Y toward red loading zone (left from blue driver perspective). The Pathplanner coordinate system has the coordinate + * system rotated such that the origin starts right of the current driver station. + *

    The transformation is self-inverse, so there is no second function to convert back. + * + * @param pose pose to convert + * @param alliance alliance PathPlanner is using for their origin + * @return converted pose + */ + public static Pose2d absolutePoseToPathPlannerPose(Pose2d pose, Alliance alliance) { + if (alliance == Alliance.Red) { + return pose.relativeTo(new Pose2d(FieldConstants.FIELD_LENGTH, FieldConstants.FIELD_WIDTH, new Rotation2d(Math.PI))); + } + return new Pose2d(pose.getX(), pose.getY(), pose.getRotation()); + } +} diff --git a/src/main/java/frc/robot/util/DynamicSlewRateLimiter.java b/src/main/java/frc/robot/util/DynamicSlewRateLimiter.java new file mode 100644 index 0000000..6d5df8c --- /dev/null +++ b/src/main/java/frc/robot/util/DynamicSlewRateLimiter.java @@ -0,0 +1,197 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.util; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.util.WPIUtilJNI; + +/** + * A class that limits the rate of change of an input value. Useful for implementing voltage, + * setpoint, and/or output ramps. A slew-rate limit is most appropriate when the quantity being + * controlled is a velocity or a voltage; when controlling a position, consider using a {@link + * edu.wpi.first.math.trajectory.TrapezoidProfile} instead. + * Edited by 972 to be "dynamic", that is, the slew rate can be modified on the fly. + * Additionally, it can be set to be continuous on a range, useful for angles. + */ +public class DynamicSlewRateLimiter { + private double positiveRateLimit; + private double negativeRateLimit; + private double prevVal; + private double prevTime; + + private boolean continuous = false; + private double lowerContinuousLimit = -1; + private double upperContinuousLimit = 1; + + /** + * Creates a new DynamicSlewRateLimiter with the given positive and negative rate limits and initial + * value. + * + * @param positiveRateLimit The rate-of-change limit in the positive direction, in units per + * second. This is expected to be positive. + * @param negativeRateLimit The rate-of-change limit in the negative direction, in units per + * second. This is expected to be negative. + * @param initialValue The initial value of the input. + */ + public DynamicSlewRateLimiter(double positiveRateLimit, double negativeRateLimit, double initialValue) { + this.positiveRateLimit = positiveRateLimit; + this.negativeRateLimit = negativeRateLimit; + prevVal = initialValue; + prevTime = WPIUtilJNI.now() * 1e-6; + } + + /** + * Creates a new DynamicSlewRateLimiter with the given positive rate limit and negative rate limit of + * -rateLimit and initial value. + * + * @param rateLimit The rate-of-change limit, in units per second. + * @param initialValue The initial value of the input. + */ + @Deprecated(since = "2023", forRemoval = true) + public DynamicSlewRateLimiter(double rateLimit, double initialValue) { + this(rateLimit, -rateLimit, initialValue); + } + + /** + * Creates a new SlewRateLimiter with the given positive rate limit and negative rate limit of + * -rateLimit. + * + * @param rateLimit The rate-of-change limit, in units per second. + */ + public DynamicSlewRateLimiter(double rateLimit) { + this(rateLimit, -rateLimit, 0); + } + + /** + * Filters the input to limit its slew rate. + * + * @param input The input value whose slew rate is to be limited. + * @return The filtered value, which will not change faster than the slew rate. + */ + public double calculate(double input) { + double currentTime = WPIUtilJNI.now() * 1e-6; + double elapsedTime = currentTime - prevTime; + prevTime = currentTime; + + double change = MathUtil.clamp( + input - prevVal, + negativeRateLimit * elapsedTime, + positiveRateLimit * elapsedTime); + + if (continuous) { + change = MathUtil.clamp( + MathUtil.inputModulus(input - prevVal, lowerContinuousLimit, upperContinuousLimit), + negativeRateLimit * elapsedTime, + positiveRateLimit * elapsedTime); + + prevVal += change; + + //Extra check to make sure it is within the limits, probably unnecessary + prevVal = MathUtil.inputModulus(prevVal, lowerContinuousLimit, upperContinuousLimit); + } else { + prevVal += change; + } + + return prevVal; + } + + /** + * Sets a new slewrate and filters the input to limit its slew rate. + * + * @param input The input value whose slew rate is to be limited. + * @param rateLimit The new rate-of-change limit, in units per second. + * @return The filtered value, which will not change faster than the slew rate. + */ + public double calculate(double input, double rateLimit) { + setRateLimit(rateLimit); + return calculate(input); + } + + /** + * Sets new slew rates and filters the input to limit its slew rate. + * + * @param input The input value whose slew rate is to be limited. + * @param positiveRateLimit The rate-of-change limit in the positive direction, in units per + * second. This is expected to be positive. + * @param negativeRateLimit The rate-of-change limit in the negative direction, in units per + * second. This is expected to be negative. + * @return The filtered value, which will not change faster than the slew rate. + */ + public double calculate(double input, double positiveRateLimit, double negativeRateLimit) { + setRateLimit(positiveRateLimit, negativeRateLimit); + return calculate(input); + } + + + /** + * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so. + * + * @param value The value to reset to. + */ + public void reset(double value) { + prevVal = value; + prevTime = WPIUtilJNI.now() * 1e-6; + } + + /** + * set positive rate limit + * + * @param positiveRateLimit new positive rate limit + */ + public void setPositiveRateLimit(double positiveRateLimit) { + this.positiveRateLimit = positiveRateLimit; + } + + /** + * set negative rate limit + * + * @param negativeRateLimit new negative rate limit + */ + public void setNegativeRateLimit(double negativeRateLimit) { + this.negativeRateLimit = negativeRateLimit; + } + + /** + * Sets positive and negative rate limits + * + * @param rateLimit new rate limits + */ + public void setRateLimit(double rateLimit) { + positiveRateLimit = rateLimit; + negativeRateLimit = -rateLimit; + } + + /** + * Sets positive and negative rate limits + * + * @param positiveRateLimit new positive rate limit + * @param negativeRateLimit new negative rate limit + */ + public void setRateLimit(double positiveRateLimit, double negativeRateLimit) { + this.positiveRateLimit = positiveRateLimit; + this.negativeRateLimit = negativeRateLimit; + } + + /** + * Sets Continuous Limits + * + * @param lowerContinuousLimit Lower Continuous Limit + * @param upperContinuousLimit Upper Continuous Limit + */ + public void setContinuousLimits(double lowerContinuousLimit, double upperContinuousLimit) { + this.lowerContinuousLimit = lowerContinuousLimit; + this.upperContinuousLimit = upperContinuousLimit; + } + + /** + * Enables or disables continuous + * WARNING: Continuous doesn't work properly with non-symmetrical rate limits + * + * @param continuous is continuous enabled + */ + public void enableContinuous(boolean continuous) { + this.continuous = continuous; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/EqualsUtil.java b/src/main/java/frc/robot/util/EqualsUtil.java new file mode 100644 index 0000000..93c3616 --- /dev/null +++ b/src/main/java/frc/robot/util/EqualsUtil.java @@ -0,0 +1,29 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.util; + +import edu.wpi.first.math.geometry.Twist2d; + +public class EqualsUtil { + public static boolean epsilonEquals(double a, double b, double epsilon) { + return (a - epsilon <= b) && (a + epsilon >= b); + } + + public static boolean epsilonEquals(double a, double b) { + return epsilonEquals(a, b, 1e-9); + } + + /** Extension methods for wpi geometry objects */ + public static class GeomExtensions { + public static boolean epsilonEquals(Twist2d twist, Twist2d other) { + return EqualsUtil.epsilonEquals(twist.dx, other.dx) + && EqualsUtil.epsilonEquals(twist.dy, other.dy) + && EqualsUtil.epsilonEquals(twist.dtheta, other.dtheta); + } + } +} diff --git a/src/main/java/frc/robot/util/FeedForwardCharacterizationData.java b/src/main/java/frc/robot/util/FeedForwardCharacterizationData.java new file mode 100644 index 0000000..a0d92e7 --- /dev/null +++ b/src/main/java/frc/robot/util/FeedForwardCharacterizationData.java @@ -0,0 +1,73 @@ +package frc.robot.util; + +import lib.PolynomialRegression; + +import java.util.LinkedList; +import java.util.List; + +/** + * A class for storing and processing feedforward characterization data. Used in automatic feedforward characterization. + */ +public class FeedForwardCharacterizationData { + private PolynomialRegression regression; + private final List velocityData = new LinkedList<>(); + private final List voltageData = new LinkedList<>(); + + /** + * Adds a data point to the data set. + * + * @param velocity the velocity of the motor + * @param voltage the voltage applied to the motor + */ + public void add(double velocity, double voltage) { + if (Math.abs(velocity) > 1E-4) { + velocityData.add(Math.abs(velocity)); + voltageData.add(Math.abs(voltage)); + } + } + + /** + * Processes the data set using {@link PolynomialRegression} + * + * @see PolynomialRegression + */ + public void process() { + // creates a new process polynomial regression to get calculated values + regression = new PolynomialRegression( + velocityData.stream().mapToDouble(Double::doubleValue).toArray(), + voltageData.stream().mapToDouble(Double::doubleValue).toArray(), + 1 + ); + } + + /** + * Gets the static voltage of the motor. + * + * @return the static voltage of the motor + */ + public double getStatic() { + // gets y-intercept + return regression.beta(0); + } + + /** + * Gets the velocity of the motor. + * + * @return the velocity of the motor + */ + public double getVelocity() { + // gets a slope of regression line + return regression.beta(1); + } + + /** + * Gets the variance of the data set. + * + * @return the variance of the data set + */ + public double getVariance() { + // gets variance of data set + return regression.R2(); + } +} + diff --git a/src/main/java/frc/robot/util/Functions.java b/src/main/java/frc/robot/util/Functions.java deleted file mode 100644 index bd18946..0000000 --- a/src/main/java/frc/robot/util/Functions.java +++ /dev/null @@ -1,50 +0,0 @@ -package frc.robot.util; - -import frc.robot.constants.Constants; - -public class Functions { - - /** - * Deadbands an input to [-1, -deadband], [deadband, 1], rescaling inputs to be linear from - * (deadband, 0) to (1,1) - * - * @param input The input value to rescale - * @param deadband The deadband - * @return the input rescaled and to fit [-1, -deadband], [deadband, 1] - */ - public static double deadband(double input, double deadband) { - if (Math.abs(input) <= deadband) { - return 0; - } else if (Math.abs(input) == 1) { - return input; - } else { - return (1 / (1 - deadband) * (input + Math.signum(-input) * deadband)); - } - } - - /** - * Deadbands an input to [-1, -Constants.oi.kDeadband], [Constants.oi.kDeadband, 1], rescaling inputs to be linear from - * (Constants.oi.kDeadband, 0) to (1,1) - * - * @param input The input value to rescale - * @return the input rescaled and to fit [-1, -kDeadband], [kDeadband, 1] - */ - public static double deadband(double input) { - return deadband(input, Constants.oi.kDeadband); - } - - /** - * An exponential function that maintains positive or negative. - * @param exponent the power to raise the base to - * @param base the base which will be raised to the power - * @return base to the power of exponent, maintaining sign of base - */ - public static double expoMS(double exponent, double base) { - // weird stuff will happen if you don't put a number > 0 - double finVal = Math.pow(Math.abs(base), exponent); - if (base < 0) { - finVal *= -1; - } - return finVal; - } -} diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java new file mode 100644 index 0000000..9cbc39e --- /dev/null +++ b/src/main/java/frc/robot/util/GeomUtil.java @@ -0,0 +1,163 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +/** Geometry utilities for working with translations, rotations, transforms, and poses. */ +public class GeomUtil { + /** + * Creates a pure translating transform + * + * @param translation The translation to create the transform with + * @return The resulting transform + */ + public static Transform2d toTransform2d(Translation2d translation) { + return new Transform2d(translation, new Rotation2d()); + } + + /** + * Creates a pure translating transform + * + * @param x The x coordinate of the translation + * @param y The y coordinate of the translation + * @return The resulting transform + */ + public static Transform2d toTransform2d(double x, double y) { + return new Transform2d(x, y, new Rotation2d()); + } + + /** + * Creates a pure rotating transform + * + * @param rotation The rotation to create the transform with + * @return The resulting transform + */ + public static Transform2d toTransform2d(Rotation2d rotation) { + return new Transform2d(new Translation2d(), rotation); + } + + /** + * Converts a Pose2d to a Transform2d to be used in a kinematic chain + * + * @param pose The pose that will represent the transform + * @return The resulting transform + */ + public static Transform2d toTransform2d(Pose2d pose) { + return new Transform2d(pose.getTranslation(), pose.getRotation()); + } + + public static Pose2d inverse(Pose2d pose) { + Rotation2d rotationInverse = pose.getRotation().unaryMinus(); + return new Pose2d( + pose.getTranslation().unaryMinus().rotateBy(rotationInverse), rotationInverse); + } + + /** + * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic + * chain + * + * @param transform The transform that will represent the pose + * @return The resulting pose + */ + public static Pose2d toPose2d(Transform2d transform) { + return new Pose2d(transform.getTranslation(), transform.getRotation()); + } + + /** + * Creates a pure translated pose + * + * @param translation The translation to create the pose with + * @return The resulting pose + */ + public static Pose2d toPose2d(Translation2d translation) { + return new Pose2d(translation, new Rotation2d()); + } + + /** + * Creates a pure rotated pose + * + * @param rotation The rotation to create the pose with + * @return The resulting pose + */ + public static Pose2d toPose2d(Rotation2d rotation) { + return new Pose2d(new Translation2d(), rotation); + } + + /** + * Multiplies a twist by a scaling factor + * + * @param twist The twist to multiply + * @param factor The scaling factor for the twist components + * @return The new twist + */ + public static Twist2d multiply(Twist2d twist, double factor) { + return new Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor); + } + + /** + * Converts a Pose3d to a Transform3d to be used in a kinematic chain + * + * @param pose The pose that will represent the transform + * @return The resulting transform + */ + public static Transform3d toTransform3d(Pose3d pose) { + return new Transform3d(pose.getTranslation(), pose.getRotation()); + } + + /** + * Converts a Transform3d to a Pose3d to be used as a position or as the start of a kinematic + * chain + * + * @param transform The transform that will represent the pose + * @return The resulting pose + */ + public static Pose3d toPose3d(Transform3d transform) { + return new Pose3d(transform.getTranslation(), transform.getRotation()); + } + + /** + * Converts a ChassisSpeeds to a Twist2d by extracting two dimensions (Y and Z). chain + * + * @param speeds The original translation + * @return The resulting translation + */ + public static Twist2d toTwist2d(ChassisSpeeds speeds) { + return new Twist2d( + speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond); + } + + /** + * Creates a new pose from an existing one using a different translation value. + * + * @param pose The original pose + * @param translation The new translation to use + * @return The new pose with the new translation and original rotation + */ + public static Pose2d withTranslation(Pose2d pose, Translation2d translation) { + return new Pose2d(translation, pose.getRotation()); + } + + /** + * Creates a new pose from an existing one using a different rotation value. + * + * @param pose The original pose + * @param rotation The new rotation to use + * @return The new pose with the original translation and new rotation + */ + public static Pose2d withRotation(Pose2d pose, Rotation2d rotation) { + return new Pose2d(pose.getTranslation(), rotation); + } +} diff --git a/src/main/java/frc/robot/util/MathUtils.java b/src/main/java/frc/robot/util/MathUtils.java new file mode 100644 index 0000000..fe52234 --- /dev/null +++ b/src/main/java/frc/robot/util/MathUtils.java @@ -0,0 +1,156 @@ +package frc.robot.util; + +import java.util.List; + +import edu.wpi.first.math.MathUtil; +import frc.robot.constants.Constants; + +/** + * Utility class for useful functions. + */ +public class MathUtils { + + /** + * Deadbands an input to [-1, -deadband], [deadband, 1], rescaling inputs to be linear from + * (deadband, 0) to (1,1) + * + * @param input The input value to rescale + * @param deadband The deadband + * @return the input rescaled and to fit [-1, -deadband], [deadband, 1] + */ + public static double deadband(double input, double deadband) { + if (Math.abs(input) <= deadband) { + return 0; + } else if (Math.abs(input) == 1) { + return input; + } else { + return (1 / (1 - deadband) * (input + Math.signum(-input) * deadband)); + } + } + + /** + * Deadbands an input to [-1, -OIConstants.DEADBAND], [OIConstants.DEADBAND, 1], rescaling inputs to be linear from + * (OIConstants.DEADBAND, 0) to (1,1) + * + * @param input The input value to rescale + * @return the input rescaled and to fit [-1, -DEADBAND], [DEADBAND, 1] + */ + public static double deadband(double input) { + return deadband(input, Constants.DEFAULT_DEADBAND); + } + + /** + * An exponential function that maintains positive or negative sign. + * + * @param exponent the power to raise the base to + * @param base the base which will be raised to the power + * @return base to the power of exponent, maintaining sign of base + */ + public static double expoMS(double base, double exponent) { + // weird stuff will happen if you don't put a number > 0 for controller inputs + double finVal = Math.pow(Math.abs(base), exponent); + if (base < 0) { + finVal *= -1; + } + return finVal; + } + + /** + * Calculates Midpoint of two numbers on modulus number line + * + * @param num1 first number + * @param num2 second number + * @param lowerBound lower bound of modulus number line + * @param upperBound upper bound of modulus number line + * @return midpoint of 2 numbers on modulus number line + */ + public static double modulusMidpoint(double num1, double num2, double lowerBound, double upperBound) { + num1 = MathUtil.inputModulus(num1, lowerBound, upperBound); + num2 = MathUtil.inputModulus(num2, lowerBound, upperBound); + if (Math.abs(num1 - num2) > (upperBound - lowerBound) / 2) { + return MathUtil.inputModulus((num1 + num2) / 2 + (upperBound - lowerBound) / 2, lowerBound, upperBound); + } + return (num1 + num2) / 2; + } + + /** + * Interpolates between two numbers on modulus number line + * + * @param num1 first number + * @param num2 second number + * @param amount the amount to interpolate, 0 = first number, 1 = second number + * @param lowerBound lower bound of modulus number line + * @param upperBound upper bound of modulus number line + * @return interpolated value between 2 numbers on modulus number line + */ + public static double modulusInterpolate(double num1, double num2, double amount, double lowerBound, double upperBound) { + num1 = MathUtil.inputModulus(num1, lowerBound, upperBound); + num2 = MathUtil.inputModulus(num2, lowerBound, upperBound); + if (Math.abs(num1 - num2) > (upperBound - lowerBound) / 2) { + if(num1 < num2){ + num1 += upperBound-lowerBound; + }else{ + num2 += upperBound-lowerBound; + } + } + return MathUtil.inputModulus((1-amount)*num1 + amount*num2, lowerBound, upperBound); + } + + /** + * Calls {@link #mean(double...)}. + * + * @param data the list of data to find the mean of + * @return the mean of the data + */ + public static double mean(List data) { + return mean(doubleListToArray(data)); + } + + /** + * Finds the mean of the provided array of doubles + * + * @param data an array of doubles + * @return the mean of the data + */ + public static double mean(double... data) { + double mean = 0; + for (double datum : data) { + mean += datum; + } + mean /= data.length; + return mean; + } + + /** + * Calls {@link #stdDev(double...)}. + * + * @param data the list of data to find the standard deviation of + * @return the standard deviation of the data + */ + public static double stdDev(List data) { + return stdDev(doubleListToArray(data)); + } + + /** + * Finds the standard deviation of the provided array of doubles + * + * @param data an array of doubles + * @return the standard deviation of the data + */ + public static double stdDev(double... data) { + if (data.length == 0 || data.length == 1) return 0; + + double mean = mean(data); + + double total = 0; + for (double datum : data) { + total += Math.pow(datum - mean, 2); + } + return Math.sqrt(total / (data.length - 1)); + } + + private static double[] doubleListToArray(List arrayList) { + return arrayList.stream().mapToDouble(Double::doubleValue).toArray(); + } + +} diff --git a/src/main/java/frc/robot/util/MotorFactory.java b/src/main/java/frc/robot/util/MotorFactory.java index 9df88b5..e545d4d 100644 --- a/src/main/java/frc/robot/util/MotorFactory.java +++ b/src/main/java/frc/robot/util/MotorFactory.java @@ -1,201 +1,163 @@ package frc.robot.util; -import java.io.IOException; - -import java.io.IOError; - -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; -import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; -import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; -import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import edu.wpi.first.wpilibj.DriverStation; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.VoltageConfigs; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + import frc.robot.constants.Constants; +/** + * Utility class for easy creation of motor controllers. + */ public class MotorFactory { - private static int talonSRXDefaultContinuousLimit = 38; - private static int kTalonSRXDefaultPeakLimit = 45; - private static int kTalonSRXDefaultPeakDuration = 125; - - private static int kSparkMaxDefaultCurrentLimit = 60; - - /////////////////////////////////////////////////////////////////////////////////////////////// - // TALON SRX - /////////////////////////////////////////////////////////////////////////////////////////////// - - /** - * Create a TalonSRX with current limiting enabled, using parameters. It will - * have current - * limiting and voltage compensation enabled and be set to Brake Mode. - * - * @param id the ID of the TalonSRX - * @param continuousCurrentLimit the continuous current limit to set in amps (A) - * @param peakCurrentLimit the peak current limit to set in amps (A) - * @param peakCurrentDuration the peak current limit duration to set in - * milliseconds (ms) - * @return a fully configured TalonSRX object - */ - public static WPI_TalonSRX createTalonSRX( - int id, int continuousCurrentLimit, int peakCurrentLimit, int peakCurrentDuration) { - - TalonSRXConfiguration config = new TalonSRXConfiguration(); - config.continuousCurrentLimit = continuousCurrentLimit; - config.peakCurrentLimit = peakCurrentLimit; - config.peakCurrentDuration = peakCurrentDuration; - config.voltageCompSaturation = Constants.kMaxVoltage; - - WPI_TalonSRX talon = new WPI_TalonSRX(id); - talon.configFactoryDefault(); - talon.configAllSettings(config); - talon.enableCurrentLimit(true); - talon.enableVoltageCompensation(true); - talon.setNeutralMode(NeutralMode.Brake); - - return talon; - } - - /** - * Create a TalonSRX using default current limits. - * - * @param id the ID of the TalonSRX - * @return a fully configured TalonSRX object - */ - public static WPI_TalonSRX createTalonSRXDefault(int id) { - return createTalonSRX(id, talonSRXDefaultContinuousLimit, kTalonSRXDefaultPeakLimit, kTalonSRXDefaultPeakDuration); - } - - /////////////////////////////////////////////////////////////////////////////////////////////// - // SPARK MAX - /////////////////////////////////////////////////////////////////////////////////////////////// - - /** - * Create a CANSparkMax with current limiting enabled - * - * @param id the ID of the Spark MAX - * @param motortype the type of motor the Spark MAX is connected to - * @param stallLimit the current limit to set at stall - * @return a fully configured CANSparkMAX - */ - public static CANSparkMax createSparkMAX(int id, MotorType motortype, int stallLimit) { - CANSparkMax sparkMAX = new CANSparkMax(id, motortype); - sparkMAX.restoreFactoryDefaults(); - sparkMAX.enableVoltageCompensation(Constants.kMaxVoltage); - sparkMAX.setSmartCurrentLimit(stallLimit); - sparkMAX.setIdleMode(IdleMode.kBrake); - - sparkMAX.burnFlash(); - return sparkMAX; - } - - /** - * Create a CANSparkMax with default current limiting enabled - * - * @param id the ID of the Spark MAX - * @param motortype the type of motor the Spark MAX is connected to - * @return a fully configured CANSparkMAX - */ - public static CANSparkMax createSparkMAXDefault(int id, MotorType motortype) { - return createSparkMAX(id, motortype, kSparkMaxDefaultCurrentLimit); - } - - /////////////////////////////////////////////////////////////////////////////////////////////// - // TALON FX (Falcon 500) - /////////////////////////////////////////////////////////////////////////////////////////////// - - /** - * - * Creates a TalonFX with all current limit options. If you would like to use - * defaults it is recommended to use the other createTalonFX.. methods. - * - * @param id the id of the the motor - * @param CANBus the CAN Bus the motor is on - * @param StatorLimitEnable whether or not to enable stator limiting - * @param StatorCurrentLimit the current, in amps, to return to after the - * stator limit is triggered - * @param StatorTriggerThreshold the threshold current to trigger the stator - * limit - * @param StatorTriggerDuration the duration, in seconds, the current is above - * the threshold before triggering - * @param SupplyLimitEnable whether or not to enable supply limiting - * @param SupplyCurrentLimit the current, in amps, to return to after the - * supply limit is triggered - * @param SupplyTriggerThreshold the threshold current to trigger the supply - * limit - * @param SupplyTriggerDuration the duration, in seconds, the current is above - * the threshold before triggering - * @return A fully configured TalonFX - */ - public static WPI_TalonFX createTalonFXFull(int id, String CANBus, boolean StatorLimitEnable, - double StatorCurrentLimit, - double StatorTriggerThreshold, double StatorTriggerDuration, boolean SupplyLimitEnable, double SupplyCurrentLimit, - double SupplyTriggerThreshold, double SupplyTriggerDuration) { - - if (id == -1) { - return null; + private static final int SPARK_MAX_DEFAULT_CURRENT_LIMIT = 60; + + /////////////////////////////////////////////////////////////////////////////////////////////// + // SPARK MAX + /////////////////////////////////////////////////////////////////////////////////////////////// + + /** + * Create a SparkMax with current limiting enabled + * + * @param id the ID of the Spark MAX + * @param motortype the type of motor the Spark MAX is connected to + * @param stallLimit the current limit to set at stall + * @return a fully configured CANSparkMAX + */ + public static SparkMax createSparkMAX(int id, MotorType motortype, int stallLimit) { + SparkMax sparkMAX = new SparkMax(id, motortype); + + sparkMAX.configure(new SparkMaxConfig() + .voltageCompensation(Constants.ROBOT_VOLTAGE) + .smartCurrentLimit(stallLimit) + .idleMode(IdleMode.kBrake), + ResetMode.kResetSafeParameters, + PersistMode.kNoPersistParameters + ); + return sparkMAX; + } + + /** + * Create a SparkMax with default current limiting enabled + * + * @param id the ID of the Spark MAX + * @param motortype the type of motor the Spark MAX is connected to + * @return a fully configured CANSparkMAX + */ + public static SparkMax createSparkMAXDefault(int id, MotorType motortype) { + return createSparkMAX(id, motortype, SPARK_MAX_DEFAULT_CURRENT_LIMIT); + } + + /////////////////////////////////////////////////////////////////////////////////////////////// + // TALON FX (Falcon 500 and Kraken X60) + /////////////////////////////////////////////////////////////////////////////////////////////// + + /** + * Creates a TalonFX with all current limit options. If you would like to use + * defaults it is recommended to use the other createTalonFX.. methods. + * + * @param id the CAN ID of the TalonFX + * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio". + * @param StatorLimitEnable whether to enable stator limiting + * @param StatorCurrentLimit the current, in amps, to return to after the + * stator limit is triggered + * @param StatorTriggerThreshold the threshold current to trigger the stator + * limit + * @param StatorTriggerDuration the duration, in seconds, the current is above + * the threshold before triggering + * @param SupplyLimitEnable whether to enable supply limiting + * @param SupplyCurrentLimit the current, in amps, to return to after the + * supply limit is triggered + * @param SupplyTriggerThreshold the threshold current to trigger the supply + * limit + * @param SupplyTriggerDuration the duration, in seconds, the current is above + * the threshold before triggering + * @return A fully configured TalonFX + */ + public static TalonFX createTalonFXFull(int id, String CANBus, boolean StatorLimitEnable, + double StatorCurrentLimit, + double StatorTriggerThreshold, double StatorTriggerDuration, boolean SupplyLimitEnable, double SupplyCurrentLimit, + double SupplyTriggerThreshold, double SupplyTriggerDuration) { + + if (id == -1) { + return null; + } + + TalonFX talon = new TalonFX(id, CANBus); + + TalonFXConfiguration config = new TalonFXConfiguration(); + + // See explanations for Supply and Stator limiting in FalconConstants.java + config.CurrentLimits = new CurrentLimitsConfigs().withStatorCurrentLimitEnable(StatorLimitEnable).withStatorCurrentLimit(StatorCurrentLimit). + withSupplyCurrentLimitEnable(SupplyLimitEnable).withSupplyCurrentLimit(SupplyCurrentLimit). + withSupplyCurrentLowerLimit(SupplyTriggerThreshold).withSupplyCurrentLowerTime(SupplyTriggerDuration); + + config.Voltage = new VoltageConfigs().withPeakForwardVoltage(Constants.ROBOT_VOLTAGE); + + talon.getConfigurator().apply(config); + talon.setNeutralMode(NeutralModeValue.Brake); + + + return talon; + } + + /** + * Creates a TalonFX with all the default settings. + * + * @param id the id of the motor + * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio". + */ + public static TalonFX createTalonFX(int id, String CANBus) { + return createTalonFXFull(id, CANBus, Constants.TALONFX_STATOR_LIMIT_ENABLE, Constants.TALONFX_STATOR_CURRENT_LIMIT, + Constants.TALONFX_STATOR_TRIGGER_THRESHOLD, Constants.TALONFX_STATOR_TRIGGER_DURATION, + Constants.TALONFX_SUPPLY_LIMIT_ENABLE, Constants.TALONFX_SUPPLY_CURRENT_LIMIT, + Constants.TALONFX_SUPPLY_TRIGGER_THRESHOLD, Constants.TALONFX_SUPPLY_TRIGGER_DURATION); } - WPI_TalonFX talon = new WPI_TalonFX(id, CANBus); - - if (talon.getFirmwareVersion() != Constants.falcon.kFirmwareVersion) { - String errorMessage = "TalonFX " + id + " firmware incorrect. Has " + talon.getFirmwareVersion() - + ", currently FalconConstants.java requires: " + Constants.falcon.kFirmwareVersion; - if (Constants.falcon.kBreakOnWrongFirmware) { - DriverStation.reportError(errorMessage, true); - throw new IOError(new IOException(errorMessage)); - } else { - DriverStation.reportWarning(errorMessage + ", ignoring due to user specification.", false); - } + /** + * Creates a TalonFX with supply current limit options. + *

    + * Supply current is current that's being drawn at the input bus voltage. + * Supply limiting is useful for preventing breakers from tripping in the PDP. + * + * @param id the CAN ID of the TalonFX + * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio". + * @param currentLimit the current, in amps, to return to after the supply limit is triggered + * @param triggerThreshold the threshold current to trigger the supply limit + * @param triggerDuration the duration, in seconds, the current is above the threshold before triggering + */ + public static TalonFX createTalonFXSupplyLimit(int id, String CANBus, double currentLimit, + double triggerThreshold, double triggerDuration) { + return createTalonFXFull(id, CANBus, Constants.TALONFX_STATOR_LIMIT_ENABLE, Constants.TALONFX_STATOR_CURRENT_LIMIT, + Constants.TALONFX_STATOR_TRIGGER_THRESHOLD, Constants.TALONFX_STATOR_TRIGGER_DURATION, true, currentLimit, + triggerThreshold, triggerDuration); } - TalonFXConfiguration config = new TalonFXConfiguration(); - - // See explanations for Supply and Stator limiting in FalconConstants.java - config.statorCurrLimit = new StatorCurrentLimitConfiguration(StatorLimitEnable, StatorCurrentLimit, - StatorTriggerThreshold, StatorTriggerDuration); - config.supplyCurrLimit = new SupplyCurrentLimitConfiguration(SupplyLimitEnable, SupplyCurrentLimit, - SupplyTriggerThreshold, SupplyTriggerDuration); - - config.voltageCompSaturation = Constants.kMaxVoltage; - - talon.configFactoryDefault(); - talon.configAllSettings(config); - talon.enableVoltageCompensation(true); - talon.setNeutralMode(NeutralMode.Brake); - talon.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); - - return talon; - } - - /** - * @param id - * @param CANBus - */ - public static WPI_TalonFX createTalonFX(int id, String CANBus) { - return createTalonFXFull(id, CANBus, Constants.falcon.kStatorLimitEnable, Constants.falcon.kStatorCurrentLimit, - Constants.falcon.kStatorTriggerThreshold, Constants.falcon.kStatorTriggerDuration, - Constants.falcon.kSupplyLimitEnable, Constants.falcon.kSupplyCurrentLimit, - Constants.falcon.kSupplyTriggerThreshold, Constants.falcon.kSupplyTriggerDuration); - } - - public static WPI_TalonFX createTalonFXSupplyLimit(int id, String CANBus, double currentLimit, - double triggerThreshold, double triggerDuration) { - return createTalonFXFull(id, CANBus, Constants.falcon.kStatorLimitEnable, Constants.falcon.kStatorCurrentLimit, - Constants.falcon.kStatorTriggerThreshold, Constants.falcon.kStatorTriggerDuration, true, currentLimit, - triggerThreshold, triggerDuration); - } - - public static WPI_TalonFX createTalonFXStatorLimit(int id, String CANBus, double currentLimit, - double triggerThreshold, double triggerDuration) { - return createTalonFXFull(id, CANBus, true, currentLimit, triggerThreshold, triggerDuration, - Constants.falcon.kSupplyLimitEnable, Constants.falcon.kSupplyCurrentLimit, - Constants.falcon.kSupplyTriggerThreshold, Constants.falcon.kSupplyTriggerDuration); - } -} + /** + * Creates a TalonFX with stator current limit options. + *

    + * Stator current is current that’s being drawn by the motor. + * Stator limiting is useful for limiting acceleration/heat. + * + * @param id the CAN ID of the TalonFX + * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio". + * @param currentLimit the current, in amps, to return to after the stator limit is triggered + * @param triggerThreshold the threshold current to trigger the stator limit + * @param triggerDuration the duration, in seconds, the current is above the threshold before triggering + */ + public static TalonFX createTalonFXStatorLimit(int id, String CANBus, double currentLimit, + double triggerThreshold, double triggerDuration) { + return createTalonFXFull(id, CANBus, true, currentLimit, triggerThreshold, triggerDuration, + Constants.TALONFX_SUPPLY_LIMIT_ENABLE, Constants.TALONFX_SUPPLY_CURRENT_LIMIT, + Constants.TALONFX_SUPPLY_TRIGGER_THRESHOLD, Constants.TALONFX_SUPPLY_TRIGGER_DURATION); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/PathGroupLoader.java b/src/main/java/frc/robot/util/PathGroupLoader.java index 084bd85..b1db28c 100644 --- a/src/main/java/frc/robot/util/PathGroupLoader.java +++ b/src/main/java/frc/robot/util/PathGroupLoader.java @@ -1,49 +1,70 @@ package frc.robot.util; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; +import frc.robot.constants.AutoConstants; + import java.io.File; -import java.util.ArrayList; import java.util.HashMap; +import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.PathConstraints; -import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; +/** + * Utility class for loading paths using pathplanner. + */ +public class PathGroupLoader { -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Filesystem; -import frc.robot.constants.Constants; - -public class PathGroupLoader{ - - private static HashMap> pathGroups = new HashMap>(); - - public static void loadPathGroups() { - double totalTime = 0; - File[] directoryListing = Filesystem.getDeployDirectory().toPath().resolve(Constants.auto.kTrajectoryDirectory).toFile().listFiles(); - if (directoryListing != null) { - for (File file : directoryListing) { - if (file.isFile() && file.getName().indexOf(".") != -1) { - long startTime = System.nanoTime(); - String name = file.getName().substring(0, file.getName().indexOf(".")); - pathGroups.put(name, PathPlanner.loadPathGroup(name, new PathConstraints(Constants.auto.kMaxAutoSpeed, Constants.auto.kMaxAutoAccel))); - double time = (System.nanoTime() - startTime) / 1000000.0; - totalTime += time; - System.out.println("Processed file: " + file.getName() + ", took " + time + " milliseconds."); + // private static final HashMap> pathGroups = new HashMap<>(); + private static final HashMap pathGroups = new HashMap<>(); + + + /** + * Loads all the paths in the trajectory directory (specified in the constants). + * These paths are loaded and stored so that they do not take time while the robot is running + * and can be accessed with {@link #getPathGroup(String)} + */ + public static void loadPathGroups() { + double totalTime = 0; + File[] directoryListing = Filesystem.getDeployDirectory().toPath().resolve(AutoConstants.TRAJECTORY_DIRECTORY).toFile().listFiles(); + + if (directoryListing != null) { + for (File file : directoryListing) { + if (file.isFile() && file.getName().contains(".")) { + try { + long startTime = System.nanoTime(); + String name = file.getName().substring(0, file.getName().indexOf(".")); + // pathGroups.put(name, PathPlannerAuto.getPathGroupFromAutoFile(name)); + pathGroups.put(name, PathPlannerPath.fromPathFile(name)); + double time = (System.nanoTime() - startTime) / 1000000.0; + totalTime += time; + System.out.println("Processed file: " + file.getName() + ", took " + time + " milliseconds."); + } catch (Exception e) { + DriverStation.reportError(e.getMessage(), true); + } + } + } + } else { + System.out.println("Error processing file"); + DriverStation.reportWarning( + "Issue with finding path files. Paths will not be loaded.", + true + ); } - } - } else { - System.out.println("Error processing file"); - DriverStation.reportWarning( - "Issue with finding path files. Paths will not be loaded.", - true - ); + System.out.println("File processing took a total of " + totalTime + " milliseconds"); } - System.out.println("File processing took a total of " + totalTime + " milliseconds"); - } - public static ArrayList getPathGroup(String pathGroupName) { - if (pathGroups.get(pathGroupName) == null) { - System.out.println("Error retrieving " + pathGroupName + " path!"); + /** + * Gets a path that has already been loaded with {@link #loadPathGroups()}. The path group is a list + * of trajectories that path planner can run. + * + * @param pathGroupName the name of the file, without any extensions. This should be the same exact name that is displayed in pathplanner + * @return a list of trajectories that path planner can run. + */ + public static PathPlannerPath getPathGroup(String pathGroupName) { + if (pathGroups.get(pathGroupName) == null) { + System.out.println("Error retrieving " + pathGroupName + " path!"); + } + return pathGroups.get(pathGroupName); } - return pathGroups.get(pathGroupName); - } + + } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/PhoenixOdometryThread.java b/src/main/java/frc/robot/util/PhoenixOdometryThread.java new file mode 100644 index 0000000..6ae5d60 --- /dev/null +++ b/src/main/java/frc/robot/util/PhoenixOdometryThread.java @@ -0,0 +1,154 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.util; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.subsystems.drivetrain.Drivetrain; + +import java.util.ArrayList; +import java.util.List; +import java.util.Queue; +import java.util.concurrent.ArrayBlockingQueue; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.DoubleSupplier; + +/** + * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. + * + *

    This version is intended for Phoenix 6 devices on both the RIO and CANivore buses. When using + * a CANivore, the thread uses the "waitForAll" blocking method to enable more consistent sampling. + * This also allows Phoenix Pro users to benefit from lower latency between devices using CANivore + * time synchronization. + */ +public class PhoenixOdometryThread extends Thread { + private final Lock signalsLock = + new ReentrantLock(); // Prevents conflicts when registering signals + private BaseStatusSignal[] phoenixSignals = new BaseStatusSignal[0]; + private final List genericSignals = new ArrayList<>(); + private final List> phoenixQueues = new ArrayList<>(); + private final List> genericQueues = new ArrayList<>(); + private final List> timestampQueues = new ArrayList<>(); + + private static PhoenixOdometryThread instance = null; + + public static PhoenixOdometryThread getInstance() { + if (instance == null) { + instance = new PhoenixOdometryThread(); + } + return instance; + } + + private PhoenixOdometryThread() { + setName("PhoenixOdometryThread"); + setDaemon(true); + } + + @Override + public void start() { + if (timestampQueues.size() > 0) { + super.start(); + } + } + + /** Registers a Phoenix signal to be read from the thread. */ + public Queue registerSignal(StatusSignal signal) { + Queue queue = new ArrayBlockingQueue<>(20); + signalsLock.lock(); + Drivetrain.odometryLock.lock(); + try { + BaseStatusSignal[] newSignals = new BaseStatusSignal[phoenixSignals.length + 1]; + System.arraycopy(phoenixSignals, 0, newSignals, 0, phoenixSignals.length); + newSignals[phoenixSignals.length] = signal; + phoenixSignals = newSignals; + phoenixQueues.add(queue); + } finally { + signalsLock.unlock(); + Drivetrain.odometryLock.unlock(); + } + return queue; + } + + /** Registers a generic signal to be read from the thread. */ + public Queue registerSignal(DoubleSupplier signal) { + Queue queue = new ArrayBlockingQueue<>(20); + signalsLock.lock(); + Drivetrain.odometryLock.lock(); + try { + genericSignals.add(signal); + genericQueues.add(queue); + } finally { + signalsLock.unlock(); + Drivetrain.odometryLock.unlock(); + } + return queue; + } + + /** Returns a new queue that returns timestamp values for each sample. */ + public Queue makeTimestampQueue() { + Queue queue = new ArrayBlockingQueue<>(20); + Drivetrain.odometryLock.lock(); + try { + timestampQueues.add(queue); + } finally { + Drivetrain.odometryLock.unlock(); + } + return queue; + } + + @Override + public void run() { + while (true) { + // Wait for updates from all signals + signalsLock.lock(); + try { + BaseStatusSignal.waitForAll(2.0 / 250, phoenixSignals);} + finally { + signalsLock.unlock(); + } + + // Save new data to queues + Drivetrain.odometryLock.lock(); + try { + // Sample timestamp is current FPGA time minus average CAN latency + // Default timestamps from Phoenix are NOT compatible with + // FPGA timestamps, this solution is imperfect but close + double timestamp = RobotController.getFPGATime() / 1e6; + double totalLatency = 0.0; + for (BaseStatusSignal signal : phoenixSignals) { + totalLatency += signal.getTimestamp().getLatency(); + } + if (phoenixSignals.length > 0) { + timestamp -= totalLatency / phoenixSignals.length; + } + + // Add new samples to queues + for (int i = 0; i < phoenixSignals.length; i++) { + phoenixQueues.get(i).offer(phoenixSignals[i].getValueAsDouble()); + } + for (int i = 0; i < genericSignals.size(); i++) { + genericQueues.get(i).offer(genericSignals.get(i).getAsDouble()); + } + for (int i = 0; i < timestampQueues.size(); i++) { + timestampQueues.get(i).offer(timestamp); + } + } finally { + Drivetrain.odometryLock.unlock(); + } + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/PhoenixUtil.java b/src/main/java/frc/robot/util/PhoenixUtil.java new file mode 100644 index 0000000..8f4f6dd --- /dev/null +++ b/src/main/java/frc/robot/util/PhoenixUtil.java @@ -0,0 +1,44 @@ +package frc.robot.util; +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import java.util.function.Supplier; + +public class PhoenixUtil { + /** Attempts to run the command until no error is produced. */ + public static void tryUntilOk(int maxAttempts, Supplier command) { + for (int i = 0; i < maxAttempts; i++) { + var error = command.get(); + if (error.isOK()) break; + } + } + + /** Signals for synchronized refresh. */ + private static BaseStatusSignal[] canivoreSignals = new BaseStatusSignal[0]; + + private static BaseStatusSignal[] rioSignals = new BaseStatusSignal[0]; + + /** Registers a set of signals for synchronized refresh. */ + public static void registerSignals(boolean canivore, BaseStatusSignal... signals) { + if (canivore) { + BaseStatusSignal[] newSignals = new BaseStatusSignal[canivoreSignals.length + signals.length]; + System.arraycopy(canivoreSignals, 0, newSignals, 0, canivoreSignals.length); + System.arraycopy(signals, 0, newSignals, canivoreSignals.length, signals.length); + canivoreSignals = newSignals; + } else { + BaseStatusSignal[] newSignals = new BaseStatusSignal[rioSignals.length + signals.length]; + System.arraycopy(rioSignals, 0, newSignals, 0, rioSignals.length); + System.arraycopy(signals, 0, newSignals, rioSignals.length, signals.length); + rioSignals = newSignals; + } + } + + /** Refresh all registered signals. */ + public static void refreshAll() { + if (canivoreSignals.length > 0) { + BaseStatusSignal.refreshAll(canivoreSignals); + } + if (rioSignals.length > 0) { + BaseStatusSignal.refreshAll(rioSignals); + } + } +} diff --git a/src/main/java/frc/robot/util/ShuffleboardManager.java b/src/main/java/frc/robot/util/ShuffleboardManager.java deleted file mode 100644 index e8dc64f..0000000 --- a/src/main/java/frc/robot/util/ShuffleboardManager.java +++ /dev/null @@ -1,70 +0,0 @@ -package frc.robot.util; - -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.PrintCommand; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Robot; - -public class ShuffleboardManager { - - SendableChooser m_autoCommand = new SendableChooser<>(); - SendableChooser m_testType = new SendableChooser<>(); - - ShuffleboardTab m_mainTab = Shuffleboard.getTab("Main"); - ShuffleboardTab m_autoTab = Shuffleboard.getTab("Auto"); - ShuffleboardTab m_testTab = Shuffleboard.getTab("Test"); - - NetworkTableEntry m_commandScheduler = m_mainTab.add("Command Scheduler", "NULL").getEntry(); - - public void setup() { - LiveWindow.disableAllTelemetry(); // LiveWindow is causing periodic loop overruns - - autoChooserUpdate(); - - m_autoTab.add("Auto Chooser", m_autoCommand); - m_testTab.add("Run Test", m_testType); - } - - public Command getAutonomousCommand() { - return m_autoCommand.getSelected(); - } - - public void autoChooserUpdate() { - m_autoCommand.addOption("Do Nothing", new PrintCommand("This will do nothing!")); - } - - - public TestType getTestType() { - if (Robot.isTestMode()) { - return m_testType.getSelected(); - } - return TestType.NONE; - } - - public Trigger isTestTypeTrigger(TestType testType) { - return new Trigger(() -> m_testType.getSelected() == testType); - } - - public void testChooserUpdate() { - m_testType.addOption("No Test", TestType.NONE); - } - - public void loadCommandSchedulerShuffleboard() { - CommandScheduler.getInstance() - .onCommandInitialize( - command -> m_commandScheduler.setString(command.getName() + " initialized.")); - CommandScheduler.getInstance() - .onCommandInterrupt( - command -> m_commandScheduler.setString(command.getName() + " interrupted.")); - CommandScheduler.getInstance() - .onCommandFinish(command -> m_commandScheduler.setString(command.getName() + " finished.")); - } -} diff --git a/src/main/java/frc/robot/util/SwerveModulePose.java b/src/main/java/frc/robot/util/SwerveModulePose.java new file mode 100644 index 0000000..7f138da --- /dev/null +++ b/src/main/java/frc/robot/util/SwerveModulePose.java @@ -0,0 +1,130 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.util; + +import java.util.Arrays; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import frc.robot.subsystems.drivetrain.Drivetrain; + +/** + * Stores and updates the position of each module + */ +public class SwerveModulePose { + + private double[] dist = {0,0,0,0}; + private Translation2d[] moduleTranslations; + private Pose2d[] modulePositions; + private double[] angles; + private Drivetrain drive; + private double prevRotation; + private Pose2d[] displayPoses; + + /** + * Creates a new SwerveModulePose object to store and update the positions of each module + * @param drive The drivetrain + * @param modulePositions The translations of the modules relative to the center of the robot + */ + public SwerveModulePose(Drivetrain drive, Translation2d... modulePositions){ + this.drive = drive; + this.moduleTranslations = modulePositions; + this.modulePositions = new Pose2d[4]; + angles = new double[4]; + reset(); + update(); + reset(); + } + + /** + * Updates the module positions + */ + public void update(){ + SwerveModuleState[] states = drive.getModuleStates(); + double currentRotation = drive.getYaw().getRadians(); + double chassisRotation = currentRotation - prevRotation; + + for(int i = 0; i<4; i++){ + double position = drive.getModules()[i].getPosition().distanceMeters; + double distance = position - dist[i]; + dist[i] = position; + + Twist2d twist = new Twist2d(distance, 0, MathUtil.angleModulus(states[i].angle.getRadians()-angles[i] + chassisRotation)); + angles[i] = states[i].angle.getRadians(); + modulePositions[i] = modulePositions[i].exp(twist); + + displayPoses[i] = new Pose2d( + modulePositions[i].getTranslation(), + EqualsUtil.epsilonEquals(states[i].speedMetersPerSecond, 0, 0.01) ? displayPoses[i].getRotation() : + states[i].speedMetersPerSecond < 0 ? modulePositions[i].getRotation().plus(new Rotation2d(Math.PI)) : + modulePositions[i].getRotation() + ); + } + prevRotation = currentRotation; + } + + /** + * Gets the positions of the modules + * @return The module poses as an array of Pose2ds + */ + public Pose2d[] getModulePoses(){ + return displayPoses; + } + + /** + * Resets the modules to the correct positions relative to the robot + */ + public void reset(){ + Pose2d chassisPose2d = drive.getPose(); + SwerveModuleState[] states = drive.getModuleStates(); + for (int i = 0; i<4; i++){ + angles[i] = states[i].angle.getRadians(); + this.modulePositions[i] = new Pose2d(moduleTranslations[i].rotateBy(chassisPose2d.getRotation()).plus(chassisPose2d.getTranslation()), new Rotation2d(angles[i]).plus(chassisPose2d.getRotation())); + } + prevRotation = drive.getYaw().getRadians(); + displayPoses = Arrays.copyOf(modulePositions, 4); + } + + /** + * Gets whehter or not the modules have slipped + * A module has slipped if it has moved 0.3m (about 1ft) from its correct position relative to the other modules + * @return True if any of the modules have slipped, false otherwise + */ + public boolean slipped(){ + Translation2d total = new Translation2d(); + for(Pose2d pose : modulePositions){ + total = total.plus(pose.getTranslation()); + } + Pose2d drivePose = new Pose2d(total.div(4), drive.getYaw()); + for(int i = 0; i < 4; i++){ + double dist = modulePositions[i].relativeTo(drivePose).getTranslation().getDistance(moduleTranslations[i]); + if(dist > 0.3){ + return true; + } + } + return false; + } + + /** + * Gets the average slip distance + * @return The average distance between each module and its correct position + */ + public double getAverageSlip(){ + Translation2d total = new Translation2d(); + for(Pose2d pose : modulePositions){ + total = total.plus(pose.getTranslation()); + } + Pose2d drivePose = new Pose2d(total.div(4), drive.getYaw()); + double slip = 0; + for(int i = 0; i < 4; i++){ + slip += modulePositions[i].relativeTo(drivePose).getTranslation().getDistance(moduleTranslations[i]); + } + return slip/4; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/SwerveStuff/ModuleLimits.java b/src/main/java/frc/robot/util/SwerveStuff/ModuleLimits.java new file mode 100644 index 0000000..e8ee61f --- /dev/null +++ b/src/main/java/frc/robot/util/SwerveStuff/ModuleLimits.java @@ -0,0 +1,11 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.util.SwerveStuff; + +public record ModuleLimits( + double maxDriveVelocity, double maxDriveAcceleration, double staticFriction, double maxSteeringVelocity) {} diff --git a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java new file mode 100644 index 0000000..374c7de --- /dev/null +++ b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java @@ -0,0 +1,13 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.util.SwerveStuff; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModuleState; + +public record SwerveSetpoint(ChassisSpeeds chassisSpeeds, SwerveModuleState[] moduleStates) {} diff --git a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java new file mode 100644 index 0000000..2c7dd20 --- /dev/null +++ b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java @@ -0,0 +1,489 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.util.SwerveStuff; + +import static frc.robot.util.EqualsUtil.*; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; + +import frc.robot.constants.Constants; +import frc.robot.constants.swerve.DriveConstants; +import frc.robot.util.EqualsUtil; +import frc.robot.util.GeomUtil; + +/** + * "Inspired" by FRC team 254. See the license file in the root directory of this project. + * + *

    Takes a prior setpoint (ChassisSpeeds), a desired setpoint (from a driver, or from a path + * follower), and outputs a new setpoint that respects all of the kinematic constraints on module + * rotation speed and wheel velocity/acceleration. By generating a new setpoint every iteration, the + * robot will converge to the desired setpoint quickly while avoiding any intermediate state that is + * kinematically infeasible (and can result in wheel slip or robot heading drift as a result). + */ + +public class SwerveSetpointGenerator { + private final SwerveDriveKinematics kinematics= DriveConstants.KINEMATICS; + private final Translation2d[] moduleLocations = DriveConstants.MODULE_LOCATIONS; + + /** + * Check if it would be faster to go to the opposite of the goal heading (and reverse drive + * direction). + * + * @param prevToGoal The rotation from the previous state to the goal state (i.e. + * prev.inverse().rotateBy(goal)). + * @return True if the shortest path to achieve this rotation involves flipping the drive + * direction. + */ + private boolean flipHeading(Rotation2d prevToGoal) { + return Math.abs(prevToGoal.getRadians()) > Math.PI / 2.0; + } + + private double unwrapAngle(double ref, double angle) { + double diff = angle - ref; + if (diff > Math.PI) { + return angle - 2.0 * Math.PI; + } else if (diff < -Math.PI) { + return angle + 2.0 * Math.PI; + } else { + return angle; + } + } + + @FunctionalInterface + private interface Function2d { + double f(double x, double y); + } + + /** + * Find the root of the generic 2D parametric function 'func' using the regula falsi technique. + * This is a pretty naive way to do root finding, but it's usually faster than simple bisection + * while being robust in ways that e.g. the Newton-Raphson method isn't. + * + * @param func The Function2d to take the root of. + * @param x_0 x value of the lower bracket. + * @param y_0 y value of the lower bracket. + * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during + * recursion) + * @param x_1 x value of the upper bracket. + * @param y_1 y value of the upper bracket. + * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during + * recursion) + * @param iterations_left Number of iterations of root finding left. + * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the + * (approximate) root. + */ + private double findRoot( + Function2d func, + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + int iterations_left) { + if (iterations_left < 0 || epsilonEquals(f_0, f_1)) { + return 1.0; + } + var s_guess = Math.max(0.0, Math.min(1.0, -f_0 / (f_1 - f_0))); + var x_guess = (x_1 - x_0) * s_guess + x_0; + var y_guess = (y_1 - y_0) * s_guess + y_0; + var f_guess = func.f(x_guess, y_guess); + if (Math.signum(f_0) == Math.signum(f_guess)) { + // 0 and guess on same side of root, so use upper bracket. + return s_guess + + (1.0 - s_guess) + * findRoot(func, x_guess, y_guess, f_guess, x_1, y_1, f_1, iterations_left - 1); + } else { + // Use lower bracket. + return s_guess + * findRoot(func, x_0, y_0, f_0, x_guess, y_guess, f_guess, iterations_left - 1); + } + } + + protected double findSteeringMaxS( + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + double max_deviation, + int max_iterations) { + f_1 = unwrapAngle(f_0, f_1); + double diff = f_1 - f_0; + if (Math.abs(diff) <= max_deviation) { + // Can go all the way to s=1. + return 1.0; + } + double offset = f_0 + Math.signum(diff) * max_deviation; + Function2d func = + (x, y) -> { + return unwrapAngle(f_0, Math.atan2(y, x)) - offset; + }; + return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations); + } + + protected double findDriveMaxS_254version( + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + double max_vel_step, + int max_iterations) { + double diff = f_1 - f_0; + if (Math.abs(diff) <= max_vel_step) { + // Can go all the way to s=1. + return 1.0; + } + double offset = f_0 + Math.signum(diff) * max_vel_step; + Function2d func = + (x, y) -> { + return Math.hypot(x, y) - offset; + }; + return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations); + } + + /** + * Limits the acceleration in all directions. + * This is different from findDriveMaxS because it includes the acceleration perpendicular to the wheel as it rotates. + * Given the same velocity step, this will return a lower S value than findDriveMaxS. + * @param x_0 The initial x velocity + * @param y_0 The initial y velocity + * @param x_1 The final x velocity + * @param y_1 The final y velocity + * @param max_vel_step The maxiumum amount the velocity can change this frame + * @param max_iterations The maximum number of iterations to use in findRoot + * @return The maximum interpolation value + */ + protected double findAccelerationMaxS( + double x_0, + double y_0, + double x_1, + double y_1, + double max_vel_step, + int max_iterations) { + double dist = Math.hypot(x_1-x_0, y_1-y_0); + if(dist <= max_vel_step){ + return 1; + } + return Math.max(0.0, Math.min(1.0, max_vel_step / dist)); + } + + protected double findDriveMaxS( + double x_0, double y_0, double x_1, double y_1, double max_vel_step) { + // Derivation: + // Want to find point P(s) between (x_0, y_0) and (x_1, y_1) where the + // length of P(s) is the target T. P(s) is linearly interpolated between the + // points, so P(s) = (x_0 + (x_1 - x_0) * s, y_0 + (y_1 - y_0) * s). + // Then, + // T = sqrt(P(s).x^2 + P(s).y^2) + // T^2 = (x_0 + (x_1 - x_0) * s)^2 + (y_0 + (y_1 - y_0) * s)^2 + // T^2 = x_0^2 + 2x_0(x_1-x_0)s + (x_1-x_0)^2*s^2 + // + y_0^2 + 2y_0(y_1-y_0)s + (y_1-y_0)^2*s^2 + // T^2 = x_0^2 + 2x_0x_1s - 2x_0^2*s + x_1^2*s^2 - 2x_0x_1s^2 + x_0^2*s^2 + // + y_0^2 + 2y_0y_1s - 2y_0^2*s + y_1^2*s^2 - 2y_0y_1s^2 + y_0^2*s^2 + // 0 = (x_0^2 + y_0^2 + x_1^2 + y_1^2 - 2x_0x_1 - 2y_0y_1)s^2 + // + (2x_0x_1 + 2y_0y_1 - 2x_0^2 - 2y_0^2)s + // + (x_0^2 + y_0^2 - T^2). + // + // To simplify, we can factor out some common parts: + // Let l_0 = x_0^2 + y_0^2, l_1 = x_1^2 + y_1^2, and + // p = x_0 * x_1 + y_0 * y_1. + // Then we have + // 0 = (l_0 + l_1 - 2p)s^2 + 2(p - l_0)s + (l_0 - T^2), + // with which we can solve for s using the quadratic formula. + + double l_0 = x_0 * x_0 + y_0 * y_0; + double l_1 = x_1 * x_1 + y_1 * y_1; + double sqrt_l_0 = Math.sqrt(l_0); + double diff = Math.sqrt(l_1) - sqrt_l_0; + if (Math.abs(diff) <= max_vel_step) { + // Can go all the way to s=1. + return 1.0; + } + + double target = sqrt_l_0 + Math.copySign(max_vel_step, diff); + double p = x_0 * x_1 + y_0 * y_1; + + // Quadratic of s + double a = l_0 + l_1 - 2 * p; + double b = 2 * (p - l_0); + double c = l_0 - target * target; + double root = Math.sqrt(b * b - 4 * a * c); + + // Check if either of the solutions are valid + // Won't divide by zero because it is only possible for a to be zero if the + // target velocity is exactly the same or the reverse of the current + // velocity, which would be caught by the difference check. + double s_1 = (-b + root) / (2 * a); + if (isValidS(s_1)) { + return s_1; + } + double s_2 = (-b - root) / (2 * a); + if (isValidS(s_2)) { + return s_2; + } + + // Since we passed the initial max_vel_step check, a solution should exist, + // but if no solution was found anyway, just don't limit movement + return 1.0; + } + + protected static boolean isValidS(double s) { + return Double.isFinite(s) && s >= 0 && s <= 1; + } + + /** + * Generate a new setpoint. + * + * @param limits The kinematic limits to respect for this setpoint. + * @param centerOfMassHeight The height of the robot's center of mass, in meters, off the ground. + * This assumes that the center of mass is in the center of the robot in the x and y directions. + * If tipping is not a potential problem this year, set this to 0. + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @param desiredState The desired state of motion, such as from the driver sticks or a path + * following algorithm. + * @param dt The loop time. + * @return A Setpoint object that satisfies all of the KinematicLimits while converging to + * desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + final ModuleLimits limits, + double centerOfMassHeight, + final SwerveSetpoint prevSetpoint, + ChassisSpeeds desiredState, + double dt) { + final Translation2d[] modules = moduleLocations; + + SwerveModuleState[] desiredModuleState = kinematics.toSwerveModuleStates(desiredState); + // Make sure desiredState respects velocity limits. + if (limits.maxDriveVelocity() > 0.0) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleState, limits.maxDriveVelocity()); + desiredState = kinematics.toChassisSpeeds(desiredModuleState); + } + + // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so + // just use the previous angle. + boolean need_to_steer = true; + if (EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(desiredState),new Twist2d())) { + need_to_steer = false; + for (int i = 0; i < modules.length; ++i) { + desiredModuleState[i].angle = prevSetpoint.moduleStates()[i].angle; + desiredModuleState[i].speedMetersPerSecond = 0.0; + } + } + + // For each module, compute local Vx and Vy vectors. + double[] prev_vx = new double[modules.length]; + double[] prev_vy = new double[modules.length]; + Rotation2d[] prev_heading = new Rotation2d[modules.length]; + double[] desired_vx = new double[modules.length]; + double[] desired_vy = new double[modules.length]; + Rotation2d[] desired_heading = new Rotation2d[modules.length]; + boolean all_modules_should_flip = true; + for (int i = 0; i < modules.length; ++i) { + prev_vx[i] = + prevSetpoint.moduleStates()[i].angle.getCos() + * prevSetpoint.moduleStates()[i].speedMetersPerSecond; + prev_vy[i] = + prevSetpoint.moduleStates()[i].angle.getSin() + * prevSetpoint.moduleStates()[i].speedMetersPerSecond; + prev_heading[i] = prevSetpoint.moduleStates()[i].angle; + if (prevSetpoint.moduleStates()[i].speedMetersPerSecond < 0.0) { + prev_heading[i] = prev_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI)); + } + desired_vx[i] = + desiredModuleState[i].angle.getCos() * desiredModuleState[i].speedMetersPerSecond; + desired_vy[i] = + desiredModuleState[i].angle.getSin() * desiredModuleState[i].speedMetersPerSecond; + desired_heading[i] = desiredModuleState[i].angle; + if (desiredModuleState[i].speedMetersPerSecond < 0.0) { + desired_heading[i] = desired_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI)); + } + if (all_modules_should_flip) { + double required_rotation_rad = + Math.abs(prev_heading[i].unaryMinus().rotateBy(desired_heading[i]).getRadians()); + if (required_rotation_rad < Math.PI / 2.0) { + all_modules_should_flip = false; + } + } + } + if (all_modules_should_flip + && !EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(prevSetpoint.chassisSpeeds()),new Twist2d()) + && !EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(desiredState),new Twist2d())) { + // It will (likely) be faster to stop the robot, rotate the modules in place to the complement + // of the desired + // angle, and accelerate again. + return generateSetpoint(limits, centerOfMassHeight, prevSetpoint, new ChassisSpeeds(), dt); + } + + // Compute the deltas between start and goal. We can then interpolate from the start state to + // the goal state; then + // find the amount we can move from start towards goal in this cycle such that no kinematic + // limit is exceeded. + double dx = desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond; + double dy = desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond; + double dtheta = + desiredState.omegaRadiansPerSecond - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond; + + // 's' interpolates between start and goal. At 0, we are at prevState and at 1, we are at + // desiredState. + double min_s = 1.0; + + // In cases where an individual module is stopped, we want to remember the right steering angle + // to command (since + // inverse kinematics doesn't care about angle, we can be opportunistically lazy). + List> overrideSteering = new ArrayList<>(modules.length); + // Enforce steering velocity limits. We do this by taking the derivative of steering angle at + // the current angle, + // and then backing out the maximum interpolant between start and goal states. We remember the + // minimum across all modules, since + // that is the active constraint. + final double max_theta_step = dt * limits.maxSteeringVelocity(); + for (int i = 0; i < modules.length; ++i) { + if (!need_to_steer) { + overrideSteering.add(Optional.of(prevSetpoint.moduleStates()[i].angle)); + continue; + } + overrideSteering.add(Optional.empty()); + if (epsilonEquals(prevSetpoint.moduleStates()[i].speedMetersPerSecond, 0.0)) { + // If module is stopped, we know that we will need to move straight to the final steering + // angle, so limit based + // purely on rotation in place. + if (epsilonEquals(desiredModuleState[i].speedMetersPerSecond, 0.0)) { + // Goal angle doesn't matter. Just leave module at its current angle. + overrideSteering.set(i, Optional.of(prevSetpoint.moduleStates()[i].angle)); + continue; + } + + var necessaryRotation = + prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(desiredModuleState[i].angle); + if (flipHeading(necessaryRotation)) { + necessaryRotation = necessaryRotation.rotateBy(Rotation2d.fromRadians(Math.PI)); + } + // getRadians() bounds to +/- Pi. + final double numStepsNeeded = Math.abs(necessaryRotation.getRadians()) / max_theta_step; + + if (numStepsNeeded <= 1.0) { + // Steer directly to goal angle. + overrideSteering.set(i, Optional.of(desiredModuleState[i].angle)); + // Don't limit the global min_s; + continue; + } else { + // Adjust steering by max_theta_step. + overrideSteering.set( + i, + Optional.of( + prevSetpoint.moduleStates()[i].angle.rotateBy( + Rotation2d.fromRadians( + Math.signum(necessaryRotation.getRadians()) * max_theta_step)))); + min_s = 0.0; + continue; + } + } + if (min_s == 0.0) { + // s can't get any lower. Save some CPU. + continue; + } + + final int kMaxIterations = 8; + double s = + findSteeringMaxS( + prev_vx[i], + prev_vy[i], + prev_heading[i].getRadians(), + desired_vx[i], + desired_vy[i], + desired_heading[i].getRadians(), + max_theta_step, + kMaxIterations); + min_s = Math.min(min_s, s); + } + + // Enforce drive wheel acceleration limits. + final double max_vel_step = dt * limits.maxDriveAcceleration(); + final double max_vel_step_2 = dt * limits.staticFriction() * Constants.GRAVITY_ACCELERATION; + for (int i = 0; i < modules.length; ++i) { + if (min_s == 0.0) { + // No need to carry on. + break; + } + double vx_min_s = + min_s == 1.0 ? desired_vx[i] : (desired_vx[i] - prev_vx[i]) * min_s + prev_vx[i]; + double vy_min_s = + min_s == 1.0 ? desired_vy[i] : (desired_vy[i] - prev_vy[i]) * min_s + prev_vy[i]; + // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we + // already know we can't go faster + // than that. + final int kMaxIterations = 10; + double s = min_s*findDriveMaxS(prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step); + + // Limit the overall acceleration of this wheel + double s2 = min_s*findAccelerationMaxS(prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step_2, kMaxIterations); + + min_s = Math.min(Math.min(min_s, s), s2); + } + + if(centerOfMassHeight > 0.02){ + // Limit the acceleration in the x and y directions separately based on the center of mass. + // To make the torque on the robot 0, we can assume all of the mass is on the back wheel, where the front is the direction the robot is accelerating toward + // Torque is equal to the force times the component of the radius perpendicular to the force + // T = torque, m = mass, a = acceleration, g = gravity acceleration, x = distance from center to wheel + // T = mgx - mah = 0 + // a = gx/h + double maxAccel = Constants.GRAVITY_ACCELERATION*(DriveConstants.TRACK_WIDTH/2)/centerOfMassHeight; + // Limit based on this calculated value + // x and y are limited separately because, when tipping in a diagonal direction, the distance is longer + double xAccel = Math.abs(desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond) / dt; + double yAccel = Math.abs(desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond) / dt; + if(!epsilonEquals(xAccel, 0)){ + double s = maxAccel / xAccel; + min_s = Math.min(min_s, s); + } + if(!epsilonEquals(yAccel, 0)){ + double s = maxAccel / yAccel; + min_s = Math.min(min_s, s); + } + } + + ChassisSpeeds retSpeeds = + new ChassisSpeeds( + prevSetpoint.chassisSpeeds().vxMetersPerSecond + min_s * dx, + prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy, + prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta); + var retStates = kinematics.toSwerveModuleStates(retSpeeds); + for (int i = 0; i < modules.length; ++i) { + final var maybeOverride = overrideSteering.get(i); + if (maybeOverride.isPresent()) { + var override = maybeOverride.get(); + if (flipHeading(retStates[i].angle.unaryMinus().rotateBy(override))) { + retStates[i].speedMetersPerSecond *= -1.0; + } + retStates[i].angle = override; + } + final var deltaRotation = + prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(retStates[i].angle); + if (flipHeading(deltaRotation)) { + retStates[i].angle = retStates[i].angle.rotateBy(Rotation2d.fromRadians(Math.PI)); + retStates[i].speedMetersPerSecond *= -1.0; + } + } + return new SwerveSetpoint(retSpeeds, retStates); + } +} diff --git a/src/main/java/frc/robot/util/SysId.java b/src/main/java/frc/robot/util/SysId.java new file mode 100644 index 0000000..6e5a655 --- /dev/null +++ b/src/main/java/frc/robot/util/SysId.java @@ -0,0 +1,46 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.util; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; + +import java.util.function.Consumer; + + +/** + * Util class for creating SysId routines +*/ +public class SysId { + + private SysIdRoutine sysIdRoutine; + + public SysId(String name, Consumer driveConsumer, Consumer logConsumer, Subsystem subsystem, Config config){ + sysIdRoutine = new SysIdRoutine( + config, + new Mechanism( + driveConsumer, + logConsumer, + subsystem, + name + ) + ); + } + public SysId(String name, Consumer driveConsumer, Subsystem subsystem, Config config){ + this(name,driveConsumer,null,subsystem,config); + } + + public Command runQuasisStatic(Direction direction){ + return sysIdRoutine.quasistatic(direction); + } + public Command runDynamic(Direction direction){ + return sysIdRoutine.dynamic(direction); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/TestType.java b/src/main/java/frc/robot/util/TestType.java deleted file mode 100644 index 364ac58..0000000 --- a/src/main/java/frc/robot/util/TestType.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc.robot.util; - -public enum TestType { - NONE, TUNE_EXAMPLE_PID, TEST_DRIVE -} diff --git a/src/main/java/frc/robot/util/TimeAccuracyTest.java b/src/main/java/frc/robot/util/TimeAccuracyTest.java new file mode 100644 index 0000000..8248fcc --- /dev/null +++ b/src/main/java/frc/robot/util/TimeAccuracyTest.java @@ -0,0 +1,51 @@ +package frc.robot.util; + +import edu.wpi.first.util.WPIUtilJNI; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +/** + * Class for testing if a value is within a certain margin of error for a certain amount of time. + */ +public class TimeAccuracyTest { + + private final BooleanSupplier accuracyTest; + private final double setpointUpdateTime; + private final double errorMargin; + private final double timeMargin; + private boolean lastUseableResult = false; + + /** + * @param actual DoubleSupplier that returns the actual value + * @param setpoint DoubleSupplier that returns the setpoint + * @param errorMargin margin of error for the test to be accurate + * @param timeMargin time in seconds that the setpoint must be held for the test to be accurate + */ + public TimeAccuracyTest(DoubleSupplier actual, DoubleSupplier setpoint, double errorMargin, double timeMargin) { + this.errorMargin = errorMargin; + this.timeMargin = timeMargin; + setpointUpdateTime = WPIUtilJNI.now() * 1e-6; + accuracyTest = () -> getDoubleAccuracyTest(actual, setpoint); + } + + /** + * Determines if the test is successful. + * + * @return true if the test is successful, false if not + */ + public boolean calculate() { + if (setpointUpdateTime + timeMargin <= WPIUtilJNI.now() * 1e-6) + lastUseableResult = accuracyTest.getAsBoolean(); + return lastUseableResult; + } + + /** + * Determines if the actual value is within the error margin of the setpoint. + * + * @return true if the actual value is within the error margin of the setpoint, false if not + */ + private boolean getDoubleAccuracyTest(DoubleSupplier actual, DoubleSupplier setpoint) { + return Math.abs(actual.getAsDouble() - setpoint.getAsDouble()) <= errorMargin; + } +} diff --git a/src/main/java/frc/robot/util/Vision/DetectedObject.java b/src/main/java/frc/robot/util/Vision/DetectedObject.java new file mode 100644 index 0000000..babcc1d --- /dev/null +++ b/src/main/java/frc/robot/util/Vision/DetectedObject.java @@ -0,0 +1,334 @@ +package frc.robot.util.Vision; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.Robot; +import frc.robot.subsystems.drivetrain.Drivetrain; + +/** + * Stores information about an object detected by vision + */ +public class DetectedObject { + private static Drivetrain drive; + public final Pose3d pose; + public final ObjectType type; + + public enum ObjectType{ + CORAL(Units.inchesToMeters(4.5/2)), + ALGAE(Units.inchesToMeters(16.25/2)), + RED_ROBOT(0), + BLUE_ROBOT(0), + NONE(0); + + public final double height; + + private ObjectType(double h){ + height = h; + } + }; + + /** + * Sets the drivetrain to use for pose calculations + * @param drive The drivetrain + */ + public static void setDrive(Drivetrain drive){ + DetectedObject.drive = drive; + } + + /** + * Creates a default DetectedObject with default attributes + */ + public DetectedObject(){ + pose = new Pose3d(); + type = ObjectType.NONE; + } + + /** + * Creates a new DetectedObject + * @param xOffset The x offset from the camera to the object in radians + * @param yOffset The y offset form the camera to the object in radians + * @param distance The distance from the camera to the object in meters + * @param type What type of object it is + * @param robotToCamera The transformation form the robot to the camera + */ + public DetectedObject(double xOffset, double yOffset, double distance, ObjectType type, Transform3d robotToCamera){ + this(xOffset, yOffset, distance, type, robotToCamera, -1); + } + + /** + * Creates a new DetectedObject + * @param xOffset The x offset from the camera to the object in radians + * @param yOffset The y offset form the camera to the object in radians + * @param distance The distance from the camera to the object in meters + * @param type What type of object it is + * @param robotToCamera The transformation form the robot to the camera + * @param timestamp The timestamp of the picture in seconds + */ + public DetectedObject(double xOffset, double yOffset, double distance, ObjectType type, Transform3d robotToCamera, double timestamp){ + this.type = type; + // Get the position relative to the camera + Translation3d translation = new Translation3d(distance, new Rotation3d(0, -yOffset, -xOffset)) + // Rotate and translate it to get the position relative to the robot + .rotateBy(robotToCamera.getRotation()) + .plus(robotToCamera.getTranslation()); + // If the drivetrain exists, rotate and translate it to get the field relative position + if(drive != null){ + Pose2d drivePose = drive.getPoseAt(timestamp); + translation = translation.rotateBy(new Rotation3d( + 0, + 0, + drivePose.getRotation().getRadians() + )).plus(new Translation3d( + drivePose.getX(), + drivePose.getY(), + 0 + )); + } + pose = new Pose3d(translation, new Rotation3d()); + } + + /** + * Creates a new DetectedObject + * @param xOffset The x offset from the camera to the object in radians + * @param yOffset The y offset form the camera to the object in radians + * @param distance The distance from the camera to the object in meters + * @param type What type of object it is + * @param robotToCamera The transformation form the robot to the camera + * @param timestamp The timestamp of the picture in seconds + */ + public DetectedObject(double xOffset, double yOffset, double distance, int type, Transform3d robotToCamera, double timestamp){ + this(xOffset, yOffset, distance, getType(type), robotToCamera, timestamp); + } + + /** + * Creates a new DetectedObject + * @param xOffset The x offset from the camera to the object in radians + * @param yOffset The y offset form the camera to the object in radians + * @param distance The distance from the camera to the object in meters + * @param type What type of object it is + * @param robotToCamera The transformation form the robot to the camera + */ + public DetectedObject(double xOffset, double yOffset, double distance, int type, Transform3d robotToCamera){ + this(xOffset, yOffset, distance, getType(type), robotToCamera, -1); + } + + /** + * Creates a new DetectedObject + * @param xOffset The x offset from the camera to the object in radians + * @param yOffset The y offset form the camera to the object in radians + * @param distance The distance from the camera to the object in meters + * @param type What type of object it is + * @param robotToCamera The transformation form the robot to the camera + * @param timestamp The timestamp of the picture in seconds + */ + public DetectedObject(double xOffset, double yOffset, double distance, String type, Transform3d robotToCamera, double timestamp){ + this(xOffset, yOffset, distance, getType(type), robotToCamera, timestamp); + } + + /** + * Creates a new DetectedObject + * @param xOffset The x offset from the camera to the object in radians + * @param yOffset The y offset form the camera to the object in radians + * @param distance The distance from the camera to the object in meters + * @param type What type of object it is + * @param robotToCamera The transformation form the robot to the camera + */ + public DetectedObject(double xOffset, double yOffset, double distance, String type, Transform3d robotToCamera){ + this(xOffset, yOffset, distance, getType(type), robotToCamera, -1); + } + + /** + * Creates a new DetectedObject, assuming the object is on the ground + * @param xOffset The x offset from the camera to the object in radians + * @param yOffset The y offset form the camera to the object in radians + * @param type What type of object it is + * @param robotToCamera The transformation form the robot to the camera + */ + public DetectedObject(double xOffset, double yOffset, ObjectType type, Transform3d robotToCamera){ + this(xOffset, yOffset, type, robotToCamera, -1); + } + + /** + * Creates a new DetectedObject, assuming the object is on the ground + * @param xOffset The x offset from the camera to the object in radians + * @param yOffset The y offset form the camera to the object in radians + * @param type What type of object it is + * @param robotToCamera The transformation form the robot to the camera + * @param timestamp The timestamp of the picture in seconds + */ + public DetectedObject(double xOffset, double yOffset, ObjectType type, Transform3d robotToCamera, double timestamp){ + this.type = type; + // Get the position relative to the camera + Translation3d translation = new Translation3d(1, new Rotation3d(0, -yOffset, -xOffset)) + // Rotate it to get the position relative to the rotated camera + .rotateBy(robotToCamera.getRotation()); + // Scale it so that the object will be on the ground (- because translation's z will be negative) + if(!isRobot()){ + translation = translation.times(-(robotToCamera.getZ()-type.height)/translation.getZ()); + }else{ + // Assume all robots are ~3m from the camera + translation = translation.times(3); + } + // Translate it to make it relative to the robot + translation = translation.plus(robotToCamera.getTranslation()); + // If the drivetrain exists, rotate and translate it to be field relative + if(drive != null){ + Pose2d drivePose = drive.getPoseAt(timestamp); + translation = translation.rotateBy(new Rotation3d( + 0, + 0, + drivePose.getRotation().getRadians() + )).plus(new Translation3d( + drivePose.getX(), + drivePose.getY(), + 0 + )); + } + pose = new Pose3d(translation, new Rotation3d()); + } + + /** + * Creates a new DetectedObject, assuming the object is on the ground + * @param xOffset The x offset from the camera to the object in radians + * @param yOffset The y offset form the camera to the object in radians + * @param type What type of object it is + * @param robotToCamera The transformation form the robot to the camera + * @param timestamp The timestamp of the picture in seconds + */ + public DetectedObject(double xOffset, double yOffset, int type, Transform3d robotToCamera, double timestamp){ + this(xOffset, yOffset, getType(type), robotToCamera, timestamp); + } + + /** + * Creates a new DetectedObject, assuming the object is on the ground + * @param xOffset The x offset from the camera to the object in radians + * @param yOffset The y offset form the camera to the object in radians + * @param type What type of object it is + * @param robotToCamera The transformation form the robot to the camera + */ + public DetectedObject(double xOffset, double yOffset, int type, Transform3d robotToCamera){ + this(xOffset, yOffset, getType(type), robotToCamera, -1); + } + + /** + * Creates a new DetectedObject, assuming the object is on the ground + * @param xOffset The x offset from the camera to the object in radians + * @param yOffset The y offset form the camera to the object in radians + * @param type What type of object it is + * @param robotToCamera The transformation form the robot to the camera + * @param timestamp The timestamp of the picture in seconds + */ + public DetectedObject(double xOffset, double yOffset, String type, Transform3d robotToCamera, double timestamp){ + this(xOffset, yOffset, getType(type), robotToCamera, timestamp); + } + + /** + * Creates a new DetectedObject, assuming the object is on the ground + * @param xOffset The x offset from the camera to the object in radians + * @param yOffset The y offset form the camera to the object in radians + * @param type What type of object it is + * @param robotToCamera The transformation form the robot to the camera + */ + public DetectedObject(double xOffset, double yOffset, String type, Transform3d robotToCamera){ + this(xOffset, yOffset, getType(type), robotToCamera, -1); + } + + /** + * Converts an int to an ObjectType + * @param type The type as an int, between 0 and the number of object types - 1 + * @return The type as an ObjectType + */ + public static ObjectType getType(int type){ + ObjectType[] values = ObjectType.values(); + if(type < 0 || type >= values.length){ + return ObjectType.NONE; + } + return values[type]; + } + + /** + * Converts a String to an ObjectType + * @param type The type as a String + * @return The type as an ObjectType + */ + public static ObjectType getType(String type){ + ObjectType result = ObjectType.valueOf(type.toUpperCase()); + return result==null ? ObjectType.NONE : result; + } + + /** + * Returns if the object is a game piece + * @return True if the object is a game piece, false otherwise + */ + public boolean isGamePiece(){ + return type==ObjectType.CORAL || type==ObjectType.ALGAE; + } + /** + * Returns if the object is a robot + * @return True if the object is a red or blue robot, false otherwise + */ + public boolean isRobot(){ + return type==ObjectType.RED_ROBOT || type==ObjectType.BLUE_ROBOT; + } + /** + * Returns if the object is a robot on the same alliance + * @return If the object is a robot on the same alliance + */ + public boolean isSameAllianceRobot(){ + return type == (Robot.getAlliance()==Alliance.Red?ObjectType.RED_ROBOT:ObjectType.BLUE_ROBOT); + } + /** + * Returns if the object is a robot on the other alliance + * @return If the object is a robot on the other alliance + */ + public boolean isOtherAllianceRobot(){ + return type == (Robot.getAlliance()==Alliance.Red?ObjectType.BLUE_ROBOT:ObjectType.RED_ROBOT); + } + + /** + * Gets the distance from the center of the robot to the object + * @return The distance in meters + */ + public double getDistance(){ + return drive.getPose().getTranslation().getDistance(pose.getTranslation().toTranslation2d()); + } + + /** + * Gets the field relative angle from the robot to the object + * @return The angle in radians + */ + public double getAngle(){ + Pose2d drivePose = drive.getPose(); + return Math.atan2(pose.getY()-drivePose.getY(), pose.getX()-drivePose.getX()); + } + + /** + * Gets the angle relative to the front of the robot (0 is in front, positive counterclockwise) + * @return The relative angle in radians + */ + public double getRelativeAngle(){ + double angle = getAngle()-drive.getYaw().getRadians(); + return MathUtil.angleModulus(angle); + } + + /** + * Gets the angle of the object relative to the robot's velocity (0 is in front, positive counterclockwise) + * @return The relative angle in radians + */ + public double getVelocityRelativeAngle(){ + ChassisSpeeds speeds = drive.getChassisSpeeds(); + double angle = getRelativeAngle() - Math.atan2(speeds.vyMetersPerSecond, speeds.vxMetersPerSecond); + return MathUtil.angleModulus(angle); + } + + public String toString(){ + return type+" at ("+pose.getX()+", "+pose.getY()+", "+pose.getZ()+")"; + } +} diff --git a/src/main/java/frc/robot/util/Vision/DriverAssist.java b/src/main/java/frc/robot/util/Vision/DriverAssist.java new file mode 100644 index 0000000..b2e9048 --- /dev/null +++ b/src/main/java/frc/robot/util/Vision/DriverAssist.java @@ -0,0 +1,185 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.util.Vision; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import frc.robot.constants.Constants; +import frc.robot.constants.VisionConstants; +import frc.robot.constants.swerve.DriveConstants; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.util.SwerveStuff.SwerveSetpoint; +import frc.robot.util.SwerveStuff.SwerveSetpointGenerator; + +/** + * A util class to assist the driver drive to a pose + */ +public class DriverAssist { + // The amount to correct the driver's inpus by + // 0 = return unchanged driver inputs, 1 = return a value much closer to the calculated speed, sometimes equal to it + // This can be greater than 1 to fully correct more of the time, like from farther away + private static final double CORRECTION_FACTOR = 1; + + + // Variables used for first method + // The setpoint generator, which limits the acceleration + private static final SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(); + private static final TrapezoidProfile xProfile = new TrapezoidProfile(new Constraints(DriveConstants.MAX_SPEED, DriveConstants.MAX_LINEAR_ACCEL)); + private static final TrapezoidProfile yProfile = new TrapezoidProfile(new Constraints(DriveConstants.MAX_SPEED, DriveConstants.MAX_LINEAR_ACCEL)); + private static final TrapezoidProfile angleProfile = new TrapezoidProfile(new Constraints(DriveConstants.MAX_ANGULAR_SPEED, DriveConstants.MAX_ANGULAR_ACCEL)); + + /** + * Combines the driver input with a speed calculated using a trapezoidal profile

    + * Called when VisionConstants.DRIVER_ASSIST_MODE is 2 + * @param drive The drivetrain + * @param driverInput The driver input speed + * @param desiredPose The pose to drive to + * @param keepAngle True to use the angle in the pose, false to point hte robot toward the pose + * @return The new speed + */ + private static ChassisSpeeds calculate2(Drivetrain drive, ChassisSpeeds driverInput, Pose2d desiredPose, boolean keepAngle) { + // Do nothing if there is no pose + if(desiredPose == null){ + return driverInput; + } + + // Store current states + Pose2d currentPose = drive.getPose(); + Rotation2d yaw = drive.getYaw(); + ChassisSpeeds driveSpeeds = drive.getChassisSpeeds(); + driveSpeeds = ChassisSpeeds. fromFieldRelativeSpeeds(driveSpeeds,yaw); // Changing this does not cause problems because getChassisSpeeds() creates a new object + State xState = new State(currentPose.getX(), driveSpeeds.vxMetersPerSecond); + State yState = new State(currentPose.getY(), driveSpeeds.vyMetersPerSecond); + State angleState = new State(currentPose.getRotation().getRadians(), driveSpeeds.omegaRadiansPerSecond); + + // Store goal states + State xGoal = new State(desiredPose.getX(), 0); + State yGoal = new State(desiredPose.getY(), 0); + Translation2d difference = desiredPose.getTranslation().minus(currentPose.getTranslation()); + double rotation = keepAngle ? desiredPose.getRotation().getRadians() : difference.getAngle().getRadians(); + if(rotation - currentPose.getRotation().getRadians() > Math.PI){ + rotation -= 2*Math.PI; + }else if(rotation - currentPose.getRotation().getRadians() < -Math.PI){ + rotation += 2*Math.PI; + } + State angleGoal = new State(rotation, 0); + + // Calculate ideal speeds for next frame + ChassisSpeeds goal = new ChassisSpeeds( + xProfile.calculate(Constants.LOOP_TIME, xState, xGoal).velocity, + yProfile.calculate(Constants.LOOP_TIME, yState, yGoal).velocity, + angleProfile.calculate(Constants.LOOP_TIME, angleState, angleGoal).velocity + ); + // Robot-relataive goal + ChassisSpeeds goalRobot = goal.times(1); + goalRobot = ChassisSpeeds. fromRobotRelativeSpeeds(goalRobot, yaw); + + // This calculates the actual acceleration we can get + // This is the only thing that needs to be robot relative + SwerveSetpoint nextSetpoint = setpointGenerator.generateSetpoint( + DriveConstants.MODULE_LIMITS, + 0, + drive.getCurrSetpoint(), goalRobot, + Constants.LOOP_TIME); + ChassisSpeeds nextChassisSpeed = nextSetpoint.chassisSpeeds(); + nextChassisSpeed = ChassisSpeeds.fromRobotRelativeSpeeds(nextChassisSpeed, yaw); + + // Robot relative driver inputs + ChassisSpeeds driverInputRobot = driverInput.times(1); // Copy so original doesn't change + driverInputRobot = ChassisSpeeds.fromFieldRelativeSpeeds(driverInputRobot, yaw); + // This is the speed the driver will be able to get next frame + // Both speeds need to be obtainable in 1 frame or the driver speed will always be farther away + SwerveSetpoint driverSetpoint = setpointGenerator.generateSetpoint( + DriveConstants.MODULE_LIMITS, + 0, + drive.getCurrSetpoint(), driverInputRobot, + Constants.LOOP_TIME); + ChassisSpeeds driverSpeeds = driverSetpoint.chassisSpeeds(); + driverSpeeds= ChassisSpeeds.fromRobotRelativeSpeeds(driverSpeeds,yaw); + + // The difference between the 2 speeds + ChassisSpeeds error = nextChassisSpeed.minus(driverSpeeds); + + // 1.2*1.2^-distance decreases the amount it correct by as distance increases + double distanceFactor = 1.2*Math.pow(1.2, -currentPose.getTranslation().getDistance(desiredPose.getTranslation())); + + // Driver input speed + double driverInputSpeed = Math.hypot(driverInput.vxMetersPerSecond, driverInput.vyMetersPerSecond); + + // The amount to correct by + ChassisSpeeds correction = error.times(Math.min(CORRECTION_FACTOR * distanceFactor * driverInputSpeed / DriveConstants.MAX_SPEED, 1)); + + return driverSpeeds.plus(correction); + // return nextChassisSpeed.times(CORRECTION_FACTOR).plus(driverInput.times(1-CORRECTION_FACTOR)); + } + + // Constants used for second method + public static final double MAX_VELOCITY_ANGLE_ERROR = Math.PI/4; + public static final double MAX_DISTANCE_ERROR = 2; + public static final double ROTATION_CORRECTION_FACTOR = 0.1; + public static final double MAX_ROTATION_ERROR = Math.PI/3; + + /** + * Combines the driver input with a calculated correction speed + * @param drive The drivetrain + * @param driverInput The driver input speed + * @param desiredPose The pose to drive to + * @param keepAngle True to use the angle in the pose, false to point hte robot toward the pose + * @return The new speed + */ + @SuppressWarnings("unused") // Needed because some code might not run for some values of DRIVER_ASSIST_MODE + public static ChassisSpeeds calculate(Drivetrain drive, ChassisSpeeds driverInput, Pose2d desiredPose, boolean keepAngle) { + if(VisionConstants.DRIVER_ASSIST_MODE < 2 || desiredPose == null){ + return driverInput; + }else if(VisionConstants.DRIVER_ASSIST_MODE == 2){ + return calculate2(drive, driverInput, desiredPose, keepAngle); + } + // Combines the driver input with a speed perpendicular to the input + Pose2d drivePose = drive.getPose(); + Translation2d difference = desiredPose.getTranslation().minus(drivePose.getTranslation()); + double distance = difference.getNorm(); + double velocityAngle = difference.getAngle().getRadians(); + double targetAngle = keepAngle ? desiredPose.getRotation().getRadians() : MathUtil.angleModulus(velocityAngle + Math.PI/2); + double inputSpeed = Math.hypot(driverInput.vxMetersPerSecond, driverInput.vyMetersPerSecond); + double driverAngle = Math.atan2(driverInput.vyMetersPerSecond, driverInput.vxMetersPerSecond); + double velocityAngleError = MathUtil.angleModulus(velocityAngle - driverAngle); + if(Math.abs(velocityAngleError) > MAX_VELOCITY_ANGLE_ERROR){ + return driverInput; + } + double perpendicularDist = Math.abs(distance * Math.sin(velocityAngleError)); + if(perpendicularDist > MAX_DISTANCE_ERROR){ + return driverInput; + } + double perpendicularAngle = MathUtil.angleModulus(driverAngle + Math.PI/2*Math.signum(velocityAngleError)); + // Different options for calculation. + double correctionSpeed = 0; + switch(VisionConstants.DRIVER_ASSIST_MODE){ + case 3: + correctionSpeed = Math.min(CORRECTION_FACTOR * inputSpeed * Math.pow(2, -perpendicularDist), Math.abs(Math.tan(velocityAngleError)*inputSpeed)); + break; + case 4: + correctionSpeed = Math.min(CORRECTION_FACTOR * Math.pow(1.5, -perpendicularDist), 1) * Math.abs(Math.tan(velocityAngleError)*inputSpeed); + break; + case 5: + correctionSpeed = Math.min(CORRECTION_FACTOR * inputSpeed * Math.pow(2, -perpendicularDist) * Math.pow(1.2, -distance+1), Math.abs(Math.tan(velocityAngleError)*inputSpeed)); + break; + } + double rotationError = MathUtil.angleModulus(targetAngle-drivePose.getRotation().getRadians()); + if(Math.abs(rotationError) > MAX_ROTATION_ERROR){ + return driverInput; + } + // We want to set the current angular velocity so that we can decelerate to 0rad/s at the setpoint + // Since 0=v0^2+2ax, v0=√(2ax) + // High correction factors will also ignore the driver's input more + double rotationalSpeed = ROTATION_CORRECTION_FACTOR * Math.signum(rotationError)*Math.sqrt(2*DriveConstants.MAX_ANGULAR_ACCEL*Math.abs(rotationError)) - ROTATION_CORRECTION_FACTOR * driverInput.omegaRadiansPerSecond; + return driverInput.plus(new ChassisSpeeds(correctionSpeed*Math.cos(perpendicularAngle), correctionSpeed*Math.sin(perpendicularAngle), rotationalSpeed)); + } +} diff --git a/src/main/java/frc/robot/util/Vision/Vision.java b/src/main/java/frc/robot/util/Vision/Vision.java new file mode 100644 index 0000000..c55b1db --- /dev/null +++ b/src/main/java/frc/robot/util/Vision/Vision.java @@ -0,0 +1,677 @@ +package frc.robot.util.Vision; + +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import java.util.function.DoubleUnaryOperator; + +import org.littletonrobotics.junction.Logger; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.constants.FieldConstants; +import frc.robot.constants.VisionConstants; +import frc.robot.constants.swerve.DriveConstants; +import frc.robot.util.MathUtils; + +// Vision and it's commands are adapted from Iron Claw's FRC2023 +public class Vision { + private NetworkTable m_objectDetectionTable; + + private NetworkTableEntry xOffset; + private NetworkTableEntry yOffset; + private NetworkTableEntry objectDistance; + private NetworkTableEntry objectClass; + private NetworkTableEntry cameraIndex; + + // The field layout. Instance variable + private AprilTagFieldLayout aprilTagFieldLayout; + // A list of the cameras on the robot. + private ArrayList cameras = new ArrayList<>(); + + private VisionSystemSim visionSim; + + private boolean sawTag = false; + + // Array of tags to use, null or empty array to use all tags + private int[] onlyUse = null; + + /** + * Creates a new instance of Vision and sets up the cameras and field layout + */ + public Vision(ArrayList> camList) { + // Initialize object_detection NetworkTable + m_objectDetectionTable = NetworkTableInstance.getDefault().getTable("object_detection"); + + // From the object detection NetworkTable, get the entries + objectDistance = m_objectDetectionTable.getEntry("distance"); + xOffset = m_objectDetectionTable.getEntry("x_offset"); + yOffset = m_objectDetectionTable.getEntry("y_offset"); + objectClass = m_objectDetectionTable.getEntry("class"); + cameraIndex = m_objectDetectionTable.getEntry("index"); + + // Start NetworkTables server + NetworkTableInstance.getDefault().startServer(); + + // Load field layout + aprilTagFieldLayout = new AprilTagFieldLayout(FieldConstants.APRIL_TAGS, FieldConstants.FIELD_LENGTH, FieldConstants.FIELD_WIDTH); + + // Sets the origin to the right side of the blue alliance wall + aprilTagFieldLayout.setOrigin(OriginPosition.kBlueAllianceWallRightSide); + + if(VisionConstants.ENABLED){ + // Puts the cameras in an array list + for (int i = 0; i < camList.size(); i++) { + cameras.add(new VisionCamera(camList.get(i).getFirst(), camList.get(i).getSecond())); + } + + if(RobotBase.isSimulation()){ + visionSim = new VisionSystemSim("Vision"); + visionSim.addAprilTags(aprilTagFieldLayout); + for(VisionCamera c : cameras){ + PhotonCameraSim cameraSim = new PhotonCameraSim(c.camera); + cameraSim.enableDrawWireframe(true); + cameraSim.prop.setAvgLatencyMs(30); + cameraSim.prop.setCalibration(1280, 720, Rotation2d.fromDegrees(78)); + visionSim.addCamera(cameraSim, c.photonPoseEstimator.getRobotToCameraTransform()); + } + } + } + } + + + /** + * Get the horizontal offsets from the crosshair to the targets + * @return An array of offsets in degrees + */ + public double[] getHorizontalOffset(){ + if(!VisionConstants.OBJECT_DETECTION_ENABLED){ + return new double[0]; + } + return xOffset.getDoubleArray(new double[0]); + } + + /** + * Get the vertical offsets from the crosshair to the targets + * @return An array of offsets in degrees + */ + public double[] getVerticalOffset(){ + if(!VisionConstants.OBJECT_DETECTION_ENABLED){ + return new double[0]; + } + return yOffset.getDoubleArray(new double[0]); + } + + /** + * Get the target distances + * @return Distance in meters + */ + @SuppressWarnings("unused") + public double[] getDistance(){ + if(!VisionConstants.OBJECT_DETECTION_ENABLED || true){ + return new double[0]; + } + return objectDistance.getDoubleArray(new double[0]); + } + + /** + * Returns whether or not a valid object is detected + * @return true or false + */ + public boolean validObjectDetected(){ + return getHorizontalOffset().length > 0; + } + + /** + * Returns what types of object are detected + * @return The object types as a String array + */ + @SuppressWarnings("unused") + public String[] getDetectedObjectClass(){ + if(!VisionConstants.OBJECT_DETECTION_ENABLED || true){ + return new String[0]; + } + return objectClass.getStringArray(new String[0]); + } + + /** + * Gets the camera indices (which camera sees the object) + * @return The indices as a long array (method returns long array instead of int array) + */ + @SuppressWarnings("unused") + public long[] getCameraIndex(){ + if(!VisionConstants.OBJECT_DETECTION_ENABLED || true){ + return new long[0]; + } + return cameraIndex.getIntegerArray(new long[0]); + } + + /** + * Stores all of the detected objects in an array + * @return The array of DetectedObjects + */ + public DetectedObject[] getDetectedObjects(){ + if(!VisionConstants.OBJECT_DETECTION_ENABLED){ + return new DetectedObject[0]; + } + double[] xOffset = getHorizontalOffset(); + double[] yOffset = getVerticalOffset(); + // double[] distance = getDistance(); + String[] objectClass = getDetectedObjectClass(); + // long[] cameraIndex = getCameraIndex(); + DetectedObject[] objects = new DetectedObject[Math.min(xOffset.length, yOffset.length)]; + for(int i = 0; i < objects.length; i++){ + objects[i] = new DetectedObject( + Units.degreesToRadians(xOffset[i]), + -Units.degreesToRadians(yOffset[i]), + // distance[i], + objectClass[i], + // VisionConstants.OBJECT_DETECTION_CAMERAS.get((int)cameraIndex[i]).getSecond() + VisionConstants.OBJECT_DETECTION_CAMERAS.get(0) + ); + } + return objects; + } + + /** + * Returns the closest game piece in front of the robot + * @param maxAngle The maximum angle between the angle to the object and the robot's heading or rotation to use, in radians + * @param relativeToVelocity Whether to compare the angle to the robot's heading or rotation, true for heading + * @return The best DetectedObject + */ + public DetectedObject getBestGamePiece(double maxAngle, boolean relativeToVelocity){ + DetectedObject[] objects = getDetectedObjects(); + DetectedObject best = null; + double closest = Double.POSITIVE_INFINITY; + for(DetectedObject object : objects){ + double dist = object.getDistance(); + if(object.isGamePiece() && Math.abs(relativeToVelocity ? object.getVelocityRelativeAngle() : object.getAngle()) < maxAngle && dist < closest){ + closest = dist; + best = object; + } + } + return best; + } + + /** + * Gets the pose as a Pose2d using PhotonVision + * @param referencePoses The reference poses in order of preference, null poses will be skipped + * @return The pose of the robot, or null if it can't see april tags + */ + public Pose2d getPose2d(Pose2d... referencePoses){ + Pose2d referencePose = new Pose2d(); + for (Pose2d checkReferencePose:referencePoses){ + if (checkReferencePose != null) { + referencePose = checkReferencePose; + break; + } + } + ArrayList estimatedPoses = getEstimatedPoses(referencePose); + + if (estimatedPoses.size() == 0) return null; + + if (estimatedPoses.size() == 1) return estimatedPoses.get(0).estimatedPose.toPose2d(); + + if (estimatedPoses.size() == 2) { + return new Pose2d( + estimatedPoses.get(0).estimatedPose.toPose2d().getTranslation() + .plus(estimatedPoses.get(1).estimatedPose.toPose2d().getTranslation()) + .div(2), + + new Rotation2d(MathUtils.modulusMidpoint( + estimatedPoses.get(0).estimatedPose.toPose2d().getRotation().getRadians(), + estimatedPoses.get(1).estimatedPose.toPose2d().getRotation().getRadians(), + -Math.PI, Math.PI) + ) + ); + } + + // The average translation is just the average of all of the translations (sum divided by total) + // Average angle is similar, except every step needs to use a modulus, since -π is the same angle as π + // This calculation is essentially newAverage = (oldAverage * valuesInOldAverage + nextValue) / newNumberOfValues + Translation2d translation = new Translation2d(); + double angle = 0; + for(int i = 0; i < estimatedPoses.size(); i ++){ + translation = translation.plus(estimatedPoses.get(i).estimatedPose.toPose2d().getTranslation()); + angle = MathUtils.modulusInterpolate(angle, estimatedPoses.get(i).estimatedPose.toPose2d().getRotation().getRadians(), 1.0/(i+1), -Math.PI, Math.PI); + } + + return new Pose2d(translation.div(estimatedPoses.size()), new Rotation2d(angle)); + } + + public AprilTagFieldLayout getAprilTagFieldLayout(){ + return aprilTagFieldLayout; + } + + /** + * Gets the pose of an april tag + * @param id AprilTag id (1-8) + * @return Pose3d of the AprilTag + */ + public Pose3d getTagPose(int id){ + if(id < 1 || id > getAprilTagFieldLayout().getTags().size()){ + System.out.println("Tried to find the pose of april tag "+id); + return null; + } + return getAprilTagFieldLayout().getTags().get(id-1).pose; + } + + /** + * Returns where it thinks the robot is + * @param referencePose The pose to use as a reference, usually the previous robot pose + * @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time + * @return An array list of estimated poses, one for each camera that can see an april tag + */ + public ArrayList getEstimatedPoses(Pose2d referencePose) { + return getEstimatedPoses(referencePose, ignoree->referencePose.getRotation().getRadians()); + } + + /** + * Returns where it thinks the robot is + * @param referencePose The pose to use as a reference, usually the previous robot pose + * @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time + * @return An array list of estimated poses, one for each camera that can see an april tag + */ + public ArrayList getEstimatedPoses(Pose2d referencePose, DoubleUnaryOperator yawFunction) { + ArrayList estimatedPoses = new ArrayList<>(); + for (int i = 0; i < cameras.size(); i++) { + if(VisionConstants.USE_MANUAL_CALCULATIONS){ + for(EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(yawFunction)){ + if(pose != null){ + estimatedPoses.add(pose); + } + } + }else{ + for(EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(referencePose)){ + // If the camera can see an april tag that exists, add it to the array list + // April tags that don't exist might return a result that is present but doesn't have a pose + if (pose.estimatedPose != null) { + estimatedPoses.add(pose); + + } + } + } + } + if(estimatedPoses.size() > 1){ + Translation2d average = new Translation2d(); + for(EstimatedRobotPose pose : estimatedPoses){ + average = average.plus(pose.estimatedPose.getTranslation().toTranslation2d()); + } + average = average.div(estimatedPoses.size()); + for(int i = estimatedPoses.size()-1; i>=0; i--){ + if(estimatedPoses.get(i).estimatedPose.getTranslation().toTranslation2d().getDistance(average) > VisionConstants.MAX_POSE_DIFFERENCE/2){ + estimatedPoses.remove(i); + } + } + } + return estimatedPoses; + } + + /** + * Updates the robot's odometry with vision + * @param poseEstimator The pose estimator to update + * @param yawFunction A function that returns the yaw as a double given the timestamp + * @param slipped True if the wheels have slipped, false otherwise + */ + public void updateOdometry(SwerveDrivePoseEstimator poseEstimator, DoubleUnaryOperator yawFunction, boolean slipped){ + // Simulate vision + // 2 ifs to avoid warning + if(VisionConstants.ENABLED_SIM){ + if(RobotBase.isSimulation()){ + visionSim.update(poseEstimator.getEstimatedPosition()); + } + } + + sawTag = false; + + // An array list of poses returned by different cameras + ArrayList estimatedPoses = getEstimatedPoses(poseEstimator.getEstimatedPosition(), yawFunction); + for (EstimatedRobotPose estimatedPose : estimatedPoses) { + // Continue if this pose doesn't exist + if(estimatedPose.timestampSeconds < 0 || !onField(estimatedPose.estimatedPose.toPose2d()) || Timer.getFPGATimestamp() < estimatedPose.timestampSeconds || Timer.getFPGATimestamp() > estimatedPose.timestampSeconds + 1){ + continue; + } + + poseEstimator.addVisionMeasurement( + estimatedPose.estimatedPose.toPose2d(), + estimatedPose.timestampSeconds, + slipped ? VisionConstants.VISION_STD_DEVS_2 : VisionConstants.VISION_STD_DEVS + ); + sawTag = true; + } + } + + /** + * Updates each camera's inputs for logging + */ + public void updateInputs(){ + for(VisionCamera c : cameras){ + c.updateInputs(); + } + } + + /** + * If vision saw any April tags last frame + * @return If vision saw an April tag last frame + */ + public boolean canSeeTag(){ + return sawTag; + } + + /** + * Enable or disable a single camera + * @param index The camera index + * @param enabled If it should be enabled or disabled + */ + public void enableCamera(int index, boolean enabled){ + try{ + cameras.get(index).enable(enabled); + }catch(IndexOutOfBoundsException e){ + DriverStation.reportWarning("Camera index "+index+" is out of bounds", false); + } + } + /** + * Sets the cameras to only use April tag in the specified array + * @param ids The ids of the tags to use, null or empty array to use all + */ + public void onlyUse(int[] ids){ + onlyUse = ids; + } + + /** + * Checks if a pose is on the field + * @param pose The pose to check + * @return If the pose is on the field + */ + public static boolean onField(Pose2d pose){ + return pose!=null && pose.getX()>0 && pose.getX()0 && pose.getY()-FieldConstants.FIELD_LENGTH/2 && pose.getX()-FieldConstants.FIELD_WIDTH/2 && pose.getY() getEstimatedPose(Pose2d referencePose) { + photonPoseEstimator.setReferencePose(referencePose); + + ArrayList list = new ArrayList<>(); + + if(!enabled){ + return list; + } + + for(PhotonPipelineResult cameraResult : inputs.results){ + if(!cameraResult.hasTargets() || cameraResult.getTimestampSeconds()<0){ + continue; + } + + // if there is a target detected and the timestamp exists, + // check the ambiguity isn't too high + List targetsUsed = cameraResult.targets; + for (int i = targetsUsed.size()-1; i >= 0; i--) { + // Remove it from the list if it should not be used or if it has too high of an ambiguity + if(!useTag(targetsUsed.get(i).getFiducialId()) || targetsUsed.get(i).getPoseAmbiguity() > VisionConstants.HIGHEST_AMBIGUITY || targetsUsed.get(i).bestCameraToTarget.getTranslation().getNorm() > VisionConstants.MAX_DISTANCE){ + targetsUsed.remove(i); + } + } + + // If there are no targets, the timestamp doesn't exist, or there there is only 1 tag and the constant is set to only use 2 tags, continue + if(targetsUsed.size() == 0 || cameraResult.getTimestampSeconds()<0 || targetsUsed.size()==1 && VisionConstants.ONLY_USE_2_TAGS){ + continue; + } + + // Set strategy to single tag if there is only 1 good tag and update + photonPoseEstimator.setPrimaryStrategy(targetsUsed.size() > 1 ? VisionConstants.POSE_STRATEGY : VisionConstants.MULTITAG_FALLBACK_STRATEGY); + Optional pose = photonPoseEstimator.update(cameraResult); + + if(pose.isPresent() && pose.get()!=null && onField(pose.get().estimatedPose.toPose2d())){ + double timestamp = cameraResult.getTimestampSeconds(); + + // If the pose moved too much, don't use it + if(lastPose==null || lastPose.getTranslation().getDistance(pose.get().estimatedPose.toPose2d().getTranslation()) > DriveConstants.MAX_SPEED*1.25*(timestamp-lastTimestamp) || timestamp < lastTimestamp){ + lastPose = pose.get().estimatedPose.toPose2d(); + lastTimestamp = timestamp; + continue; + } + + // Otherwise, add the pose to the list + lastPose = pose.get().estimatedPose.toPose2d(); + lastTimestamp = timestamp; + list.add(pose.get()); + } + } + return list; + } + + /** + * Updates the VisionIOInputs object with the results from PhotonVision for logging + */ + @Override + public void updateInputs() { + inputs.connected = camera.isConnected(); + inputs.results = camera.getAllUnreadResults(); + + Logger.processInputs("Vision/"+camera.getName(), inputs); + + // Mechanical Advantage's vision logging + // // Read new camera observations + // Set tagIds = new HashSet<>(); + // List poseObservations = new LinkedList<>(); + // for (var result : camera.getAllUnreadResults()) { + // // Update latest target observation + // if (result.hasTargets()) { + // inputs.latestTargetObservation = + // new TargetObservation( + // Rotation2d.fromDegrees(result.getBestTarget().getYaw()), + // Rotation2d.fromDegrees(result.getBestTarget().getPitch())); + // } else { + // inputs.latestTargetObservation = new TargetObservation(new Rotation2d(), new Rotation2d()); + // } + // } + // // Add pose observation + // if (result.multitagResult.isPresent()) { // Multitag result + // var multitagResult = result.multitagResult.get(); + + // // Calculate robot pose + // Transform3d fieldToCamera = multitagResult.estimatedPose.best; + // Transform3d fieldToRobot = fieldToCamera.plus(VisionConstants.APRIL_TAG_CAMERAS.get(0).getSecond().inverse()); + // Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + + // // Calculate average tag distance + // double totalTagDistance = 0.0; + // for (var target : result.targets) { + // totalTagDistance += target.bestCameraToTarget.getTranslation().getNorm(); + // } + + // // Add tag IDs + // tagIds.addAll(multitagResult.fiducialIDsUsed); + + // // Add observation + // poseObservations.add( + // new PoseObservation( + // result.getTimestampSeconds(), // Timestamp + // robotPose, // 3D pose estimate + // multitagResult.estimatedPose.ambiguity, // Ambiguity + // multitagResult.fiducialIDsUsed.size(), // Tag count + // totalTagDistance / result.targets.size(), // Average tag distance + // PoseObservationType.PHOTONVISION)); // Observation type + + // } else if (!result.targets.isEmpty()) { // Single tag result + // var target = result.targets.get(0); + + // // Calculate robot pose + // var tagPose = aprilTagLayout.getTagPose(target.fiducialId); + // if (tagPose.isPresent()) { + // Transform3d fieldToTarget = + // new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation()); + // Transform3d cameraToTarget = target.bestCameraToTarget; + // Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse()); + // Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); + // Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + + // // Add tag ID + // tagIds.add((short) target.fiducialId); + + // // Add observation + // poseObservations.add( + // new PoseObservation( + // result.getTimestampSeconds(), // Timestamp + // robotPose, // 3D pose estimate + // target.poseAmbiguity, // Ambiguity + // 1, // Tag count + // cameraToTarget.getTranslation().getNorm(), // Average tag distance + // PoseObservationType.PHOTONVISION)); // Observation type + // } + // } + // } + } + + /** + * Gets the pose using manual calculations + * @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time + * @return A list of estimated poses as EstimatedRobotPoses + */ + public ArrayList getEstimatedPose(DoubleUnaryOperator yawFunction){ + ArrayList list = new ArrayList<>(); + + // Do nothing if this camera is disabled + if(!enabled){ + return list; + } + + // The latest camera results + for(PhotonPipelineResult result : inputs.results){ + // TODO: This could be improved by averaging all targets instead of only using 1 + // Gets the best target to use for the calculations + PhotonTrackedTarget target = result.getBestTarget(); + // Continue if the target doesn't exist or it should be ignored + if(target==null){ + continue; + } + // Continue if the id is too high or too low + int id = target.getFiducialId(); + if(!useTag(id) || target.bestCameraToTarget.getTranslation().getNorm() > VisionConstants.MAX_DISTANCE || target.poseAmbiguity > VisionConstants.HIGHEST_AMBIGUITY){ + continue; + } + // Stores target pose and robot to camera transformation for easy access later + Pose3d targetPose = FieldConstants.APRIL_TAGS.get(id-1).pose; + Transform3d robotToCamera = photonPoseEstimator.getRobotToCameraTransform(); + + double timestamp = result.getTimestampSeconds(); + double yaw = yawFunction.applyAsDouble(timestamp); + + // Get the tag position relative to the robot, assuming the robot is on the ground + Translation3d translation = target.getBestCameraToTarget().getTranslation() + .rotateBy(robotToCamera.getRotation()); + translation = translation//.times((targetPose.getZ()-robotToCamera.getZ())/translation.getZ()) + .plus(robotToCamera.getTranslation()) + .rotateBy(new Rotation3d(0, 0, yaw)) + + // Invert it to get the robot position relative to the April tag + // Multiply by a constant. I don't know why this works, but it was consistently 10% off in 2023 Fall Semester + .times(-VisionConstants.DISTANCE_SCALE) + // Get the field relative robot pose + .plus(targetPose.getTranslation()); + try{ + // Adds an EstimatedRobotPose + list.add(new EstimatedRobotPose( + new Pose3d(translation.getX(), translation.getY(), 0, new Rotation3d(0, 0, yaw)), + timestamp, + List.of(target), + VisionConstants.POSE_STRATEGY + )); + }catch(Exception e){ + DriverStation.reportError("Error creating EstimatedRobotPose", true); + } + } + return list; + } + + public boolean useTag(int id){ + // Never use tags that don't exist + if(id <= 0 || id > FieldConstants.APRIL_TAGS.size()){ + return false; + } + // Return false if it is in the list of tags to ignore + for(int id2 : VisionConstants.TAGS_TO_IGNORE){ + if(id == id2){ + return false; + } + } + // If it's in the array to only use and not in the array to ignore, return true + for(int j = 0; onlyUse != null && j < onlyUse.length; j++){ + if(id == onlyUse[j]){ + return true; + } + } + // If it isn't in the array to only use, only reutrn true if the array is empty/null + return onlyUse == null || onlyUse.length == 0; + } + + /** + * Enables or disables this camera + * @param enable If it should be enabled or disabled + */ + public void enable(boolean enable){ + enabled = enable; + } + } +} diff --git a/src/main/java/frc/robot/util/Vision/VisionIO.java b/src/main/java/frc/robot/util/Vision/VisionIO.java new file mode 100644 index 0000000..02bb492 --- /dev/null +++ b/src/main/java/frc/robot/util/Vision/VisionIO.java @@ -0,0 +1,82 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.util.Vision; + +import java.util.ArrayList; +import java.util.List; + +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.inputs.LoggableInputs; +import org.photonvision.targeting.PhotonPipelineResult; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; + +public interface VisionIO { + public static class VisionIOInputs implements LoggableInputs { + public boolean connected = false; + public List results = new ArrayList<>(); + + // PhotonVision should never return more than 5 results, except possibly for very long loop overruns + private static final int maxLength = 5; + + private boolean intitalized = false; + + @Override + public void toLog(LogTable table) { + // LogTable does not easily allow removal of logs, especially ProtobufSerializables, so extra values will need to be ignored + // This is not very efficient, since unused values are still taking up memory, but there is no easy way to remove them + if (!intitalized){ + for(int i = 0; i < maxLength; i++){ + table.put("Results"+i, new PhotonPipelineResult()); + intitalized = true; + } + } + table.put("Connected", connected); + double length = Math.min(results.size(), maxLength); + table.put("Length", length); + for(int i = 0; i < length; i++){ + table.put("Results"+i, results.get(i)); + } + } + + @Override + public void fromLog(LogTable table) { + connected = table.get("Connected", false); + int length = table.get("Length", 0); + results = new ArrayList<>(length); + // Java gets confused when null is used for a generic type argument + PhotonPipelineResult nullResult = null; + for(int i = 0; i < length; i++){ + PhotonPipelineResult result = table.get("Results"+i, nullResult); + if(result != null){ + results.add(result); + } + } + } + } + + /** Represents the angle to a simple target, not used for pose estimation. */ + public static record TargetObservation(Rotation2d tx, Rotation2d ty) {} + + /** Represents a robot pose sample used for pose estimation. */ + public static record PoseObservation( + double timestamp, + Pose3d pose, + double ambiguity, + int tagCount, + double averageTagDistance) {} + + public default void updateInputs() {} +} \ No newline at end of file diff --git a/src/main/java/lib/COTSFalconSwerveConstants.java b/src/main/java/lib/COTSFalconSwerveConstants.java new file mode 100644 index 0000000..d4a9009 --- /dev/null +++ b/src/main/java/lib/COTSFalconSwerveConstants.java @@ -0,0 +1,139 @@ +package lib; + +import edu.wpi.first.math.util.Units; + +/* Contains values and required settings for common COTS swerve modules. */ +public class COTSFalconSwerveConstants { + public final double wheelDiameter; + public final double wheelCircumference; + public final double angleGearRatio; + public final double driveGearRatio; + public final double angleKP; + public final double angleKI; + public final double angleKD; + public final double angleKF; + public final boolean driveMotorInvert; + public final boolean angleMotorInvert; + public final boolean canCoderInvert; + + public COTSFalconSwerveConstants(double wheelDiameter, double angleGearRatio, double driveGearRatio, double angleKP, double angleKI, double angleKD, double angleKF, boolean driveMotorInvert, boolean angleMotorInvert, boolean canCoderInvert) { + this.wheelDiameter = wheelDiameter; + this.wheelCircumference = wheelDiameter * Math.PI; + this.angleGearRatio = angleGearRatio; + this.driveGearRatio = driveGearRatio; + this.angleKP = angleKP; + this.angleKI = angleKI; + this.angleKD = angleKD; + this.angleKF = angleKF; + this.driveMotorInvert = driveMotorInvert; + this.angleMotorInvert = angleMotorInvert; + this.canCoderInvert = canCoderInvert; + } + + /** + * Swerve Drive Specialties - MK3 Module + */ + public static COTSFalconSwerveConstants SDSMK3(double driveGearRatio) { + double wheelDiameter = Units.inchesToMeters(4.0); + + /** 12.8 : 1 */ + double angleGearRatio = (12.8); + + double angleKP = 0.2; + double angleKI = 0.0; + double angleKD = 0.0; + double angleKF = 0.0; + + boolean driveMotorInvert = false; + boolean angleMotorInvert = false; + boolean canCoderInvert = false; + return new COTSFalconSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, angleKF, driveMotorInvert, angleMotorInvert, canCoderInvert); + } + + /** + * Swerve Drive Specialties - MK4 Module + */ + public static COTSFalconSwerveConstants SDSMK4(double driveGearRatio) { + double wheelDiameter = Units.inchesToMeters(4.0); + + /** 12.8 : 1 */ + double angleGearRatio = (12.8); + + double angleKP = 0.2; + double angleKI = 0.0; + double angleKD = 0.0; + double angleKF = 0.0; + + boolean driveMotorInvert = false; + boolean angleMotorInvert = false; + boolean canCoderInvert = false; + return new COTSFalconSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, angleKF, driveMotorInvert, angleMotorInvert, canCoderInvert); + } + + /** + * Swerve Drive Specialties - MK4i Module + */ + public static COTSFalconSwerveConstants SDSMK4i(double driveGearRatio) { + double wheelDiameter = Units.inchesToMeters(4.0); + + /** (150 / 7) : 1 */ + double angleGearRatio = ((150.0 / 7.0)); + + double angleKP = 0.3; + double angleKI = 0.0; + double angleKD = 0.0; + double angleKF = 0.0; + + boolean driveMotorInvert = false; + boolean angleMotorInvert = true; + boolean canCoderInvert = false; + return new COTSFalconSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, angleKF, driveMotorInvert, angleMotorInvert, canCoderInvert); + } + + /* Drive Gear Ratios for all supported modules */ + public static class DriveGearRatios { + /* SDS MK3 */ + /** + * SDS MK3 - 8.16 : 1 + */ + public static final double SDSMK3_Standard = (8.16); + /** + * SDS MK3 - 6.86 : 1 + */ + public static final double SDSMK3_Fast = (6.86); + + /* SDS MK4 */ + /** + * SDS MK4 - 8.14 : 1 + */ + public static final double SDSMK4_L1 = (8.14); + /** + * SDS MK4 - 6.75 : 1 + */ + public static final double SDSMK4_L2 = (6.75); + /** + * SDS MK4 - 6.12 : 1 + */ + public static final double SDSMK4_L3 = (6.12); + /** + * SDS MK4 - 5.14 : 1 + */ + public static final double SDSMK4_L4 = (5.14); + + /* SDS MK4i */ + /** + * SDS MK4i - 8.14 : 1 + */ + public static final double SDSMK4i_L1 = (8.14); + /** + * SDS MK4i - 6.75 : 1 + */ + public static final double SDSMK4i_L2 = (6.75); + /** + * SDS MK4i - 6.12 : 1 + */ + public static final double SDSMK4i_L3 = (6.12); + } +} + + \ No newline at end of file diff --git a/src/main/java/lib/CTREModuleState.java b/src/main/java/lib/CTREModuleState.java new file mode 100644 index 0000000..8e8005d --- /dev/null +++ b/src/main/java/lib/CTREModuleState.java @@ -0,0 +1,60 @@ +package lib; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; + +public class CTREModuleState { + + /** + * Minimize the change in heading the desired swerve module state would require by potentially + * reversing the direction the wheel spins. Customized from WPILib's version to include placing + * in appropriate scope for CTRE onboard control. + * + * @param desiredState The desired state. + * @param currentAngle The current module angle. + */ + public static SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle) { + double targetAngle = placeInAppropriate0To360Scope(currentAngle.getDegrees(), desiredState.angle.getDegrees()); + double targetSpeed = desiredState.speedMetersPerSecond; + double delta = targetAngle - currentAngle.getDegrees(); + if (Math.abs(delta) > 90) { + targetSpeed = -targetSpeed; + if (delta > 90) { + targetAngle -= 180; + } else { + targetAngle += 180; + } + } + return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle)); + } + + /** + * @param scopeReference Current Angle + * @param newAngle Target Angle + * @return Closest angle within scope + */ + private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) { + double lowerBound; + double upperBound; + double lowerOffset = scopeReference % 360; + if (lowerOffset >= 0) { + lowerBound = scopeReference - lowerOffset; + upperBound = scopeReference + (360 - lowerOffset); + } else { + upperBound = scopeReference - lowerOffset; + lowerBound = scopeReference - (360 + lowerOffset); + } + while (newAngle < lowerBound) { + newAngle += 360; + } + while (newAngle > upperBound) { + newAngle -= 360; + } + if (newAngle - scopeReference > 180) { + newAngle -= 360; + } else if (newAngle - scopeReference < -180) { + newAngle += 360; + } + return newAngle; + } +} diff --git a/src/main/java/lib/PolynomialRegression.java b/src/main/java/lib/PolynomialRegression.java new file mode 100644 index 0000000..04d4f65 --- /dev/null +++ b/src/main/java/lib/PolynomialRegression.java @@ -0,0 +1,195 @@ +package lib; + +import Jama.Matrix; +import Jama.QRDecomposition; + + +// NOTE: This file is available at +// http://algs4.cs.princeton.edu/14analysis/PolynomialRegression.java.html + +/** + * The {@code PolynomialRegression} class performs a polynomial regression on an set of N + * data points (yi, xi). That is, it fits a polynomial + * y = β0 + β1 x + β2 + * x2 + ... + βd xd (where + * y is the response variable, x is the predictor variable, and the + * βi are the regression coefficients) that minimizes the sum of squared + * residuals of the multiple regression model. It also computes associated the coefficient of + * determination R2. + * + *

    This implementation performs a QR-decomposition of the underlying Vandermonde matrix, so it is + * neither the fastest nor the most numerically stable way to perform the polynomial regression. + * + * @author Robert Sedgewick + * @author Kevin Wayne + */ +public class PolynomialRegression implements Comparable { + private final String variableName; // name of the predictor variable + private int degree; // degree of the polynomial regression + private final Matrix beta; // the polynomial regression coefficients + private final double sse; // sum of squares due to error + private double sst; // total sum of squares + + /** + * Performs a polynomial regression on the data points {@code (y[i], x[i])}. Uses n as the name + * of the predictor variable. + * + * @param x the values of the predictor variable + * @param y the corresponding values of the response variable + * @param degree the degree of the polynomial to fit + * @throws IllegalArgumentException if the lengths of the two arrays are not equal + */ + public PolynomialRegression(double[] x, double[] y, int degree) { + this(x, y, degree, "n"); + } + + /** + * Performs a polynomial regression on the data points {@code (y[i], x[i])}. + * + * @param x the values of the predictor variable + * @param y the corresponding values of the response variable + * @param degree the degree of the polynomial to fit + * @param variableName the name of the predictor variable + * @throws IllegalArgumentException if the lengths of the two arrays are not equal + */ + public PolynomialRegression(double[] x, double[] y, int degree, String variableName) { + this.degree = degree; + this.variableName = variableName; + + int n = x.length; + QRDecomposition qr = null; + Matrix matrixX = null; + + // in case Vandermonde matrix does not have full rank, reduce degree until it + // does + while (true) { + + // build Vandermonde matrix + double[][] vandermonde = new double[n][this.degree + 1]; + for (int i = 0; i < n; i++) { + for (int j = 0; j <= this.degree; j++) { + vandermonde[i][j] = Math.pow(x[i], j); + } + } + matrixX = new Matrix(vandermonde); + + // find least squares solution + qr = new QRDecomposition(matrixX); + if (qr.isFullRank()) break; + + // decrease degree and try again + this.degree--; + } + + // create matrix from vector + Matrix matrixY = new Matrix(y, n); + + // linear regression coefficients + beta = qr.solve(matrixY); + + // mean of y[] values + double sum = 0.0; + for (int i = 0; i < n; i++) sum += y[i]; + double mean = sum / n; + + // total variation to be accounted for + for (int i = 0; i < n; i++) { + double dev = y[i] - mean; + sst += dev * dev; + } + + // variation not accounted for + Matrix residuals = matrixX.times(beta).minus(matrixY); + sse = residuals.norm2() * residuals.norm2(); + } + + /** + * Returns the {@code j}th regression coefficient. + * + * @param j the index + * @return the {@code j}th regression coefficient + */ + public double beta(int j) { + // to make -0.0 print as 0.0 + if (Math.abs(beta.get(j, 0)) < 1E-4) return 0.0; + return beta.get(j, 0); + } + + /** + * Returns the degree of the polynomial to fit. + * + * @return the degree of the polynomial to fit + */ + public int degree() { + return degree; + } + + /** + * Returns the coefficient of determination R2. + * + * @return the coefficient of determination R2, which is a real number between + * 0 and 1 + */ + public double R2() { + if (sst == 0.0) return 1.0; // constant function + return 1.0 - sse / sst; + } + + /** + * Returns the expected response {@code y} given the value of the predictor variable {@code x}. + * + * @param x the value of the predictor variable + * @return the expected response {@code y} given the value of the predictor variable {@code x} + */ + public double predict(double x) { + // horner's method + double y = 0.0; + for (int j = degree; j >= 0; j--) y = beta(j) + (x * y); + return y; + } + + /** + * Returns a string representation of the polynomial regression model. + * + * @return a string representation of the polynomial regression model, including the best-fit + * polynomial and the coefficient of determination R2 + */ + public String toString() { + StringBuilder s = new StringBuilder(); + int j = degree; + + // ignoring leading zero coefficients + while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--; + + // create remaining terms + while (j >= 0) { + if (j == 0) s.append(String.format("%.4f ", beta(j))); + else if (j == 1) s.append(String.format("%.4f %s + ", beta(j), variableName)); + else s.append(String.format("%.4f %s^%d + ", beta(j), variableName, j)); + j--; + } + s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")"); + + // replace "+ -2n" with "- 2n" + return s.toString().replace("+ -", "- "); + } + + /** + * Compare lexicographically. + */ + public int compareTo(PolynomialRegression that) { + double EPSILON = 1E-5; + int maxDegree = Math.max(this.degree(), that.degree()); + for (int j = maxDegree; j >= 0; j--) { + double term1 = 0.0; + double term2 = 0.0; + if (this.degree() >= j) term1 = this.beta(j); + if (that.degree() >= j) term2 = that.beta(j); + if (Math.abs(term1) < EPSILON) term1 = 0.0; + if (Math.abs(term2) < EPSILON) term2 = 0.0; + if (term1 < term2) return -1; + else if (term1 > term2) return +1; + } + return 0; + } +} diff --git a/src/main/java/lib/controllers/Controller.java b/src/main/java/lib/controllers/Controller.java index 8fba25c..2b1dfb7 100644 --- a/src/main/java/lib/controllers/Controller.java +++ b/src/main/java/lib/controllers/Controller.java @@ -1,11 +1,18 @@ package lib.controllers; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +import java.util.function.BooleanSupplier; public class Controller { - protected final Joystick m_controller; + protected final Joystick controller; + + public Controller(int port) { + this.controller = new Joystick(port); + } - public Controller(int port) { - this.m_controller = new Joystick(port); - } + public Trigger get(BooleanSupplier sup) { + return new Trigger(sup); + } } diff --git a/src/main/java/lib/controllers/Ex3DProController.java b/src/main/java/lib/controllers/Ex3DProController.java index 7d09c52..29d1377 100644 --- a/src/main/java/lib/controllers/Ex3DProController.java +++ b/src/main/java/lib/controllers/Ex3DProController.java @@ -1,55 +1,77 @@ package lib.controllers; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj2.command.button.*; +import edu.wpi.first.wpilibj2.command.button.Trigger; public class Ex3DProController extends Controller { - public Ex3DProController(int port) { - super(port); - } - - public enum Button { - B1(1), - B2(2), - B3(3), - B4(4), - B6(6), - B7(7), - B8(8), - B9(9), - B10(10), - B11(11), - B12(12); - - public final int id; - - Button(final int id) { - this.id = id; - } - } - - public enum Axis { - X(0), - Y(1), - Z(2), - SLIDER(3); - - public final int id; - - Axis(final int id) { - this.id = id; - } - } - - public JoystickButton get(Button button) { - return new JoystickButton(m_controller, button.id); - } - - public double get(Axis axis) { - return m_controller.getRawAxis(axis.id); - } - - public Joystick get() { - return m_controller; - } + public Ex3DProController(int port) { + super(port); + } + + public enum Ex3DProButton { + B1(1), + B2(2), + B3(3), + B4(4), + B6(6), + B7(7), + B8(8), + B9(9), + B10(10), + B11(11), + B12(12); + + public final int id; + + Ex3DProButton(final int id) { + this.id = id; + } + } + + public enum Ex3DProAxis { + X(0), + Y(1), + Z(2), + SLIDER(3); + + public final int id; + + Ex3DProAxis(final int id) { + this.id = id; + } + } + + public enum Ex3DProHatSwitch { + UNPRESSED(-1), + UP(0), + UP_RIGHT(45), + RIGHT(90), + DOWN_RIGHT(135), + DOWN(180), + DOWN_LEFT(235), + LEFT(270), + UP_LEFT(315); + + public final int angle; + + Ex3DProHatSwitch(final int angle) { + this.angle = angle; + } + } + + public Trigger get(Ex3DProButton button) { + return new Trigger(() -> controller.getRawButton(button.id)); + } + + public double get(Ex3DProAxis axis) { + return controller.getRawAxis(axis.id); + } + + public Trigger get(Ex3DProHatSwitch hatSwitch) { + return new Trigger(() -> controller.getPOV() == hatSwitch.angle); + } + + public Joystick get() { + return controller; + } } diff --git a/src/main/java/lib/controllers/GameController.java b/src/main/java/lib/controllers/GameController.java index 01e8119..37f9347 100644 --- a/src/main/java/lib/controllers/GameController.java +++ b/src/main/java/lib/controllers/GameController.java @@ -2,111 +2,115 @@ package lib.controllers; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj2.command.button.*; +import edu.wpi.first.wpilibj2.command.button.Trigger; + import java.util.function.BooleanSupplier; public class GameController extends Controller { - // These are the different controller triggers - public final BooleanSupplier LEFT_TRIGGER_BUTTON = () -> get(Axis.LEFT_TRIGGER) > 0.5, - RIGHT_TRIGGER_BUTTON = () -> get(Axis.RIGHT_TRIGGER) > 0.5; - public final Trigger ALL_UP = get(DPad.UP).or(get(DPad.UP_LEFT)).or(get(DPad.UP_RIGHT)), - ALL_DOWN = get(DPad.DOWN).or(get(DPad.DOWN_LEFT)).or(get(DPad.DOWN_RIGHT)), - ALL_LEFT = get(DPad.LEFT).or(get(DPad.UP_LEFT)).or(get(DPad.DOWN_LEFT)), - ALL_RIGHT = get(DPad.RIGHT).or(get(DPad.UP_RIGHT)).or(get(DPad.DOWN_RIGHT)); - - public GameController(int port) { - super(port); - } - - public enum Button { - A(1), - B(2), - X(3), - Y(4), - LB(5), - RB(6), - BACK(7), - START(8), - LEFT_JOY(9), - RIGHT_JOY(10); - - public final int id; - - Button(final int id) { - this.id = id; + // These are the different controller triggers + public final BooleanSupplier LEFT_TRIGGER_BUTTON = () -> get(Axis.LEFT_TRIGGER) > 0.5, + RIGHT_TRIGGER_BUTTON = () -> get(Axis.RIGHT_TRIGGER) > 0.5; + public final Trigger ALL_UP = get(DPad.UP).or(get(DPad.UP_LEFT)).or(get(DPad.UP_RIGHT)), + ALL_DOWN = get(DPad.DOWN).or(get(DPad.DOWN_LEFT)).or(get(DPad.DOWN_RIGHT)), + ALL_LEFT = get(DPad.LEFT).or(get(DPad.UP_LEFT)).or(get(DPad.DOWN_LEFT)), + ALL_RIGHT = get(DPad.RIGHT).or(get(DPad.UP_RIGHT)).or(get(DPad.DOWN_RIGHT)); + + public final BooleanSupplier + LEFT_STICK_LEFT = () -> get(Axis.LEFT_X) < -0.75, + LEFT_STICK_RIGHT = () -> get(Axis.LEFT_X) > 0.75, + LEFT_STICK_UP = () -> get(Axis.LEFT_Y) < -0.75, + LEFT_STICK_DOWN = () -> get(Axis.LEFT_Y) > 0.75; + public final BooleanSupplier + RIGHT_STICK_LEFT = () -> get(Axis.RIGHT_X) < -0.75, + RIGHT_STICK_RIGHT = () -> get(Axis.RIGHT_X) > 0.75, + RIGHT_STICK_UP = () -> get(Axis.RIGHT_Y) < -0.75, + RIGHT_STICK_DOWN = () -> get(Axis.RIGHT_Y) > 0.75; + + public GameController(int port) { + super(port); + } + + public enum Button { + A(1), + B(2), + X(3), + Y(4), + LB(5), + RB(6), + BACK(7), + START(8), + LEFT_JOY(9), + RIGHT_JOY(10); + + public final int id; + + Button(final int id) { + this.id = id; + } } - } - public enum Axis { - LEFT_X(0), - LEFT_Y(1), - LEFT_TRIGGER(2), - RIGHT_TRIGGER(3), - RIGHT_X(4), - RIGHT_Y(5); + public enum Axis { + LEFT_X(0), + LEFT_Y(1), + LEFT_TRIGGER(2), + RIGHT_TRIGGER(3), + RIGHT_X(4), + RIGHT_Y(5); - public final int id; + public final int id; - Axis(final int id) { - this.id = id; + Axis(final int id) { + this.id = id; + } } - } - - public enum DPad { - UNPRESSED(-1), - UP(0), - UP_RIGHT(45), - RIGHT(90), - DOWN_RIGHT(135), - DOWN(180), - DOWN_LEFT(235), - LEFT(270), - UP_LEFT(315); - - public final int angle; - - DPad(final int angle) { - this.angle = angle; + + public enum DPad { + UNPRESSED(-1), + UP(0), + UP_RIGHT(45), + RIGHT(90), + DOWN_RIGHT(135), + DOWN(180), + DOWN_LEFT(235), + LEFT(270), + UP_LEFT(315); + + public final int angle; + + DPad(final int angle) { + this.angle = angle; + } } - } - public enum RumbleStatus { - RUMBLE_ON(0.7), - RUMBLE_OFF(0); + public enum RumbleStatus { + RUMBLE_ON(0.7), + RUMBLE_OFF(0); - public final double rumbleValue; + public final double rumbleValue; - RumbleStatus(final double rumbleValue) { - this.rumbleValue = rumbleValue; + RumbleStatus(final double rumbleValue) { + this.rumbleValue = rumbleValue; + } } - } - - public JoystickButton get(Button button) { - return new JoystickButton(m_controller, button.id); - } - - public double get(Axis axis) { - return m_controller.getRawAxis(axis.id); - } - public POVButton get(DPad dPad) { - return new POVButton(m_controller, dPad.angle); - } + public Trigger get(Button button) { + return new Trigger(() -> controller.getRawButton(button.id)); + } - public Trigger get(BooleanSupplier condition) { - return new Trigger(condition); - } + public double get(Axis axis) { + return controller.getRawAxis(axis.id); + } - public Trigger get(Trigger trigger) { - return trigger; - } + public Trigger get(DPad dPad) { + return new Trigger(() -> controller.getPOV() == dPad.angle); + } - public Joystick get() { - return m_controller; - } + public Joystick get() { + return controller; + } - public void setRumble(RumbleStatus rumbleStatus) { - m_controller.setRumble(RumbleType.kLeftRumble, rumbleStatus.rumbleValue); - m_controller.setRumble(RumbleType.kRightRumble, rumbleStatus.rumbleValue); - } -} + public void setRumble(RumbleStatus rumbleStatus) { + controller.setRumble(RumbleType.kLeftRumble, rumbleStatus.rumbleValue); + controller.setRumble(RumbleType.kRightRumble, rumbleStatus.rumbleValue); + } +} \ No newline at end of file diff --git a/src/main/java/lib/controllers/MadCatzController.java b/src/main/java/lib/controllers/MadCatzController.java index 0fbd04c..c94ae69 100644 --- a/src/main/java/lib/controllers/MadCatzController.java +++ b/src/main/java/lib/controllers/MadCatzController.java @@ -1,82 +1,78 @@ package lib.controllers; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj2.command.button.*; +import edu.wpi.first.wpilibj2.command.button.Trigger; public class MadCatzController extends Controller { - public final Trigger - ALL_UP = get(HatSwitch.UP).or(get(HatSwitch.UP_LEFT)).or(get(HatSwitch.UP_RIGHT)), - ALL_DOWN = get(HatSwitch.DOWN).or(get(HatSwitch.DOWN_LEFT)).or(get(HatSwitch.DOWN_RIGHT)), - ALL_LEFT = get(HatSwitch.LEFT).or(get(HatSwitch.UP_LEFT)).or(get(HatSwitch.DOWN_LEFT)), - ALL_RIGHT = get(HatSwitch.RIGHT).or(get(HatSwitch.UP_RIGHT)).or(get(HatSwitch.DOWN_RIGHT)); - - public MadCatzController(int port) { - super(port); - } - - public enum Button { - B1(1), - B2(2), - B3(3), - B4(4), - B6(6), - B7(7); - - public final int id; - - Button(final int id) { - this.id = id; + public final Trigger + ALL_UP = get(MadCatzHatSwitch.UP).or(get(MadCatzHatSwitch.UP_LEFT)).or(get(MadCatzHatSwitch.UP_RIGHT)), + ALL_DOWN = get(MadCatzHatSwitch.DOWN).or(get(MadCatzHatSwitch.DOWN_LEFT)).or(get(MadCatzHatSwitch.DOWN_RIGHT)), + ALL_LEFT = get(MadCatzHatSwitch.LEFT).or(get(MadCatzHatSwitch.UP_LEFT)).or(get(MadCatzHatSwitch.DOWN_LEFT)), + ALL_RIGHT = get(MadCatzHatSwitch.RIGHT).or(get(MadCatzHatSwitch.UP_RIGHT)).or(get(MadCatzHatSwitch.DOWN_RIGHT)); + + public MadCatzController(int port) { + super(port); } - } - public enum Axis { - X(0), - Y(1), - ZAXIS(2), - ZROTATE(3); + public enum MadCatzButton { + B1(1), + B2(2), + B3(3), + B4(4), + B6(6), + B7(7); - public final int id; + public final int id; - Axis(final int id) { - this.id = id; + MadCatzButton(final int id) { + this.id = id; + } } - } - - public enum HatSwitch { - UNPRESSED(-1), - UP(0), - UP_RIGHT(45), - RIGHT(90), - DOWN_RIGHT(135), - DOWN(180), - DOWN_LEFT(235), - LEFT(270), - UP_LEFT(315); - - public final int angle; - - HatSwitch(final int angle) { - this.angle = angle; + + public enum MadCatzAxis { + X(0), + Y(1), + SLIDER(2), + ZROTATE(3); + + public final int id; + + MadCatzAxis(final int id) { + this.id = id; + } } - } - public JoystickButton get(Button button) { - return new JoystickButton(m_controller, button.id); - } + public enum MadCatzHatSwitch { + UNPRESSED(-1), + UP(0), + UP_RIGHT(45), + RIGHT(90), + DOWN_RIGHT(135), + DOWN(180), + DOWN_LEFT(235), + LEFT(270), + UP_LEFT(315); + + public final int angle; + + MadCatzHatSwitch(final int angle) { + this.angle = angle; + } + } - public double get(Axis axis) { - return m_controller.getRawAxis(axis.id); - } + public Trigger get(MadCatzButton button) { + return new Trigger(() -> controller.getRawButton(button.id)); + } - public POVButton get(HatSwitch hatSwitch) { - return new POVButton(m_controller, hatSwitch.angle); - } + public double get(MadCatzAxis axis) { + return controller.getRawAxis(axis.id); + } - public Trigger get(Trigger trigger) { - return trigger; - } + public Trigger get(MadCatzHatSwitch hatSwitch) { + return new Trigger(() -> controller.getPOV() == hatSwitch.angle); + } - public Joystick get() { - return m_controller; - } + public Joystick get() { + return controller; + } } diff --git a/src/main/java/lib/controllers/PS5Controller.java b/src/main/java/lib/controllers/PS5Controller.java new file mode 100644 index 0000000..8f45a52 --- /dev/null +++ b/src/main/java/lib/controllers/PS5Controller.java @@ -0,0 +1,115 @@ +package lib.controllers; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +import java.util.function.BooleanSupplier; + +public class PS5Controller extends Controller { + // These are the different controller triggers + public final Trigger ALL_UP = get(DPad.UP).or(get(DPad.UP_LEFT)).or(get(DPad.UP_RIGHT)), + ALL_DOWN = get(DPad.DOWN).or(get(DPad.DOWN_LEFT)).or(get(DPad.DOWN_RIGHT)), + ALL_LEFT = get(DPad.LEFT).or(get(DPad.UP_LEFT)).or(get(DPad.DOWN_LEFT)), + ALL_RIGHT = get(DPad.RIGHT).or(get(DPad.UP_RIGHT)).or(get(DPad.DOWN_RIGHT)); + + public final BooleanSupplier + LEFT_STICK_LEFT = () -> get(PS5Axis.LEFT_X) < -0.75, + LEFT_STICK_RIGHT = () -> get(PS5Axis.LEFT_X) > 0.75, + LEFT_STICK_UP = () -> get(PS5Axis.LEFT_Y) < -0.75, + LEFT_STICK_DOWN = () -> get(PS5Axis.LEFT_Y) > 0.75; + public final BooleanSupplier + RIGHT_STICK_LEFT = () -> get(PS5Axis.RIGHT_X) < -0.75, + RIGHT_STICK_RIGHT = () -> get(PS5Axis.RIGHT_X) > 0.75, + RIGHT_STICK_UP = () -> get(PS5Axis.RIGHT_Y) < -0.75, + RIGHT_STICK_DOWN = () -> get(PS5Axis.RIGHT_Y) > 0.75; + + public PS5Controller(int port) { + super(port); + } + + public enum PS5Button { + SQUARE(1), + CROSS(2), + CIRCLE(3), + TRIANGLE(4), + LB(5), + RB(6), + LEFT_TRIGGER(7), + RIGHT_TRIGGER(8), + CREATE(9), + OPTIONS(10), + LEFT_JOY(11), + RIGHT_JOY(12), + PS(13), + TOUCHPAD(14), + MUTE(15); + + public final int id; + + PS5Button(final int id) { + this.id = id; + } + } + + public enum PS5Axis { + LEFT_X(0), + LEFT_Y(1), + RIGHT_X(2), + /** + * note: ps5 controller trigger goes from -1 when unpressed, to 1 when fully pressed + */ + LEFT_TRIGGER(3), + /** + * note: ps5 controller trigger goes from -1 when unpressed, to 1 when fully pressed + */ + RIGHT_TRIGGER(4), + RIGHT_Y(5); + + public final int id; + + PS5Axis(final int id) { + this.id = id; + } + } + + public enum DPad { + UNPRESSED(-1), + UP(0), + UP_RIGHT(45), + RIGHT(90), + DOWN_RIGHT(135), + DOWN(180), + DOWN_LEFT(235), + LEFT(270), + UP_LEFT(315); + + public final int angle; + + DPad(final int angle) { + this.angle = angle; + } + } + + public Trigger get(PS5Button button) { + return new Trigger(() -> controller.getRawButton(button.id)); + } + + public double get(PS5Axis axis) { + return controller.getRawAxis(axis.id); + } + + public Trigger get(DPad dPad) { + return new Trigger(() -> controller.getPOV() == dPad.angle); + } + + public Joystick get() { + return controller; + } + public void rumbleOn(){ + controller.setRumble(RumbleType.kBothRumble,0.1); + } + public void rumbleOff(){ + controller.setRumble(RumbleType.kBothRumble,0); + } +} \ No newline at end of file diff --git a/src/main/java/lib/controllers/PistolController.java b/src/main/java/lib/controllers/PistolController.java index 0acb6af..b7d1123 100644 --- a/src/main/java/lib/controllers/PistolController.java +++ b/src/main/java/lib/controllers/PistolController.java @@ -1,57 +1,52 @@ package lib.controllers; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; public class PistolController extends Controller { - public final Trigger TOP_BACK_ONLY = get(Button.TOP_BACK).and(get(Button.TOP_FRONT).negate()), - TOP_FRONT_ONLY = get(Button.TOP_FRONT).and(get(Button.TOP_BACK).negate()), - BOTTOM_BACK_ONLY = get(Button.BOTTOM_BACK).and(get(Button.BOTTOM_FRONT).negate()), - BOTTOM_FRONT_ONLY = get(Button.BOTTOM_FRONT).and(get(Button.BOTTOM_BACK).negate()); - - public PistolController(int port) { - super(port); - } - - public enum Button { - TOP_BACK(1), - TOP_FRONT(2), - BOTTOM_FRONT(3), - BOTTOM_BACK(4), - BOTTOM(5); - - public final int id; - - Button(final int id) { - this.id = id; + public final Trigger TOP_BACK_ONLY = get(Button.TOP_BACK).and(get(Button.TOP_FRONT).negate()), + TOP_FRONT_ONLY = get(Button.TOP_FRONT).and(get(Button.TOP_BACK).negate()), + BOTTOM_BACK_ONLY = get(Button.BOTTOM_BACK).and(get(Button.BOTTOM_FRONT).negate()), + BOTTOM_FRONT_ONLY = get(Button.BOTTOM_FRONT).and(get(Button.BOTTOM_BACK).negate()); + + public PistolController(int port) { + super(port); } - } - public enum Axis { - WHEEL(0), - TRIGGER(1); + public enum Button { + TOP_BACK(1), + TOP_FRONT(2), + BOTTOM_FRONT(3), + BOTTOM_BACK(4), + BOTTOM(5); - public final int id; + public final int id; - Axis(final int id) { - this.id = id; + Button(final int id) { + this.id = id; + } } - } - public JoystickButton get(Button button) { - return new JoystickButton(m_controller, button.id); - } + public enum Axis { + WHEEL(0), + TRIGGER(1); + + public final int id; - public double get(Axis axis) { - return m_controller.getRawAxis(axis.id); - } + Axis(final int id) { + this.id = id; + } + } - public Trigger get(Trigger trigger) { - return trigger; - } + public Trigger get(Button button) { + return new Trigger(() -> controller.getRawButton(button.id)); + } - public Joystick get() { - return m_controller; - } + public double get(Axis axis) { + return controller.getRawAxis(axis.id); + } + + public Joystick get() { + return controller; + } } diff --git a/src/main/java/lib/ctre_shims/PhoenixMotorControllerGroup.java b/src/main/java/lib/ctre_shims/PhoenixMotorControllerGroup.java deleted file mode 100644 index 28b6371..0000000 --- a/src/main/java/lib/ctre_shims/PhoenixMotorControllerGroup.java +++ /dev/null @@ -1,110 +0,0 @@ -package lib.ctre_shims; - -import com.ctre.phoenix.motorcontrol.InvertType; -import com.ctre.phoenix.motorcontrol.can.BaseMotorController; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.util.sendable.SendableRegistry; -import edu.wpi.first.wpilibj.motorcontrol.MotorController; -import java.util.Arrays; - -/** - * Allows multiple {@link MotorController} objects to be linked together. - * - *

    Reimplementation with supports CTRE CAN follower functionality. - */ -public class PhoenixMotorControllerGroup implements MotorController, Sendable, AutoCloseable { - private final MotorController[] m_motorControllers; - private final MotorController m_leadMotorController; - private static int instances; - - /** - * Create a new PhoenixMotorControllerGroup with the provided MotorControllers. - * - * @param leadMotorController The lead MotorController to add - * @param motorControllers The MotorControllers to add - */ - public PhoenixMotorControllerGroup( - MotorController leadMotorController, MotorController... motorControllers) { - boolean validArgumentTypes = leadMotorController instanceof BaseMotorController; - for (MotorController motorController : motorControllers) { - validArgumentTypes = validArgumentTypes && (motorController instanceof BaseMotorController); - } - - if (!validArgumentTypes) { - throw new IllegalArgumentException( - "One or more MotorControllers do not inherit from BaseMotorController, i.e. are not CTRE classes"); - } - m_leadMotorController = leadMotorController; - m_motorControllers = Arrays.copyOf(motorControllers, motorControllers.length); - init(); - } - - private void init() { - SendableRegistry.addChild(this, m_leadMotorController); - for (MotorController controller : m_motorControllers) { - SendableRegistry.addChild(this, controller); - ((BaseMotorController) controller).follow((BaseMotorController) m_leadMotorController); - ((BaseMotorController) controller).setInverted(InvertType.FollowMaster); - } - instances++; - SendableRegistry.addLW(this, "PhoenixMotorControllerGroup", instances); - } - - @Override - public void close() { - SendableRegistry.remove(this); - } - - @Override - public void set(double speed) { - m_leadMotorController.set(speed); - } - - @Override - public double get() { - return m_leadMotorController.get(); - } - - @Override - public void setInverted(boolean isInverted) { - m_leadMotorController.setInverted(isInverted); - } - - @Override - public boolean getInverted() { - return m_leadMotorController.getInverted(); - } - - @Override - public void disable() { - m_leadMotorController.disable(); - for (MotorController motorController : m_motorControllers) { - motorController.disable(); - } - } - - @Override - public void stopMotor() { - m_leadMotorController.stopMotor(); - for (MotorController motorController : m_motorControllers) { - motorController.stopMotor(); - } - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("Phoenix Motor Controller"); - builder.setActuator(true); - builder.setSafeState(this::stopMotor); - builder.addDoubleProperty("Value", this::get, this::set); - } - - @Override - public void setVoltage(double outputVolts) { - m_leadMotorController.setVoltage(outputVolts); - for (MotorController motorController : m_motorControllers) { - motorController.setVoltage(outputVolts); - } - } -} diff --git a/src/main/java/lib/ctre_shims/TalonEncoder.java b/src/main/java/lib/ctre_shims/TalonEncoder.java deleted file mode 100644 index 1c7de84..0000000 --- a/src/main/java/lib/ctre_shims/TalonEncoder.java +++ /dev/null @@ -1,181 +0,0 @@ -package lib.ctre_shims; - -import com.ctre.phoenix.ParamEnum; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.can.BaseTalon; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.util.sendable.SendableRegistry; - -/** A class to read encoder data from CTRE motors, frc::Encoder compatible. */ -public class TalonEncoder implements Sendable, AutoCloseable { - private final BaseTalon m_motor; - private double m_distancePerPulse = 1; - - public TalonEncoder(BaseTalon motor) { - this(motor, false); - } - - public TalonEncoder(BaseTalon motor, boolean reverseDirection) { - m_motor = motor; - setReverseDirection(reverseDirection); - SendableRegistry.addLW(this, "Talon Encoder", motor.getDeviceID()); - } - - @Override - public void close() { - SendableRegistry.remove(this); - } - - /** - * Gets the current count. Returns the current count on the Encoder. This method compensates for - * the decoding type. - * - * @return Current count from the Encoder adjusted for the 1x, 2x, or 4x scale factor. - */ - public int get() { - return (int) m_motor.getSelectedSensorPosition(); - } - - /** Reset the Encoder distance to zero. Resets the current count to zero on the encoder. */ - public void reset() { - m_motor.setSelectedSensorPosition(0); - } - - /** - * Returns the period of the most recent pulse. Returns the period of the most recent Encoder - * pulse in seconds. This method compensates for the decoding type. - * - *

    Warning: This returns unscaled periods. Use getRate() for rates that are scaled using - * the value from setDistancePerPulse(). - * - * @return Period in seconds of the most recent pulse. - * @deprecated Use getRate() in favor of this method. - */ - @Deprecated - public double getPeriod() { - // distance / (distance / second) = seconds - return m_distancePerPulse / getRate(); - } - - /** - * The last direction the encoder value changed. - * - * @return The last direction the encoder value changed. - */ - public boolean getDirection() { - if (getRate() >= 0) { - return true; - } else { - return false; - } - } - - /** - * Get the distance the robot has driven since the last reset as scaled by the value from {@link - * #setDistancePerPulse(double)}. - * - * @return The distance driven since the last reset - */ - public double getDistance() { - return get() * m_distancePerPulse; - } - - /** - * Get the current rate of the encoder. Units are distance per second as scaled by the value from - * setDistancePerPulse(). - * - * @return The current rate of the encoder. - */ - public double getRate() { - return m_motor.getSelectedSensorVelocity() * 10 * m_distancePerPulse; - } - - /** - * Set the distance per pulse for this encoder. This sets the multiplier used to determine the - * distance driven based on the count value from the encoder. Do not include the decoding type in - * this scale. The library already compensates for the decoding type. Set this value based on the - * encoder's rated Pulses per Revolution and factor in gearing reductions following the encoder - * shaft. This distance can be in any units you like, linear or angular. - * - * @param distancePerPulse The scale factor that will be used to convert pulses to useful units. - */ - public void setDistancePerPulse(double distancePerPulse) { - m_distancePerPulse = distancePerPulse; - } - - /** - * Get the distance per pulse for this encoder. - * - * @return The scale factor that will be used to convert pulses to useful units. - */ - public double getDistancePerPulse() { - return m_distancePerPulse; - } - - /** - * Set the direction sensing for this encoder. This sets the direction sensing on the encoder so - * that it could count in the correct software direction regardless of the mounting. - * - * @param reverseDirection true if the encoder direction should be reversed - */ - public void setReverseDirection(boolean reverseDirection) { - m_motor.setSensorPhase(reverseDirection); - } - - /** - * Set the Samples to Average which specifies the number of samples of the timer to average when - * calculating the period. Perform averaging to account for mechanical imperfections or as - * oversampling to increase resolution. - * - *

    Defaults to 64. See also, CTRE's ConfigVelocityMeasurementPeriod(). - * https://docs.ctre-phoenix.com/en/stable/ch14_MCSensor.html#changing-velocity-measurement-parameters - * See also, the note in Measurement-Delays.md in the root of this repository. - * - * @param samplesToAverage The number of samples to average (one of 1, 2, 4, 8, 16, 32, or 64). - */ - public void setSamplesToAverage(int samplesToAverage) { - // (n & (n-1)) checks if it is a power of two. See: - // http:// - // www.graphics.stanford.edu/~seander/bithacks.html#DetermineIfPowerOf2 - if (samplesToAverage < 1 - || samplesToAverage > 64 - || (samplesToAverage & (samplesToAverage - 1)) != 0) { - throw new IllegalArgumentException( - "Samples to average must be a power of 2 between 1 and 64, got " + samplesToAverage); - } - m_motor.configVelocityMeasurementWindow(samplesToAverage); - } - - /** - * Get the Samples to Average which specifies the number of samples of the timer to average when - * calculating the period. Perform averaging to account for mechanical imperfections or as - * oversampling to increase resolution. - * - *

    Defaults to 64. See also, CTRE's ConfigVelocityMeasurementPeriod(). - * https://docs.ctre-phoenix.com/en/stable/ch14_MCSensor.html#changing-velocity-measurement-parameters - * See also, the note in Measurement-Delays.md in the root of this repository. - * - * @return samplesToAverage The number of samples to average (one of 1, 2, 4, 8, 16, 32, or 64). - */ - public int getSamplesToAverage() { - return (int) m_motor.configGetParameter(ParamEnum.eSampleVelocityWindow, 0); - } - - public BaseTalon getMotor() { - return m_motor; - } - - public FeedbackDevice getSelectedFeedbackSensor() { - return FeedbackDevice.valueOf(m_motor.configGetParameter(ParamEnum.eFeedbackSensorType, 0)); - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType( - String.format("Encoder (%s)", getSelectedFeedbackSensor().name())); - builder.addDoubleProperty("Speed", this::getRate, null); - builder.addDoubleProperty("Distance", this::getDistance, null); - builder.addDoubleProperty("Distance per Tick", this::getDistancePerPulse, null); - } -} diff --git a/src/main/java/lib/ctre_shims/TalonEncoderSim.java b/src/main/java/lib/ctre_shims/TalonEncoderSim.java deleted file mode 100644 index 996c35d..0000000 --- a/src/main/java/lib/ctre_shims/TalonEncoderSim.java +++ /dev/null @@ -1,194 +0,0 @@ -package lib.ctre_shims; - -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.TalonFXSimCollection; -import com.ctre.phoenix.motorcontrol.TalonSRXSimCollection; -import com.ctre.phoenix.motorcontrol.can.TalonFX; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; - -/** Class to control a simulated encoder. */ -public class TalonEncoderSim { - private TalonEncoder m_encoder; - - private TalonSRXSimCollection m_simCollectionSRX = null; - private TalonFXSimCollection m_simCollectionFX = null; - - /** - * Constructs from an Encoder object. - * - * @param encoder Encoder to simulate - */ - public TalonEncoderSim(TalonEncoder encoder) { - m_encoder = encoder; - - if (m_encoder.getMotor() instanceof TalonSRX) { - m_simCollectionSRX = new TalonSRXSimCollection(m_encoder.getMotor()); - } else if (m_encoder.getMotor() instanceof TalonFX) { - m_simCollectionFX = new TalonFXSimCollection(m_encoder.getMotor()); - } else { - throw new IllegalStateException( - "Motor type " + m_encoder.getMotor().getClass().getName() + " is unsupported."); - } - } - - /** - * Read the count of the encoder. - * - * @return the count - */ - public int getCount() { - return m_encoder.get(); - } - - /** - * Change the count of the encoder. - * - * @param count the new count - */ - public void setCount(int count) { - if (m_simCollectionSRX != null) { - FeedbackDevice selected = m_encoder.getSelectedFeedbackSensor(); - - switch (selected) { - case QuadEncoder: - case CTRE_MagEncoder_Relative: - m_simCollectionSRX.setQuadratureRawPosition(count); - break; - - case Analog: - m_simCollectionSRX.setAnalogPosition(count); - break; - - default: - throw new IllegalStateException( - "Selected feedback sensor is not supported: " + selected.name()); - } - } else if (m_simCollectionFX != null) { - m_simCollectionFX.setIntegratedSensorRawPosition(count); - } else { - // This should have errored already in the constructor. - assert false; - } - } - - /** - * Read the period of the encoder. - * - * @return the encoder period - */ - public double getPeriod() { - return m_encoder.getRate(); - } - - /** - * Change the encoder period. - * - * @param period the new period - */ - public void setPeriod(double period) { - // seconds -> distance/second - // m_distancePerPulse (distance) / period (seconds) = distance / second - setRate(m_encoder.getDistancePerPulse() / period); - } - - // These are no-op in WPILib - // boolean getReset(); - // void setReset(); - - /** - * Get the direction of the encoder. - * - * @return the direction of the encoder - */ - public boolean getDirection() { - return m_encoder.getDirection(); - } - - /** - * Get the samples-to-average value. - * - *

    See {@link TalonEncoder#getSamplesToAverage()}. - * - * @return the samples-to-average value - */ - public int getSamplesToAverage() { - return m_encoder.getSamplesToAverage(); - } - - /** - * Set the samples-to-average value. - * - *

    See {@link TalonEncoder#setSamplesToAverage()}. - * - * @param samplesToAverage the new value - */ - public void setSamplesToAverage(int samplesToAverage) { - m_encoder.setSamplesToAverage(samplesToAverage); - } - - /** - * Change the encoder distance. - * - * @param distance the new distance - */ - public void setDistance(double distance) { - setCount((int) (distance / m_encoder.getDistancePerPulse())); - } - - /** - * Read the distance of the encoder. - * - * @return the encoder distance - */ - public double getDistance() { - return m_encoder.getDistance(); - } - - /** - * Change the rate of the encoder. - * - * @param rate the new rate - */ - public void setRate(double rate) { - int rateInNativeUnits = (int) (rate / (10 * m_encoder.getDistancePerPulse())); - - if (m_simCollectionSRX != null) { - FeedbackDevice selected = m_encoder.getSelectedFeedbackSensor(); - - switch (selected) { - case QuadEncoder: - case CTRE_MagEncoder_Relative: - m_simCollectionSRX.setQuadratureVelocity(rateInNativeUnits); - break; - - case Analog: - m_simCollectionSRX.setAnalogVelocity(rateInNativeUnits); - break; - - default: - throw new IllegalStateException( - "Selected feedback sensor is not supported: " + selected.name()); - } - } else if (m_simCollectionFX != null) { - m_simCollectionFX.setIntegratedSensorVelocity(rateInNativeUnits); - } else { - // This should have errored already in the constructor. - assert false; - } - } - - /** - * Get the rate of the encoder. - * - * @return the rate of change - */ - public double getRate() { - return m_encoder.getRate(); - } - - /** Resets all simulation data for this encoder. */ - public void resetData() { - setDistance(0); - setRate(0); - } -} diff --git a/src/test/java/frc/robot/constants/AprilTagPoseTest.java b/src/test/java/frc/robot/constants/AprilTagPoseTest.java new file mode 100644 index 0000000..b5ce24d --- /dev/null +++ b/src/test/java/frc/robot/constants/AprilTagPoseTest.java @@ -0,0 +1,125 @@ +package frc.robot.constants; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.util.ArrayList; +import java.util.List; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import frc.robot.util.Vision.Vision; + +/** + * Tests if all of the AprilTags are in the right spot + */ +public class AprilTagPoseTest { + /** + * Tests if there are the right number of AprilTags, that the tags in Vision match the ones in FieldConstants, and that they are on the right side of the field + */ + @Test + public void testTagPoses() { + // Construct the vision instance + // makes the field layout + Vision vision = new Vision(new ArrayList>()); + + // we should have 22 tags + assertEquals(22, FieldConstants.APRIL_TAGS.size()); + assertEquals(22, vision.getAprilTagFieldLayout().getTags().size()); + + // Check each tag in the field layout + for (int i = 0; i < vision.getAprilTagFieldLayout().getTags().size(); i++) { + // The expected tagId. The ArrayList is zero-based and our tags start at 1, so the tagId is i+1. + int tagId = i + 1; + + // Get the poses from the two sources + // From the ArrayList source + AprilTag apriltag1 = FieldConstants.APRIL_TAGS.get(i); + Pose3d p1 = apriltag1.pose; + // From the vision source + Pose3d p2 = vision.getTagPose(tagId); + + // Check the tag id in the ArrayList + assertEquals(tagId, apriltag1.ID); + + // Make sure the points match + assertEquals(p1.getX(), p2.getX(), 0.0001); + assertEquals(p1.getY(), p2.getY(), 0.0001); + assertEquals(p1.getZ(), p2.getZ(), 0.0001); + + // Make sure the rotations match + assertEquals(p1.getRotation().getX(), p2.getRotation().getX(), 0.0001); + assertEquals(p1.getRotation().getY(), p2.getRotation().getY(), 0.0001); + assertEquals(p1.getRotation().getZ(), p2.getRotation().getZ(), 0.0001); + + // 1-11 should be on the right, and 12-22 should be on the left + if(tagId > 11){ + assertTrue(p1.getX() < FieldConstants.FIELD_LENGTH/2); + }else{ + assertTrue(p1.getX() > FieldConstants.FIELD_LENGTH/2); + } + } + } + + @Test + public void testReefTags(){ + List redPoses = FieldConstants.APRIL_TAGS.subList(5, 11).stream().map(tag->tag.pose).toList(); + List bluePoses = FieldConstants.APRIL_TAGS.subList(16, 22).stream().map(tag->tag.pose).toList(); + Pose3d redCenter = findCenter(redPoses); + Pose3d blueCenter = findCenter(bluePoses); + + // The tags should be symmetrical, so the total rotation should be 0 + assertEquals(redCenter.getRotation().getX(), 0, 0.0001); + assertEquals(blueCenter.getRotation().getX(), 0, 0.0001); + assertEquals(redCenter.getRotation().getY(), 0, 0.0001); + assertEquals(blueCenter.getRotation().getY(), 0, 0.0001); + assertEquals(MathUtil.angleModulus(redCenter.getRotation().getZ()), Math.PI, 0.0001); + assertEquals(MathUtil.angleModulus(blueCenter.getRotation().getZ()), Math.PI, 0.0001); + + // Y and Z should be equal + assertEquals(redCenter.getY(), blueCenter.getY(), 0.0001); + assertEquals(redCenter.getZ(), blueCenter.getZ(), 0.0001); + + // X should be mirrored + assertEquals(redCenter.getX(), FieldConstants.FIELD_LENGTH-blueCenter.getX(), 0.01); + + // Compare each matching pair of tags + for(int i = 5; i < 11; i++){ + Pose3d red = FieldConstants.APRIL_TAGS.get(i).pose; + Pose3d blue = FieldConstants.APRIL_TAGS.get(i+11).pose; + assertEquals(red.getY(), blue.getY(), 0.0001); + assertEquals(red.getZ(), blue.getZ(), 0.0001); + assertEquals(red.getZ(), redCenter.getZ(), 0.0001); + assertEquals(red.getX(), FieldConstants.FIELD_LENGTH-blue.getX(), 0.01); + assertEquals(MathUtil.angleModulus(red.getRotation().getZ()), MathUtil.angleModulus(Math.PI-blue.getRotation().getZ()), 0.0001); + } + } + + /** + * Gets the center pose with the sum of the rotations, used for checking the reef + * @param poses The poses to find the center of + * @return A pose with the translation at the center and the sum of the rotations + */ + private Pose3d findCenter(List poses){ + double x = 0; + double y = 0; + double z = 0; + Rotation3d rot = new Rotation3d(); + for(Pose3d pose : poses){ + x += pose.getX(); + y += pose.getY(); + z += pose.getZ(); + rot = rot.plus(pose.getRotation()); + } + x /= poses.size(); + y /= poses.size(); + z /= poses.size(); + return new Pose3d(x, y, z, rot); + } +} diff --git a/src/test/java/frc/robot/constants/ConstantsTest.java b/src/test/java/frc/robot/constants/ConstantsTest.java new file mode 100644 index 0000000..b368426 --- /dev/null +++ b/src/test/java/frc/robot/constants/ConstantsTest.java @@ -0,0 +1,92 @@ +package frc.robot.constants; + +import edu.wpi.first.math.util.Units; +import frc.robot.constants.swerve.DriveConstants; +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +/** + * Check some robot constants/parameters. + */ +public class ConstantsTest { + + @Test + public void testRobotSize() { + // The competition robot frame width and length is 26 inches. + // It has 3/16 inch plates on all sides, + // so frame width is 26.375! + double widthFrame = Units.inchesToMeters(26.0); + double widthFrameAndPlates = widthFrame + 2.0 * Units.inchesToMeters(3.0 / 16.0); + + // At Port Hueneme, the frame perimeter was 105 inches + // frame perimeter is 105.5... + assertEquals(105.0, 4 * Units.metersToInches(widthFrameAndPlates), 0.501); + + // Bumpers. + // The backing board is 0.75 inches. + // The noodles are 2.5 inches. + // Board + noodles = 3.25 inches + // Measure the red bumpers, and they are 3.5 inches. + // + // The latch studs are centered on the 1x2 frame rails. That's 0.5 inches + // The bumper latch bracket holes for the studs are 3/4 in from the inside surface of the bumpers. + // That puts the bumper inside surface at 3/4 - (0.5) = 1/4 inch out from widthFrame + double thickBumpers = Units.inchesToMeters(3.5 + 0.25); + + // so width with bumpers is + @SuppressWarnings("unused") + double widthFrameWithBumpers = widthFrame + 2 * thickBumpers; + + // check with values in DriveConstants + // System.out.printf("widthFrameWithBumpers = %8f %8f\n", widthFrameWithBumpers, Units.metersToInches(widthFrameWithBumpers)); + // System.out.printf("kRobotWidthWithBumpers = %8f %8f\n", DriveConstants.kRobotWidthWithBumpers, Units.metersToInches(DriveConstants.kRobotWidthWithBumpers)); + // assertEquals(widthFrameWithBumpers, DriveConstants.kRobotWidthWithBumpers, 0.001); + } + + /** + * MK4i module + * https://www.swervedrivespecialties.com/products/mk4i-swerve-module + */ + enum SwerveDriveSpecialties { + // Gearbox ratios from the SDS webpage + // https://www.swervedrivespecialties.com/products/mk4i-swerve-module + L1((50.0 / 14.0) * (19.0 / 25.0) * (45.0 / 15.0)), + L2((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)), + L3((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)), + // 16-tooth pinion + // https://www.swervedrivespecialties.com/products/kit-adapter-16t-drive-pinion-gear-mk4i + L1P((50.0 / 16.0) * (19.0 / 25.0) * (45.0 / 15.0)), + L2P((50.0 / 16.0) * (17.0 / 27.0) * (45.0 / 15.0)), + L3P((50.0 / 16.0) * (16.0 / 28.0) * (45.0 / 15.0)); + + /** + * Drive gear ratio varies for each module + */ + final double driveRatio; + /** + * Steering Gear ratio (same for all MK4i modules) + */ + final double steerRatio = 150.0 / 7.0; + + SwerveDriveSpecialties(double drive) { + this.driveRatio = drive; + } + } + + @Test + public void testSwerveRatios() { + // check the mroe exact ratios against the published-to-2-digits ratios + assertEquals(8.14, SwerveDriveSpecialties.L1.driveRatio, 0.01); + assertEquals(6.75, SwerveDriveSpecialties.L2.driveRatio, 0.01); + assertEquals(6.12, SwerveDriveSpecialties.L3.driveRatio, 0.01); + + // The drive ratio could be more accurate, but does not hurt + assertEquals(SwerveDriveSpecialties.L2P.driveRatio, DriveConstants.DRIVE_GEAR_RATIO, 0.01); + + // The steer ratio + // print the relative error: 0.6e-4. After 100 rotations, error would be 0.6e-2 rotations (about 1.5 degrees) + // System.out.println((DriveConstants.kSteerGearRatio - SwerveDriveSpecialties.L2.steerRatio) / SwerveDriveSpecialties.L2.steerRatio); + assertEquals(SwerveDriveSpecialties.L2P.steerRatio, DriveConstants.STEER_GEAR_RATIO, 0.01); + } +} diff --git a/src/test/java/frc/robot/util/ArithTest.java b/src/test/java/frc/robot/util/ArithTest.java new file mode 100644 index 0000000..59e3a4f --- /dev/null +++ b/src/test/java/frc/robot/util/ArithTest.java @@ -0,0 +1,44 @@ +package frc.robot.util; + +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Disabled; +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +/** + * Example of a JUnit test class. + * This test should run everytime someone builds the robot code. + * See https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-simulation/unit-testing.html + *

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

    + * We have had problems with syntax errors in a path. + */ + @Test + public void pathGroupLoaderTest() { + // load the paths + // may throw a ParseException; that error will fail this test + PathGroupLoader.loadPathGroups(); + } +} diff --git a/src/test/java/frc/robot/util/PolynomialRegressionTest.java b/src/test/java/frc/robot/util/PolynomialRegressionTest.java new file mode 100644 index 0000000..7ac3687 --- /dev/null +++ b/src/test/java/frc/robot/util/PolynomialRegressionTest.java @@ -0,0 +1,25 @@ +package frc.robot.util; + +import lib.PolynomialRegression; +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +public class PolynomialRegressionTest { + + /** + * Unit tests the {@code PolynomialRegression} data type. + */ + @Test + public void testRegression() { + double[] x = {10, 20, 40, 80, 160, 200}; + double[] y = {100, 350, 1500, 6700, 20160, 40000}; + PolynomialRegression regression = new PolynomialRegression(x, y, 3); + + assertEquals(regression.beta(3), 0.0092, 0.0001); + assertEquals(regression.beta(2), -1.6395, 0.0001); + assertEquals(regression.beta(1), 168.9232, 0.0001); + assertEquals(regression.beta(0), -2113.7306, 0.0001); + } + +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..bef4a15 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,35 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "4.1.2", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2025", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "4.1.2" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "4.1.2", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file diff --git a/vendordeps/ChoreoLib-2025.0.3.json b/vendordeps/ChoreoLib-2025.0.3.json new file mode 100644 index 0000000..5a8cd54 --- /dev/null +++ b/vendordeps/ChoreoLib-2025.0.3.json @@ -0,0 +1,44 @@ +{ + "fileName": "ChoreoLib-2025.0.3.json", + "name": "ChoreoLib", + "version": "2025.0.3", + "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", + "frcYear": "2025", + "mavenUrls": [ + "https://lib.choreo.autos/dep", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://lib.choreo.autos/dep/ChoreoLib2025.json", + "javaDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-java", + "version": "2025.0.3" + }, + { + "groupId": "com.google.code.gson", + "artifactId": "gson", + "version": "2.11.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-cpp", + "version": "2025.0.3", + "libName": "ChoreoLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index b0508c9..10f6cd2 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,8 +1,9 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2023.0.1", + "version": "2025.2.6", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2025", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], @@ -11,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2023.0.1" + "version": "2025.2.6" } ], "jniDependencies": [], @@ -19,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2023.0.1", + "version": "2025.2.6", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -27,8 +28,10 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", - "osxx86-64", - "linuxathena" + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" ] } ] diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json deleted file mode 100644 index dd0b3f4..0000000 --- a/vendordeps/Phoenix.json +++ /dev/null @@ -1,257 +0,0 @@ -{ - "fileName": "Phoenix.json", - "name": "CTRE-Phoenix", - "version": "5.21.2", - "frcYear": 2022, - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix-frc2022-latest.json", - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.21.2" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.21.2" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.21.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena" - ] - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.21.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "simTalonSRX", - "version": "5.21.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "simTalonFX", - "version": "5.21.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "simVictorSPX", - "version": "5.21.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "simPigeonIMU", - "version": "5.21.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "simCANCoder", - "version": "5.21.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxx86-64" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-cpp", - "version": "5.21.2", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.21.2", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.21.2", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena" - ] - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "5.21.2", - "libName": "CTRE_Phoenix_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "api-cpp-sim", - "version": "5.21.2", - "libName": "CTRE_PhoenixSim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.21.2", - "libName": "CTRE_PhoenixCCISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "simTalonSRX", - "version": "5.21.2", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "simTalonFX", - "version": "5.21.2", - "libName": "CTRE_SimTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "simVictorSPX", - "version": "5.21.2", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "simPigeonIMU", - "version": "5.21.2", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "simCANCoder", - "version": "5.21.2", - "libName": "CTRE_SimCANCoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxx86-64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/Phoenix5-frc2025-latest.json b/vendordeps/Phoenix5-frc2025-latest.json new file mode 100644 index 0000000..c1098dc --- /dev/null +++ b/vendordeps/Phoenix5-frc2025-latest.json @@ -0,0 +1,171 @@ +{ + "fileName": "Phoenix5-frc2025-latest.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.35.1", + "frcYear": "2025", + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6-frc2025-latest.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json" + } + ], + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users must use the Phoenix 5 replay vendordep when using the Phoenix 6 replay vendordep.", + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" + }, + { + "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df", + "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.", + "offlineFileName": "Phoenix5-replay-frc2025-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.35.1" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.35.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.35.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.35.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.35.1", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.35.1", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.35.1", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.35.1", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.35.1", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.35.1", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-frc2025-latest.json new file mode 100644 index 0000000..d7bea57 --- /dev/null +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -0,0 +1,449 @@ +{ + "fileName": "Phoenix6-frc2025-latest.json", + "name": "CTRE-Phoenix (v6)", + "version": "25.3.2", + "frcYear": "2025", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "25.3.2" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.3.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.3.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "25.3.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.3.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.3.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.3.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.3.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.3.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.3.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.3.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.3.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.3.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.3.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "25.3.2", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.3.2", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "25.3.2", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.3.2", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.3.2", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.3.2", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.3.2", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.3.2", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.3.2", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.3.2", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.3.2", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.3.2", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.3.2", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.2", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 997e2a4..ac62be8 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,34 +1,34 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2022.1.1", + "version": "2025.0.3", + "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "https://maven.revrobotics.com/" ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib.json", + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", "javaDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2022.1.1" + "version": "2025.0.3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2022.1.1", + "version": "2025.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ "windowsx86-64", - "windowsx86", - "linuxaarch64bionic", + "linuxarm64", "linuxx86-64", "linuxathena", - "linuxraspbian", - "osxx86-64" + "linuxarm32", + "osxuniversal" ] } ], @@ -36,37 +36,35 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2022.1.1", + "version": "2025.0.3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", - "linuxaarch64bionic", + "linuxarm64", "linuxx86-64", "linuxathena", - "linuxraspbian", - "osxx86-64" + "linuxarm32", + "osxuniversal" ] }, { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2022.1.1", + "version": "2025.0.3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", - "linuxaarch64bionic", + "linuxarm64", "linuxx86-64", "linuxathena", - "linuxraspbian", - "osxx86-64" + "linuxarm32", + "osxuniversal" ] } ] diff --git a/vendordeps/Studica-2025.0.1.json b/vendordeps/Studica-2025.0.1.json new file mode 100644 index 0000000..5010be0 --- /dev/null +++ b/vendordeps/Studica-2025.0.1.json @@ -0,0 +1,71 @@ +{ + "fileName": "Studica-2025.0.1.json", + "name": "Studica", + "version": "2025.0.1", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2025", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2025/" + ], + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json", + "cppDependencies": [ + { + "artifactId": "Studica-cpp", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "Studica", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.1" + }, + { + "artifactId": "Studica-driver", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "StudicaDriver", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.1" + } + ], + "javaDependencies": [ + { + "artifactId": "Studica-java", + "groupId": "com.studica.frc", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "artifactId": "Studica-driver", + "groupId": "com.studica.frc", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "version": "2025.0.1" + } + ] +} \ No newline at end of file diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index d7bd9b0..3718e0a 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -1,37 +1,38 @@ { "fileName": "WPILibNewCommands.json", "name": "WPILib-New-Commands", - "version": "2020.0.0", + "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "frcYear": "2025", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-java", - "version": "wpilib" - } + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } ], "jniDependencies": [], "cppDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-cpp", - "version": "wpilib", - "libName": "wpilibNewCommands", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxraspbian", - "linuxaarch64bionic", - "windowsx86-64", - "windowsx86", - "linuxx86-64", - "osxx86-64" - ] - } + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } ] } diff --git a/vendordeps/libgrapplefrc2025.json b/vendordeps/libgrapplefrc2025.json new file mode 100644 index 0000000..a49af31 --- /dev/null +++ b/vendordeps/libgrapplefrc2025.json @@ -0,0 +1,74 @@ +{ + "fileName": "libgrapplefrc2025.json", + "name": "libgrapplefrc", + "version": "2025.1.3", + "frcYear": "2025", + "uuid": "8ef3423d-9532-4665-8339-206dae1d7168", + "mavenUrls": [ + "https://storage.googleapis.com/grapple-frc-maven" + ], + "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2025.json", + "javaDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcjava", + "version": "2025.1.3" + } + ], + "jniDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.1.3", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrccpp", + "version": "2025.1.3", + "libName": "grapplefrc", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.1.3", + "libName": "grapplefrcdriver", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/navx_frc.json b/vendordeps/navx_frc.json deleted file mode 100644 index 1718692..0000000 --- a/vendordeps/navx_frc.json +++ /dev/null @@ -1,35 +0,0 @@ -{ - "fileName": "navx_frc.json", - "name": "KauaiLabs_navX_FRC", - "version": "4.0.447", - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "mavenUrls": [ - "https://repo1.maven.org/maven2/" - ], - "jsonUrl": "https://www.kauailabs.com/dist/frc/2022/navx_frc.json", - "javaDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-java", - "version": "4.0.447" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-cpp", - "version": "4.0.447", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": false, - "libName": "navx_frc", - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxraspbian", - "windowsx86-64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..1219919 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.2.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.2.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.2.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.2.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.2.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.2.1" + } + ] +} \ No newline at end of file