diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml index 5ed5bfe..c75341d 100644 --- a/.github/workflows/gradle.yml +++ b/.github/workflows/gradle.yml @@ -28,7 +28,7 @@ jobs: - name: Setup run: | chmod +x gradlew # gradlew shell file run permission setup - git config --global --add safe.directory $GITHUB_WORKSPACE/y2025 # git safe directory setup + git config --global --add safe.directory $GITHUB_WORKSPACE # git safe directory setup #Clean cache - name: Clean Cache diff --git a/.gitignore b/.gitignore index 692f7d9..9a9ca7b 100644 --- a/.gitignore +++ b/.gitignore @@ -1,187 +1,187 @@ -# This gitignore has been specially created by the WPILib team. -# If you remove items from this file, intellisense might break. - -### C++ ### -# Prerequisites -*.d - -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app - -### Java ### -# Compiled class file -*.class - -# Log file -*.log - -# BlueJ files -*.ctxt - -# Mobile Tools for Java (J2ME) -.mtj.tmp/ - -# Package Files # -*.jar -*.war -*.nar -*.ear -*.zip -*.tar.gz -*.rar - -# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml -hs_err_pid* - -### Linux ### -*~ - -# temporary files which can be created if a process still has a handle open of a deleted file -.fuse_hidden* - -# KDE directory preferences -.directory - -# Linux trash folder which might appear on any partition or disk -.Trash-* - -# .nfs files are created when an open file is removed but is still being accessed -.nfs* - -### macOS ### -# General -.DS_Store -.AppleDouble -.LSOverride - -# Icon must end with two \r -Icon - -# Thumbnails -._* - -# Files that might appear in the root of a volume -.DocumentRevisions-V100 -.fseventsd -.Spotlight-V100 -.TemporaryItems -.Trashes -.VolumeIcon.icns -.com.apple.timemachine.donotpresent - -# Directories potentially created on remote AFP share -.AppleDB -.AppleDesktop -Network Trash Folder -Temporary Items -.apdisk - -### VisualStudioCode ### -.vscode/* -!.vscode/settings.json -!.vscode/tasks.json -!.vscode/launch.json -!.vscode/extensions.json - -### Windows ### -# Windows thumbnail cache files -Thumbs.db -ehthumbs.db -ehthumbs_vista.db - -# Dump file -*.stackdump - -# Folder config file -[Dd]esktop.ini - -# Recycle Bin used on file shares -$RECYCLE.BIN/ - -# Windows Installer files -*.cab -*.msi -*.msix -*.msm -*.msp - -# Windows shortcuts -*.lnk - -### Gradle ### -.gradle -/build/ - -# Ignore Gradle GUI config -gradle-app.setting - -# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) -!gradle-wrapper.jar - -# Cache of project -.gradletasknamecache - -# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 -# gradle/wrapper/gradle-wrapper.properties - -# # VS Code Specific Java Settings -# DO NOT REMOVE .classpath and .project -.classpath -.project -.settings/ -bin/ - -# IntelliJ -*.iml -*.ipr -*.iws -.idea/ -out/ - -# Fleet -.fleet - -# Simulation GUI and other tools window save file -networktables.json -simgui*.json -*-window.json - -# Simulation data log directory -logs/ - -# Folder that has CTRE Phoenix Sim device config storage -ctre_sim/ - -# clangd -/.cache -compile_commands.json - -# Eclipse generated file for annotation processors -.factorypath +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +# DO NOT REMOVE .classpath and .project +.classpath +.project +.settings/ +bin/ + +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + +# Simulation GUI and other tools window save file +networktables.json +simgui.json +*-window.json + +# Simulation data log directory +logs/ + +# Folder that has CTRE Phoenix Sim device config storage +ctre_sim/ + +# clangd +/.cache +compile_commands.json + +# Eclipse generated file for annotation processors +.factorypath diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..c9c9713 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,21 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + + { + "type": "wpilib", + "name": "WPILib Desktop Debug", + "request": "launch", + "desktop": true, + }, + { + "type": "wpilib", + "name": "WPILib roboRIO Debug", + "request": "launch", + "desktop": false, + } + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json index c5f3f6b..5e6ede8 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,3 +1,61 @@ { - "java.configuration.updateBuildConfiguration": "interactive" -} \ No newline at end of file + "java.configuration.updateBuildConfiguration": "automatic", + "java.server.launchMode": "Standard", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true, + "**/*~": true + }, + "java.test.config": [ + { + "name": "WPIlibUnitTests", + "workingDirectory": "${workspaceFolder}/build/jni/release", + "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], + "env": { + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } + }, + ], + "java.test.defaultConfig": "WPIlibUnitTests", + "java.import.gradle.annotationProcessing.enabled": false, + "java.completion.favoriteStaticMembers": [ + "org.junit.Assert.*", + "org.junit.Assume.*", + "org.junit.jupiter.api.Assertions.*", + "org.junit.jupiter.api.Assumptions.*", + "org.junit.jupiter.api.DynamicContainer.*", + "org.junit.jupiter.api.DynamicTest.*", + "org.mockito.Mockito.*", + "org.mockito.ArgumentMatchers.*", + "org.mockito.Answers.*", + "edu.wpi.first.units.Units.*" + ], + "java.completion.filteredTypes": [ + "java.awt.*", + "com.sun.*", + "sun.*", + "jdk.*", + "org.graalvm.*", + "io.micrometer.shaded.*", + "java.beans.*", + "java.util.Base64.*", + "java.util.Timer", + "java.sql.*", + "javax.swing.*", + "javax.management.*", + "javax.smartcardio.*", + "edu.wpi.first.math.proto.*", + "edu.wpi.first.math.**.proto.*", + "edu.wpi.first.math.**.struct.*", + ], + "java.dependency.enableDependencyCheckup": false +} diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 66f8f8c..7520a40 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2025", + "projectYear": "2026", "teamNumber": 9584 -} +} \ No newline at end of file diff --git a/LICENSE b/LICENSE index 261eeb9..db3c8be 100644 --- a/LICENSE +++ b/LICENSE @@ -1,201 +1,21 @@ - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. +MIT License + +Copyright (c) 2026 wcpllc + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/README.md b/README.md index a472d80..4a56a8b 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,5 @@ -9584 Java Robot Code for offseason 2025 -============================================================================ -This is the java code for 9584 -PLEASE CONTRIBUTE ACCORDING TO THE CONTRIBUTING.md +# 2026CompetitiveConcept -This will the frame code for on season 9584 robot -============================================================================ +This repository contains the code used for the WestCoast Products 2026 [Competitive Concept](https://wcproducts.com/pages/wcp-competitive-concepts). + +The project is based on one of CTRE's [Phoenix 6 example projects](https://github.com/CrossTheRoadElec/Phoenix6-Examples/tree/main/java/SwerveWithChoreo). It uses WPILib [command-based programming](https://docs.wpilib.org/en/stable/docs/software/commandbased/what-is-command-based.html) to manage robot subsystems and actions, a [Limelight](https://limelightvision.io/) for vision, and [Choreo](https://choreo.autos/) for autonomous path following. \ No newline at end of file diff --git a/WPILib-License.md b/WPILib-License.md index 645e542..eb3061b 100644 --- a/WPILib-License.md +++ b/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2024 FIRST and other WPILib contributors +Copyright (c) 2009-2026 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 471a256..e9b0020 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.3.2" + id "edu.wpi.first.GradleRIO" version "2026.1.1" } java { @@ -43,7 +43,8 @@ deploy { def deployArtifact = deploy.targets.roborio.artifacts.frcJava -// Set to true to use debug for JNI. +// Set to true to use debug for all targets including JNI, which will drastically impact +// performance. wpi.java.debugJni = false // Set this to true to enable desktop support. @@ -55,9 +56,6 @@ dependencies { annotationProcessor wpi.java.deps.wpilibAnnotations() implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() - implementation 'com.fasterxml.jackson.core:jackson-databind:2.15.0' - implementation 'com.fasterxml.jackson.core:jackson-core:2.15.0' - implementation 'com.fasterxml.jackson.core:jackson-annotations:2.15.0' roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) @@ -91,7 +89,9 @@ 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 + from('src') { into 'backup/src' } + from('vendordeps') { into 'backup/vendordeps' } + from('build.gradle') { into 'backup' } manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE } diff --git a/settings.gradle b/settings.gradle index c493958..25f6f6e 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2025' + String frcYear = '2026' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/src/main/deploy/9584-elastic-layout.json b/src/main/deploy/9584-elastic-layout.json deleted file mode 100644 index 9a197c8..0000000 --- a/src/main/deploy/9584-elastic-layout.json +++ /dev/null @@ -1,345 +0,0 @@ -{ - "version": 1.0, - "grid_size": 128, - "tabs": [ - { - "name": "9584", - "grid_layout": { - "layouts": [ - { - "title": "Tuning Constants", - "x": 1024.0, - "y": 0.0, - "width": 384.0, - "height": 640.0, - "type": "List Layout", - "properties": { - "label_position": "TOP" - }, - "children": [ - { - "title": "MaxSpeed", - "x": 0.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Number Slider", - "properties": { - "topic": "/Tuning Constants/MaxSpeed", - "period": 0.06, - "data_type": "double", - "min_value": 0.0, - "max_value": 7.0, - "divisions": 5, - "update_continuously": false - } - }, - { - "title": "MaxAngularRate", - "x": 0.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Number Slider", - "properties": { - "topic": "/Tuning Constants/MaxAngularRate", - "period": 0.06, - "data_type": "double", - "min_value": 0.0, - "max_value": 10.0, - "divisions": 5, - "update_continuously": false - } - }, - { - "title": "ControllerRotationCurveExponent", - "x": 0.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Number Slider", - "properties": { - "topic": "/Tuning Constants/ControllerRotationCurveExponent", - "period": 0.06, - "data_type": "double", - "min_value": 0.0, - "max_value": 5.0, - "divisions": 5, - "update_continuously": false - } - }, - { - "title": "ControllerVelocityCurveExponent", - "x": 0.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Number Slider", - "properties": { - "topic": "/Tuning Constants/ControllerVelocityCurveExponent", - "period": 0.06, - "data_type": "double", - "min_value": 0.0, - "max_value": 3.0, - "divisions": 5, - "update_continuously": false - } - }, - { - "title": "ControllerDeadbandPercentage", - "x": 0.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Number Slider", - "properties": { - "topic": "/Tuning Constants/ControllerDeadbandPercentage", - "period": 0.06, - "data_type": "double", - "min_value": 0.0, - "max_value": 0.1, - "divisions": 5, - "update_continuously": false - } - }, - { - "title": "SlewTranslateLimit", - "x": 0.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Number Slider", - "properties": { - "topic": "/Tuning Constants/SlewTranslateLimit", - "period": 0.06, - "data_type": "double", - "min_value": 0.0, - "max_value": 14.0, - "divisions": 5, - "update_continuously": false - } - }, - { - "title": "SlewRotateLimit", - "x": 0.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Number Slider", - "properties": { - "topic": "/Tuning Constants/SlewRotateLimit", - "period": 0.06, - "data_type": "double", - "min_value": 0.0, - "max_value": 30.0, - "divisions": 5, - "update_continuously": false - } - }, - { - "title": "ClimbButton", - "x": 0.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Tuning Constants/ClimbButton", - "period": 0.06, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "UnclimbButton", - "x": 0.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Tuning Constants/UnclimbButton", - "period": 0.06, - "data_type": "double", - "show_submit_button": true - } - } - ] - } - ], - "containers": [ - { - "title": "Pose", - "x": 0.0, - "y": 0.0, - "width": 768.0, - "height": 384.0, - "type": "Field", - "properties": { - "topic": "/Pose", - "period": 0.06, - "field_game": "Reefscape", - "robot_width": 0.85, - "robot_length": 0.85, - "show_other_objects": true, - "show_trajectories": true, - "field_rotation": 0.0, - "robot_color": 4294198070, - "trajectory_color": 4294967295 - } - }, - { - "title": "FMSInfo", - "x": 128.0, - "y": -128.0, - "width": 384.0, - "height": 128.0, - "type": "FMSInfo", - "properties": { - "topic": "/FMSInfo", - "period": 0.06 - } - }, - { - "title": "FMSInfo", - "x": 1024.0, - "y": 640.0, - "width": 384.0, - "height": 128.0, - "type": "FMSInfo", - "properties": { - "topic": "/FMSInfo", - "period": 0.06 - } - }, - { - "title": "CAN Utilization %", - "x": 0.0, - "y": 384.0, - "width": 256.0, - "height": 256.0, - "type": "Radial Gauge", - "properties": { - "topic": "/SmartDashboard/CAN Utilization %", - "period": 0.06, - "data_type": "double", - "start_angle": -140.0, - "end_angle": 140.0, - "min_value": 0.0, - "max_value": 100.0, - "number_of_labels": 8, - "wrap_value": false, - "show_pointer": true, - "show_ticks": true - } - }, - { - "title": "CPU Temperature", - "x": 256.0, - "y": 384.0, - "width": 256.0, - "height": 256.0, - "type": "Radial Gauge", - "properties": { - "topic": "/SmartDashboard/CPU Temperature", - "period": 0.06, - "data_type": "double", - "start_angle": -140.0, - "end_angle": 140.0, - "min_value": 0.0, - "max_value": 100.0, - "number_of_labels": 8, - "wrap_value": false, - "show_pointer": true, - "show_ticks": true - } - }, - { - "title": "Enabled", - "x": 512.0, - "y": 640.0, - "width": 128.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/SmartDashboard/RSL", - "period": 0.06, - "data_type": "boolean", - "true_color": 4294966555, - "false_color": 4278190080, - "true_icon": "Checkmark", - "false_icon": "X" - } - }, - { - "title": "Auto Mode", - "x": 0.0, - "y": 640.0, - "width": 256.0, - "height": 128.0, - "type": "ComboBox Chooser", - "properties": { - "topic": "/SmartDashboard/Auto Mode", - "period": 0.06, - "sort_options": false - } - }, - { - "title": "Alliance", - "x": 256.0, - "y": 640.0, - "width": 256.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/FMSInfo/IsRedAlliance", - "period": 0.06, - "data_type": "boolean", - "true_color": 4294901760, - "false_color": 4281745652, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Voltage", - "x": 512.0, - "y": 384.0, - "width": 256.0, - "height": 256.0, - "type": "Radial Gauge", - "properties": { - "topic": "/SmartDashboard/Voltage", - "period": 0.06, - "data_type": "double", - "start_angle": -140.0, - "end_angle": 140.0, - "min_value": 0.0, - "max_value": 100.0, - "number_of_labels": 8, - "wrap_value": false, - "show_pointer": true, - "show_ticks": true - } - }, - { - "title": "Match Time", - "x": 768.0, - "y": 0.0, - "width": 256.0, - "height": 128.0, - "type": "Match Time", - "properties": { - "topic": "/SmartDashboard/Match Time", - "period": 0.06, - "data_type": "double", - "time_display_mode": "Minutes and Seconds", - "red_start_time": 15, - "yellow_start_time": 30 - } - } - ] - } - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/ChoreoProject.chor b/src/main/deploy/choreo/ChoreoProject.chor new file mode 100644 index 0000000..8f19da9 --- /dev/null +++ b/src/main/deploy/choreo/ChoreoProject.chor @@ -0,0 +1,84 @@ +{ + "name":"ChoreoProject", + "version":2, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"10 in", + "val":0.254 + }, + "y":{ + "exp":"10 in", + "val":0.254 + } + }, + "backLeft":{ + "x":{ + "exp":"-10 in", + "val":-0.254 + }, + "y":{ + "exp":"10 in", + "val":0.254 + } + }, + "mass":{ + "exp":"137 lbs", + "val":62.142154690000005 + }, + "inertia":{ + "exp":"6 kg m ^ 2", + "val":6.0 + }, + "gearing":{ + "exp":"5.8909", + "val":5.8909 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"4800 RPM", + "val":502.6548245743669 + }, + "tmax":{ + "exp":"0.58 N * m", + "val":0.58 + }, + "cof":{ + "exp":"1.5", + "val":1.5 + }, + "bumper":{ + "front":{ + "exp":"16.875 in", + "val":0.428625 + }, + "side":{ + "exp":"17.25 in", + "val":0.43815 + }, + "back":{ + "exp":"16.875 in", + "val":0.428625 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + } + }, + "generationFeatures":[], + "codegen":{ + "root":"../../java/frc/robot/generated", + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/src/main/deploy/choreo/OutpostAndDepotTrajectory.traj b/src/main/deploy/choreo/OutpostAndDepotTrajectory.traj new file mode 100644 index 0000000..68bc05c --- /dev/null +++ b/src/main/deploy/choreo/OutpostAndDepotTrajectory.traj @@ -0,0 +1,438 @@ +{ + "name":"OutpostAndDepotTrajectory", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.5981619358062744, "y":0.6398493051528931, "heading":3.141592653589793, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.5814626336097718, "y":0.6689007878303528, "heading":3.141592653589793, "intervals":40, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.287879705429077, "y":3.77241849899292, "heading":3.141592653589793, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.8745373487472536, "y":5.963770389556885, "heading":3.141592653589793, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.668334896183014, "y":5.963770389556885, "heading":3.141592653589793, "intervals":19, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.2745373487472529, "y":5.963770389556885, "heading":3.141592653589793, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.915850114822387, "y":5.0424517631530765, "heading":0.0, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.4983891487121577, "y":4.035276412963867, "heading":0.0, "intervals":71, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.014005661010742, "y":3.743614673614502, "heading":3.141592653589793, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.9413638234138488, "y":3.5643614673614503, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":7, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":0.03658962249755848}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"KeepInLane", "props":{"tolerance":0.02120256423950222}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"MaxAngularVelocity", "props":{"max":0.08726646259971647}}, "enabled":true}, + {"from":7, "to":9, "data":{"type":"MaxVelocity", "props":{"max":0.6096}}, "enabled":true}, + {"from":8, "to":9, "data":{"type":"MaxAngularVelocity", "props":{"max":0.08726646259971647}}, "enabled":true}, + {"from":7, "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":2.1336}}, "enabled":true}, + {"from":7, "to":8, "data":{"type":"MaxAngularVelocity", "props":{"max":2.0943951023931953}}, "enabled":true}, + {"from":6, "to":7, "data":{"type":"MaxAngularVelocity", "props":{"max":0.08726646259971647}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":3.6576}}, "enabled":true}, + {"from":4, "to":7, "data":{"type":"MaxAcceleration", "props":{"max":2.7432000000000003}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.5981619358062744 m", "val":3.5981619358062744}, "y":{"exp":"0.6398493051528931 m", "val":0.6398493051528931}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.5814626336097718 m", "val":0.5814626336097718}, "y":{"exp":"0.6689007878303528 m", "val":0.6689007878303528}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.287879705429077 m", "val":2.287879705429077}, "y":{"exp":"3.77241849899292 m", "val":3.77241849899292}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.8745373487472534 m", "val":1.8745373487472536}, "y":{"exp":"5.963770389556885 m", "val":5.963770389556885}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"(0.693734896183014 m) - (1 in)", "val":0.668334896183014}, "y":{"exp":"5.963770389556885 m", "val":5.963770389556885}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":19, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.2745373487472529 m", "val":1.2745373487472529}, "y":{"exp":"5.963770389556885 m", "val":5.963770389556885}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.915850114822387 m", "val":1.915850114822387}, "y":{"exp":"5.0424517631530765 m", "val":5.0424517631530765}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.4983891487121577 m", "val":2.4983891487121577}, "y":{"exp":"4.035276412963867 m", "val":4.035276412963867}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":71, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.014005661010742 m", "val":2.014005661010742}, "y":{"exp":"3.743614673614502 m", "val":3.743614673614502}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.9413638234138489 m", "val":0.9413638234138488}, "y":{"exp":"3.5643614673614503 m", "val":3.5643614673614503}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":7, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"36.58962249755847 mm", "val":0.03658962249755848}}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"21.20256423950222 mm", "val":0.02120256423950222}}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"5 deg / s", "val":0.08726646259971647}}}, "enabled":true}, + {"from":7, "to":9, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 ft / s", "val":0.6096}}}, "enabled":true}, + {"from":8, "to":9, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"5 deg / s", "val":0.08726646259971647}}}, "enabled":true}, + {"from":7, "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"7 ft / s ^ 2", "val":2.1336}}}, "enabled":true}, + {"from":7, "to":8, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"120 deg / s", "val":2.0943951023931953}}}, "enabled":true}, + {"from":6, "to":7, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"5 deg / s", "val":0.08726646259971647}}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"12 ft / s ^ 2", "val":3.6576}}}, "enabled":true}, + {"from":4, "to":7, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"9 ft / s ^ 2", "val":2.7432000000000003}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.254, + "y":0.254 + }, + "backLeft":{ + "x":-0.254, + "y":0.254 + }, + "mass":62.142154690000005, + "inertia":6.0, + "gearing":5.8909, + "radius":0.0508, + "vmax":502.6548245743669, + "tmax":0.58, + "cof":1.5, + "bumper":{ + "front":0.428625, + "side":0.43815, + "back":0.428625 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.81742,3.26169,4.31343,5.21841,5.91821,6.85164,7.80119,9.42838,11.3563], + "samples":[ + {"t":0.0, "x":3.59816, "y":0.63985, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.65589, "ay":0.03521, "alpha":0.00018, "fx":[-56.79622,-56.79621,-56.79637,-56.79639], "fy":[0.54597,0.54796,0.54799,0.54596]}, + {"t":0.04912, "x":3.59375, "y":0.63989, "heading":3.14159, "vx":-0.17958, "vy":0.00173, "omega":0.00001, "ax":-3.65662, "ay":0.03521, "alpha":0.00017, "fx":[-56.80745,-56.80743,-56.80759,-56.8076], "fy":[0.54614,0.54798,0.54806,0.54612]}, + {"t":0.09824, "x":3.58052, "y":0.64002, "heading":-3.14159, "vx":-0.35919, "vy":0.00346, "omega":0.00002, "ax":-3.65657, "ay":0.03521, "alpha":0.00016, "fx":[-56.80664,-56.80663,-56.80679,-56.80684], "fy":[0.54701,0.54796,0.54795,0.54536]}, + {"t":0.14736, "x":3.55847, "y":0.64023, "heading":-3.14159, "vx":-0.5388, "vy":0.00519, "omega":0.00003, "ax":-3.65651, "ay":0.03521, "alpha":0.00017, "fx":[-56.80576,-56.80574,-56.80589,-56.80591], "fy":[0.54615,0.54801,0.54795,0.54614]}, + {"t":0.19648, "x":3.52759, "y":0.64053, "heading":-3.14159, "vx":-0.7184, "vy":0.00692, "omega":0.00003, "ax":-3.65644, "ay":0.03521, "alpha":0.00007, "fx":[-56.80471,-56.80471,-56.80486,-56.80489], "fy":[0.54677,0.54738,0.54737,0.5467]}, + {"t":0.2456, "x":3.48789, "y":0.64091, "heading":-3.14159, "vx":-0.89801, "vy":0.00865, "omega":0.00004, "ax":-3.65637, "ay":0.03521, "alpha":0.00016, "fx":[-56.80354,-56.80352,-56.80367,-56.80369], "fy":[0.54615,0.54803,0.54785,0.54614]}, + {"t":0.29472, "x":3.43937, "y":0.64138, "heading":-3.14159, "vx":-1.0776, "vy":0.01038, "omega":0.00005, "ax":-3.65628, "ay":0.03521, "alpha":0.00007, "fx":[-56.80214,-56.80213,-56.80228,-56.80232], "fy":[0.5469,0.54735,0.54734,0.54653]}, + {"t":0.34384, "x":3.38203, "y":0.64193, "heading":-3.14158, "vx":-1.2572, "vy":0.01211, "omega":0.00005, "ax":-3.65617, "ay":0.03521, "alpha":0.00016, "fx":[-56.80051,-56.80049,-56.80064,-56.80065], "fy":[0.54615,0.54806,0.54771,0.54615]}, + {"t":0.39296, "x":3.31586, "y":0.64257, "heading":-3.14158, "vx":-1.43679, "vy":0.01384, "omega":0.00006, "ax":-3.65604, "ay":0.03521, "alpha":0.00016, "fx":[-56.79852,-56.79851,-56.79865,-56.79872], "fy":[0.54729,0.54786,0.54786,0.54498]}, + {"t":0.44208, "x":3.24088, "y":0.64329, "heading":-3.14158, "vx":-1.61637, "vy":0.01557, "omega":0.00006, "ax":-3.65589, "ay":0.03521, "alpha":0.00012, "fx":[-56.79612,-56.79611,-56.79625,-56.79626], "fy":[0.54635,0.54762,0.5476,0.54634]}, + {"t":0.4912, "x":3.15707, "y":0.6441, "heading":-3.14158, "vx":-1.79595, "vy":0.0173, "omega":0.00007, "ax":-3.65569, "ay":0.03521, "alpha":0.00014, "fx":[-56.79307,-56.79307,-56.79321,-56.79329], "fy":[0.54703,0.54772,0.54771,0.54534]}, + {"t":0.54032, "x":3.06445, "y":0.64499, "heading":-3.14157, "vx":-1.97551, "vy":0.01903, "omega":0.00008, "ax":-3.65544, "ay":0.0352, "alpha":0.00018, "fx":[-56.7892,-56.78918,-56.78933,-56.78934], "fy":[0.54593,0.54828,0.54753,0.54592]}, + {"t":0.58943, "x":2.963, "y":0.64597, "heading":-3.14157, "vx":-2.15507, "vy":0.02075, "omega":0.00009, "ax":-3.65511, "ay":0.0352, "alpha":0.00019, "fx":[-56.78395,-56.78395,-56.78409,-56.7842], "fy":[0.5471,0.5479,0.54788,0.54458]}, + {"t":0.63855, "x":2.85273, "y":0.64703, "heading":-3.14156, "vx":-2.33461, "vy":0.02248, "omega":0.0001, "ax":-3.65463, "ay":0.0352, "alpha":0.00021, "fx":[-56.77665,-56.77663,-56.77678,-56.7768], "fy":[0.54567,0.54848,0.5474,0.54566]}, + {"t":0.68767, "x":2.73365, "y":0.64817, "heading":-3.14156, "vx":-2.51412, "vy":0.02421, "omega":0.00011, "ax":-3.65392, "ay":0.03519, "alpha":0.00017, "fx":[-56.76553,-56.76553,-56.76568,-56.76586], "fy":[0.5471,0.5476,0.54757,0.54454]}, + {"t":0.73679, "x":2.60575, "y":0.64941, "heading":-3.14155, "vx":-2.6936, "vy":0.02594, "omega":0.00011, "ax":-3.65272, "ay":0.03518, "alpha":-0.0002, "fx":[-56.74694,-56.74697,-56.7471,-56.74708], "fy":[0.54778,0.54457,0.54603,0.54777]}, + {"t":0.78591, "x":2.46904, "y":0.65072, "heading":-3.14155, "vx":-2.87302, "vy":0.02767, "omega":0.0001, "ax":-3.65028, "ay":0.03516, "alpha":-0.00011, "fx":[-56.70868,-56.70869,-56.70886,-56.71009], "fy":[0.54581,0.54513,0.54514,0.54872]}, + {"t":0.83503, "x":2.32351, "y":0.65212, "heading":-3.14154, "vx":-3.05232, "vy":0.0294, "omega":0.0001, "ax":-3.64259, "ay":0.03509, "alpha":-0.00013, "fx":[-56.58943,-56.58941,-56.58965,-56.5896], "fy":[0.54602,0.54807,0.5404,0.54599]}, + {"t":0.88415, "x":2.16919, "y":0.65361, "heading":-3.14154, "vx":-3.23124, "vy":0.03112, "omega":0.00009, "ax":-0.00077, "ay":-0.00006, "alpha":-0.00317, "fx":[-0.02327,-0.02332,-0.00589,0.00476], "fy":[0.00372,-0.00834,-0.00839,0.00905]}, + {"t":0.93327, "x":2.01047, "y":0.65514, "heading":-3.14153, "vx":-3.23128, "vy":0.03112, "omega":-0.00006, "ax":3.64259, "ay":-0.03507, "alpha":-0.0006, "fx":[56.58964,56.58955,56.58942,56.58948], "fy":[-0.54113,-0.54989,-0.54692,-0.54114]}, + {"t":0.98239, "x":1.85614, "y":0.65663, "heading":-3.14154, "vx":-3.05236, "vy":0.02939, "omega":-0.00009, "ax":3.65028, "ay":-0.03515, "alpha":-0.00015, "fx":[56.70884,56.70883,56.70869,56.70996], "fy":[-0.5452,-0.54672,-0.54669,-0.54556]}, + {"t":1.03151, "x":1.71062, "y":0.65803, "heading":-3.14154, "vx":-2.87306, "vy":0.02767, "omega":-0.0001, "ax":3.65272, "ay":-0.03517, "alpha":0.00007, "fx":[56.74709,56.74711,56.74694,56.74695], "fy":[-0.54679,-0.54435,-0.54782,-0.54679]}, + {"t":1.08063, "x":1.5739, "y":0.65934, "heading":-3.14155, "vx":-2.69364, "vy":0.02594, "omega":-0.0001, "ax":3.65392, "ay":-0.03519, "alpha":0.00007, "fx":[56.76566,56.76568,56.76555,56.76571], "fy":[-0.54823,-0.54622,-0.54622,-0.54584]}, + {"t":1.12975, "x":1.446, "y":0.66058, "heading":-3.14155, "vx":-2.51416, "vy":0.02421, "omega":-0.00009, "ax":3.65463, "ay":-0.03519, "alpha":0.00004, "fx":[56.77677,56.77677,56.77666,56.77666], "fy":[-0.5469,-0.54722,-0.54595,-0.5469]}, + {"t":1.17887, "x":1.32691, "y":0.66172, "heading":-3.14156, "vx":-2.33464, "vy":0.02248, "omega":-0.00009, "ax":3.65511, "ay":-0.0352, "alpha":0.00011, "fx":[56.78407,56.78409,56.78398,56.78405], "fy":[-0.54804,-0.54621,-0.5462,-0.54683]}, + {"t":1.22799, "x":1.21665, "y":0.66278, "heading":-3.14156, "vx":-2.15511, "vy":0.02075, "omega":-0.00009, "ax":3.65544, "ay":-0.0352, "alpha":0.00005, "fx":[56.78931,56.78931,56.78922,56.78921], "fy":[-0.54711,-0.5471,-0.54617,-0.54711]}, + {"t":1.27711, "x":1.1152, "y":0.66376, "heading":-3.14157, "vx":-1.97555, "vy":0.01902, "omega":-0.00008, "ax":3.65569, "ay":-0.0352, "alpha":0.00011, "fx":[56.79318,56.7932,56.7931,56.79316], "fy":[-0.54832,-0.54627,-0.54627,-0.54679]}, + {"t":1.32623, "x":1.02257, "y":0.66465, "heading":-3.14157, "vx":-1.79599, "vy":0.0173, "omega":-0.00008, "ax":3.65589, "ay":-0.03521, "alpha":0.00014, "fx":[56.79622,56.79624,56.79615,56.79613], "fy":[-0.54772,-0.54626,-0.54609,-0.54772]}, + {"t":1.37535, "x":0.93876, "y":0.66546, "heading":-3.14157, "vx":-1.61641, "vy":0.01557, "omega":-0.00007, "ax":3.65604, "ay":-0.03521, "alpha":0.0001, "fx":[56.79862,56.79864,56.79855,56.7986], "fy":[-0.54861,-0.54641,-0.5464,-0.54645]}, + {"t":1.42447, "x":0.86377, "y":0.66618, "heading":-3.14158, "vx":-1.43683, "vy":0.01384, "omega":-0.00007, "ax":3.65617, "ay":-0.03521, "alpha":0.00011, "fx":[56.80061,56.80062,56.80054,56.80053], "fy":[-0.54759,-0.54661,-0.54617,-0.54759]}, + {"t":1.47359, "x":0.79761, "y":0.66682, "heading":-3.14158, "vx":-1.25724, "vy":0.01211, "omega":-0.00006, "ax":3.65628, "ay":-0.03521, "alpha":0.00021, "fx":[56.80223,56.80225,56.80218,56.8022], "fy":[-0.54835,-0.54582,-0.54582,-0.54803]}, + {"t":1.52271, "x":0.74027, "y":0.66737, "heading":-3.14158, "vx":-1.07764, "vy":0.01038, "omega":-0.00005, "ax":3.65637, "ay":-0.03521, "alpha":0.00012, "fx":[56.80363,56.80364,56.80358,56.80357], "fy":[-0.54768,-0.54647,-0.54625,-0.54768]}, + {"t":1.57183, "x":0.69174, "y":0.66784, "heading":-3.14159, "vx":-0.89804, "vy":0.00865, "omega":-0.00005, "ax":3.65644, "ay":-0.03521, "alpha":0.00023, "fx":[56.80481,56.80483,56.80477,56.80477], "fy":[-0.54828,-0.54567,-0.54567,-0.54851]}, + {"t":1.62095, "x":0.65204, "y":0.66822, "heading":-3.14159, "vx":-0.71844, "vy":0.00692, "omega":-0.00003, "ax":3.65651, "ay":-0.03521, "alpha":0.00013, "fx":[56.80584,56.80586,56.8058,56.80579], "fy":[-0.54777,-0.54635,-0.54629,-0.54777]}, + {"t":1.67007, "x":0.62116, "y":0.66852, "heading":-3.14159, "vx":-0.53883, "vy":0.00519, "omega":-0.00003, "ax":3.65657, "ay":-0.03521, "alpha":0.00021, "fx":[56.80673,56.80676,56.8067,56.80671], "fy":[-0.54847,-0.54583,-0.54584,-0.54808]}, + {"t":1.71919, "x":0.59911, "y":0.66873, "heading":-3.14159, "vx":-0.35923, "vy":0.00346, "omega":-0.00002, "ax":3.65662, "ay":-0.03521, "alpha":0.00014, "fx":[56.80753,56.80755,56.8075,56.80749], "fy":[-0.54785,-0.5462,-0.54634,-0.54786]}, + {"t":1.7683, "x":0.58587, "y":0.66886, "heading":-3.14159, "vx":-0.17961, "vy":0.00173, "omega":-0.00001, "ax":3.65666, "ay":-0.03521, "alpha":0.00023, "fx":[56.80822,56.80825,56.8082,56.80821], "fy":[-0.54864,-0.54574,-0.54576,-0.54812]}, + {"t":1.81742, "x":0.58146, "y":0.6689, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.17332, "ay":2.93931, "alpha":0.00146, "fx":[33.76621,33.78198,33.74951,33.75735], "fy":[45.66201,45.64979,45.67438,45.6691]}, + {"t":1.85353, "x":0.58288, "y":0.67082, "heading":3.14159, "vx":0.07847, "vy":0.10613, "omega":0.00005, "ax":2.16674, "ay":2.94542, "alpha":0.00127, "fx":[33.67635,33.65836,33.65908,33.65181], "fy":[45.74779,45.76028,45.76014,45.76634]}, + {"t":1.88964, "x":0.58713, "y":0.67657, "heading":-3.14159, "vx":0.15671, "vy":0.21248, "omega":0.0001, "ax":2.15904, "ay":2.95102, "alpha":0.0016, "fx":[33.53661,33.57967,33.52216,33.52867], "fy":[45.84947,45.81754,45.86007,45.85573]}, + {"t":1.92574, "x":0.59419, "y":0.68616, "heading":-3.14159, "vx":0.23466, "vy":0.31903, "omega":0.00016, "ax":2.15079, "ay":2.95699, "alpha":0.00167, "fx":[33.43835,33.40182,33.4149,33.39952], "fy":[45.92068,45.94648,45.93729,45.9493]}, + {"t":1.96185, "x":0.60407, "y":0.69961, "heading":-3.14158, "vx":0.31232, "vy":0.4258, "omega":0.00022, "ax":2.14193, "ay":2.96336, "alpha":0.00147, "fx":[33.27365,33.30544,33.2595,33.26563], "fy":[46.03914,46.0158,46.0494,46.04531]}, + {"t":1.99796, "x":0.61674, "y":0.71692, "heading":-3.14157, "vx":0.38966, "vy":0.53279, "omega":0.00027, "ax":2.1324, "ay":2.97017, "alpha":0.00205, "fx":[33.14434,33.12364,33.11457,33.1292], "fy":[46.13169,46.14597,46.15272,46.14267]}, + {"t":2.03406, "x":0.6322, "y":0.73809, "heading":-3.14156, "vx":0.46665, "vy":0.64004, "omega":0.00034, "ax":2.1221, "ay":2.97748, "alpha":0.00138, "fx":[32.96861,32.98906,32.95419,32.96028], "fy":[46.25629,46.2415,46.26663,46.26253]}, + {"t":2.07017, "x":0.65043, "y":0.76314, "heading":-3.14155, "vx":0.54327, "vy":0.74754, "omega":0.00039, "ax":2.11096, "ay":2.98533, "alpha":0.00027, "fx":[32.79015,32.79144,32.78211,32.81586], "fy":[46.38211,46.38096,46.38774,46.36389]}, + {"t":2.10628, "x":0.67142, "y":0.79208, "heading":-3.14154, "vx":0.61949, "vy":0.85533, "omega":0.0004, "ax":2.09885, "ay":2.99378, "alpha":0.00073, "fx":[32.62221,32.58464,32.60681,32.61357], "fy":[46.49922,46.52533,46.5101,46.50551]}, + {"t":2.14238, "x":0.69516, "y":0.82491, "heading":-3.14152, "vx":0.69527, "vy":0.96343, "omega":0.00043, "ax":2.08565, "ay":3.00292, "alpha":-0.0025, "fx":[32.36825,32.40783,32.39899,32.43202], "fy":[46.67514,46.64781,46.65409,46.63083]}, + {"t":2.17849, "x":0.72162, "y":0.86166, "heading":-3.14151, "vx":0.77058, "vy":1.07185, "omega":0.00034, "ax":2.07121, "ay":3.01282, "alpha":0.00061, "fx":[32.19423,32.15091,32.17917,32.18534], "fy":[46.79415,46.82384,46.8046,46.80042]}, + {"t":2.2146, "x":0.75079, "y":0.90232, "heading":-3.14149, "vx":0.84537, "vy":1.18064, "omega":0.00036, "ax":2.05534, "ay":3.02358, "alpha":-0.00233, "fx":[31.89898,31.93629,31.92751,31.96071], "fy":[46.99452,46.96937,46.97541,46.95241]}, + {"t":2.2507, "x":0.78266, "y":0.94692, "heading":-3.14148, "vx":0.91958, "vy":1.28981, "omega":0.00028, "ax":2.03783, "ay":3.03532, "alpha":0.00059, "fx":[31.67504,31.63345,31.6603,31.66616], "fy":[47.14427,47.17226,47.15429,47.15031]}, + {"t":2.28681, "x":0.81719, "y":0.99547, "heading":-3.14147, "vx":0.99316, "vy":1.3994, "omega":0.0003, "ax":2.01839, "ay":3.04817, "alpha":-0.00185, "fx":[31.33199,31.36614,31.35739,31.37183], "fy":[47.37134,47.34891,47.3547,47.3448]}, + {"t":2.32292, "x":0.85436, "y":1.04798, "heading":-3.14146, "vx":1.06603, "vy":1.50946, "omega":0.00023, "ax":1.99672, "ay":3.0623, "alpha":0.00084, "fx":[31.03148,31.00937,31.0169,31.02251], "fy":[47.56686,47.58157,47.57651,47.5727]}, + {"t":2.35902, "x":0.89416, "y":1.10448, "heading":-3.14145, "vx":1.13813, "vy":1.62003, "omega":0.00026, "ax":1.97239, "ay":3.07789, "alpha":-0.00643, "fx":[30.56528,30.65959,30.65201,30.69148], "fy":[47.86544,47.80597,47.81081,47.78466]}, + {"t":2.39513, "x":0.93654, "y":1.16498, "heading":-3.14144, "vx":1.20934, "vy":1.73116, "omega":0.00003, "ax":1.94489, "ay":3.09519, "alpha":0.00037, "fx":[30.23334,30.1832,30.219,30.22434], "fy":[48.07385,48.1057,48.08303,48.07944]}, + {"t":2.43124, "x":0.98147, "y":1.22951, "heading":-3.14144, "vx":1.27957, "vy":1.84292, "omega":0.00004, "ax":1.91359, "ay":3.11448, "alpha":-0.00153, "fx":[29.70479,29.72803,29.71986,29.76172], "fy":[48.39986,48.38588,48.39077,48.3642]}, + {"t":2.46734, "x":1.02892, "y":1.29808, "heading":-3.14144, "vx":1.34866, "vy":1.95538, "omega":-0.00001, "ax":1.87763, "ay":3.13611, "alpha":0.00054, "fx":[29.18434,29.14986,29.17042,29.17536], "fy":[48.71238,48.73353,48.72092,48.71764]}, + {"t":2.50345, "x":1.07884, "y":1.37073, "heading":-3.14144, "vx":1.41646, "vy":2.06861, "omega":0.00001, "ax":1.83592, "ay":3.16049, "alpha":-0.00203, "fx":[28.49313,28.5247,28.51661,28.55388], "fy":[49.11684,49.09884,49.10337,49.08075]}, + {"t":2.53956, "x":1.13118, "y":1.44748, "heading":-3.14144, "vx":1.48275, "vy":2.18272, "omega":-0.00007, "ax":1.78701, "ay":3.18816, "alpha":0.00038, "fx":[27.77754,27.73855,27.76412,27.76844], "fy":[49.52101,49.54355,49.52876,49.52595]}, + {"t":2.57566, "x":1.18588, "y":1.52836, "heading":-3.14144, "vx":1.54727, "vy":2.29784, "omega":-0.00005, "ax":1.72889, "ay":3.21977, "alpha":-0.00228, "fx":[26.82759,26.86296,26.85485,26.89172], "fy":[50.03795,50.0193,50.02345,50.00252]}, + {"t":2.61177, "x":1.24287, "y":1.61343, "heading":-3.14144, "vx":1.60969, "vy":2.41409, "omega":-0.00013, "ax":1.6588, "ay":3.2561, "alpha":0.00024, "fx":[25.78603,25.74569,25.77286,25.77688], "fy":[50.57707,50.59832,50.58405,50.58154]}, + {"t":2.64788, "x":1.30207, "y":1.70272, "heading":-3.14145, "vx":1.66959, "vy":2.53166, "omega":-0.00013, "ax":1.57277, "ay":3.29812, "alpha":-0.00045, "fx":[24.42435,24.42013,24.41154,24.47912], "fy":[51.24264,51.24498,51.24885,51.21567]}, + {"t":2.68398, "x":1.36338, "y":1.79628, "heading":-3.14145, "vx":1.72637, "vy":2.65074, "omega":-0.00014, "ax":1.46496, "ay":3.34692, "alpha":0.00013, "fx":[22.77427,22.73487,22.76143,22.76504], "fy":[51.98916,52.00734,51.9951,51.993]}, + {"t":2.72009, "x":1.42667, "y":1.89417, "heading":-3.14146, "vx":1.77927, "vy":2.77159, "omega":-0.00014, "ax":1.32648, "ay":3.40358, "alpha":-0.0025, "fx":[20.57341,20.60961,20.60016,20.64727], "fy":[52.8896,52.87601,52.8795,52.86057]}, + {"t":2.7562, "x":1.49178, "y":1.99646, "heading":-3.14146, "vx":1.82716, "vy":2.89448, "omega":-0.00023, "ax":1.14333, "ay":3.46864, "alpha":-0.00029, "fx":[17.77885,17.73351,17.76705,17.76976], "fy":[53.88143,53.89754,53.8857,53.88426]}, + {"t":2.7923, "x":1.5585, "y":2.10323, "heading":-3.14147, "vx":1.86845, "vy":3.01972, "omega":-0.00024, "ax":0.89259, "ay":3.5405, "alpha":-0.00558, "fx":[13.82413,13.81602,13.80875,14.01882], "fy":[55.01466,55.01657,55.01828,54.96453]}, + {"t":2.82841, "x":1.62654, "y":2.21457, "heading":-3.14148, "vx":1.90067, "vy":3.14756, "omega":-0.00044, "ax":0.5359, "ay":3.61046, "alpha":0.00071, "fx":[8.32984,8.32868,8.32128,8.32223], "fy":[56.0894,56.09066,56.09107,56.09046]}, + {"t":2.86452, "x":1.69552, "y":2.33057, "heading":-3.1415, "vx":1.92002, "vy":3.27792, "omega":-0.00041, "ax":0.00994, "ay":3.64828, "alpha":-0.01919, "fx":[0.03698,0.04497,0.04544,0.49015], "fy":[56.67907,56.67803,56.67803,56.67678]}, + {"t":2.90062, "x":1.76485, "y":2.45131, "heading":-3.14151, "vx":1.92038, "vy":3.40965, "omega":-0.00111, "ax":-0.77227, "ay":3.56334, "alpha":0.00453, "fx":[-12.02039,-11.93277,-12.02038,-12.01692], "fy":[55.3524,55.37626,55.35209,55.35312]}, + {"t":2.93673, "x":1.83369, "y":2.57674, "heading":-3.14155, "vx":1.8925, "vy":3.53831, "omega":-0.00094, "ax":-1.838, "ay":3.14629, "alpha":0.01165, "fx":[-28.35885,-28.58281,-28.57217,-28.70345], "fy":[49.00029,48.86189,48.86738,48.78773]}, + {"t":2.97284, "x":1.90082, "y":2.70655, "heading":-3.14159, "vx":1.82613, "vy":3.65191, "omega":-0.00052, "ax":-2.90623, "ay":2.1967, "alpha":0.0216, "fx":[-45.20783,-44.97809,-45.21701,-45.19669], "fy":[34.05018,34.36213,34.03294,34.06244]}, + {"t":3.00894, "x":1.96486, "y":2.83984, "heading":3.14158, "vx":1.7212, "vy":3.73123, "omega":0.00026, "ax":-3.5097, "ay":0.98205, "alpha":-0.00229, "fx":[-54.48636,-54.53545,-54.53145,-54.54734], "fy":[15.39844,15.22517,15.23277,15.17056]}, + {"t":3.04505, "x":2.02472, "y":2.9752, "heading":3.14159, "vx":1.59448, "vy":3.76668, "omega":0.00018, "ax":-3.64679, "ay":-0.02189, "alpha":-0.00791, "fx":[-56.65586,-56.65501,-56.65422,-56.65434], "fy":[-0.29471,-0.44841,-0.32412,-0.2933]}, + {"t":3.08116, "x":2.07991, "y":3.11119, "heading":-3.14159, "vx":1.4628, "vy":3.76589, "omega":-0.00011, "ax":-3.58014, "ay":-0.70509, "alpha":0.00829, "fx":[-55.58837,-55.62804,-55.62572,-55.63556], "fy":[-11.11856,-10.91411,-10.91819,-10.86465]}, + {"t":3.11726, "x":2.1304, "y":3.2467, "heading":3.14159, "vx":1.33354, "vy":3.74044, "omega":0.00019, "ax":-3.46284, "ay":-1.15526, "alpha":-0.0101, "fx":[-53.82455,-53.73173,-53.8113,-53.82063], "fy":[-17.86537,-18.14956,-17.90273,-17.87271]}, + {"t":3.15337, "x":2.17629, "y":3.381, "heading":-3.14159, "vx":1.2085, "vy":3.69872, "omega":-0.00018, "ax":-3.34696, "ay":-1.46018, "alpha":0.00815, "fx":[-51.93718,-52.0112,-52.00558,-52.03305], "fy":[-22.82397,-22.65418,-22.66392,-22.59639]}, + {"t":3.18948, "x":2.21774, "y":3.5136, "heading":-3.14159, "vx":1.08766, "vy":3.646, "omega":0.00012, "ax":-3.24558, "ay":-1.67532, "alpha":-0.00662, "fx":[-50.45468,-50.35063,-50.43446,-50.44745], "fy":[-25.96309,-26.16898,-26.00142,-25.97443]}, + {"t":3.22558, "x":2.2549, "y":3.64415, "heading":-3.14159, "vx":0.97047, "vy":3.58551, "omega":-0.00012, "ax":-3.15985, "ay":-1.8332, "alpha":0.00544, "fx":[-49.04386,-49.10398,-49.09562,-49.11625], "fy":[-28.56104,-28.4576,-28.46971,-28.43069]}, + {"t":3.26169, "x":2.28788, "y":3.77242, "heading":3.14159, "vx":0.85638, "vy":3.51932, "omega":0.00008, "ax":-3.05832, "ay":-1.9899, "alpha":-0.00359, "fx":[-47.53296,-47.48721,-47.50762,-47.52258], "fy":[-30.88339,-30.95369,-30.92206,-30.89743]}, + {"t":3.29456, "x":2.31437, "y":3.88701, "heading":-3.14159, "vx":0.75586, "vy":3.45392, "omega":-0.00004, "ax":-2.94809, "ay":-2.15793, "alpha":0.00523, "fx":[-45.74913,-45.81523,-45.80552,-45.83091], "fy":[-33.59564,-33.50601,-33.51753,-33.47939]}, + {"t":3.32742, "x":2.33762, "y":3.99937, "heading":-3.14159, "vx":0.65897, "vy":3.38299, "omega":0.00013, "ax":-2.84277, "ay":-2.29537, "alpha":-0.0033, "fx":[-44.19417,-44.11072,-44.16873,-44.18241], "fy":[-35.62233,-35.72796,-35.65384,-35.63528]}, + {"t":3.36029, "x":2.35775, "y":4.10932, "heading":-3.14159, "vx":0.56553, "vy":3.30755, "omega":0.00002, "ax":-2.74633, "ay":-2.41033, "alpha":0.00263, "fx":[-42.62839,-42.67299,-42.66127,-42.70037], "fy":[-37.4884,-37.43938,-37.45115,-37.40407]}, + {"t":3.39316, "x":2.37485, "y":4.21672, "heading":-3.14159, "vx":0.47527, "vy":3.22833, "omega":0.00011, "ax":-2.65845, "ay":-2.50727, "alpha":-0.00262, "fx":[-41.32877,-41.25542,-41.30208,-41.31527], "fy":[-38.92164,-39.00101,-38.95011,-38.93449]}, + {"t":3.42603, "x":2.38904, "y":4.32147, "heading":-3.14158, "vx":0.38789, "vy":3.14592, "omega":0.00002, "ax":-2.57851, "ay":-2.58971, "alpha":0.00204, "fx":[-40.02335,-40.06259,-40.04971,-40.09827], "fy":[-40.26716,-40.23024,-40.2416,-40.19093]}, + {"t":3.45889, "x":2.40039, "y":4.42347, "heading":-3.14158, "vx":0.30315, "vy":3.06081, "omega":0.00009, "ax":-2.50581, "ay":-2.66037, "alpha":-0.00222, "fx":[-38.95604,-38.88958,-38.92915,-38.94148], "fy":[-41.30478,-41.36883,-41.3304,-41.31712]}, + {"t":3.49176, "x":2.409, "y":4.52263, "heading":-3.14158, "vx":0.22079, "vy":2.97337, "omega":0.00002, "ax":-2.43963, "ay":-2.72141, "alpha":0.00184, "fx":[-37.87498,-37.91613,-37.90011,-37.91243], "fy":[-42.30139,-42.26658,-42.27956,-42.2669]}, + {"t":3.52463, "x":2.41494, "y":4.61889, "heading":-3.14158, "vx":0.14061, "vy":2.88393, "omega":0.00008, "ax":-2.37928, "ay":-2.77453, "alpha":-0.00192, "fx":[-36.98889,-36.9283,-36.96249,-36.97385], "fy":[-43.08184,-43.13495,-43.10486,-43.09341]}, + {"t":3.55749, "x":2.41828, "y":4.71218, "heading":-3.14158, "vx":0.06241, "vy":2.79274, "omega":0.00001, "ax":-2.32413, "ay":-2.82106, "alpha":0.00061, "fx":[-36.0911,-36.11453,-36.10042,-36.1207], "fy":[-43.83878,-43.82179,-43.83206,-43.8139]}, + {"t":3.59036, "x":2.41907, "y":4.80244, "heading":-3.14158, "vx":-0.01398, "vy":2.70002, "omega":0.00003, "ax":-2.27363, "ay":-2.86208, "alpha":-0.00188, "fx":[-35.34237,-35.3023,-35.3164,-35.32715], "fy":[-44.44765,-44.48072,-44.46874,-44.45843]}, + {"t":3.62323, "x":2.41739, "y":4.88964, "heading":-3.14157, "vx":-0.08871, "vy":2.60595, "omega":-0.00003, "ax":-2.22726, "ay":-2.89845, "alpha":0.00069, "fx":[-34.58475,-34.60954,-34.5953,-34.61684], "fy":[-45.04124,-45.02457,-45.03419,-45.01605]}, + {"t":3.65609, "x":2.41327, "y":4.97372, "heading":-3.14158, "vx":-0.16191, "vy":2.51069, "omega":-0.00001, "ax":-2.18457, "ay":-2.93089, "alpha":-0.00177, "fx":[-33.95711,-33.92271,-33.93209,-33.94196], "fy":[-45.51887,-45.54581,-45.53805,-45.52886]}, + {"t":3.68896, "x":2.40677, "y":5.05466, "heading":-3.14158, "vx":-0.23371, "vy":2.41436, "omega":-0.00006, "ax":-2.14518, "ay":-2.95995, "alpha":0.00033, "fx":[-33.31039,-33.32716,-33.31294,-33.35574], "fy":[-45.9952,-45.98582,-45.99481,-45.96203]}, + {"t":3.72183, "x":2.39793, "y":5.13241, "heading":-3.14158, "vx":-0.30422, "vy":2.31707, "omega":-0.00005, "ax":-2.10875, "ay":-2.98612, "alpha":-0.00135, "fx":[-32.78482,-32.72692,-32.76056,-32.76992], "fy":[-46.37379,-46.41584,-46.39154,-46.38301]}, + {"t":3.75469, "x":2.38679, "y":5.20695, "heading":-3.14158, "vx":-0.37353, "vy":2.21893, "omega":-0.0001, "ax":-2.07497, "ay":-3.00979, "alpha":0.00365, "fx":[-32.18548,-32.24789,-32.2345,-32.27513], "fy":[-46.79213,-46.75229,-46.76026,-46.7303]}, + {"t":3.78756, "x":2.37339, "y":5.27826, "heading":-3.14158, "vx":-0.44172, "vy":2.12001, "omega":0.00002, "ax":-2.04358, "ay":-3.03128, "alpha":-0.00126, "fx":[-31.77043,-31.71857,-31.74735,-31.75595], "fy":[-47.07736,-47.11366,-47.0936,-47.08581]}, + {"t":3.82043, "x":2.35777, "y":5.3463, "heading":-3.14158, "vx":-0.50889, "vy":2.02038, "omega":-0.00002, "ax":-2.01434, "ay":-3.05087, "alpha":0.00219, "fx":[-31.2577,-31.29692,-31.28377,-31.337], "fy":[-47.41963,-47.39683,-47.40423,-47.36704]}, + {"t":3.8533, "x":2.33996, "y":5.41105, "heading":-3.14158, "vx":-0.57509, "vy":1.9201, "omega":0.00005, "ax":-1.98705, "ay":-3.06879, "alpha":-0.00125, "fx":[-30.8889,-30.84856,-30.86737,-30.87503], "fy":[-47.6628,-47.69036,-47.67747,-47.67045]}, + {"t":3.88616, "x":2.31998, "y":5.4725, "heading":-3.14158, "vx":-0.6404, "vy":1.81924, "omega":0.00001, "ax":-1.96154, "ay":-3.08523, "alpha":0.0014, "fx":[-30.44773,-30.47822,-30.46559,-30.50249], "fy":[-47.94607,-47.92963,-47.93637,-47.91087]}, + {"t":3.91903, "x":2.29787, "y":5.53063, "heading":-3.14158, "vx":-0.70487, "vy":1.71784, "omega":0.00006, "ax":-1.93763, "ay":-3.10037, "alpha":-0.0011, "fx":[-30.12025,-30.08057,-30.10032,-30.10715], "fy":[-48.15428,-48.18061,-48.16752,-48.16113]}, + {"t":3.9519, "x":2.27366, "y":5.58542, "heading":-3.14158, "vx":-0.76856, "vy":1.61594, "omega":0.00002, "ax":-1.91518, "ay":-3.11434, "alpha":0.00162, "fx":[-29.72563,-29.75843,-29.7467,-29.78279], "fy":[-48.39893,-48.38181,-48.38774,-48.3635]}, + {"t":3.98476, "x":2.24737, "y":5.63685, "heading":-3.14158, "vx":-0.8315, "vy":1.51358, "omega":0.00007, "ax":-1.89408, "ay":-3.12728, "alpha":-0.00099, "fx":[-29.4424,-29.40534,-29.42416,-29.43022], "fy":[-48.57347,-48.59755,-48.58535,-48.5795]}, + {"t":4.01763, "x":2.21901, "y":5.6849, "heading":-3.14158, "vx":-0.89375, "vy":1.4108, "omega":0.00004, "ax":-1.8742, "ay":-3.13929, "alpha":0.00173, "fx":[-29.08825,-29.12172,-29.11102,-29.14576], "fy":[-48.78629,-48.76946,-48.77456,-48.75172]}, + {"t":4.0505, "x":2.18863, "y":5.72958, "heading":-3.14157, "vx":-0.95535, "vy":1.30762, "omega":0.0001, "ax":-1.85545, "ay":-3.15046, "alpha":-0.0009, "fx":[-28.84035,-28.80796,-28.82391,-28.82917], "fy":[-48.93489,-48.9557,-48.94544,-48.9401]}, + {"t":4.08336, "x":2.15622, "y":5.77085, "heading":-3.14157, "vx":-1.01634, "vy":1.20407, "omega":0.00007, "ax":-1.83773, "ay":-3.16087, "alpha":-0.00094, "fx":[-28.55208,-28.54598,-28.53574,-28.5665], "fy":[-49.10372,-49.11012,-49.11474,-49.09477]}, + {"t":4.11623, "x":2.12183, "y":5.80872, "heading":-3.14157, "vx":-1.07674, "vy":1.10018, "omega":0.00004, "ax":-1.82096, "ay":-3.1706, "alpha":-0.00166, "fx":[-28.29101,-28.30984,-28.27667,-28.28089], "fy":[-49.25585,-49.24701,-49.265,-49.26026]}, + {"t":4.1491, "x":2.08546, "y":5.84317, "heading":-3.14157, "vx":-1.13659, "vy":0.99598, "omega":-0.00002, "ax":-1.80508, "ay":-3.17972, "alpha":-0.00157, "fx":[-28.05261,-28.03627,-28.02671,-28.05568], "fy":[-49.3921,-49.40422,-49.40831,-49.38973]}, + {"t":4.18196, "x":2.04712, "y":5.87418, "heading":-3.14157, "vx":-1.19591, "vy":0.89147, "omega":-0.00007, "ax":-1.79, "ay":-3.18826, "alpha":-0.00126, "fx":[-27.81292,-27.81726,-27.8005,-27.80394], "fy":[-49.52861,-49.52822,-49.53653,-49.53222]}, + {"t":4.21483, "x":2.00685, "y":5.90176, "heading":-3.14157, "vx":-1.25475, "vy":0.78668, "omega":-0.00011, "ax":-1.77569, "ay":-3.1963, "alpha":-0.00477, "fx":[-27.63365,-27.57012,-27.56137,-27.57975], "fy":[-49.62914,-49.66698,-49.67046,-49.65818]}, + {"t":4.2477, "x":1.96465, "y":5.92589, "heading":-3.14157, "vx":-1.31311, "vy":0.68163, "omega":-0.00027, "ax":-1.76207, "ay":-3.20386, "alpha":-0.00127, "fx":[-27.37643,-27.3881,-27.36568,-27.36848], "fy":[-49.77228,-49.76802,-49.77919,-49.77519]}, + {"t":4.28057, "x":1.92054, "y":5.94656, "heading":-3.14158, "vx":-1.37102, "vy":0.57633, "omega":-0.00031, "ax":-1.7491, "ay":-3.21099, "alpha":-0.00276, "fx":[-27.19788,-27.16022,-27.15248,-27.18236], "fy":[-49.87013,-49.89351,-49.89635,-49.87777]}, + {"t":4.31343, "x":1.87454, "y":5.96377, "heading":3.14159, "vx":-1.42851, "vy":0.47079, "omega":-0.0004, "ax":-1.80606, "ay":-3.17823, "alpha":-0.00142, "fx":[-28.06077,-28.0713,-28.04852,-28.05173], "fy":[-49.37339,-49.37024,-49.3815,-49.37667]}, + {"t":4.35278, "x":1.81693, "y":5.97983, "heading":3.14158, "vx":-1.49957, "vy":0.34574, "omega":-0.00045, "ax":-1.98374, "ay":-3.07134, "alpha":0.00201, "fx":[-30.78891,-30.8274,-30.81989,-30.83753], "fy":[-47.73302,-47.71191,-47.71484,-47.70007]}, + {"t":4.39213, "x":1.75639, "y":5.99106, "heading":3.14156, "vx":-1.57762, "vy":0.22489, "omega":-0.00038, "ax":-2.2813, "ay":-2.85662, "alpha":-0.00097, "fx":[-35.45412,-35.42231,-35.4425,-35.44622], "fy":[-44.36852,-44.39771,-44.37821,-44.37223]}, + {"t":4.43147, "x":1.69255, "y":5.9977, "heading":3.14154, "vx":-1.66739, "vy":0.11249, "omega":-0.00041, "ax":-2.84159, "ay":-2.29835, "alpha":0.00258, "fx":[-44.11575,-44.15367,-44.14763,-44.16568], "fy":[-35.74602,-35.69991,-35.70368,-35.67457]}, + {"t":4.47082, "x":1.62475, "y":6.00035, "heading":3.14153, "vx":-1.77919, "vy":0.02206, "omega":-0.00031, "ax":-3.47612, "ay":-1.1225, "alpha":0.00034, "fx":[-54.00293,-54.01565,-53.99628,-53.99879], "fy":[-17.44506,-17.4029,-17.4586,-17.44787]}, + {"t":4.51017, "x":1.55205, "y":6.00034, "heading":3.14152, "vx":-1.91597, "vy":-0.02211, "omega":-0.0003, "ax":-3.63679, "ay":-0.31069, "alpha":-0.03193, "fx":[-56.54171,-56.48503,-56.48143,-56.48991], "fy":[-4.34382,-5.0006,-5.00214,-4.96018]}, + {"t":4.54951, "x":1.47385, "y":5.99923, "heading":3.1415, "vx":-2.05907, "vy":-0.03433, "omega":-0.00156, "ax":-3.62488, "ay":-0.35032, "alpha":0.01659, "fx":[-56.30595,-56.34849,-56.30097,-56.30225], "fy":[-5.55179,-5.09409,-5.56907,-5.55452]}, + {"t":4.58886, "x":1.39002, "y":5.99761, "heading":3.14144, "vx":-2.20169, "vy":-0.04812, "omega":-0.0009, "ax":1.20178, "ay":-1.32659, "alpha":0.03638, "fx":[18.60277,18.62809,18.64857,18.80158], "fy":[-21.4384,-20.33992,-20.33932,-20.3194]}, + {"t":4.62821, "x":1.30432, "y":5.99469, "heading":3.14141, "vx":-2.15441, "vy":-0.10031, "omega":0.00053, "ax":3.6409, "ay":0.0953, "alpha":-0.00272, "fx":[56.5613,56.56242,56.56502,56.56488], "fy":[1.49527,1.44694,1.48502,1.4948]}, + {"t":4.66755, "x":1.22237, "y":5.99082, "heading":3.14143, "vx":-2.01115, "vy":-0.09656, "omega":0.00042, "ax":3.64697, "ay":0.15172, "alpha":0.00418, "fx":[56.6587,56.65485,56.65789,56.65896], "fy":[2.29742,2.383,2.38221,2.36567]}, + {"t":4.7069, "x":1.14606, "y":5.98714, "heading":3.14144, "vx":-1.86765, "vy":-0.09059, "omega":0.00059, "ax":3.64886, "ay":0.16666, "alpha":-0.00241, "fx":[56.68483,56.68674,56.68856,56.68816], "fy":[2.60225,2.56244,2.58983,2.60186]}, + {"t":4.74625, "x":1.0754, "y":5.9837, "heading":3.14147, "vx":-1.72408, "vy":-0.08404, "omega":0.00049, "ax":3.64983, "ay":0.17279, "alpha":-0.00042, "fx":[56.70106,56.70048,56.70356,56.70339], "fy":[2.67914,2.68353,2.68311,2.69195]}, + {"t":4.7856, "x":1.01039, "y":5.98053, "heading":3.14149, "vx":-1.58047, "vy":-0.07724, "omega":0.00048, "ax":3.65044, "ay":0.17578, "alpha":-0.0013, "fx":[56.70963,56.71025,56.71327,56.71287], "fy":[2.73705,2.72397,2.7254,2.73673]}, + {"t":4.82494, "x":0.95103, "y":5.97762, "heading":3.14151, "vx":-1.43684, "vy":-0.07032, "omega":0.00042, "ax":3.65086, "ay":0.17723, "alpha":-0.00129, "fx":[56.71605,56.71672,56.71979,56.71983], "fy":[2.76674,2.74764,2.74727,2.75176]}, + {"t":4.86429, "x":0.89732, "y":5.975, "heading":3.14152, "vx":-1.29319, "vy":-0.06335, "omega":0.00037, "ax":3.65117, "ay":0.17814, "alpha":-0.00071, "fx":[56.72123,56.72113,56.72477,56.72442], "fy":[2.77007,2.7703,2.75964,2.76976]}, + {"t":4.90364, "x":0.84926, "y":5.97264, "heading":3.14154, "vx":-1.14953, "vy":-0.05634, "omega":0.00035, "ax":3.65141, "ay":0.17863, "alpha":-0.00167, "fx":[56.72424,56.72535,56.72841,56.72842], "fy":[2.794,2.76719,2.76686,2.77226]}, + {"t":4.94298, "x":0.80686, "y":5.97056, "heading":3.14155, "vx":-1.00585, "vy":-0.04931, "omega":0.00028, "ax":3.6516, "ay":0.17888, "alpha":-0.00026, "fx":[56.72809,56.72741,56.73159,56.73124], "fy":[2.7789,2.79024,2.7685,2.77859]}, + {"t":4.98233, "x":0.77011, "y":5.96876, "heading":3.14156, "vx":-0.86218, "vy":-0.04227, "omega":0.00027, "ax":3.65176, "ay":0.17901, "alpha":-0.00224, "fx":[56.72941,56.73094,56.734,56.73375], "fy":[2.80447,2.76984,2.7695,2.78031]}, + {"t":5.02168, "x":0.73901, "y":5.96724, "heading":3.14157, "vx":-0.71849, "vy":-0.03523, "omega":0.00018, "ax":3.65189, "ay":0.17906, "alpha":-0.00004, "fx":[56.73265,56.73168,56.73612,56.73578], "fy":[2.78035,2.79698,2.77003,2.78003]}, + {"t":5.06102, "x":0.71357, "y":5.96599, "heading":3.14158, "vx":-0.5748, "vy":-0.02818, "omega":0.00018, "ax":3.652, "ay":0.17907, "alpha":-0.00258, "fx":[56.73292,56.7348,56.73785,56.73754], "fy":[2.80979,2.76885,2.76851,2.78075]}, + {"t":5.10037, "x":0.69378, "y":5.96502, "heading":3.14159, "vx":-0.4311, "vy":-0.02114, "omega":0.00008, "ax":3.65209, "ay":0.17908, "alpha":0.00012, "fx":[56.73589,56.73471,56.73934,56.73899], "fy":[2.7796,2.80019,2.76933,2.77926]}, + {"t":5.13972, "x":0.67964, "y":5.96432, "heading":3.14159, "vx":-0.28741, "vy":-0.01409, "omega":0.00008, "ax":3.65217, "ay":0.17906, "alpha":-0.00252, "fx":[56.73563,56.7375,56.74055,56.7403], "fy":[2.80962,2.76899,2.76863,2.77974]}, + {"t":5.17906, "x":0.67116, "y":5.96391, "heading":-3.14159, "vx":-0.1437, "vy":-0.00705, "omega":-0.00002, "ax":3.65224, "ay":0.17906, "alpha":0.00038, "fx":[56.73832,56.73682,56.74176,56.74142], "fy":[2.77764,2.8047,2.76735,2.77728]}, + {"t":5.21841, "x":0.66833, "y":5.96377, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.73684, "ay":0.16239, "alpha":-0.01983, "fx":[42.47417,42.47688,42.56397,42.55827], "fy":[2.59976,2.4917,2.40526,2.59445]}, + {"t":5.25524, "x":0.67019, "y":5.96388, "heading":3.14159, "vx":0.1008, "vy":0.00598, "omega":-0.00073, "ax":2.7376, "ay":0.16163, "alpha":-0.00463, "fx":[42.53032,42.53463,42.53334,42.52219], "fy":[2.5781,2.48156,2.48107,2.50322]}, + {"t":5.29207, "x":0.67576, "y":5.96421, "heading":3.14157, "vx":0.20163, "vy":0.01193, "omega":-0.0009, "ax":2.73759, "ay":0.16069, "alpha":0.00088, "fx":[42.52755,42.52515,42.53384,42.53308], "fy":[2.48786,2.54306,2.46713,2.48731]}, + {"t":5.32891, "x":0.68504, "y":5.96476, "heading":3.14153, "vx":0.30246, "vy":0.01785, "omega":-0.00087, "ax":2.73757, "ay":0.15956, "alpha":-0.00107, "fx":[42.5326,42.53503,42.52693,42.52407], "fy":[2.5162,2.46829,2.46844,2.46252]}, + {"t":5.36574, "x":0.69804, "y":5.96552, "heading":3.1415, "vx":0.40329, "vy":0.02373, "omega":-0.00091, "ax":2.73755, "ay":0.15819, "alpha":0.00033, "fx":[42.52701,42.52519,42.53293,42.53235], "fy":[2.45253,2.48965,2.43585,2.45206]}, + {"t":5.40257, "x":0.71475, "y":5.96651, "heading":3.14147, "vx":0.50412, "vy":0.02956, "omega":-0.0009, "ax":2.73753, "ay":0.15647, "alpha":0.0004, "fx":[42.51853,42.51821,42.51969,42.55974], "fy":[2.44593,2.44253,2.44512,2.3895]}, + {"t":5.4394, "x":0.73518, "y":5.9677, "heading":3.14143, "vx":0.60495, "vy":0.03532, "omega":-0.00088, "ax":2.73751, "ay":0.15424, "alpha":-0.0007, "fx":[42.52565,42.52443,42.5326,42.53204], "fy":[2.3968,2.41162,2.37976,2.39644]}, + {"t":5.47623, "x":0.75931, "y":5.96911, "heading":3.1414, "vx":0.70577, "vy":0.041, "omega":-0.00091, "ax":2.73748, "ay":0.15125, "alpha":0.00105, "fx":[42.5126,42.51149,42.5172,42.57181], "fy":[2.35418,2.3706,2.37387,2.30058]}, + {"t":5.51306, "x":0.78717, "y":5.97072, "heading":3.14137, "vx":0.8066, "vy":0.04657, "omega":-0.00087, "ax":2.73745, "ay":0.14708, "alpha":-0.00168, "fx":[42.52406,42.5233,42.53214,42.53178], "fy":[2.29077,2.28254,2.27591,2.29077]}, + {"t":5.54989, "x":0.81873, "y":5.97253, "heading":3.14134, "vx":0.90742, "vy":0.05199, "omega":-0.00093, "ax":2.73742, "ay":0.14093, "alpha":0.00228, "fx":[42.50179,42.49827,42.51106,42.59796], "fy":[2.16301,2.22806,2.23228,2.13441]}, + {"t":5.58673, "x":0.85401, "y":5.97454, "heading":3.1413, "vx":1.00825, "vy":0.05718, "omega":-0.00085, "ax":2.73737, "ay":0.13118, "alpha":0.0001, "fx":[42.52801,42.52774,42.52477,42.52527], "fy":[2.03873,2.02241,2.05192,2.03892]}, + {"t":5.62356, "x":0.893, "y":5.97674, "heading":3.14127, "vx":1.10907, "vy":0.06201, "omega":-0.00084, "ax":2.73724, "ay":0.11397, "alpha":-0.00395, "fx":[42.56948,42.57281,42.55851,42.39735], "fy":[1.71079,1.71403,1.68711,1.97019]}, + {"t":5.66039, "x":0.93571, "y":5.9791, "heading":3.14124, "vx":1.20988, "vy":0.06621, "omega":-0.00099, "ax":2.73674, "ay":0.07758, "alpha":0.004, "fx":[42.5226,42.521,42.51146,42.51168], "fy":[1.18635,1.25028,1.19723,1.18718]}, + {"t":5.69722, "x":0.98212, "y":5.98159, "heading":3.1412, "vx":1.31068, "vy":0.06906, "omega":-0.00084, "ax":2.73233, "ay":-0.03655, "alpha":0.00077, "fx":[42.44644,42.44502,42.32572,42.57577], "fy":[-0.38216,-0.55252,-0.56888,-0.76761]}, + {"t":5.73405, "x":1.03225, "y":5.98411, "heading":3.14117, "vx":1.41132, "vy":0.06772, "omega":-0.00081, "ax":0.97501, "ay":-2.42988, "alpha":-0.00063, "fx":[15.24111,15.18679,15.08583,15.07539], "fy":[-37.67271,-38.05529,-37.58443,-37.68539]}, + {"t":5.77088, "x":1.08489, "y":5.98496, "heading":3.14114, "vx":1.44723, "vy":-0.02178, "omega":-0.00083, "ax":-2.28389, "ay":-1.50217, "alpha":0.0028, "fx":[-35.47671,-35.28534,-35.45894,-35.70463], "fy":[-23.01946,-23.45904,-23.38276,-23.48701]}, + {"t":5.80771, "x":1.13665, "y":5.98314, "heading":3.14111, "vx":1.36311, "vy":-0.0771, "omega":-0.00073, "ax":-2.11301, "ay":-1.74264, "alpha":0.05038, "fx":[-32.65277,-32.65949,-33.05946,-32.93501], "fy":[-27.26236,-27.082,-26.80988,-27.13722]}, + {"t":5.84455, "x":1.18542, "y":5.97911, "heading":3.14109, "vx":1.28529, "vy":-0.14129, "omega":0.00112, "ax":-2.05674, "ay":-1.81103, "alpha":0.31314, "fx":[-30.69122,-31.51327,-33.07524,-32.53036], "fy":[-29.51695,-27.37961,-26.89314,-28.75176]}, + {"t":5.88138, "x":1.23136, "y":5.97268, "heading":3.14113, "vx":1.20953, "vy":-0.20799, "omega":0.01266, "ax":-2.02888, "ay":-1.84327, "alpha":2.02175, "fx":[-23.6537,-27.89051,-39.50576,-35.02896], "fy":[-37.12041,-24.46842,-20.42022,-32.53573]}, + {"t":5.91821, "x":1.27454, "y":5.96377, "heading":3.14159, "vx":1.13481, "vy":-0.27588, "omega":0.08712, "ax":-0.86114, "ay":-0.73542, "alpha":14.89044, "fx":[15.22303,44.51506,-65.08273,-48.16874], "fy":[-65.39947,49.98672,16.53944,-46.82756]}, + {"t":5.93807, "x":1.2969, "y":5.95815, "heading":-3.13986, "vx":1.1177, "vy":-0.29049, "omega":0.38285, "ax":-0.86014, "ay":-0.76376, "alpha":14.84634, "fx":[14.79205,44.99594,-65.31242,-47.92626], "fy":[-65.49435,49.51625,15.58788,-47.07159]}, + {"t":5.95793, "x":1.31893, "y":5.95223, "heading":-3.13226, "vx":1.10062, "vy":-0.30566, "omega":0.6777, "ax":-0.86344, "ay":-0.78941, "alpha":14.79804, "fx":[14.63762,44.80025,-65.59728,-47.49679], "fy":[-65.52446,49.64978,14.31967,-47.50046]}, + {"t":5.97779, "x":1.34062, "y":5.946, "heading":-3.1188, "vx":1.08347, "vy":-0.32133, "omega":0.97159, "ax":-0.87338, "ay":-0.81184, "alpha":14.74253, "fx":[14.70512,43.82772,-65.9253,-46.88098], "fy":[-65.50409,50.45913,12.69878,-48.1036]}, + {"t":5.99765, "x":1.36197, "y":5.93946, "heading":-3.0995, "vx":1.06613, "vy":-0.33746, "omega":1.26438, "ax":-0.89233, "ay":-0.83017, "alpha":14.67762, "fx":[14.97378,41.92154,-66.27004,-46.07681], "fy":[-65.43717,51.9933,10.72482,-48.8694]}, + {"t":6.01751, "x":1.38296, "y":5.93259, "heading":-3.07439, "vx":1.04841, "vy":-0.35394, "omega":1.55588, "ax":-0.92429, "ay":-0.84373, "alpha":14.59853, "fx":[15.42197,38.82161,-66.59928,-45.08198], "fy":[-65.32609,54.27617,8.40218,-49.78325]}, + {"t":6.03737, "x":1.4036, "y":5.9254, "heading":-3.04349, "vx":1.03005, "vy":-0.3707, "omega":1.84581, "ax":-0.97549, "ay":-0.85298, "alpha":14.49508, "fx":[16.02377,34.12634,-66.87662,-43.89269], "fy":[-65.17313,57.26174,5.73434,-50.8292]}, + {"t":6.05723, "x":1.42387, "y":5.91787, "heading":-3.00684, "vx":1.01068, "vy":-0.38764, "omega":2.13368, "ax":-1.05572, "ay":-0.86057, "alpha":14.34699, "fx":[16.74919,27.21478,-67.06095,-42.50761], "fy":[-64.98133,60.75537,2.73524,-51.98695]}, + {"t":6.07709, "x":1.44373, "y":5.91, "heading":-2.96446, "vx":0.98971, "vy":-0.40473, "omega":2.41862, "ax":-1.17772, "ay":-0.87517, "alpha":14.11582, "fx":[17.56505,17.28304,-67.10874,-40.92532], "fy":[-64.75497,64.1824,-0.5775,-53.23497]}, + {"t":6.09695, "x":1.46316, "y":5.90179, "heading":-2.91643, "vx":0.96632, "vy":-0.42211, "omega":2.69896, "ax":-1.35352, "ay":-0.91672, "alpha":13.73695, "fx":[18.43301,3.57882,-66.97536,-39.14699], "fy":[-64.50098,66.26096,-4.17771,-54.54895]}, + {"t":6.11681, "x":1.48208, "y":5.89322, "heading":-2.86282, "vx":0.93944, "vy":-0.44032, "omega":2.97178, "ax":-1.57826, "ay":-1.01878, "alpha":13.13718, "fx":[19.31848,-13.60404,-66.6174,-37.17376], "fy":[-64.22703,64.85181,-8.02938,-55.90467]}, + {"t":6.13667, "x":1.50043, "y":5.88428, "heading":-2.8038, "vx":0.90809, "vy":-0.46055, "omega":3.23268, "ax":-1.8107, "ay":-1.20952, "alpha":12.30986, "fx":[20.1995,-31.71591,-65.99704,-35.00766], "fy":[-63.93864,58.13203,-12.07787,-57.2776]}, + {"t":6.15653, "x":1.5181, "y":5.87489, "heading":-2.7396, "vx":0.87213, "vy":-0.48457, "omega":3.47716, "ax":-1.97928, "ay":-1.46864, "alpha":11.41977, "fx":[21.18185,-46.46151,-65.0948,-32.62214], "fy":[-63.59994,47.21683,-16.2221,-58.65947]}, + {"t":6.17639, "x":1.53503, "y":5.86498, "heading":-2.67055, "vx":0.83282, "vy":-0.51374, "omega":3.70396, "ax":-2.01983, "ay":-1.70108, "alpha":10.84039, "fx":[22.94516,-54.63554,-63.96582,-29.86063], "fy":[-62.95979,37.5376,-20.18638,-60.10006]}, + {"t":6.19625, "x":1.55118, "y":5.85444, "heading":-2.59698, "vx":0.79271, "vy":-0.54753, "omega":3.91925, "ax":-1.9564, "ay":-1.85515, "alpha":10.67667, "fx":[25.87669,-58.13242,-62.68221,-26.63681], "fy":[-61.77854,31.9107,-23.83461,-61.58054]}, + {"t":6.21611, "x":1.56653, "y":5.8432, "heading":-2.51915, "vx":0.75386, "vy":-0.58437, "omega":4.13129, "ax":-1.85755, "ay":-1.97881, "alpha":10.64864, "fx":[29.22186,-60.31634,-61.1976,-23.13998], "fy":[-60.22284,27.59022,-27.37705,-62.95783]}, + {"t":6.23597, "x":1.58114, "y":5.83121, "heading":-2.4371, "vx":0.71696, "vy":-0.62367, "omega":4.34277, "ax":-1.74343, "ay":-2.0907, "alpha":10.6552, "fx":[32.64467,-62.03002,-59.48758,-19.46778], "fy":[-58.37649,23.4861,-30.8669,-64.16336]}, + {"t":6.25583, "x":1.59503, "y":5.81841, "heading":-2.35085, "vx":0.68234, "vy":-0.66519, "omega":4.55439, "ax":-1.6193, "ay":-2.19397, "alpha":10.67123, "fx":[36.02358,-63.42131,-57.55295,-15.67638], "fy":[-56.26706,19.37185,-34.28169,-65.16137]}, + {"t":6.27569, "x":1.60827, "y":5.80476, "heading":-2.2604, "vx":0.65018, "vy":-0.70876, "omega":4.76632, "ax":-1.48814, "ay":-2.28849, "alpha":10.68599, "fx":[39.2723,-64.51264,-55.41067,-11.82536], "fy":[-53.92064,15.21198,-37.57673,-65.92634]}, + {"t":6.29555, "x":1.62089, "y":5.79024, "heading":-2.16574, "vx":0.62062, "vy":-0.75421, "omega":4.97854, "ax":-1.35307, "ay":-2.37322, "alpha":10.69061, "fx":[42.29941,-65.29652,-53.09435,-7.99133], "fy":[-51.3762,11.03438,-40.69525,-66.44005]}, + {"t":6.31541, "x":1.63294, "y":5.77479, "heading":-2.06687, "vx":0.59375, "vy":-0.80135, "omega":5.19086, "ax":-1.21826, "ay":-2.44686, "alpha":10.67305, "fx":[44.98215,-65.75585,-50.65685,-4.27477], "fy":[-48.6929,6.89965,-43.56881,-66.69136]}, + {"t":6.33527, "x":1.6445, "y":5.75839, "heading":-1.96377, "vx":0.56956, "vy":-0.84994, "omega":5.40283, "ax":-1.0897, "ay":-2.50805, "alpha":10.61113, "fx":[47.1156,-65.85935,-48.16509,-0.80727], "fy":[-45.94631,2.87696,-46.11633,-66.6698]}, + {"t":6.35513, "x":1.65559, "y":5.74102, "heading":-1.85647, "vx":0.54792, "vy":-0.89975, "omega":5.61357, "ax":-0.97638, "ay":-2.5553, "alpha":10.4519, "fx":[48.20114,-65.49868,-45.64569,2.2691], "fy":[-43.1954,-1.02342,-48.24834,-66.32492]}, + {"t":6.37499, "x":1.66628, "y":5.72264, "heading":-1.74499, "vx":0.52852, "vy":-0.9505, "omega":5.82114, "ax":-0.88906, "ay":-2.58779, "alpha":9.98515, "fx":[45.97501,-63.95202,-42.7892,5.51801], "fy":[-40.02025,-5.81975,-49.77153,-65.19953]}, + {"t":6.39485, "x":1.6766, "y":5.70326, "heading":-1.62938, "vx":0.51087, "vy":-1.00189, "omega":6.01945, "ax":-0.83262, "ay":-2.60711, "alpha":-0.29044, "fx":[-13.93435,-11.33676,-11.86387,-14.60581], "fy":[-40.5396,-41.22767,-40.46215,-39.78212]}, + {"t":6.41471, "x":1.68658, "y":5.68284, "heading":-1.50983, "vx":0.49433, "vy":-1.05367, "omega":6.01368, "ax":-0.66259, "ay":-2.65439, "alpha":-9.59984, "fx":[-35.56319,14.22072,43.14944,-62.98169], "fy":[-55.20484,-63.72484,-31.38412,-14.63598]}, + {"t":6.43457, "x":1.69627, "y":5.6614, "heading":-1.3904, "vx":0.48117, "vy":-1.10639, "omega":5.82303, "ax":-0.43776, "ay":-2.69911, "alpha":-9.89159, "fx":[-32.7403,17.8231,51.3531,-63.63901], "fy":[-57.83844,-63.78364,-28.51647,-17.59022]}, + {"t":6.45443, "x":1.70574, "y":5.63889, "heading":-1.27475, "vx":0.47248, "vy":-1.15999, "omega":5.62658, "ax":-0.19088, "ay":-2.72553, "alpha":-9.93013, "fx":[-27.61491,23.50252,54.53305,-62.28248], "fy":[-60.7458,-62.26577,-23.16817,-23.19038]}, + {"t":6.47429, "x":1.71509, "y":5.61531, "heading":-1.16301, "vx":0.46869, "vy":-1.21412, "omega":5.42937, "ax":0.07198, "ay":-2.72761, "alpha":-9.94451, "fx":[-21.71064,29.41842,56.7219,-59.95689], "fy":[-63.23845,-59.88733,-17.21719,-29.15628]}, + {"t":6.49415, "x":1.72441, "y":5.59066, "heading":-1.05518, "vx":0.47012, "vy":-1.26829, "omega":5.23187, "ax":0.34648, "ay":-2.69907, "alpha":-9.97956, "fx":[-15.34973,35.27651,58.48396,-56.87974], "fy":[-65.15788,-56.75782,-10.79617,-35.01397]}, + {"t":6.51401, "x":1.73381, "y":5.56494, "heading":-0.95127, "vx":0.477, "vy":-1.3219, "omega":5.03367, "ax":0.62979, "ay":-2.62459, "alpha":-10.09205, "fx":[-8.71732,40.90537,60.11661,-53.16821], "fy":[-66.42519,-52.93561,-3.17789,-40.55902]}, + {"t":6.53387, "x":1.74341, "y":5.53817, "heading":-0.8513, "vx":0.48951, "vy":-1.37402, "omega":4.83324, "ax":0.82296, "ay":-2.18416, "alpha":-11.44177, "fx":[-2.12045,46.45524,56.2307,-49.42489], "fy":[-66.99715,-48.20647,24.59925,-45.12385]}, + {"t":6.55373, "x":1.7533, "y":5.51045, "heading":-0.75532, "vx":0.50585, "vy":-1.4174, "omega":4.60601, "ax":0.25845, "ay":-1.43815, "alpha":-13.94915, "fx":[2.97157,52.9071,9.35478,-49.17261], "fy":[-66.98035,-41.04912,64.10224,-45.44252]}, + {"t":6.57359, "x":1.76339, "y":5.48202, "heading":-0.66384, "vx":0.51098, "vy":-1.44596, "omega":4.32897, "ax":0.13559, "ay":-1.29824, "alpha":-14.39537, "fx":[7.96606,58.12417,-9.06875,-48.59564], "fy":[-66.5842,-33.28287,65.28761,-46.096]}, + {"t":6.59345, "x":1.77357, "y":5.45305, "heading":-0.57787, "vx":0.51368, "vy":-1.47174, "omega":4.04308, "ax":0.14472, "ay":-1.20176, "alpha":-14.66221, "fx":[12.86043,61.98669,-18.30502,-47.54889], "fy":[-65.82601,-25.41273,63.76492,-47.20629]}, + {"t":6.61331, "x":1.7838, "y":5.42358, "heading":-0.49757, "vx":0.51655, "vy":-1.49561, "omega":3.75189, "ax":0.18391, "ay":-1.11378, "alpha":-14.86731, "fx":[17.56717,64.62218,-24.60412,-46.15687], "fy":[-64.73982,-17.72284,61.84418,-48.59446]}, + {"t":6.63317, "x":1.79409, "y":5.39366, "heading":-0.42306, "vx":0.5202, "vy":-1.51773, "omega":3.45662, "ax":0.22885, "ay":-1.03145, "alpha":-15.03059, "fx":[22.01175,66.20619,-29.47138,-44.52533], "fy":[-63.37669,-10.42973,59.82562,-50.1156]}, + {"t":6.65304, "x":1.80447, "y":5.36331, "heading":-0.35441, "vx":0.52475, "vy":-1.53822, "omega":3.15811, "ax":0.27227, "ay":-0.95455, "alpha":-15.16188, "fx":[26.14277,66.93547,-33.41571,-42.74306], "fy":[-61.7968,-3.67601,57.81725,-51.66254]}, + {"t":6.6729, "x":1.81495, "y":5.33258, "heading":-0.29169, "vx":0.53016, "vy":-1.55717, "omega":2.85699, "ax":0.31166, "ay":-0.8834, "alpha":-15.26771, "fx":[29.92362,67.0045,-36.67648,-40.88468], "fy":[-60.06612,2.44979,55.88112,-53.16103]}, + {"t":6.69276, "x":1.82554, "y":5.30148, "heading":-0.23495, "vx":0.53634, "vy":-1.57472, "omega":2.55377, "ax":0.34661, "ay":-0.81771, "alpha":-15.35358, "fx":[33.3368,66.59239,-39.36996,-39.01997], "fy":[-58.24955,7.91741,54.07574,-54.55781]}, + {"t":6.71262, "x":1.83626, "y":5.27004, "heading":-0.18423, "vx":0.54323, "vy":-1.59096, "omega":2.24885, "ax":0.37736, "ay":-0.75735, "alpha":-15.42339, "fx":[36.37637,65.85563,-41.57388,-37.20836], "fy":[-56.40947,12.71953,52.44716,-55.82059]}, + {"t":6.73248, "x":1.84712, "y":5.23829, "heading":-0.13957, "vx":0.55072, "vy":-1.606, "omega":1.94254, "ax":0.40437, "ay":-0.70192, "alpha":-15.48038, "fx":[39.04617,64.92494,-43.33996,-35.5026], "fy":[-54.60324,16.87944,51.036,-56.93071]}, + {"t":6.75234, "x":1.85814, "y":5.20626, "heading":-0.10099, "vx":0.55875, "vy":-1.61994, "omega":1.63509, "ax":0.42827, "ay":-0.65102, "alpha":-15.52697, "fx":[41.3572,63.90777,-44.70324,-33.94825], "fy":[-52.88181,20.42722,49.87856,-57.87979]}, + {"t":6.7722, "x":1.86932, "y":5.17396, "heading":-0.06851, "vx":0.56726, "vy":-1.63287, "omega":1.32673, "ax":0.44957, "ay":-0.6042, "alpha":-15.56516, "fx":[43.32562,62.88859,-45.68998,-32.58703], "fy":[-51.28865,23.40335,49.00382,-58.66466]}, + {"t":6.79206, "x":1.88067, "y":5.14141, "heading":-0.04216, "vx":0.57619, "vy":-1.64487, "omega":1.0176, "ax":0.46881, "ay":-0.56109, "alpha":-15.59637, "fx":[44.96924,61.93383,-46.31788,-31.45227], "fy":[-49.86063,25.8461,48.43449,-59.28758]}, + {"t":6.81192, "x":1.89221, "y":5.10863, "heading":-0.02195, "vx":0.5855, "vy":-1.65601, "omega":0.70785, "ax":0.48562, "ay":-0.52144, "alpha":-15.6224, "fx":[46.29636,61.08856,-46.61309,-30.59425], "fy":[-48.63736,27.80391,48.17109,-59.74089]}, + {"t":6.83178, "x":1.90393, "y":5.07564, "heading":-0.0079, "vx":0.59514, "vy":-1.66637, "omega":0.39759, "ax":0.50284, "ay":-0.48453, "alpha":-15.64234, "fx":[47.3553,60.39997,-46.52619,-29.9814], "fy":[-47.61312,29.28636,48.2731,-60.05614]}, + {"t":6.85164, "x":1.91585, "y":5.04245, "heading":0.0, "vx":0.60513, "vy":-1.67599, "omega":0.08693, "ax":2.03719, "ay":-1.8246, "alpha":-2.15471, "fx":[35.53216,40.02678,27.49728,23.53928], "fy":[-32.59079,-19.81099,-23.69334,-37.28974]}, + {"t":6.88681, "x":1.93839, "y":4.98238, "heading":0.00306, "vx":0.67677, "vy":-1.74016, "omega":0.01115, "ax":2.14801, "ay":-1.69744, "alpha":-0.36547, "fx":[33.895,34.7428,32.73004,32.114], "fy":[-27.37311,-24.80297,-25.51817,-27.78846]}, + {"t":6.92197, "x":1.96352, "y":4.92013, "heading":0.00345, "vx":0.75232, "vy":-1.79986, "omega":-0.0017, "ax":2.32499, "ay":-1.4412, "alpha":-0.05819, "fx":[36.27076,36.28205,36.03753,35.88937], "fy":[-22.50206,-22.29325,-22.11183,-22.65231]}, + {"t":6.95714, "x":1.99142, "y":4.85594, "heading":0.00339, "vx":0.83408, "vy":-1.85054, "omega":-0.00375, "ax":2.61686, "ay":-0.77936, "alpha":-0.00302, "fx":[40.84046,40.67306,40.60647,40.49704], "fy":[-11.56011,-12.18466,-12.2021,-12.48454]}, + {"t":6.99231, "x":2.02237, "y":4.79038, "heading":0.00326, "vx":0.92611, "vy":-1.87795, "omega":-0.00385, "ax":2.39535, "ay":1.29246, "alpha":-0.00634, "fx":[37.17036,36.93629,37.40138,37.34447], "fy":[19.90241,20.9939,19.55615,19.86403]}, + {"t":7.02748, "x":2.05642, "y":4.72513, "heading":0.00312, "vx":1.01036, "vy":-1.8325, "omega":-0.00408, "ax":0.33209, "ay":2.70815, "alpha":-0.00198, "fx":[5.23398,5.43207,5.42272,4.54786], "fy":[42.45353,41.9079,41.91392,42.01508]}, + {"t":7.06265, "x":2.09216, "y":4.66236, "heading":0.00298, "vx":1.02204, "vy":-1.73725, "omega":-0.00415, "ax":-0.59755, "ay":2.66849, "alpha":0.00194, "fx":[-9.32556,-9.28919,-9.26089,-9.25723], "fy":[41.4403,41.51816,41.42018,41.44732]}, + {"t":7.09782, "x":2.12773, "y":4.60292, "heading":0.00283, "vx":1.00102, "vy":-1.64341, "omega":-0.00408, "ax":-0.95557, "ay":2.5651, "alpha":0.00057, "fx":[-14.91469,-14.82575,-14.72812,-14.91293], "fy":[39.79147,39.85836,39.8857,39.86559]}, + {"t":7.13299, "x":2.16235, "y":4.54671, "heading":0.00269, "vx":0.96741, "vy":-1.5532, "omega":-0.00406, "ax":-1.13444, "ay":2.49276, "alpha":-0.0032, "fx":[-17.50022,-17.69048,-17.61001,-17.69611], "fy":[38.7694,38.62988,38.80305,38.70329]}, + {"t":7.16815, "x":2.19567, "y":4.49362, "heading":0.00255, "vx":0.92752, "vy":-1.46553, "omega":-0.00417, "ax":-1.24015, "ay":2.44289, "alpha":0.00663, "fx":[-19.60846,-19.18999,-19.27816,-18.98879], "fy":[37.61575,38.04777,38.04366,38.09933]}, + {"t":7.20332, "x":2.22752, "y":4.44359, "heading":0.0024, "vx":0.8839, "vy":-1.37962, "omega":-0.00394, "ax":-1.30954, "ay":2.40708, "alpha":-0.00244, "fx":[-20.40997,-20.28716,-20.35432,-20.32638], "fy":[37.35935,37.46707,37.36054,37.39414]}, + {"t":7.23849, "x":2.2578, "y":4.39656, "heading":0.00226, "vx":0.83785, "vy":-1.29496, "omega":-0.00402, "ax":-1.35849, "ay":2.38029, "alpha":-0.00241, "fx":[-20.9503,-21.14827,-21.05404,-21.26685], "fy":[37.13959,36.92395,36.95125,36.90143]}, + {"t":7.27366, "x":2.28642, "y":4.35249, "heading":0.00212, "vx":0.79007, "vy":-1.21125, "omega":-0.00411, "ax":-1.39481, "ay":2.35956, "alpha":0.002, "fx":[-21.7657,-21.61575,-21.67516,-21.61979], "fy":[36.61609,36.73048,36.60319,36.67835]}, + {"t":7.30883, "x":2.31334, "y":4.31135, "heading":0.00198, "vx":0.74102, "vy":-1.12827, "omega":-0.00404, "ax":-1.42281, "ay":2.34307, "alpha":-0.00197, "fx":[-21.94773,-22.15476,-22.05616,-22.25794], "fy":[36.56457,36.34316,36.37575,36.31992]}, + {"t":7.344, "x":2.33853, "y":4.27312, "heading":0.00183, "vx":0.69098, "vy":-1.04587, "omega":-0.00411, "ax":-1.44505, "ay":2.32966, "alpha":0.00057, "fx":[-22.51225,-22.41127,-22.45727,-22.41758], "fy":[36.16143,36.2459,36.15656,36.2059]}, + {"t":7.37917, "x":2.36193, "y":4.23778, "heading":0.00169, "vx":0.64016, "vy":-0.96393, "omega":-0.00409, "ax":-1.46313, "ay":2.31853, "alpha":0.00101, "fx":[-22.78347,-22.73028,-22.64598,-22.76222], "fy":[35.97768,36.01705,36.06337,36.02058]}, + {"t":7.41433, "x":2.38354, "y":4.20532, "heading":0.00155, "vx":0.5887, "vy":-0.88239, "omega":-0.00405, "ax":-1.47812, "ay":2.30917, "alpha":-0.00007, "fx":[-23.02349,-22.9206,-22.97653,-22.93269], "fy":[35.84192,35.92769,35.83896,35.88808]}, + {"t":7.4495, "x":2.40333, "y":4.17571, "heading":0.0014, "vx":0.53672, "vy":-0.80118, "omega":-0.00405, "ax":-1.49074, "ay":2.30117, "alpha":-0.001, "fx":[-23.08817,-23.18715,-23.11357,-23.24883], "fy":[35.82908,35.71922,35.749,35.70266]}, + {"t":7.48467, "x":2.42128, "y":4.14896, "heading":0.00126, "vx":0.48429, "vy":-0.72026, "omega":-0.00409, "ax":-1.50152, "ay":2.29427, "alpha":0.00064, "fx":[-23.35073,-23.31456,-23.33189,-23.31021], "fy":[35.63019,35.66203,35.62755,35.65137]}, + {"t":7.51984, "x":2.43739, "y":4.12505, "heading":0.00112, "vx":0.43149, "vy":-0.63957, "omega":-0.00407, "ax":-1.51082, "ay":2.28826, "alpha":0.00005, "fx":[-23.44115,-23.48618,-23.4553,-23.50307], "fy":[35.58316,35.53422,35.54825,35.53166]}, + {"t":7.55501, "x":2.45163, "y":4.10397, "heading":0.00097, "vx":0.37835, "vy":-0.55909, "omega":-0.00406, "ax":-1.51894, "ay":2.28297, "alpha":-0.00008, "fx":[-23.63874,-23.56485,-23.60981,-23.57677], "fy":[35.4467,35.5052,35.43846,35.47808]}, + {"t":7.59018, "x":2.464, "y":4.08572, "heading":0.00083, "vx":0.32493, "vy":-0.47881, "omega":-0.00407, "ax":-1.52608, "ay":2.27828, "alpha":-0.00016, "fx":[-23.64382,-23.73128,-23.71825,-23.74064], "fy":[35.45849,35.37268,35.37578,35.37007]}, + {"t":7.62535, "x":2.47448, "y":4.07029, "heading":0.00069, "vx":0.27126, "vy":-0.39868, "omega":-0.00407, "ax":-1.53241, "ay":2.27409, "alpha":0.00018, "fx":[-23.82641,-23.7927,-23.81418,-23.79427], "fy":[35.31899,35.34775,35.31392,35.33626]}, + {"t":7.66051, "x":2.48307, "y":4.05767, "heading":0.00055, "vx":0.21737, "vy":-0.3187, "omega":-0.00407, "ax":-1.53807, "ay":2.27033, "alpha":0.00005, "fx":[-23.89731,-23.90082,-23.85284,-23.9279], "fy":[35.27794,35.26292,35.28692,35.25565]}, + {"t":7.69568, "x":2.48976, "y":4.04787, "heading":0.0004, "vx":0.16328, "vy":-0.23886, "omega":-0.00406, "ax":-1.54315, "ay":2.26694, "alpha":0.00275, "fx":[-23.96129,-24.00263,-23.96264,-23.96787], "fy":[35.22877,35.19559,35.22491,35.22337]}, + {"t":7.73085, "x":2.49455, "y":4.04087, "heading":0.00026, "vx":0.10901, "vy":-0.15914, "omega":-0.00397, "ax":-1.54773, "ay":2.26386, "alpha":0.01567, "fx":[-24.09127,-24.11277,-23.98688,-23.98846], "fy":[35.1906,35.11319,35.15682,35.22081]}, + {"t":7.76602, "x":2.49743, "y":4.03667, "heading":0.00012, "vx":0.05458, "vy":-0.07952, "omega":-0.00342, "ax":-1.55189, "ay":2.26106, "alpha":0.09712, "fx":[-24.3105,-24.55248,-23.87712,-23.69789], "fy":[35.28999,34.7623,34.98812,35.46675]}, + {"t":7.80119, "x":2.49839, "y":4.03528, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.007, "ay":-0.0068, "alpha":16.07485, "fx":[-47.6789,-47.46882,47.25084,47.46187], "fy":[47.25088,-47.46274,-47.67891,47.46801]}, + {"t":7.82411, "x":2.49839, "y":4.03527, "heading":0.0, "vx":-0.00016, "vy":-0.00016, "omega":0.36841, "ax":-0.00833, "ay":-0.00809, "alpha":16.06697, "fx":[-47.69598,-47.44656,47.18698,47.43774], "fy":[47.18703,-47.43903,-47.69599,47.44535]}, + {"t":7.84703, "x":2.49838, "y":4.03527, "heading":0.00844, "vx":-0.00035, "vy":-0.00034, "omega":0.73663, "ax":-0.01051, "ay":-0.01019, "alpha":16.05387, "fx":[-48.11879,-47.01048,47.48341,46.99295], "fy":[46.67771,-47.79573,-47.3238,47.80883]}, + {"t":7.86994, "x":2.49837, "y":4.03526, "heading":0.02533, "vx":-0.00059, "vy":-0.00057, "omega":1.10455, "ax":-0.0148, "ay":-0.01434, "alpha":16.02783, "fx":[-48.94713,-46.13463,48.06953,46.09243], "fy":[45.65036,-48.495,-46.5732,48.52654]}, + {"t":7.89286, "x":2.49835, "y":4.03524, "heading":0.05064, "vx":-0.00093, "vy":-0.0009, "omega":1.47188, "ax":-0.02743, "ay":-0.02654, "alpha":15.95107, "fx":[-50.20496,-44.7234,48.63238,44.59155], "fy":[43.78381,-49.38356,-45.52194,49.47234]}, + {"t":7.91578, "x":2.49832, "y":4.03521, "heading":0.08437, "vx":-0.00156, "vy":-0.00151, "omega":1.83745, "ax":-0.54334, "ay":-0.55328, "alpha":11.17036, "fx":[-46.88571,-35.28433,23.55531,24.85064], "fy":[16.94696,-39.923,-43.45918,32.05337]}, + {"t":7.9387, "x":2.49815, "y":4.03503, "heading":0.12648, "vx":-0.01401, "vy":-0.01419, "omega":2.09345, "ax":-0.91937, "ay":-0.88681, "alpha":0.00124, "fx":[-14.26395,-13.74253,-14.55381,-14.57131], "fy":[-13.74336,-13.57075,-14.55366,-13.24042]}, + {"t":7.96162, "x":2.49758, "y":4.03448, "heading":0.17446, "vx":-0.03508, "vy":-0.03452, "omega":2.09348, "ax":-0.87702, "ay":-0.85732, "alpha":0.0, "fx":[-13.61662,-13.61382,-13.4487,-13.82084], "fy":[-13.67521,-13.1235,-13.47263,-13.00458]}, + {"t":7.98453, "x":2.49655, "y":4.03346, "heading":0.22244, "vx":-0.05518, "vy":-0.05416, "omega":2.09348, "ax":-0.84452, "ay":-0.81501, "alpha":0.0, "fx":[-12.91157,-12.976,-13.34353,-13.24922], "fy":[-12.43892,-12.77418,-12.94467,-12.48895]}, + {"t":8.00745, "x":2.49506, "y":4.032, "heading":0.27042, "vx":-0.07454, "vy":-0.07284, "omega":2.09348, "ax":-0.80028, "ay":-0.78446, "alpha":0.0, "fx":[-12.49588,-12.33934,-12.51433,-12.38131], "fy":[-12.18547,-12.19091,-12.20943,-12.16228]}, + {"t":8.03037, "x":2.49314, "y":4.03013, "heading":0.3184, "vx":-0.09288, "vy":-0.09082, "omega":2.09348, "ax":-0.76432, "ay":-0.73147, "alpha":0.0, "fx":[-11.76176,-11.80349,-11.98908,-11.94203], "fy":[-11.304,-11.55297,-11.32951,-11.26865]}, + {"t":8.05329, "x":2.49082, "y":4.02786, "heading":0.36638, "vx":-0.11039, "vy":-0.10758, "omega":2.09348, "ax":-0.7202, "ay":-0.69772, "alpha":0.0, "fx":[-11.27062,-11.18485,-11.30804,-10.99101], "fy":[-10.76735,-10.86662,-10.79556,-10.92809]}, + {"t":8.07621, "x":2.4881, "y":4.02521, "heading":0.41436, "vx":-0.1269, "vy":-0.12358, "omega":2.09348, "ax":-0.67781, "ay":-0.66286, "alpha":0.0, "fx":[-10.75686,-10.61014,-10.30636,-10.44711], "fy":[-10.42992,-10.20461,-10.00429,-10.55293]}, + {"t":8.09912, "x":2.48501, "y":4.0222, "heading":0.46233, "vx":-0.14243, "vy":-0.13877, "omega":2.09348, "ax":-0.63754, "ay":-0.6115, "alpha":0.0, "fx":[-9.97459,-9.90253,-9.99436,-9.74676], "fy":[-9.45978,-9.52677,-9.4555,-9.55771]}, + {"t":8.12204, "x":2.48158, "y":4.01886, "heading":0.51031, "vx":-0.15705, "vy":-0.15278, "omega":2.09348, "ax":-0.59672, "ay":-0.57528, "alpha":0.0, "fx":[-9.43518,-9.30515,-9.10797,-9.2334], "fy":[-9.00486,-8.86779,-8.73683,-9.13961]}, + {"t":8.14496, "x":2.47782, "y":4.01521, "heading":0.55829, "vx":-0.17072, "vy":-0.16597, "omega":2.09348, "ax":-0.55729, "ay":-0.52716, "alpha":0.0, "fx":[-8.70638,-8.66531,-8.71434,-8.54517], "fy":[-8.18445,-8.2005,-8.15947,-8.21452]}, + {"t":8.16788, "x":2.47376, "y":4.01126, "heading":0.60627, "vx":-0.18349, "vy":-0.17805, "omega":2.09348, "ax":-0.51858, "ay":-0.48982, "alpha":0.0, "fx":[-8.14335,-8.0659,-7.97057,-8.04612], "fy":[-7.62718,-7.57143,-7.51258,-7.72704]}, + {"t":8.1908, "x":2.46942, "y":4.00706, "heading":0.65425, "vx":-0.19538, "vy":-0.18927, "omega":2.09348, "ax":-0.48138, "ay":-0.44633, "alpha":0.0, "fx":[-7.51126,-7.49346,-7.51399,-7.39505], "fy":[-6.94651,-6.93355,-6.91605,-6.93979]}, + {"t":8.21372, "x":2.46482, "y":4.0026, "heading":0.70223, "vx":-0.20641, "vy":-0.1995, "omega":2.09348, "ax":-0.44567, "ay":-0.40952, "alpha":0.0, "fx":[-6.96066,-6.92486,-6.88712,-6.92246], "fy":[-6.35837,-6.34784,-6.32331,-6.41866]}, + {"t":8.23663, "x":2.45997, "y":3.99792, "heading":0.75021, "vx":-0.21662, "vy":-0.20889, "omega":2.09348, "ax":-0.41169, "ay":-0.37061, "alpha":0.0, "fx":[-6.41967,-6.4133,-6.4196,-6.33072], "fy":[-5.77675,-5.75274,-5.74665,-5.75466]}, + {"t":8.25955, "x":2.4549, "y":3.99304, "heading":0.79818, "vx":-0.22606, "vy":-0.21738, "omega":2.09348, "ax":-0.37979, "ay":-0.33584, "alpha":0.0, "fx":[-5.91214,-5.90003,-5.88835,-5.90042], "fy":[-5.20802,-5.21737,-5.20309,-5.24123]}, + {"t":8.28247, "x":2.44962, "y":3.98797, "heading":0.84616, "vx":-0.23476, "vy":-0.22508, "omega":2.09348, "ax":-0.34964, "ay":-0.30118, "alpha":0.0, "fx":[-5.4503,-5.44828,-5.44857,-5.38029], "fy":[-4.69975,-4.67299,-4.67115,-4.67233]}, + {"t":8.30539, "x":2.44414, "y":3.98273, "heading":0.89414, "vx":-0.24278, "vy":-0.23198, "omega":2.09348, "ax":-0.32205, "ay":-0.26932, "alpha":0.0, "fx":[-5.0038,-5.00365,-5.00252,-5.00269], "fy":[-4.17429,-4.19136,-4.17809,-4.19212]}, + {"t":8.32831, "x":2.4385, "y":3.97734, "heading":0.94212, "vx":-0.25016, "vy":-0.23815, "omega":2.09348, "ax":-0.296, "ay":-0.23858, "alpha":0.0, "fx":[-4.61296,-4.61228,-4.61057,-4.55836], "fy":[-3.7268,-3.70056,-3.69995,-3.69868]}, + {"t":8.35122, "x":2.43268, "y":3.97182, "heading":0.9901, "vx":-0.25694, "vy":-0.24362, "omega":2.09348, "ax":-0.27293, "ay":-0.20995, "alpha":0.0, "fx":[-4.2362,-4.24161,-4.24415,-4.23875], "fy":[-3.25312,-3.27254,-3.2582,-3.26294]}, + {"t":8.37414, "x":2.42672, "y":3.96618, "heading":1.03808, "vx":-0.2632, "vy":-0.24843, "omega":2.09348, "ax":-0.25099, "ay":-0.18281, "alpha":0.0, "fx":[-3.9101,-3.9098,-3.9079,-3.86901], "fy":[-2.85878,-2.83447,-2.83418,-2.83249]}, + {"t":8.39706, "x":2.42063, "y":3.96044, "heading":1.08606, "vx":-0.26895, "vy":-0.25262, "omega":2.09348, "ax":-0.23246, "ay":-0.15738, "alpha":0.0, "fx":[-3.60608,-3.61351,-3.61668,-3.60924], "fy":[-2.43754,-2.45676,-2.44194,-2.44347]}, + {"t":8.41998, "x":2.4144, "y":3.95461, "heading":1.13404, "vx":-0.27428, "vy":-0.25623, "omega":2.09348, "ax":-0.2144, "ay":-0.13344, "alpha":0.0, "fx":[-3.33852,-3.3385,-3.33711,-3.30945], "fy":[-2.08956,-2.06806,-2.06802,-2.06674]}, + {"t":8.4429, "x":2.40806, "y":3.9487, "heading":1.18201, "vx":-0.27919, "vy":-0.25929, "omega":2.09348, "ax":-0.20029, "ay":-0.11095, "alpha":0.0, "fx":[-3.10673,-3.11405,-3.11659,-3.10925], "fy":[-1.71743,-1.73496,-1.72099,-1.72159]}, + {"t":8.46581, "x":2.40161, "y":3.94273, "heading":1.22999, "vx":-0.28378, "vy":-0.26183, "omega":2.09348, "ax":-0.18581, "ay":-0.0898, "alpha":0.0, "fx":[-2.89128,-2.89168,-2.89095,-2.87275], "fy":[-1.40861,-1.39066,-1.39104,-1.39035]}, + {"t":8.48873, "x":2.39506, "y":3.93671, "heading":1.27797, "vx":-0.28804, "vy":-0.26389, "omega":2.09348, "ax":-0.17591, "ay":-0.06987, "alpha":0.0, "fx":[-2.72909,-2.73499,-2.73653,-2.73061], "fy":[-1.08025,-1.09495,-1.08319,-1.08358]}, + {"t":8.51165, "x":2.38841, "y":3.93064, "heading":1.32595, "vx":-0.29207, "vy":-0.26549, "omega":2.09348, "ax":-0.16462, "ay":-0.05105, "alpha":0.0, "fx":[-2.55949,-2.5604,-2.56018,-2.54996], "fy":[-0.8029,-0.78924,-0.79014,-0.78994]}, + {"t":8.53457, "x":2.38167, "y":3.92454, "heading":1.37393, "vx":-0.29584, "vy":-0.26666, "omega":2.09348, "ax":-0.15869, "ay":-0.03322, "alpha":0.0, "fx":[-2.46313,-2.46684,-2.46748,-2.46376], "fy":[-0.51202,-0.52287,-0.51451,-0.5148]}, + {"t":8.55749, "x":2.37485, "y":3.91842, "heading":1.42191, "vx":-0.29948, "vy":-0.26742, "omega":2.09348, "ax":-0.15023, "ay":-0.01625, "alpha":0.0, "fx":[-2.33376,-2.33514,-2.33518,-2.3317], "fy":[-0.25829,-0.24963,-0.25101,-0.25106]}, + {"t":8.58041, "x":2.36795, "y":3.91229, "heading":1.46989, "vx":-0.30292, "vy":-0.26779, "omega":2.09348, "ax":-0.14803, "ay":-0.00006, "alpha":0.0, "fx":[-2.29914,-2.30028,-2.30037,-2.29923], "fy":[0.00166,-0.00432,-0.00039,-0.00056]}, + {"t":8.60332, "x":2.36097, "y":3.90615, "heading":1.51787, "vx":-0.30632, "vy":-0.26779, "omega":2.09348, "ax":-0.14208, "ay":0.0155, "alpha":0.0, "fx":[-2.20545,-2.20705,-2.20711,-2.20935], "fy":[0.23941,0.24241,0.24081,0.24074]}, + {"t":8.62624, "x":2.35391, "y":3.90002, "heading":1.56584, "vx":-0.30957, "vy":-0.26744, "omega":2.09348, "ax":-0.14341, "ay":0.03051, "alpha":0.0, "fx":[-2.22875,-2.22726,-2.22725,-2.22874], "fy":[0.47487,0.47475,0.47332,0.47331]}, + {"t":8.64916, "x":2.34678, "y":3.8939, "heading":1.61382, "vx":-0.31286, "vy":-0.26674, "omega":2.09348, "ax":-0.1397, "ay":0.0451, "alpha":0.0, "fx":[-2.16753,-2.16892,-2.16885,-2.17596], "fy":[0.70375,0.70054,0.69914,0.69922]}, + {"t":8.67208, "x":2.33957, "y":3.8878, "heading":1.6618, "vx":-0.31606, "vy":-0.26571, "omega":2.09348, "ax":-0.14444, "ay":0.05935, "alpha":0.0, "fx":[-2.24567,-2.2418,-2.24213,-2.24601], "fy":[0.9208,0.92751,0.91989,0.92003]}, + {"t":8.695, "x":2.33229, "y":3.88172, "heading":1.70978, "vx":-0.31937, "vy":-0.26435, "omega":2.09348, "ax":-0.14279, "ay":0.07335, "alpha":0.0, "fx":[-2.21527,-2.21584,-2.21562,-2.22685], "fy":[1.14724,1.13735,1.13676,1.13699]}, + {"t":8.71791, "x":2.32493, "y":3.87568, "heading":1.75776, "vx":-0.32264, "vy":-0.26266, "omega":2.09348, "ax":-0.15088, "ay":0.08722, "alpha":0.0, "fx":[-2.34632,-2.34064,-2.34162,-2.3473], "fy":[1.35138,1.3659,1.35131,1.35155]}, + {"t":8.74083, "x":2.3175, "y":3.86969, "heading":1.80574, "vx":-0.3261, "vy":-0.26067, "omega":2.09348, "ax":-0.15123, "ay":0.10099, "alpha":0.0, "fx":[-2.3467,-2.34568,-2.34551,-2.36013], "fy":[1.58107,1.56416,1.56517,1.56535]}, + {"t":8.76375, "x":2.30998, "y":3.86374, "heading":1.85372, "vx":-0.32957, "vy":-0.25835, "omega":2.09348, "ax":-0.16271, "ay":0.1148, "alpha":0.0, "fx":[-2.53025,-2.52377,-2.52543,-2.53192], "fy":[1.77702,1.80042,1.77828,1.77847]}, + {"t":8.78667, "x":2.30239, "y":3.85785, "heading":1.9017, "vx":-0.3333, "vy":-0.25572, "omega":2.09348, "ax":-0.1651, "ay":0.12864, "alpha":0.0, "fx":[-2.56324,-2.55958,-2.56002,-2.57708], "fy":[2.01507,1.99077,1.99442,1.99398]}, + {"t":8.80959, "x":2.29471, "y":3.85202, "heading":1.94967, "vx":-0.33708, "vy":-0.25277, "omega":2.09348, "ax":-0.18016, "ay":0.14267, "alpha":0.0, "fx":[-2.80076,-2.79511,-2.79696,-2.80262], "fy":[2.20625,2.23996,2.21011,2.20976]}, + {"t":8.8325, "x":2.28693, "y":3.84627, "heading":1.99765, "vx":-0.34121, "vy":-0.2495, "omega":2.09348, "ax":-0.18474, "ay":0.15685, "alpha":0.0, "fx":[-2.87016,-2.86237,-2.86469,-2.8827], "fy":[2.45762,2.42541,2.43319,2.43088]}, + {"t":8.85542, "x":2.27906, "y":3.84059, "heading":2.04563, "vx":-0.34544, "vy":-0.24591, "omega":2.09348, "ax":-0.20371, "ay":0.17129, "alpha":0.0, "fx":[-3.16554,-3.16348,-3.16394,-3.166], "fy":[2.64537,2.69174,2.65491,2.65256]}, + {"t":8.87834, "x":2.27109, "y":3.835, "heading":2.09361, "vx":-0.35011, "vy":-0.24198, "omega":2.09348, "ax":-0.21075, "ay":0.18605, "alpha":0.0, "fx":[-3.27724,-3.2631,-3.27,-3.28613], "fy":[2.91576,2.87471,2.88884,2.88196]}, + {"t":8.90126, "x":2.26301, "y":3.8295, "heading":2.14159, "vx":-0.35494, "vy":-0.23772, "omega":2.09348, "ax":-0.23421, "ay":0.20099, "alpha":0.0, "fx":[-3.63777,-3.64413,-3.63934,-3.63297], "fy":[3.0976,3.16102,3.11986,3.11166]}, + {"t":8.92418, "x":2.25482, "y":3.82411, "heading":2.18957, "vx":-0.36031, "vy":-0.23311, "omega":2.09348, "ax":-0.24411, "ay":0.21656, "alpha":0.0, "fx":[-3.79962,-3.77575,-3.79304,-3.80135], "fy":[3.39545,3.34375,3.36764,3.35036]}, + {"t":8.9471, "x":2.2465, "y":3.81882, "heading":2.23755, "vx":-0.3659, "vy":-0.22815, "omega":2.09348, "ax":-0.27294, "ay":0.23193, "alpha":0.0, "fx":[-4.23794,-4.26158,-4.24265,-4.21898], "fy":[3.56147,3.65075,3.61207,3.58839]}, + {"t":8.97001, "x":2.23804, "y":3.81365, "heading":2.28552, "vx":-0.37216, "vy":-0.22283, "omega":2.09348, "ax":-0.28623, "ay":0.2486, "alpha":0.0, "fx":[-4.45867,-4.42013,-4.46029,-4.44765], "fy":[3.90232,3.83651,3.87503,3.83488]}, + {"t":8.99293, "x":2.22944, "y":3.80861, "heading":2.3335, "vx":-0.37872, "vy":-0.21714, "omega":2.09348, "ax":-0.32179, "ay":0.26402, "alpha":0.0, "fx":[-4.99713,-5.05444,-5.00134,-4.94398], "fy":[4.02779,4.16073,4.14022,4.07817]}, + {"t":9.01585, "x":2.22067, "y":3.8037, "heading":2.38148, "vx":-0.38609, "vy":-0.21108, "omega":2.09348, "ax":-0.33908, "ay":0.28228, "alpha":0.0, "fx":[-5.28312,-5.22447,-5.31264,-5.25063], "fy":[4.4428,4.35671,4.41513,4.32704]}, + {"t":9.03877, "x":2.21173, "y":3.79894, "heading":2.42946, "vx":-0.39386, "vy":-0.20461, "omega":2.09348, "ax":-0.38351, "ay":0.2968, "alpha":0.0, "fx":[-5.96343,-6.08048,-5.95251,-5.83542], "fy":[4.47734,4.68607,4.7144,4.56605]}, + {"t":9.06169, "x":2.20261, "y":3.79433, "heading":2.47744, "vx":-0.40265, "vy":-0.19781, "omega":2.09348, "ax":-0.40563, "ay":0.31737, "alpha":0.0, "fx":[-6.31283,-6.23466,-6.41106,-6.24791], "fy":[5.02172,4.90737,4.98446,4.80854]}, + {"t":9.0846, "x":2.19327, "y":3.78988, "heading":2.52542, "vx":-0.41195, "vy":-0.19054, "omega":2.09348, "ax":-0.46213, "ay":0.32926, "alpha":0.0, "fx":[-7.21099,-7.40906,-7.14796,-6.94976], "fy":[4.89203,5.21331,5.33,5.02541]}, + {"t":9.10752, "x":2.18371, "y":3.7856, "heading":2.5734, "vx":-0.42254, "vy":-0.18299, "omega":2.09348, "ax":-0.49097, "ay":0.35252, "alpha":0.0, "fx":[-7.61811,-7.54184,-7.83327,-7.51684], "fy":[5.61986,5.47666,5.54958,5.26008]}, + {"t":9.13044, "x":2.1739, "y":3.7815, "heading":2.62138, "vx":-0.43379, "vy":-0.17491, "omega":2.09348, "ax":-0.5639, "ay":0.35985, "alpha":0.0, "fx":[-8.8428,-9.08388,-8.67885,-8.43653], "fy":[5.29546,5.71213,5.91356,5.44043]}, + {"t":9.15336, "x":2.16381, "y":3.77758, "heading":2.66935, "vx":-0.44672, "vy":-0.16667, "omega":2.09348, "ax":-0.60456, "ay":0.38358, "alpha":0.0, "fx":[-9.35281,-9.32137,-9.652,-9.24243], "fy":[6.13288,5.99286,6.01777,5.69283]}, + {"t":9.17628, "x":2.15341, "y":3.77386, "heading":2.71733, "vx":-0.46057, "vy":-0.15788, "omega":2.09348, "ax":-0.69916, "ay":0.38648, "alpha":0.0, "fx":[-10.96995,-11.08893,-10.75715,-10.63097], "fy":[5.79809,6.12383,6.23621,5.85846]}, + {"t":9.19919, "x":2.14267, "y":3.77035, "heading":2.76531, "vx":-0.4766, "vy":-0.14902, "omega":2.09348, "ax":-0.7634, "ay":0.40208, "alpha":0.0, "fx":[-11.79208,-11.89949,-11.93856,-11.80908], "fy":[6.27748,6.30227,6.24384,6.16264]}, + {"t":9.22211, "x":2.13155, "y":3.76704, "heading":2.81329, "vx":-0.49409, "vy":-0.1398, "omega":2.09348, "ax":-0.88766, "ay":0.40304, "alpha":0.0, "fx":[-13.76371,-13.57113,-13.83258,-13.99361], "fy":[6.40663,6.31155,5.97199,6.35573]}, + {"t":9.24503, "x":2.11999, "y":3.76394, "heading":2.86127, "vx":-0.51443, "vy":-0.13057, "omega":2.09348, "ax":-0.97933, "ay":0.39468, "alpha":0.0, "fx":[-15.19552,-15.06446,-15.02227,-15.57558], "fy":[5.98608,6.00458,5.91266,6.62295]}, + {"t":9.26795, "x":2.10794, "y":3.76105, "heading":2.90925, "vx":-0.53688, "vy":-0.12152, "omega":2.09348, "ax":-1.17929, "ay":0.37993, "alpha":-0.00056, "fx":[-18.39218,-18.08653,-18.2956,-18.50918], "fy":[5.76265,6.03917,5.53361,6.27401]}, + {"t":9.29087, "x":2.09533, "y":3.75837, "heading":2.95723, "vx":-0.56391, "vy":-0.11281, "omega":2.09347, "ax":-1.1195, "ay":0.27179, "alpha":-7.41326, "fx":[-34.15047,-41.38741,-1.1878,7.15737], "fy":[26.38306,-10.93996,-26.17112,27.61767]}, + {"t":9.31379, "x":2.08211, "y":3.75585, "heading":3.00521, "vx":-0.58956, "vy":-0.10659, "omega":1.92357, "ax":-0.09177, "ay":0.00153, "alpha":-15.94233, "fx":[-41.96614,-54.07899,38.40638,51.93595], "fy":[51.73978,-38.91268,-54.39294,41.66073]}, + {"t":9.3367, "x":2.06858, "y":3.75341, "heading":3.04929, "vx":-0.59167, "vy":-0.10655, "omega":1.5582, "ax":-0.07601, "ay":-0.00391, "alpha":-16.02251, "fx":[-44.16906,-52.35616,41.25998,50.54206], "fy":[50.2929,-41.70619,-52.68819,43.85878]}, + {"t":9.35962, "x":2.055, "y":3.75097, "heading":3.085, "vx":-0.59341, "vy":-0.10664, "omega":1.191, "ax":-0.07606, "ay":-0.00667, "alpha":-16.04872, "fx":[-46.01481,-50.91696,43.18159,49.02366], "fy":[48.75754,-43.61561,-51.27151,45.7151]}, + {"t":9.38254, "x":2.04138, "y":3.74852, "heading":3.1123, "vx":-0.59515, "vy":-0.10679, "omega":0.82319, "ax":-0.08296, "ay":-0.00928, "alpha":-16.06092, "fx":[-47.48231,-49.83537,44.46783,47.69469], "fy":[47.40633,-44.92797,-50.23461,47.17983]}, + {"t":9.40546, "x":2.02772, "y":3.74607, "heading":3.13116, "vx":-0.59705, "vy":-0.10701, "omega":0.45511, "ax":-0.10041, "ay":-0.01319, "alpha":-16.06601, "fx":[-48.68126,-49.2123,45.08763,46.56643], "fy":[46.22215,-45.65799,-49.72385,48.33985]}, + {"t":9.42838, "x":2.01401, "y":3.74361, "heading":3.14159, "vx":-0.59935, "vy":-0.10731, "omega":0.0869, "ax":-0.03902, "ay":0.15107, "alpha":-1.84065, "fx":[-5.77694,-5.99727,4.65692,4.69256], "fy":[7.82967,-3.39202,-3.09206,8.04251]}, + {"t":9.47428, "x":1.98645, "y":3.73885, "heading":-3.1376, "vx":-0.60114, "vy":-0.10037, "omega":0.00241, "ax":0.0002, "ay":-0.00121, "alpha":-0.0983, "fx":[-0.77885,-0.7766,1.08458,0.48338], "fy":[-0.51215,0.17895,0.18662,0.07166]}, + {"t":9.52018, "x":1.95886, "y":3.73424, "heading":-3.13749, "vx":-0.60114, "vy":-0.10043, "omega":-0.0021, "ax":-0.0001, "ay":0.00063, "alpha":-0.00342, "fx":[0.03471,0.034,-0.03772,-0.03748], "fy":[0.06576,-0.27081,0.17888,0.06508]}, + {"t":9.56608, "x":1.93126, "y":3.72963, "heading":-3.13759, "vx":-0.60114, "vy":-0.1004, "omega":-0.00226, "ax":-0.00016, "ay":0.00098, "alpha":0.00058, "fx":[0.03037,0.03037,0.06387,-0.13478], "fy":[0.07002,-0.01418,-0.01411,0.01912]}, + {"t":9.61199, "x":1.90367, "y":3.72502, "heading":-3.13769, "vx":-0.60115, "vy":-0.10035, "omega":-0.00223, "ax":0.0, "ay":-0.00001, "alpha":0.00012, "fx":[-0.00961,-0.00959,0.00964,0.00962], "fy":[-0.01033,0.06962,-0.04932,-0.01028]}, + {"t":9.65789, "x":1.87608, "y":3.72042, "heading":-3.1378, "vx":-0.60115, "vy":-0.10035, "omega":-0.00223, "ax":0.00004, "ay":-0.00022, "alpha":-0.00028, "fx":[0.00205,0.00185,-0.04637,0.04475], "fy":[0.05453,-0.0065,-0.00672,-0.05494]}, + {"t":9.70379, "x":1.84848, "y":3.71581, "heading":-3.1379, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.00001, "ay":-0.00005, "alpha":-0.00006, "fx":[-0.01145,-0.01142,0.0117,0.01166], "fy":[-0.01184,0.06753,-0.04678,-0.0118]}, + {"t":9.7497, "x":1.82089, "y":3.7112, "heading":-3.138, "vx":-0.60115, "vy":-0.10037, "omega":-0.00224, "ax":-0.00001, "ay":0.00007, "alpha":-0.0001, "fx":[0.0012,0.00104,-0.033,0.02998], "fy":[0.04145,-0.00081,-0.00097,-0.03501]}, + {"t":9.7956, "x":1.79329, "y":3.70659, "heading":-3.1381, "vx":-0.60115, "vy":-0.10036, "omega":-0.00225, "ax":0.0, "ay":-0.00001, "alpha":0.00011, "fx":[-0.00713,-0.00712,0.00719,0.00718], "fy":[-0.00794,0.04408,-0.02889,-0.00793]}, + {"t":9.8415, "x":1.7657, "y":3.70199, "heading":-3.13821, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":0.00001, "alpha":-0.00006, "fx":[0.00235,0.00224,-0.01674,0.01201], "fy":[0.02744,-0.00247,-0.00258,-0.02156]}, + {"t":9.88741, "x":1.7381, "y":3.69738, "heading":-3.13831, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":-0.00002, "alpha":0.00008, "fx":[-0.00411,-0.00411,0.00419,0.00419], "fy":[-0.00482,0.02551,-0.0168,-0.00483]}, + {"t":9.93331, "x":1.71051, "y":3.69277, "heading":-3.13841, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":0.0, "alpha":-0.00006, "fx":[0.00225,0.00219,-0.0068,0.00232], "fy":[0.01687,-0.0025,-0.00256,-0.01155]}, + {"t":9.97921, "x":1.68292, "y":3.68817, "heading":-3.13851, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":0.0, "alpha":0.00004, "fx":[-0.00218,-0.00219,0.00219,0.0022], "fy":[-0.00244,0.01359,-0.00883,-0.00246]}, + {"t":10.02511, "x":1.65532, "y":3.68356, "heading":-3.13862, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":0.0, "alpha":-0.00004, "fx":[0.00156,0.00153,-0.00223,-0.00088], "fy":[0.00917,-0.00175,-0.00178,-0.00554]}, + {"t":10.07102, "x":1.62773, "y":3.67895, "heading":-3.13872, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":0.0, "alpha":0.00002, "fx":[-0.001,-0.00101,0.001,0.00101], "fy":[-0.00112,0.00647,-0.00419,-0.00114]}, + {"t":10.11692, "x":1.60013, "y":3.67435, "heading":-3.13882, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[0.00087,0.00085,-0.0005,-0.00121], "fy":[0.00428,-0.00097,-0.00099,-0.00234]}, + {"t":10.16282, "x":1.57254, "y":3.66974, "heading":-3.13893, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[-0.00038,-0.00038,0.00038,0.00038], "fy":[-0.00044,0.00263,-0.00171,-0.00045]}, + {"t":10.20873, "x":1.54494, "y":3.66513, "heading":-3.13903, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[0.00038,0.00037,0.00003,-0.00077], "fy":[0.0016,-0.00043,-0.00044,-0.00077]}, + {"t":10.25463, "x":1.51735, "y":3.66052, "heading":-3.13913, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[-0.00008,-0.00008,0.00008,0.00008], "fy":[-0.00011,0.00073,-0.00047,-0.00011]}, + {"t":10.30053, "x":1.48975, "y":3.65592, "heading":-3.13924, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00011,0.00011,0.00021,-0.00043], "fy":[0.00029,-0.00014,-0.00014,-0.00005]}, + {"t":10.34643, "x":1.46216, "y":3.65131, "heading":-3.13934, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00006,0.00006,-0.00006,-0.00007], "fy":[0.00006,-0.00021,0.00014,0.00007]}, + {"t":10.39234, "x":1.43457, "y":3.6467, "heading":-3.13944, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00008,0.00009,0.00057,-0.00072], "fy":[-0.00026,-0.00011,-0.0001,0.00038]}, + {"t":10.43824, "x":1.40697, "y":3.6421, "heading":-3.13954, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00018,0.00018,-0.00019,-0.00019], "fy":[0.00021,-0.00089,0.00059,0.00022]}, + {"t":10.48414, "x":1.37938, "y":3.63749, "heading":-3.13965, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00035,0.00036,0.00176,-0.00243], "fy":[-0.00044,-0.0004,-0.00038,0.00102]}, + {"t":10.53005, "x":1.35178, "y":3.63288, "heading":-3.13975, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":0.00001, "alpha":0.0, "fx":[0.00041,0.00042,-0.00044,-0.00045], "fy":[0.00051,-0.00208,0.00137,0.00052]}, + {"t":10.57595, "x":1.32419, "y":3.62828, "heading":-3.13985, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":-0.00001, "alpha":0.0, "fx":[0.00116,0.00117,0.00497,-0.00722], "fy":[-0.00054,-0.00126,-0.00125,0.00255]}, + {"t":10.62185, "x":1.29659, "y":3.62367, "heading":-3.13996, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":0.00001, "alpha":0.0, "fx":[0.00096,0.00096,-0.00102,-0.00103], "fy":[0.00117,-0.00479,0.00317,0.00118]}, + {"t":10.66776, "x":1.269, "y":3.61906, "heading":-3.14006, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":-0.00002, "alpha":0.0, "fx":[0.00284,0.00286,0.0118,-0.01734], "fy":[-0.00083,-0.00306,-0.00304,0.0059]}, + {"t":10.71366, "x":1.24141, "y":3.61445, "heading":-3.14016, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":0.00002, "alpha":0.0, "fx":[0.002,0.002,-0.00211,-0.00211], "fy":[0.00237,-0.01019,0.00676,0.00237]}, + {"t":10.75956, "x":1.21381, "y":3.60985, "heading":-3.14026, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":-0.00002, "alpha":-0.00001, "fx":[0.00505,0.00508,0.02178,-0.03168], "fy":[-0.002,-0.00538,-0.00535,0.01135]}, + {"t":10.80546, "x":1.18622, "y":3.60524, "heading":-3.14037, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.0, "ay":0.00001, "alpha":0.00001, "fx":[0.00309,0.00309,-0.00314,-0.00314], "fy":[0.0032,-0.01696,0.01119,0.0032]}, + {"t":10.85137, "x":1.15862, "y":3.60063, "heading":-3.14047, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":-0.00001, "ay":0.00003, "alpha":-0.00002, "fx":[0.00277,0.0028,0.01888,-0.02479], "fy":[-0.00668,-0.00248,-0.00246,0.01363]}, + {"t":10.89727, "x":1.13103, "y":3.59603, "heading":-3.14057, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.00002, "ay":-0.00011, "alpha":-0.00003, "fx":[-0.00219,-0.00218,0.00278,0.00278], "fy":[-0.00407,0.00274,-0.00171,-0.00406]}, + {"t":10.94317, "x":1.10343, "y":3.59142, "heading":-3.14068, "vx":-0.60115, "vy":-0.10037, "omega":-0.00224, "ax":-0.00003, "ay":0.0002, "alpha":0.00011, "fx":[-0.04835,-0.04843,-0.16619,0.26088], "fy":[-0.02438,0.05159,0.05151,-0.06624]}, + {"t":10.98908, "x":1.07584, "y":3.58681, "heading":-3.14078, "vx":-0.60115, "vy":-0.10036, "omega":-0.00224, "ax":0.00004, "ay":-0.00021, "alpha":-0.00007, "fx":[-0.01957,-0.0196,0.02084,0.02079], "fy":[-0.02312,0.10316,-0.0703,-0.02309]}, + {"t":11.03498, "x":1.04824, "y":3.58221, "heading":-3.14088, "vx":-0.60114, "vy":-0.10037, "omega":-0.00224, "ax":0.47623, "ay":0.07962, "alpha":-0.00006, "fx":[7.41626,7.41622,7.49441,7.2673], "fy":[1.20206,1.21918,1.21879,1.30793]}, + {"t":11.08088, "x":1.02115, "y":3.57768, "heading":-3.14098, "vx":-0.57928, "vy":-0.09671, "omega":-0.00224, "ax":2.10158, "ay":0.35086, "alpha":-0.00011, "fx":[32.6431,32.63816,32.65846,32.65706], "fy":[5.4436,5.49744,5.42008,5.44224]}, + {"t":11.12678, "x":0.99678, "y":3.57361, "heading":-3.14109, "vx":-0.48282, "vy":-0.08061, "omega":-0.00225, "ax":2.10303, "ay":0.35111, "alpha":0.00035, "fx":[32.64591,32.64506,32.61088,32.78503], "fy":[5.47445,5.47941,5.4864,5.37834]}, + {"t":11.17269, "x":0.97683, "y":3.57028, "heading":-3.14119, "vx":-0.38628, "vy":-0.06449, "omega":-0.00223, "ax":2.10351, "ay":0.35119, "alpha":-0.00017, "fx":[32.6776,32.67706,32.68085,32.68129], "fy":[5.45474,5.4595,5.45408,5.45534]}, + {"t":11.21859, "x":0.96131, "y":3.56769, "heading":-3.14129, "vx":-0.28972, "vy":-0.04837, "omega":-0.00224, "ax":2.10375, "ay":0.35123, "alpha":0.0003, "fx":[32.67604,32.67238,32.67347,32.70984], "fy":[5.43921,5.46668,5.46735,5.45294]}, + {"t":11.26449, "x":0.95023, "y":3.56584, "heading":-3.1414, "vx":-0.19315, "vy":-0.03225, "omega":-0.00223, "ax":2.1039, "ay":0.35125, "alpha":0.00358, "fx":[32.69183,32.6876,32.68034,32.68091], "fy":[5.44003,5.49172,5.45535,5.4406]}, + {"t":11.3104, "x":0.94358, "y":3.56473, "heading":-3.1415, "vx":-0.09658, "vy":-0.01612, "omega":-0.00206, "ax":2.10399, "ay":0.35127, "alpha":0.04494, "fx":[32.80346,32.78406,32.57087,32.58826], "fy":[5.29008,5.60672,5.62423,5.30768]}, + {"t":11.3563, "x":0.94136, "y":3.56436, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0,37,132,225] + }, + "events":[] +} diff --git a/src/main/deploy/pathplanner/Tests.path b/src/main/deploy/pathplanner/Tests.path deleted file mode 100644 index 9cb675b..0000000 --- a/src/main/deploy/pathplanner/Tests.path +++ /dev/null @@ -1,234 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 7.855128995972278, - "y": 3.8506658441888315 - }, - "prevControl": null, - "nextControl": { - "x": 9.51563917413549, - "y": 3.864980587104032 - }, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.673101346202692, - "y": 4.909956819913639 - }, - "prevControl": { - "x": 9.658786603287492, - "y": 4.2657933887296355 - }, - "nextControl": { - "x": 9.658786603287492, - "y": 4.2657933887296355 - }, - "holonomicAngle": 0.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.744675060778693, - "y": 3.278076127580827 - }, - "prevControl": { - "x": 9.701730832033093, - "y": 3.922239558764832 - }, - "nextControl": { - "x": 9.701730832033093, - "y": 3.922239558764832 - }, - "holonomicAngle": 0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 7.855128995972278, - "y": 3.8506658441888315 - }, - "prevControl": { - "x": 9.071882143764288, - "y": 3.879295330019232 - }, - "nextControl": { - "x": 9.071882143764288, - "y": 3.879295330019232 - }, - "holonomicAngle": 0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.673101346202692, - "y": 4.89564207699844 - }, - "prevControl": { - "x": 9.687416089117892, - "y": 4.222849159984035 - }, - "nextControl": { - "x": 9.687416089117892, - "y": 4.222849159984035 - }, - "holonomicAngle": 125.92486296979446, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.730360317863493, - "y": 3.263761384665627 - }, - "prevControl": { - "x": 9.748383531854783, - "y": 4.107878917512221 - }, - "nextControl": { - "x": 9.748383531854783, - "y": 4.107878917512221 - }, - "holonomicAngle": -91.03738139006352, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.013854168059215, - "y": 3.3824178174672146 - }, - "prevControl": { - "x": 9.077332014313154, - "y": 4.307380720024598 - }, - "nextControl": { - "x": 9.077332014313154, - "y": 4.307380720024598 - }, - "holonomicAngle": 59.62087398863161, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 8.315597859265894, - "y": 3.3461447624649643 - }, - "prevControl": { - "x": 8.415348760522082, - "y": 3.863035796247031 - }, - "nextControl": { - "x": 8.415348760522082, - "y": 3.863035796247031 - }, - "holonomicAngle": -155.7255588655606, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 7.84404814423664, - "y": 3.863035796247031 - }, - "prevControl": { - "x": 8.78714757429515, - "y": 3.8086262137436555 - }, - "nextControl": null, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 2.0, - "maxAcceleration": 3.0, - "isReversed": null, - "markers": [ - { - "position": 3.4181818181818175, - "names": [ - "marker" - ] - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/CurveLimelightTest.auto b/src/main/deploy/pathplanner/autos/CurveLimelightTest.auto deleted file mode 100644 index a070e6f..0000000 --- a/src/main/deploy/pathplanner/autos/CurveLimelightTest.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "CurvePathForLimelight" - } - } - ] - } - }, - "resetOdom": true, - "folder": "TestAutos", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HELPLeftAuto (Coral x3).auto b/src/main/deploy/pathplanner/autos/HELPLeftAuto (Coral x3).auto deleted file mode 100644 index cbd3b76..0000000 --- a/src/main/deploy/pathplanner/autos/HELPLeftAuto (Coral x3).auto +++ /dev/null @@ -1,85 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Help" - } - }, - { - "type": "path", - "data": { - "pathName": "HelpToPos1" - } - }, - { - "type": "path", - "data": { - "pathName": "Pos1ToReef" - } - }, - { - "type": "named", - "data": { - "name": "Eject Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefToStationL" - } - }, - { - "type": "named", - "data": { - "name": "Intake Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "StationLToReef2" - } - }, - { - "type": "named", - "data": { - "name": "Eject Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Reef2ToStationL" - } - }, - { - "type": "named", - "data": { - "name": "Intake Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "StationLToReef3" - } - }, - { - "type": "named", - "data": { - "name": "Eject Coral" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HELPMOBILITY RightAuto (Coral x3).auto b/src/main/deploy/pathplanner/autos/HELPMOBILITY RightAuto (Coral x3).auto deleted file mode 100644 index 39704d5..0000000 --- a/src/main/deploy/pathplanner/autos/HELPMOBILITY RightAuto (Coral x3).auto +++ /dev/null @@ -1,73 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "HELPMOBILITY Pos2ToReef SlightRight" - } - }, - { - "type": "named", - "data": { - "name": "Eject Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "HELPMOBILITY Reef2ToStationR" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "StationRToReef2" - } - }, - { - "type": "named", - "data": { - "name": "Eject Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Reef2ToStationR" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "StationRToReef3" - } - }, - { - "type": "named", - "data": { - "name": "Eject Coral" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LeftAuto (Coral x2, Algae).auto b/src/main/deploy/pathplanner/autos/LeftAuto (Coral x2, Algae).auto deleted file mode 100644 index f373fc0..0000000 --- a/src/main/deploy/pathplanner/autos/LeftAuto (Coral x2, Algae).auto +++ /dev/null @@ -1,61 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Pos1ToReef" - } - }, - { - "type": "named", - "data": { - "name": "Eject Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefToStationL" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "StationLToReef2" - } - }, - { - "type": "named", - "data": { - "name": "Eject Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Reef2ToAlgae" - } - }, - { - "type": "named", - "data": { - "name": "Intake Algae" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LeftAuto (Coral x3).auto b/src/main/deploy/pathplanner/autos/LeftAuto (Coral x3).auto deleted file mode 100644 index 494ffe6..0000000 --- a/src/main/deploy/pathplanner/autos/LeftAuto (Coral x3).auto +++ /dev/null @@ -1,73 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Pos1ToReef" - } - }, - { - "type": "named", - "data": { - "name": "Eject Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefToStationL" - } - }, - { - "type": "named", - "data": { - "name": "Intake Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "StationLToReef2" - } - }, - { - "type": "named", - "data": { - "name": "Eject Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Reef2ToStationL" - } - }, - { - "type": "named", - "data": { - "name": "Intake Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "StationLToReef3" - } - }, - { - "type": "named", - "data": { - "name": "Eject Coral" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LeftAuto (Coral, Algae).auto b/src/main/deploy/pathplanner/autos/LeftAuto (Coral, Algae).auto deleted file mode 100644 index e87c77d..0000000 --- a/src/main/deploy/pathplanner/autos/LeftAuto (Coral, Algae).auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Pos1ToReef" - } - }, - { - "type": "named", - "data": { - "name": "Eject Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefToAlgae1" - } - }, - { - "type": "named", - "data": { - "name": "Intake Algae" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MiddleAuto Middle.auto b/src/main/deploy/pathplanner/autos/MiddleAuto Middle.auto deleted file mode 100644 index 4c3f287..0000000 --- a/src/main/deploy/pathplanner/autos/MiddleAuto Middle.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0 - } - }, - { - "type": "path", - "data": { - "pathName": "Pos2ToReef Middle" - } - }, - { - "type": "named", - "data": { - "name": "Eject Coral" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MiddleAuto SlightLeft.auto b/src/main/deploy/pathplanner/autos/MiddleAuto SlightLeft.auto deleted file mode 100644 index eac76ba..0000000 --- a/src/main/deploy/pathplanner/autos/MiddleAuto SlightLeft.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0 - } - }, - { - "type": "path", - "data": { - "pathName": "Pos2ToReef SlightLeft" - } - }, - { - "type": "named", - "data": { - "name": "Eject Coral" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MiddleAuto SlightRight.auto b/src/main/deploy/pathplanner/autos/MiddleAuto SlightRight.auto deleted file mode 100644 index 7d61c97..0000000 --- a/src/main/deploy/pathplanner/autos/MiddleAuto SlightRight.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.0 - } - }, - { - "type": "path", - "data": { - "pathName": "Pos2ToReef SlightRight" - } - }, - { - "type": "named", - "data": { - "name": "Eject Coral" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MobilityAuto.auto b/src/main/deploy/pathplanner/autos/MobilityAuto.auto deleted file mode 100644 index 83a99a2..0000000 --- a/src/main/deploy/pathplanner/autos/MobilityAuto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.0 - } - }, - { - "type": "path", - "data": { - "pathName": "Mobility" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RightAuto (Coral x3).auto b/src/main/deploy/pathplanner/autos/RightAuto (Coral x3).auto deleted file mode 100644 index 7819569..0000000 --- a/src/main/deploy/pathplanner/autos/RightAuto (Coral x3).auto +++ /dev/null @@ -1,73 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Pos3ToReef" - } - }, - { - "type": "named", - "data": { - "name": "Eject Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefToStationR" - } - }, - { - "type": "named", - "data": { - "name": "Intake Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "StationRToReef2" - } - }, - { - "type": "named", - "data": { - "name": "Eject Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Reef2ToStationR" - } - }, - { - "type": "named", - "data": { - "name": "Intake Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "StationRToReef3" - } - }, - { - "type": "named", - "data": { - "name": "Eject Coral" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RightAuto (Coral, Algae x2).auto b/src/main/deploy/pathplanner/autos/RightAuto (Coral, Algae x2).auto deleted file mode 100644 index 8ad5b86..0000000 --- a/src/main/deploy/pathplanner/autos/RightAuto (Coral, Algae x2).auto +++ /dev/null @@ -1,73 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Pos3ToReef" - } - }, - { - "type": "named", - "data": { - "name": "Eject Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefToAlgae3" - } - }, - { - "type": "named", - "data": { - "name": "Intake Algae" - } - }, - { - "type": "path", - "data": { - "pathName": "Algae3ToProcessor" - } - }, - { - "type": "named", - "data": { - "name": "Eject Algae" - } - }, - { - "type": "path", - "data": { - "pathName": "ProcessorToAlgae2" - } - }, - { - "type": "named", - "data": { - "name": "Intake Algae" - } - }, - { - "type": "path", - "data": { - "pathName": "Algae2ToProcessor" - } - }, - { - "type": "named", - "data": { - "name": "Eject Algae" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ShortStraightTest.auto b/src/main/deploy/pathplanner/autos/ShortStraightTest.auto deleted file mode 100644 index a3f01c4..0000000 --- a/src/main/deploy/pathplanner/autos/ShortStraightTest.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "TestPath" - } - } - ] - } - }, - "resetOdom": true, - "folder": "TestAutos", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/StraightLimelightTest.auto b/src/main/deploy/pathplanner/autos/StraightLimelightTest.auto deleted file mode 100644 index 361fcb1..0000000 --- a/src/main/deploy/pathplanner/autos/StraightLimelightTest.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "StraightPathForLimelight" - } - } - ] - } - }, - "resetOdom": true, - "folder": "TestAutos", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json deleted file mode 100644 index 23e0db9..0000000 --- a/src/main/deploy/pathplanner/navgrid.json +++ /dev/null @@ -1 +0,0 @@ -{"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/Algae2ToProcessor.path b/src/main/deploy/pathplanner/paths/Algae2ToProcessor.path deleted file mode 100755 index bbc0f54..0000000 --- a/src/main/deploy/pathplanner/paths/Algae2ToProcessor.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.67, - "y": 4.094 - }, - "prevControl": null, - "nextControl": { - "x": 2.736067767351275, - "y": 2.7748416474858346 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.0, - "y": 0.5423151765046293 - }, - "prevControl": { - "x": 6.199193865740741, - "y": 1.3884895833333322 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -70.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Algae3ToProcessor.path b/src/main/deploy/pathplanner/paths/Algae3ToProcessor.path deleted file mode 100755 index f46bb44..0000000 --- a/src/main/deploy/pathplanner/paths/Algae3ToProcessor.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.644, - "y": 2.178 - }, - "prevControl": null, - "nextControl": { - "x": 3.284171628049267, - "y": 1.625800423208023 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.0, - "y": 0.48874168113425864 - }, - "prevControl": { - "x": 6.205608623545862, - "y": 1.0959125003688621 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -70.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CurvePathForLimelight.path b/src/main/deploy/pathplanner/paths/CurvePathForLimelight.path deleted file mode 100755 index 862f535..0000000 --- a/src/main/deploy/pathplanner/paths/CurvePathForLimelight.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.0, - "y": 0.5 - }, - "prevControl": null, - "nextControl": { - "x": 6.662395597759046, - "y": 0.4853380705180928 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.0, - "y": 0.775079345703125 - }, - "prevControl": { - "x": 5.585608873869243, - "y": 0.3631122789884864 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 140.0 - }, - "reversed": false, - "folder": "TestPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HELPMOBILITY Pos2ToReef SlightRight.path b/src/main/deploy/pathplanner/paths/HELPMOBILITY Pos2ToReef SlightRight.path deleted file mode 100755 index 9b90fd0..0000000 --- a/src/main/deploy/pathplanner/paths/HELPMOBILITY Pos2ToReef SlightRight.path +++ /dev/null @@ -1,61 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.5, - "y": 4.0 - }, - "prevControl": null, - "nextControl": { - "x": 6.538988905120828, - "y": 4.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.8, - "y": 3.8 - }, - "prevControl": { - "x": 6.055655952782719, - "y": 3.8031145927959704 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Eject Coral", - "waypointRelativePos": 1.0, - "endWaypointRelativePos": null, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HELPMOBILITY Reef2ToStationR.path b/src/main/deploy/pathplanner/paths/HELPMOBILITY Reef2ToStationR.path deleted file mode 100755 index b3826cf..0000000 --- a/src/main/deploy/pathplanner/paths/HELPMOBILITY Reef2ToStationR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.8, - "y": 3.8 - }, - "prevControl": null, - "nextControl": { - "x": 6.769397886839207, - "y": 1.8773356621696027 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.046663730524316, - "y": 0.5888928592028526 - }, - "prevControl": { - "x": 2.3104617629317215, - "y": 1.5867231543417402 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -125.00000000000001 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Help.path b/src/main/deploy/pathplanner/paths/Help.path deleted file mode 100755 index 40048de..0000000 --- a/src/main/deploy/pathplanner/paths/Help.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 8.150993214602051, - "y": 7.5 - }, - "prevControl": null, - "nextControl": { - "x": 6.858203116723177, - "y": 7.5 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.0, - "y": 7.5 - }, - "prevControl": { - "x": 7.363738636968835, - "y": 7.5 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 180.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HelpToPos1.path b/src/main/deploy/pathplanner/paths/HelpToPos1.path deleted file mode 100755 index 634ff3c..0000000 --- a/src/main/deploy/pathplanner/paths/HelpToPos1.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.0, - "y": 7.5 - }, - "prevControl": null, - "nextControl": { - "x": 7.2628906642480935, - "y": 6.228770434702727 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.3, - "y": 5.5 - }, - "prevControl": { - "x": 7.230089406499463, - "y": 5.753810642766122 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 180.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Mobility.path b/src/main/deploy/pathplanner/paths/Mobility.path deleted file mode 100755 index a06f646..0000000 --- a/src/main/deploy/pathplanner/paths/Mobility.path +++ /dev/null @@ -1,61 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.3, - "y": 4.0 - }, - "prevControl": null, - "nextControl": { - "x": 7.010220393094382, - "y": 4.0060923771309085 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.212301136363636, - "y": 4.0 - }, - "prevControl": { - "x": 6.467957089146355, - "y": 4.003114592795971 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Eject Coral", - "waypointRelativePos": 1.0, - "endWaypointRelativePos": null, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Pos1ToReef.path b/src/main/deploy/pathplanner/paths/Pos1ToReef.path deleted file mode 100755 index f2e3318..0000000 --- a/src/main/deploy/pathplanner/paths/Pos1ToReef.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.3, - "y": 5.5 - }, - "prevControl": null, - "nextControl": { - "x": 6.4331660960750074, - "y": 5.51604768260885 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.063, - "y": 5.069 - }, - "prevControl": { - "x": 5.467573743888087, - "y": 5.113099569161518 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -119.99999999999999 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Pos2ToReef Middle.path b/src/main/deploy/pathplanner/paths/Pos2ToReef Middle.path deleted file mode 100755 index 58787bb..0000000 --- a/src/main/deploy/pathplanner/paths/Pos2ToReef Middle.path +++ /dev/null @@ -1,61 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.3, - "y": 4.0 - }, - "prevControl": null, - "nextControl": { - "x": 7.010220393094382, - "y": 4.0060923771309085 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.852791030534352, - "y": 4.0 - }, - "prevControl": { - "x": 6.108446983317071, - "y": 4.003114592795971 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Eject Coral", - "waypointRelativePos": 1.0, - "endWaypointRelativePos": null, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 2.75, - "maxAcceleration": 2.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Pos2ToReef SlightLeft.path b/src/main/deploy/pathplanner/paths/Pos2ToReef SlightLeft.path deleted file mode 100755 index 6946831..0000000 --- a/src/main/deploy/pathplanner/paths/Pos2ToReef SlightLeft.path +++ /dev/null @@ -1,61 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.3, - "y": 4.0 - }, - "prevControl": null, - "nextControl": { - "x": 7.010220393094382, - "y": 4.0060923771309085 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.8, - "y": 4.2 - }, - "prevControl": { - "x": 6.055655952782719, - "y": 4.203114592795971 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Eject Coral", - "waypointRelativePos": 1.0, - "endWaypointRelativePos": null, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Pos2ToReef SlightRight.path b/src/main/deploy/pathplanner/paths/Pos2ToReef SlightRight.path deleted file mode 100755 index 1f78b6f..0000000 --- a/src/main/deploy/pathplanner/paths/Pos2ToReef SlightRight.path +++ /dev/null @@ -1,61 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.3, - "y": 4.0 - }, - "prevControl": null, - "nextControl": { - "x": 7.010220393094382, - "y": 4.0060923771309085 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.8, - "y": 3.8 - }, - "prevControl": { - "x": 6.055655952782719, - "y": 3.8031145927959704 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Eject Coral", - "waypointRelativePos": 1.0, - "endWaypointRelativePos": null, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Pos3ToReef.path b/src/main/deploy/pathplanner/paths/Pos3ToReef.path deleted file mode 100755 index affcd67..0000000 --- a/src/main/deploy/pathplanner/paths/Pos3ToReef.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.25, - "y": 2.5 - }, - "prevControl": null, - "nextControl": { - "x": 5.931081098259078, - "y": 2.5997788279403156 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.145340909090908, - "y": 2.8832528409090914 - }, - "prevControl": { - "x": 6.6540303561665155, - "y": 2.4230708122332083 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 119.99999999999999 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ProcessorToAlgae2.path b/src/main/deploy/pathplanner/paths/ProcessorToAlgae2.path deleted file mode 100755 index 4c8fe76..0000000 --- a/src/main/deploy/pathplanner/paths/ProcessorToAlgae2.path +++ /dev/null @@ -1,70 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 6.0, - "y": 0.5048137297453695 - }, - "prevControl": null, - "nextControl": { - "x": 3.1288309135977346, - "y": 2.1227820371108077 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.655554520456414, - "y": 3.10368363229852 - }, - "prevControl": { - "x": 3.2786354466488485, - "y": 2.1816531532689147 - }, - "nextControl": { - "x": 2.5155766937595994, - "y": 3.310821769861932 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.67, - "y": 4.094 - }, - "prevControl": { - "x": 2.3040964355468745, - "y": 4.10819959138569 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -70.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ProcessorToAlgae3.path b/src/main/deploy/pathplanner/paths/ProcessorToAlgae3.path deleted file mode 100755 index 5760663..0000000 --- a/src/main/deploy/pathplanner/paths/ProcessorToAlgae3.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 6.356, - "y": 0.515 - }, - "prevControl": null, - "nextControl": { - "x": 3.256259722947189, - "y": 1.5477146485115298 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.844, - "y": 2.178 - }, - "prevControl": { - "x": 2.8420963262884644, - "y": 1.8348344378863908 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ProcessorToReef.path b/src/main/deploy/pathplanner/paths/ProcessorToReef.path deleted file mode 100755 index 9689860..0000000 --- a/src/main/deploy/pathplanner/paths/ProcessorToReef.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 6.356, - "y": 0.515 - }, - "prevControl": null, - "nextControl": { - "x": 5.849150786528717, - "y": 1.5004041517103026 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.12, - "y": 2.93 - }, - "prevControl": { - "x": 5.607646055479307, - "y": 1.9025260306693406 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 119.99999999999999 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef2ToAlgae.path b/src/main/deploy/pathplanner/paths/Reef2ToAlgae.path deleted file mode 100755 index b3d76b5..0000000 --- a/src/main/deploy/pathplanner/paths/Reef2ToAlgae.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.715849345439189, - "y": 5.120543278874577 - }, - "prevControl": null, - "nextControl": { - "x": 2.0918598408242657, - "y": 4.9486821557160665 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.644, - "y": 5.9 - }, - "prevControl": { - "x": 2.2585240162037024, - "y": 5.9570546875 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -59.99999999999999 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef2ToStationL.path b/src/main/deploy/pathplanner/paths/Reef2ToStationL.path deleted file mode 100755 index 3479bdf..0000000 --- a/src/main/deploy/pathplanner/paths/Reef2ToStationL.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.768307577597128, - "y": 5.135944923194679 - }, - "prevControl": null, - "nextControl": { - "x": 2.029706648549937, - "y": 6.532392564926681 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.147, - "y": 7.496 - }, - "prevControl": { - "x": 1.9934612268518512, - "y": 6.828369429976852 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 125.00000000000001 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -59.99999999999999 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef2ToStationR.path b/src/main/deploy/pathplanner/paths/Reef2ToStationR.path deleted file mode 100755 index ad883a2..0000000 --- a/src/main/deploy/pathplanner/paths/Reef2ToStationR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.911, - "y": 2.891 - }, - "prevControl": null, - "nextControl": { - "x": 2.77482718460648, - "y": 1.9847183159722217 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.117, - "y": 0.694 - }, - "prevControl": { - "x": 2.380794837083764, - "y": 1.691834342150147 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -125.00000000000001 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 59.99999999999999 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefToAlgae1.path b/src/main/deploy/pathplanner/paths/ReefToAlgae1.path deleted file mode 100755 index eae93af..0000000 --- a/src/main/deploy/pathplanner/paths/ReefToAlgae1.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.078545283564814, - "y": 5.139852792245369 - }, - "prevControl": null, - "nextControl": { - "x": 4.307950183529404, - "y": 6.522546909632054 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.71430447048611, - "y": 5.895297309027777 - }, - "prevControl": { - "x": 2.712400809898293, - "y": 6.159476974396048 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -119.99999999999999 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefToAlgae2.path b/src/main/deploy/pathplanner/paths/ReefToAlgae2.path deleted file mode 100755 index acdd023..0000000 --- a/src/main/deploy/pathplanner/paths/ReefToAlgae2.path +++ /dev/null @@ -1,70 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.141138355152027, - "y": 2.896279494826857 - }, - "prevControl": null, - "nextControl": { - "x": 4.339114076646361, - "y": 1.125970757220624 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.3034375000000002, - "y": 3.994 - }, - "prevControl": { - "x": 2.2600518989022667, - "y": 2.947312544263456 - }, - "nextControl": { - "x": 2.3137912062727337, - "y": 4.243785509520505 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.856, - "y": 3.994 - }, - "prevControl": { - "x": 2.105988540904193, - "y": 3.996393619976676 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 119.99999999999999 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefToAlgae3.path b/src/main/deploy/pathplanner/paths/ReefToAlgae3.path deleted file mode 100755 index 7aed6f2..0000000 --- a/src/main/deploy/pathplanner/paths/ReefToAlgae3.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.141833166173987, - "y": 2.9095388051625846 - }, - "prevControl": null, - "nextControl": { - "x": 4.471014402894951, - "y": 1.6216763981158409 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.644, - "y": 2.178 - }, - "prevControl": { - "x": 2.2149184445821533, - "y": 1.8831029789305933 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 119.99999999999999 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefToStationL.path b/src/main/deploy/pathplanner/paths/ReefToStationL.path deleted file mode 100755 index 706db69..0000000 --- a/src/main/deploy/pathplanner/paths/ReefToStationL.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.1206459073039685, - "y": 5.156779612660277 - }, - "prevControl": null, - "nextControl": { - "x": 4.992417100600133, - "y": 6.368297331423932 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.147, - "y": 7.495516748273755 - }, - "prevControl": { - "x": 1.578478508262126, - "y": 7.098709469342259 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 125.00000000000001 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -119.99999999999999 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefToStationR.path b/src/main/deploy/pathplanner/paths/ReefToStationR.path deleted file mode 100755 index d975b04..0000000 --- a/src/main/deploy/pathplanner/paths/ReefToStationR.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.147, - "y": 2.922755129306895 - }, - "prevControl": null, - "nextControl": { - "x": 4.811380775176799, - "y": 1.8924837239583325 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.117, - "y": 0.694 - }, - "prevControl": { - "x": 1.3408771300046387, - "y": 0.8052610923049304 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.6211014546351086, - "rotationDegrees": -150.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -125.00000000000001 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 119.99999999999999 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StationLToReef2.path b/src/main/deploy/pathplanner/paths/StationLToReef2.path deleted file mode 100755 index 5cbdc01..0000000 --- a/src/main/deploy/pathplanner/paths/StationLToReef2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.147, - "y": 7.496 - }, - "prevControl": null, - "nextControl": { - "x": 2.854713396990741, - "y": 5.397433015046296 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.744162894584037, - "y": 5.133397282780827 - }, - "prevControl": { - "x": 3.3853369397229263, - "y": 5.75694618324379 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -59.99999999999999 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 125.00000000000001 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StationLToReef3.path b/src/main/deploy/pathplanner/paths/StationLToReef3.path deleted file mode 100755 index ab31909..0000000 --- a/src/main/deploy/pathplanner/paths/StationLToReef3.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.147, - "y": 7.496 - }, - "prevControl": null, - "nextControl": { - "x": 2.976838952750378, - "y": 5.569954719308532 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.2453417968749987, - "y": 4.0849790219907405 - }, - "prevControl": { - "x": 2.5410832609953693, - "y": 4.444270833333333 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 125.00000000000001 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StationRToReef2.path b/src/main/deploy/pathplanner/paths/StationRToReef2.path deleted file mode 100755 index 01006c9..0000000 --- a/src/main/deploy/pathplanner/paths/StationRToReef2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.117, - "y": 0.6936359497185378 - }, - "prevControl": null, - "nextControl": { - "x": 2.768577690972221, - "y": 2.1302208744870583 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.878948863636363, - "y": 2.963025568181818 - }, - "prevControl": { - "x": 2.980135877728736, - "y": 2.097847956463158 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 59.99999999999999 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -125.00000000000001 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StationRToReef3.path b/src/main/deploy/pathplanner/paths/StationRToReef3.path deleted file mode 100755 index c5c23ee..0000000 --- a/src/main/deploy/pathplanner/paths/StationRToReef3.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.117, - "y": 0.694 - }, - "prevControl": null, - "nextControl": { - "x": 2.5861578652862662, - "y": 2.222558862597946 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.2806534090909087, - "y": 3.990099431818182 - }, - "prevControl": { - "x": 2.440945356669226, - "y": 2.430921813223458 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -125.00000000000001 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StraightPathForLimelight.path b/src/main/deploy/pathplanner/paths/StraightPathForLimelight.path deleted file mode 100755 index 59a3afe..0000000 --- a/src/main/deploy/pathplanner/paths/StraightPathForLimelight.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.0, - "y": 0.5 - }, - "prevControl": null, - "nextControl": { - "x": 6.2743821894581, - "y": 0.515032359595887 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.0, - "y": 0.55 - }, - "prevControl": { - "x": 5.601219316617162, - "y": 0.5416367984477829 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "TestPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TestPath.path b/src/main/deploy/pathplanner/paths/TestPath.path deleted file mode 100755 index f14bf29..0000000 --- a/src/main/deploy/pathplanner/paths/TestPath.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 10.25, - "y": 0.5 - }, - "prevControl": null, - "nextControl": { - "x": 10.75714753803454, - "y": 0.486014597039474 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 11.0, - "y": 0.5 - }, - "prevControl": { - "x": 10.493197011157182, - "y": 0.49473173557802147 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.85, - "maxAcceleration": 3.125, - "maxAngularVelocity": 720.0, - "maxAngularAcceleration": 960.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "TestPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json deleted file mode 100755 index 5fedf1f..0000000 --- a/src/main/deploy/pathplanner/settings.json +++ /dev/null @@ -1,36 +0,0 @@ -{ - "robotWidth": 0.905, - "robotLength": 0.905, - "holonomicMode": true, - "pathFolders": [ - "TestPaths" - ], - "autoFolders": [ - "TestAutos" - ], - "defaultMaxVel": 3.85, - "defaultMaxAccel": 3.125, - "defaultMaxAngVel": 720.0, - "defaultMaxAngAccel": 960.0, - "defaultNominalVoltage": 12.0, - "robotMass": 29.079, - "robotMOI": 5.7547, - "robotTrackwidth": 0.546, - "driveWheelRadius": 0.051, - "driveGearing": 5.68, - "maxDriveSpeed": 4.9, - "driveMotorType": "krakenX60", - "driveCurrentLimit": 60.0, - "wheelCOF": 1.2, - "flModuleX": 0.305, - "flModuleY": 0.305, - "frModuleX": 0.305, - "frModuleY": -0.305, - "blModuleX": -0.305, - "blModuleY": 0.305, - "brModuleX": -0.305, - "brModuleY": -0.305, - "bumperOffsetX": 0.0, - "bumperOffsetY": 0.0, - "robotFeatures": [] -} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..c68ec6f --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.LinearVelocity; +import frc.robot.generated.TunerConstants; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be declared + * globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static class Driving { + public static final LinearVelocity kMaxSpeed = TunerConstants.kSpeedAt12Volts; + public static final AngularVelocity kMaxRotationalRate = RotationsPerSecond.of(1); + public static final AngularVelocity kPIDRotationDeadband = kMaxRotationalRate.times(0.005); + } + + public static class KrakenX60 { + public static final AngularVelocity kFreeSpeed = RPM.of(6000); + } +} diff --git a/src/main/java/frc/robot/Landmarks.java b/src/main/java/frc/robot/Landmarks.java new file mode 100644 index 0000000..26831d9 --- /dev/null +++ b/src/main/java/frc/robot/Landmarks.java @@ -0,0 +1,19 @@ +package frc.robot; + +import static edu.wpi.first.units.Units.Inches; + +import java.util.Optional; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +public class Landmarks { + public static Translation2d hubPosition() { + final Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent() && alliance.get() == Alliance.Blue) { + return new Translation2d(Inches.of(182.105), Inches.of(158.845)); + } + return new Translation2d(Inches.of(469.115), Inches.of(158.845)); + } +} diff --git a/src/main/java/frc/robot/LimelightHelpers.java b/src/main/java/frc/robot/LimelightHelpers.java new file mode 100644 index 0000000..56aa00f --- /dev/null +++ b/src/main/java/frc/robot/LimelightHelpers.java @@ -0,0 +1,1698 @@ +//LimelightHelpers v1.12 (REQUIRES LLOS 2025.0 OR LATER) + +package frc.robot; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.Arrays; +import java.util.Map; +import java.util.concurrent.CompletableFuture; +import java.util.concurrent.ConcurrentHashMap; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; + +/** + * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC. + * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking. + */ +public class LimelightHelpers { + + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + /** + * Represents a Color/Retroreflective Target Result extracted from JSON Output + */ + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + /** + * Represents an AprilTag/Fiducial Target Result extracted from JSON Output + */ + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + /** + * Represents a Barcode Target Result extracted from JSON Output + */ + public static class LimelightTarget_Barcode { + + /** + * Barcode family type (e.g. "QR", "DataMatrix", etc.) + */ + @JsonProperty("fam") + public String family; + + /** + * Gets the decoded data content of the barcode + */ + @JsonProperty("data") + public String data; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("pts") + public double[][] corners; + + public LimelightTarget_Barcode() { + } + + public String getFamily() { + return family; + } + } + + /** + * Represents a Neural Classifier Pipeline Result extracted from JSON Output + */ + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + /** + * Represents a Neural Detector Pipeline Result extracted from JSON Output + */ + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + public LimelightTarget_Detector() { + } + } + + /** + * Limelight Results object, parsed from a Limelight's JSON results output. + */ + public static class LimelightResults { + + public String error; + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public LimelightResults() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + + } + + + } + + /** + * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. + */ + public static class RawFiducial { + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null || getClass() != obj.getClass()) return false; + RawFiducial other = (RawFiducial) obj; + return id == other.id && + Double.compare(txnc, other.txnc) == 0 && + Double.compare(tync, other.tync) == 0 && + Double.compare(ta, other.ta) == 0 && + Double.compare(distToCamera, other.distToCamera) == 0 && + Double.compare(distToRobot, other.distToRobot) == 0 && + Double.compare(ambiguity, other.ambiguity) == 0; + } + + } + + /** + * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. + */ + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + + public RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y ) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + + /** + * Represents a 3D Pose Estimate. + */ + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + + public RawFiducial[] rawFiducials; + public boolean isMegaTag2; + + /** + * Instantiates a PoseEstimate object with default values + */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[]{}; + this.isMegaTag2 = false; + } + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + this.isMegaTag2 = isMegaTag2; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null || getClass() != obj.getClass()) return false; + PoseEstimate that = (PoseEstimate) obj; + // We don't compare the timestampSeconds as it isn't relevant for equality and makes + // unit testing harder + return Double.compare(that.latency, latency) == 0 + && tagCount == that.tagCount + && Double.compare(that.tagSpan, tagSpan) == 0 + && Double.compare(that.avgTagDist, avgTagDist) == 0 + && Double.compare(that.avgTagArea, avgTagArea) == 0 + && pose.equals(that.pose) + && Arrays.equals(rawFiducials, that.rawFiducials); + } + + } + + /** + * Encapsulates the state of an internal Limelight IMU. + */ + public static class IMUData { + public double robotYaw = 0.0; + public double Roll = 0.0; + public double Pitch = 0.0; + public double Yaw = 0.0; + public double gyroX = 0.0; + public double gyroY = 0.0; + public double gyroZ = 0.0; + public double accelX = 0.0; + public double accelY = 0.0; + public double accelZ = 0.0; + + public IMUData() {} + + public IMUData(double[] imuData) { + if (imuData != null && imuData.length >= 10) { + this.robotYaw = imuData[0]; + this.Roll = imuData[1]; + this.Pitch = imuData[2]; + this.Yaw = imuData[3]; + this.gyroX = imuData[4]; + this.gyroY = imuData[5]; + this.gyroZ = imuData[6]; + this.accelX = imuData[7]; + this.accelY = imuData[8]; + this.accelZ = imuData[9]; + } + } + } + + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if ("".equals(name) || name == null) { + return "limelight"; + } + return name; + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose3d object. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose3d object representing the pose, or empty Pose3d if invalid data + */ + public static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose2d object. + * Uses only x, y, and yaw components, ignoring z, roll, and pitch. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose2d object representing the pose, or empty Pose2d if invalid data + */ + public static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + /** + * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * + * @param pose The Pose3d object to convert + * @return A 6-element array containing [x, y, z, roll, pitch, yaw] + */ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw. + * + * @param pose The Pose2d object to convert + * @return A 6-element array containing [x, y, 0, 0, 0, yaw] + */ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) { + DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate + } + + var pose = toPose2D(poseArray); + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int)extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2); + } + + /** + * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawFiducial objects containing detection details + */ + public static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + /** + * Gets the latest raw neural detector results from NetworkTables + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawDetection objects containing detection details + */ + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 12; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + /** + * Prints detailed information about a PoseEstimate to standard output. + * Includes timestamp, latency, tag count, tag span, average tag distance, + * average tag area, and detailed information about each detected fiducial. + * + * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." + */ + public static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static Boolean validPoseEstimate(PoseEstimate pose) { + return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent(key, k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + /** + * Does the Limelight have a valid target? + * @param limelightName Name of the Limelight camera ("" for default) + * @return True if a valid target is present, false otherwise + */ + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + /** + * Gets the horizontal offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + /** + * Gets the vertical offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + /** + * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTXNC(String limelightName) { + return getLimelightNTDouble(limelightName, "txnc"); + } + + /** + * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTYNC(String limelightName) { + return getLimelightNTDouble(limelightName, "tync"); + } + + /** + * Gets the target area as a percentage of the image (0-100%). + * @param limelightName Name of the Limelight camera ("" for default) + * @return Target area percentage (0-100) + */ + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + /** + * T2D is an array that contains several targeting metrcis + * @param limelightName Name of the Limelight camera + * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, + * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees] + */ + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + /** + * Gets the number of targets currently detected. + * @param limelightName Name of the Limelight camera + * @return Number of detected targets + */ + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[1]; + } + return 0; + } + + /** + * Gets the classifier class index from the currently running neural classifier pipeline + * @param limelightName Name of the Limelight camera + * @return Class index from classifier pipeline + */ + public static int getClassifierClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[10]; + } + return 0; + } + + /** + * Gets the detector class index from the primary result of the currently running neural detector pipeline. + * @param limelightName Name of the Limelight camera + * @return Class index from detector pipeline + */ + public static int getDetectorClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[11]; + } + return 0; + } + + /** + * Gets the current neural classifier result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from classifier pipeline + */ + public static String getClassifierClass (String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + + /** + * Gets the primary neural detector result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from detector pipeline + */ + public static String getDetectorClass (String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + /** + * Gets the pipeline's processing latency contribution. + * @param limelightName Name of the Limelight camera + * @return Pipeline latency in milliseconds + */ + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + /** + * Gets the capture latency. + * @param limelightName Name of the Limelight camera + * @return Capture latency in milliseconds + */ + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + /** + * Gets the active pipeline index. + * @param limelightName Name of the Limelight camera + * @return Current pipeline index (0-9) + */ + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + /** + * Gets the current pipeline type. + * @param limelightName Name of the Limelight camera + * @return Pipeline type string (e.g. "retro", "apriltag", etc) + */ + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + /** + * Gets the full JSON results dump. + * @param limelightName Name of the Limelight camera + * @return JSON string containing all current results + */ + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + /** + * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Red Alliance field space + */ + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + /** + * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space + */ + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + /** + * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation relative to the target + */ + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the target + */ + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the camera's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the camera + */ + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the robot + */ + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the robot + */ + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); + } + + /** + * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * Make sure you are calling setRobotOrientation() before calling this method. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired", false); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + /** + * Gets the current IMU data from NetworkTables. + * IMU data is formatted as [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. + * Returns all zeros if data is invalid or unavailable. + * + * @param limelightName Name/identifier of the Limelight + * @return IMUData object containing all current IMU data + */ + public static IMUData getIMUData(String limelightName) { + double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); + if (imuData == null || imuData.length < 10) { + return new IMUData(); // Returns object with all zeros + } + return new IMUData(imuData); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * Sets LED mode to be controlled by the current pipeline. + * @param limelightName Name of the Limelight camera + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + /** + * Enables standard side-by-side stream mode. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + /** + * Enables Picture-in-Picture mode with secondary stream in the corner. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + /** + * Enables Picture-in-Picture mode with primary stream in the corner. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + + /** + * Sets the crop window for the camera. The crop window in the UI must be completely open. + * @param limelightName Name of the Limelight camera + * @param cropXMin Minimum X value (-1 to 1) + * @param cropXMax Maximum X value (-1 to 1) + * @param cropYMin Minimum Y value (-1 to 1) + * @param cropYMax Maximum Y value (-1 to 1) + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + /** + * Sets 3D offset point for easy 3D targeting. + */ + public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Sets robot orientation values used by MegaTag2 localization algorithm. + * + * @param limelightName Name/identifier of the Limelight + * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC + * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second + * @param pitch (Unnecessary) Robot pitch in degrees + * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second + * @param roll (Unnecessary) Robot roll in degrees + * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second + */ + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate, boolean flush) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if(flush) + { + Flush(); + } + } + + /** + * Configures the IMU mode for MegaTag2 Localization + * + * @param limelightName Name/identifier of the Limelight + * @param mode IMU mode. + */ + public static void SetIMUMode(String limelightName, int mode) { + setLimelightNTDouble(limelightName, "imumode_set", mode); + } + + /** + * Configures the complementary filter alpha value for IMU Assist Modes (Modes 3 and 4) + * + * @param limelightName Name/identifier of the Limelight + * @param alpha Defaults to .001. Higher values will cause the internal IMU to converge onto the assist source more rapidly. + */ + public static void SetIMUAssistAlpha(String limelightName, double alpha) { + setLimelightNTDouble(limelightName, "imuassistalpha_set", alpha); + } + + + /** + * Configures the throttle value. Set to 100-200 while disabled to reduce thermal output/temperature. + * + * @param limelightName Name/identifier of the Limelight + * @param throttle Defaults to 0. Your Limelgiht will process one frame after skipping frames. + */ + public static void SetThrottle(String limelightName, int throttle) { + setLimelightNTDouble(limelightName, "throttle_set", throttle); + } + + /** + * Sets the 3D point-of-interest offset for the current fiducial pipeline. + * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking + * + * @param limelightName Name/identifier of the Limelight + * @param x X offset in meters + * @param y Y offset in meters + * @param z Z offset in meters + */ + public static void SetFidcuial3DOffset(String limelightName, double x, double y, + double z) { + + double[] entries = new double[3]; + entries[0] = x; + entries[1] = y; + entries[2] = z; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Overrides the valid AprilTag IDs that will be used for localization. + * Tags not in this list will be ignored for robot pose estimation. + * + * @param limelightName Name/identifier of the Limelight + * @param validIDs Array of valid AprilTag IDs to track + */ + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + /** + * Sets the downscaling factor for AprilTag detection. + * Increasing downscale can improve performance at the cost of potentially reduced detection range. + * + * @param limelightName Name/identifier of the Limelight + * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control. + */ + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) + { + int d = 0; // pipeline + if (downscale == 1.0) + { + d = 1; + } + if (downscale == 1.5) + { + d = 2; + } + if (downscale == 2) + { + d = 3; + } + if (downscale == 3) + { + d = 4; + } + if (downscale == 4) + { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + + /** + * Sets the camera pose relative to the robot. + * @param limelightName Name of the Limelight camera + * @param forward Forward offset in meters + * @param side Side offset in meters + * @param up Up offset in meters + * @param roll Roll angle in degrees + * @param pitch Pitch angle in degrees + * @param yaw Yaw angle in degrees + */ + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && !"".equals(snapshotName)) { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Gets the latest JSON results output and returns a LimelightResults object. + * @param limelightName Name of the Limelight camera + * @return LimelightResults object containing all current target data + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} \ 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 8227ced..8776e5d 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -6,10 +6,20 @@ import edu.wpi.first.wpilibj.RobotBase; +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ public final class 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); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/NetworkTables.java b/src/main/java/frc/robot/NetworkTables.java deleted file mode 100644 index da35adf..0000000 --- a/src/main/java/frc/robot/NetworkTables.java +++ /dev/null @@ -1,581 +0,0 @@ -package frc.robot; - -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.units.AngularAccelerationUnit; -import edu.wpi.first.units.AngularVelocityUnit; -import edu.wpi.first.units.CurrentUnit; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.TimeUnit; -import edu.wpi.first.units.measure.LinearAcceleration; -import edu.wpi.first.units.measure.LinearVelocity; -import frc.robot.generated.TunerConstants; - -import static edu.wpi.first.units.Units.*; - -import java.util.ArrayList; -import java.util.List; - -public class NetworkTables { - public enum ConstantType { - Double, - Int, - Boolean, - String, - Velocity, - AngularRate, - Acceleration, - AngularAcceleration, - Time, - Current - } - - public enum ConstantId { - MaxSpeed, - MaxAngularRate, - ControllerVelocityCurveExponent, - ControllerRotationCurveExponent, - ControllerDeadbandPercentage, - SlewTranslateLimit, - SlewRotateLimit, - RollerMovementHoldVelocity, - RollerMovementForwardVelocity, - RollerMovementBackwardVelocity, - RollerMovementCoralEjectVelocity, - RollerMovementAlgaeIntakeVelocity, - RollerMovementAlgaeEjectVelocity, - ArmUpVelocity, - ArmDownVelocity, - ArmIntakePosition, - ArmHoldPosition, - ArmCoralEjectPosition, - ArmDefaultPosition, - ArmMotorForwardNominalPercentOutput, - ArmMotorReverseNominalPercentOutput, - ArmMotorForwardPeakPercentOutput, - ArmMotorReversePeakPercentOutput, - ArmMotorMagicMotionCruiseVelocity, - ArmMotorMagicMotionAccelerationVelocity, - ArmMotorProportionalGainValue, - ArmMotorIntegralGainValue, - ArmMotorDerivativeGainValue, - ArmMotorFeedForwardGainValue, - ArmSelectedSensorPosition, - ArmMotorAllowableCloseLoopError, - ClimbVelocity, - UnclimbVelocity, - ClimberTorqueCurrentLimit, - AutoIntakeAlgaeWait, - AutoEjectAlgaeWait, - AutoEjectCoralWait, - AlgaeIntakeSequenceWait, - ArmCoralEjectSequenceWait, - ClimbButton, - UnclimbButton, - RollerForwardButton, - RollerBackwardButton, - ArmUpButton, - ArmDownButton, - ResetEncoderButton, - AlgaeIntakeButtonAxis, - AlgaeEjectButtonAxis, - kNumConstants - } - - public static class ConstantEntry { - public String networkTableKey; - public ConstantType type; - - public static class Value { - public double doubleValue; - public int intValue; - public boolean boolValue; - public String stringValue; - } - - public Value defaultValue = new Value(); - } - - private final NetworkTable table; - private static final String kTableName = "Tuning Constants"; - private static final double kMaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - private static final double kMaxAngularRate = 1.2; - - private static final List constantEntries = new ArrayList<>() { - { - add(new ConstantEntry() { - { - networkTableKey = "MaxSpeed"; - type = ConstantType.Velocity; - defaultValue.doubleValue = kMaxSpeed; // From C++ - } - }); - add(new ConstantEntry() { - { - networkTableKey = "MaxAngularRate"; - type = ConstantType.AngularRate; - defaultValue.doubleValue = kMaxAngularRate * 2 * Math.PI; // From C++ - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ControllerVelocityCurveExponent"; - type = ConstantType.Double; - defaultValue.doubleValue = 2.0; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ControllerRotationCurveExponent"; - type = ConstantType.Double; - defaultValue.doubleValue = 2.0; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ControllerDeadbandPercentage"; - type = ConstantType.Double; - defaultValue.doubleValue = 0.02; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "SlewTranslateLimit"; - type = ConstantType.Acceleration; - defaultValue.doubleValue = 10.0 * kMaxSpeed; // From C++ - } - }); - add(new ConstantEntry() { - { - networkTableKey = "SlewRotateLimit"; - type = ConstantType.AngularAcceleration; - defaultValue.doubleValue = 30.0 * kMaxAngularRate; // From C++ - } - }); - add(new ConstantEntry() { - { - networkTableKey = "RollerMovementHoldVelocity"; - type = ConstantType.Double; - defaultValue.doubleValue = 0.05; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "RollerMovementForwardVelocity"; - type = ConstantType.Double; - defaultValue.doubleValue = 0.6; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "RollerMovementBackwardVelocity"; - type = ConstantType.Double; - defaultValue.doubleValue = -0.6; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "RollerMovementCoralEjectVelocity"; - type = ConstantType.Double; - defaultValue.doubleValue = 1.0; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "RollerMovementAlgaeIntakeVelocity"; - type = ConstantType.Double; - defaultValue.doubleValue = 1.0; // Fixed: was 0.5 → now 1.0 (matches C++) - } - }); - add(new ConstantEntry() { - { - networkTableKey = "RollerMovementAlgaeEjectVelocity"; - type = ConstantType.Double; - defaultValue.doubleValue = -0.6; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmUpVelocity"; - type = ConstantType.Double; - defaultValue.doubleValue = 0.6; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmDownVelocity"; - type = ConstantType.Double; - defaultValue.doubleValue = -0.2; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmIntakePosition"; - type = ConstantType.Double; - defaultValue.doubleValue = -1600; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmHoldPosition"; - type = ConstantType.Double; - defaultValue.doubleValue = -500; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmCoralEjectPosition"; - type = ConstantType.Double; - defaultValue.doubleValue = -700; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmDefaultPosition"; - type = ConstantType.Double; - defaultValue.doubleValue = 700; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmMotorForwardNominalPercentOutput"; - type = ConstantType.Double; - defaultValue.doubleValue = 0.0; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmMotorReverseNominalPercentOutput"; - type = ConstantType.Double; - defaultValue.doubleValue = 0.0; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmMotorForwardPeakPercentOutput"; - type = ConstantType.Double; - defaultValue.doubleValue = 0.4; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmMotorReversePeakPercentOutput"; - type = ConstantType.Double; - defaultValue.doubleValue = -0.4; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmMotorMagicMotionCruiseVelocity"; - type = ConstantType.Double; - defaultValue.doubleValue = 50.0; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmMotorMagicMotionAccelerationVelocity"; - type = ConstantType.Double; - defaultValue.doubleValue = 50.0; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmMotorProportionalGainValue"; - type = ConstantType.Double; - defaultValue.doubleValue = 5.0; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmMotorIntegralGainValue"; - type = ConstantType.Double; - defaultValue.doubleValue = 0.0; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmMotorDerivativeGainValue"; - type = ConstantType.Double; - defaultValue.doubleValue = 5.0; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmMotorFeedForwardGainValue"; - type = ConstantType.Double; - defaultValue.doubleValue = 0.1; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmSelectedSensorPosition"; - type = ConstantType.Double; - defaultValue.doubleValue = 0.0; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmMotorAllowableCloseLoopError"; - type = ConstantType.Double; - defaultValue.doubleValue = 5.0; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ClimbVelocity"; - type = ConstantType.Double; - defaultValue.doubleValue = 1.0; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "UnclimbVelocity"; - type = ConstantType.Double; - defaultValue.doubleValue = -1.0; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ClimberTorqueCurrentLimit"; - type = ConstantType.Current; - defaultValue.doubleValue = 22.0; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "AutoIntakeAlgaeWait"; - type = ConstantType.Time; - defaultValue.doubleValue = 1.0; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "AutoEjectAlgaeWait"; - type = ConstantType.Time; - defaultValue.doubleValue = 1.0; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "AutoEjectCoralWait"; - type = ConstantType.Time; - defaultValue.doubleValue = 0.5; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "AlgaeIntakeSequenceWait"; - type = ConstantType.Time; - defaultValue.doubleValue = 0.01; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmCoralEjectSequenceWait"; - type = ConstantType.Time; - defaultValue.doubleValue = 0.1; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ClimbButton"; - type = ConstantType.Int; - defaultValue.intValue = 2; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "UnclimbButton"; - type = ConstantType.Int; - defaultValue.intValue = 1; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "RollerForwardButton"; - type = ConstantType.Int; - defaultValue.intValue = 4; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "RollerBackwardButton"; - type = ConstantType.Int; - defaultValue.intValue = 3; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmUpButton"; - type = ConstantType.Int; - defaultValue.intValue = 6; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ArmDownButton"; - type = ConstantType.Int; - defaultValue.intValue = 5; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "ResetEncoderButton"; - type = ConstantType.Int; - defaultValue.intValue = 8; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "AlgaeIntakeButtonAxis"; - type = ConstantType.Int; - defaultValue.intValue = 2; - } - }); - add(new ConstantEntry() { - { - networkTableKey = "AlgaeEjectButtonAxis"; - type = ConstantType.Int; - defaultValue.intValue = 3; - } - }); - } - }; - - public NetworkTables() { - table = NetworkTableInstance.getDefault().getTable(kTableName); - SetDefaults(); - } - - private void SetDefaults() { - for (ConstantEntry entry : constantEntries) { - switch (entry.type) { - case Double: - case Velocity: - case AngularRate: - case Acceleration: - case AngularAcceleration: - case Time: - case Current: - table.getEntry(entry.networkTableKey).setDefaultDouble(entry.defaultValue.doubleValue); - break; - case Int: - table.getEntry(entry.networkTableKey).setDefaultInteger(entry.defaultValue.intValue); - break; - case Boolean: - table.getEntry(entry.networkTableKey).setDefaultBoolean(entry.defaultValue.boolValue); - break; - case String: - table.getEntry(entry.networkTableKey).setDefaultString(entry.defaultValue.stringValue); - break; - } - } - } - - public void RestoreDefaults() { - for (ConstantEntry entry : constantEntries) { - switch (entry.type) { - case Double: - case Velocity: - case AngularRate: - case Acceleration: - case AngularAcceleration: - case Time: - case Current: - table.getEntry(entry.networkTableKey).setDouble(entry.defaultValue.doubleValue); - break; - case Int: - table.getEntry(entry.networkTableKey).setInteger(entry.defaultValue.intValue); - break; - case Boolean: - table.getEntry(entry.networkTableKey).setBoolean(entry.defaultValue.boolValue); - break; - case String: - table.getEntry(entry.networkTableKey).setString(entry.defaultValue.stringValue); - break; - } - } - } - - public double getDoubleValue(ConstantId id) { - ConstantEntry entry = constantEntries.get(id.ordinal()); - if (entry.type != ConstantType.Double) { - throw new RuntimeException("Constant type mismatch for " + entry.networkTableKey); - } - return table.getEntry(entry.networkTableKey).getDouble(entry.defaultValue.doubleValue); - } - - public int getIntValue(ConstantId id) { - ConstantEntry entry = constantEntries.get(id.ordinal()); - if (entry.type != ConstantType.Int) { - throw new RuntimeException("Constant type mismatch for " + entry.networkTableKey); - } - return (int) table.getEntry(entry.networkTableKey).getInteger(entry.defaultValue.intValue); - } - - public boolean getBooleanValue(ConstantId id) { - ConstantEntry entry = constantEntries.get(id.ordinal()); - if (entry.type != ConstantType.Boolean) { - throw new RuntimeException("Constant type mismatch for " + entry.networkTableKey); - } - return table.getEntry(entry.networkTableKey).getBoolean(entry.defaultValue.boolValue); - } - - public String getStringValue(ConstantId id) { - ConstantEntry entry = constantEntries.get(id.ordinal()); - if (entry.type != ConstantType.String) { - throw new RuntimeException("Constant type mismatch for " + entry.networkTableKey); - } - return table.getEntry(entry.networkTableKey).getString(entry.defaultValue.stringValue); - } - - public Measure getAngularRateValue(ConstantId id) { - ConstantEntry entry = constantEntries.get(id.ordinal()); - if (entry.type != ConstantType.AngularRate) { - throw new RuntimeException("Constant type mismatch for " + entry.networkTableKey); - } - return RadiansPerSecond.of(table.getEntry(entry.networkTableKey).getDouble(entry.defaultValue.doubleValue)); - } - - public LinearVelocity getVelocityValue(ConstantId id) { - ConstantEntry entry = constantEntries.get(id.ordinal()); - if (entry.type != ConstantType.Velocity) { - throw new RuntimeException("Constant type mismatch for " + entry.networkTableKey); - } - return MetersPerSecond.of(table.getEntry(entry.networkTableKey).getDouble(entry.defaultValue.doubleValue)); - } - - public LinearAcceleration getAccelerationValue(ConstantId id) { - ConstantEntry entry = constantEntries.get(id.ordinal()); - if (entry.type != ConstantType.Acceleration) { - throw new RuntimeException("Constant type mismatch for " + entry.networkTableKey); - } - return MetersPerSecondPerSecond - .of(table.getEntry(entry.networkTableKey).getDouble(entry.defaultValue.doubleValue)); - } - - public Measure getAngularAccelerationValue(ConstantId id) { - ConstantEntry entry = constantEntries.get(id.ordinal()); - if (entry.type != ConstantType.AngularAcceleration) { - throw new RuntimeException("Constant type mismatch for " + entry.networkTableKey); - } - return RadiansPerSecondPerSecond - .of(table.getEntry(entry.networkTableKey).getDouble(entry.defaultValue.doubleValue)); - } - - public Measure getTimeValue(ConstantId id) { - ConstantEntry entry = constantEntries.get(id.ordinal()); - if (entry.type != ConstantType.Time) { - throw new RuntimeException("Constant type mismatch for " + entry.networkTableKey); - } - return Seconds.of(table.getEntry(entry.networkTableKey).getDouble(entry.defaultValue.doubleValue)); - } - - public Measure getCurrentValue(ConstantId id) { - ConstantEntry entry = constantEntries.get(id.ordinal()); - if (entry.type != ConstantType.Current) { - throw new RuntimeException("Constant type mismatch for " + entry.networkTableKey); - } - return Amps.of(table.getEntry(entry.networkTableKey).getDouble(entry.defaultValue.doubleValue)); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/Ports.java b/src/main/java/frc/robot/Ports.java new file mode 100644 index 0000000..e02c0f6 --- /dev/null +++ b/src/main/java/frc/robot/Ports.java @@ -0,0 +1,23 @@ +package frc.robot; + +import com.ctre.phoenix6.CANBus; + +public final class Ports { + // CAN Buses + public static final CANBus kRoboRioCANBus = new CANBus("rio"); + public static final CANBus kCANivoreCANBus = new CANBus("main"); + + // Talon FX IDs + public static final int kIntakePivot = 10; + public static final int kIntakeRollers = 11; + public static final int kFloor = 12; + public static final int kFeeder = 13; + public static final int kShooterLeft = 14; + public static final int kShooterMiddle = 15; + public static final int kShooterRight = 16; + public static final int kHanger = 18; + + // PWM Ports + public static final int kHoodLeftServo = 3; + public static final int kHoodRightServo = 4; +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d303fb5..b0efc03 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,254 +1,49 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + package frc.robot; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotBase; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.subsystems.Limelights.*; -import frc.robot.subsystems.Limelights.LimelightHelpers.PoseEstimate; -import frc.robot.subsystems.Led.LEDSubsystem; +/** + * The methods in this class are called automatically corresponding to each mode, as described in + * the TimedRobot documentation. If you change the name of this class or the package after creating + * this project, you must also update the Main.java file in the project. + */ public class Robot extends TimedRobot { - - private static final double kLowBatteryVoltage = 11.8; - private static final double kLowBatteryDisabledTime = 1.5; //seconds - - private final Timer batteryTimer = new Timer(); - private boolean lowBatteryAlert = false; - - - private Command m_autonomousCommand; - - private final RobotContainer m_container = new RobotContainer(); - private final LEDSubsystem m_leds = new LEDSubsystem(); - - private static final boolean kUseLimelight = true; // Set to true to enable limelight for now - - private double m_autonomousStartTime = -1.0; - - private void configureLimelights() { - for (String limelightName : Constants.LimelightConstants.limelightNames) { - LimelightHelpers.setLEDMode_PipelineControl(limelightName); - LimelightHelpers.setPipelineIndex(limelightName, 0); - System.out.println("Configured Limelight: " + limelightName); - } - } + private final RobotContainer m_robotContainer; - private void updateLimelightTelemetry() { - for (String limelightName : Constants.LimelightConstants.limelightNames) { - boolean hasTarget = LimelightHelpers.hasTarget(limelightName); - SmartDashboard.putBoolean(limelightName + "/HasTarget", hasTarget); - - if (hasTarget) { - SmartDashboard.putNumber(limelightName + "/TX", - LimelightHelpers.getTX(limelightName)); - SmartDashboard.putNumber(limelightName + "/TY", - LimelightHelpers.getTY(limelightName)); - SmartDashboard.putNumber(limelightName + "/TA", - LimelightHelpers.getTA(limelightName)); - SmartDashboard.putNumber(limelightName + "/FiducialID", - LimelightHelpers.getFiducialID(limelightName)); - } - - SmartDashboard.putNumber(limelightName + "/Pipeline", - LimelightHelpers.getCurrentPipelineIndex(limelightName)); - SmartDashboard.putNumber(limelightName + "/Latency", - LimelightHelpers.getLatency_Pipeline(limelightName) + - LimelightHelpers.getLatency_Capture(limelightName)); - } + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + public Robot() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + SmartDashboard.putData(CommandScheduler.getInstance()); + RobotController.setBrownoutVoltage(Volts.of(6.1)); } - private void updateVisionMeasurement() { - var driveState = m_container.drivetrain.getState(); - var heading = driveState.Pose.getRotation().getDegrees(); - var omega = driveState.Speeds.omegaRadiansPerSecond; - - for (String limelightName : Constants.LimelightConstants.limelightNames) { - LimelightHelpers.setRobotOrientation( - limelightName, - heading, - omega * 180.0 / Math.PI, - 0, 0, 0, 0 - ); - - PoseEstimate llMeasurement = null; - var alliance = DriverStation.getAlliance(); - - if (alliance.isPresent()) { - if (alliance.get() == DriverStation.Alliance.Blue) { - llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName); - } else { - llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(limelightName); - } - } else { - llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName); - } - - double currentTime = Timer.getFPGATimestamp() * 1000.0; - - if (m_autonomousStartTime < 0 || currentTime - m_autonomousStartTime < 200.0) { - continue; - } - - if (llMeasurement != null && - llMeasurement.tagCount > 0 && - Math.abs(omega) < 2.0) { - - double xyStdDev = llMeasurement.avgTagDist * 0.5; - double rotStdDev = llMeasurement.avgTagDist * 0.5; - - if (llMeasurement.tagCount >= 2) { - xyStdDev *= 0.5; - rotStdDev *= 0.5; - } - - SmartDashboard.putNumber(limelightName + "/VisionTagCount", llMeasurement.tagCount); - SmartDashboard.putNumber(limelightName + "/VisionAvgDist", llMeasurement.avgTagDist); - - m_container.drivetrain.addVisionMeasurement( - llMeasurement.pose, - llMeasurement.timestampSeconds, - VecBuilder.fill(xyStdDev, xyStdDev, rotStdDev) - ); - //TODO: Not sure if this is working or no - } - } - } - - @Override - public void testPeriodic() {} - - public void robotInit() { - m_container.robotInit(); - - if (kUseLimelight) { - configureLimelights(); - } - - //simulation joystick warning suppression - if (RobotBase.isSimulation()) { - DriverStation.silenceJoystickConnectionWarning(true); - } - - batteryTimer.start(); - - } - + /** + * This function is called every 20 ms, 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(); - - // Removed getCANUsagePercent() as it doesn't exist in newer WPILib - SmartDashboard.putNumber("Voltage", RobotController.getBatteryVoltage()); - SmartDashboard.putNumber("CPU Temperature", RobotController.getCPUTemp()); - SmartDashboard.putBoolean("RSL", RobotController.getRSLState()); - SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime()); - - SmartDashboard.putNumber("Code Runtime (ms)", Timer.getFPGATimestamp() * 1000.0); - - //TODO: Vision updates - if (kUseLimelight) { - updateLimelightTelemetry(); - updateVisionMeasurement(); - - // NOTE: You need to add LimelightHelpers.java to your project - // Download from: https://github.com/LimelightVision/limelightlib-wpijava - /* - for (String limelightName : Constants.LimelightConstants.limelightNames) { - LimelightHelpers.setRobotOrientation(limelightName, heading, 0, 0, 0, 0, 0); - var llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); - double currentTime = Timer.getFPGATimestamp() * 1000.0; - if (m_autonomousStartTime < 0 || currentTime - m_autonomousStartTime < 200.0) { - // Skip - } else { - if (llMeasurement != null && llMeasurement.tagCount > 0 && Math.abs(omega) < 2) { - m_container.drivetrain.addVisionMeasurement( - llMeasurement.pose, - llMeasurement.timestampSeconds, - new double[]{llMeasurement.avgTagDist * 5, llMeasurement.avgTagDist * 5, llMeasurement.avgTagDist * 5} - ); - } - } - } - */ - } - - if(DriverStation.isEnabled()){ - batteryTimer.reset(); - } - double batteryVoltage = RobotController.getBatteryVoltage(); - - if(batteryVoltage<= kLowBatteryVoltage && batteryTimer.hasElapsed(kLowBatteryDisabledTime)){ - lowBatteryAlert = true; - } else { - lowBatteryAlert = false; - } - - SmartDashboard.putBoolean("LowBattery", lowBatteryAlert); - } - - @Override - public void disabledInit() { - m_leds.setDisabledMode(); - } - - @Override - public void disabledPeriodic() {} - - @Override - public void autonomousInit() { - m_autonomousStartTime = Timer.getFPGATimestamp() * 1000.0; - m_autonomousCommand = m_container.getAutonomousCommand(); - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } - m_container.autonomousInit(); - m_leds.setAutonomousMode(); - - if (kUseLimelight) { - for (String limelightName : Constants.LimelightConstants.limelightNames) { - LimelightHelpers.setPipelineIndex(limelightName, 0); - LimelightHelpers.setLEDMode_PipelineControl(limelightName); - } - } - } - - @Override - public void autonomousPeriodic() {} - - @Override - public void teleopInit() { - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } - m_container.teleopInit(); - m_leds.setTeleopMode(); - - if (kUseLimelight) { - for (String limelightName : Constants.LimelightConstants.limelightNames) { - LimelightHelpers.setPipelineIndex(limelightName, 0); - LimelightHelpers.setLEDMode_PipelineControl(limelightName); - } - } - } - - @Override - public void teleopPeriodic() {} - - @Override - public void testInit() { - CommandScheduler.getInstance().cancelAll(); - } - - @Override - public void simulationPeriodic() { - // This prevents the default message and gives you control - // If you have physics simulation, put it here - // For now: do nothing fast } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dadb8e0..a73e369 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,252 +1,136 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + package frc.robot; import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; -import com.ctre.phoenix6.swerve.SwerveRequest; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.commands.FollowPathCommand; +import java.util.Optional; -import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandJoystick; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.AutoCommands.AutoCommands; -import frc.robot.subsystems.Climber.Climber; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.Superstructure; -import frc.robot.NetworkTables; - - -public class RobotContainer extends TimedRobot { - private final NetworkTables networkTables = new NetworkTables(); - - private final SwerveRequest.FieldCentric fieldCentricDrive = new SwerveRequest.FieldCentric() - .withDeadband(networkTables.getVelocityValue(NetworkTables.ConstantId.MaxSpeed).in(MetersPerSecond) * networkTables.getDoubleValue(NetworkTables.ConstantId.ControllerDeadbandPercentage)) - .withRotationalDeadband(networkTables.getAngularRateValue(NetworkTables.ConstantId.MaxAngularRate).in(RadiansPerSecond) * networkTables.getDoubleValue(NetworkTables.ConstantId.ControllerDeadbandPercentage)); - - private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); - private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - private final SwerveRequest.RobotCentric robotCentricDrive = new SwerveRequest.RobotCentric(); - - private double MAX_SPEED = - TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - - private final Telemetry logger = new Telemetry(MAX_SPEED); +import frc.robot.Constants.Driving; +import frc.robot.commands.AutoRoutines; +import frc.robot.commands.ManualDriveCommand; +import frc.robot.commands.SubsystemCommands; +import frc.robot.subsystems.Feeder; +import frc.robot.subsystems.Floor; +import frc.robot.subsystems.Hanger; +import frc.robot.subsystems.Hood; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Limelight; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.Swerve; +import frc.util.SwerveTelemetry; + +/** + * 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 { + private final Swerve swerve = new Swerve(); + private final Intake intake = new Intake(); + private final Floor floor = new Floor(); + private final Feeder feeder = new Feeder(); + private final Shooter shooter = new Shooter(); + private final Hood hood = new Hood(); + private final Hanger hanger = new Hanger(); + private final Limelight limelight = new Limelight("limelight"); + + private final SwerveTelemetry swerveTelemetry = new SwerveTelemetry(Driving.kMaxSpeed.in(MetersPerSecond)); - private final CommandXboxController controller = new CommandXboxController(0); - private final Joystick simController = new Joystick(2); - private final CommandJoystick buttonBoard = new CommandJoystick(1); - - public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); - - public final Climber climber = new Climber(networkTables); - private final Superstructure superstructure = new Superstructure(networkTables, drivetrain); - private final AutoCommands autoCommands = new AutoCommands(superstructure, networkTables); - - private final SendableChooser autoChooser; - - private final SlewRateLimiter fieldXSlewFilter = new SlewRateLimiter(networkTables.getAccelerationValue(NetworkTables.ConstantId.SlewTranslateLimit).in(MetersPerSecondPerSecond)); - private final SlewRateLimiter fieldYSlewFilter = new SlewRateLimiter(networkTables.getAccelerationValue(NetworkTables.ConstantId.SlewTranslateLimit).in(MetersPerSecondPerSecond)); - private final SlewRateLimiter fieldRotateSlewFilter = new SlewRateLimiter(networkTables.getAngularAccelerationValue(NetworkTables.ConstantId.SlewRotateLimit).in(RadiansPerSecondPerSecond)); - private final SlewRateLimiter robotXSlewFilter = new SlewRateLimiter(networkTables.getAccelerationValue(NetworkTables.ConstantId.SlewTranslateLimit).in(MetersPerSecondPerSecond)); - private final SlewRateLimiter robotYSlewFilter = new SlewRateLimiter(networkTables.getAccelerationValue(NetworkTables.ConstantId.SlewTranslateLimit).in(MetersPerSecondPerSecond)); - private final SlewRateLimiter robotRotateSlewFilter = new SlewRateLimiter(networkTables.getAngularAccelerationValue(NetworkTables.ConstantId.SlewRotateLimit).in(RadiansPerSecondPerSecond)); - - // Register autonomous commands for PathPlanner - + private final CommandXboxController driver = new CommandXboxController(0); + + private final AutoRoutines autoRoutines = new AutoRoutines( + swerve, + intake, + floor, + feeder, + shooter, + hood, + hanger, + limelight + ); + private final SubsystemCommands subsystemCommands = new SubsystemCommands( + swerve, + intake, + floor, + feeder, + shooter, + hood, + hanger, + () -> -driver.getLeftY(), + () -> -driver.getLeftX() + ); + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - NamedCommands.registerCommand("Eject Coral", superstructure.AutoCoral()); - NamedCommands.registerCommand("Intake Algae", autoCommands.IntakeAlgae()); - NamedCommands.registerCommand("Eject Algae", autoCommands.EjectAlgae()); - - autoChooser = AutoBuilder.buildAutoChooser("Tests"); - SmartDashboard.putData("Auto Mode", autoChooser); - SmartDashboard.putData("Restore Defaults", Commands.runOnce(networkTables::RestoreDefaults)); - - if (!RobotBase.isSimulation()){ - configureBindings(); - } - else{ - configureSimBindings(); - } - - // Warmup PathPlanner to avoid Java pauses - FollowPathCommand.warmupCommand().schedule(); + configureBindings(); + autoRoutines.configure(); + swerve.registerTelemetry(swerveTelemetry::telemeterize); } - - public void robotInit() { - - SmartDashboard.putBoolean("IsSimulation", RobotBase.isSimulation()); - - if (RobotBase.isSimulation()) { - System.setProperty("phoenix.staleCheckingEnabled", "false"); // Disable stale checking in simulation - } - - superstructure.RobotInit(); - } - + + /** + * Use this method to define your trigger->command mappings. Triggers can be created via the + * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary + * predicate, or via the named factories in {@link + * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link + * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller + * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight + * joysticks}. + */ private void configureBindings() { - drivetrain.setDefaultCommand( - drivetrain.applyRequest(() -> { - double exponentVelocity = networkTables.getDoubleValue(NetworkTables.ConstantId.ControllerVelocityCurveExponent); - double exponentRotation = networkTables.getDoubleValue(NetworkTables.ConstantId.ControllerRotationCurveExponent); - if (!controller.rightBumper().getAsBoolean()) { - double fieldX = fieldXSlewFilter.calculate( - networkTables.getVelocityValue(NetworkTables.ConstantId.MaxSpeed).in(MetersPerSecond) * ExponentialConvert(-controller.getLeftY(), exponentVelocity) - ); - double fieldY = fieldYSlewFilter.calculate( - networkTables.getVelocityValue(NetworkTables.ConstantId.MaxSpeed).in(MetersPerSecond) * ExponentialConvert(-controller.getLeftX(), exponentVelocity) - ); - double fieldRotate = fieldRotateSlewFilter.calculate( - networkTables.getAngularRateValue(NetworkTables.ConstantId.MaxAngularRate).in(RadiansPerSecond) * ExponentialConvert(-controller.getRightX(), exponentRotation) - ); - return fieldCentricDrive.withVelocityX(fieldX).withVelocityY(fieldY).withRotationalRate(fieldRotate); - } else { - double robotX = robotXSlewFilter.calculate( - networkTables.getVelocityValue(NetworkTables.ConstantId.MaxSpeed).in(MetersPerSecond) * ExponentialConvert(-controller.getLeftY(), exponentVelocity) - ); - double robotY = robotYSlewFilter.calculate( - networkTables.getVelocityValue(NetworkTables.ConstantId.MaxSpeed).in(MetersPerSecond) * ExponentialConvert(-controller.getLeftX(), exponentVelocity) - ); - double robotRotate = robotRotateSlewFilter.calculate( - networkTables.getAngularRateValue(NetworkTables.ConstantId.MaxAngularRate).in(RadiansPerSecond) * ExponentialConvert(-controller.getRightX(), exponentRotation) - ); - return robotCentricDrive.withVelocityX(robotX).withVelocityY(robotY).withRotationalRate(robotRotate); - } - }) - ); - - new Trigger(controller.a().whileTrue(drivetrain.applyRequest(() -> brake))); - new Trigger(controller.b().whileTrue(drivetrain.applyRequest(() -> point.withModuleDirection(new Rotation2d(-controller.getLeftY(), -controller.getLeftX()))))); - - new Trigger(controller.leftBumper().onTrue(Commands.runOnce(() -> drivetrain.seedFieldCentric()))); - - // ButtonBoard bindings - new Trigger(buttonBoard.button(networkTables.getIntValue(NetworkTables.ConstantId.ArmUpButton)) - .onTrue(superstructure.ArmUpPressed()) - .onFalse(superstructure.ArmUpReleased())); - - new Trigger(buttonBoard.button(networkTables.getIntValue(NetworkTables.ConstantId.ArmDownButton)) - .onTrue(superstructure.ArmDownPressed()) - .onFalse(superstructure.ArmDownReleased())); - - new Trigger(buttonBoard - .button(networkTables.getIntValue(NetworkTables.ConstantId.RollerForwardButton)) - .onTrue(superstructure.RollerForwardPressed()) - .onFalse(superstructure.RollerForwardReleased())); - - new Trigger(buttonBoard - .button(networkTables.getIntValue(NetworkTables.ConstantId.RollerBackwardButton)) - .onTrue(superstructure.RollerBackwardPressed()) - .onFalse(superstructure.RollerBackwardReleased())); - - new Trigger(buttonBoard.button(networkTables.getIntValue(NetworkTables.ConstantId.ClimbButton)) - .onTrue(climber.ClimbPressed()) - .onFalse(climber.ClimbReleased())); //working - - new Trigger(buttonBoard.button(networkTables.getIntValue(NetworkTables.ConstantId.UnclimbButton)) - .onTrue(climber.UnclimbPressed()) - .onFalse(climber.UnclimbReleased())); //working - - new Trigger(buttonBoard.button(networkTables.getIntValue(NetworkTables.ConstantId.ResetEncoderButton)) - .onTrue(superstructure.ResetEncoderPositionCommand())); - - new Trigger(buttonBoard - .axisGreaterThan( - networkTables.getIntValue(NetworkTables.ConstantId.AlgaeIntakeButtonAxis), 0.75) - .onTrue(superstructure.AlgaeIntakePressed()) - .onFalse(superstructure.AlgaeIntakeReleased())); - - new Trigger(buttonBoard - .axisGreaterThan( - networkTables.getIntValue(NetworkTables.ConstantId.AlgaeEjectButtonAxis), 0.75) - .onTrue(superstructure.AlgaeEjectPressed()) - .onFalse(superstructure.AlgaeEjectReleased())); + configureManualDriveBindings(); + limelight.setDefaultCommand(updateVisionCommand()); - new Trigger(buttonBoard.povUp().onTrue(superstructure.CoralEjectPressed()).onFalse(superstructure.CoralEjectReleased())); + RobotModeTriggers.autonomous().or(RobotModeTriggers.teleop()) + .onTrue(intake.homingCommand()) + .onTrue(hanger.homingCommand()); - new Trigger(DriverStation::isEnabled).onTrue(climber.ClimbReleased()); //working - - for (int i = 1; i <= 11; i++) { - buttonBoard.button(i).onTrue(Commands.print("Button " + i + " pressed")); - } - - // Register telemetry with explicit type - // fixed - drivetrain.registerTelemetry(logger::telemeterize); + driver.rightTrigger().whileTrue(subsystemCommands.aimAndShoot()); + driver.rightBumper().whileTrue(subsystemCommands.shootManually()); + driver.leftTrigger().whileTrue(intake.intakeCommand()); + driver.leftBumper().onTrue(intake.runOnce(() -> intake.set(Intake.Position.STOWED))); + driver.povUp().onTrue(hanger.positionCommand(Hanger.Position.HANGING)); + driver.povDown().onTrue(hanger.positionCommand(Hanger.Position.HUNG)); } - public void configureSimBindings(){ - drivetrain.setDefaultCommand( - drivetrain.applyRequest(() -> { - double exponentVelocity = networkTables.getDoubleValue(NetworkTables.ConstantId.ControllerVelocityCurveExponent); - double exponentRotation = networkTables.getDoubleValue(NetworkTables.ConstantId.ControllerRotationCurveExponent); - if (!simController.getRawButton(1)) { - double fieldX = fieldXSlewFilter.calculate( - networkTables.getVelocityValue(NetworkTables.ConstantId.MaxSpeed).in(MetersPerSecond) * ExponentialConvert(-simController.getRawAxis(0), exponentVelocity) - ); - double fieldY = fieldYSlewFilter.calculate( - networkTables.getVelocityValue(NetworkTables.ConstantId.MaxSpeed).in(MetersPerSecond) * ExponentialConvert(-simController.getRawAxis(1), exponentVelocity) - ); - double fieldRotate = fieldRotateSlewFilter.calculate( - networkTables.getAngularRateValue(NetworkTables.ConstantId.MaxAngularRate).in(RadiansPerSecond) * ExponentialConvert(-simController.getRawAxis(2), exponentRotation) - ); - return fieldCentricDrive.withVelocityX(fieldX).withVelocityY(fieldY).withRotationalRate(fieldRotate); - } else { - double robotX = robotXSlewFilter.calculate( - networkTables.getVelocityValue(NetworkTables.ConstantId.MaxSpeed).in(MetersPerSecond) * ExponentialConvert(-simController.getRawAxis(0), exponentVelocity) - ); - double robotY = robotYSlewFilter.calculate( - networkTables.getVelocityValue(NetworkTables.ConstantId.MaxSpeed).in(MetersPerSecond) * ExponentialConvert(-simController.getRawAxis(1), exponentVelocity) - ); - double robotRotate = robotRotateSlewFilter.calculate( - networkTables.getAngularRateValue(NetworkTables.ConstantId.MaxAngularRate).in(RadiansPerSecond) * ExponentialConvert(-simController.getRawAxis(2), exponentRotation) - ); - return robotCentricDrive.withVelocityX(robotX).withVelocityY(robotY).withRotationalRate(robotRotate); - } - }) + private void configureManualDriveBindings() { + final ManualDriveCommand manualDriveCommand = new ManualDriveCommand( + swerve, + () -> -driver.getLeftY(), + () -> -driver.getLeftX(), + () -> -driver.getRightX() ); - - // Register telemetry with explicit type - // fixed - drivetrain.registerTelemetry(logger::telemeterize); + swerve.setDefaultCommand(manualDriveCommand); + driver.a().onTrue(Commands.runOnce(() -> manualDriveCommand.setLockedHeading(Rotation2d.k180deg))); + driver.b().onTrue(Commands.runOnce(() -> manualDriveCommand.setLockedHeading(Rotation2d.kCW_90deg))); + driver.x().onTrue(Commands.runOnce(() -> manualDriveCommand.setLockedHeading(Rotation2d.kCCW_90deg))); + driver.y().onTrue(Commands.runOnce(() -> manualDriveCommand.setLockedHeading(Rotation2d.kZero))); + driver.back().onTrue(Commands.runOnce(() -> manualDriveCommand.seedFieldCentric())); } - public void teleopInit() { - superstructure.TeleopInit(); - } - - public void autonomousInit() { - superstructure.AutonomousInit(); - } - - public static double ExponentialConvert(double controllerValue, double exponent) { - return Math.copySign(Math.pow(Math.abs(controllerValue), exponent), controllerValue); - } - - public Command getAutonomousCommand() { - Command selectedAuto = autoChooser.getSelected(); - - if (selectedAuto != null) { - return selectedAuto; // Respect chooser selection on both real robot and sim - } - - DriverStation.reportWarning("No autonomous selected; running no-op auto.", false); - return Commands.none(); + private Command updateVisionCommand() { + return limelight.run(() -> { + final Pose2d currentRobotPose = swerve.getState().Pose; + final Optional measurement = limelight.getMeasurement(currentRobotPose); + measurement.ifPresent(m -> { + swerve.addVisionMeasurement( + m.poseEstimate.pose, + m.poseEstimate.timestampSeconds, + m.standardDeviations + ); + }); + }) + .ignoringDisable(true); } } diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java deleted file mode 100644 index e4658fb..0000000 --- a/src/main/java/frc/robot/Superstructure.java +++ /dev/null @@ -1,316 +0,0 @@ -package frc.robot; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; -import com.ctre.phoenix.motorcontrol.VictorSPXControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix.motorcontrol.can.VictorSPX; - -import edu.wpi.first.units.Units; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.NetworkTables; -import frc.robot.NetworkTables.ConstantId; - -import edu.wpi.first.wpilibj.RobotBase; - -public class Superstructure extends edu.wpi.first.wpilibj2.command.SubsystemBase { - private final DigitalInput m_coralBeamBreak = new DigitalInput(0); - private final NetworkTables m_networkTables; - private final CommandSwerveDrivetrain m_robotDrivetrain; - - private final TalonSRX armMotor = new TalonSRX(16); - private final VictorSPX rollerMotor = new VictorSPX(18); - - public Superstructure(NetworkTables networkTables, CommandSwerveDrivetrain drivetrain) { - m_networkTables = networkTables; - m_robotDrivetrain = drivetrain; - } - - public void RobotInit() { - // Configure arm motor using Phoenix 5 API - armMotor.configFactoryDefault(); - armMotor.setNeutralMode(NeutralMode.Brake); - - // Configure PID for arm motor - armMotor.config_kP(0, m_networkTables.getDoubleValue(ConstantId.ArmMotorProportionalGainValue)); - armMotor.config_kI(0, m_networkTables.getDoubleValue(ConstantId.ArmMotorIntegralGainValue)); - armMotor.config_kD(0, m_networkTables.getDoubleValue(ConstantId.ArmMotorDerivativeGainValue)); - armMotor.config_kF(0, m_networkTables.getDoubleValue(ConstantId.ArmMotorFeedForwardGainValue)); - - // Configure motion magic - armMotor.configMotionCruiseVelocity( - m_networkTables.getDoubleValue(ConstantId.ArmMotorMagicMotionCruiseVelocity) - ); - armMotor.configMotionAcceleration( - m_networkTables.getDoubleValue(ConstantId.ArmMotorMagicMotionAccelerationVelocity) - ); - - // Configure output limits - armMotor.configNominalOutputForward( - m_networkTables.getDoubleValue(ConstantId.ArmMotorForwardNominalPercentOutput) - ); - armMotor.configNominalOutputReverse( - m_networkTables.getDoubleValue(ConstantId.ArmMotorReverseNominalPercentOutput) - ); - armMotor.configPeakOutputForward( - m_networkTables.getDoubleValue(ConstantId.ArmMotorForwardPeakPercentOutput) - ); - armMotor.configPeakOutputReverse( - m_networkTables.getDoubleValue(ConstantId.ArmMotorReversePeakPercentOutput) - ); - - // Set initial position - armMotor.setSelectedSensorPosition( - m_networkTables.getDoubleValue(ConstantId.ArmSelectedSensorPosition) - ); - - // Configure roller motor - rollerMotor.configFactoryDefault(); - rollerMotor.setNeutralMode(NeutralMode.Brake); - } - - public void TeleopInit() {} - - public void ResetDefaultPosition() { - System.out.println("Resetting position"); - armMotor.setSelectedSensorPosition(0,0,10); - armMotor.set(ControlMode.Position, - m_networkTables.getDoubleValue(ConstantId.ArmDefaultPosition) - ); - } - - public Command ResetEncoderPositionCommand() { - return Commands.runOnce(this::ResetDefaultPosition); - } - - public void AutonomousInit() { - ResetDefaultPosition(); - rollerMotor.set(VictorSPXControlMode.PercentOutput, 0); - armMotor.set(ControlMode.Position, - m_networkTables.getDoubleValue(ConstantId.ArmDefaultPosition) - ); - } - - public void PrintPosition() { - System.out.println("Position: " + armMotor.getSelectedSensorPosition()); - } - - public Command AutoCoral() { - return Commands.sequence( - Commands.runOnce(() -> { - System.out.println("============ CoralEjectPressed\nmoving rollers forward\n"); - armMotor.set(TalonSRXControlMode.Position, - m_networkTables.getDoubleValue(ConstantId.ArmDefaultPosition) - ); - rollerMotor.set(VictorSPXControlMode.PercentOutput, - m_networkTables.getDoubleValue(ConstantId.RollerMovementCoralEjectVelocity) - ); - }), - Commands.waitSeconds(0.2), - Commands.runOnce(() -> { - System.out.println("stop roller"); - rollerMotor.set(VictorSPXControlMode.PercentOutput,0); - }), - Commands.waitSeconds( - m_networkTables.getTimeValue(ConstantId.ArmCoralEjectSequenceWait).in(Units.Seconds) - ), - Commands.runOnce(() -> { - System.out.println("lower arm"); - armMotor.set(ControlMode.Position, - m_networkTables.getDoubleValue(ConstantId.ArmCoralEjectPosition) - ); - }), - Commands.waitSeconds(0.5), - Commands.runOnce(() -> { - System.out.println("Reset arm and roller"); - rollerMotor.set(VictorSPXControlMode.PercentOutput, 0); - armMotor.set(ControlMode.Position, - m_networkTables.getDoubleValue(ConstantId.ArmDefaultPosition) - ); - }) - ); - } - - public Command AlgaeIntakePressed() { - return Commands.sequence( - Commands.runOnce(() -> { - System.out.println("============ AlgaeIntakePressed"); - System.out.println("lowering arm"); - if (RobotBase.isReal()) System.out.println("Position1: " + armMotor.getSelectedSensorPosition(0)); - armMotor.set(ControlMode.Position, - m_networkTables.getDoubleValue(ConstantId.ArmIntakePosition) - ); - }), - Commands.waitSeconds( - m_networkTables.getTimeValue(ConstantId.AlgaeIntakeSequenceWait).in(Units.Seconds) - ), - Commands.runOnce(() -> { - System.out.println("stopping the lowering of arm"); - if (RobotBase.isReal()) System.out.println("Position2: " + armMotor.getSelectedSensorPosition()); - - rollerMotor.set(VictorSPXControlMode.PercentOutput, - m_networkTables.getDoubleValue(ConstantId.RollerMovementAlgaeIntakeVelocity) - ); - }) - ); - } - - public Command AlgaeIntakeReleased() { - return Commands.sequence( - Commands.runOnce(() -> { - System.out.println("============ AlgaeIntakeReleased"); - if (RobotBase.isReal()) System.out.println("Position3:" + armMotor.getSelectedSensorPosition()); - - armMotor.set(ControlMode.Position, - m_networkTables.getDoubleValue(ConstantId.ArmHoldPosition) - ); - }), - Commands.waitSeconds( - m_networkTables.getTimeValue(ConstantId.AlgaeIntakeSequenceWait).in(Units.Seconds) - ), - Commands.runOnce(() -> { - System.out.println("============ AlgaeIntakeReleased"); - if (RobotBase.isReal()) System.out.println("Position4:" + armMotor.getSelectedSensorPosition(0)); - rollerMotor.set(VictorSPXControlMode.PercentOutput, - m_networkTables.getDoubleValue(ConstantId.RollerMovementHoldVelocity)); - }) - ); - } - - public Command ArmUpPressed() { - return Commands.runOnce(() -> { - System.out.println("============ ArmUpPressed"); - armMotor.set(ControlMode.PercentOutput, - m_networkTables.getDoubleValue(ConstantId.ArmUpVelocity) - ); - }); - } - - public Command ArmUpReleased() { - return Commands.runOnce(() -> { - System.out.println("============ Arm stopped"); - armMotor.set(ControlMode.Position, - armMotor.getSelectedSensorPosition(0)); - }); - } - - public Command ArmDownPressed() { - return Commands.runOnce(() -> { - System.out.println("=========== ArmDownPressed"); - armMotor.set(ControlMode.PercentOutput, - m_networkTables.getDoubleValue(ConstantId.ArmDownVelocity) - ); - }); - } - - public Command ArmDownReleased() { - return Commands.runOnce(() -> { - System.out.println("============ Arm stopped"); - armMotor.set(ControlMode.Position, armMotor.getSelectedSensorPosition(0)); - }); - } - - public Command AutoIntakeCoral() { - return Commands.waitUntil(() -> { - System.out.println("Beambreak value: " + m_coralBeamBreak.get()); - return m_coralBeamBreak.get(); - }); - } - - public Command AlgaeEjectPressed() { - return Commands.runOnce(() -> { - System.out.println("============= AlgaeEjectPressed"); - System.out.println("Set rollers to algae eject velocity"); - rollerMotor.set(VictorSPXControlMode.PercentOutput, - m_networkTables.getDoubleValue(ConstantId.RollerMovementAlgaeEjectVelocity) - ); - }); - } - - public Command AlgaeEjectReleased() { - return Commands.runOnce(() -> { - System.out.println("========== AlgaeEjectReleased"); - System.out.println("Reset rollers"); - armMotor.set(TalonSRXControlMode.Position, - m_networkTables.getDoubleValue(ConstantId.ArmDefaultPosition) - ); - rollerMotor.set(VictorSPXControlMode.PercentOutput, 0); - }); - } - - //fixed - public Command CoralEjectPressed() { - return Commands.sequence( - Commands.runOnce(() -> { - System.out.println("============ CoralEjectPressed\nmoving rollers forward//\n"); - armMotor.set(TalonSRXControlMode.Position, - m_networkTables.getDoubleValue(ConstantId.ArmDefaultPosition) - ); - rollerMotor.set(VictorSPXControlMode.PercentOutput, - m_networkTables.getDoubleValue(ConstantId.RollerMovementCoralEjectVelocity) - ); - }), - Commands.waitSeconds(0.2), - Commands.runOnce(() -> { - System.out.println("stop roller"); - rollerMotor.set(VictorSPXControlMode.PercentOutput,0); - }), - Commands.waitSeconds( - m_networkTables.getTimeValue(ConstantId.ArmCoralEjectSequenceWait).in(Units.Seconds) - ), - Commands.runOnce(() -> { - System.out.println("lower arm"); - armMotor.set(ControlMode.Position, - m_networkTables.getDoubleValue(ConstantId.ArmCoralEjectPosition) - ); - }) - ) - .finallyDo(() -> {System.out.println("=== CoralEjectPressed FINALLY: stopping all motors ==="); - rollerMotor.set(VictorSPXControlMode.PercentOutput, 0);}); - } - - public Command CoralEjectReleased() { - return Commands.runOnce(() -> { - System.out.println("============ CoralEjectReleased"); - armMotor.set(TalonSRXControlMode.Position, - m_networkTables.getDoubleValue(ConstantId.ArmDefaultPosition) - ); - }); - } - - public Command RollerForwardPressed() { - return Commands.runOnce(() -> { - System.out.println("============ Rollers Forward"); - rollerMotor.set(VictorSPXControlMode.PercentOutput, - m_networkTables.getDoubleValue(ConstantId.RollerMovementForwardVelocity) - ); - }); - } - - public Command RollerForwardReleased() { - return Commands.runOnce(() -> { - System.out.println("============ Rollers Stopped"); - rollerMotor.set(VictorSPXControlMode.PercentOutput, 0); - }); - } - - public Command RollerBackwardPressed() { - return Commands.runOnce(() -> { - System.out.println("=========== Rollers Backward"); - rollerMotor.set(VictorSPXControlMode.PercentOutput, - m_networkTables.getDoubleValue(ConstantId.RollerMovementBackwardVelocity) - ); - }); - } - - public Command RollerBackwardReleased() { - return Commands.runOnce(() -> { - System.out.println("============ Rollers Stopped"); - rollerMotor.set(VictorSPXControlMode.PercentOutput, 0); - }); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/Telemetry.java b/src/main/java/frc/robot/Telemetry.java deleted file mode 100644 index 7dcac4c..0000000 --- a/src/main/java/frc/robot/Telemetry.java +++ /dev/null @@ -1,122 +0,0 @@ -package frc.robot; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.networktables.DoublePublisher; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StringPublisher; -import edu.wpi.first.networktables.StructArrayPublisher; -import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; -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.networktables.DoubleArrayPublisher; - -import static edu.wpi.first.units.Units.*; - -public class Telemetry { - public static Object telemeterize; - - private final double MaxSpeed; - - private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); - - private final StructPublisher drivePose; - private final StructPublisher driveSpeeds; - private final StructArrayPublisher driveModuleStates; - private final StructArrayPublisher driveModuleTargets; - private final StructArrayPublisher driveModulePositions; - private final DoublePublisher driveTimestamp; - private final DoublePublisher driveOdometryFrequency; - private final StringPublisher fieldTypePub; - private final DoubleArrayPublisher fieldPub; - private final Field2d field = new Field2d(); - private final FieldObject2d robotPose = field.getObject("RobotPose"); - - private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[4]; - private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[4]; - private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[4]; - - private final double[] poseArray = new double[3]; - private final double[] moduleStatesArray = new double[8]; - private final double[] moduleTargetsArray = new double[8]; - - public Telemetry(double maxSpeed) { - MaxSpeed = maxSpeed; - - var driveStateTable = inst.getTable("DriveState"); - drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); - driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); - driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish(); - driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish(); - driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish(); - driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish(); - driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish(); - fieldTypePub = driveStateTable.getStringTopic("FieldType").publish(); - fieldPub = driveStateTable.getDoubleArrayTopic("Field").publish(); - - for (int i = 0; i < 4; i++) { - m_moduleMechanisms[i] = new Mechanism2d(1, 1); - MechanismRoot2d rootSpeed = m_moduleMechanisms[i].getRoot("RootSpeed", 0.5, 0.5); - m_moduleSpeeds[i] = rootSpeed.append(new MechanismLigament2d("Speed", 0.5, 0, 5, new Color8Bit(Color.kBlue))); - - MechanismRoot2d rootDirection = m_moduleMechanisms[i].getRoot("RootDirection", 0.5, 0.5); - m_moduleDirections[i] = rootDirection.append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))); - } - - SignalLogger.start(); - } - - public void telemeterize(SwerveDriveState state) { - /* Telemeterize the swerve drive state */ - drivePose.set(state.Pose); - driveSpeeds.set(state.Speeds); - driveModuleStates.set(state.ModuleStates); - driveModuleTargets.set(state.ModuleTargets); - driveModulePositions.set(state.ModulePositions); - driveTimestamp.set(state.Timestamp); - driveOdometryFrequency.set(1.0 / state.OdometryPeriod); - - /* Also write to log file */ - poseArray[0] = state.Pose.getX(); - poseArray[1] = state.Pose.getY(); - poseArray[2] = state.Pose.getRotation().getDegrees(); - - for (int i = 0; i < 4; ++i) { - moduleStatesArray[i * 2 + 0] = state.ModuleStates[i].angle.getRadians(); - moduleStatesArray[i * 2 + 1] = state.ModuleStates[i].speedMetersPerSecond; - moduleTargetsArray[i * 2 + 0] = state.ModuleTargets[i].angle.getRadians(); - moduleTargetsArray[i * 2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; - } - - SignalLogger.writeDoubleArray("DriveState/Pose", poseArray); - SignalLogger.writeDoubleArray("DriveState/ModuleStates", moduleStatesArray); - SignalLogger.writeDoubleArray("DriveState/ModuleTargets", moduleTargetsArray); - SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); - - /* Telemeterize the pose to a Field2d */ - robotPose.setPose(state.Pose); - fieldTypePub.set("Field2d"); - fieldPub.set(poseArray); - - /* Telemeterize the module states to a Mechanism2d */ - - for (int i = 0; i < 4; i++) { - m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle.getDegrees()); - m_moduleDirections[i].setAngle(state.ModuleStates[i].angle.getDegrees()); - m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); - - SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); - } - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/AimAndDriveCommand.java b/src/main/java/frc/robot/commands/AimAndDriveCommand.java new file mode 100644 index 0000000..735a5c5 --- /dev/null +++ b/src/main/java/frc/robot/commands/AimAndDriveCommand.java @@ -0,0 +1,81 @@ +package frc.robot.commands; + +import static edu.wpi.first.units.Units.Degrees; + +import java.util.function.DoubleSupplier; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.Driving; +import frc.robot.Landmarks; +import frc.robot.subsystems.Swerve; +import frc.util.DriveInputSmoother; +import frc.util.GeometryUtil; +import frc.util.ManualDriveInput; + +public class AimAndDriveCommand extends Command { + private static final Angle kAimTolerance = Degrees.of(5); + + private final Swerve swerve; + private final DriveInputSmoother inputSmoother; + + private final SwerveRequest.FieldCentricFacingAngle fieldCentricFacingAngleRequest = new SwerveRequest.FieldCentricFacingAngle() + .withRotationalDeadband(Driving.kPIDRotationDeadband) + .withMaxAbsRotationalRate(Driving.kMaxRotationalRate) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage) + .withSteerRequestType(SteerRequestType.MotionMagicExpo) + .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective) + .withHeadingPID(5, 0, 0); + + public AimAndDriveCommand( + Swerve swerve, + DoubleSupplier forwardInput, + DoubleSupplier leftInput + ) { + this.swerve = swerve; + this.inputSmoother = new DriveInputSmoother(forwardInput, leftInput); + addRequirements(swerve); + } + + public AimAndDriveCommand(Swerve swerve) { + this(swerve, () -> 0, () -> 0); + } + + public boolean isAimed() { + final Rotation2d targetHeading = fieldCentricFacingAngleRequest.TargetDirection; + final Rotation2d currentHeadingInBlueAlliancePerspective = swerve.getState().Pose.getRotation(); + final Rotation2d currentHeadingInOperatorPerspective = currentHeadingInBlueAlliancePerspective.rotateBy(swerve.getOperatorForwardDirection()); + return GeometryUtil.isNear(targetHeading, currentHeadingInOperatorPerspective, kAimTolerance); + } + + private Rotation2d getDirectionToHub() { + final Translation2d hubPosition = Landmarks.hubPosition(); + final Translation2d robotPosition = swerve.getState().Pose.getTranslation(); + final Rotation2d hubDirectionInBlueAlliancePerspective = hubPosition.minus(robotPosition).getAngle(); + final Rotation2d hubDirectionInOperatorPerspective = hubDirectionInBlueAlliancePerspective.rotateBy(swerve.getOperatorForwardDirection()); + return hubDirectionInOperatorPerspective; + } + + @Override + public void execute() { + final ManualDriveInput input = inputSmoother.getSmoothedInput(); + swerve.setControl( + fieldCentricFacingAngleRequest + .withVelocityX(Driving.kMaxSpeed.times(input.forward)) + .withVelocityY(Driving.kMaxSpeed.times(input.left)) + .withTargetDirection(getDirectionToHub()) + ); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/AutoRoutines.java b/src/main/java/frc/robot/commands/AutoRoutines.java new file mode 100644 index 0000000..e7f9a40 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoRoutines.java @@ -0,0 +1,121 @@ +// 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; + +import static frc.robot.generated.ChoreoTraj.OutpostAndDepotTrajectory$0; +import static frc.robot.generated.ChoreoTraj.OutpostAndDepotTrajectory$1; +import static frc.robot.generated.ChoreoTraj.OutpostAndDepotTrajectory$2; +import static frc.robot.generated.ChoreoTraj.OutpostAndDepotTrajectory$3; + +import choreo.auto.AutoChooser; +import choreo.auto.AutoFactory; +import choreo.auto.AutoRoutine; +import choreo.auto.AutoTrajectory; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import frc.robot.subsystems.Feeder; +import frc.robot.subsystems.Floor; +import frc.robot.subsystems.Hanger; +import frc.robot.subsystems.Hood; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Limelight; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.Swerve; + +public final class AutoRoutines { + private final Swerve swerve; + private final Intake intake; + private final Floor floor; + private final Feeder feeder; + private final Shooter shooter; + private final Hood hood; + private final Hanger hanger; + private final Limelight limelight; + + private final SubsystemCommands subsystemCommands; + + private final AutoFactory autoFactory; + private final AutoChooser autoChooser; + + public AutoRoutines( + Swerve swerve, + Intake intake, + Floor floor, + Feeder feeder, + Shooter shooter, + Hood hood, + Hanger hanger, + Limelight limelight + ) { + this.swerve = swerve; + this.intake = intake; + this.floor = floor; + this.feeder = feeder; + this.shooter = shooter; + this.hood = hood; + this.hanger = hanger; + this.limelight = limelight; + + this.subsystemCommands = new SubsystemCommands(swerve, intake, floor, feeder, shooter, hood, hanger); + + this.autoFactory = swerve.createAutoFactory(); + this.autoChooser = new AutoChooser(); + } + + public void configure() { + autoChooser.addRoutine("Outpost and Depot", this::outpostAndDepotRoutine); + SmartDashboard.putData("Auto Chooser", autoChooser); + RobotModeTriggers.autonomous().whileTrue(autoChooser.selectedCommandScheduler()); + } + + private AutoRoutine outpostAndDepotRoutine() { + final AutoRoutine routine = autoFactory.newRoutine("Outpost and Depot"); + final AutoTrajectory startToOutpost = OutpostAndDepotTrajectory$0.asAutoTraj(routine); + final AutoTrajectory outpostToDepot = OutpostAndDepotTrajectory$1.asAutoTraj(routine); + final AutoTrajectory depotToShootingPose = OutpostAndDepotTrajectory$2.asAutoTraj(routine); + final AutoTrajectory shootingPoseToTower = OutpostAndDepotTrajectory$3.asAutoTraj(routine); + + routine.active().onTrue( + Commands.sequence( + startToOutpost.resetOdometry(), + startToOutpost.cmd() + ) + ); + + routine.observe(hanger::isHomed).onTrue( + Commands.sequence( + Commands.waitSeconds(0.5), + intake.runOnce(() -> intake.set(Intake.Position.INTAKE)) + ) + ); + + startToOutpost.doneDelayed(1).onTrue(outpostToDepot.cmd()); + + outpostToDepot.atTimeBeforeEnd(1).onTrue(intake.intakeCommand()); + outpostToDepot.doneDelayed(0.1).onTrue(depotToShootingPose.cmd()); + + depotToShootingPose.active().whileTrue(limelight.idle()); + depotToShootingPose.atTime(0.5).onTrue( + Commands.parallel( + shooter.spinUpCommand(2600), + hood.positionCommand(0.32) + ) + ); + depotToShootingPose.done().onTrue( + Commands.sequence( + subsystemCommands.aimAndShoot() + .withTimeout(5), + shootingPoseToTower.cmd() + ) + ); + + shootingPoseToTower.active().whileTrue(limelight.idle()); + shootingPoseToTower.active().onTrue(hanger.positionCommand(Hanger.Position.HANGING)); + shootingPoseToTower.done().onTrue(hanger.positionCommand(Hanger.Position.HUNG)); + + return routine; + } +} diff --git a/src/main/java/frc/robot/commands/ManualDriveCommand.java b/src/main/java/frc/robot/commands/ManualDriveCommand.java new file mode 100644 index 0000000..636c9be --- /dev/null +++ b/src/main/java/frc/robot/commands/ManualDriveCommand.java @@ -0,0 +1,148 @@ +package frc.robot.commands; + +import static edu.wpi.first.units.Units.Seconds; + +import java.util.Optional; +import java.util.function.DoubleSupplier; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.Driving; +import frc.robot.subsystems.Swerve; +import frc.util.DriveInputSmoother; +import frc.util.ManualDriveInput; +import frc.util.Stopwatch; + +/** + * Teleop manual drive command for the swerve drivetrain. + * + * Handles field-centric driving with manual rotation input and + * heading-hold behavior after a short delay once rotation input + * returns to zero. + */ +public class ManualDriveCommand extends Command { + private enum State { + IDLING, + DRIVING_WITH_MANUAL_ROTATION, + DRIVING_WITH_LOCKED_HEADING + } + + private static final Time kHeadingLockDelay = Seconds.of(0.25); // time to wait before locking heading + + private final Swerve swerve; + private final DriveInputSmoother inputSmoother; + private final SwerveRequest.Idle idleRequest = new SwerveRequest.Idle(); + + private final SwerveRequest.FieldCentric fieldCentricRequest = new SwerveRequest.FieldCentric() + .withDriveRequestType(DriveRequestType.OpenLoopVoltage) + .withSteerRequestType(SteerRequestType.MotionMagicExpo) + .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective); + + private final SwerveRequest.FieldCentricFacingAngle fieldCentricFacingAngleRequest = new SwerveRequest.FieldCentricFacingAngle() + .withRotationalDeadband(Driving.kPIDRotationDeadband) + .withMaxAbsRotationalRate(Driving.kMaxRotationalRate) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage) + .withSteerRequestType(SteerRequestType.MotionMagicExpo) + .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective) + .withHeadingPID(5, 0, 0); + + private State currentState = State.IDLING; + private Optional lockedHeading = Optional.empty(); + private Stopwatch headingLockStopwatch = new Stopwatch(); + private ManualDriveInput previousInput = new ManualDriveInput(); + + public ManualDriveCommand( + Swerve swerve, + DoubleSupplier forwardInput, + DoubleSupplier leftInput, + DoubleSupplier rotationInput + ) { + this.swerve = swerve; + this.inputSmoother = new DriveInputSmoother(forwardInput, leftInput, rotationInput); + addRequirements(swerve); + } + + public void seedFieldCentric() { + initialize(); + swerve.seedFieldCentric(); + } + + public void setLockedHeading(Rotation2d heading) { + lockedHeading = Optional.of(heading); + currentState = State.DRIVING_WITH_LOCKED_HEADING; + } + + private void setLockedHeadingToCurrent() { + final Rotation2d headingInBlueAlliancePerspective = swerve.getState().Pose.getRotation(); + final Rotation2d headingInOperatorPerspective = headingInBlueAlliancePerspective.rotateBy(swerve.getOperatorForwardDirection()); + setLockedHeading(headingInOperatorPerspective); + } + + private void lockHeadingIfRotationStopped(ManualDriveInput input) { + if (input.hasRotation()) { + headingLockStopwatch.reset(); + lockedHeading = Optional.empty(); + } else { + headingLockStopwatch.startIfNotRunning(); + if (headingLockStopwatch.elapsedTime().gt(kHeadingLockDelay)) { + setLockedHeadingToCurrent(); + } + } + } + + @Override + public void initialize() { + currentState = State.IDLING; + lockedHeading = Optional.empty(); + headingLockStopwatch.reset(); + previousInput = new ManualDriveInput(); + } + + @Override + public void execute() { + final ManualDriveInput input = inputSmoother.getSmoothedInput(); + if (input.hasRotation()) { + currentState = State.DRIVING_WITH_MANUAL_ROTATION; + } else if (input.hasTranslation()) { + currentState = lockedHeading.isPresent() ? State.DRIVING_WITH_LOCKED_HEADING : State.DRIVING_WITH_MANUAL_ROTATION; + } else if (previousInput.hasRotation() || previousInput.hasTranslation()) { + currentState = State.IDLING; + } + previousInput = input; + + switch (currentState) { + case IDLING: + swerve.setControl(idleRequest); + break; + case DRIVING_WITH_MANUAL_ROTATION: + lockHeadingIfRotationStopped(input); + swerve.setControl( + fieldCentricRequest + .withVelocityX(Driving.kMaxSpeed.times(input.forward)) + .withVelocityY(Driving.kMaxSpeed.times(input.left)) + .withRotationalRate(Driving.kMaxRotationalRate.times(input.rotation)) + ); + break; + case DRIVING_WITH_LOCKED_HEADING: + swerve.setControl( + fieldCentricFacingAngleRequest + .withVelocityX(Driving.kMaxSpeed.times(input.forward)) + .withVelocityY(Driving.kMaxSpeed.times(input.left)) + .withTargetDirection(lockedHeading.get()) + ); + break; + } + } + + @Override + public boolean isFinished() { + // Default drive command: runs until interrupted + return false; + } +} diff --git a/src/main/java/frc/robot/commands/PrepareShotCommand.java b/src/main/java/frc/robot/commands/PrepareShotCommand.java new file mode 100644 index 0000000..8b8187c --- /dev/null +++ b/src/main/java/frc/robot/commands/PrepareShotCommand.java @@ -0,0 +1,89 @@ +package frc.robot.commands; + +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; + +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.interpolation.InterpolatingTreeMap; +import edu.wpi.first.math.interpolation.Interpolator; +import edu.wpi.first.math.interpolation.InverseInterpolator; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Landmarks; +import frc.robot.subsystems.Hood; +import frc.robot.subsystems.Shooter; + +public class PrepareShotCommand extends Command { + private static final InterpolatingTreeMap distanceToShotMap = new InterpolatingTreeMap<>( + (startValue, endValue, q) -> + InverseInterpolator.forDouble() + .inverseInterpolate(startValue.in(Meters), endValue.in(Meters), q.in(Meters)), + (startValue, endValue, t) -> + new Shot( + Interpolator.forDouble() + .interpolate(startValue.shooterRPM, endValue.shooterRPM, t), + Interpolator.forDouble() + .interpolate(startValue.hoodPosition, endValue.hoodPosition, t) + ) + ); + + static { + distanceToShotMap.put(Inches.of(52.0), new Shot(2800, 0.19)); + distanceToShotMap.put(Inches.of(114.4), new Shot(3275, 0.40)); + distanceToShotMap.put(Inches.of(165.5), new Shot(3650, 0.48)); + } + + private final Shooter shooter; + private final Hood hood; + private final Supplier robotPoseSupplier; + + public PrepareShotCommand(Shooter shooter, Hood hood, Supplier robotPoseSupplier) { + this.shooter = shooter; + this.hood = hood; + this.robotPoseSupplier = robotPoseSupplier; + addRequirements(shooter, hood); + } + + public boolean isReadyToShoot() { + return shooter.isVelocityWithinTolerance() && hood.isPositionWithinTolerance(); + } + + private Distance getDistanceToHub() { + final Translation2d robotPosition = robotPoseSupplier.get().getTranslation(); + final Translation2d hubPosition = Landmarks.hubPosition(); + return Meters.of(robotPosition.getDistance(hubPosition)); + } + + @Override + public void execute() { + final Distance distanceToHub = getDistanceToHub(); + final Shot shot = distanceToShotMap.get(distanceToHub); + shooter.setRPM(shot.shooterRPM); + hood.setPosition(shot.hoodPosition); + SmartDashboard.putNumber("Distance to Hub (inches)", distanceToHub.in(Inches)); + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + shooter.stop(); + } + + public static class Shot { + public final double shooterRPM; + public final double hoodPosition; + + public Shot(double shooterRPM, double hoodPosition) { + this.shooterRPM = shooterRPM; + this.hoodPosition = hoodPosition; + } + } +} diff --git a/src/main/java/frc/robot/commands/SubsystemCommands.java b/src/main/java/frc/robot/commands/SubsystemCommands.java new file mode 100644 index 0000000..e47a08c --- /dev/null +++ b/src/main/java/frc/robot/commands/SubsystemCommands.java @@ -0,0 +1,100 @@ +package frc.robot.commands; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.Feeder; +import frc.robot.subsystems.Floor; +import frc.robot.subsystems.Hanger; +import frc.robot.subsystems.Hood; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.Swerve; + +public final class SubsystemCommands { + private final Swerve swerve; + private final Intake intake; + private final Floor floor; + private final Feeder feeder; + private final Shooter shooter; + private final Hood hood; + private final Hanger hanger; + + private final DoubleSupplier forwardInput; + private final DoubleSupplier leftInput; + + public SubsystemCommands( + Swerve swerve, + Intake intake, + Floor floor, + Feeder feeder, + Shooter shooter, + Hood hood, + Hanger hanger, + DoubleSupplier forwardInput, + DoubleSupplier leftInput + ) { + this.swerve = swerve; + this.intake = intake; + this.floor = floor; + this.feeder = feeder; + this.shooter = shooter; + this.hood = hood; + this.hanger = hanger; + + this.forwardInput = forwardInput; + this.leftInput = leftInput; + } + + public SubsystemCommands( + Swerve swerve, + Intake intake, + Floor floor, + Feeder feeder, + Shooter shooter, + Hood hood, + Hanger hanger + ) { + this( + swerve, + intake, + floor, + feeder, + shooter, + hood, + hanger, + () -> 0, + () -> 0 + ); + } + + public Command aimAndShoot() { + final AimAndDriveCommand aimAndDriveCommand = new AimAndDriveCommand(swerve, forwardInput, leftInput); + final PrepareShotCommand prepareShotCommand = new PrepareShotCommand(shooter, hood, () -> swerve.getState().Pose); + return Commands.parallel( + aimAndDriveCommand, + Commands.waitSeconds(0.25) + .andThen(prepareShotCommand), + Commands.waitUntil(() -> aimAndDriveCommand.isAimed() && prepareShotCommand.isReadyToShoot()) + .andThen(feed()) + ); + } + + public Command shootManually() { + return shooter.dashboardSpinUpCommand() + .andThen(feed()) + .handleInterrupt(() -> shooter.stop()); + } + + private Command feed() { + return Commands.sequence( + Commands.waitSeconds(0.25), + Commands.parallel( + feeder.feedCommand(), + Commands.waitSeconds(0.125) + .andThen(floor.feedCommand().alongWith(intake.agitateCommand())) + ) + ); + } +} diff --git a/src/main/java/frc/robot/generated/ChoreoTraj.java b/src/main/java/frc/robot/generated/ChoreoTraj.java new file mode 100644 index 0000000..57638bf --- /dev/null +++ b/src/main/java/frc/robot/generated/ChoreoTraj.java @@ -0,0 +1,98 @@ + +package frc.robot.generated; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import java.util.Map; +import java.util.OptionalInt; + +// If these imports cause errors because you're not using ChoreoLib, +// turn off "Include ChoreoLib-specific Helpers" in Choreo's codegen settings. +import choreo.auto.AutoRoutine; +import choreo.auto.AutoTrajectory; + +/** + * A class containing the name, start pose, end pose, and total time of every Choreo trajectory. + * This prevents your code from referencing deleted or misspelled trajectories, + * and removes the need for JSON parsing to load a trajectory's essential data. + * DO NOT MODIFY THIS FILE YOURSELF! It is automatically generated by Choreo. + */ +public record ChoreoTraj( + String name, + OptionalInt segment, + double totalTimeSecs, + Pose2d initialPoseBlue, + Pose2d endPoseBlue +) { + public static final ChoreoTraj OutpostAndDepotTrajectory = new ChoreoTraj( + "OutpostAndDepotTrajectory", + OptionalInt.empty(), + 11.3563, + new Pose2d(3.598, 0.64, Rotation2d.fromRadians(3.142)), + new Pose2d(0.941, 3.564, Rotation2d.fromRadians(3.142)) + ); + public static final ChoreoTraj OutpostAndDepotTrajectory$0 = new ChoreoTraj( + "OutpostAndDepotTrajectory", + OptionalInt.of(0), + 1.81742, + new Pose2d(3.598, 0.64, Rotation2d.fromRadians(3.142)), + new Pose2d(0.581, 0.669, Rotation2d.fromRadians(3.142)) + ); + public static final ChoreoTraj OutpostAndDepotTrajectory$1 = new ChoreoTraj( + "OutpostAndDepotTrajectory", + OptionalInt.of(1), + 3.40099, + new Pose2d(0.581, 0.669, Rotation2d.fromRadians(3.142)), + new Pose2d(0.668, 5.964, Rotation2d.fromRadians(3.142)) + ); + public static final ChoreoTraj OutpostAndDepotTrajectory$2 = new ChoreoTraj( + "OutpostAndDepotTrajectory", + OptionalInt.of(2), + 2.5827799999999996, + new Pose2d(0.668, 5.964, Rotation2d.fromRadians(3.142)), + new Pose2d(2.498, 4.035, Rotation2d.fromRadians(0)) + ); + public static final ChoreoTraj OutpostAndDepotTrajectory$3 = new ChoreoTraj( + "OutpostAndDepotTrajectory", + OptionalInt.of(3), + 3.555109999999999, + new Pose2d(2.498, 4.035, Rotation2d.fromRadians(0)), + new Pose2d(0.941, 3.564, Rotation2d.fromRadians(3.142)) + ); + + /** + * A map between trajectory names and their corresponding data. + * This allows for trajectory data to be looked up with strings during runtime. + */ + public static final Map ALL_TRAJECTORIES = Map.ofEntries( + Map.entry("OutpostAndDepotTrajectory", OutpostAndDepotTrajectory), + Map.entry("OutpostAndDepotTrajectory$0", OutpostAndDepotTrajectory$0), + Map.entry("OutpostAndDepotTrajectory$1", OutpostAndDepotTrajectory$1), + Map.entry("OutpostAndDepotTrajectory$2", OutpostAndDepotTrajectory$2), + Map.entry("OutpostAndDepotTrajectory$3", OutpostAndDepotTrajectory$3) + ); + + /** + * Looks up the ChoreoTraj segment of the given overall ChoreoTraj. + * WARNING: will raise an exception if not called with a valid segment index. + */ + public ChoreoTraj segment(int segment) { + var traj = ChoreoTraj.ALL_TRAJECTORIES.get(this.name + "$" + segment); + if (traj == null) { + throw new NullPointerException("Trajectory " + this.name + " does not have segment #" + segment + "."); + } + return traj; + } + + // If these methods cause errors because you're not using ChoreoLib, + // turn off "Include ChoreoLib-specific Helpers" in Choreo's codegen settings. + /** + * Load an AutoTrajectory directly from a ChoreoTraj, which may be a segment of a larger trajectory. + */ + public AutoTrajectory asAutoTraj(AutoRoutine routine) { + if (this.segment.isPresent()) { + return routine.trajectory(this.name, this.segment.getAsInt()); + } + return routine.trajectory(this.name); + } +} diff --git a/src/main/java/frc/robot/generated/ChoreoVars.java b/src/main/java/frc/robot/generated/ChoreoVars.java new file mode 100644 index 0000000..f351fe9 --- /dev/null +++ b/src/main/java/frc/robot/generated/ChoreoVars.java @@ -0,0 +1,16 @@ +package frc.robot.generated; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.*; + +/** + * Generated file containing variables defined in Choreo. + * DO NOT MODIFY THIS FILE YOURSELF; instead, change these values + * in the Choreo GUI. + */ +public final class ChoreoVars { + + private ChoreoVars() {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index df3c6b5..e1a3618 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -1,20 +1,39 @@ package frc.robot.generated; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.configs.*; -import com.ctre.phoenix6.hardware.*; -import com.ctre.phoenix6.signals.*; -import com.ctre.phoenix6.swerve.*; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.units.measure.*; - -import frc.robot.subsystems.CommandSwerveDrivetrain; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.MomentOfInertia; +import edu.wpi.first.units.measure.Voltage; // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html @@ -25,7 +44,7 @@ public class TunerConstants { // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() .withKP(100).withKI(0).withKD(0.5) - .withKS(0.1).withKV(1.5).withKA(0) + .withKS(0.1).withKV(1.16).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput @@ -70,24 +89,24 @@ public class TunerConstants { // CAN bus that the devices are located on; // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("Drivetrain Bus", "./logs/example.hoot"); + public static final CANBus kCANBus = new CANBus("main", "./logs/example.hoot"); // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.16); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.42); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 5.4; + private static final double kCoupleRatio = 4.909090909090909; - private static final double kDriveGearRatio = 7.673684210526316; + private static final double kDriveGearRatio = 5.8909090909090915; private static final double kSteerGearRatio = 12.1; private static final Distance kWheelRadius = Inches.of(2); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; - private static final int kPigeonId = 15; + private static final int kPigeonId = 45; // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); @@ -126,48 +145,48 @@ public class TunerConstants { // Front Left - private static final int kFrontLeftDriveMotorId = 1; + private static final int kFrontLeftDriveMotorId = 3; private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 9; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.108642578125); + private static final int kFrontLeftEncoderId = 21; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.172607421875); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(12); - private static final Distance kFrontLeftYPos = Inches.of(12); + private static final Distance kFrontLeftXPos = Inches.of(10); + private static final Distance kFrontLeftYPos = Inches.of(10); // Front Right - private static final int kFrontRightDriveMotorId = 7; - private static final int kFrontRightSteerMotorId = 8; - private static final int kFrontRightEncoderId = 12; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.3681640625); + private static final int kFrontRightDriveMotorId = 1; + private static final int kFrontRightSteerMotorId = 0; + private static final int kFrontRightEncoderId = 20; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.337890625); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(12); - private static final Distance kFrontRightYPos = Inches.of(-12); + private static final Distance kFrontRightXPos = Inches.of(10); + private static final Distance kFrontRightYPos = Inches.of(-10); // Back Left - private static final int kBackLeftDriveMotorId = 3; + private static final int kBackLeftDriveMotorId = 5; private static final int kBackLeftSteerMotorId = 4; - private static final int kBackLeftEncoderId = 10; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.193359375); + private static final int kBackLeftEncoderId = 22; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.01171875); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(-12); - private static final Distance kBackLeftYPos = Inches.of(12); + private static final Distance kBackLeftXPos = Inches.of(-10); + private static final Distance kBackLeftYPos = Inches.of(10); // Back Right - private static final int kBackRightDriveMotorId = 5; + private static final int kBackRightDriveMotorId = 7; private static final int kBackRightSteerMotorId = 6; - private static final int kBackRightEncoderId = 11; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.29833984375); + private static final int kBackRightEncoderId = 23; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.326171875); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(-12); - private static final Distance kBackRightYPos = Inches.of(-12); + private static final Distance kBackRightXPos = Inches.of(-10); + private static final Distance kBackRightYPos = Inches.of(-10); public static final SwerveModuleConstants FrontLeft = @@ -191,17 +210,6 @@ public class TunerConstants { kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted ); - /** - * Creates a CommandSwerveDrivetrain instance. - * This should only be called once in your robot program,. - */ - public static CommandSwerveDrivetrain createDrivetrain() { - return new CommandSwerveDrivetrain( - DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight - ); - } - - /** * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ diff --git a/src/main/java/frc/robot/sim/PhysicsSim.java b/src/main/java/frc/robot/sim/PhysicsSim.java deleted file mode 100644 index 2e6fed3..0000000 --- a/src/main/java/frc/robot/sim/PhysicsSim.java +++ /dev/null @@ -1,29 +0,0 @@ -package frc.robot.sim; - -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.units.measure.MomentOfInertia; - -import java.util.ArrayList; -import java.util.List; - -public class PhysicsSim { - private static final PhysicsSim instance = new PhysicsSim(); - private final List simProfiles = new ArrayList<>(); - - private PhysicsSim() {} - - public static PhysicsSim getInstance() { - return instance; - } - - public void addTalonFX(TalonFX talonFX, MomentOfInertia rotorInertia) { - simProfiles.add(new TalonFXSimProfile(talonFX, rotorInertia)); - } - - public void run() { - for (SimProfile simProfile : simProfiles) { - simProfile.run(); - } - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/sim/SimProfile.java b/src/main/java/frc/robot/sim/SimProfile.java deleted file mode 100644 index eadd180..0000000 --- a/src/main/java/frc/robot/sim/SimProfile.java +++ /dev/null @@ -1,25 +0,0 @@ -package frc.robot.sim; - -import com.ctre.phoenix6.Utils; -import edu.wpi.first.units.measure.Time; -import static edu.wpi.first.units.Units.*; - -public abstract class SimProfile { - private boolean running = false; - private double lastTime = 0; - - public abstract void run(); - - protected Time getPeriod() { - if (!running) { - lastTime = Utils.getCurrentTimeSeconds(); - running = true; - } - - double now = Utils.getCurrentTimeSeconds(); - double period = now - lastTime; - lastTime = now; - - return Seconds.of(period); - } -} diff --git a/src/main/java/frc/robot/sim/TalonFXSimProfile.java b/src/main/java/frc/robot/sim/TalonFXSimProfile.java deleted file mode 100644 index 4f4f03b..0000000 --- a/src/main/java/frc/robot/sim/TalonFXSimProfile.java +++ /dev/null @@ -1,32 +0,0 @@ -package frc.robot.sim; - -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.sim.TalonFXSimState; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import edu.wpi.first.units.measure.MomentOfInertia; -import static edu.wpi.first.units.Units.*; - -public class TalonFXSimProfile extends SimProfile { - private DCMotorSim motorSim; - private TalonFXSimState talonFXSim; - private static final double kMotorResistance = 0.002; - - public TalonFXSimProfile(TalonFX talonFX, MomentOfInertia rotorInertia) { - motorSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), rotorInertia.in(KilogramSquareMeters), 1), - DCMotor.getKrakenX60(1) - ); - talonFXSim = talonFX.getSimState(); - } - - @Override - public void run() { - motorSim.setInputVoltage(talonFXSim.getMotorVoltage()); - motorSim.update(getPeriod().in(Seconds)); - talonFXSim.setRawRotorPosition(motorSim.getAngularPositionRad()); - talonFXSim.setRotorVelocity(motorSim.getAngularVelocityRadPerSec() / (2 * Math.PI)); - talonFXSim.setSupplyVoltage(12 - talonFXSim.getSupplyCurrent() * kMotorResistance); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/AutoCommands/AutoCommands.java b/src/main/java/frc/robot/subsystems/AutoCommands/AutoCommands.java deleted file mode 100644 index 57898fc..0000000 --- a/src/main/java/frc/robot/subsystems/AutoCommands/AutoCommands.java +++ /dev/null @@ -1,45 +0,0 @@ -package frc.robot.subsystems.AutoCommands; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.NetworkTables; -import frc.robot.Superstructure; - -public class AutoCommands { - private final Superstructure superstructure; - private final NetworkTables networkTables; - - public AutoCommands(Superstructure superstructure, NetworkTables networkTables) { - this.superstructure = superstructure; - this.networkTables = networkTables; - } - - public Command EjectCoral() { - return Commands.sequence( - superstructure.CoralEjectPressed(), - Commands.waitSeconds(networkTables.getTimeValue(NetworkTables.ConstantId.AutoEjectCoralWait).in(edu.wpi.first.units.Units.Seconds)), - superstructure.CoralEjectReleased() - ); - } - - public Command IntakeAlgae() { - return Commands.sequence( - superstructure.AlgaeIntakePressed(), - Commands.waitSeconds(networkTables.getTimeValue(NetworkTables.ConstantId.AutoIntakeAlgaeWait).in(edu.wpi.first.units.Units.Seconds)), - superstructure.AlgaeIntakeReleased() - ); - } - - public Command EjectAlgae() { - return Commands.sequence( - // Implement algae eject - superstructure.AlgaeEjectPressed(), - Commands.waitSeconds(networkTables.getTimeValue(NetworkTables.ConstantId.AutoEjectAlgaeWait).in(edu.wpi.first.units.Units.Seconds)), - superstructure.AlgaeEjectReleased() - ); - } - - public Command IntakeCoral() { - return superstructure.AutoIntakeCoral(); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Climber/Climber.java b/src/main/java/frc/robot/subsystems/Climber/Climber.java deleted file mode 100644 index 52fa6a6..0000000 --- a/src/main/java/frc/robot/subsystems/Climber/Climber.java +++ /dev/null @@ -1,106 +0,0 @@ -package frc.robot.subsystems.Climber; - -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.configs.FeedbackConfigs; -import com.ctre.phoenix6.configs.MotionMagicConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionDutyCycle; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; - -import frc.robot.NetworkTables; -import frc.robot.NetworkTables.ConstantId; -import frc.robot.sim.PhysicsSim; - -import static edu.wpi.first.units.Units.*; - -public class Climber extends edu.wpi.first.wpilibj2.command.SubsystemBase { - private final TalonFX m_motor = new TalonFX(13, "rio"); - private final NetworkTables m_networkTables; - private double maxCurrentGoingUp = 0; - - public Climber(NetworkTables networkTables) { - m_networkTables = networkTables; - - TalonFXConfiguration cfg = new TalonFXConfiguration(); - - FeedbackConfigs fdb = cfg.Feedback; - fdb.SensorToMechanismRatio = 125; - - MotorOutputConfigs moc = cfg.MotorOutput; - moc.NeutralMode = NeutralModeValue.Brake; - - MotionMagicConfigs mm = cfg.MotionMagic; - mm.MotionMagicCruiseVelocity = 5; - mm.MotionMagicAcceleration = 10; - mm.MotionMagicJerk = 100; - - Slot0Configs slot0 = cfg.Slot0; - slot0.kS = 0.25; - slot0.kV = 0.12; - slot0.kA = 0.01; - slot0.kP = 60; - slot0.kI = 0; - slot0.kD = 0.5; - - StatusCode status = m_motor.getConfigurator().apply(cfg, 1.0); - if (!status.isOK()) { - System.out.println("Could not configure climber motor: " + status.toString()); - } - - if (edu.wpi.first.wpilibj.RobotBase.isSimulation()) { - PhysicsSim.getInstance().addTalonFX(m_motor, KilogramSquareMeters.of(0.001)); - } - } - - public Command ClimbPressed() { - return Commands.sequence( - Commands.runOnce(() -> { - maxCurrentGoingUp = 0; - System.out.println("Climbing"); - m_motor.set(m_networkTables.getDoubleValue(ConstantId.ClimbVelocity)); - }), - Commands.waitSeconds(0.1), - Commands.waitUntil(() -> { - double torqueCurrent = m_motor.getTorqueCurrent().getValueAsDouble(); - if (Math.abs(torqueCurrent) > maxCurrentGoingUp) { - maxCurrentGoingUp = torqueCurrent; - } - System.out.println("Going up. Torque Current: " + torqueCurrent + " Max current ever: " + maxCurrentGoingUp); - return Math.abs(torqueCurrent) > m_networkTables.getCurrentValue(ConstantId.ClimberTorqueCurrentLimit).in(Amps); - }), - Commands.runOnce(() -> { - System.out.println("Stopping climb because it is at full extension"); - m_motor.set(0); - m_motor.setControl(new PositionDutyCycle(m_motor.getPosition().getValueAsDouble())); - }) - ); - } - - public Command ClimbReleased() { - return Commands.runOnce(() -> { - System.out.println("Climbing stopped"); - m_motor.set(0); - m_motor.setControl(new PositionDutyCycle(m_motor.getPosition().getValueAsDouble())); - }); - } - - public Command UnclimbPressed() { - return Commands.runOnce(() -> { - System.out.println("Unclimbing"); - m_motor.set(m_networkTables.getDoubleValue(ConstantId.UnclimbVelocity)); - }); - } - - public Command UnclimbReleased() { - return Commands.runOnce(() -> { - System.out.println("Unclimbing stopped"); - m_motor.set(0); - }); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java deleted file mode 100644 index 953500b..0000000 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ /dev/null @@ -1,167 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.swerve.SwerveDrivetrain; -import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.swerve.SwerveRequest; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.robot.generated.TunerConstants; -import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; - -import frc.robot.utils.simulation.MapleSimSwerveDrivetrain; -import frc.robot.utils.simulation.SimSwerveConstants; - -import static edu.wpi.first.units.Units.Seconds; - -import org.littletonrobotics.junction.Logger; - -public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { - private static final double kSimLoopPeriod = 0.005; - private Notifier m_simNotifier = null; - private static double m_lastSimTime; - - private final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0); - private final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180); - private boolean m_hasAppliedOperatorPerspective = false; - - private final SwerveRequest.ApplyRobotSpeeds pathApplyRobotSpeeds = - new SwerveRequest.ApplyRobotSpeeds(); - - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules) { - super(drivetrainConstants, - MapleSimSwerveDrivetrain.regulateModuleConstantsForSimulation(modules)); - - configureAutoBuilder(); - if (Utils.isSimulation()) { - startSimThread(); - } - } - - private void configureAutoBuilder() { - try { - var config = RobotConfig.fromGUISettings(); - AutoBuilder.configure( - () -> getState().Pose, // Supplier of current robot pose - this::resetPose, // Consumer for seeding pose against auto - () -> getState().Speeds, // Supplier of current robot speeds - // Consumer of ChassisSpeeds and feedforwards to drive the robot - (speeds, feedforwards) -> - setControl( - pathApplyRobotSpeeds - .withSpeeds(speeds) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())), - new PPHolonomicDriveController( - // PID constants for translation - new PIDConstants(10, 0, 0), - // PID constants for rotation - new PIDConstants(7, 0, 0)), - config, - // Assume the path needs to be flipped for Red vs Blue, this is normally the - // case - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this // Subsystem for requirements - ); - } catch (Exception ex) { - DriverStation.reportError( - "Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); - } - } - - - @Override - public void periodic() { - if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance().ifPresent(alliance -> { - setOperatorPerspectiveForward( - alliance == DriverStation.Alliance.Red ? - kRedAlliancePerspectiveRotation : - kBlueAlliancePerspectiveRotation - ); - m_hasAppliedOperatorPerspective = true; - }); - } - - if (mapleSimSwerveDrivetrain != null) { - Pose2d simPose = mapleSimSwerveDrivetrain.mapleSimDrive.getSimulatedDriveTrainPose(); - super.resetPose(simPose); - Logger.recordOutput("Drive/Pose", simPose); - } else { - Logger.recordOutput("Drive/Pose", getState().Pose); - } - - Logger.recordOutput("BatteryVoltage", RobotController.getBatteryVoltage()); - Logger.recordOutput("Drive/TargetStates", getState().ModuleTargets); - Logger.recordOutput("Drive/MeasuredStates", getState().ModuleStates); - Logger.recordOutput("Drive/MeasuredSpeeds", getState().Speeds); - } - - - private MapleSimSwerveDrivetrain mapleSimSwerveDrivetrain = null; - - private void startSimThread() { - mapleSimSwerveDrivetrain = - new MapleSimSwerveDrivetrain( - Seconds.of(kSimLoopPeriod), - SimSwerveConstants.ROBOT_MASS, - SimSwerveConstants.BUMPER_LENGTH_X, - SimSwerveConstants.BUMPER_LENGTH_Y, - SimSwerveConstants.DRIVE_MOTOR_WHEEL, - SimSwerveConstants.STEER_MOTOR_WHEEL, - SimSwerveConstants.WHEEL_COF, - getModuleLocations(), - getPigeon2(), - getModules(), - TunerConstants.FrontLeft, - TunerConstants.FrontRight, - TunerConstants.BackLeft, - TunerConstants.BackRight); - /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = new Notifier(mapleSimSwerveDrivetrain::update); - m_simNotifier.startPeriodic(kSimLoopPeriod); - - // Initialize simulation pose to inside the field on black line for red alliance - double redAllianceInitialSimX = 10.2; - int redAllianceInitialSimY = 4; - - Pose2d initialSimPose = new Pose2d(redAllianceInitialSimX, redAllianceInitialSimY, new Rotation2d(0)); - mapleSimSwerveDrivetrain.mapleSimDrive.setSimulationWorldPose(initialSimPose); - } - - @Override - public void resetPose(Pose2d pose) { - if (this.mapleSimSwerveDrivetrain != null) - mapleSimSwerveDrivetrain.mapleSimDrive.setSimulationWorldPose(pose); - Timer.delay(0.1); // wait for simulation to update - super.resetPose(pose); - } - - public Command applyRequest(java.util.function.Supplier requestSupplier) { - return Commands.run(() -> setControl(requestSupplier.get()), this); - } - - @FunctionalInterface - public interface DeviceConstructor { - BaseStatusSignal create(int deviceId, String canbus); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Feeder.java b/src/main/java/frc/robot/subsystems/Feeder.java new file mode 100644 index 0000000..317a318 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Feeder.java @@ -0,0 +1,98 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.KrakenX60; +import frc.robot.Ports; + +public class Feeder extends SubsystemBase { + public enum Speed { + FEED(5000); + + private final double rpm; + + private Speed(double rpm) { + this.rpm = rpm; + } + + public AngularVelocity angularVelocity() { + return RPM.of(rpm); + } + } + + private final TalonFX motor; + private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0); + private final VoltageOut voltageRequest = new VoltageOut(0); + + public Feeder() { + motor = new TalonFX(Ports.kFeeder, Ports.kRoboRioCANBus); + + final TalonFXConfiguration config = new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast) + ) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(120)) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(Amps.of(50)) + .withSupplyCurrentLimitEnable(true) + ) + .withSlot0( + new Slot0Configs() + .withKP(1) + .withKI(0) + .withKD(0) + .withKV(12.0 / KrakenX60.kFreeSpeed.in(RotationsPerSecond)) // 12 volts when requesting max RPS + ); + + motor.getConfigurator().apply(config); + SmartDashboard.putData(this); + } + + public void set(Speed speed) { + motor.setControl( + velocityRequest + .withVelocity(speed.angularVelocity()) + ); + } + + public void setPercentOutput(double percentOutput) { + motor.setControl( + voltageRequest + .withOutput(Volts.of(percentOutput * 12.0)) + ); + } + + public Command feedCommand() { + return startEnd(() -> set(Speed.FEED), () -> setPercentOutput(0)); + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.addStringProperty("Command", () -> getCurrentCommand() != null ? getCurrentCommand().getName() : "null", null); + builder.addDoubleProperty("RPM", () -> motor.getVelocity().getValue().in(RPM), null); + builder.addDoubleProperty("Stator Current", () -> motor.getStatorCurrent().getValue().in(Amps), null); + builder.addDoubleProperty("Supply Current", () -> motor.getSupplyCurrent().getValue().in(Amps), null); + } +} diff --git a/src/main/java/frc/robot/subsystems/Floor.java b/src/main/java/frc/robot/subsystems/Floor.java new file mode 100644 index 0000000..6d2a16f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Floor.java @@ -0,0 +1,80 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Ports; + +public class Floor extends SubsystemBase { + public enum Speed { + STOP(0), + FEED(0.83); + + private final double percentOutput; + + private Speed(double percentOutput) { + this.percentOutput = percentOutput; + } + + public Voltage voltage() { + return Volts.of(percentOutput * 12.0); + } + } + + private final TalonFX motor; + private final VoltageOut voltageRequest = new VoltageOut(0); + + public Floor() { + motor = new TalonFX(Ports.kFloor, Ports.kRoboRioCANBus); + + final TalonFXConfiguration config = new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + ) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(120)) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(Amps.of(30)) + .withSupplyCurrentLimitEnable(true) + ); + + motor.getConfigurator().apply(config); + SmartDashboard.putData(this); + } + + public void set(Speed speed) { + motor.setControl( + voltageRequest + .withOutput(speed.voltage()) + ); + } + + public Command feedCommand() { + return startEnd(() -> set(Speed.FEED), () -> set(Speed.STOP)); + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.addStringProperty("Command", () -> getCurrentCommand() != null ? getCurrentCommand().getName() : "null", null); + builder.addDoubleProperty("RPM", () -> motor.getVelocity().getValue().in(RPM), null); + builder.addDoubleProperty("Stator Current", () -> motor.getStatorCurrent().getValue().in(Amps), null); + builder.addDoubleProperty("Supply Current", () -> motor.getSupplyCurrent().getValue().in(Amps), null); + } +} diff --git a/src/main/java/frc/robot/subsystems/Hanger.java b/src/main/java/frc/robot/subsystems/Hanger.java new file mode 100644 index 0000000..fe4b78f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Hanger.java @@ -0,0 +1,151 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.DistanceUnit; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.Per; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.KrakenX60; +import frc.robot.Ports; + +public class Hanger extends SubsystemBase { + public enum Position { + HOMED(0), + EXTEND_HOPPER(2), + HANGING(6), + HUNG(0.2); + + private final double inches; + + private Position(double inches) { + this.inches = inches; + } + + public Angle motorAngle() { + final Measure angleMeasure = Inches.of(inches).divideRatio(kHangerExtensionPerMotorAngle); + return Rotations.of(angleMeasure.in(Rotations)); // Promote from Measure to Angle + } + } + + private static final Per kHangerExtensionPerMotorAngle = Inches.of(6).div(Rotations.of(142)); + private static final Distance kExtensionTolerance = Inches.of(1); + + private final TalonFX motor; + private final MotionMagicVoltage motionMagicRequest = new MotionMagicVoltage(0).withSlot(0); + private final VoltageOut voltageRequest = new VoltageOut(0); + + private boolean isHomed = false; + + public Hanger() { + motor = new TalonFX(Ports.kHanger, Ports.kRoboRioCANBus); + + final TalonFXConfiguration config = new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + ) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(20)) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(Amps.of(70)) + .withSupplyCurrentLimitEnable(true) + ) + .withMotionMagic( + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(KrakenX60.kFreeSpeed) + .withMotionMagicAcceleration(KrakenX60.kFreeSpeed.per(Second)) + ) + .withSlot0( + new Slot0Configs() + .withKP(10) + .withKI(0) + .withKD(0) + .withKV(12.0 / KrakenX60.kFreeSpeed.in(RotationsPerSecond)) // 12 volts when requesting max RPS + ); + + motor.getConfigurator().apply(config); + SmartDashboard.putData(this); + } + + public void set(Position position) { + motor.setControl( + motionMagicRequest + .withPosition(position.motorAngle()) + ); + } + + public void setPercentOutput(double percentOutput) { + motor.setControl( + voltageRequest + .withOutput(Volts.of(percentOutput * 12.0)) + ); + } + + public Command positionCommand(Position position) { + return runOnce(() -> set(position)) + .andThen(Commands.waitUntil(this::isExtensionWithinTolerance)); + } + + public Command homingCommand() { + return Commands.sequence( + runOnce(() -> setPercentOutput(-0.05)), + Commands.waitUntil(() -> motor.getSupplyCurrent().getValue().in(Amps) > 0.4), + runOnce(() -> { + motor.setPosition(Position.HOMED.motorAngle()); + isHomed = true; + set(Position.EXTEND_HOPPER); + }) + ) + .unless(() -> isHomed) + .withInterruptBehavior(InterruptionBehavior.kCancelIncoming); + } + + public boolean isHomed() { + return isHomed; + } + + private boolean isExtensionWithinTolerance() { + final Distance currentExtension = motorAngleToExtension(motor.getPosition().getValue()); + final Distance targetExtension = motorAngleToExtension(motionMagicRequest.getPositionMeasure()); + return currentExtension.isNear(targetExtension, kExtensionTolerance); + } + + private Distance motorAngleToExtension(Angle motorAngle) { + final Measure extensionMeasure = motorAngle.timesRatio(kHangerExtensionPerMotorAngle); + return Inches.of(extensionMeasure.in(Inches)); // Promote from Measure to Distance + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.addStringProperty("Command", () -> getCurrentCommand() != null ? getCurrentCommand().getName() : "null", null); + builder.addDoubleProperty("Extension (inches)", () -> motorAngleToExtension(motor.getPosition().getValue()).in(Inches), null); + builder.addDoubleProperty("Supply Current", () -> motor.getSupplyCurrent().getValue().in(Amps), null); + } +} diff --git a/src/main/java/frc/robot/subsystems/Hood.java b/src/main/java/frc/robot/subsystems/Hood.java new file mode 100644 index 0000000..8753661 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Hood.java @@ -0,0 +1,90 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Millimeters; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Value; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Ports; + +public class Hood extends SubsystemBase { + private static final Distance kServoLength = Millimeters.of(100); + private static final LinearVelocity kMaxServoSpeed = Millimeters.of(20).per(Second); + private static final double kMinPosition = 0.01; + private static final double kMaxPosition = 0.77; + private static final double kPositionTolerance = 0.01; + + private final Servo leftServo; + private final Servo rightServo; + + private double currentPosition = 0.5; + private double targetPosition = 0.5; + private Time lastUpdateTime = Seconds.of(0); + + public Hood() { + leftServo = new Servo(Ports.kHoodLeftServo); + rightServo = new Servo(Ports.kHoodRightServo); + leftServo.setBoundsMicroseconds(2000, 1800, 1500, 1200, 1000); + rightServo.setBoundsMicroseconds(2000, 1800, 1500, 1200, 1000); + setPosition(currentPosition); + SmartDashboard.putData(this); + } + + /** Expects a position between 0.0 and 1.0 */ + public void setPosition(double position) { + final double clampedPosition = MathUtil.clamp(position, kMinPosition, kMaxPosition); + leftServo.set(clampedPosition); + rightServo.set(clampedPosition); + targetPosition = clampedPosition; + } + + /** Expects a position between 0.0 and 1.0 */ + public Command positionCommand(double position) { + return runOnce(() -> setPosition(position)) + .andThen(Commands.waitUntil(this::isPositionWithinTolerance)); + } + + public boolean isPositionWithinTolerance() { + return MathUtil.isNear(targetPosition, currentPosition, kPositionTolerance); + } + + private void updateCurrentPosition() { + final Time currentTime = Seconds.of(Timer.getFPGATimestamp()); + final Time elapsedTime = currentTime.minus(lastUpdateTime); + lastUpdateTime = currentTime; + + if (isPositionWithinTolerance()) { + currentPosition = targetPosition; + return; + } + + final Distance maxDistanceTraveled = kMaxServoSpeed.times(elapsedTime); + final double maxPercentageTraveled = maxDistanceTraveled.div(kServoLength).in(Value); + currentPosition = targetPosition > currentPosition + ? Math.min(targetPosition, currentPosition + maxPercentageTraveled) + : Math.max(targetPosition, currentPosition - maxPercentageTraveled); + } + + @Override + public void periodic() { + updateCurrentPosition(); + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.addStringProperty("Command", () -> getCurrentCommand() != null ? getCurrentCommand().getName() : "null", null); + builder.addDoubleProperty("Current Position", () -> currentPosition, null); + builder.addDoubleProperty("Target Position", () -> targetPosition, value -> setPosition(value)); + } +} diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java new file mode 100644 index 0000000..87b1439 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -0,0 +1,214 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.KrakenX60; +import frc.robot.Ports; + +public class Intake extends SubsystemBase { + public enum Speed { + STOP(0), + INTAKE(0.8); + + private final double percentOutput; + + private Speed(double percentOutput) { + this.percentOutput = percentOutput; + } + + public Voltage voltage() { + return Volts.of(percentOutput * 12.0); + } + } + + public enum Position { + HOMED(110), + STOWED(100), + INTAKE(-4), + AGITATE(20); + + private final double degrees; + + private Position(double degrees) { + this.degrees = degrees; + } + + public Angle angle() { + return Degrees.of(degrees); + } + } + + private static final double kPivotReduction = 50.0; + private static final AngularVelocity kMaxPivotSpeed = KrakenX60.kFreeSpeed.div(kPivotReduction); + private static final Angle kPositionTolerance = Degrees.of(5); + + private final TalonFX pivotMotor, rollerMotor; + private final VoltageOut pivotVoltageRequest = new VoltageOut(0); + private final MotionMagicVoltage pivotMotionMagicRequest = new MotionMagicVoltage(0).withSlot(0); + private final VoltageOut rollerVoltageRequest = new VoltageOut(0); + + private boolean isHomed = false; + + public Intake() { + pivotMotor = new TalonFX(Ports.kIntakePivot, Ports.kCANivoreCANBus); + rollerMotor = new TalonFX(Ports.kIntakeRollers, Ports.kRoboRioCANBus); + configurePivotMotor(); + configureRollerMotor(); + SmartDashboard.putData(this); + } + + private void configurePivotMotor() { + final TalonFXConfiguration config = new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + ) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(120)) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(Amps.of(70)) + .withSupplyCurrentLimitEnable(true) + ) + .withFeedback( + new FeedbackConfigs() + .withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor) + .withSensorToMechanismRatio(kPivotReduction) + ) + .withMotionMagic( + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(kMaxPivotSpeed) + .withMotionMagicAcceleration(kMaxPivotSpeed.per(Second)) + ) + .withSlot0( + new Slot0Configs() + .withKP(300) + .withKI(0) + .withKD(0) + .withKV(12.0 / kMaxPivotSpeed.in(RotationsPerSecond)) // 12 volts when requesting max RPS + ); + pivotMotor.getConfigurator().apply(config); + } + + private void configureRollerMotor() { + final TalonFXConfiguration config = new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + ) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(120)) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(Amps.of(70)) + .withSupplyCurrentLimitEnable(true) + ); + rollerMotor.getConfigurator().apply(config); + } + + private boolean isPositionWithinTolerance() { + final Angle currentPosition = pivotMotor.getPosition().getValue(); + final Angle targetPosition = pivotMotionMagicRequest.getPositionMeasure(); + return currentPosition.isNear(targetPosition, kPositionTolerance); + } + + private void setPivotPercentOutput(double percentOutput) { + pivotMotor.setControl( + pivotVoltageRequest + .withOutput(Volts.of(percentOutput * 12.0)) + ); + } + + public void set(Position position) { + pivotMotor.setControl( + pivotMotionMagicRequest + .withPosition(position.angle()) + ); + } + + public void set(Speed speed) { + rollerMotor.setControl( + rollerVoltageRequest + .withOutput(speed.voltage()) + ); + } + + public Command intakeCommand() { + return startEnd( + () -> { + set(Position.INTAKE); + set(Speed.INTAKE); + }, + () -> set(Speed.STOP) + ); + } + + public Command agitateCommand() { + return runOnce(() -> set(Speed.INTAKE)) + .andThen( + Commands.sequence( + runOnce(() -> set(Position.AGITATE)), + Commands.waitUntil(this::isPositionWithinTolerance), + runOnce(() -> set(Position.INTAKE)), + Commands.waitUntil(this::isPositionWithinTolerance) + ) + .repeatedly() + ) + .handleInterrupt(() -> { + set(Position.INTAKE); + set(Speed.STOP); + }); + } + + public Command homingCommand() { + return Commands.sequence( + runOnce(() -> setPivotPercentOutput(0.1)), + Commands.waitUntil(() -> pivotMotor.getSupplyCurrent().getValue().in(Amps) > 6), + runOnce(() -> { + pivotMotor.setPosition(Position.HOMED.angle()); + isHomed = true; + set(Position.STOWED); + }) + ) + .unless(() -> isHomed) + .withInterruptBehavior(InterruptionBehavior.kCancelIncoming); + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.addStringProperty("Command", () -> getCurrentCommand() != null ? getCurrentCommand().getName() : "null", null); + builder.addDoubleProperty("Angle (degrees)", () -> pivotMotor.getPosition().getValue().in(Degrees), null); + builder.addDoubleProperty("RPM", () -> rollerMotor.getVelocity().getValue().in(RPM), null); + builder.addDoubleProperty("Pivot Supply Current", () -> pivotMotor.getSupplyCurrent().getValue().in(Amps), null); + builder.addDoubleProperty("Roller Supply Current", () -> rollerMotor.getSupplyCurrent().getValue().in(Amps), null); + } +} diff --git a/src/main/java/frc/robot/subsystems/Led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/Led/LEDSubsystem.java deleted file mode 100644 index bd61292..0000000 --- a/src/main/java/frc/robot/subsystems/Led/LEDSubsystem.java +++ /dev/null @@ -1,255 +0,0 @@ -package frc.robot.subsystems.Led; - -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class LEDSubsystem extends SubsystemBase { - private static final int kPWMPort = 1; - private static final int kLength = 30; - - private final AddressableLED m_led; - private final AddressableLEDBuffer m_ledBuffer; - - private Pattern m_currentPattern = Pattern.OFF; - private Color m_primaryColor = new Color(0, 0, 0); - private Color m_secondaryColor = new Color(0, 0, 0); - private double m_animationCounter = 0.0; - private double m_blinkFrequency = 1.0; - private Color spartanGold = new Color(0xFE, 0xC8, 0x08); - private enum Pattern { - SOLID, - RAINBOW, - CHASE, - BLINK, - ALTERNATING, - BREATHE, - GRADIENT_WAVE, - ALLIANCE_GRADIENT, - OFF - } - - public LEDSubsystem() { - m_led = new AddressableLED(kPWMPort); - m_ledBuffer = new AddressableLEDBuffer(kLength); - - m_led.setLength(kLength); - m_led.setData(m_ledBuffer); - m_led.start(); - } - - @Override - public void periodic() { - updateLEDs(); - m_led.setData(m_ledBuffer); - m_animationCounter += 0.02; - } - - public void setSolidColor(Color color) { - m_currentPattern = Pattern.SOLID; - m_primaryColor = color; - } - - public void setRainbow() { - m_currentPattern = Pattern.RAINBOW; - } - - public void setChase(Color color) { - m_currentPattern = Pattern.CHASE; - m_primaryColor = color; - } - - public void setBlink(Color color, double frequency) { - m_currentPattern = Pattern.BLINK; - m_primaryColor = color; - m_blinkFrequency = frequency; - } - - public void setBlink(Color color) { - setBlink(color, 1.0); - } - - public void setAlternating(Color color1, Color color2) { - m_currentPattern = Pattern.ALTERNATING; - m_primaryColor = color1; - m_secondaryColor = color2; - } - - public void setBreathe(Color color) { - m_currentPattern = Pattern.BREATHE; - m_primaryColor = color; - } - - public void setGradientWave(Color targetColor) { - m_currentPattern = Pattern.GRADIENT_WAVE; - m_primaryColor = targetColor; - } - - public void setAllianceGradient(Color fromColor, Color toColor) { - m_currentPattern = Pattern.ALLIANCE_GRADIENT; - m_primaryColor = fromColor; - m_secondaryColor = toColor; - } - - public void off() { - m_currentPattern = Pattern.OFF; - for (int i = 0; i < kLength; i++) { - m_ledBuffer.setRGB(i, 0, 0, 0); - } - } - - public void setDisabledMode() { - setRainbow(); - } - - public void setTeleopMode() { - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - if (alliance.get() == DriverStation.Alliance.Red) { - setAllianceGradient(Color.kRed, spartanGold); - } else { - setAllianceGradient(Color.kBlue, spartanGold); - } - } else { - setGradientWave(spartanGold); - } - } - - public void setAutonomousMode() { - setChase(spartanGold); - } - - public void setAllianceColor() { - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - if (alliance.get() == DriverStation.Alliance.Red) { - setSolidColor(Color.kRed); - } else { - setSolidColor(Color.kBlue); - } - } else { - setSolidColor(Color.kWhite); - } - } - - private void updateLEDs() { - switch (m_currentPattern) { - case SOLID: - updateSolid(); - break; - case RAINBOW: - updateRainbow(); - break; - case CHASE: - updateChase(); - break; - case BLINK: - updateBlink(); - break; - case ALTERNATING: - updateAlternating(); - break; - case BREATHE: - updateBreathe(); - break; - case GRADIENT_WAVE: - updateGradientWave(); - break; - case ALLIANCE_GRADIENT: - updateAllianceGradient(); - break; - case OFF: - break; - } - } - - private void updateSolid() { - for (int i = 0; i < kLength; i++) { - m_ledBuffer.setLED(i, m_primaryColor); - } - } - - private void updateRainbow() { - int rainbowFirstPixelHue = (int)(m_animationCounter * 90) % 180; - - for (int i = 0; i < kLength; i++) { - int hue = (rainbowFirstPixelHue + (i * 180 / kLength)) % 180; - m_ledBuffer.setHSV(i, hue, 255, 128); - } - } - - private void updateChase() { - int position = (int)(m_animationCounter * 20) % kLength; - int chaseLength = 5; - - for (int i = 0; i < kLength; i++) { - if (Math.abs(i - position) < chaseLength) { - m_ledBuffer.setLED(i, m_primaryColor); - } else { - m_ledBuffer.setRGB(i, 0, 0, 0); - } - } - } - - private void updateBlink() { - double blinkPhase = (m_animationCounter * m_blinkFrequency) % 1.0; - boolean isOn = blinkPhase < 0.5; - - for (int i = 0; i < kLength; i++) { - if (isOn) { - m_ledBuffer.setLED(i, m_primaryColor); - } else { - m_ledBuffer.setRGB(i, 0, 0, 0); - } - } - } - - private void updateAlternating() { - for (int i = 0; i < kLength; i++) { - if (i % 2 == 0) { - m_ledBuffer.setLED(i, m_primaryColor); - } else { - m_ledBuffer.setLED(i, m_secondaryColor); - } - } - } - - private void updateBreathe() { - double brightness = (Math.sin(m_animationCounter * 2.0) + 1.0) / 2.0; - - for (int i = 0; i < kLength; i++) { - int r = (int)(m_primaryColor.red * 255 * brightness); - int g = (int)(m_primaryColor.green * 255 * brightness); - int b = (int)(m_primaryColor.blue * 255 * brightness); - - m_ledBuffer.setRGB(i, g, r, b); - } - } - - private void updateGradientWave() { - double offset = (m_animationCounter * 0.5) % 1.0; - - for (int i = 0; i < kLength; i++) { - double position = ((double)i / kLength + offset) % 1.0; - - int r = (int)(m_primaryColor.red * 255 * position); - int g = (int)(m_primaryColor.green * 255 * position); - int b = (int)(m_primaryColor.blue * 255 * position); - m_ledBuffer.setRGB(i, g, r, b); - } - } - - private void updateAllianceGradient() { - double offset = (m_animationCounter * 0.5) % 1.0; - - for (int i = 0; i < kLength; i++) { - double position = ((double)i / kLength + offset) % 1.0; - int r = (int)((m_primaryColor.red + (m_secondaryColor.red - m_primaryColor.red) * position) * 255); - int g = (int)((m_primaryColor.green + (m_secondaryColor.green - m_primaryColor.green) * position) * 255); - int b = (int)((m_primaryColor.blue + (m_secondaryColor.blue - m_primaryColor.blue) * position) * 255); - m_ledBuffer.setRGB(i, g, r, b); - } - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java new file mode 100644 index 0000000..4e46567 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -0,0 +1,65 @@ +package frc.robot.subsystems; + +import java.util.Optional; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.LimelightHelpers; +import frc.robot.LimelightHelpers.PoseEstimate; + +public class Limelight extends SubsystemBase { + private final String name; + private final NetworkTable telemetryTable; + private final StructPublisher posePublisher; + + public Limelight(String name) { + this.name = name; + this.telemetryTable = NetworkTableInstance.getDefault().getTable("SmartDashboard/" + name); + this.posePublisher = telemetryTable.getStructTopic("Estimated Robot Pose", Pose2d.struct).publish(); + } + + public Optional getMeasurement(Pose2d currentRobotPose) { + LimelightHelpers.SetRobotOrientation(name, currentRobotPose.getRotation().getDegrees(), 0, 0, 0, 0, 0); + + final PoseEstimate poseEstimate_MegaTag1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(name); + final PoseEstimate poseEstimate_MegaTag2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name); + if ( + poseEstimate_MegaTag1 == null + || poseEstimate_MegaTag2 == null + || poseEstimate_MegaTag1.tagCount == 0 + || poseEstimate_MegaTag2.tagCount == 0 + ) { + return Optional.empty(); + } + + // Combine the readings from MegaTag1 and MegaTag2: + // 1. Use the more stable position from MegaTag2 + // 2. Use the rotation from MegaTag1 (with low confidence) to counteract gyro drift + poseEstimate_MegaTag2.pose = new Pose2d( + poseEstimate_MegaTag2.pose.getTranslation(), + poseEstimate_MegaTag1.pose.getRotation() + ); + final Matrix standardDeviations = VecBuilder.fill(0.1, 0.1, 10.0); + + posePublisher.set(poseEstimate_MegaTag2.pose); + + return Optional.of(new Measurement(poseEstimate_MegaTag2, standardDeviations)); + } + + public static class Measurement { + public final PoseEstimate poseEstimate; + public final Matrix standardDeviations; + + public Measurement(PoseEstimate poseEstimate, Matrix standardDeviations) { + this.poseEstimate = poseEstimate; + this.standardDeviations = standardDeviations; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/Limelights/Constants.java b/src/main/java/frc/robot/subsystems/Limelights/Constants.java deleted file mode 100644 index 67ca83d..0000000 --- a/src/main/java/frc/robot/subsystems/Limelights/Constants.java +++ /dev/null @@ -1,27 +0,0 @@ -package frc.robot.subsystems.Limelights; - -import java.util.List; - -public class Constants { - public static class LimelightConstants { - public static final List limelightNames = List.of("limelight-down"); - - public static final double CAMERA_HEIGHT_METERS = 0.5; - public static final double CAMERA_PITCH_DEGREES = 25.0; - - public static final double VISION_STD_DEV_COEFFICIENT = 0.5; - public static final double MULTI_TAG_STD_DEV_MULTIPLIER = 0.5; - public static final double MAX_ANGULAR_VELOCITY_FOR_VISION = 2.0; - - public static final double MIN_TAG_AREA = 0.1; - public static final double MAX_AMBIGUITY = 0.2; - public static final double AUTO_VISION_DELAY_MS = 200.0; - - public static final int APRILTAG_PIPELINE = 0; - public static final int RETROREFLECTIVE_PIPELINE = 1; - public static final int DRIVER_CAMERA_PIPELINE = 9; - - public static final double SPEAKER_TARGET_HEIGHT_METERS = 2.0; - public static final double AMP_TARGET_HEIGHT_METERS = 1.37; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Limelights/LimelightHelpers.java b/src/main/java/frc/robot/subsystems/Limelights/LimelightHelpers.java deleted file mode 100644 index afdbd0b..0000000 --- a/src/main/java/frc/robot/subsystems/Limelights/LimelightHelpers.java +++ /dev/null @@ -1,1084 +0,0 @@ -package frc.robot.subsystems.Limelights; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.DeserializationFeature; -import com.fasterxml.jackson.databind.ObjectMapper; - -import java.io.IOException; -import java.util.ArrayList; -import java.util.List; - -//TODO: compare to the origin wpilib official LimelightHelpers.java document - -/** - * LimelightHelpers v1.6.0 (Java) - * - * A utility class for interfacing with Limelight vision systems in FRC robots. - * Based on the official LimelightHelpers library. - * - * This is a single-file library - just copy this file into your robot package. - */ -public class LimelightHelpers { - - /** - * Sanitizes the Limelight name. Returns "limelight" if name is empty or null. - */ - public static String sanitizeName(String name) { - if (name == null || name.isEmpty()) { - return "limelight"; - } - return name; - } - - /** - * Gets the NetworkTable for the specified Limelight. - */ - public static NetworkTable getLimelightNTTable(String tableName) { - return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); - } - - /** - * Gets a specific NetworkTableEntry from the Limelight table. - */ - public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { - return getLimelightNTTable(tableName).getEntry(entryName); - } - - /** - * Gets a double value from the Limelight NetworkTable. - */ - public static double getLimelightNTDouble(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); - } - - /** - * Gets a double array from the Limelight NetworkTable. - */ - public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); - } - - /** - * Gets a string from the Limelight NetworkTable. - */ - public static String getLimelightNTString(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getString(""); - } - - /** - * Sets a double value in the Limelight NetworkTable. - */ - public static void setLimelightNTDouble(String tableName, String entryName, double val) { - getLimelightNTTableEntry(tableName, entryName).setDouble(val); - } - - /** - * Sets a double array in the Limelight NetworkTable. - */ - public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] vals) { - getLimelightNTTableEntry(tableName, entryName).setDoubleArray(vals); - } - - // ========== BASIC TARGETING DATA ========== - - /** - * Gets the horizontal offset from crosshair to target (degrees). - */ - public static double getTX(String limelightName) { - return getLimelightNTDouble(limelightName, "tx"); - } - - /** - * Gets whether the Limelight has a valid target (0 or 1). - */ - public static double getTV(String limelightName) { - return getLimelightNTDouble(limelightName, "tv"); - } - - /** - * Gets the vertical offset from crosshair to target (degrees). - */ - public static double getTY(String limelightName) { - return getLimelightNTDouble(limelightName, "ty"); - } - - /** - * Gets the target area (0% to 100% of image). - */ - public static double getTA(String limelightName) { - return getLimelightNTDouble(limelightName, "ta"); - } - - /** - * Gets the pipeline's latency contribution (ms). Add to "cl" for total latency. - */ - public static double getLatency_Pipeline(String limelightName) { - return getLimelightNTDouble(limelightName, "tl"); - } - - /** - * Gets the capture pipeline latency (ms). - */ - public static double getLatency_Capture(String limelightName) { - return getLimelightNTDouble(limelightName, "cl"); - } - - /** - * Gets the JSON dump of targeting results. - */ - public static String getJSONDump(String limelightName) { - return getLimelightNTString(limelightName, "json"); - } - - // ========== POSE ESTIMATION ========== - - /** - * Converts a double array to Pose3d. - */ - public static Pose3d toPose3D(double[] inData) { - if (inData.length < 6) { - return new Pose3d(); - } - return new Pose3d( - new Translation3d(inData[0], inData[1], inData[2]), - new Rotation3d( - Math.toRadians(inData[3]), - Math.toRadians(inData[4]), - Math.toRadians(inData[5]) - ) - ); - } - - /** - * Converts a double array to Pose2d. - */ - public static Pose2d toPose2D(double[] inData) { - if (inData.length < 6) { - return new Pose2d(); - } - return new Pose2d( - new Translation2d(inData[0], inData[1]), - Rotation2d.fromDegrees(inData[5]) - ); - } - - /** - * Gets the robot's position in field space (meters). - */ - public static double[] getBotpose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - /** - * Gets the robot's position in field space (blue alliance origin). - */ - public static double[] getBotpose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - /** - * Gets the robot's position in field space (red alliance origin). - */ - public static double[] getBotpose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - /** - * Gets the robot's position relative to the primary AprilTag. - */ - public static double[] getBotpose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - } - - /** - * Gets the camera's position relative to the primary AprilTag. - */ - public static double[] getCameraPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - } - - /** - * Gets the camera's position relative to the robot. - */ - public static double[] getCameraPose_RobotSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); - } - - /** - * Gets the target's position relative to the camera. - */ - public static double[] getTargetPose_CameraSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - } - - /** - * Gets the target's position relative to the robot. - */ - public static double[] getTargetPose_RobotSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - } - - /** - * Gets the average color of the target (HSV). - */ - public static double[] getTargetColor(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "tc"); - } - - /** - * Gets the ID of the primary AprilTag. - */ - public static double getFiducialID(String limelightName) { - return getLimelightNTDouble(limelightName, "tid"); - } - - /** - * Gets the class ID of the detected object. - */ - public static String getNeuralClassID(String limelightName) { - return getLimelightNTString(limelightName, "tclass"); - } - - // ========== CONFIGURATION ========== - - /** - * Sets the active pipeline index (0-9). - */ - public static void setPipelineIndex(String limelightName, int index) { - setLimelightNTDouble(limelightName, "pipeline", index); - } - - /** - * Sets the priority tag ID for multi-tag detection. - */ - public static void setPriorityTagID(String limelightName, int ID) { - setLimelightNTDouble(limelightName, "priorityid", ID); - } - - /** - * Sets LED mode to pipeline control. - */ - public static void setLEDMode_PipelineControl(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 0); - } - - /** - * Forces LEDs off. - */ - public static void setLEDMode_ForceOff(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 1); - } - - /** - * Forces LEDs to blink. - */ - public static void setLEDMode_ForceBlink(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 2); - } - - /** - * Forces LEDs on. - */ - public static void setLEDMode_ForceOn(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 3); - } - - /** - * Sets stream mode to standard (side-by-side if two cameras). - */ - public static void setStreamMode_Standard(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 0); - } - - /** - * Sets stream mode to PiP Main (secondary camera in primary). - */ - public static void setStreamMode_PiPMain(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 1); - } - - /** - * Sets stream mode to PiP Secondary (primary camera in secondary). - */ - public static void setStreamMode_PiPSecondary(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 2); - } - - /** - * Sets the crop window for the pipeline. - * The crop window in the UI must be completely open for dynamic cropping to work. - */ - public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, - double cropYMin, double cropYMax) { - double[] cropWindow = {cropXMin, cropXMax, cropYMin, cropYMax}; - setLimelightNTDoubleArray(limelightName, "crop", cropWindow); - } - - /** - * Sets the robot's orientation for MegaTag2. - * - * @param yaw Yaw in degrees - * @param yawRate Yaw rate in degrees/sec - * @param pitch Pitch in degrees - * @param pitchRate Pitch rate in degrees/sec - * @param roll Roll in degrees - * @param rollRate Roll rate in degrees/sec - */ - public static void setRobotOrientation(String limelightName, double yaw, double yawRate, - double pitch, double pitchRate, double roll, double rollRate) { - double[] entries = {yaw, yawRate, pitch, pitchRate, roll, rollRate}; - setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); - } - - /** - * Sets fiducial downscaling for performance. - * - * @param downscale Valid values: 1.0, 1.5, 2.0, 3.0, 4.0 - */ - public static void setFiducialDownscaling(String limelightName, double downscale) { - int d = 1; // Default to 1.0 - if (downscale == 1.5) d = 2; - else if (downscale == 2.0) d = 3; - else if (downscale == 3.0) d = 4; - else if (downscale == 4.0) d = 5; - setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); - } - - /** - * Sets which AprilTag IDs the pipeline should look for. - */ - public static void setFiducialIDFiltersOverride(String limelightName, int[] validIDs) { - double[] validIDsDouble = new double[validIDs.length]; - for (int i = 0; i < validIDs.length; i++) { - validIDsDouble[i] = validIDs[i]; - } - setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); - } - - /** - * Sets the camera pose in robot space. - * The UI camera pose must be set to zeros for this to work. - */ - public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, - double up, double roll, double pitch, double yaw) { - double[] entries = {forward, side, up, roll, pitch, yaw}; - setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); - } - - /** - * Sets data to be sent to Python scripts. - */ - public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { - setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); - } - - /** - * Gets data from Python scripts. - */ - public static double[] getPythonScriptData(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "llpython"); - } - - // ========== HELPER METHODS ========== - - private static double extractArrayEntry(double[] inData, int position) { - if (inData.length < position + 1) { - return 0.0; - } - return inData[position]; - } - - // ========== RAW FIDUCIALS ========== - - /** - * Represents a raw fiducial detection. - */ - public static class RawFiducial { - public int id; - public double txnc; - public double tync; - public double ta; - public double distToCamera; - public double distToRobot; - public double ambiguity; - - public RawFiducial(int id, double txnc, double tync, double ta, - double distToCamera, double distToRobot, double ambiguity) { - this.id = id; - this.txnc = txnc; - this.tync = tync; - this.ta = ta; - this.distToCamera = distToCamera; - this.distToRobot = distToRobot; - this.ambiguity = ambiguity; - } - } - - /** - * Gets raw fiducial data from the Limelight. - */ - public static List getRawFiducials(String limelightName) { - double[] rawFiducialArray = getLimelightNTDoubleArray(limelightName, "rawfiducials"); - int valsPerEntry = 7; - - if (rawFiducialArray.length % valsPerEntry != 0) { - return new ArrayList<>(); - } - - int numFiducials = rawFiducialArray.length / valsPerEntry; - List rawFiducials = new ArrayList<>(); - - for (int i = 0; i < numFiducials; i++) { - int baseIndex = i * valsPerEntry; - int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); - double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); - double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); - double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); - double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); - double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); - double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); - - rawFiducials.add(new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity)); - } - - return rawFiducials; - } - - // ========== RAW DETECTIONS ========== - - /** - * Represents a raw neural network detection. - */ - public static class RawDetection { - public int classId; - public double txnc; - public double tync; - public double ta; - public double corner0_X; - public double corner0_Y; - public double corner1_X; - public double corner1_Y; - public double corner2_X; - public double corner2_Y; - public double corner3_X; - public double corner3_Y; - - public RawDetection(int classId, double txnc, double tync, double ta, - double corner0_X, double corner0_Y, double corner1_X, double corner1_Y, - double corner2_X, double corner2_Y, double corner3_X, double corner3_Y) { - this.classId = classId; - this.txnc = txnc; - this.tync = tync; - this.ta = ta; - this.corner0_X = corner0_X; - this.corner0_Y = corner0_Y; - this.corner1_X = corner1_X; - this.corner1_Y = corner1_Y; - this.corner2_X = corner2_X; - this.corner2_Y = corner2_Y; - this.corner3_X = corner3_X; - this.corner3_Y = corner3_Y; - } - } - - /** - * Gets raw detection data from the Limelight. - */ - public static List getRawDetections(String limelightName) { - double[] rawDetectionArray = getLimelightNTDoubleArray(limelightName, "rawdetections"); - int valsPerEntry = 12; - - if (rawDetectionArray.length % valsPerEntry != 0) { - return new ArrayList<>(); - } - - int numDetections = rawDetectionArray.length / valsPerEntry; - List rawDetections = new ArrayList<>(); - - for (int i = 0; i < numDetections; i++) { - int baseIndex = i * valsPerEntry; - int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); - double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); - double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); - double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); - double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); - double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); - double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); - double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); - double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); - double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); - double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); - double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); - - rawDetections.add(new RawDetection(classId, txnc, tync, ta, - corner0_X, corner0_Y, corner1_X, corner1_Y, - corner2_X, corner2_Y, corner3_X, corner3_Y)); - } - - return rawDetections; - } - - // ========== POSE ESTIMATE ========== - - /** - * Represents a complete pose estimate from the Limelight. - */ - public static class PoseEstimate { - public Pose2d pose; - public double timestampSeconds; - public double latency; - public int tagCount; - public double tagSpan; - public double avgTagDist; - public double avgTagArea; - public List rawFiducials; - - public PoseEstimate() { - this.pose = new Pose2d(); - this.timestampSeconds = 0.0; - this.latency = 0.0; - this.tagCount = 0; - this.tagSpan = 0.0; - this.avgTagDist = 0.0; - this.avgTagArea = 0.0; - this.rawFiducials = new ArrayList<>(); - } - - public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, - int tagCount, double tagSpan, double avgTagDist, double avgTagArea, - List rawFiducials) { - this.pose = pose; - this.timestampSeconds = timestampSeconds; - this.latency = latency; - this.tagCount = tagCount; - this.tagSpan = tagSpan; - this.avgTagDist = avgTagDist; - this.avgTagArea = avgTagArea; - this.rawFiducials = rawFiducials; - } - } - - /** - * Gets a pose estimate from the specified NetworkTable entry. - */ - private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { - NetworkTableEntry poseEntry = getLimelightNTTableEntry(limelightName, entryName); - double[] poseArray = poseEntry.getDoubleArray(new double[0]); - - if (poseArray.length == 0) { - return null; - } - - Pose2d pose = toPose2D(poseArray); - double latency = extractArrayEntry(poseArray, 6); - int tagCount = (int) extractArrayEntry(poseArray, 7); - double tagSpan = extractArrayEntry(poseArray, 8); - double tagDist = extractArrayEntry(poseArray, 9); - double tagArea = extractArrayEntry(poseArray, 10); - - // Calculate timestamp: getLastChange is in microseconds, latency is in milliseconds - double timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency / 1000.0); - - List rawFiducials = new ArrayList<>(); - int valsPerFiducial = 7; - int expectedTotalVals = 11 + valsPerFiducial * tagCount; - - if (poseArray.length == expectedTotalVals) { - for (int i = 0; i < tagCount; i++) { - int baseIndex = 11 + (i * valsPerFiducial); - int id = (int) extractArrayEntry(poseArray, baseIndex); - double txnc = extractArrayEntry(poseArray, baseIndex + 1); - double tync = extractArrayEntry(poseArray, baseIndex + 2); - double ta = extractArrayEntry(poseArray, baseIndex + 3); - double distToCamera = extractArrayEntry(poseArray, baseIndex + 4); - double distToRobot = extractArrayEntry(poseArray, baseIndex + 5); - double ambiguity = extractArrayEntry(poseArray, baseIndex + 6); - rawFiducials.add(new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity)); - } - } - - return new PoseEstimate(pose, timestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); - } - - /** - * Gets the robot pose estimate in WPILib blue alliance coordinates. - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpiblue"); - } - - /** - * Gets the robot pose estimate in WPILib red alliance coordinates. - */ - public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpired"); - } - - /** - * Gets the robot pose estimate using MegaTag2 in blue alliance coordinates. - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); - } - - /** - * Gets the robot pose estimate using MegaTag2 in red alliance coordinates. - */ - public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); - } - - // ========== JSON RESULTS (Advanced Usage) ========== - // Note: This requires Jackson for JSON parsing - // Add to build.gradle: implementation 'com.fasterxml.jackson.core:jackson-databind:2.15.0' - - private static final ObjectMapper mapper = new ObjectMapper() - .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); - - /** - * Results from JSON parsing - contains all targeting data. - */ - public static class LimelightResults { - @JsonProperty("Results") - public Results targetingResults; - - public LimelightResults() { - this.targetingResults = new Results(); - } - } - - public static class Results { - @JsonProperty("pID") - public double pipelineID; - - @JsonProperty("tl") - public double latency_pipeline; - - @JsonProperty("cl") - public double latency_capture; - - @JsonProperty("ts") - public double timestamp_LIMELIGHT_publish; - - @JsonProperty("ts_rio") - public double timestamp_RIOFPGA_capture; - - @JsonProperty("v") - public int valid; - - @JsonProperty("botpose") - public double[] botpose; - - @JsonProperty("botpose_wpiblue") - public double[] botpose_wpiblue; - - @JsonProperty("botpose_wpired") - public double[] botpose_wpired; - - @JsonProperty("Retro") - public List retroResults; - - @JsonProperty("Fiducial") - public List fiducialResults; - - @JsonProperty("Detector") - public List detectorResults; - - @JsonProperty("Classifier") - public List classifierResults; - - public Results() { - this.retroResults = new ArrayList<>(); - this.fiducialResults = new ArrayList<>(); - this.detectorResults = new ArrayList<>(); - this.classifierResults = new ArrayList<>(); - } - } - - public static class RetroreflectiveResult { - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("ta") - public double ta; - - @JsonProperty("pts") - public double[][] corners; - } - - public static class FiducialResult { - @JsonProperty("fID") - public int fiducialID; - - @JsonProperty("fam") - public String family; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("ta") - public double ta; - - @JsonProperty("t6c_ts") - public double[] cameraPose_TargetSpace; - - @JsonProperty("t6t_cs") - public double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - public double[] targetPose_RobotSpace; - - @JsonProperty("t6r_fs") - public double[] robotPose_FieldSpace; - - @JsonProperty("pts") - public double[][] corners; - } - - public static class DetectorResult { - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public int classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("ta") - public double ta; - - @JsonProperty("pts") - public double[][] corners; - } - - public static class ClassifierResult { - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public int classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("ta") - public double ta; - } - - /** - * Gets the latest targeting results by parsing the JSON dump. - * This provides access to all detection data in a structured format. - * - * @param limelightName Name of the Limelight - * @return LimelightResults object containing all targeting data - */ - public static LimelightResults getLatestResults(String limelightName) { - long start = System.nanoTime(); - String jsonString = getJSONDump(limelightName); - - try { - LimelightResults results = mapper.readValue(jsonString, LimelightResults.class); - long end = System.nanoTime(); - double latencyMs = (end - start) / 1_000_000.0; - - // Store JSON parsing latency - if (results.targetingResults != null) { - // You can add a custom field to store this if needed - System.out.println("JSON parse latency: " + latencyMs + " ms"); - } - - return results; - } catch (JsonProcessingException e) { - System.err.println("Error parsing Limelight JSON: " + e.getMessage()); - return new LimelightResults(); - } - } - - /** - * Prints a summary of the Limelight results to the console. - * Useful for debugging. - */ - public static void printResults(LimelightResults results) { - if (results == null || results.targetingResults == null) { - System.out.println("No results available"); - return; - } - - Results r = results.targetingResults; - System.out.println("=== Limelight Results ==="); - System.out.println("Pipeline: " + r.pipelineID); - System.out.println("Valid: " + (r.valid == 1 ? "Yes" : "No")); - System.out.println("Latency: " + r.latency_pipeline + " ms"); - - if (r.botpose_wpiblue != null && r.botpose_wpiblue.length >= 6) { - Pose2d pose = toPose2D(r.botpose_wpiblue); - System.out.println("Robot Pose (Blue): " + pose.toString()); - } - - System.out.println("Fiducials detected: " + r.fiducialResults.size()); - for (FiducialResult fid : r.fiducialResults) { - System.out.println(" - ID: " + fid.fiducialID + ", TX: " + fid.tx + ", TY: " + fid.ty); - } - - System.out.println("Detections: " + r.detectorResults.size()); - for (DetectorResult det : r.detectorResults) { - System.out.println(" - Class: " + det.className + ", Confidence: " + det.confidence); - } - } - - // ========== UTILITY METHODS FOR VISION PROCESSING ========== - - /** - * Checks if the Limelight has any valid targets. - */ - public static boolean hasTarget(String limelightName) { - return getTV(limelightName) == 1.0; - } - - /** - * Gets the pipeline index currently running. - */ - public static int getCurrentPipelineIndex(String limelightName) { - return (int) getLimelightNTDouble(limelightName, "getpipe"); - } - - /** - * Gets the horizontal offset with fallback if no target. - */ - public static double getTXSafe(String limelightName, double defaultValue) { - if (!hasTarget(limelightName)) { - return defaultValue; - } - return getTX(limelightName); - } - - /** - * Gets the vertical offset with fallback if no target. - */ - public static double getTYSafe(String limelightName, double defaultValue) { - if (!hasTarget(limelightName)) { - return defaultValue; - } - return getTY(limelightName); - } - - /** - * Gets the target area with fallback if no target. - */ - public static double getTASafe(String limelightName, double defaultValue) { - if (!hasTarget(limelightName)) { - return defaultValue; - } - return getTA(limelightName); - } - - /** - * Calculates the distance to target using pinhole camera model. - * - * @param targetHeightMeters Height of the target above ground - * @param cameraHeightMeters Height of the camera above ground - * @param cameraPitchRadians Camera pitch angle in radians (positive = up) - * @param targetPitchRadians Target pitch from camera in radians - * @return Distance to target in meters - */ - public static double calculateDistanceToTarget(double targetHeightMeters, - double cameraHeightMeters, - double cameraPitchRadians, - double targetPitchRadians) { - double heightDifference = targetHeightMeters - cameraHeightMeters; - double angleToTarget = cameraPitchRadians + targetPitchRadians; - return heightDifference / Math.tan(angleToTarget); - } - - /** - * Calculates the horizontal distance to target using the target's vertical offset. - * - * @param limelightName Name of the Limelight - * @param targetHeightMeters Height of the target above ground - * @param cameraHeightMeters Height of the camera above ground - * @param cameraPitchDegrees Camera pitch angle in degrees (positive = up) - * @return Distance to target in meters, or -1 if no target - */ - public static double getDistanceToTarget(String limelightName, - double targetHeightMeters, - double cameraHeightMeters, - double cameraPitchDegrees) { - if (!hasTarget(limelightName)) { - return -1.0; - } - - double ty = getTY(limelightName); - double cameraPitchRadians = Math.toRadians(cameraPitchDegrees); - double targetPitchRadians = Math.toRadians(ty); - - return calculateDistanceToTarget(targetHeightMeters, cameraHeightMeters, - cameraPitchRadians, targetPitchRadians); - } - - /** - * Takes a snapshot and saves it to the Limelight. - * Useful for debugging or logging. - */ - public static void takeSnapshot(String limelightName) { - setLimelightNTDouble(limelightName, "snapshot", 1.0); - } - - /** - * Resets the snapshot flag. - */ - public static void resetSnapshot(String limelightName) { - setLimelightNTDouble(limelightName, "snapshot", 0.0); - } - - // ========== EXAMPLE USAGE ========== - - /** - * Example method showing how to use LimelightHelpers for vision alignment. - * This is just a template - customize for your robot! - */ - public static class UsageExamples { - - /** - * Example: Get pose estimate and check quality - */ - public static void examplePoseEstimation() { - String limelightName = "limelight"; - - PoseEstimate poseEstimate = getBotPoseEstimate_wpiBlue(limelightName); - - if (poseEstimate != null && poseEstimate.tagCount > 0) { - System.out.println("Robot pose: " + poseEstimate.pose); - System.out.println("Tags seen: " + poseEstimate.tagCount); - System.out.println("Average tag distance: " + poseEstimate.avgTagDist + " m"); - - // Check if pose is reliable (multiple tags, not too far away) - boolean isReliable = poseEstimate.tagCount >= 2 && - poseEstimate.avgTagDist < 5.0 && - poseEstimate.avgTagArea > 0.1; - - if (isReliable) { - System.out.println("Pose estimate is reliable!"); - // Use this pose to update odometry - } - } - } - - /** - * Example: Check for specific AprilTag - */ - public static void exampleFiducialDetection() { - String limelightName = "limelight"; - - List fiducials = getRawFiducials(limelightName); - - // Look for a specific tag (e.g., ID 4) - for (RawFiducial fid : fiducials) { - if (fid.id == 4) { - System.out.println("Found tag 4!"); - System.out.println("Distance: " + fid.distToRobot + " m"); - System.out.println("Ambiguity: " + fid.ambiguity); - - // Low ambiguity = good detection - if (fid.ambiguity < 0.2) { - System.out.println("High confidence detection!"); - } - } - } - } - - /** - * Example: Configure Limelight for different game modes - */ - public static void exampleConfiguration() { - String limelightName = "limelight"; - - // Autonomous: Use AprilTags with LEDs off - setPipelineIndex(limelightName, 0); - setLEDMode_ForceOff(limelightName); - - // Teleop: Use retroreflective targets with LEDs on - setPipelineIndex(limelightName, 1); - setLEDMode_ForceOn(limelightName); - - // Driver mode: Turn off processing to save CPU - setPipelineIndex(limelightName, 9); // Assume pipeline 9 is driver camera - setLEDMode_ForceOff(limelightName); - } - - /** - * Example: Basic targeting for simple aiming - */ - public static void exampleSimpleTargeting() { - String limelightName = "limelight"; - - if (hasTarget(limelightName)) { - double tx = getTX(limelightName); - double ty = getTY(limelightName); - double area = getTA(limelightName); - - System.out.println("Target found!"); - System.out.println("Horizontal offset: " + tx + " degrees"); - System.out.println("Vertical offset: " + ty + " degrees"); - System.out.println("Area: " + area + "%"); - - // Use tx to aim the robot - // Use ty or area to determine distance - } else { - System.out.println("No target found"); - } - } - - /** - * Example: Using JSON results for complete data - */ - public static void exampleJSONResults() { - String limelightName = "limelight"; - - LimelightResults results = getLatestResults(limelightName); - - if (results.targetingResults.valid == 1) { - // Access all detections - for (FiducialResult fid : results.targetingResults.fiducialResults) { - System.out.println("Fiducial ID " + fid.fiducialID + - " at (" + fid.tx + ", " + fid.ty + ")"); - } - - for (DetectorResult det : results.targetingResults.detectorResults) { - System.out.println("Detected " + det.className + - " with " + (det.confidence * 100) + "% confidence"); - } - } - - printResults(results); - } - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java new file mode 100644 index 0000000..64be08c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -0,0 +1,137 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; + +import java.util.List; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.VoltageConfigs; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.KrakenX60; +import frc.robot.Ports; + +public class Shooter extends SubsystemBase { + private static final AngularVelocity kVelocityTolerance = RPM.of(100); + + private final TalonFX leftMotor, middleMotor, rightMotor; + private final List motors; + private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0); + private final VoltageOut voltageRequest = new VoltageOut(0); + + private double dashboardTargetRPM = 0.0; + + public Shooter() { + leftMotor = new TalonFX(Ports.kShooterLeft, Ports.kRoboRioCANBus); + middleMotor = new TalonFX(Ports.kShooterMiddle, Ports.kRoboRioCANBus); + rightMotor = new TalonFX(Ports.kShooterRight, Ports.kRoboRioCANBus); + motors = List.of(leftMotor, middleMotor, rightMotor); + + configureMotor(leftMotor, InvertedValue.CounterClockwise_Positive); + configureMotor(middleMotor, InvertedValue.Clockwise_Positive); + configureMotor(rightMotor, InvertedValue.Clockwise_Positive); + + SmartDashboard.putData(this); + } + + private void configureMotor(TalonFX motor, InvertedValue invertDirection) { + final TalonFXConfiguration config = new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(invertDirection) + .withNeutralMode(NeutralModeValue.Coast) + ) + .withVoltage( + new VoltageConfigs() + .withPeakReverseVoltage(Volts.of(0)) + ) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(120)) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(Amps.of(70)) + .withSupplyCurrentLimitEnable(true) + ) + .withSlot0( + new Slot0Configs() + .withKP(0.5) + .withKI(2) + .withKD(0) + .withKV(12.0 / KrakenX60.kFreeSpeed.in(RotationsPerSecond)) // 12 volts when requesting max RPS + ); + + motor.getConfigurator().apply(config); + } + + public void setRPM(double rpm) { + for (final TalonFX motor : motors) { + motor.setControl( + velocityRequest + .withVelocity(RPM.of(rpm)) + ); + } + } + + public void setPercentOutput(double percentOutput) { + for (final TalonFX motor : motors) { + motor.setControl( + voltageRequest + .withOutput(Volts.of(percentOutput * 12.0)) + ); + } + } + + public void stop() { + setPercentOutput(0.0); + } + + public Command spinUpCommand(double rpm) { + return runOnce(() -> setRPM(rpm)) + .andThen(Commands.waitUntil(this::isVelocityWithinTolerance)); + } + + public Command dashboardSpinUpCommand() { + return defer(() -> spinUpCommand(dashboardTargetRPM)); + } + + public boolean isVelocityWithinTolerance() { + return motors.stream().allMatch(motor -> { + final boolean isInVelocityMode = motor.getAppliedControl().equals(velocityRequest); + final AngularVelocity currentVelocity = motor.getVelocity().getValue(); + final AngularVelocity targetVelocity = velocityRequest.getVelocityMeasure(); + return isInVelocityMode && currentVelocity.isNear(targetVelocity, kVelocityTolerance); + }); + } + + private void initSendable(SendableBuilder builder, TalonFX motor, String name) { + builder.addDoubleProperty(name + " RPM", () -> motor.getVelocity().getValue().in(RPM), null); + builder.addDoubleProperty(name + " Stator Current", () -> motor.getStatorCurrent().getValue().in(Amps), null); + builder.addDoubleProperty(name + " Supply Current", () -> motor.getSupplyCurrent().getValue().in(Amps), null); + } + + @Override + public void initSendable(SendableBuilder builder) { + initSendable(builder, leftMotor, "Left"); + initSendable(builder, middleMotor, "Middle"); + initSendable(builder, rightMotor, "Right"); + builder.addStringProperty("Command", () -> getCurrentCommand() != null ? getCurrentCommand().getName() : "null", null); + builder.addDoubleProperty("Dashboard RPM", () -> dashboardTargetRPM, value -> dashboardTargetRPM = value); + builder.addDoubleProperty("Target RPM", () -> velocityRequest.getVelocityMeasure().in(RPM), null); + } +} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java new file mode 100644 index 0000000..4b1d4da --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -0,0 +1,174 @@ +package frc.robot.subsystems; + +import java.util.function.Supplier; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import choreo.Choreo.TrajectoryLogger; +import choreo.auto.AutoFactory; +import choreo.trajectory.SwerveSample; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; +import frc.robot.generated.TunerConstants; +import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; + +public class Swerve extends TunerSwerveDrivetrain implements Subsystem { + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; + /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ + private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; + /* Keep track if we've ever applied the operator perspective before or not */ + private boolean m_hasAppliedOperatorPerspective = false; + + /** Swerve request to apply during field-centric path following */ + private final SwerveRequest.ApplyFieldSpeeds pathFieldSpeedsRequest = new SwerveRequest.ApplyFieldSpeeds(); + private final PIDController pathXController = new PIDController(10, 0, 0); + private final PIDController pathYController = new PIDController(10, 0, 0); + private final PIDController pathThetaController = new PIDController(7, 0, 0); + + public Swerve() { + super( + TunerConstants.DrivetrainConstants, + 0, + VecBuilder.fill(0.1, 0.1, 0.1), + VecBuilder.fill(0.1, 0.1, 0.1), + TunerConstants.FrontLeft, + TunerConstants.FrontRight, + TunerConstants.BackLeft, + TunerConstants.BackRight + ); + } + + /** + * Creates a new auto factory for this drivetrain. + * + * @return AutoFactory for this drivetrain + */ + public AutoFactory createAutoFactory() { + return createAutoFactory((sample, isStart) -> {}); + } + + /** + * Creates a new auto factory for this drivetrain with the given + * trajectory logger. + * + * @param trajLogger Logger for the trajectory + * @return AutoFactory for this drivetrain + */ + public AutoFactory createAutoFactory(TrajectoryLogger trajLogger) { + return new AutoFactory( + () -> getState().Pose, + this::resetPose, + this::followPath, + true, + this, + trajLogger + ); + } + + /** + * Returns a command that applies the specified control request to this swerve drivetrain. + * + * @param request Function returning the request to apply + * @return Command to run + */ + public Command applyRequest(Supplier requestSupplier) { + return run(() -> this.setControl(requestSupplier.get())); + } + + /** + * Follows the given field-centric path sample with PID. + * + * @param sample Sample along the path to follow + */ + public void followPath(SwerveSample sample) { + pathThetaController.enableContinuousInput(-Math.PI, Math.PI); + + var pose = getState().Pose; + + var targetSpeeds = sample.getChassisSpeeds(); + targetSpeeds.vxMetersPerSecond += pathXController.calculate( + pose.getX(), sample.x + ); + targetSpeeds.vyMetersPerSecond += pathYController.calculate( + pose.getY(), sample.y + ); + targetSpeeds.omegaRadiansPerSecond += pathThetaController.calculate( + pose.getRotation().getRadians(), sample.heading + ); + + setControl( + pathFieldSpeedsRequest.withSpeeds(targetSpeeds) + .withWheelForceFeedforwardsX(sample.moduleForcesX()) + .withWheelForceFeedforwardsY(sample.moduleForcesY()) + ); + } + + @Override + public void periodic() { + /* + * Periodically try to apply the operator perspective. + * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. + * This allows us to correct the perspective in case the robot code restarts mid-match. + * Otherwise, only check and apply the operator perspective if the DS is disabled. + * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. + */ + if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance().ifPresent(allianceColor -> { + setOperatorPerspectiveForward( + allianceColor == Alliance.Red + ? kRedAlliancePerspectiveRotation + : kBlueAlliancePerspectiveRotation + ); + if (!m_hasAppliedOperatorPerspective) { + seedFieldCentric(); + } + m_hasAppliedOperatorPerspective = true; + }); + } + } + + /** + * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate + * while still accounting for measurement noise. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. + * @param timestampSeconds The timestamp of the vision measurement in seconds. + */ + @Override + public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); + } + + /** + * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate + * while still accounting for measurement noise. + *

+ * Note that the vision measurement standard deviations passed into this method + * will continue to apply to future measurements until a subsequent call to + * {@link #setVisionMeasurementStdDevs(Matrix)} or this method. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. + * @param timestampSeconds The timestamp of the vision measurement in seconds. + * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement + * in the form [x, y, theta]ᵀ, with units in meters and radians. + */ + @Override + public void addVisionMeasurement( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs + ) { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); + } +} diff --git a/src/main/java/frc/robot/utils/simulation/MapleSimSwerveDrivetrain.java b/src/main/java/frc/robot/utils/simulation/MapleSimSwerveDrivetrain.java deleted file mode 100644 index cfa1fd4..0000000 --- a/src/main/java/frc/robot/utils/simulation/MapleSimSwerveDrivetrain.java +++ /dev/null @@ -1,275 +0,0 @@ -package frc.robot.utils.simulation; - -// Copyright 2021-2025 Iron Maple 5516 -// Original Source: -// https://github.com/Shenzhen-Robotics-Alliance/maple-sim/blob/main/templates/CTRE%20Swerve%20with%20maple-sim/src/main/java/frc/robot/utils/simulation/MapleSimSwerveDrivetrain.java -// -// This code is licensed under MIT license (see https://mit-license.org/) - -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.sim.CANcoderSimState; -import com.ctre.phoenix6.sim.Pigeon2SimState; -import com.ctre.phoenix6.sim.TalonFXSimState; -import com.ctre.phoenix6.swerve.SwerveDrivetrain; -import com.ctre.phoenix6.swerve.SwerveModule; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.units.measure.*; -import edu.wpi.first.wpilibj.RobotBase; -import org.ironmaple.simulation.SimulatedArena; -import org.ironmaple.simulation.drivesims.COTS; -import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; -import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; -import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; -import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; -import org.ironmaple.simulation.motorsims.SimulatedBattery; -import org.ironmaple.simulation.motorsims.SimulatedMotorController; - -/** - * - * - *

Injects Maple-Sim simulation data into a CTRE swerve drivetrain.

- * - *

This class retrieves simulation data from Maple-Sim and injects it into the CTRE {@link - * com.ctre.phoenix6.swerve.SwerveDrivetrain} instance. - * - *

It replaces the {@link com.ctre.phoenix6.swerve.SimSwerveDrivetrain} class. - */ -public class MapleSimSwerveDrivetrain { - private final Pigeon2SimState pigeonSim; - private final SimSwerveModule[] simModules; - public final SwerveDriveSimulation mapleSimDrive; - - /** - * - * - *

Constructs a drivetrain simulation using the specified parameters.

- * - * @param simPeriod the time period of the simulation - * @param robotMassWithBumpers the total mass of the robot, including bumpers - * @param bumperLengthX the length of the bumper along the X-axis (influences the collision space - * of the robot) - * @param bumperWidthY the width of the bumper along the Y-axis (influences the collision space of - * the robot) - * @param driveMotorModel the {@link DCMotor} model for the drive motor, typically - * DCMotor.getKrakenX60Foc() - * - * @param steerMotorModel the {@link DCMotor} model for the steer motor, typically - * DCMotor.getKrakenX60Foc() - * - * @param wheelCOF the coefficient of friction of the drive wheels - * @param moduleLocations the locations of the swerve modules on the robot, in the order - * FL, FR, BL, BR - * @param pigeon the {@link Pigeon2} IMU used in the drivetrain - * @param modules the {@link SwerveModule}s, typically obtained via {@link - * SwerveDrivetrain#getModules()} - * @param moduleConstants the constants for the swerve modules - */ - public MapleSimSwerveDrivetrain( - Time simPeriod, - Mass robotMassWithBumpers, - Distance bumperLengthX, - Distance bumperWidthY, - DCMotor driveMotorModel, - DCMotor steerMotorModel, - double wheelCOF, - Translation2d[] moduleLocations, - Pigeon2 pigeon, - SwerveModule[] modules, - SwerveModuleConstants... - moduleConstants) { - this.pigeonSim = pigeon.getSimState(); - simModules = new SimSwerveModule[moduleConstants.length]; - DriveTrainSimulationConfig simulationConfig = - DriveTrainSimulationConfig.Default() - .withRobotMass(robotMassWithBumpers) - .withBumperSize(bumperLengthX, bumperWidthY) - .withGyro(COTS.ofPigeon2()) - .withCustomModuleTranslations(moduleLocations) - .withSwerveModule( - new SwerveModuleSimulationConfig( - driveMotorModel, - steerMotorModel, - moduleConstants[0].DriveMotorGearRatio, - moduleConstants[0].SteerMotorGearRatio, - Volts.of(moduleConstants[0].DriveFrictionVoltage), - Volts.of(moduleConstants[0].SteerFrictionVoltage), - Meters.of(moduleConstants[0].WheelRadius), - KilogramSquareMeters.of(moduleConstants[0].SteerInertia), - wheelCOF)); - mapleSimDrive = new SwerveDriveSimulation(simulationConfig, new Pose2d()); - - SwerveModuleSimulation[] moduleSimulations = mapleSimDrive.getModules(); - for (int i = 0; i < this.simModules.length; i++) - simModules[i] = new SimSwerveModule(moduleConstants[0], moduleSimulations[i], modules[i]); - - SimulatedArena.overrideSimulationTimings(simPeriod, 1); - SimulatedArena.getInstance().addDriveTrainSimulation(mapleSimDrive); - } - - /** - * - * - *

Update the simulation.

- * - *

Updates the Maple-Sim simulation and injects the results into the simulated CTRE devices, - * including motors and the IMU. - */ - public void update() { - SimulatedArena.getInstance().simulationPeriodic(); - pigeonSim.setRawYaw(mapleSimDrive.getSimulatedDriveTrainPose().getRotation().getMeasure()); - pigeonSim.setAngularVelocityZ( - RadiansPerSecond.of( - mapleSimDrive.getDriveTrainSimulatedChassisSpeedsRobotRelative() - .omegaRadiansPerSecond)); - } - - /** - * - * - *

Represents the simulation of a single {@link SwerveModule}.

- */ - protected static class SimSwerveModule { - public final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - moduleConstant; - public final SwerveModuleSimulation moduleSimulation; - - public SimSwerveModule( - SwerveModuleConstants - moduleConstant, - SwerveModuleSimulation moduleSimulation, - SwerveModule module) { - this.moduleConstant = moduleConstant; - this.moduleSimulation = moduleSimulation; - moduleSimulation.useDriveMotorController( - new TalonFXMotorControllerSim(module.getDriveMotor())); - moduleSimulation.useSteerMotorController( - new TalonFXMotorControllerWithRemoteCanCoderSim( - module.getSteerMotor(), module.getEncoder())); - } - } - - // Static utils classes - public static class TalonFXMotorControllerSim implements SimulatedMotorController { - public final int id; - - private final TalonFXSimState talonFXSimState; - - public TalonFXMotorControllerSim(TalonFX talonFX) { - this.id = talonFX.getDeviceID(); - this.talonFXSimState = talonFX.getSimState(); - } - - @Override - public Voltage updateControlSignal( - Angle mechanismAngle, - AngularVelocity mechanismVelocity, - Angle encoderAngle, - AngularVelocity encoderVelocity) { - talonFXSimState.setRawRotorPosition(encoderAngle); - talonFXSimState.setRotorVelocity(encoderVelocity); - talonFXSimState.setSupplyVoltage(SimulatedBattery.getBatteryVoltage()); - - return talonFXSimState.getMotorVoltageMeasure(); - } - } - - public static class TalonFXMotorControllerWithRemoteCanCoderSim - extends TalonFXMotorControllerSim { - private final int encoderId; - private final CANcoderSimState remoteCancoderSimState; - - public TalonFXMotorControllerWithRemoteCanCoderSim(TalonFX talonFX, CANcoder cancoder) { - super(talonFX); - this.remoteCancoderSimState = cancoder.getSimState(); - - this.encoderId = cancoder.getDeviceID(); - } - - @Override - public Voltage updateControlSignal( - Angle mechanismAngle, - AngularVelocity mechanismVelocity, - Angle encoderAngle, - AngularVelocity encoderVelocity) { - remoteCancoderSimState.setSupplyVoltage(SimulatedBattery.getBatteryVoltage()); - remoteCancoderSimState.setRawPosition(mechanismAngle); - remoteCancoderSimState.setVelocity(mechanismVelocity); - - return super.updateControlSignal( - mechanismAngle, mechanismVelocity, encoderAngle, encoderVelocity); - } - } - - /** - * - * - *

Regulates all {@link SwerveModuleConstants} for a drivetrain simulation.

- * - *

This method processes an array of {@link SwerveModuleConstants} to apply necessary - * adjustments for simulation purposes, ensuring compatibility and avoiding known bugs. - * - * @see #regulateModuleConstantForSimulation(SwerveModuleConstants) - */ - public static SwerveModuleConstants[] regulateModuleConstantsForSimulation( - SwerveModuleConstants[] moduleConstants) { - for (SwerveModuleConstants moduleConstant : moduleConstants) - regulateModuleConstantForSimulation(moduleConstant); - - return moduleConstants; - } - - /** - * - * - *

Regulates the {@link SwerveModuleConstants} for a single module.

- * - *

This method applies specific adjustments to the {@link SwerveModuleConstants} for simulation - * purposes. These changes have no effect on real robot operations and address known simulation - * bugs: - * - *

    - *
  • Inverted Drive Motors: Prevents drive PID issues caused by inverted - * configurations. - *
  • Non-zero CanCoder Offsets: Fixes potential module state optimization - * issues. - *
  • Steer Motor PID: Adjusts PID values tuned for real robots to improve - * simulation performance. - *
- * - *

Note:This function is skipped when running on a real robot, ensuring no impact on constants - * used on real robot hardware.

- */ - private static void regulateModuleConstantForSimulation( - SwerveModuleConstants moduleConstants) { - // Skip regulation if running on a real robot - if (RobotBase.isReal()) return; - - // Apply simulation-specific adjustments to module constants - moduleConstants - // Disable encoder offsets - .withEncoderOffset(0) - // Disable motor inversions for drive and steer motors - .withDriveMotorInverted(false) - .withSteerMotorInverted(false) - // Disable CanCoder inversion - .withEncoderInverted(false) - // Adjust steer motor PID gains for simulation - .withSteerMotorGains(SimSwerveConstants.STEER_MOTOR_GAINS) - .withSteerMotorGearRatio(SimSwerveConstants.STEER_MOTOR_GEAR_RATIO) - // Adjust friction voltages - .withDriveFrictionVoltage(SimSwerveConstants.DRIVE_FRICTION_VOLTAGE) - .withSteerFrictionVoltage(SimSwerveConstants.STEER_FRICTION_VOLTAGE) - // Adjust steer inertia - .withSteerInertia(SimSwerveConstants.STEER_INERTIA); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/simulation/SimSwerveConstants.java b/src/main/java/frc/robot/utils/simulation/SimSwerveConstants.java deleted file mode 100644 index fb7af5c..0000000 --- a/src/main/java/frc/robot/utils/simulation/SimSwerveConstants.java +++ /dev/null @@ -1,34 +0,0 @@ -package frc.robot.utils.simulation; - -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.configs.Slot0Configs; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.Mass; -import edu.wpi.first.units.measure.MomentOfInertia; -import edu.wpi.first.units.measure.Voltage; -import frc.robot.generated.TunerConstants; - -public final class SimSwerveConstants { - // TODO: update constants to what's physically on the robot - public static final Mass ROBOT_MASS = Pounds.of(115); - public static final Distance BUMPER_LENGTH_X = Inches.of(30); - public static final Distance BUMPER_LENGTH_Y = Inches.of(30); - public static final double WHEEL_COF = 1.2; - public static final DCMotor DRIVE_MOTOR_WHEEL = DCMotor.getKrakenX60(1); - public static final DCMotor STEER_MOTOR_WHEEL = DCMotor.getKrakenX60(1); - - // Use values from generated TunerConstants where available to avoid duplicate - // definitions. TunerConstants is generated by the Tuner tool and must not be - // edited; prefer those values for module/drivetrain tuning parameters. - public static final Slot0Configs STEER_MOTOR_GAINS = TunerConstants.FrontLeft.SteerMotorGains; - public static final double STEER_MOTOR_GEAR_RATIO = - TunerConstants.FrontLeft.SteerMotorGearRatio; - public static final Voltage DRIVE_FRICTION_VOLTAGE = - Volts.of(TunerConstants.FrontLeft.DriveFrictionVoltage); - public static final Voltage STEER_FRICTION_VOLTAGE = - Volts.of(TunerConstants.FrontLeft.SteerFrictionVoltage); - public static final MomentOfInertia STEER_INERTIA = - KilogramSquareMeters.of(TunerConstants.FrontLeft.SteerInertia); - } \ No newline at end of file diff --git a/src/main/java/frc/util/DriveInputSmoother.java b/src/main/java/frc/util/DriveInputSmoother.java new file mode 100644 index 0000000..71065a6 --- /dev/null +++ b/src/main/java/frc/util/DriveInputSmoother.java @@ -0,0 +1,43 @@ +package frc.util; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.numbers.N2; + +public class DriveInputSmoother { + private static final double kJoystickDeadband = 0.15; + private static final double kCurveExponent = 1.5; + + private final DoubleSupplier forwardInput; + private final DoubleSupplier leftInput; + private final DoubleSupplier rotationInput; + + public DriveInputSmoother(DoubleSupplier forwardInput, DoubleSupplier leftInput, DoubleSupplier rotationInput) { + this.forwardInput = forwardInput; + this.leftInput = leftInput; + this.rotationInput = rotationInput; + } + + public DriveInputSmoother(DoubleSupplier forwardInput, DoubleSupplier leftInput) { + this(forwardInput, leftInput, () -> 0); + } + + public ManualDriveInput getSmoothedInput() { + final Vector rawTranslationInput = VecBuilder.fill(forwardInput.getAsDouble(), leftInput.getAsDouble()); + final Vector deadbandedTranslationInput = MathUtil.applyDeadband(rawTranslationInput, kJoystickDeadband); + final Vector curvedTranslationInput = MathUtil.copyDirectionPow(deadbandedTranslationInput, kCurveExponent); + + final double rawRotationInput = rotationInput.getAsDouble(); + final double deadbandedRotationInput = MathUtil.applyDeadband(rawRotationInput, kJoystickDeadband); + final double curvedRotationInput = MathUtil.copyDirectionPow(deadbandedRotationInput, kCurveExponent); + + return new ManualDriveInput( + curvedTranslationInput.get(0), + curvedTranslationInput.get(1), + curvedRotationInput + ); + } +} diff --git a/src/main/java/frc/util/GeometryUtil.java b/src/main/java/frc/util/GeometryUtil.java new file mode 100644 index 0000000..1fadcad --- /dev/null +++ b/src/main/java/frc/util/GeometryUtil.java @@ -0,0 +1,15 @@ +package frc.util; + +import static edu.wpi.first.units.Units.Radians; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; + +public final class GeometryUtil { + public static boolean isNear(Rotation2d expected, Rotation2d actual, Angle tolerance) { + final double expectedRadians = MathUtil.angleModulus(expected.getRadians()); + final double actualRadians = MathUtil.angleModulus(actual.getRadians()); + return MathUtil.isNear(expectedRadians, actualRadians, tolerance.in(Radians), -Math.PI, Math.PI); + } +} diff --git a/src/main/java/frc/util/ManualDriveInput.java b/src/main/java/frc/util/ManualDriveInput.java new file mode 100644 index 0000000..5be1bde --- /dev/null +++ b/src/main/java/frc/util/ManualDriveInput.java @@ -0,0 +1,25 @@ +package frc.util; + +public class ManualDriveInput { + public final double forward; + public final double left; + public final double rotation; + + public ManualDriveInput(double forward, double left, double rotation) { + this.forward = forward; + this.left = left; + this.rotation = rotation; + } + + public ManualDriveInput() { + this(0, 0, 0); + } + + public boolean hasTranslation() { + return Math.hypot(forward, left) > 0; + } + + public boolean hasRotation() { + return Math.abs(rotation) > 0; + } +} diff --git a/src/main/java/frc/util/Stopwatch.java b/src/main/java/frc/util/Stopwatch.java new file mode 100644 index 0000000..13e6717 --- /dev/null +++ b/src/main/java/frc/util/Stopwatch.java @@ -0,0 +1,35 @@ +package frc.util; + +import static edu.wpi.first.units.Units.Seconds; + +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj.Timer; + +public class Stopwatch { + private double startTimeInSeconds = Double.POSITIVE_INFINITY; + + public void start() { + startTimeInSeconds = Timer.getFPGATimestamp(); + } + + public void startIfNotRunning() { + if (Double.isInfinite(startTimeInSeconds)) { + start(); + } + } + + public void reset() { + startTimeInSeconds = Double.POSITIVE_INFINITY; + } + + public double elapsedSeconds() { + if (Double.isInfinite(startTimeInSeconds)) { + return 0.0; + } + return Timer.getFPGATimestamp() - startTimeInSeconds; + } + + public Time elapsedTime() { + return Seconds.of(elapsedSeconds()); + } +} diff --git a/src/main/java/frc/util/SwerveTelemetry.java b/src/main/java/frc/util/SwerveTelemetry.java new file mode 100644 index 0000000..0e1d750 --- /dev/null +++ b/src/main/java/frc/util/SwerveTelemetry.java @@ -0,0 +1,111 @@ +package frc.util; + +import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class SwerveTelemetry { + private final double MaxSpeed; + + /** + * Construct a telemetry object, with the specified max speed of the robot + * + * @param maxSpeed Maximum speed in meters per second + */ + public SwerveTelemetry(double maxSpeed) { + MaxSpeed = maxSpeed; + + /* Set up the module state Mechanism2d telemetry */ + for (int i = 0; i < 4; ++i) { + SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); + } + } + + /* What to publish over networktables for telemetry */ + private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); + + /* Robot swerve drive state */ + private final NetworkTable driveStateTable = inst.getTable("DriveState"); + private final StructPublisher drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); + private final StructPublisher driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); + private final StructArrayPublisher driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish(); + private final StructArrayPublisher driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish(); + private final StructArrayPublisher driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish(); + private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish(); + private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish(); + + /* Robot pose for field positioning */ + private final NetworkTable table = inst.getTable("Pose"); + private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); + private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); + + /* Mechanisms to represent the swerve module states */ + private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + }; + /* A direction and length changing ligament for speed representation */ + private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + }; + /* A direction changing and length constant ligament for module direction */ + private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + }; + + private final double[] m_poseArray = new double[3]; + + /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ + public void telemeterize(SwerveDriveState state) { + /* Telemeterize the swerve drive state */ + drivePose.set(state.Pose); + driveSpeeds.set(state.Speeds); + driveModuleStates.set(state.ModuleStates); + driveModuleTargets.set(state.ModuleTargets); + driveModulePositions.set(state.ModulePositions); + driveTimestamp.set(state.Timestamp); + driveOdometryFrequency.set(1.0 / state.OdometryPeriod); + + /* Telemeterize the pose to a Field2d */ + fieldTypePub.set("Field2d"); + + m_poseArray[0] = state.Pose.getX(); + m_poseArray[1] = state.Pose.getY(); + m_poseArray[2] = state.Pose.getRotation().getDegrees(); + fieldPub.set(m_poseArray); + + /* Telemeterize each module state to a Mechanism2d */ + for (int i = 0; i < 4; ++i) { + m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); + m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); + m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); + } + } +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json deleted file mode 100644 index bef4a15..0000000 --- a/vendordeps/AdvantageKit.json +++ /dev/null @@ -1,35 +0,0 @@ -{ - "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/ChoreoLib2026.json b/vendordeps/ChoreoLib2026.json new file mode 100644 index 0000000..322c9e2 --- /dev/null +++ b/vendordeps/ChoreoLib2026.json @@ -0,0 +1,44 @@ +{ + "fileName": "ChoreoLib2026.json", + "name": "ChoreoLib", + "version": "2026.0.1", + "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", + "frcYear": "2026", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/sleipnirgroup-mvn-release/", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://choreo.autos/lib/ChoreoLib2026.json", + "javaDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-java", + "version": "2026.0.1" + }, + { + "groupId": "com.google.code.gson", + "artifactId": "gson", + "version": "2.11.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-cpp", + "version": "2026.0.1", + "libName": "ChoreoLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json deleted file mode 100644 index c496379..0000000 --- a/vendordeps/PathplannerLib.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "fileName": "PathplannerLib.json", - "name": "PathplannerLib", - "version": "2025.2.7", - "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2025", - "mavenUrls": [ - "https://3015rangerrobotics.github.io/pathplannerlib/repo" - ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", - "javaDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-java", - "version": "2025.2.7" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-cpp", - "version": "2025.2.7", - "libName": "PathplannerLib", - "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/Phoenix5-frc2025-latest.json b/vendordeps/Phoenix5-frc2025-latest.json deleted file mode 100644 index 964ceca..0000000 --- a/vendordeps/Phoenix5-frc2025-latest.json +++ /dev/null @@ -1,171 +0,0 @@ -{ - "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-frc2026-latest.json similarity index 85% rename from vendordeps/Phoenix6-frc2025-latest.json rename to vendordeps/Phoenix6-frc2026-latest.json index 6f40c84..8f6e30f 100644 --- a/vendordeps/Phoenix6-frc2025-latest.json +++ b/vendordeps/Phoenix6-frc2026-latest.json @@ -1,32 +1,32 @@ { - "fileName": "Phoenix6-frc2025-latest.json", + "fileName": "Phoenix6-frc2026-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.4.0", - "frcYear": "2025", + "version": "26.1.0", + "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-latest.json", "conflictsWith": [ { "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", - "offlineFileName": "Phoenix6-replay-frc2025-latest.json" + "offlineFileName": "Phoenix6-replay-frc2026-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.4.0" + "version": "26.1.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.4.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.4.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.4.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.4.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.4.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.4.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,21 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.4.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.4.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.4.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.4.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.4.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.4.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "25.4.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -222,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "25.4.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -238,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.4.0", + "version": "26.1.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -254,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.4.0", + "version": "26.1.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -270,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.4.0", + "version": "26.1.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -286,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.4.0", + "version": "26.1.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -302,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.4.0", + "version": "26.1.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -318,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.4.0", + "version": "26.1.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -334,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.4.0", + "version": "26.1.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -347,26 +333,10 @@ ], "simMode": "swsim" }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.4.0", - "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.4.0", + "version": "26.1.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -382,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.4.0", + "version": "26.1.0", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -398,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.4.0", + "version": "26.1.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -414,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.4.0", + "version": "26.1.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -430,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.4.0", + "version": "26.1.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -446,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "25.4.0", + "version": "26.1.0", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -462,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "25.4.0", + "version": "26.1.0", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index 3718e0a..d90630e 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -3,7 +3,7 @@ "name": "WPILib-New-Commands", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "frcYear": "2025", + "frcYear": "2026", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [ @@ -25,6 +25,7 @@ "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ + "linuxsystemcore", "linuxathena", "linuxarm32", "linuxarm64", diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json deleted file mode 100644 index dd92223..0000000 --- a/vendordeps/maple-sim.json +++ /dev/null @@ -1,26 +0,0 @@ -{ - "fileName": "maple-sim.json", - "name": "maplesim", - "version": "0.3.8", - "frcYear": "2025", - "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", - "mavenUrls": [ - "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/repos/releases", - "https://repo1.maven.org/maven2" - ], - "jsonUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json", - "javaDependencies": [ - { - "groupId": "org.ironmaple", - "artifactId": "maplesim-java", - "version": "0.3.8" - }, - { - "groupId": "org.dyn4j", - "artifactId": "dyn4j", - "version": "5.0.2" - } - ], - "jniDependencies": [], - "cppDependencies": [] -} \ No newline at end of file