--- /dev/null
+*.gradle text eol=lf
+*.java text eol=lf
+*.md text eol=lf
+*.xml text eol=lf
\ No newline at end of file
--- /dev/null
+# Make the pull reviewers code owners for every file, they will get requested for review when a PR is made
+* @MaxwellTTan20 @Arnav814 @moogoesmeow0 @iefomit @Wesley28W @EileenHan-972
--- /dev/null
+## Overview
+<!-- A general very short overview of the PR -->
+This PR will...
+
+
+## Documentation
+<!-- Link your approved design doc or your project documentation. We will be checking that the code fits everything in the linked file. If the PR is only meant to address part of it, note here what is not yet implemented -->
+
+
+## Tests Ran
+<!-- At a minimum, make sure the code builds and that the code works in the sim. -->
+
+
+## Additional Notes
+<!-- Anything else that will help the reviewer understand your PR -->
\ No newline at end of file
--- /dev/null
+# This is a basic workflow to build robot code. credit: WPI
+
+name: CI
+
+# Controls when the action will run. Triggers the workflow on push or pull request
+# events but only for the main branch.
+on:
+ push:
+ branches: [ main ]
+ pull_request:
+ branches: [ main ]
+
+# A workflow run is made up of one or more jobs that can run sequentially or in parallel
+jobs:
+ # This workflow contains a single job called "build"
+ build:
+ # The type of runner that the job will run on
+ runs-on: ubuntu-latest
+
+ # This grabs the WPILib docker container
+ container: wpilib/roborio-cross-ubuntu:2023-22.04
+
+ # Steps represent a sequence of tasks that will be executed as part of the job
+ steps:
+ # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it
+ - uses: actions/checkout@v3
+
+ # Declares the repository safe and not under dubious ownership.
+ - name: Add repository to git safe directories
+ run: git config --global --add safe.directory $GITHUB_WORKSPACE
+
+ # Grant execute permission for gradlew
+ - name: Grant execute permission for gradlew
+ run: chmod +x gradlew
+
+ # Call gversion to make the version file
+ - name: Create BuildData.java
+ run: ./gradlew createVersionFile
+
+ # Runs a single command using the runners shell
+ - name: Compile and run tests on robot code
+ run: ./gradlew build
--- /dev/null
+# 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
+networktables.json.bck
+simgui.json
+simgui-ds.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
+
+# File that has git and version data
+src/main/java/frc/robot/util/BuildData.java
+
--- /dev/null
+{
+ "robotWidth": 0.76,
+ "robotLength": 0.91,
+ "holonomicMode": true,
+ "pathFolders": [
+ "Final Stuff",
+ "Stuff"
+ ],
+ "autoFolders": [],
+ "defaultMaxVel": 3.0,
+ "defaultMaxAccel": 3.0,
+ "defaultMaxAngVel": 540.0,
+ "defaultMaxAngAccel": 720.0,
+ "maxModuleSpeed": 4.5
+}
\ No newline at end of file
--- /dev/null
+{
+ // 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,
+ }
+ ]
+}
--- /dev/null
+{
+ "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
+}
--- /dev/null
+{
+ "enableCppIntellisense": false,
+ "currentLanguage": "java",
+ "projectYear": "2026",
+ "teamNumber": 972
+}
\ No newline at end of file
--- /dev/null
+# FRC2026
+
+[](https://github.com/iron-claw-972/FRC2026/actions/workflows/main.yml)
+
+Code for the 2025-2026 FRC Season.
--- /dev/null
+Copyright (c) 2009-2026 FIRST and other WPILib contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+ * Neither the name of FIRST, WPILib, nor the names of other WPILib
+ contributors may be used to endorse or promote products derived from
+ this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
--- /dev/null
+plugins {
+ id "java"
+ id "edu.wpi.first.GradleRIO" version "2026.2.1"
+ id "com.peterabeles.gversion" version "1.10.3"
+}
+
+java {
+ sourceCompatibility = JavaVersion.VERSION_17
+ targetCompatibility = JavaVersion.VERSION_17
+}
+
+def ROBOT_MAIN_CLASS = "frc.robot.Main"
+
+// Define my targets (RoboRIO) and artifacts (deployable files)
+// This is added by GradleRIO's backing project DeployUtils.
+deploy {
+ targets {
+ roborio(getTargetTypeClass('RoboRIO')) {
+ // Team number is loaded either from the .wpilib/wpilib_preferences.json
+ // or from command line. If not found an exception will be thrown.
+ // You can use getTeamOrDefault(team) instead of getTeamNumber if you
+ // want to store a team number in this file.
+ team = project.frc.getTeamNumber()
+ debug = project.frc.getDebugOrDefault(false)
+
+ artifacts {
+ // First part is artifact name, 2nd is artifact type
+ // getTargetTypeClass is a shortcut to get the class type using a string
+
+ frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
+ jvmArgs.add("-Dcom.sun.management.jmxremote=true")
+ jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198")
+ jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false")
+ jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false")
+ jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false")
+ jvmArgs.add("-XX:+HeapDumpOnOutOfMemoryError")
+ jvmArgs.add("-XX:HeapDumpPath=/home/lvuser/frc-usercode.hprof")
+ jvmArgs.add("-Djava.rmi.server.hostname=10.9.72.2")
+ }
+
+ // Static files artifact
+ frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+ files = project.fileTree('src/main/deploy')
+ directory = '/home/lvuser/deploy'
+ deleteOldFiles = false // Change to true to delete files on roboRIO that no
+ // longer exist in deploy directory of this project
+ }
+ }
+ }
+ }
+}
+
+def deployArtifact = deploy.targets.roborio.artifacts.frcJava
+
+// 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.
+def includeDesktopSupport = false
+
+task(replayWatch, type: JavaExec) {
+ mainClass = "org.littletonrobotics.junction.ReplayWatch"
+ classpath = sourceSets.main.runtimeClasspath
+}
+
+// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+// Also defines JUnit 5.
+dependencies {
+ annotationProcessor wpi.java.deps.wpilibAnnotations()
+ implementation wpi.java.deps.wpilib()
+ implementation wpi.java.vendor.java()
+
+ roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
+ roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
+
+ roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
+ roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)
+
+ nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
+ nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
+ simulationDebug wpi.sim.enableDebug()
+
+ nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop)
+ nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
+ simulationRelease wpi.sim.enableRelease()
+
+ implementation "gov.nist.math:jama:1.0.3"
+
+ testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
+ testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
+ def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
+ annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
+}
+
+test {
+ useJUnitPlatform()
+ systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
+}
+
+// Simulation configuration (e.g. environment variables).
+wpi.sim.addGui().defaultEnabled = true
+wpi.sim.addDriverstation()
+
+// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
+// in order to make them all available at runtime. Also adding the manifest so WPILib
+// knows where to look for our Robot Class.
+jar {
+ from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
+ from('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
+}
+
+project.compileJava.dependsOn(createVersionFile)
+gversion {
+ srcDir = file("src/main/java")
+ classPackage = "frc.robot.util"
+ className = "BuildData" // This will generate BuildData.java
+}
+
+
+// Configure jar and deploy tasks
+deployArtifact.jarTask = jar
+wpi.java.configureExecutableTasks(jar)
+wpi.java.configureTestTasks(test)
+
+// Configure string concat to always inline compile
+tasks.withType(JavaCompile) {
+ options.encoding = 'UTF-8' // prevent breaks in CI
+ options.compilerArgs.add '-XDstringConcat=inline'
+}
--- /dev/null
+distributionBase=GRADLE_USER_HOME
+distributionPath=permwrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip
+networkTimeout=10000
+validateDistributionUrl=true
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=permwrapper/dists
--- /dev/null
+#!/bin/sh
+
+#
+# Copyright © 2015-2021 the original authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# https://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# SPDX-License-Identifier: Apache-2.0
+#
+
+##############################################################################
+#
+# Gradle start up script for POSIX generated by Gradle.
+#
+# Important for running:
+#
+# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is
+# noncompliant, but you have some other compliant shell such as ksh or
+# bash, then to run this script, type that shell name before the whole
+# command line, like:
+#
+# ksh Gradle
+#
+# Busybox and similar reduced shells will NOT work, because this script
+# requires all of these POSIX shell features:
+# * functions;
+# * expansions «$var», «${var}», «${var:-default}», «${var+SET}»,
+# «${var#prefix}», «${var%suffix}», and «$( cmd )»;
+# * compound commands having a testable exit status, especially «case»;
+# * various built-in commands including «command», «set», and «ulimit».
+#
+# Important for patching:
+#
+# (2) This script targets any POSIX shell, so it avoids extensions provided
+# by Bash, Ksh, etc; in particular arrays are avoided.
+#
+# The "traditional" practice of packing multiple parameters into a
+# space-separated string is a well documented source of bugs and security
+# problems, so this is (mostly) avoided, by progressively accumulating
+# options in "$@", and eventually passing that to Java.
+#
+# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS,
+# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly;
+# see the in-line comments for details.
+#
+# There are tweaks for specific operating systems such as AIX, CygWin,
+# Darwin, MinGW, and NonStop.
+#
+# (3) This script is generated from the Groovy template
+# https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+# within the Gradle project.
+#
+# You can find Gradle at https://github.com/gradle/gradle/.
+#
+##############################################################################
+
+# Attempt to set APP_HOME
+
+# Resolve links: $0 may be a link
+app_path=$0
+
+# Need this for daisy-chained symlinks.
+while
+ APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path
+ [ -h "$app_path" ]
+do
+ ls=$( ls -ld "$app_path" )
+ link=${ls#*' -> '}
+ case $link in #(
+ /*) app_path=$link ;; #(
+ *) app_path=$APP_HOME$link ;;
+ esac
+done
+
+# This is normally unused
+# shellcheck disable=SC2034
+APP_BASE_NAME=${0##*/}
+# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036)
+APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s
+' "$PWD" ) || exit
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD=maximum
+
+warn () {
+ echo "$*"
+} >&2
+
+die () {
+ echo
+ echo "$*"
+ echo
+ exit 1
+} >&2
+
+# OS specific support (must be 'true' or 'false').
+cygwin=false
+msys=false
+darwin=false
+nonstop=false
+case "$( uname )" in #(
+ CYGWIN* ) cygwin=true ;; #(
+ Darwin* ) darwin=true ;; #(
+ MSYS* | MINGW* ) msys=true ;; #(
+ NONSTOP* ) nonstop=true ;;
+esac
+
+CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
+
+
+# Determine the Java command to use to start the JVM.
+if [ -n "$JAVA_HOME" ] ; then
+ if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
+ # IBM's JDK on AIX uses strange locations for the executables
+ JAVACMD=$JAVA_HOME/jre/sh/java
+ else
+ JAVACMD=$JAVA_HOME/bin/java
+ fi
+ if [ ! -x "$JAVACMD" ] ; then
+ die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+ fi
+else
+ JAVACMD=java
+ if ! command -v java >/dev/null 2>&1
+ then
+ die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+ fi
+fi
+
+# Increase the maximum file descriptors if we can.
+if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
+ case $MAX_FD in #(
+ max*)
+ # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked.
+ # shellcheck disable=SC2039,SC3045
+ MAX_FD=$( ulimit -H -n ) ||
+ warn "Could not query maximum file descriptor limit"
+ esac
+ case $MAX_FD in #(
+ '' | soft) :;; #(
+ *)
+ # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked.
+ # shellcheck disable=SC2039,SC3045
+ ulimit -n "$MAX_FD" ||
+ warn "Could not set maximum file descriptor limit to $MAX_FD"
+ esac
+fi
+
+# Collect all arguments for the java command, stacking in reverse order:
+# * args from the command line
+# * the main class name
+# * -classpath
+# * -D...appname settings
+# * --module-path (only if needed)
+# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables.
+
+# For Cygwin or MSYS, switch paths to Windows format before running java
+if "$cygwin" || "$msys" ; then
+ APP_HOME=$( cygpath --path --mixed "$APP_HOME" )
+ CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" )
+
+ JAVACMD=$( cygpath --unix "$JAVACMD" )
+
+ # Now convert the arguments - kludge to limit ourselves to /bin/sh
+ for arg do
+ if
+ case $arg in #(
+ -*) false ;; # don't mess with options #(
+ /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath
+ [ -e "$t" ] ;; #(
+ *) false ;;
+ esac
+ then
+ arg=$( cygpath --path --ignore --mixed "$arg" )
+ fi
+ # Roll the args list around exactly as many times as the number of
+ # args, so each arg winds up back in the position where it started, but
+ # possibly modified.
+ #
+ # NB: a `for` loop captures its iteration list before it begins, so
+ # changing the positional parameters here affects neither the number of
+ # iterations, nor the values presented in `arg`.
+ shift # remove old arg
+ set -- "$@" "$arg" # push replacement arg
+ done
+fi
+
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+
+# Collect all arguments for the java command:
+# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments,
+# and any embedded shellness will be escaped.
+# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be
+# treated as '${Hostname}' itself on the command line.
+
+set -- \
+ "-Dorg.gradle.appname=$APP_BASE_NAME" \
+ -classpath "$CLASSPATH" \
+ org.gradle.wrapper.GradleWrapperMain \
+ "$@"
+
+# Stop when "xargs" is not available.
+if ! command -v xargs >/dev/null 2>&1
+then
+ die "xargs is not available"
+fi
+
+# Use "xargs" to parse quoted args.
+#
+# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
+#
+# In Bash we could simply go:
+#
+# readarray ARGS < <( xargs -n1 <<<"$var" ) &&
+# set -- "${ARGS[@]}" "$@"
+#
+# but POSIX shell has neither arrays nor command substitution, so instead we
+# post-process each arg (as a line of input to sed) to backslash-escape any
+# character that might be a shell metacharacter, then use eval to reverse
+# that process (while maintaining the separation between arguments), and wrap
+# the whole thing up as a single "set" statement.
+#
+# This will of course break if any of these variables contains a newline or
+# an unmatched quote.
+#
+
+eval "set -- $(
+ printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
+ xargs -n1 |
+ sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
+ tr '\n' ' '
+ )" '"$@"'
+
+exec "$JAVACMD" "$@"
--- /dev/null
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+@rem SPDX-License-Identifier: Apache-2.0
+@rem
+
+@if "%DEBUG%"=="" @echo off
+@rem ##########################################################################
+@rem
+@rem Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%"=="" set DIRNAME=.
+@rem This is normally unused
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Resolve any "." and ".." in APP_HOME to make it shorter.
+for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if %ERRORLEVEL% equ 0 goto execute
+
+echo. 1>&2
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto execute
+
+echo. 1>&2
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
+
+goto fail
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+
+:end
+@rem End local scope for the variables with windows NT shell
+if %ERRORLEVEL% equ 0 goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+set EXIT_CODE=%ERRORLEVEL%
+if %EXIT_CODE% equ 0 set EXIT_CODE=1
+if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
+exit /b %EXIT_CODE%
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega
--- /dev/null
+import org.gradle.internal.os.OperatingSystem
+
+pluginManagement {
+ repositories {
+ mavenLocal()
+ gradlePluginPortal()
+ String frcYear = '2026'
+ File frcHome
+ if (OperatingSystem.current().isWindows()) {
+ String publicFolder = System.getenv('PUBLIC')
+ if (publicFolder == null) {
+ publicFolder = "C:\\Users\\Public"
+ }
+ def homeRoot = new File(publicFolder, "wpilib")
+ frcHome = new File(homeRoot, frcYear)
+ } else {
+ def userFolder = System.getProperty("user.home")
+ def homeRoot = new File(userFolder, "wpilib")
+ frcHome = new File(homeRoot, frcYear)
+ }
+ def frcHomeMaven = new File(frcHome, 'maven')
+ maven {
+ name = 'frcHome'
+ url = frcHomeMaven
+ }
+ }
+}
+
+Properties props = System.getProperties();
+props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true");
--- /dev/null
+{
+ "name":"conservative",
+ "version":3,
+ "snapshot":{
+ "waypoints":[
+ {"x":4.420742511749268, "y":7.652679920196533, "heading":-1.5707963267948966, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":7.575807094573975, "y":6.538276672363281, "heading":-1.2256388132346097, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":6.706699371337891, "y":5.0463480949401855, "heading":2.63742627823249, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":6.0891547203063965, "y":5.935906410217285, "heading":0.0, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":5.89302921295166, "y":7.210275173187256, "heading":0.4241938953287057, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":4.036591053009033, "y":7.649001598358154, "heading":0.0, "intervals":18, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":5.986230850219727, "y":6.575506210327148, "heading":-1.453686475392422, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":5.831844806671143, "y":4.693465709686279, "heading":-1.2575197731729428, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":7.272781848907471, "y":4.93607234954834, "heading":1.1659049132718895, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":6.655237197875977, "y":6.612264633178711, "heading":1.2811046784461615, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":5.787734508514404, "y":7.46506404876709, "heading":0.0, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":4.036591053009033, "y":7.649001598358154, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepOutCircle", "props":{"x":4.594947936013341, "y":6.01006807340309, "r":1.0}}, "enabled":true},
+ {"from":5, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}],
+ "targetDt":0.05
+ },
+ "params":{
+ "waypoints":[
+ {"x":{"exp":"4.420742511749268 m", "val":4.420742511749268}, "y":{"exp":"7.652679920196533 m", "val":7.652679920196533}, "heading":{"exp":"-90 deg", "val":-1.5707963267948966}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"7.575807094573975 m", "val":7.575807094573975}, "y":{"exp":"6.538276672363281 m", "val":6.538276672363281}, "heading":{"exp":"-1.2256388132346097 rad", "val":-1.2256388132346097}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"6.706699371337891 m", "val":6.706699371337891}, "y":{"exp":"5.0463480949401855 m", "val":5.0463480949401855}, "heading":{"exp":"2.63742627823249 rad", "val":2.63742627823249}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"6.0891547203063965 m", "val":6.0891547203063965}, "y":{"exp":"5.935906410217285 m", "val":5.935906410217285}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":{"exp":"5.89302921295166 m", "val":5.89302921295166}, "y":{"exp":"7.210275173187256 m", "val":7.210275173187256}, "heading":{"exp":"0.4241938953287057 rad", "val":0.4241938953287057}, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"shootPos.x", "val":4.036591053009033}, "y":{"exp":"shootPos.y", "val":7.649001598358154}, "heading":{"exp":"shootPos.heading", "val":0.0}, "intervals":18, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"5.986230850219727 m", "val":5.986230850219727}, "y":{"exp":"6.575506210327148 m", "val":6.575506210327148}, "heading":{"exp":"-1.4536864753924221 rad", "val":-1.453686475392422}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"5.831844806671143 m", "val":5.831844806671143}, "y":{"exp":"4.693465709686279 m", "val":4.693465709686279}, "heading":{"exp":"-1.2575197731729428 rad", "val":-1.2575197731729428}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"7.272781848907471 m", "val":7.272781848907471}, "y":{"exp":"4.93607234954834 m", "val":4.93607234954834}, "heading":{"exp":"1.1659049132718895 rad", "val":1.1659049132718895}, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"6.655237197875977 m", "val":6.655237197875977}, "y":{"exp":"6.612264633178711 m", "val":6.612264633178711}, "heading":{"exp":"1.2811046784461615 rad", "val":1.2811046784461615}, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"5.787734508514404 m", "val":5.787734508514404}, "y":{"exp":"7.46506404876709 m", "val":7.46506404876709}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"shootPos.x", "val":4.036591053009033}, "y":{"exp":"shootPos.y", "val":7.649001598358154}, "heading":{"exp":"shootPos.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"4.594947936013341 m", "val":4.594947936013341}, "y":{"exp":"6.01006807340309 m", "val":6.01006807340309}, "r":{"exp":"1 m", "val":1.0}}}, "enabled":true},
+ {"from":5, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}],
+ "targetDt":{
+ "exp":"0.05 s",
+ "val":0.05
+ }
+ },
+ "trajectory":{
+ "config":{
+ "frontLeft":{
+ "x":0.52705,
+ "y":0.52705
+ },
+ "backLeft":{
+ "x":-0.52705,
+ "y":0.52705
+ },
+ "mass":56.69904625,
+ "inertia":7.0,
+ "gearing":7.03125,
+ "radius":0.0508,
+ "vmax":628.3185307179587,
+ "tmax":7.0,
+ "cof":1.0,
+ "bumper":{
+ "front":0.4064,
+ "side":0.4064,
+ "back":0.4064
+ },
+ "differentialTrackWidth":0.5588
+ },
+ "sampleType":"Swerve",
+ "waypoints":[0.0,1.03442,1.8534,2.23085,2.71973,3.42961,4.32037,4.83861,5.63843,6.09785,6.57231,7.21461],
+ "samples":[
+ {"t":0.0, "x":4.42074, "y":7.65268, "heading":-1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.48053, "ay":-2.02916, "alpha":5.91517, "fx":[138.53336,138.2864,128.19559,132.52178], "fy":[-8.81991,-11.71133,-53.1868,-41.33342]},
+ {"t":0.05444, "x":4.43479, "y":7.64967, "heading":-1.56203, "vx":0.51615, "vy":-0.11047, "omega":0.32204, "ax":9.51121, "ay":-1.88337, "alpha":5.85892, "fx":[138.62352,138.4277,129.10631,133.11884], "fy":[-6.96663,-9.61845,-50.8846,-39.31555]},
+ {"t":0.10889, "x":4.47699, "y":7.64087, "heading":-1.53581, "vx":1.03397, "vy":-0.21301, "omega":0.64102, "ax":9.54914, "ay":-1.69384, "alpha":5.73346, "fx":[138.70524,138.53569,130.23845,133.94788], "fy":[-4.46638,-7.41986,-47.83949,-36.31327]},
+ {"t":0.16333, "x":4.54744, "y":7.62676, "heading":-1.49242, "vx":1.55386, "vy":-0.30523, "omega":0.95317, "ax":9.59794, "ay":-1.43293, "alpha":5.45617, "fx":[138.74009,138.60924,131.79661,135.0483], "fy":[-1.25067,-4.89708,-43.24853,-31.8492]},
+ {"t":0.21777, "x":4.64626, "y":7.60802, "heading":-1.43244, "vx":2.0764, "vy":-0.38324, "omega":1.25022, "ax":9.66195, "ay":-1.05334, "alpha":4.79811, "fx":[138.67331,138.62562,134.08215,136.44204], "fy":[2.4587,-1.90831,-35.32327,-24.95052]},
+ {"t":0.27222, "x":4.77362, "y":7.58559, "heading":-1.35726, "vx":2.60243, "vy":-0.44059, "omega":1.51145, "ax":9.74416, "ay":-0.66362, "alpha":1.89176, "fx":[138.58005,138.47701,137.48095,137.94646], "fy":[-1.91493,-4.89923,-17.36318,-13.44922]},
+ {"t":0.32666, "x":4.92975, "y":7.56062, "heading":-1.27217, "vx":3.13294, "vy":-0.47672, "omega":1.61444, "ax":9.57131, "ay":-0.39285, "alpha":-8.12014, "fx":[131.68292,137.21475,136.39732,137.38918], "fy":[-42.36284,-18.70673,23.7177,15.07758]},
+ {"t":0.3811, "x":5.1145, "y":7.53408, "heading":-1.19631, "vx":3.65403, "vy":-0.49811, "omega":1.17235, "ax":9.26692, "ay":-0.25978, "alpha":-12.76773, "fx":[122.53051,136.6199,132.18695,134.08829], "fy":[-62.9145,-20.37529,39.67967,28.88089]},
+ {"t":0.43555, "x":5.32717, "y":7.50658, "heading":-1.1514, "vx":4.15855, "vy":-0.51225, "omega":0.47723, "ax":6.66408, "ay":2.72427, "alpha":-8.73854, "fx":[98.28995,107.84362,93.05919,78.65407], "fy":[11.88119,24.27284,59.25367,59.05584]},
+ {"t":0.48999, "x":5.56346, "y":7.48273, "heading":-1.13837, "vx":4.52137, "vy":-0.36393, "omega":0.00148, "ax":0.03162, "ay":0.38679, "alpha":-0.00125, "fx":[0.44724,0.45102,0.44927,0.4455], "fy":[5.47983,5.48157,5.48534,5.4836]},
+ {"t":0.54443, "x":5.80966, "y":7.46349, "heading":-1.13829, "vx":4.52309, "vy":-0.34287, "omega":0.00141, "ax":-0.30361, "ay":-3.18756, "alpha":0.00013, "fx":[-4.30346,-4.3039,-4.30368,-4.30324], "fy":[-45.18262,-45.18278,-45.18314,-45.18298]},
+ {"t":0.59888, "x":6.05546, "y":7.4401, "heading":-1.13822, "vx":4.50656, "vy":-0.51642, "omega":0.00142, "ax":-1.42597, "ay":-8.50782, "alpha":0.001, "fx":[-20.212,-20.21687,-20.2137,-20.20883], "fy":[-120.59593,-120.59554,-120.59669,-120.59708]},
+ {"t":0.65332, "x":6.2987, "y":7.39937, "heading":-1.13814, "vx":4.42893, "vy":-0.97961, "omega":0.00147, "ax":-2.57925, "ay":-9.13807, "alpha":0.00287, "fx":[-36.55911,-36.57272,-36.56128,-36.54767], "fy":[-129.52985,-129.5264,-129.53009,-129.53354]},
+ {"t":0.70776, "x":6.536, "y":7.3325, "heading":-1.13805, "vx":4.2885, "vy":-1.47712, "omega":0.00163, "ax":-3.70426, "ay":-8.92982, "alpha":0.03417, "fx":[-52.51215,-52.65314,-52.50194,-52.36086], "fy":[-126.57369,-126.51753,-126.58258,-126.63859]},
+ {"t":0.76221, "x":6.764, "y":7.23884, "heading":-1.13791, "vx":4.08683, "vy":-1.96329, "omega":0.00349, "ax":-6.41126, "ay":-7.28892, "alpha":2.80284, "fx":[-94.82707,-99.58932,-87.84128,-81.25466], "fy":[-100.00414,-95.45399,-106.42287,-111.39378]},
+ {"t":0.81665, "x":6.97699, "y":7.12115, "heading":-1.13357, "vx":3.73778, "vy":-2.36012, "omega":0.15609, "ax":-8.82853, "ay":-4.15748, "alpha":0.99594, "fx":[-126.56074,-126.52894,-123.79081,-123.68897], "fy":[-55.88487,-56.02127,-61.83698,-61.98227]},
+ {"t":0.87109, "x":7.16741, "y":6.9865, "heading":-1.12359, "vx":3.25713, "vy":-2.58647, "omega":0.21031, "ax":-9.30993, "ay":-2.34292, "alpha":-7.98837, "fx":[-125.80281,-126.04857,-138.51964,-137.49312], "fy":[-58.20384,-57.33792,0.41557,-17.71536]},
+ {"t":0.92554, "x":7.33094, "y":6.84221, "heading":-1.12398, "vx":2.75026, "vy":-2.71402, "omega":-0.2246, "ax":-9.24245, "ay":-1.52349, "alpha":-12.35983, "fx":[-125.41529,-125.52717,-134.3895,-138.70612], "fy":[-59.25922,-58.63036,34.02463,-2.51552]},
+ {"t":0.97998, "x":7.46697, "y":6.69219, "heading":-1.15453, "vx":2.24707, "vy":-2.79697, "omega":-0.89752, "ax":-9.11247, "ay":-1.1067, "alpha":-15.00976, "fx":[-125.58396,-123.92891,-128.51602,-138.63947], "fy":[-59.02017,-62.05712,52.1494,6.17882]},
+ {"t":1.03442, "x":7.57581, "y":6.53828, "heading":-1.22564, "vx":1.75096, "vy":-2.85722, "omega":-1.7147, "ax":-9.56052, "ay":-0.35558, "alpha":-8.8513, "fx":[-134.05895,-135.46823,-134.16182,-138.38352], "fy":[-35.61997,-29.29982,35.00307,9.7555]},
+ {"t":1.07753, "x":7.6424, "y":6.41479, "heading":-1.30777, "vx":1.33886, "vy":-2.87255, "omega":-2.09622, "ax":-9.68094, "ay":0.1557, "alpha":-5.93522, "fx":[-137.30197,-137.89806,-135.69244,-138.00736], "fy":[-19.34183,-13.66028,28.267,13.5631]},
+ {"t":1.12063, "x":7.69112, "y":6.29111, "heading":-1.40364, "vx":0.92157, "vy":-2.86584, "omega":-2.35206, "ax":-9.6916, "ay":0.58599, "alpha":-4.85567, "fx":[-138.291,-138.39896,-135.49001,-137.32435], "fy":[-9.05401,-5.33353,28.84925,18.76316]},
+ {"t":1.16374, "x":7.72184, "y":6.16813, "heading":-1.50954, "vx":0.50383, "vy":-2.84058, "omega":-2.56135, "ax":-9.67252, "ay":1.04466, "alpha":-3.68386, "fx":[-138.45897,-138.33164,-135.20071,-136.43124], "fy":[1.92937,3.93874,29.61221,23.75069]},
+ {"t":1.20684, "x":7.73457, "y":6.04666, "heading":-1.62336, "vx":0.0869, "vy":-2.79555, "omega":-2.72014, "ax":-9.61963, "ay":1.49921, "alpha":-2.38316, "fx":[-137.63123,-137.48307,-134.86025,-135.44937], "fy":[13.18138,13.88646,30.16167,27.77442]},
+ {"t":1.24994, "x":7.72938, "y":5.92755, "heading":-1.74283, "vx":-0.32774, "vy":-2.73093, "omega":-2.82287, "ax":-9.54315, "ay":1.82541, "alpha":-0.8046, "fx":[-135.78994,-135.74806,-134.7332,-134.81628], "fy":[23.28352,23.26421,28.56473,28.38653]},
+ {"t":1.29305, "x":7.70638, "y":5.81154, "heading":-1.86525, "vx":-0.73909, "vy":-2.65224, "omega":-2.85755, "ax":-9.50151, "ay":1.19294, "alpha":2.26583, "fx":[-133.75002,-133.76171,-135.64409,-135.57073], "fy":[23.22008,24.96511,11.33891,8.11443]},
+ {"t":1.33615, "x":7.6657, "y":5.69832, "heading":-1.98632, "vx":-1.14864, "vy":-2.60082, "omega":-2.75988, "ax":-9.13213, "ay":-0.75175, "alpha":6.98869, "fx":[-129.83815,-131.32976,-131.86169,-124.75363], "fy":[0.51536,16.83365,-18.26723,-41.70509]},
+ {"t":1.37926, "x":7.60771, "y":5.58552, "heading":-2.09879, "vx":-1.54228, "vy":-2.63323, "omega":-2.45864, "ax":-8.74632, "ay":-0.25527, "alpha":7.40997, "fx":[-122.23226,-124.63945,-128.27267,-120.7638], "fy":[5.62135,25.25423,-9.74959,-35.59976]},
+ {"t":1.42236, "x":7.5331, "y":5.47178, "heading":-2.19788, "vx":-1.91928, "vy":-2.64423, "omega":-2.13924, "ax":-6.2456, "ay":3.28214, "alpha":2.02145, "fx":[-84.82726,-87.13325,-92.08618,-90.0726], "fy":[49.64812,52.1135,43.7057,40.62715]},
+ {"t":1.46546, "x":7.44457, "y":5.36085, "heading":-2.28821, "vx":-2.18849, "vy":-2.50276, "omega":-2.05211, "ax":-1.55787, "ay":8.59081, "alpha":-7.67078, "fx":[-52.85099,-26.44442,10.91136,-19.94553], "fy":[115.29834,118.95069,125.71358,127.12794]},
+ {"t":1.50857, "x":7.34879, "y":5.26095, "heading":-2.38379, "vx":-2.25564, "vy":-2.13246, "omega":-2.38275, "ax":-0.56136, "ay":9.2288, "alpha":-7.98251, "fx":[-42.6526,-12.33736,28.69712,-5.53586], "fy":[126.95295,131.0845,130.58404,134.64288]},
+ {"t":1.55167, "x":7.25104, "y":5.17761, "heading":-2.49391, "vx":-2.27984, "vy":-1.73466, "omega":-2.72683, "ax":2.35019, "ay":8.98206, "alpha":-8.88638, "fx":[-7.48379,40.2167,69.9721,30.54856], "fy":[134.63957,126.78342,115.57303,132.27839]},
+ {"t":1.59478, "x":7.15496, "y":5.11118, "heading":-2.61971, "vx":-2.17853, "vy":-1.3475, "omega":-3.10987, "ax":4.56585, "ay":8.12232, "alpha":-9.06929, "fx":[26.01512,77.61695,95.82549,59.42197], "fy":[132.74575,109.10122,96.13881,122.54228]},
+ {"t":1.63788, "x":7.06529, "y":5.06064, "heading":-2.76218, "vx":-1.98173, "vy":-0.99739, "omega":-3.50079, "ax":5.07317, "ay":7.86311, "alpha":-8.8489, "fx":[35.10749,80.74735,102.48257,69.30666], "fy":[131.17337,107.53369,89.50326,117.6206]},
+ {"t":1.68098, "x":6.98459, "y":5.02496, "heading":-2.9213, "vx":-1.76305, "vy":-0.65846, "omega":-3.88221, "ax":4.02334, "ay":8.52896, "alpha":-7.83653, "fx":[25.17923,52.07561,89.18914,61.67555], "fy":[133.97538,124.70233,102.87639,122.03009]},
+ {"t":1.72409, "x":6.91233, "y":5.0045, "heading":-3.09592, "vx":-1.58963, "vy":-0.29083, "omega":-4.22, "ax":1.33921, "ay":9.50833, "alpha":-3.91086, "fx":[5.42662,7.81418,34.60299,28.08796], "fy":[136.76532,136.23253,132.09723,134.01812]},
+ {"t":1.76719, "x":6.84505, "y":5.00079, "heading":3.00174, "vx":-1.5319, "vy":0.11902, "omega":-4.38857, "ax":-1.4472, "ay":9.5714, "alpha":2.5998, "fx":[-14.39176,-10.47035,-25.81476,-31.37825], "fy":[136.60903,137.14384,135.14169,133.79496]},
+ {"t":1.8103, "x":6.77768, "y":5.01482, "heading":2.81499, "vx":-1.59428, "vy":0.53159, "omega":-4.27651, "ax":-2.43169, "ay":9.27717, "alpha":7.2936, "fx":[-26.11863,-3.75881,-40.52773,-67.46946], "fy":[135.30712,138.08447,132.16605,120.44932]},
+ {"t":1.8534, "x":6.7067, "y":5.04635, "heading":2.63743, "vx":-1.6991, "vy":0.93147, "omega":-3.96213, "ax":-2.30096, "ay":9.19323, "alpha":9.38177, "fx":[-31.06929,9.98544,-34.80471,-74.57351], "fy":[133.86121,137.61438,133.70829,116.06326]},
+ {"t":1.88485, "x":6.65212, "y":5.08019, "heading":2.51744, "vx":-1.77148, "vy":1.22064, "omega":-3.66703, "ax":-2.32838, "ay":9.08073, "alpha":10.75056, "fx":[-40.87491,17.88282,-30.69039,-78.33438], "fy":[130.56311,136.49771,134.51753,113.29024]},
+ {"t":1.91631, "x":6.59524, "y":5.12308, "heading":2.40741, "vx":-1.84471, "vy":1.50627, "omega":-3.32888, "ax":-2.45106, "ay":8.94862, "alpha":11.79932, "fx":[-53.58098,21.52037,-27.26114,-79.65079], "fy":[125.01105,135.464,134.94219,111.96086]},
+ {"t":1.94776, "x":6.53601, "y":5.17489, "heading":2.30854, "vx":-1.92181, "vy":1.78774, "omega":-2.95773, "ax":-2.50593, "ay":8.84881, "alpha":12.36934, "fx":[-62.32843,22.09362,-23.82452,-78.02453], "fy":[119.67348,134.53982,135.07095,112.43461]},
+ {"t":1.97922, "x":6.47432, "y":5.2355, "heading":2.22163, "vx":-2.00063, "vy":2.06608, "omega":-2.56866, "ax":-2.19535, "ay":8.84919, "alpha":12.31477, "fx":[-60.33144,25.04261,-17.43728,-71.74837], "fy":[118.67028,132.60418,135.11638,115.34965]},
+ {"t":2.01067, "x":6.4103, "y":5.30486, "heading":2.14692, "vx":-2.06969, "vy":2.34443, "omega":-2.18131, "ax":-0.64448, "ay":9.00125, "alpha":10.96842, "fx":[-28.94223,41.23245,1.14025,-49.97203], "fy":[126.19498,125.95103,134.35114,123.86522]},
+ {"t":2.04213, "x":6.34488, "y":5.38306, "heading":2.08374, "vx":-2.08996, "vy":2.62755, "omega":-1.8363, "ax":4.57385, "ay":7.79999, "alpha":4.69691, "fx":[69.60419,81.1892,61.90432,46.63535], "fy":[106.62982,100.90942,114.68599,120.02676]},
+ {"t":2.07358, "x":6.28141, "y":5.46957, "heading":2.0283, "vx":-1.94609, "vy":2.8729, "omega":-1.68856, "ax":8.78837, "ay":2.75083, "alpha":-4.19376, "fx":[121.03281,120.1999,128.49817,128.56143], "fy":[52.26586,50.32766,23.41412,29.96158]},
+ {"t":2.10504, "x":6.22454, "y":5.56129, "heading":1.97311, "vx":-1.66966, "vy":2.95943, "omega":-1.82048, "ax":9.33371, "ay":0.49911, "alpha":-6.30836, "fx":[131.07043,131.21618,132.11701,134.80899], "fy":[30.26475,21.54966,-20.56519,-2.9503]},
+ {"t":2.13649, "x":6.17664, "y":5.65463, "heading":1.91273, "vx":-1.37607, "vy":2.97512, "omega":-2.0189, "ax":9.46472, "ay":0.00581, "alpha":-5.96183, "fx":[134.02585,134.12567,132.76773,135.72118], "fy":[22.28404,14.04478,-26.16631,-9.83289]},
+ {"t":2.16795, "x":6.13804, "y":5.74821, "heading":1.84628, "vx":-1.07836, "vy":2.97531, "omega":-2.20643, "ax":9.56326, "ay":0.15744, "alpha":-4.70199, "fx":[135.27455,135.28963,135.11069,136.55266], "fy":[19.30045,14.60147,-17.71176,-7.26332]},
+ {"t":2.1994, "x":6.10885, "y":5.84187, "heading":1.77455, "vx":-0.77755, "vy":2.98026, "omega":-2.35433, "ax":9.62581, "ay":0.58516, "alpha":-2.93363, "fx":[135.89524,135.81708,136.88973,137.17243], "fy":[18.47072,17.02856,-3.14725,0.82584]},
+ {"t":2.23085, "x":6.08915, "y":5.93591, "heading":1.69905, "vx":-0.47478, "vy":2.99867, "omega":-2.4466, "ax":9.61792, "ay":0.87577, "alpha":-0.89897, "fx":[136.10055,136.02686,136.56974,136.62992], "fy":[15.39221,15.28929,9.31138,9.66231]},
+ {"t":2.27159, "x":6.07779, "y":6.0588, "heading":1.59863, "vx":-0.08295, "vy":3.03434, "omega":-2.48323, "ax":9.22522, "ay":1.84471, "alpha":3.8665, "fx":[132.52287,133.64397,129.72904,127.16523], "fy":[15.16016,13.31658,35.20601,40.91049]},
+ {"t":2.31233, "x":6.08207, "y":6.18395, "heading":1.50067, "vx":0.29288, "vy":3.1095, "omega":-2.32571, "ax":8.77042, "ay":1.67492, "alpha":4.8868, "fx":[125.22822,128.20464,124.52751,119.31434], "fy":[11.67972,7.50174,33.25799,42.52683]},
+ {"t":2.35307, "x":6.10128, "y":6.31202, "heading":1.40998, "vx":0.65019, "vy":3.17773, "omega":-2.12662, "ax":2.49882, "ay":-7.63101, "alpha":-11.29854, "fx":[77.42296,15.2057,0.21421,48.83758], "fy":[-89.09989,-108.02157,-121.61268,-113.93661]},
+ {"t":2.39381, "x":6.12984, "y":6.43514, "heading":1.31396, "vx":0.75199, "vy":2.86685, "omega":-2.58692, "ax":-8.01224, "ay":-4.99016, "alpha":-3.47143, "fx":[-113.19265,-121.55376,-114.60192,-104.93831], "fy":[-71.09739,-57.09211,-70.89774,-83.85008]},
+ {"t":2.43455, "x":6.15383, "y":6.5478, "heading":1.20569, "vx":0.42557, "vy":2.66355, "omega":-2.72834, "ax":-9.22372, "ay":-2.92033, "alpha":-0.67819, "fx":[-130.83244,-131.67306,-130.69274,-129.778], "fy":[-41.04019,-38.42045,-41.74474,-44.37468]},
+ {"t":2.47529, "x":6.16351, "y":6.65389, "heading":1.09398, "vx":0.0498, "vy":2.54458, "omega":-2.75597, "ax":-9.4264, "ay":-2.392, "alpha":-0.53346, "fx":[-133.64666,-134.21732,-133.60972,-132.99428], "fy":[-33.75679,-31.50089,-34.05551,-36.31104]},
+ {"t":2.51603, "x":6.15772, "y":6.75557, "heading":0.98126, "vx":-0.33423, "vy":2.44713, "omega":-2.77771, "ax":-9.50831, "ay":-2.13882, "alpha":-0.85927, "fx":[-134.76203,-135.63059,-134.84857,-133.8708], "fy":[-30.42135,-26.39364,-30.23158,-34.22264]},
+ {"t":2.55677, "x":6.13621, "y":6.85349, "heading":0.86738, "vx":-0.72159, "vy":2.35999, "omega":-2.81271, "ax":-9.55455, "ay":-1.96634, "alpha":-1.40443, "fx":[-135.31275,-136.67706,-135.68575,-134.05851], "fy":[-28.69752,-21.4344,-27.15053,-34.20738]},
+ {"t":2.59751, "x":6.09889, "y":6.948, "heading":0.75162, "vx":-1.11084, "vy":2.27988, "omega":-2.86993, "ax":-9.58493, "ay":-1.81536, "alpha":-2.14358, "fx":[-135.5493,-137.53568,-136.44055,-133.93105], "fy":[-28.08251,-15.93619,-23.74747,-35.16297]},
+ {"t":2.63825, "x":6.04568, "y":7.03938, "heading":0.63293, "vx":-1.50133, "vy":2.20593, "omega":-2.95726, "ax":-9.15719, "ay":-2.58615, "alpha":9.65635, "fx":[-136.61506,-119.99903,-124.26946,-138.32035], "fy":[-23.51657,-69.34042,-60.90967,7.13458]},
+ {"t":2.67899, "x":5.97691, "y":7.1271, "heading":0.52046, "vx":-1.87439, "vy":2.10057, "omega":-2.56386, "ax":-9.06459, "ay":-2.89102, "alpha":9.86048, "fx":[-136.64076,-119.63342,-119.10228,-138.57715], "fy":[-23.7252,-70.12415,-70.74135,0.6727]},
+ {"t":2.71973, "x":5.89303, "y":7.21028, "heading":0.42419, "vx":-2.24368, "vy":1.98279, "omega":-2.16215, "ax":-9.05838, "ay":-3.10146, "alpha":8.78303, "fx":[-136.42386,-120.99081,-117.8738,-138.31333], "fy":[-25.22424,-67.87994,-72.98961,-9.75581]},
+ {"t":2.77434, "x":5.75701, "y":7.31392, "heading":0.31922, "vx":-2.73832, "vy":1.81343, "omega":-1.68254, "ax":-8.94021, "ay":-3.49168, "alpha":8.16916, "fx":[-135.69553,-120.43706,-113.91104,-136.8577], "fy":[-28.5932,-68.75073,-78.9199,-21.71116]},
+ {"t":2.82894, "x":5.59415, "y":7.40774, "heading":0.23952, "vx":-3.22651, "vy":1.62276, "omega":-1.23646, "ax":-8.59867, "ay":-4.26208, "alpha":7.96394, "fx":[-133.52721,-116.31554,-104.86709,-132.82647], "fy":[-37.02004,-75.33938,-90.40705,-38.88938]},
+ {"t":2.88355, "x":5.40514, "y":7.49, "heading":0.18388, "vx":-3.69605, "vy":1.39003, "omega":-0.80158, "ax":-7.49513, "ay":-6.02274, "alpha":6.98158, "fx":[-123.41671,-102.79343,-84.03522,-114.72129], "fy":[-62.38852,-92.61055,-109.71899,-76.76579]},
+ {"t":2.93815, "x":5.19214, "y":7.55692, "heading":0.15052, "vx":-4.10533, "vy":1.06115, "omega":-0.42034, "ax":-1.32339, "ay":-9.61296, "alpha":-1.02926, "fx":[-14.66394,-16.42124,-22.98468,-20.96532], "fy":[-136.82714,-136.56348,-135.63321,-136.02156]},
+ {"t":2.99276, "x":4.96599, "y":7.60054, "heading":0.12603, "vx":-4.17759, "vy":0.53623, "omega":-0.47654, "ax":8.37744, "ay":-4.95932, "alpha":-1.34271, "fx":[118.45848,121.84475,119.19003,115.49963], "fy":[-70.99447,-64.94599,-69.6085,-75.63951]},
+ {"t":3.04737, "x":4.75036, "y":7.62243, "heading":0.09801, "vx":-3.72013, "vy":0.26542, "omega":-0.54986, "ax":9.53894, "ay":-2.08066, "alpha":1.3835, "fx":[135.88911,133.90184,134.67292,136.38502], "fy":[-26.49581,-35.22678,-32.26984,-23.97881]},
+ {"t":3.10197, "x":4.56144, "y":7.63382, "heading":0.07004, "vx":-3.19925, "vy":0.1518, "omega":-0.47432, "ax":9.70773, "ay":-1.10095, "alpha":1.8192, "fx":[138.19898,136.71931,137.1571,138.34342], "fy":[-10.56864,-22.80239,-20.17033,-8.88123]},
+ {"t":3.15658, "x":4.40122, "y":7.64046, "heading":0.04686, "vx":-2.66915, "vy":0.09168, "omega":-0.37498, "ax":9.75382, "ay":-0.66602, "alpha":1.69181, "fx":[138.61633,137.77746,137.98061,138.6577], "fy":[-4.31035,-15.84525,-14.13696,-3.47035]},
+ {"t":3.21118, "x":4.27001, "y":7.64448, "heading":0.0289, "vx":-2.13653, "vy":0.05531, "omega":-0.28259, "ax":9.77201, "ay":-0.42741, "alpha":1.50692, "fx":[138.72565,138.24935,138.34732,138.7411], "fy":[-1.26875,-11.56777,-10.49541,-0.90186]},
+ {"t":3.26579, "x":4.16791, "y":7.64686, "heading":0.01572, "vx":-1.60292, "vy":0.03198, "omega":-0.20031, "ax":9.78072, "ay":-0.28068, "alpha":1.34351, "fx":[138.76245,138.48658,138.53697,138.77138], "fy":[0.41193,-8.76256,-8.08591,0.52198]},
+ {"t":3.3204, "x":4.09496, "y":7.64819, "heading":0.00678, "vx":-1.06884, "vy":0.01665, "omega":-0.12694, "ax":9.78542, "ay":-0.18473, "alpha":1.21262, "fx":[138.77816,138.61616,138.64423,138.78559], "fy":[1.41202,-6.8546,-6.41732,1.3856]},
+ {"t":3.375, "x":4.05118, "y":7.64882, "heading":0.00166, "vx":-0.53449, "vy":0.00656, "omega":-0.06073, "ax":9.78819, "ay":-0.12014, "alpha":1.1121, "fx":[138.78699,138.69139,138.70868,138.79406], "fy":[2.03004,-5.53835,-5.24057,1.93701]},
+ {"t":3.42961, "x":4.03659, "y":7.649, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.27754, "ay":-2.63455, "alpha":-7.29221, "fx":[129.21267,138.22803,137.58552,121.0015], "fy":[-50.72763,-12.66093,-18.02862,-67.95953]},
+ {"t":3.47909, "x":4.04795, "y":7.64578, "heading":-0.00893, "vx":0.45911, "vy":-0.13038, "omega":-0.36087, "ax":9.32371, "ay":-2.44309, "alpha":-7.4086, "fx":[129.90093,138.41891,138.03369,122.29212], "fy":[-48.88965,-10.13068,-13.94307,-65.55737]},
+ {"t":3.52858, "x":4.08209, "y":7.63633, "heading":-0.03586, "vx":0.92051, "vy":-0.25128, "omega":-0.72749, "ax":9.38139, "ay":-2.17809, "alpha":-7.55521, "fx":[130.69461,138.58623,138.47416,124.16065], "fy":[-46.6537,-7.04249,-7.93325,-61.86596]},
+ {"t":3.57807, "x":4.13913, "y":7.62123, "heading":-0.08111, "vx":1.38477, "vy":-0.35906, "omega":-1.10137, "ax":9.4558, "ay":-1.78183, "alpha":-7.70382, "fx":[131.79946,138.70068,138.64843,126.98652], "fy":[-43.3136,-2.82215,0.8047,-55.69711]},
+ {"t":3.62755, "x":4.21923, "y":7.60128, "heading":-0.14505, "vx":1.8527, "vy":-0.44724, "omega":-1.48261, "ax":9.54969, "ay":-1.11888, "alpha":-7.75708, "fx":[133.61485,138.61559,137.87468,131.35317], "fy":[-37.09589,3.90266,13.87883,-44.12521]},
+ {"t":3.67704, "x":4.32261, "y":7.57778, "heading":-0.22791, "vx":2.32528, "vy":-0.50261, "omega":-1.86648, "ax":9.61798, "ay":0.21075, "alpha":-7.34505, "fx":[136.75286,137.48223,133.92232,137.17286], "fy":[-22.12892,17.20238,35.03774,-18.16161]},
+ {"t":3.72653, "x":4.44946, "y":7.55316, "heading":-0.32927, "vx":2.80124, "vy":-0.49218, "omega":-2.22996, "ax":9.48769, "ay":1.00986, "alpha":8.42555, "fx":[127.34713,137.72205,136.97538,135.89835], "fy":[53.18535,-3.23925,-18.03006,25.34232]},
+ {"t":3.77601, "x":4.5997, "y":7.53004, "heading":-0.42931, "vx":3.27076, "vy":-0.4422, "omega":-1.81301, "ax":8.44647, "ay":-3.65089, "alpha":13.11081, "fx":[136.24934,96.95218,110.96605,134.73907], "fy":[-3.62159,-95.72003,-80.94357,-26.71656]},
+ {"t":3.8255, "x":4.7719, "y":7.50369, "heading":-0.50298, "vx":3.68874, "vy":-0.62287, "omega":-1.1642, "ax":2.49076, "ay":-9.10098, "alpha":5.93611, "fx":[34.92522,8.94164,36.18785,61.16919], "fy":[-129.58927,-134.95014,-130.87892,-120.5983]},
+ {"t":3.87499, "x":4.95749, "y":7.46172, "heading":-0.55332, "vx":3.812, "vy":-1.07325, "omega":-0.87044, "ax":-0.91398, "ay":-9.01253, "alpha":2.50481, "fx":[-16.72406,-22.95569,-9.741,-2.40132], "fy":[-126.70339,-126.88976,-129.02377,-128.3847]},
+ {"t":3.92447, "x":5.14502, "y":7.39758, "heading":-0.59333, "vx":3.76677, "vy":-1.51925, "omega":-0.74649, "ax":-7.71036, "ay":-3.96814, "alpha":-8.63702, "fx":[-94.80994,-100.88555,-123.47238,-118.00219], "fy":[-84.79027,-67.17409,-21.75472,-51.27057]},
+ {"t":3.97396, "x":5.32198, "y":7.31753, "heading":-0.64085, "vx":3.38521, "vy":-1.71562, "omega":-1.17391, "ax":-8.82163, "ay":-0.89781, "alpha":-10.87613, "fx":[-116.3312,-125.1628,-126.72932,-131.95441], "fy":[-60.17705,-9.51013,33.91767,-15.13544]},
+ {"t":4.02345, "x":5.4787, "y":7.23154, "heading":-0.71226, "vx":2.94866, "vy":-1.76005, "omega":-1.71213, "ax":-9.0646, "ay":-0.33967, "alpha":-10.38745, "fx":[-122.94104,-129.62226,-126.9786,-134.41213], "fy":[-51.49358,-2.29206,40.9336,-6.40718]},
+ {"t":4.07293, "x":5.61352, "y":7.14402, "heading":-0.8097, "vx":2.50009, "vy":-1.77686, "omega":-2.22617, "ax":-9.15824, "ay":-1.43328, "alpha":-8.68149, "fx":[-122.65251,-129.13288,-132.89148,-134.58669], "fy":[-55.94687,-30.14211,20.04099,-15.2175]},
+ {"t":4.12242, "x":5.72603, "y":7.05433, "heading":-0.9305, "vx":2.04688, "vy":-1.84779, "omega":-2.65579, "ax":-8.5932, "ay":-4.08807, "alpha":-4.59749, "fx":[-115.14368,-115.93102,-129.19753,-126.954], "fy":[-72.35276,-69.71305,-40.71155,-49.01219]},
+ {"t":4.17191, "x":5.8168, "y":6.95789, "heading":-1.06755, "vx":1.62163, "vy":-2.05009, "omega":-2.8833, "ax":-7.06698, "ay":-6.58514, "alpha":1.46394, "fx":[-102.60947,-104.04827,-97.9896,-96.04351], "fy":[-90.66,-89.20733,-95.8647,-97.63905]},
+ {"t":4.22139, "x":5.88839, "y":6.84837, "heading":-1.20845, "vx":1.27191, "vy":-2.37597, "omega":-2.81085, "ax":-5.8591, "ay":-7.62982, "alpha":6.06794, "fx":[-90.06775,-103.10642,-79.61612,-59.41533], "fy":[-103.97257,-91.56711,-112.73113,-124.33261]},
+ {"t":4.27088, "x":5.94416, "y":6.72145, "heading":-1.34011, "vx":0.98196, "vy":-2.75354, "omega":-2.51057, "ax":-5.32931, "ay":-7.90801, "alpha":8.71234, "fx":[-78.96483,-107.24934,-76.27557,-39.67732], "fy":[-113.12585,-87.29866,-115.49259,-132.45934]},
+ {"t":4.32037, "x":5.98623, "y":6.57551, "heading":-1.45369, "vx":0.71823, "vy":-3.14488, "omega":-2.07943, "ax":-5.25603, "ay":-7.86488, "alpha":10.04854, "fx":[-71.72767,-111.91155,-79.41544,-34.9572], "fy":[-117.67956,-81.14032,-113.33912,-133.77215]},
+ {"t":4.36355, "x":6.01235, "y":6.43235, "heading":-1.53412, "vx":0.49124, "vy":-3.48454, "omega":-1.64546, "ax":-5.28074, "ay":-7.76203, "alpha":10.89058, "fx":[-66.92258,-115.11962,-82.79256,-34.57826], "fy":[-119.87821,-75.96647,-110.63817,-133.61683]},
+ {"t":4.40674, "x":6.02864, "y":6.27463, "heading":-1.59503, "vx":0.26318, "vy":-3.81976, "omega":-1.17512, "ax":-5.43969, "ay":-7.54548, "alpha":11.58402, "fx":[-66.27893,-118.55589,-86.89722,-36.69336], "fy":[-119.05627,-69.31681,-106.91496,-132.53329]},
+ {"t":4.44993, "x":6.03493, "y":6.10262, "heading":-1.63497, "vx":0.02825, "vy":-4.14563, "omega":-0.67484, "ax":-6.20443, "ay":-6.76699, "alpha":11.68034, "fx":[-83.02135,-124.48952,-95.78431,-48.48993], "fy":[-104.5764,-54.45171,-97.49337,-127.16043]},
+ {"t":4.49312, "x":6.03037, "y":5.91727, "heading":-1.65323, "vx":-0.2397, "vy":-4.43788, "omega":-0.1704, "ax":-9.06647, "ay":-1.10786, "alpha":3.64551, "fx":[-128.90123,-130.53302,-128.63712,-125.98898], "fy":[-6.02843,-3.38119,-23.75981,-29.64492]},
+ {"t":4.5363, "x":6.01156, "y":5.72458, "heading":-1.65719, "vx":-0.63126, "vy":-4.48573, "omega":-0.01296, "ax":-7.10043, "ay":5.36665, "alpha":-6.75621, "fx":[-116.63868,-104.44828,-81.26733,-100.23313], "fy":[54.42793,68.66943,98.51693,82.66992]},
+ {"t":4.57949, "x":5.97767, "y":5.53586, "heading":-1.66405, "vx":-0.93791, "vy":-4.25395, "omega":-0.30474, "ax":-0.81279, "ay":9.56438, "alpha":2.51187, "fx":[-2.54132,-17.67797,-21.37908,-4.48578], "fy":[136.41161,135.33136,134.49921,136.0488]},
+ {"t":4.62268, "x":5.93641, "y":5.36106, "heading":-1.67486, "vx":-0.97301, "vy":-3.84089, "omega":-0.19626, "ax":1.68658, "ay":7.03675, "alpha":32.50793, "fx":[75.69597,-19.91066,-91.01339,130.85551], "fy":[115.86345,136.91255,102.7209,43.47995]},
+ {"t":4.66587, "x":5.89596, "y":5.20175, "heading":-1.65302, "vx":-0.90017, "vy":-3.537, "omega":1.20767, "ax":4.96797, "ay":7.55333, "alpha":15.93249, "fx":[89.28758,23.25209,41.89684,127.24249], "fy":[105.97536,136.53435,131.32034,54.43637]},
+ {"t":4.70905, "x":5.86172, "y":5.05604, "heading":-1.58601, "vx":-0.68562, "vy":-3.21079, "omega":1.89575, "ax":6.67623, "ay":6.67372, "alpha":10.75513, "fx":[98.50689,56.03729,96.96121,127.03053], "fy":[97.58396,126.75082,98.66658,55.39217]},
+ {"t":4.75224, "x":5.83833, "y":4.9236, "heading":-1.49411, "vx":-0.39729, "vy":-2.92257, "omega":2.36024, "ax":7.36507, "ay":6.07301, "alpha":9.0918, "fx":[103.12402,73.59449,113.29346,127.58056], "fy":[92.77021,117.50298,79.71765,54.34319]},
+ {"t":4.79543, "x":5.82804, "y":4.80304, "heading":-1.3837, "vx":-0.07921, "vy":-2.66029, "omega":2.75289, "ax":7.74289, "ay":5.69914, "alpha":7.81466, "fx":[105.60685,85.91653,120.29156,127.19963], "fy":[89.98528,108.8736,68.91693,55.35976]},
+ {"t":4.83861, "x":5.83184, "y":4.69347, "heading":-1.25752, "vx":0.25518, "vy":-2.41416, "omega":3.09038, "ax":7.78917, "ay":5.74664, "alpha":6.17846, "fx":[104.88193,92.91062,120.07724,123.76871], "fy":[90.82677,102.97614,69.35531,62.67064]},
+ {"t":4.88305, "x":5.85087, "y":4.59187, "heading":-1.1141, "vx":0.60129, "vy":-2.15881, "omega":3.36491, "ax":7.59969, "ay":6.0624, "alpha":4.69349, "fx":[101.68542,94.92739,116.21745,118.06503], "fy":[94.32469,101.05613,75.58185,72.7694]},
+ {"t":4.92748, "x":5.88509, "y":4.50193, "heading":-0.95995, "vx":0.93898, "vy":-1.88944, "omega":3.57347, "ax":7.38495, "ay":6.37212, "alpha":2.95266, "fx":[99.5178,97.17995,110.81182,111.21022], "fy":[96.50933,98.80455,83.21985,82.75962]},
+ {"t":4.97192, "x":5.93411, "y":4.42426, "heading":-0.79825, "vx":1.26712, "vy":-1.6063, "omega":3.70467, "ax":7.15726, "ay":6.64932, "alpha":0.8769, "fx":[99.47776,99.47761,103.50729,103.34691], "fy":[96.38995,96.36578,92.02456,92.22998]},
+ {"t":5.01635, "x":5.99748, "y":4.35945, "heading":-0.63277, "vx":1.58515, "vy":-1.31084, "omega":3.74363, "ax":6.96466, "ay":6.8277, "alpha":-1.53623, "fx":[102.88484,101.57418,94.76354,95.66682], "fy":[92.45804,93.9556,100.81251,99.89773]},
+ {"t":5.06079, "x":6.07479, "y":4.30795, "heading":-0.46794, "vx":1.89462, "vy":-1.00745, "omega":3.67537, "ax":6.92557, "ay":6.76975, "alpha":-4.34393, "fx":[111.09863,104.11287,86.22556,91.23637], "fy":[81.81451,90.77436,107.85381,103.39571]},
+ {"t":5.10522, "x":6.16581, "y":4.26986, "heading":-0.30892, "vx":2.20235, "vy":-0.70665, "omega":3.48235, "ax":6.80359, "ay":6.7346, "alpha":-6.73134, "fx":[117.19283,103.23462,76.55039,88.77941], "fy":[71.79266,91.20446,114.3583,104.49013]},
+ {"t":5.14965, "x":6.27038, "y":4.24511, "heading":-0.16083, "vx":2.50466, "vy":-0.4074, "omega":3.18325, "ax":6.01673, "ay":7.33664, "alpha":-7.36426, "fx":[110.47613,92.39899,61.1468,77.12108], "fy":[80.30925,101.2844,122.4254,111.96135]},
+ {"t":5.19409, "x":6.38762, "y":4.23425, "heading":-0.02665, "vx":2.77201, "vy":-0.0814, "omega":2.85602, "ax":5.02965, "ay":7.79365, "alpha":-7.5638, "fx":[99.14691,78.63221,44.90514,62.49187], "fy":[89.58019,109.47756,126.44899,116.38571]},
+ {"t":5.23852, "x":6.51575, "y":4.23833, "heading":0.09278, "vx":2.9955, "vy":0.2649, "omega":2.51993, "ax":-2.49813, "ay":7.80985, "alpha":1.9646, "fx":[-39.61265,-41.64972,-30.84963,-29.52988], "fy":[110.61203,107.89089,110.96713,113.34076]},
+ {"t":5.28296, "x":6.64639, "y":4.25781, "heading":0.2067, "vx":2.8845, "vy":0.61193, "omega":2.60723, "ax":-5.49483, "ay":7.79617, "alpha":3.9025, "fx":[-83.18319,-91.68509,-71.76053,-64.92284], "fy":[107.77251,100.00357,114.79581,119.46344]},
+ {"t":5.32739, "x":6.76914, "y":4.2927, "heading":0.3264, "vx":2.64034, "vy":0.95835, "omega":2.78063, "ax":-5.85183, "ay":7.72849, "alpha":2.24455, "fx":[-86.55951,-90.51308,-78.97709,-75.74323], "fy":[107.08274,103.59638,112.57614,114.94275]},
+ {"t":5.37183, "x":6.88068, "y":4.34291, "heading":0.45217, "vx":2.38032, "vy":1.30175, "omega":2.88037, "ax":-6.27867, "ay":7.45219, "alpha":0.73964, "fx":[-90.3395,-91.28252,-87.613,-86.75942], "fy":[104.53858,103.68183,106.79167,107.52027]},
+ {"t":5.41626, "x":6.98025, "y":4.40811, "heading":0.58089, "vx":2.10133, "vy":1.63289, "omega":2.91323, "ax":-6.68041, "ay":7.11798, "alpha":-0.75488, "fx":[-93.17934,-92.61871,-96.15598,-96.81904], "fy":[102.31124,102.844,99.5489,98.87865]},
+ {"t":5.46069, "x":7.06703, "y":4.48769, "heading":0.70959, "vx":1.80449, "vy":1.94917, "omega":2.87969, "ax":-7.03412, "ay":6.76496, "alpha":-2.21761, "fx":[-94.86772,-94.43836,-104.07466,-105.44694], "fy":[100.91775,101.3753,91.45999,89.8135]},
+ {"t":5.50513, "x":7.14026, "y":4.58098, "heading":0.83536, "vx":1.49193, "vy":2.24977, "omega":2.78115, "ax":-7.33445, "ay":6.41294, "alpha":-3.60837, "fx":[-95.55143,-96.75468,-111.0821,-112.46829], "fy":[100.38098,99.29281,82.95469,80.97906]},
+ {"t":5.54956, "x":7.19932, "y":4.68728, "heading":0.95537, "vx":1.16603, "vy":2.53472, "omega":2.62082, "ax":-7.58422, "ay":6.07312, "alpha":-4.90747, "fx":[-95.5001,-99.45214,-117.08192,-117.98392], "fy":[100.50744,96.67622,74.35178,72.80459]},
+ {"t":5.594, "x":7.24364, "y":4.8059, "heading":1.06698, "vx":0.82903, "vy":2.80457, "omega":2.40276, "ax":-7.7965, "ay":5.62336, "alpha":-7.94248, "fx":[-90.28618,-100.89848,-124.78779,-126.08152], "fy":[105.26005,95.23391,60.63449,57.71043]},
+ {"t":5.63843, "x":7.27278, "y":4.93607, "heading":1.1659, "vx":0.4826, "vy":3.05444, "omega":2.04984, "ax":-7.859, "ay":5.01428, "alpha":-13.17096, "fx":[-76.97386,-101.71435,-132.09614,-134.81367], "fy":[115.32026,94.37108,42.42345,32.18986]},
+ {"t":5.6802, "x":7.28608, "y":5.06802, "heading":1.24003, "vx":0.15437, "vy":3.26387, "omega":1.49974, "ax":-8.01649, "ay":4.88287, "alpha":-12.10843, "fx":[-82.07464,-105.63684,-132.90269,-133.91319], "fy":[111.70163,89.91183,39.70172,35.53886]},
+ {"t":5.72196, "x":7.28554, "y":5.20859, "heading":1.29211, "vx":-0.18045, "vy":3.4678, "omega":0.99403, "ax":-8.21467, "ay":4.68429, "alpha":-10.87315, "fx":[-89.27663,-109.5991,-133.47228,-133.41587], "fy":[105.95864,84.96267,37.559,37.11452]},
+ {"t":5.76373, "x":7.27084, "y":5.35751, "heading":1.32414, "vx":-0.52354, "vy":3.66345, "omega":0.5399, "ax":-8.49013, "ay":4.35355, "alpha":-9.18263, "fx":[-99.69828,-114.35256,-133.89487,-133.43669], "fy":[96.08579,78.31575,35.72555,36.71504]},
+ {"t":5.80549, "x":7.24157, "y":5.51432, "heading":1.33868, "vx":-0.87814, "vy":3.84528, "omega":0.15638, "ax":-8.90134, "ay":3.72443, "alpha":-6.40057, "fx":[-114.96504,-121.44756,-134.30211,-133.9828], "fy":[76.90128,66.49147,33.5991,34.17983]},
+ {"t":5.84726, "x":7.19713, "y":5.67817, "heading":1.33963, "vx":-1.24991, "vy":4.00083, "omega":-0.11094, "ax":-9.47127, "ay":2.27173, "alpha":-0.99748, "fx":[-133.44327,-133.52787,-135.02586,-135.01519], "fy":[35.48561,35.31705,29.06823,28.93413]},
+ {"t":5.88903, "x":7.13666, "y":5.84725, "heading":1.33413, "vx":-1.64548, "vy":4.09571, "omega":-0.1526, "ax":-9.58145, "ay":-1.26373, "alpha":3.53042, "fx":[-135.53071,-133.31778,-136.90617,-137.50456], "fy":[-23.92135,-33.33668,-10.57478,-3.81943]},
+ {"t":5.93079, "x":7.05958, "y":6.0172, "heading":1.33083, "vx":-2.04566, "vy":4.04293, "omega":-0.00515, "ax":-8.42251, "ay":-4.64524, "alpha":0.08181, "fx":[-119.38597,-119.20467,-119.38873,-119.5691], "fy":[-65.86228,-66.17482,-65.82813,-65.51526]},
+ {"t":5.97256, "x":6.9668, "y":6.18201, "heading":1.33069, "vx":-2.39743, "vy":3.84892, "omega":-0.00174, "ax":-7.11061, "ay":-6.13488, "alpha":-1.35813, "fx":[-99.94916,-104.6838,-101.66379,-96.86822], "fy":[-87.63574,-82.29152,-86.42786,-91.48663]},
+ {"t":6.01432, "x":6.86046, "y":6.33741, "heading":1.32943, "vx":-2.69441, "vy":3.59269, "omega":-0.05846, "ax":5.3771, "ay":-7.47847, "alpha":-12.42216, "fx":[117.8447,85.34908,24.48211,77.20076], "fy":[-70.22366,-105.05943,-134.87678,-113.86197]},
+ {"t":6.05609, "x":6.75262, "y":6.48094, "heading":1.31615, "vx":-2.46983, "vy":3.28035, "omega":-0.57728, "ax":6.61777, "ay":-6.51429, "alpha":-12.54227, "fx":[125.93589,113.3227,46.14248,89.82036], "fy":[-56.71221,-77.53625,-130.00886,-105.09665]},
+ {"t":6.09785, "x":6.65524, "y":6.61226, "heading":1.2811, "vx":-2.19344, "vy":3.00827, "omega":-1.10112, "ax":6.59387, "ay":-6.54282, "alpha":-12.53667, "fx":[125.10576,114.26399,46.01592,88.48025], "fy":[-58.52782,-76.1873,-130.03627,-106.2203]},
+ {"t":6.13435, "x":6.57958, "y":6.7177, "heading":1.23257, "vx":-1.95278, "vy":2.76948, "omega":-1.55867, "ax":6.03935, "ay":-7.00467, "alpha":-12.81112, "fx":[121.05661,106.83776,33.94809,80.58285], "fy":[-66.05991,-85.52484,-133.44941,-112.12403]},
+ {"t":6.17085, "x":6.51233, "y":6.81411, "heading":1.16715, "vx":-1.73237, "vy":2.51383, "omega":-2.02623, "ax":5.32374, "ay":-7.48256, "alpha":-13.26846, "fx":[115.64081,97.25723,18.64755,70.30556], "fy":[-74.52938,-95.13376,-136.05414,-118.53697]},
+ {"t":6.20734, "x":6.45265, "y":6.90087, "heading":1.08436, "vx":-1.53807, "vy":2.24074, "omega":-2.51049, "ax":4.40589, "ay":-7.93498, "alpha":-13.93573, "fx":[108.42293,84.59712,-0.34251,57.13215], "fy":[-83.73182,-104.5847,-136.66952,-124.91967]},
+ {"t":6.24384, "x":6.39945, "y":6.97737, "heading":0.98346, "vx":-1.37727, "vy":1.95114, "omega":-3.0191, "ax":3.28144, "ay":-8.25448, "alpha":-14.83421, "fx":[98.91029,68.30016,-22.13894,40.98277], "fy":[-92.89583,-111.66424,-133.40039,-130.0606]},
+ {"t":6.28034, "x":6.35137, "y":7.04308, "heading":0.86339, "vx":-1.2575, "vy":1.64988, "omega":-3.5605, "ax":2.35387, "ay":-8.02604, "alpha":-15.67825, "fx":[86.68004,55.98344,-35.51092,26.30954], "fy":[-97.58586,-103.80626,-124.1265,-129.55009]},
+ {"t":6.31683, "x":6.30704, "y":7.09795, "heading":0.723, "vx":-1.17159, "vy":1.35696, "omega":-4.1327, "ax":-0.08032, "ay":8.05451, "alpha":8.97348, "fx":[-35.79144,-4.58976,34.88321,0.94384], "fy":[113.13682,108.77538,112.6108,122.16021]},
+ {"t":6.35333, "x":6.26423, "y":7.15284, "heading":0.57815, "vx":-1.17453, "vy":1.65092, "omega":-3.8052, "ax":-8.94957, "ay":-1.90323, "alpha":12.81199, "fx":[-136.44667,-118.57725,-120.22181,-132.18615], "fy":[-11.05342,-67.85937,-60.36168,31.36319]},
+ {"t":6.38983, "x":6.2154, "y":7.21183, "heading":0.4478, "vx":-1.50116, "vy":1.58146, "omega":-3.3376, "ax":-9.15007, "ay":-2.11319, "alpha":10.69508, "fx":[-137.36165,-123.19851,-121.86349,-136.37638], "fy":[-11.49289,-61.65896,-62.63181,15.968]},
+ {"t":6.42633, "x":6.15452, "y":7.26814, "heading":0.33311, "vx":-1.83511, "vy":1.50433, "omega":-2.94727, "ax":-9.25451, "ay":-2.10448, "alpha":9.47742, "fx":[-137.82306,-126.22237,-123.11537,-137.56085], "fy":[-9.78509,-56.1461,-61.75251,8.36197]},
+ {"t":6.46282, "x":6.08138, "y":7.32164, "heading":0.23186, "vx":-2.17286, "vy":1.42753, "omega":-2.60137, "ax":-9.30071, "ay":-2.11307, "alpha":8.85809, "fx":[-138.09612,-127.93823,-123.27306,-138.03414], "fy":[-8.24416,-52.63031,-62.18161,3.24685]},
+ {"t":6.49932, "x":5.99588, "y":7.37233, "heading":0.14282, "vx":-2.51231, "vy":1.35041, "omega":-2.27808, "ax":-9.30479, "ay":-2.1672, "alpha":8.72057, "fx":[-138.2789,-128.7563,-122.3126,-138.22503], "fy":[-6.89991,-50.92557,-64.4623,-0.59041]},
+ {"t":6.53582, "x":5.89799, "y":7.42018, "heading":0.06548, "vx":-2.85191, "vy":1.27131, "omega":-1.95981, "ax":-9.26968, "ay":-2.26682, "alpha":9.07744, "fx":[-138.42434,-128.89319,-119.98123,-138.28304], "fy":[-5.36631,-50.81017,-68.95441,-3.39544]},
+ {"t":6.57231, "x":5.78773, "y":7.46506, "heading":0.0, "vx":-3.19022, "vy":1.18858, "omega":-1.62851, "ax":-8.93253, "ay":-2.58945, "alpha":13.29548, "fx":[-138.66661,-125.7231,-103.64241,-138.4341], "fy":[1.2838,-58.56619,-91.97865,2.44167]},
+ {"t":6.63654, "x":5.5644, "y":7.53607, "heading":-0.07717, "vx":-3.76396, "vy":1.02226, "omega":-0.77454, "ax":-8.63308, "ay":-3.61166, "alpha":11.94547, "fx":[-138.08464,-122.08942,-94.27074,-135.0427], "fy":[-9.77392,-65.39763,-101.21462,-28.39151]},
+ {"t":6.70077, "x":5.30483, "y":7.59427, "heading":-0.10228, "vx":-4.31846, "vy":0.79028, "omega":-0.00728, "ax":-1.07656, "ay":-9.52918, "alpha":4.00574, "fx":[-29.42027,-26.8679,-3.47046,-1.28107], "fy":[-133.12745,-134.11136,-136.72881,-136.32757]},
+ {"t":6.765, "x":5.02524, "y":7.62538, "heading":-0.09449, "vx":-4.38761, "vy":0.17822, "omega":0.25001, "ax":9.62395, "ay":-1.40971, "alpha":2.75436, "fx":[137.69005,134.84108,135.37189,137.76585], "fy":[-10.89481,-29.94633,-27.92252,-11.1655]},
+ {"t":6.82923, "x":4.76327, "y":7.63392, "heading":-0.07275, "vx":-3.76946, "vy":0.08768, "omega":0.42692, "ax":9.76379, "ay":-0.34962, "alpha":-1.43674, "fx":[138.24009,138.58083,138.5585,138.2184], "fy":[-9.71623,-0.54639,0.12868,-9.68927]},
+ {"t":6.89346, "x":4.5413, "y":7.63883, "heading":-0.04829, "vx":-3.14233, "vy":0.06522, "omega":0.33464, "ax":9.77658, "ay":-0.23424, "alpha":-1.32813, "fx":[138.48416,138.69502,138.67717,138.46648], "fy":[-7.68423,0.82967,1.32145,-7.7482]},
+ {"t":6.95769, "x":4.35964, "y":7.64253, "heading":-0.02954, "vx":-2.51438, "vy":0.05017, "omega":0.24933, "ax":9.78259, "ay":-0.19873, "alpha":-1.16412, "fx":[138.59555,138.7499,138.73864,138.57954], "fy":[-6.60238,0.89722,1.1825,-6.74506]},
+ {"t":7.02192, "x":4.21832, "y":7.64535, "heading":-0.01592, "vx":-1.88605, "vy":0.03741, "omega":0.17456, "ax":9.78607, "ay":-0.18919, "alpha":-1.01839, "fx":[138.65716,138.78395,138.7769,138.64266], "fy":[-5.96394,0.62338,0.76456,-6.15063]},
+ {"t":7.08615, "x":4.11736, "y":7.64736, "heading":-0.00681, "vx":-1.25749, "vy":0.02526, "omega":0.10915, "ax":9.78824, "ay":-0.19176, "alpha":-0.89786, "fx":[138.69414,138.80663,138.8019,138.68116], "fy":[-5.59327,0.23274,0.28438,-5.79655]},
+ {"t":7.15038, "x":4.05678, "y":7.64859, "heading":-0.00165, "vx":-0.62879, "vy":0.01294, "omega":0.05148, "ax":9.78962, "ay":-0.20149, "alpha":-0.80147, "fx":[138.71659,138.82194,138.81841,138.70504], "fy":[-5.41257,-0.19931,-0.19813,-5.61451]},
+ {"t":7.21461, "x":4.03659, "y":7.649, "heading":0.0, "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,75]
+ },
+ "events":[]
+}
--- /dev/null
+{
+ "name":"depot",
+ "version":3,
+ "snapshot":{
+ "waypoints":[
+ {"x":3.5792136192321777, "y":4.4951395988464355, "heading":3.141592653589793, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":0.4407301247119904, "y":5.105472087860107, "heading":1.5741615318803677, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":0.43530577421188354, "y":7.034785270690918, "heading":1.5707963267948966, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":3.5617806911468506, "y":7.495027542114258, "heading":0.0, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":7.616069793701172, "y":7.413266181945801, "heading":-0.7389209115342424, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true},
+ {"from":0, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":0.48153873719275, "y":3.177359360270202, "r":1.4154387569402775}}, "enabled":true},
+ {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}],
+ "targetDt":0.05
+ },
+ "params":{
+ "waypoints":[
+ {"x":{"exp":"3.5792136192321777 m", "val":3.5792136192321777}, "y":{"exp":"4.4951395988464355 m", "val":4.4951395988464355}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"0.44073012471199036 m", "val":0.4407301247119904}, "y":{"exp":"5.105472087860107 m", "val":5.105472087860107}, "heading":{"exp":"1.5741615318803677 rad", "val":1.5741615318803677}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"0.43530577421188354 m", "val":0.43530577421188354}, "y":{"exp":"7.034785270690918 m", "val":7.034785270690918}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"3.5617806911468506 m", "val":3.5617806911468506}, "y":{"exp":"7.495027542114258 m", "val":7.495027542114258}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"7.616069793701172 m", "val":7.616069793701172}, "y":{"exp":"7.413266181945801 m", "val":7.413266181945801}, "heading":{"exp":"-0.7389209115342424 rad", "val":-0.7389209115342424}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true},
+ {"from":0, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"0.48153873719275 m", "val":0.48153873719275}, "y":{"exp":"3.177359360270202 m", "val":3.177359360270202}, "r":{"exp":"1.4154387569402775 m", "val":1.4154387569402775}}}, "enabled":true},
+ {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}],
+ "targetDt":{
+ "exp":"0.05 s",
+ "val":0.05
+ }
+ },
+ "trajectory":{
+ "config":{
+ "frontLeft":{
+ "x":0.52705,
+ "y":0.52705
+ },
+ "backLeft":{
+ "x":-0.52705,
+ "y":0.52705
+ },
+ "mass":56.69904625,
+ "inertia":7.0,
+ "gearing":7.03125,
+ "radius":0.0508,
+ "vmax":628.3185307179587,
+ "tmax":7.0,
+ "cof":1.0,
+ "bumper":{
+ "front":0.4064,
+ "side":0.4064,
+ "back":0.4064
+ },
+ "differentialTrackWidth":0.5588
+ },
+ "sampleType":"Swerve",
+ "waypoints":[0.0,1.17237,2.18058,3.15126,4.28571],
+ "samples":[
+ {"t":0.0, "x":3.57921, "y":4.49514, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.26547, "ay":0.64148, "alpha":-44.35812, "fx":[-117.72002,-132.86448,-32.07928,-15.88327], "fy":[73.59562,-40.23018,-134.84389,137.84947]},
+ {"t":0.05329, "x":3.57174, "y":4.49605, "heading":3.07861, "vx":-0.2806, "vy":0.03418, "omega":-2.36383, "ax":-7.1629, "ay":1.55108, "alpha":-32.1252, "fx":[-118.18105,-136.83767,-103.5752,-47.53593], "fy":[72.7829,-23.1563,-91.96441,130.28243]},
+ {"t":0.10658, "x":3.54661, "y":4.50007, "heading":2.90703, "vx":-0.6623, "vy":0.11684, "omega":-4.07578, "ax":-8.98692, "ay":2.88118, "alpha":-11.31773, "fx":[-122.3314,-138.04186,-138.5517,-110.62487], "fy":[65.36641,13.44514,1.08759,83.46119]},
+ {"t":0.15987, "x":3.49856, "y":4.51039, "heading":2.67376, "vx":-1.14122, "vy":0.27038, "omega":-4.6789, "ax":-8.78321, "ay":2.94752, "alpha":13.85815, "fx":[-137.6292,-107.33459,-115.62638,-137.40926], "fy":[-15.03588,87.32658,76.45574,18.37512]},
+ {"t":0.21316, "x":3.42527, "y":4.52899, "heading":2.4441, "vx":-1.60927, "vy":0.42745, "omega":-3.9404, "ax":-8.89897, "ay":2.51969, "alpha":13.64814, "fx":[-135.47674,-119.16581,-112.96669,-136.95415], "fy":[-28.39227,69.84839,80.18957,21.21805]},
+ {"t":0.26645, "x":3.32688, "y":4.55534, "heading":2.25349, "vx":-2.08349, "vy":0.56172, "omega":-3.21309, "ax":-9.01649, "ay":2.19741, "alpha":12.81573, "fx":[-134.79137,-129.78165,-110.64414,-136.00933], "fy":[-31.20469,46.41588,83.15536,26.22434]},
+ {"t":0.31974, "x":3.20305, "y":4.5884, "heading":2.10047, "vx":-2.56398, "vy":0.67882, "omega":-2.53015, "ax":-9.07875, "ay":2.02841, "alpha":12.2963, "fx":[-135.48438,-134.94301,-109.66398,-134.66485], "fy":[-27.35975,26.4066,84.10358,31.85851]},
+ {"t":0.37303, "x":3.05352, "y":4.62745, "heading":1.9831, "vx":-3.04779, "vy":0.78692, "omega":-1.87488, "ax":-9.05891, "ay":2.03999, "alpha":12.23633, "fx":[-136.12168,-135.998,-108.48574,-133.02601], "fy":[-22.19209,15.72273,84.97965,37.15499]},
+ {"t":0.42632, "x":2.87824, "y":4.67228, "heading":1.90056, "vx":-3.53053, "vy":0.89563, "omega":-1.22281, "ax":-8.94167, "ay":2.17055, "alpha":12.42568, "fx":[-135.90245,-134.58462,-105.68286,-130.81425], "fy":[-17.49512,11.92617,86.72568,41.91133]},
+ {"t":0.47961, "x":2.67741, "y":4.72309, "heading":1.85304, "vx":-4.00703, "vy":1.0113, "omega":-0.56065, "ax":-7.10114, "ay":2.53261, "alpha":10.37874, "fx":[-113.68644,-95.34298,-84.48417,-109.1143], "fy":[1.86792,24.81186,72.48396,44.4329]},
+ {"t":0.5329, "x":2.45379, "y":4.78058, "heading":1.8379, "vx":-4.38545, "vy":1.14626, "omega":-0.00757, "ax":0.02077, "ay":0.11585, "alpha":0.01278, "fx":[0.27959,0.32052,0.30931,0.26838], "fy":[1.61607,1.62726,1.66818,1.65699]},
+ {"t":0.58619, "x":2.22012, "y":4.84183, "heading":1.83751, "vx":-4.38434, "vy":1.15243, "omega":-0.00689, "ax":-0.0797, "ay":-0.30573, "alpha":-0.00022, "fx":[-1.12946,-1.13017,-1.12997,-1.12926], "fy":[-4.33319,-4.33338,-4.33409,-4.3339]},
+ {"t":0.63948, "x":1.98637, "y":4.90281, "heading":1.83715, "vx":-4.38859, "vy":1.13614, "omega":-0.0069, "ax":-0.66601, "ay":-2.79515, "alpha":-0.01138, "fx":[-9.42721,-9.46606,-9.45394,-9.41509], "fy":[-39.59973,-39.6075,-39.64153,-39.63376]},
+ {"t":0.69277, "x":1.75155, "y":4.95938, "heading":1.83676, "vx":-4.42408, "vy":0.98719, "omega":-0.0075, "ax":1.67235, "ay":-8.26921, "alpha":-4.4637, "fx":[36.21259,8.7945,13.48177,36.33168], "fy":[-112.63299,-118.31169,-121.44016,-116.47159]},
+ {"t":0.74606, "x":1.51817, "y":5.00025, "heading":1.83002, "vx":-4.33496, "vy":0.54652, "omega":-0.24537, "ax":7.75121, "ay":-4.93754, "alpha":-12.33287, "fx":[135.55463,111.35244,75.73755,116.84185], "fy":[-18.11554,-75.77196,-113.94716,-72.11907]},
+ {"t":0.79935, "x":1.29817, "y":5.02236, "heading":1.79944, "vx":-3.9219, "vy":0.2834, "omega":-0.90259, "ax":8.0018, "ay":-4.49749, "alpha":-13.45546, "fx":[137.63371,121.47116,75.38525,119.20445], "fy":[-7.68581,-62.21156,-115.36591,-69.74021]},
+ {"t":0.85264, "x":1.10053, "y":5.03108, "heading":1.73223, "vx":-3.49549, "vy":0.04373, "omega":-1.61963, "ax":9.22568, "ay":-2.97043, "alpha":4.60465, "fx":[123.44802,129.76894,136.24598,133.62438], "fy":[-62.12592,-47.86313,-23.53068,-34.90103]},
+ {"t":0.90593, "x":0.92736, "y":5.02919, "heading":1.65246, "vx":-3.00385, "vy":-0.11456, "omega":-1.37425, "ax":9.11768, "ay":0.71457, "alpha":15.53723, "fx":[128.91765,136.81985,131.59705,119.62928], "fy":[-50.33507,-22.31913,43.5882,69.58167]},
+ {"t":0.95922, "x":0.78023, "y":5.0241, "heading":1.60129, "vx":-2.51798, "vy":-0.07648, "omega":-0.54627, "ax":9.02776, "ay":3.45009, "alpha":6.51404, "fx":[134.34727,136.22849,125.2177,116.07205], "fy":[34.10272,26.01781,59.66695,75.82929]},
+ {"t":1.01251, "x":0.65887, "y":5.02492, "heading":1.58143, "vx":-2.03689, "vy":0.10737, "omega":-0.19914, "ax":8.58806, "ay":4.64456, "alpha":2.93081, "fx":[124.08856,127.57215,120.23366,115.04023], "fy":[62.00694,54.54211,69.25323,77.53977]},
+ {"t":1.0658, "x":0.56251, "y":5.03724, "heading":1.57498, "vx":-1.57923, "vy":0.35488, "omega":-0.04296, "ax":8.25301, "ay":5.26202, "alpha":0.97075, "fx":[117.56583,119.32824,116.49414,114.54933], "fy":[73.73005,70.85299,75.4279,78.34081]},
+ {"t":1.11908, "x":0.49008, "y":5.06362, "heading":1.57407, "vx":-1.13943, "vy":0.63529, "omega":0.00877, "ax":8.01085, "ay":5.63121, "alpha":-0.26264, "fx":[113.4318,112.85085,113.67836,114.24659], "fy":[79.99801,80.81354,79.6439,78.82865]},
+ {"t":1.17237, "x":0.44073, "y":5.10547, "heading":1.57416, "vx":-0.71254, "vy":0.93538, "omega":-0.00522, "ax":7.72895, "ay":6.0065, "alpha":-0.68154, "fx":[109.32759,107.59018,109.82398,111.48242], "fy":[85.46903,87.64032,84.81942,82.634]},
+ {"t":1.22038, "x":0.41543, "y":5.1573, "heading":1.57313, "vx":-0.34147, "vy":1.22375, "omega":-0.03794, "ax":6.41128, "ay":7.37863, "alpha":-0.43956, "fx":[90.99024,89.3246,90.77312,92.42524], "fy":[104.50983,105.93084,104.68482,103.23569]},
+ {"t":1.26839, "x":0.40642, "y":5.22456, "heading":1.5708, "vx":-0.03366, "vy":1.578, "omega":-0.05904, "ax":2.38591, "ay":8.74697, "alpha":2.21408, "fx":[28.76222,42.04765,38.36086,26.10819], "fy":[124.76578,121.27258,123.49452,126.41202]},
+ {"t":1.3164, "x":0.40756, "y":5.3104, "heading":1.57051, "vx":0.08088, "vy":1.99794, "omega":0.04725, "ax":-0.45288, "ay":0.01625, "alpha":-1.40195, "fx":[-4.09611,-8.74092,-8.7432,-4.09788], "fy":[2.56612,2.5595,-2.09821,-2.10604]},
+ {"t":1.36441, "x":0.41092, "y":5.40634, "heading":1.57117, "vx":0.05914, "vy":1.99872, "omega":-0.02005, "ax":-0.30191, "ay":0.00783, "alpha":0.45448, "fx":[-5.03313,-3.52551,-3.526,-5.0336], "fy":[-0.64425,-0.64412,0.86685,0.86571]},
+ {"t":1.41242, "x":0.41341, "y":5.5023, "heading":1.57073, "vx":0.04465, "vy":1.99909, "omega":0.00176, "ax":-0.21274, "ay":0.0042, "alpha":-0.001, "fx":[-3.01383,-3.01714,-3.01714,-3.01383], "fy":[0.06126,0.06126,0.05794,0.05794]},
+ {"t":1.46043, "x":0.41531, "y":5.59829, "heading":1.57081, "vx":0.03443, "vy":1.9993, "omega":0.00172, "ax":-0.1743, "ay":0.00263, "alpha":-0.03067, "fx":[-2.41969,-2.5215,-2.5215,-2.41969], "fy":[0.08829,0.08828,-0.01359,-0.01359]},
+ {"t":1.50844, "x":0.41676, "y":5.69427, "heading":1.57086, "vx":0.02606, "vy":1.99942, "omega":0.00024, "ax":-0.15389, "ay":0.00172, "alpha":-0.00686, "fx":[-2.17,-2.19276,-2.19276,-2.16999], "fy":[0.03577,0.03576,0.01299,0.01299]},
+ {"t":1.55645, "x":0.41783, "y":5.79027, "heading":1.57086, "vx":0.01868, "vy":1.9995, "omega":-0.00008, "ax":-0.14217, "ay":0.00108, "alpha":0.00038, "fx":[-2.01588,-2.01462,-2.01462,-2.01588], "fy":[0.01473,0.01473,0.01599,0.01599]},
+ {"t":1.60446, "x":0.41857, "y":5.88627, "heading":1.57086, "vx":0.01185, "vy":1.99956, "omega":-0.00007, "ax":-0.13569, "ay":0.00058, "alpha":0.00017, "fx":[-1.92368,-1.9231,-1.9231,-1.92368], "fy":[0.00796,0.00796,0.00854,0.00854]},
+ {"t":1.65247, "x":0.41898, "y":5.98227, "heading":1.57086, "vx":0.00534, "vy":1.99958, "omega":-0.00006, "ax":-0.133, "ay":0.00014, "alpha":-0.0005, "fx":[-1.88441,-1.88606,-1.88606,-1.88441], "fy":[0.00283,0.00283,0.00118,0.00118]},
+ {"t":1.70048, "x":0.41908, "y":6.07827, "heading":1.57085, "vx":-0.00105, "vy":1.99959, "omega":-0.00008, "ax":-0.13357, "ay":-0.00029, "alpha":-0.00063, "fx":[-1.8923,-1.8944,-1.8944,-1.8923], "fy":[-0.00299,-0.00299,-0.00509,-0.00509]},
+ {"t":1.74849, "x":0.41888, "y":6.17427, "heading":1.57085, "vx":-0.00746, "vy":1.99958, "omega":-0.00011, "ax":-0.13751, "ay":-0.00074, "alpha":-0.00111, "fx":[-1.94736,-1.95104,-1.95104,-1.94736], "fy":[-0.00866,-0.00866,-0.01234,-0.01234]},
+ {"t":1.7965, "x":0.41836, "y":6.27026, "heading":1.57084, "vx":-0.01406, "vy":1.99954, "omega":-0.00017, "ax":-0.1456, "ay":-0.00128, "alpha":-0.00266, "fx":[-2.05948,-2.0683,-2.0683,-2.05948], "fy":[-0.01372,-0.01372,-0.02254,-0.02254]},
+ {"t":1.84451, "x":0.41752, "y":6.36626, "heading":1.57083, "vx":-0.02105, "vy":1.99948, "omega":-0.00029, "ax":-0.15979, "ay":-0.00199, "alpha":-0.0015, "fx":[-2.26246,-2.26744,-2.26744,-2.26246], "fy":[-0.02571,-0.02571,-0.03069,-0.03069]},
+ {"t":1.89252, "x":0.41632, "y":6.46225, "heading":1.57081, "vx":-0.02873, "vy":1.99938, "omega":-0.00037, "ax":-0.18478, "ay":-0.00306, "alpha":0.02009, "fx":[-2.6525,-2.58582,-2.58583,-2.6525], "fy":[-0.0768,-0.0768,-0.01008,-0.01008]},
+ {"t":1.94053, "x":0.41473, "y":6.55824, "heading":1.57082, "vx":-0.0376, "vy":1.99924, "omega":0.0006, "ax":-0.23421, "ay":-0.00506, "alpha":0.05549, "fx":[-3.41201,-3.22786,-3.22787,-3.41202], "fy":[-0.16395,-0.16395,0.02042,0.02041]},
+ {"t":1.98854, "x":0.41266, "y":6.65422, "heading":1.57091, "vx":-0.04884, "vy":1.99899, "omega":0.00326, "ax":-0.34874, "ay":-0.00998, "alpha":-0.34226, "fx":[-4.37584,-5.51085,-5.51063,-4.37564], "fy":[0.42766,0.42727,-0.71027,-0.71055]},
+ {"t":2.03655, "x":0.40991, "y":6.75018, "heading":1.57067, "vx":-0.06558, "vy":1.99852, "omega":-0.01317, "ax":0.15005, "ay":0.00462, "alpha":0.37403, "fx":[1.5061,2.74775,2.74787,1.50624], "fy":[-0.55563,-0.55563,0.68642,0.68676]},
+ {"t":2.08456, "x":0.40693, "y":6.84613, "heading":1.57047, "vx":-0.05838, "vy":1.99874, "omega":0.00479, "ax":8.43792, "ay":-0.61905, "alpha":16.10155, "fx":[106.63293,126.02782,129.3841,116.37743], "fy":[-68.69353,-39.58078,25.70073,47.47385]},
+ {"t":2.13257, "x":0.41386, "y":6.94138, "heading":1.58926, "vx":0.34672, "vy":1.96902, "omega":0.77782, "ax":4.16862, "ay":-0.97577, "alpha":-48.42373, "fx":[135.11369,-9.31051,0.77288,109.78046], "fy":[30.84409,137.02597,-138.49879,-84.69633]},
+ {"t":2.18058, "x":0.43531, "y":7.03479, "heading":1.5708, "vx":0.54686, "vy":1.92217, "omega":-1.547, "ax":4.57013, "ay":-1.21174, "alpha":-46.21507, "fx":[136.953,12.81098,2.48027,106.87757], "fy":[21.63048,136.64979,-138.56993,-88.41469]},
+ {"t":2.2247, "x":0.46388, "y":7.11841, "heading":1.45756, "vx":0.7485, "vy":1.86871, "omega":-3.58607, "ax":5.37025, "ay":-8.0256, "alpha":-5.72786, "fx":[97.86105,74.44586,53.69694,78.48393], "fy":[-97.57697,-116.21919,-127.36075,-113.88711]},
+ {"t":2.26883, "x":0.50213, "y":7.19305, "heading":1.29376, "vx":0.98544, "vy":1.5146, "omega":-3.8388, "ax":3.93337, "ay":-8.80135, "alpha":-4.89841, "fx":[76.35835,53.82369,35.02055,57.81545], "fy":[-114.47638,-126.3614,-133.11317,-125.07728]},
+ {"t":2.31295, "x":0.54944, "y":7.25131, "heading":1.11962, "vx":1.15899, "vy":1.12628, "omega":-4.05492, "ax":5.21096, "ay":-8.08882, "alpha":0.24472, "fx":[72.93671,73.65404,74.78579,74.07978], "fy":[-115.24635,-114.81861,-114.06677,-114.49655]},
+ {"t":2.35707, "x":0.60565, "y":7.29313, "heading":0.94095, "vx":1.3889, "vy":0.76939, "omega":-4.04412, "ax":7.84963, "ay":-5.02885, "alpha":7.26037, "fx":[96.77428,101.25143,122.25338,124.78751], "fy":[-91.66274,-89.09379,-57.08906,-47.28535]},
+ {"t":2.40119, "x":0.67457, "y":7.32218, "heading":0.76958, "vx":1.73524, "vy":0.54751, "omega":-3.72379, "ax":9.00997, "ay":-0.96708, "alpha":10.20289, "fx":[127.77235,120.49067,133.86299,128.73094], "fy":[-20.04109,-56.80348,-10.93893,32.95111]},
+ {"t":2.44531, "x":0.7599, "y":7.3454, "heading":0.61521, "vx":2.13277, "vy":0.50484, "omega":-3.27362, "ax":8.97017, "ay":0.77181, "alpha":10.15722, "fx":[125.41974,126.71129,133.51679,122.95201], "fy":[27.18811,-35.39682,2.30473,49.66495]},
+ {"t":2.48943, "x":0.86274, "y":7.36843, "heading":0.48066, "vx":2.52855, "vy":0.53889, "omega":-2.82547, "ax":8.85447, "ay":0.75213, "alpha":10.2088, "fx":[121.91137,124.81699,132.14499,123.16669], "fy":[33.31112,-34.09372,-2.12448,45.55192]},
+ {"t":2.53356, "x":0.98292, "y":7.39293, "heading":0.36594, "vx":2.91922, "vy":0.57207, "omega":-2.37504, "ax":8.63704, "ay":-0.21723, "alpha":10.6668, "fx":[120.00504,116.3833,128.72862,124.59472], "fy":[20.6495,-48.29119,-16.92729,32.25223]},
+ {"t":2.57768, "x":1.12012, "y":7.41796, "heading":0.27153, "vx":3.3003, "vy":0.56249, "omega":-1.90441, "ax":8.07267, "ay":-1.64812, "alpha":10.98499, "fx":[113.27117,100.51258,120.69455,123.2344], "fy":[-3.47663,-67.00275,-35.88657,12.91922]},
+ {"t":2.6218, "x":1.2736, "y":7.44118, "heading":0.19819, "vx":3.65648, "vy":0.48977, "omega":-1.41973, "ax":7.04173, "ay":-2.90209, "alpha":10.54334, "fx":[96.17977,81.03171,107.22531,114.82248], "fy":[-27.90215,-77.6916,-51.17719,-7.77498]},
+ {"t":2.66592, "x":1.44178, "y":7.45996, "heading":0.14582, "vx":3.96717, "vy":0.36173, "omega":-0.95454, "ax":5.53978, "ay":-3.36245, "alpha":8.93329, "fx":[70.27125,61.69088,87.4854,94.65286], "fy":[-38.45904,-73.02967,-55.94861,-23.21063]},
+ {"t":2.71004, "x":1.62221, "y":7.47265, "heading":0.1124, "vx":4.2116, "vy":0.21337, "omega":-0.56039, "ax":3.27843, "ay":-2.56796, "alpha":5.51474, "fx":[37.96449,37.20267,54.57024,56.1462], "fy":[-29.55687,-48.51839,-42.89835,-24.62707]},
+ {"t":2.75416, "x":1.81122, "y":7.47956, "heading":0.09304, "vx":4.35624, "vy":0.10007, "omega":-0.31707, "ax":1.0119, "ay":-0.98089, "alpha":1.71224, "fx":[11.29093,11.728,17.37002,16.98486], "fy":[-11.39093,-17.0621,-16.39902,-10.7635]},
+ {"t":2.79828, "x":2.00441, "y":7.48302, "heading":0.08071, "vx":4.40089, "vy":0.05679, "omega":-0.24153, "ax":0.15897, "ay":-0.17724, "alpha":0.23596, "fx":[1.83152,1.89433,2.67524,2.61262], "fy":[-2.15363,-2.93452,-2.87084,-2.09007]},
+ {"t":2.84241, "x":2.19874, "y":7.48536, "heading":0.07029, "vx":4.40791, "vy":0.04897, "omega":-0.23112, "ax":-0.05196, "ay":0.04786, "alpha":-0.139, "fx":[-0.49012,-0.52252,-0.98292,-0.95054], "fy":[0.46436,0.92477,0.89233,0.43193]},
+ {"t":2.88653, "x":2.39317, "y":7.48756, "heading":0.05996, "vx":4.40561, "vy":0.05108, "omega":-0.23725, "ax":-0.09371, "ay":0.06376, "alpha":-0.21836, "fx":[-0.94471,-0.98809,-1.71176,-1.66843], "fy":[0.56361,1.2874,1.24383,0.52011]},
+ {"t":2.93065, "x":2.58746, "y":7.48988, "heading":0.04927, "vx":4.40148, "vy":0.0539, "omega":-0.24688, "ax":-0.07456, "ay":-0.02358, "alpha":-0.18728, "fx":[-0.731,-0.76165,-1.38271,-1.35206], "fy":[-0.62946,-0.00831,-0.03894,-0.66005]},
+ {"t":2.97477, "x":2.78159, "y":7.49224, "heading":0.0382, "vx":4.39819, "vy":0.05286, "omega":-0.25515, "ax":-0.01707, "ay":-0.19586, "alpha":-0.08098, "fx":[-0.10239,-0.11267,-0.38146,-0.37116], "fy":[-2.90549,-2.63692,-2.64717,-2.91574]},
+ {"t":3.01889, "x":2.97563, "y":7.49438, "heading":0.02686, "vx":4.39744, "vy":0.04421, "omega":-0.25872, "ax":0.07947, "ay":-0.42261, "alpha":0.10662, "fx":[0.94452,0.95383,1.30833,1.29909], "fy":[-5.81858,-6.17184,-6.16209,-5.80885]},
+ {"t":3.06301, "x":3.16973, "y":7.49592, "heading":0.01555, "vx":4.40094, "vy":0.02557, "omega":-0.25402, "ax":0.40411, "ay":-0.65402, "alpha":0.72595, "fx":[4.50616,4.53332,6.94745,6.92595], "fy":[-8.09352,-10.49838,-10.44615,-8.04447]},
+ {"t":3.10713, "x":3.3643, "y":7.49641, "heading":0.00505, "vx":4.41877, "vy":-0.00329, "omega":-0.22199, "ax":2.58996, "ay":-1.26886, "alpha":4.87325, "fx":[29.4881,28.61353,43.79626,44.95064], "fy":[-9.81463,-27.32775,-25.68282,-9.11807]},
+ {"t":3.15126, "x":3.56178, "y":7.49503, "heading":0.0, "vx":4.53305, "vy":-0.05927, "omega":-0.00697, "ax":0.03427, "ay":-0.67277, "alpha":0.10695, "fx":[0.3074,0.3073,0.66403,0.66427], "fy":[-9.35972,-9.71319,-9.71301,-9.35955]},
+ {"t":3.20058, "x":3.78541, "y":7.49129, "heading":-0.00021, "vx":4.53474, "vy":-0.09246, "omega":-0.0017, "ax":-0.00045, "ay":-0.02205, "alpha":0.0, "fx":[-0.00638,-0.00638,-0.00638,-0.00638], "fy":[-0.31254,-0.31255,-0.31255,-0.31254]},
+ {"t":3.2499, "x":4.00908, "y":7.4867, "heading":-0.0003, "vx":4.53471, "vy":-0.09354, "omega":-0.0017, "ax":0.00101, "ay":0.04975, "alpha":0.0, "fx":[0.0143,0.0143,0.01429,0.01429], "fy":[0.70519,0.7052,0.7052,0.70519]},
+ {"t":3.29923, "x":4.23275, "y":7.48215, "heading":-0.00038, "vx":4.53476, "vy":-0.09109, "omega":-0.0017, "ax":0.00112, "ay":0.0567, "alpha":0.0, "fx":[0.01584,0.01584,0.01583,0.01583], "fy":[0.80368,0.8037,0.8037,0.80368]},
+ {"t":3.34855, "x":4.45643, "y":7.47772, "heading":-0.00046, "vx":4.53482, "vy":-0.08829, "omega":-0.0017, "ax":0.00108, "ay":0.05648, "alpha":0.0, "fx":[0.01529,0.01529,0.01528,0.01528], "fy":[0.80058,0.80059,0.80059,0.80058]},
+ {"t":3.39788, "x":4.6801, "y":7.47343, "heading":-0.00055, "vx":4.53487, "vy":-0.08551, "omega":-0.0017, "ax":0.00103, "ay":0.05553, "alpha":0.0, "fx":[0.01456,0.01456,0.01455,0.01455], "fy":[0.78713,0.78714,0.78714,0.78713]},
+ {"t":3.4472, "x":4.90378, "y":7.46928, "heading":-0.00063, "vx":4.53492, "vy":-0.08277, "omega":-0.0017, "ax":0.00098, "ay":0.05457, "alpha":0.0, "fx":[0.01385,0.01385,0.01384,0.01384], "fy":[0.77345,0.77346,0.77346,0.77345]},
+ {"t":3.49652, "x":5.12746, "y":7.46527, "heading":-0.00072, "vx":4.53497, "vy":-0.08008, "omega":-0.0017, "ax":0.00093, "ay":0.05365, "alpha":0.0, "fx":[0.01317,0.01317,0.01316,0.01316], "fy":[0.76053,0.76054,0.76054,0.76053]},
+ {"t":3.54585, "x":5.35115, "y":7.46138, "heading":-0.0008, "vx":4.53502, "vy":-0.07743, "omega":-0.0017, "ax":0.00088, "ay":0.05278, "alpha":0.0, "fx":[0.01252,0.01252,0.01252,0.01252], "fy":[0.74811,0.74812,0.74812,0.74811]},
+ {"t":3.59517, "x":5.57483, "y":7.45763, "heading":-0.00088, "vx":4.53506, "vy":-0.07483, "omega":-0.0017, "ax":0.00084, "ay":0.05173, "alpha":0.0, "fx":[0.01186,0.01186,0.01185,0.01185], "fy":[0.73322,0.73323,0.73323,0.73322]},
+ {"t":3.6445, "x":5.79852, "y":7.454, "heading":-0.00097, "vx":4.5351, "vy":-0.07228, "omega":-0.0017, "ax":0.00076, "ay":0.04867, "alpha":0.0, "fx":[0.01078,0.01078,0.01078,0.01078], "fy":[0.68991,0.68991,0.68991,0.68991]},
+ {"t":3.69382, "x":6.02221, "y":7.4505, "heading":-0.00105, "vx":4.53514, "vy":-0.06988, "omega":-0.0017, "ax":0.00041, "ay":0.02726, "alpha":0.0, "fx":[0.00587,0.00587,0.00586,0.00586], "fy":[0.38644,0.38644,0.38644,0.38644]},
+ {"t":3.74314, "x":6.24591, "y":7.44708, "heading":-0.00113, "vx":4.53516, "vy":-0.06853, "omega":-0.0017, "ax":-0.00505, "ay":-0.15864, "alpha":-0.00262, "fx":[-0.06725,-0.06724,-0.07595,-0.07596], "fy":[-2.25311,-2.24441,-2.2444,-2.2531]},
+ {"t":3.79247, "x":6.46959, "y":7.44351, "heading":-0.00122, "vx":4.53491, "vy":-0.07636, "omega":-0.00183, "ax":-6.96593, "ay":-0.61076, "alpha":-12.58152, "fx":[-85.61995,-90.8241,-110.53863,-107.97899], "fy":[-47.56144,27.26282,17.93736,-32.26823]},
+ {"t":3.84179, "x":6.6848, "y":7.439, "heading":-0.01662, "vx":4.19132, "vy":-0.10648, "omega":-0.6224, "ax":-9.11963, "ay":-0.05153, "alpha":-14.87678, "fx":[-123.85505,-125.12211,-134.04926,-134.04777], "fy":[-58.58523,55.70679,32.0658,-32.10905]},
+ {"t":3.89112, "x":6.88044, "y":7.43369, "heading":-0.06541, "vx":3.74151, "vy":-0.10902, "omega":-1.35618, "ax":-9.3212, "ay":0.23652, "alpha":-12.65755, "fx":[-129.85775,-128.2854,-134.17054,-136.18965], "fy":[-46.7045,50.77029,33.77161,-24.42675]},
+ {"t":3.94044, "x":7.05364, "y":7.4286, "heading":-0.1477, "vx":3.28175, "vy":-0.09736, "omega":-1.9805, "ax":-9.46126, "ay":0.3863, "alpha":-10.50303, "fx":[-133.12214,-131.62144,-134.22578,-137.47525], "fy":[-37.65759,42.4565,34.26312,-17.15909]},
+ {"t":3.98976, "x":7.204, "y":7.42426, "heading":-0.25816, "vx":2.81508, "vy":-0.0783, "omega":-2.49855, "ax":-9.77115, "ay":0.31832, "alpha":-0.40886, "fx":[-138.55158,-138.4692,-138.451,-138.54329], "fy":[2.87805,5.54045,6.12212,3.50759]},
+ {"t":4.03909, "x":7.33097, "y":7.42079, "heading":-0.3819, "vx":2.33313, "vy":-0.0626, "omega":-2.51872, "ax":-9.69621, "ay":0.27078, "alpha":5.43332, "fx":[-136.43445,-138.59971,-137.09218,-137.63935], "fy":[24.86972,-5.22578,-20.69751,16.40658]},
+ {"t":4.08841, "x":7.43425, "y":7.41803, "heading":-0.49952, "vx":1.85487, "vy":-0.04925, "omega":-2.25073, "ax":-9.55699, "ay":0.25699, "alpha":8.95749, "fx":[-133.1894,-138.59932,-133.10514,-136.97843], "fy":[38.82844,-6.59747,-38.9973,21.33722]},
+ {"t":4.13774, "x":7.51412, "y":7.41591, "heading":-0.59964, "vx":1.38348, "vy":-0.03657, "omega":-1.80891, "ax":-9.43626, "ay":0.25769, "alpha":11.04441, "fx":[-129.86385,-138.70058,-129.38645,-137.07578], "fy":[48.89763,-4.97969,-50.08558,20.77834]},
+ {"t":4.18706, "x":7.57088, "y":7.41442, "heading":-0.67543, "vx":0.91805, "vy":-0.02386, "omega":-1.26415, "ax":-9.34356, "ay":0.25006, "alpha":12.34729, "fx":[-126.79688,-138.78388,-126.62445,-137.56582], "fy":[56.42672,-2.78079,-56.78082,17.31303]},
+ {"t":4.23638, "x":7.60479, "y":7.41355, "heading":-0.72276, "vx":0.45719, "vy":-0.01153, "omega":-0.65514, "ax":-9.26902, "ay":0.23368, "alpha":13.28228, "fx":[-124.19876,-138.82236,-124.53948,-137.98399], "fy":[61.9718,-1.12968,-61.26731,13.67462]},
+ {"t":4.28571, "x":7.61607, "y":7.41327, "heading":-0.73892, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+ "splits":[0]
+ },
+ "events":[]
+}
--- /dev/null
+{
+ "name":"depotBump",
+ "version":3,
+ "snapshot":{
+ "waypoints":[
+ {"x":3.5792136192321777, "y":4.4951395988464355, "heading":3.141592653589793, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":0.4469754695892334, "y":5.089114189147949, "heading":1.5741615318803677, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":0.4469754695892334, "y":7.081206798553467, "heading":1.5707963267948966, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":2.852884292602539, "y":5.705027103424072, "heading":-2.3447879882246334, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":6.201909065246582, "y":5.416318416595459, "heading":-2.4093358095692223, "intervals":6, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":5.980565547943115, "y":5.156479835510254, "heading":0.0, "intervals":7, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":5.739974498748779, "y":4.63680362701416, "heading":-1.5707963267948966, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":5.909343242645264, "y":3.5838329792022705, "heading":-1.0754850095735469, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":7.597336292266846, "y":3.549332857131958, "heading":1.0628225299704972, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":8.011152267456055, "y":5.7627692222595215, "heading":1.611868659838111, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":6.567607402801514, "y":7.552765369415283, "heading":0.0, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":4.036591053009033, "y":7.649001598358154, "heading":0.0, "intervals":17, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":6.054632186889648, "y":7.439671993255615, "heading":-1.4002829791692777, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":5.930652618408203, "y":4.921932220458984, "heading":-1.1479422447393446, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":8.257654190063477, "y":4.6548991203308105, "heading":1.489584669362912, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":7.714051723480225, "y":6.714868068695068, "heading":1.3765551236309548, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":6.054457664489746, "y":7.6343512535095215, "heading":0.0, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":4.036591053009033, "y":7.649001598358154, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true},
+ {"from":0, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":0.48153873719275, "y":3.177359360270202, "r":1.4154387569402775}}, "enabled":true},
+ {"from":1, "to":3, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true},
+ {"from":11, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":15, "to":17, "data":{"type":"KeepOutCircle", "props":{"x":4.86896484461613, "y":5.965750575065613, "r":1.0}}, "enabled":true}],
+ "targetDt":0.05
+ },
+ "params":{
+ "waypoints":[
+ {"x":{"exp":"3.5792136192321777 m", "val":3.5792136192321777}, "y":{"exp":"4.4951395988464355 m", "val":4.4951395988464355}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"0.4469754695892334 m", "val":0.4469754695892334}, "y":{"exp":"5.089114189147949 m", "val":5.089114189147949}, "heading":{"exp":"1.5741615318803677 rad", "val":1.5741615318803677}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"0.4469754695892334 m", "val":0.4469754695892334}, "y":{"exp":"7.081206798553467 m", "val":7.081206798553467}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"2.852884292602539 m", "val":2.852884292602539}, "y":{"exp":"5.705027103424072 m", "val":5.705027103424072}, "heading":{"exp":"-2.3447879882246334 rad", "val":-2.3447879882246334}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"6.201909065246582 m", "val":6.201909065246582}, "y":{"exp":"5.416318416595459 m", "val":5.416318416595459}, "heading":{"exp":"-2.4093358095692223 rad", "val":-2.4093358095692223}, "intervals":6, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"5.980565547943115 m", "val":5.980565547943115}, "y":{"exp":"5.156479835510254 m", "val":5.156479835510254}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":7, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+ {"x":{"exp":"5.739974498748779 m", "val":5.739974498748779}, "y":{"exp":"4.63680362701416 m", "val":4.63680362701416}, "heading":{"exp":"-1.5707963267948966 rad", "val":-1.5707963267948966}, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"5.909343242645264 m", "val":5.909343242645264}, "y":{"exp":"3.5838329792022705 m", "val":3.5838329792022705}, "heading":{"exp":"-1.0754850095735469 rad", "val":-1.0754850095735469}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"7.597336292266846 m", "val":7.597336292266846}, "y":{"exp":"3.549332857131958 m", "val":3.549332857131958}, "heading":{"exp":"1.0628225299704972 rad", "val":1.0628225299704972}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"8.011152267456055 m", "val":8.011152267456055}, "y":{"exp":"5.7627692222595215 m", "val":5.7627692222595215}, "heading":{"exp":"1.611868659838111 rad", "val":1.611868659838111}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"6.567607402801514 m", "val":6.567607402801514}, "y":{"exp":"7.552765369415283 m", "val":7.552765369415283}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"shootPos.x", "val":4.036591053009033}, "y":{"exp":"shootPos.y", "val":7.649001598358154}, "heading":{"exp":"shootPos.heading", "val":0.0}, "intervals":17, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"6.054632186889648 m", "val":6.054632186889648}, "y":{"exp":"7.439671993255615 m", "val":7.439671993255615}, "heading":{"exp":"-1.4002829791692777 rad", "val":-1.4002829791692777}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"5.930652618408203 m", "val":5.930652618408203}, "y":{"exp":"4.921932220458984 m", "val":4.921932220458984}, "heading":{"exp":"-1.1479422447393446 rad", "val":-1.1479422447393446}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"8.257654190063477 m", "val":8.257654190063477}, "y":{"exp":"4.6548991203308105 m", "val":4.6548991203308105}, "heading":{"exp":"1.489584669362912 rad", "val":1.489584669362912}, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"7.714051723480225 m", "val":7.714051723480225}, "y":{"exp":"6.714868068695068 m", "val":6.714868068695068}, "heading":{"exp":"1.3765551236309548 rad", "val":1.3765551236309548}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"6.054457664489746 m", "val":6.054457664489746}, "y":{"exp":"7.6343512535095215 m", "val":7.6343512535095215}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"shootPos.x", "val":4.036591053009033}, "y":{"exp":"shootPos.y", "val":7.649001598358154}, "heading":{"exp":"shootPos.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true},
+ {"from":0, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"0.48153873719275 m", "val":0.48153873719275}, "y":{"exp":"3.177359360270202 m", "val":3.177359360270202}, "r":{"exp":"1.4154387569402775 m", "val":1.4154387569402775}}}, "enabled":true},
+ {"from":1, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true},
+ {"from":11, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":15, "to":17, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"4.86896484461613 m", "val":4.86896484461613}, "y":{"exp":"5.965750575065613 m", "val":5.965750575065613}, "r":{"exp":"1 m", "val":1.0}}}, "enabled":true}],
+ "targetDt":{
+ "exp":"0.05 s",
+ "val":0.05
+ }
+ },
+ "trajectory":{
+ "config":{
+ "frontLeft":{
+ "x":0.52705,
+ "y":0.52705
+ },
+ "backLeft":{
+ "x":-0.52705,
+ "y":0.52705
+ },
+ "mass":56.69904625,
+ "inertia":7.0,
+ "gearing":7.03125,
+ "radius":0.0508,
+ "vmax":628.3185307179587,
+ "tmax":7.0,
+ "cof":1.0,
+ "bumper":{
+ "front":0.4064,
+ "side":0.4064,
+ "back":0.4064
+ },
+ "differentialTrackWidth":0.5588
+ },
+ "sampleType":"Swerve",
+ "waypoints":[0.0,1.17306,2.24645,3.73187,4.79122,5.0063,5.19372,5.50015,6.22959,6.80755,7.5838,8.37977,9.22201,10.00039,11.02982,11.56569,12.19975,12.88018],
+ "samples":[
+ {"t":0.0, "x":3.57921, "y":4.49514, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.73364, "ay":0.57802, "alpha":-46.96681, "fx":[-116.73614,-131.99911,-13.03555,-6.62202], "fy":[75.14131,-42.97379,-137.995,138.60055]},
+ {"t":0.05332, "x":3.57248, "y":4.49596, "heading":3.07483, "vx":-0.2524, "vy":0.03082, "omega":-2.50431, "ax":-6.94327, "ay":1.50029, "alpha":-33.68753, "fx":[-117.19602,-136.73674,-97.99248,-41.75142], "fy":[74.34043,-23.67945,-97.82605,132.2301]},
+ {"t":0.10664, "x":3.54916, "y":4.49974, "heading":2.89341, "vx":-0.62262, "vy":0.11082, "omega":-4.30055, "ax":-9.09215, "ay":3.10308, "alpha":-7.90333, "fx":[-123.81061,-136.79377,-137.42318,-117.48866], "fy":[62.41564,22.59982,17.49637,73.4299]},
+ {"t":0.15996, "x":3.50303, "y":4.51006, "heading":2.65286, "vx":-1.10742, "vy":0.27628, "omega":-4.72196, "ax":-8.79101, "ay":2.86996, "alpha":14.02736, "fx":[-137.24128,-108.29127,-115.47657,-137.4325], "fy":[-17.84872,86.00778,76.61364,17.95131]},
+ {"t":0.21328, "x":3.43149, "y":4.52887, "heading":2.42103, "vx":-1.57617, "vy":0.4293, "omega":-3.97401, "ax":-8.94081, "ay":2.35038, "alpha":13.58269, "fx":[-134.74486,-121.79792,-113.32964,-137.06269], "fy":[-31.46838,64.89336,79.58819,20.25111]},
+ {"t":0.2666, "x":3.33473, "y":4.5551, "heading":2.22844, "vx":-2.0529, "vy":0.55463, "omega":-3.24977, "ax":-9.06935, "ay":1.97617, "alpha":12.69307, "fx":[-134.24169,-132.27402,-111.56351,-136.14408], "fy":[-33.26897,38.25115,81.80165,25.26308]},
+ {"t":0.31992, "x":3.21238, "y":4.58748, "heading":2.0732, "vx":-2.53648, "vy":0.66, "omega":-2.57297, "ax":-9.1249, "ay":1.83185, "alpha":12.17578, "fx":[-135.29944,-136.08548,-111.2188,-134.76919], "fy":[-27.93152,18.78454,81.8748,31.13659]},
+ {"t":0.37325, "x":3.06416, "y":4.62528, "heading":1.95332, "vx":-3.02303, "vy":0.75767, "omega":-1.92375, "ax":-9.08705, "ay":1.90677, "alpha":12.17457, "fx":[-136.05952,-136.37548,-109.80488,-132.98706], "fy":[-22.00436,10.11358,83.03842,36.9644]},
+ {"t":0.42657, "x":2.89005, "y":4.66839, "heading":1.86805, "vx":-3.50756, "vy":0.85935, "omega":-1.27459, "ax":-8.94123, "ay":2.12504, "alpha":12.45124, "fx":[-135.88862,-134.62682,-106.01903,-130.4245], "fy":[-16.54228,8.36905,85.97439,42.68678]},
+ {"t":0.47989, "x":2.69032, "y":4.71723, "heading":1.81778, "vx":-3.98431, "vy":0.97265, "omega":-0.61068, "ax":-7.61508, "ay":2.79582, "alpha":11.27057, "fx":[-122.21292,-106.74501,-87.96912,-114.84061], "fy":[1.13569,26.03241,81.87118,49.4808]},
+ {"t":0.53321, "x":2.46705, "y":4.77307, "heading":1.80124, "vx":-4.39035, "vy":1.12173, "omega":-0.00972, "ax":0.04668, "ay":0.26171, "alpha":0.02808, "fx":[0.62685,0.7177,0.69638,0.60554], "fy":[3.65374,3.67498,3.76571,3.74447]},
+ {"t":0.58653, "x":2.23302, "y":4.83325, "heading":1.80077, "vx":-4.38786, "vy":1.13568, "omega":-0.00823, "ax":-0.07447, "ay":-0.29026, "alpha":-0.00035, "fx":[-1.0551,-1.05625,-1.05598,-1.05483], "fy":[-4.11369,-4.11396,-4.11511,-4.11484]},
+ {"t":0.63985, "x":1.99895, "y":4.89339, "heading":1.80033, "vx":-4.39183, "vy":1.12021, "omega":-0.00825, "ax":-0.64839, "ay":-2.78267, "alpha":-0.01803, "fx":[-9.16811,-9.23037,-9.21352,-9.15127], "fy":[-39.41127,-39.42165,-39.47602,-39.46564]},
+ {"t":0.69317, "x":1.76385, "y":4.94917, "heading":1.79986, "vx":-4.4264, "vy":0.97183, "omega":-0.00921, "ax":2.17094, "ay":-8.16239, "alpha":-5.20957, "fx":[46.66434,14.57806,17.91033,43.93746], "fy":[-109.15052,-117.62558,-121.37494,-114.64896]},
+ {"t":0.74649, "x":1.53092, "y":4.98938, "heading":1.79196, "vx":-4.31065, "vy":0.53661, "omega":-0.28699, "ax":7.52545, "ay":-5.24131, "alpha":-12.31865, "fx":[134.41337,107.55003,71.03805,113.68419], "fy":[-23.6919,-80.22011,-116.62872,-76.63652]},
+ {"t":0.79981, "x":1.31177, "y":5.01055, "heading":1.75915, "vx":-3.90939, "vy":0.25714, "omega":-0.94383, "ax":7.80159, "ay":-4.78732, "alpha":-13.61802, "fx":[137.15109,118.83528,70.07969,116.27686], "fy":[-12.30446,-66.33096,-118.48584,-74.31512]},
+ {"t":0.85313, "x":1.1144, "y":5.01745, "heading":1.68947, "vx":-3.4934, "vy":0.00187, "omega":-1.66995, "ax":9.14213, "ay":-2.96776, "alpha":6.92808, "fx":[117.80113,128.38134,137.31481,134.85293], "fy":[-72.07972,-51.3943,-15.78205,-29.01283]},
+ {"t":0.90645, "x":0.94113, "y":5.01333, "heading":1.61027, "vx":-3.00593, "vy":-0.15637, "omega":-1.30054, "ax":8.88349, "ay":0.93766, "alpha":18.1156, "fx":[127.14434,136.49105,130.13467,109.91507], "fy":[-54.47899,-24.20343,47.77744,84.06938]},
+ {"t":0.95977, "x":0.79348, "y":5.00633, "heading":1.56668, "vx":-2.53226, "vy":-0.10637, "omega":-0.3346, "ax":8.87917, "ay":3.81571, "alpha":6.49651, "fx":[132.1426,135.3316,123.47527,112.49076], "fy":[41.80691,30.30687,63.18737,81.04574]},
+ {"t":1.01309, "x":0.67108, "y":5.00608, "heading":1.55807, "vx":-2.05882, "vy":0.09708, "omega":0.0118, "ax":8.40894, "ay":4.97989, "alpha":2.28518, "fx":[120.73067,124.25699,118.167,113.62443], "fy":[68.32124,61.71602,72.71624,79.6014]},
+ {"t":1.06642, "x":0.57326, "y":5.01834, "heading":1.56195, "vx":-1.61044, "vy":0.36262, "omega":0.13365, "ax":8.05738, "ay":5.56164, "alpha":-0.00406, "fx":[114.20957,114.20076,114.21325,114.22207], "fy":[78.83764,78.85038,78.83225,78.81952]},
+ {"t":1.11974, "x":0.49884, "y":5.04558, "heading":1.56907, "vx":-1.18082, "vy":0.65917, "omega":0.13343, "ax":7.80671, "ay":5.90197, "alpha":-1.42327, "fx":[110.20127,106.5791,111.29071,114.56219], "fy":[84.40074,88.92219,82.94147,78.37194]},
+ {"t":1.17306, "x":0.44698, "y":5.08911, "heading":1.57416, "vx":-0.76456, "vy":0.97386, "omega":0.05754, "ax":7.52246, "ay":6.25086, "alpha":-1.87787, "fx":[106.23561,100.88279,107.30112,112.09679], "fy":[89.30617,95.29892,87.99616,81.81676]},
+ {"t":1.22417, "x":0.41772, "y":5.14706, "heading":1.57465, "vx":-0.38005, "vy":1.29337, "omega":-0.03845, "ax":6.22315, "ay":7.53338, "alpha":-1.44343, "fx":[88.72607,82.9939,87.75398,93.37298], "fy":[106.48413,110.99345,107.24597,102.41205]},
+ {"t":1.27528, "x":0.40643, "y":5.22301, "heading":1.5708, "vx":-0.06196, "vy":1.67843, "omega":-0.11223, "ax":3.11314, "ay":6.23746, "alpha":4.28408, "fx":[37.28789,56.63264,50.19229,32.39912], "fy":[86.63982,80.30868,90.73866,95.97078]},
+ {"t":1.3264, "x":0.40733, "y":5.31695, "heading":1.57066, "vx":0.09716, "vy":1.99725, "omega":0.10675, "ax":-0.81186, "ay":0.0311, "alpha":-3.29299, "fx":[-6.07626,-16.93227,-16.94207,-6.08087], "fy":[5.98364,5.90732,-5.02985,-5.09778]},
+ {"t":1.37751, "x":0.41123, "y":5.41908, "heading":1.57181, "vx":0.05566, "vy":1.99884, "omega":-0.06157, "ax":-0.27712, "ay":0.00673, "alpha":1.36883, "fx":[-6.19659,-1.65512,-1.65968,-6.20093], "fy":[-2.17919,-2.17862,2.37426,2.36522]},
+ {"t":1.42863, "x":0.41371, "y":5.52125, "heading":1.57045, "vx":0.0415, "vy":1.99919, "omega":0.0084, "ax":-0.19159, "ay":0.00351, "alpha":-0.08255, "fx":[-2.5787,-2.8527,-2.8528,-2.5788], "fy":[0.18675,0.18683,-0.08737,-0.08747]},
+ {"t":1.47974, "x":0.41559, "y":5.62345, "heading":1.57078, "vx":0.03171, "vy":1.99937, "omega":0.00418, "ax":-0.1585, "ay":0.00219, "alpha":-0.0832, "fx":[-2.10859,-2.38478,-2.38478,-2.1086], "fy":[0.16922,0.16921,-0.10711,-0.10712]},
+ {"t":1.53086, "x":0.417, "y":5.72564, "heading":1.57088, "vx":0.0236, "vy":1.99948, "omega":-0.00007, "ax":-0.141, "ay":0.00141, "alpha":-0.00608, "fx":[-1.9886,-2.00877,-2.00877,-1.9886], "fy":[0.03006,0.03006,0.00988,0.00988]},
+ {"t":1.58197, "x":0.41802, "y":5.82785, "heading":1.57087, "vx":0.0164, "vy":1.99955, "omega":-0.00038, "ax":-0.13111, "ay":0.00085, "alpha":0.00537, "fx":[-1.86741,-1.84958,-1.84958,-1.86741], "fy":[0.00319,0.00319,0.02103,0.02102]},
+ {"t":1.63308, "x":0.41869, "y":5.93005, "heading":1.57086, "vx":0.0097, "vy":1.99959, "omega":-0.00011, "ax":-0.12595, "ay":0.00041, "alpha":0.0013, "fx":[-1.78745,-1.78314,-1.78314,-1.78745], "fy":[0.00361,0.00361,0.00792,0.00792]},
+ {"t":1.6842, "x":0.41902, "y":6.03226, "heading":1.57085, "vx":0.00326, "vy":1.99962, "omega":-0.00004, "ax":-0.12434, "ay":0.0, "alpha":-0.00077, "fx":[-1.76117,-1.76373,-1.76373,-1.76117], "fy":[0.00134,0.00134,-0.00122,-0.00122]},
+ {"t":1.73531, "x":0.41902, "y":6.13447, "heading":1.57085, "vx":-0.0031, "vy":1.99962, "omega":-0.00008, "ax":-0.12595, "ay":-0.0004, "alpha":-0.001, "fx":[-1.78361,-1.78695,-1.78695,-1.78361], "fy":[-0.00398,-0.00398,-0.00732,-0.00732]},
+ {"t":1.78643, "x":0.4187, "y":6.23668, "heading":1.57084, "vx":-0.00954, "vy":1.9996, "omega":-0.00013, "ax":-0.1311, "ay":-0.00085, "alpha":-0.00104, "fx":[-1.8566,-1.86007,-1.86007,-1.8566], "fy":[-0.01025,-0.01025,-0.01372,-0.01372]},
+ {"t":1.83754, "x":0.41804, "y":6.33888, "heading":1.57084, "vx":-0.01624, "vy":1.99955, "omega":-0.00019, "ax":-0.14096, "ay":-0.0014, "alpha":0.0002, "fx":[-1.99837,-1.99772,-1.99772,-1.99837], "fy":[-0.02015,-0.02015,-0.0195,-0.0195]},
+ {"t":1.88865, "x":0.41703, "y":6.44109, "heading":1.57083, "vx":-0.02344, "vy":1.99948, "omega":-0.00018, "ax":-0.15836, "ay":-0.00218, "alpha":0.00682, "fx":[-2.256,-2.23335,-2.23336,-2.256], "fy":[-0.04219,-0.04219,-0.01953,-0.01954]},
+ {"t":1.93977, "x":0.41562, "y":6.54329, "heading":1.57083, "vx":-0.03154, "vy":1.99937, "omega":0.00017, "ax":-0.19118, "ay":-0.00348, "alpha":0.00595, "fx":[-2.71983,-2.70009,-2.70009,-2.71983], "fy":[-0.05924,-0.05924,-0.03949,-0.03949]},
+ {"t":1.99088, "x":0.41376, "y":6.64548, "heading":1.57084, "vx":-0.04131, "vy":1.99919, "omega":0.00048, "ax":-0.27885, "ay":-0.00676, "alpha":-0.11544, "fx":[-3.76116,-4.14416,-4.14413,-3.76113], "fy":[0.09607,0.09604,-0.28756,-0.28758]},
+ {"t":2.042, "x":0.41128, "y":6.74766, "heading":1.57072, "vx":-0.05556, "vy":1.99885, "omega":-0.00543, "ax":-0.99706, "ay":-0.04048, "alpha":0.27888, "fx":[-14.59095,-13.67457,-13.67525,-14.59168], "fy":[-1.04089,-1.04238,-0.1062,-0.10598]},
+ {"t":2.09311, "x":0.40714, "y":6.84977, "heading":1.5708, "vx":-0.10653, "vy":1.99678, "omega":0.00883, "ax":3.62199, "ay":-5.4403, "alpha":-0.35116, "fx":[52.2645,50.8956,50.42002,51.78326], "fy":[-76.39159,-76.8724,-77.83598,-77.35981]},
+ {"t":2.14422, "x":0.40643, "y":6.94473, "heading":1.5708, "vx":0.07861, "vy":1.7187, "omega":-0.00912, "ax":5.89065, "ay":-7.80221, "alpha":0.16562, "fx":[82.88539,83.58551,84.11155,83.41204], "fy":[-111.05651,-110.53252,-110.1301,-110.65882]},
+ {"t":2.19534, "x":0.41814, "y":7.02239, "heading":1.57055, "vx":0.3797, "vy":1.3199, "omega":-0.00065, "ax":7.21489, "ay":-6.61706, "alpha":0.21709, "fx":[101.58004,102.24177,102.95541,102.30002], "fy":[-94.54396,-93.82954,-93.04446,-93.76293]},
+ {"t":2.24645, "x":0.44698, "y":7.08121, "heading":1.5708, "vx":0.74849, "vy":0.98167, "omega":0.01044, "ax":7.34977, "ay":-6.46503, "alpha":0.23492, "fx":[103.45297,104.13769,104.90556,104.22881], "fy":[-92.46478,-91.69495,-90.81366,-91.58752]},
+ {"t":2.28554, "x":0.48185, "y":7.11464, "heading":1.57138, "vx":1.03579, "vy":0.72896, "omega":0.01962, "ax":7.17727, "ay":-6.6514, "alpha":0.26285, "fx":[100.89665,101.70837,102.57105,101.76841], "fy":[-95.18374,-94.31861,-93.37717,-94.2485]},
+ {"t":2.32463, "x":0.52782, "y":7.13805, "heading":1.57235, "vx":1.31635, "vy":0.46895, "omega":0.0299, "ax":6.86447, "ay":-6.96608, "alpha":0.31138, "fx":[96.26039,97.31617,98.33897,97.29352], "fy":[-99.76383,-98.73827,-97.71549,-98.7523]},
+ {"t":2.36372, "x":0.58452, "y":7.15106, "heading":1.57376, "vx":1.58468, "vy":0.19665, "omega":0.04207, "ax":6.13493, "ay":-7.6012, "alpha":0.41522, "fx":[85.45644,87.13195,88.46222,86.79409], "fy":[-108.9521,-107.62379,-106.52427,-107.88051]},
+ {"t":2.40281, "x":0.65115, "y":7.15294, "heading":1.57572, "vx":1.8245, "vy":-0.10048, "omega":0.0583, "ax":3.0847, "ay":-9.22639, "alpha":0.75029, "fx":[40.79794,45.19861,46.711,42.19226], "fy":[-131.75972,-130.32684,-129.75725,-131.28389]},
+ {"t":2.4419, "x":0.72483, "y":7.14197, "heading":1.57857, "vx":1.94508, "vy":-0.46114, "omega":0.08763, "ax":-3.06363, "ay":-9.08112, "alpha":1.71796, "fx":[-46.60549,-36.87804,-39.95566,-50.26565], "fy":[-127.88541,-130.94961,-129.79773,-126.25811]},
+ {"t":2.48099, "x":0.79852, "y":7.117, "heading":1.58331, "vx":1.82532, "vy":-0.81612, "omega":0.15479, "ax":-4.11511, "ay":-7.46508, "alpha":6.88484, "fx":[-66.18996,-35.90089,-48.69288,-82.53915], "fy":[-106.69293,-118.59899,-107.92968,-90.0415]},
+ {"t":2.52008, "x":0.86673, "y":7.0794, "heading":1.59462, "vx":1.66446, "vy":-1.10793, "omega":0.42391, "ax":-1.06814, "ay":-1.54267, "alpha":25.01737, "fx":[-51.86935,25.06536,28.81985,-62.57857], "fy":[-60.41863,-65.23792,21.87559,16.31307]},
+ {"t":2.55917, "x":0.93098, "y":7.03491, "heading":1.63031, "vx":1.62271, "vy":-1.16824, "omega":1.40184, "ax":-0.08257, "ay":-0.11435, "alpha":16.07957, "fx":[-26.11903,26.96236,23.99525,-29.52023], "fy":[-29.75101,-26.78354,26.70905,23.34177]},
+ {"t":2.59826, "x":0.99435, "y":6.98915, "heading":1.69739, "vx":1.61948, "vy":-1.17271, "omega":2.0304, "ax":-0.00465, "ay":-0.00641, "alpha":7.75871, "fx":[-11.21598,14.33716,11.08674,-14.47131], "fy":[-14.49356,-11.24369,14.31487,11.05906]},
+ {"t":2.63735, "x":1.05765, "y":6.94331, "heading":1.78269, "vx":1.6193, "vy":-1.17296, "omega":2.33368, "ax":-0.00017, "ay":-0.00023, "alpha":3.32661, "fx":[-4.24009,6.5584,4.23531,-6.5632], "fy":[-6.56403,-4.24095,6.55757,4.23445]},
+ {"t":2.67644, "x":1.12095, "y":6.89746, "heading":1.87645, "vx":1.61929, "vy":-1.17297, "omega":2.46372, "ax":0.00007, "ay":0.0001, "alpha":1.48453, "fx":[-1.60764,3.09305,1.60971,-3.09098], "fy":[-3.09053,-1.6072,3.09349,1.61016]},
+ {"t":2.71553, "x":1.18425, "y":6.85161, "heading":1.97389, "vx":1.61929, "vy":-1.17296, "omega":2.52175, "ax":0.00016, "ay":0.00023, "alpha":0.8057, "fx":[-0.70335,1.75745,0.70801,-1.75278], "fy":[-1.75185,-0.70242,1.75838,0.70894]},
+ {"t":2.75462, "x":1.24754, "y":6.80575, "heading":2.07308, "vx":1.6193, "vy":-1.17295, "omega":2.55325, "ax":0.00028, "ay":0.00039, "alpha":0.5548, "fx":[-0.35991,1.2547,0.36781,-1.2468], "fy":[-1.24526,-0.35837,1.25624,0.36935]},
+ {"t":2.79371, "x":1.31084, "y":6.7599, "heading":2.17331, "vx":1.61931, "vy":-1.17294, "omega":2.57493, "ax":0.0004, "ay":0.00056, "alpha":0.42481, "fx":[-0.17567,0.98647,0.18709,-0.97505], "fy":[-0.97285,-0.17347,0.98867,0.18929]},
+ {"t":2.8328, "x":1.37414, "y":6.71405, "heading":2.27429, "vx":1.61933, "vy":-1.17292, "omega":2.59154, "ax":0.00052, "ay":0.00071, "alpha":0.29911, "fx":[-0.05015,0.70722,0.06475,-0.69262], "fy":[-0.68981,-0.04734,0.71002,0.06755]},
+ {"t":2.87189, "x":1.43744, "y":6.66821, "heading":2.37582, "vx":1.61935, "vy":-1.17289, "omega":2.60323, "ax":0.00059, "ay":0.00082, "alpha":0.14531, "fx":[0.01507,0.34947,0.00167,-0.33274], "fy":[-0.32953,0.01828,0.35268,0.00488]},
+ {"t":2.91098, "x":1.50074, "y":6.62236, "heading":2.47769, "vx":1.61937, "vy":-1.17286, "omega":2.60891, "ax":0.00061, "ay":0.00084, "alpha":-0.02659, "fx":[0.00102,-0.05338,0.01615,0.07056], "fy":[0.07385,0.00432,-0.05009,0.01945]},
+ {"t":2.95007, "x":1.56405, "y":6.57651, "heading":2.57966, "vx":1.61939, "vy":-1.17282, "omega":2.60787, "ax":0.00056, "ay":0.00077, "alpha":-0.18696, "fx":[-0.0894,-0.42016,0.10516,0.43592], "fy":[0.43895,-0.08637,-0.41714,0.10818]},
+ {"t":2.98916, "x":1.62735, "y":6.53067, "heading":2.68146, "vx":1.61942, "vy":-1.17279, "omega":2.60056, "ax":0.00045, "ay":0.00063, "alpha":-0.30518, "fx":[-0.22253,-0.6725,0.23542,0.68539], "fy":[0.68787,-0.22005,-0.67002,0.2379]},
+ {"t":3.02825, "x":1.69065, "y":6.48482, "heading":2.78288, "vx":1.61943, "vy":-1.17277, "omega":2.58863, "ax":0.00033, "ay":0.00046, "alpha":-0.36298, "fx":[-0.34804,-0.77116,0.35736,0.78048], "fy":[0.78228,-0.34624,-0.76935,0.35916]},
+ {"t":3.06734, "x":1.75395, "y":6.43898, "heading":2.88379, "vx":1.61945, "vy":-1.17275, "omega":2.57445, "ax":0.0002, "ay":0.00028, "alpha":-0.3582, "fx":[-0.42051,-0.72374,0.42631,0.72953], "fy":[0.73066,-0.41939,-0.72261,0.42744]},
+ {"t":3.10643, "x":1.81726, "y":6.39314, "heading":2.98415, "vx":1.61945, "vy":-1.17274, "omega":2.56044, "ax":0.0001, "ay":0.00014, "alpha":-0.30058, "fx":[-0.41321,-0.56968,0.41602,0.5725], "fy":[0.57306,-0.41265,-0.56912,0.41658]},
+ {"t":3.14552, "x":1.88056, "y":6.34729, "heading":3.08401, "vx":1.61946, "vy":-1.17274, "omega":2.54869, "ax":0.00002, "ay":0.00003, "alpha":-0.20573, "fx":[-0.32102,-0.36033,0.32165,0.36095], "fy":[0.3611,-0.32088,-0.36019,0.32179]},
+ {"t":3.18461, "x":1.94387, "y":6.30145, "heading":-3.0997, "vx":1.61946, "vy":-1.17273, "omega":2.54065, "ax":-0.00002, "ay":-0.00003, "alpha":-0.09088, "fx":[-0.1574,-0.14477,0.15671,0.14407], "fy":[0.14396,-0.15752,-0.14488,0.1566]},
+ {"t":3.2237, "x":2.00717, "y":6.25561, "heading":-3.00046, "vx":1.61946, "vy":-1.17274, "omega":2.5371, "ax":-0.00004, "ay":-0.00005, "alpha":0.02467, "fx":[0.04576,0.03424,-0.04687,-0.03535], "fy":[-0.03554,0.04557,0.03404,-0.04706]},
+ {"t":3.26279, "x":2.07048, "y":6.20977, "heading":-2.90126, "vx":1.61946, "vy":-1.17274, "omega":2.53806, "ax":-0.00002, "ay":-0.00003, "alpha":0.11414, "fx":[0.22887,0.13866,-0.22945,-0.13924], "fy":[-0.13933,0.22878,0.13857,-0.22954]},
+ {"t":3.30188, "x":2.13378, "y":6.16393, "heading":-2.80196, "vx":1.61946, "vy":-1.17274, "omega":2.54253, "ax":0.00004, "ay":0.00005, "alpha":0.12859, "fx":[0.27292,0.13067,-0.27192,-0.12967], "fy":[-0.12947,0.27312,0.13088,-0.27171]},
+ {"t":3.34097, "x":2.19709, "y":6.11808, "heading":-2.70248, "vx":1.61946, "vy":-1.17274, "omega":2.54755, "ax":0.00013, "ay":0.00018, "alpha":-0.03626, "fx":[-0.07821,-0.02703,0.08194,0.03076], "fy":[0.03148,-0.07749,-0.0263,0.08266]},
+ {"t":3.38006, "x":2.26039, "y":6.07224, "heading":-2.60292, "vx":1.61946, "vy":-1.17273, "omega":2.54614, "ax":0.00026, "ay":0.00036, "alpha":-0.61523, "fx":[-1.397,-0.34905,1.40445,0.35651], "fy":[0.35793,-1.39557,-0.34763,1.40587]},
+ {"t":3.41915, "x":2.3237, "y":6.0264, "heading":-2.50386, "vx":1.61947, "vy":-1.17272, "omega":2.52209, "ax":0.00039, "ay":0.00054, "alpha":-2.12619, "fx":[-4.93216,-0.72897,4.94316,0.73999], "fy":[0.74209,-4.93007,-0.72687,4.94525]},
+ {"t":3.45824, "x":2.387, "y":5.98056, "heading":-2.4069, "vx":1.61949, "vy":-1.17269, "omega":2.43897, "ax":0.0005, "ay":0.00069, "alpha":-5.59204, "fx":[-13.10537,-0.65825,13.11949,0.67258], "fy":[0.67531,-13.10273,-0.65553,13.12214]},
+ {"t":3.49733, "x":2.45031, "y":5.93472, "heading":-2.31583, "vx":1.61951, "vy":-1.17267, "omega":2.22038, "ax":0.00231, "ay":0.00318, "alpha":-12.3891, "fx":[-29.03302,1.20803,29.09523,-1.13947], "fy":[-1.12676,-29.02086,1.22072,29.10739]},
+ {"t":3.53642, "x":2.51362, "y":5.88888, "heading":-2.2385, "vx":1.6196, "vy":-1.17254, "omega":1.73609, "ax":0.03095, "ay":0.04279, "alpha":-22.3561, "fx":[-51.76851,6.6847,52.48362,-5.6452], "fy":[-5.48539,-51.58858,6.83766,52.66243]},
+ {"t":3.57551, "x":2.57695, "y":5.84308, "heading":-2.18772, "vx":1.62081, "vy":-1.17087, "omega":0.86219, "ax":0.33921, "ay":0.47527, "alpha":-32.12387, "fx":[-71.60017,19.7921,77.22358,-6.18273], "fy":[-4.16178,-68.9673,20.45007,79.62625]},
+ {"t":3.6146, "x":2.64057, "y":5.79767, "heading":-2.17856, "vx":1.63407, "vy":-1.15229, "omega":-0.39353, "ax":2.63941, "ay":4.15424, "alpha":-28.20381, "fx":[-45.8068,81.19469,92.26408,22.00024], "fy":[73.63821,-11.91383,63.5001,110.3167]},
+ {"t":3.65369, "x":2.70646, "y":5.7558, "heading":-2.21549, "vx":1.73724, "vy":-0.9899, "omega":-1.49602, "ax":3.85612, "ay":8.48446, "alpha":-4.75658, "fx":[36.72466,66.2059,69.70759,46.0003], "fy":[127.33177,114.35048,113.84565,125.53274]},
+ {"t":3.69278, "x":2.77731, "y":5.72359, "heading":-2.2776, "vx":1.88798, "vy":-0.65824, "omega":-1.68196, "ax":2.31523, "ay":9.38207, "alpha":-1.87871, "fx":[24.60854,35.64834,40.65952,30.35502], "fy":[134.85832,132.28515,130.99092,133.82026]},
+ {"t":3.73187, "x":2.85288, "y":5.70503, "heading":-2.34479, "vx":1.97848, "vy":-0.2915, "omega":-1.7554, "ax":9.75226, "ay":0.8071, "alpha":0.43718, "fx":[138.25818,138.06033,138.22835,138.39715], "fy":[11.29809,13.4755,11.58824,9.39981]},
+ {"t":3.79073, "x":2.98621, "y":5.68927, "heading":-2.44734, "vx":2.55243, "vy":-0.244, "omega":-1.72967, "ax":9.72458, "ay":0.84202, "alpha":2.68797, "fx":[138.31319,136.5485,137.87092,138.64182], "fy":[9.91914,24.05837,14.39469,-0.63075]},
+ {"t":3.84958, "x":3.15327, "y":5.67637, "heading":-2.54448, "vx":3.12474, "vy":-0.19444, "omega":-1.57147, "ax":9.58092, "ay":0.91373, "alpha":7.22059, "fx":[138.47622,131.86317,136.05142,136.83844], "fy":[5.35916,42.4781,25.25595,-21.28555]},
+ {"t":3.90843, "x":3.35376, "y":5.66651, "heading":-2.62446, "vx":3.68861, "vy":-0.14067, "omega":-1.14652, "ax":9.2545, "ay":1.06875, "alpha":12.3649, "fx":[138.30305,125.79163,129.47296,131.15369], "fy":[-0.26938,57.2674,46.53983,-42.94058]},
+ {"t":3.96728, "x":3.58687, "y":5.66008, "heading":-2.67052, "vx":4.23326, "vy":-0.07777, "omega":-0.41881, "ax":5.02531, "ay":5.70642, "alpha":7.10972, "fx":[89.76376,68.64258,49.50248,77.02146], "fy":[67.79404,91.75219,95.81383,68.18847]},
+ {"t":4.02614, "x":3.84472, "y":5.66538, "heading":-2.68286, "vx":4.52901, "vy":0.25807, "omega":-0.00039, "ax":-0.06737, "ay":1.06743, "alpha":0.00038, "fx":[-0.95416,-0.95473,-0.95588,-0.95532], "fy":[15.13028,15.1314,15.13085,15.12972]},
+ {"t":4.08499, "x":4.11114, "y":5.68242, "heading":-2.68288, "vx":4.52505, "vy":0.32089, "omega":-0.00036, "ax":-0.00445, "ay":0.06239, "alpha":0.0, "fx":[-0.06309,-0.06309,-0.06309,-0.06309], "fy":[0.88432,0.88432,0.88432,0.88432]},
+ {"t":4.14384, "x":4.37745, "y":5.70141, "heading":-2.6829, "vx":4.52478, "vy":0.32456, "omega":-0.00036, "ax":0.01918, "ay":-0.27428, "alpha":0.0, "fx":[0.2718,0.27181,0.27181,0.27181], "fy":[-3.88787,-3.88788,-3.88788,-3.88787]},
+ {"t":4.20269, "x":4.64378, "y":5.72004, "heading":-2.68292, "vx":4.52591, "vy":0.30842, "omega":-0.00036, "ax":0.15648, "ay":-3.42755, "alpha":-0.00019, "fx":[2.21753,2.21783,2.21847,2.21816], "fy":[-48.58453,-48.58502,-48.58477,-48.58428]},
+ {"t":4.26155, "x":4.91041, "y":5.73226, "heading":-2.68295, "vx":4.53512, "vy":0.1067, "omega":-0.00038, "ax":-1.27205, "ay":-8.76855, "alpha":-1.0877, "fx":[-22.55904,-18.90937,-13.54942,-17.10622], "fy":[-123.52019,-124.65597,-124.99969,-123.99236]},
+ {"t":4.3204, "x":5.17511, "y":5.72335, "heading":-2.68485, "vx":4.46026, "vy":-0.40935, "omega":-0.06439, "ax":-9.63935, "ay":-1.40457, "alpha":-0.82554, "fx":[-136.91822,-136.14324,-136.37074,-137.10969], "fy":[-18.21841,-23.259,-21.69064,-16.46955]},
+ {"t":4.37925, "x":5.42091, "y":5.69683, "heading":-2.69007, "vx":3.89296, "vy":-0.49202, "omega":-0.11298, "ax":-9.69706, "ay":-1.04786, "alpha":2.60596, "fx":[-136.96717,-138.46505,-138.18444,-136.19726], "fy":[-20.47861,-3.49212,-10.0825,-25.35957]},
+ {"t":4.43811, "x":5.63323, "y":5.66605, "heading":-2.69221, "vx":3.32226, "vy":-0.55369, "omega":0.04039, "ax":-9.69859, "ay":-0.93193, "alpha":3.71829, "fx":[-136.96622,-138.60465,-138.5181,-135.81165], "fy":[-21.37214,3.29426,-6.75056,-28.01119]},
+ {"t":4.49696, "x":5.81196, "y":5.63185, "heading":-2.68339, "vx":2.75147, "vy":-0.60853, "omega":0.25922, "ax":-9.69628, "ay":-0.87494, "alpha":4.26535, "fx":[-136.99295,-138.54725,-138.64115,-135.58847], "fy":[-21.64928,6.74985,-5.33716,-29.37179]},
+ {"t":4.55581, "x":5.9571, "y":5.59453, "heading":-2.66075, "vx":2.18082, "vy":-0.66003, "omega":0.51025, "ax":-9.69405, "ay":-0.84118, "alpha":4.58519, "fx":[-137.0726,-138.46589,-138.69501,-135.40999], "fy":[-21.41473,8.91519,-4.842,-30.35228]},
+ {"t":4.61466, "x":6.06866, "y":5.55422, "heading":-2.62278, "vx":1.6103, "vy":-0.70953, "omega":0.7801, "ax":-9.69237, "ay":-0.81887, "alpha":4.78816, "fx":[-137.21582,-138.38589,-138.71353,-135.23296], "fy":[-20.66821,10.45521,-4.97858,-31.23774]},
+ {"t":4.67352, "x":6.14664, "y":5.51105, "heading":-2.56857, "vx":1.03988, "vy":-0.75772, "omega":1.0619, "ax":-9.69124, "ay":-0.80299, "alpha":4.92151, "fx":[-137.42456,-138.31323,-138.70459,-135.04145], "fy":[-19.37731,11.61879,-5.6427,-32.12734]},
+ {"t":4.73237, "x":6.19106, "y":5.46506, "heading":-2.49756, "vx":0.46952, "vy":-0.80498, "omega":1.35154, "ax":-9.69054, "ay":-0.79092, "alpha":5.01069, "fx":[-137.69187,-138.25331,-138.66531,-134.83401], "fy":[-17.4979,12.48333,-6.79216,-33.03789]},
+ {"t":4.79122, "x":6.20191, "y":5.41632, "heading":-2.40934, "vx":-0.10079, "vy":-0.85153, "omega":1.64643, "ax":-9.64948, "ay":-1.15067, "alpha":5.0191, "fx":[-137.08472,-138.55027,-138.17032,-133.31123], "fy":[-21.2419,7.45403,-12.95278,-38.50122]},
+ {"t":4.82707, "x":6.1921, "y":5.38506, "heading":-2.34709, "vx":-0.44669, "vy":-0.89278, "omega":1.82635, "ax":-9.48493, "ay":-2.11518, "alpha":4.93184, "fx":[-133.97795,-138.53701,-136.27154,-128.99978], "fy":[-35.80647,-7.00499,-26.08525,-51.03202]},
+ {"t":4.86291, "x":6.16999, "y":5.35169, "heading":-2.27846, "vx":-0.78668, "vy":-0.9686, "omega":2.00314, "ax":-9.04986, "ay":-3.54815, "alpha":4.71165, "fx":[-126.1014,-135.47986,-131.07687,-120.4603], "fy":[-57.57972,-29.52705,-45.35309,-68.71713]},
+ {"t":4.89876, "x":6.13598, "y":5.31469, "heading":-2.20363, "vx":-1.11109, "vy":-1.09578, "omega":2.17203, "ax":-7.91057, "ay":-5.66635, "alpha":4.12491, "fx":[-106.61328,-122.25653,-117.03262,-102.61907], "fy":[-88.50007,-65.26038,-74.32016,-93.19589]},
+ {"t":4.9346, "x":6.09107, "y":5.27177, "heading":-2.12312, "vx":-1.39465, "vy":-1.2989, "omega":2.31989, "ax":-5.19093, "ay":-8.25757, "alpha":2.668, "fx":[-65.69803,-81.53019,-80.52939,-66.56302], "fy":[-121.94016,-111.9759,-112.75604,-121.52437]},
+ {"t":4.97045, "x":6.03774, "y":5.21991, "heading":-2.03824, "vx":-1.58072, "vy":-1.5949, "omega":2.41553, "ax":-0.79612, "ay":-9.74042, "alpha":0.3709, "fx":[-9.68182,-11.96587,-12.87157,-10.61977], "fy":[-138.19343,-138.01183,-137.93661,-138.13069]},
+ {"t":5.0063, "x":5.98057, "y":5.15648, "heading":-1.95142, "vx":-1.60926, "vy":-1.94406, "omega":2.42882, "ax":1.68444, "ay":-9.61193, "alpha":-1.54364, "fx":[17.02494,25.4489,30.8646,22.16784], "fy":[-137.37324,-136.08249,-134.92727,-136.60433]},
+ {"t":5.03307, "x":5.93808, "y":5.10098, "heading":-1.88694, "vx":-1.56416, "vy":-2.20141, "omega":2.38749, "ax":2.51672, "ay":-9.39719, "alpha":-3.41943, "fx":[20.85285,38.67616,50.99974,32.16681], "fy":[-136.79796,-132.90509,-128.60953,-134.49891]},
+ {"t":5.05985, "x":5.8971, "y":5.03867, "heading":-1.82424, "vx":-1.49677, "vy":-2.45302, "omega":2.29594, "ax":3.49203, "ay":-9.02176, "alpha":-5.41989, "fx":[26.62097,53.23955,72.99319,45.14083], "fy":[-135.7409,-127.74426,-117.45891,-130.58097]},
+ {"t":5.08662, "x":5.85828, "y":4.96976, "heading":-1.76471, "vx":-1.40328, "vy":-2.69457, "omega":2.15082, "ax":4.57643, "ay":-8.46258, "alpha":-6.80989, "fx":[36.93782,68.06233,92.36745,62.1116], "fy":[-133.24115,-120.46568,-102.84993,-123.26339]},
+ {"t":5.1134, "x":5.82235, "y":4.89458, "heading":-1.70957, "vx":-1.28074, "vy":-2.92115, "omega":1.96849, "ax":5.71568, "ay":-7.74232, "alpha":-6.62484, "fx":[55.18626,82.37302,104.92718,81.5869], "fy":[-126.6985,-111.10847,-89.92997,-111.24532]},
+ {"t":5.14017, "x":5.79011, "y":4.81359, "heading":-1.65924, "vx":-1.12771, "vy":-3.12845, "omega":1.79111, "ax":6.88141, "ay":-6.77393, "alpha":-5.60011, "fx":[78.04291,96.99571,114.65678,100.47398], "fy":[-113.96709,-98.5182,-77.07523,-94.51495]},
+ {"t":5.16694, "x":5.76238, "y":4.7274, "heading":-1.61329, "vx":-0.94346, "vy":-3.30982, "omega":1.64117, "ax":7.9692, "ay":-5.52421, "alpha":-4.04831, "fx":[101.62487,111.43113,122.72797,116.06229], "fy":[-93.48328,-81.73868,-63.38591,-74.60955]},
+ {"t":5.19372, "x":5.73997, "y":4.6368, "heading":-1.5708, "vx":-0.73009, "vy":-3.45773, "omega":1.53278, "ax":8.64876, "ay":-4.42611, "alpha":-3.49166, "fx":[114.87831,120.96551,129.12213,125.4107], "fy":[-76.78554,-66.98059,-49.34116,-57.84918]},
+ {"t":5.22777, "x":5.72013, "y":4.51651, "heading":-1.52063, "vx":-0.43562, "vy":-3.60843, "omega":1.4139, "ax":9.08196, "ay":-3.4289, "alpha":-3.24569, "fx":[123.21865,127.34305,133.30361,131.07343], "fy":[-62.11669,-53.44815,-35.9281,-42.92225]},
+ {"t":5.26181, "x":5.71056, "y":4.39167, "heading":-1.47437, "vx":-0.12641, "vy":-3.72517, "omega":1.30339, "ax":9.52307, "ay":-1.88562, "alpha":-2.09469, "fx":[133.1228,134.31229,136.56038,135.95358], "fy":[-35.38503,-31.02274,-18.59373,-21.91147]},
+ {"t":5.29586, "x":5.71178, "y":4.26374, "heading":-1.43121, "vx":0.19783, "vy":-3.78937, "omega":1.23207, "ax":9.68515, "ay":0.51862, "alpha":0.42996, "fx":[137.21594,137.20057,137.35269,137.36985], "fy":[8.86476,8.64904,5.80938,6.0819]},
+ {"t":5.32991, "x":5.72413, "y":4.13502, "heading":-1.38901, "vx":0.52758, "vy":-3.77171, "omega":1.24671, "ax":8.86581, "ay":3.75486, "alpha":4.42277, "fx":[122.46084,117.93101,130.47671,131.81425], "fy":[62.38569,69.96312,41.83922,38.7091]},
+ {"t":5.36396, "x":5.74723, "y":4.00878, "heading":-1.344, "vx":0.82944, "vy":-3.64387, "omega":1.3973, "ax":6.79416, "ay":6.70846, "alpha":7.00123, "fx":[93.41456,71.60962,103.93603,116.26218], "fy":[101.03739,117.09346,88.97283,73.25967]},
+ {"t":5.398, "x":5.77941, "y":3.88861, "heading":-1.29237, "vx":1.06076, "vy":-3.41546, "omega":1.63567, "ax":4.85198, "ay":8.13535, "alpha":9.05427, "fx":[69.72198,30.62924,71.80247,102.9491], "fy":[118.99661,134.20086,116.66047,91.40877]},
+ {"t":5.43205, "x":5.81834, "y":3.77704, "heading":-1.23143, "vx":1.22596, "vy":-3.13848, "omega":1.94394, "ax":3.46975, "ay":8.77312, "alpha":10.07553, "fx":[53.29099,5.60724,45.76365,92.06976], "fy":[127.45648,137.84273,129.44636,102.6819]},
+ {"t":5.4661, "x":5.86209, "y":3.67526, "heading":-1.15941, "vx":1.3441, "vy":-2.83978, "omega":2.28699, "ax":2.57627, "ay":9.06803, "alpha":10.44716, "fx":[41.51859,-9.12797,30.38021,83.30139], "fy":[131.92957,137.85731,134.2008,110.16122]},
+ {"t":5.50015, "x":5.90934, "y":3.58383, "heading":-1.07549, "vx":1.43181, "vy":-2.53103, "omega":2.64269, "ax":2.34495, "ay":9.18614, "alpha":9.61894, "fx":[35.65932,-10.41761,31.40995,76.30488], "fy":[133.67655,137.81498,134.13962,115.21414]},
+ {"t":5.54067, "x":5.96929, "y":3.48881, "heading":-0.96049, "vx":1.52684, "vy":-2.15877, "omega":3.03249, "ax":2.68928, "ay":9.17202, "alpha":7.51942, "fx":[35.73431,3.20354,43.62363,69.91834], "fy":[133.29082,137.72986,130.2486,118.77539]},
+ {"t":5.58119, "x":6.03337, "y":3.40886, "heading":-0.83143, "vx":1.63582, "vy":-1.78708, "omega":3.33721, "ax":3.12952, "ay":9.05818, "alpha":4.79963, "fx":[39.54511,23.15773,51.45457,63.28329], "fy":[131.22112,134.71066,126.28951,121.36885]},
+ {"t":5.62172, "x":6.10224, "y":3.34387, "heading":-0.69225, "vx":1.76264, "vy":-1.42, "omega":3.53171, "ax":5.41749, "ay":7.84455, "alpha":-1.04525, "fx":[79.32859,79.78123,74.39522,73.66154], "fy":[109.32745,109.22603,112.98132,113.24394]},
+ {"t":5.66224, "x":6.17811, "y":3.29277, "heading":-0.54999, "vx":1.98218, "vy":-1.1021, "omega":3.48936, "ax":7.59728, "ay":5.41911, "alpha":-6.21483, "fx":[122.35067,114.5266,93.8094,100.07201], "fy":[53.4037,70.77733,96.02785,87.04966]},
+ {"t":5.70277, "x":6.26468, "y":3.25256, "heading":-0.41369, "vx":2.29006, "vy":-0.8825, "omega":3.2375, "ax":8.19446, "ay":4.17602, "alpha":-8.17121, "fx":[130.8071,121.03361,98.58371,114.19339], "fy":[24.76556,58.27346,89.89768,63.83982]},
+ {"t":5.74329, "x":6.36421, "y":3.22022, "heading":-0.2892, "vx":2.62214, "vy":-0.71327, "omega":2.90637, "ax":7.70833, "ay":4.88626, "alpha":-8.54612, "fx":[127.70821,114.16395,87.71718,107.46577], "fy":[35.20157,69.7894,99.57638,72.47901]},
+ {"t":5.78382, "x":6.4768, "y":3.19533, "heading":-0.17843, "vx":2.93451, "vy":-0.51525, "omega":2.56004, "ax":6.42582, "ay":6.34591, "alpha":-8.08936, "fx":[114.79326,97.69087,66.99709,84.85656], "fy":[62.21936,89.34552,112.87045,95.37157]},
+ {"t":5.82434, "x":6.601, "y":3.17966, "heading":-0.08133, "vx":3.19492, "vy":-0.25809, "omega":2.23223, "ax":5.26262, "ay":6.61405, "alpha":-7.59903, "fx":[98.183,81.99505,51.73057,66.47674], "fy":[72.15175,94.82791,111.91006,96.12055]},
+ {"t":5.86487, "x":6.73479, "y":3.17463, "heading":0.00289, "vx":3.40818, "vy":0.00994, "omega":1.92428, "ax":-0.44877, "ay":6.56414, "alpha":0.26565, "fx":[-6.97129,-7.0271,-5.74563,-5.7006], "fy":[93.26152,92.7731,92.82903,93.31663]},
+ {"t":5.90539, "x":6.87254, "y":3.18043, "heading":0.08109, "vx":3.38999, "vy":0.27595, "omega":1.93504, "ax":-5.86909, "ay":6.16479, "alpha":7.69025, "fx":[-89.04431,-105.39069,-77.96259,-60.37437], "fy":[89.39846,63.66066,88.30521,108.17318]},
+ {"t":5.94591, "x":7.0051, "y":3.19667, "heading":0.16582, "vx":3.15215, "vy":0.52578, "omega":2.24669, "ax":-6.95216, "ay":5.82535, "alpha":8.09155, "fx":[-103.31286,-120.06213,-95.86702,-74.93875], "fy":[83.53307,53.24029,85.30444,108.21375]},
+ {"t":5.98644, "x":7.12713, "y":3.22276, "heading":0.26351, "vx":2.87042, "vy":0.76184, "omega":2.57459, "ax":-8.13279, "ay":4.21817, "alpha":8.5743, "fx":[-118.50006,-130.73926,-116.75271,-95.12928], "fy":[63.02328,25.25974,57.90296,92.98003]},
+ {"t":6.02696, "x":7.23677, "y":3.2571, "heading":0.37488, "vx":2.54084, "vy":0.93278, "omega":2.92206, "ax":-8.63495, "ay":3.23997, "alpha":8.33221, "fx":[-125.56883,-133.38735,-123.94564,-106.69159], "fy":[48.8356,10.71125,43.6171,80.53946]},
+ {"t":6.06749, "x":7.33265, "y":3.29756, "heading":0.50014, "vx":2.19092, "vy":1.06408, "omega":3.25972, "ax":-8.34995, "ay":4.17406, "alpha":6.79776, "fx":[-122.7852,-130.64432,-115.47147,-104.53317], "fy":[56.22901,30.27247,65.58855,84.57536]},
+ {"t":6.10801, "x":7.41458, "y":3.34411, "heading":0.63782, "vx":1.85254, "vy":1.23323, "omega":3.5352, "ax":-6.81358, "ay":6.65541, "alpha":2.42119, "fx":[-101.05252,-102.92702,-91.62145,-90.72226], "fy":[90.22478,87.46304,99.13668,100.53077]},
+ {"t":6.14854, "x":7.48405, "y":3.39955, "heading":0.78307, "vx":1.57642, "vy":1.50294, "omega":3.63331, "ax":-4.59282, "ay":8.43499, "alpha":-3.63541, "fx":[-51.29566,-58.99409,-77.18133,-72.93739], "fy":[126.53606,123.51564,112.94381,115.26035]},
+ {"t":6.18906, "x":7.54417, "y":3.46738, "heading":0.92732, "vx":1.3903, "vy":1.84476, "omega":3.48599, "ax":-3.86338, "ay":8.76162, "alpha":-7.02353, "fx":[-23.66504,-48.85862,-80.92866,-65.59754], "fy":[135.60252,128.98214,111.52802,120.66309]},
+ {"t":6.22959, "x":7.59734, "y":3.54933, "heading":1.06282, "vx":1.23374, "vy":2.19982, "omega":3.20137, "ax":-3.75806, "ay":8.71642, "alpha":-8.88724, "fx":[-12.94432,-50.57892,-88.23044,-61.32468], "fy":[137.14292,128.4166,105.89375,122.75946]},
+ {"t":6.26812, "x":7.64208, "y":3.64056, "heading":1.17958, "vx":1.08894, "vy":2.53567, "omega":2.85893, "ax":-3.45155, "ay":8.70261, "alpha":-10.45446, "fx":[-2.78436,-51.36663,-92.53961,-49.00908], "fy":[137.27613,127.71281,101.41103,127.0297]},
+ {"t":6.30665, "x":7.68148, "y":3.74473, "heading":1.28197, "vx":0.95595, "vy":2.87099, "omega":2.45612, "ax":-2.69194, "ay":8.78932, "alpha":-12.01732, "fx":[8.74545,-48.4235,-90.84028,-22.11211], "fy":[136.26771,128.13417,101.48131,132.46304]},
+ {"t":6.34518, "x":7.71631, "y":3.86187, "heading":1.36769, "vx":0.85223, "vy":3.20965, "omega":1.99308, "ax":-1.38365, "ay":8.8729, "alpha":-13.38098, "fx":[21.7999,-38.85619,-77.34248,15.94694], "fy":[133.3813,129.90536,109.21473,130.58342]},
+ {"t":6.38371, "x":7.74812, "y":3.99213, "heading":1.43455, "vx":0.79891, "vy":3.55153, "omega":1.4775, "ax":0.43397, "ay":8.66538, "alpha":-14.05898, "fx":[39.25871,-20.89896,-47.97593,54.22205], "fy":[126.41345,130.90248,118.91768,115.0849]},
+ {"t":6.42224, "x":7.77923, "y":4.1354, "heading":1.48104, "vx":0.81563, "vy":3.88542, "omega":0.9358, "ax":2.44023, "ay":7.70088, "alpha":-13.2837, "fx":[57.88132,3.27309,-3.44581,80.64994], "fy":[111.67927,124.77436,113.57409,86.60467]},
+ {"t":6.46077, "x":7.81247, "y":4.29083, "heading":1.50724, "vx":0.90966, "vy":4.18214, "omega":0.42397, "ax":3.48381, "ay":4.57721, "alpha":-8.6221, "fx":[60.91009,30.7866,36.10345,69.7287], "fy":[72.30799,82.69705,58.14478,46.37387]},
+ {"t":6.4993, "x":7.8501, "y":4.45537, "heading":1.51718, "vx":1.04389, "vy":4.3585, "omega":0.09175, "ax":0.88033, "ay":0.23109, "alpha":-0.71324, "fx":[13.70967,11.36335,11.24612,13.59498], "fy":[4.39596,4.53737,2.1518,2.01742]},
+ {"t":6.53783, "x":7.89098, "y":4.62347, "heading":1.52018, "vx":1.07781, "vy":4.3674, "omega":0.06427, "ax":-0.38965, "ay":0.06284, "alpha":0.05221, "fx":[-5.61401,-5.44114,-5.4323,-5.60517], "fy":[0.80839,0.79968,0.9731,0.98179]},
+ {"t":6.57636, "x":7.93222, "y":4.7918, "heading":1.5227, "vx":1.0628, "vy":4.36982, "omega":0.06628, "ax":-2.72104, "ay":-0.06012, "alpha":1.12009, "fx":[-40.35972,-36.93298,-36.7782,-40.20967], "fy":[-2.73508,-2.96984,1.06026,1.23592]},
+ {"t":6.61489, "x":7.97115, "y":4.96013, "heading":1.52608, "vx":0.95795, "vy":4.36751, "omega":0.10944, "ax":-6.58367, "ay":-2.47734, "alpha":6.21466, "fx":[-97.05542,-83.05789,-90.67892,-102.49577], "fy":[-44.26832,-54.30177,-23.9194,-17.97317]},
+ {"t":6.65342, "x":8.00317, "y":5.12657, "heading":1.53491, "vx":0.70428, "vy":4.27205, "omega":0.34889, "ax":-8.00017, "ay":-2.9647, "alpha":7.05963, "fx":[-114.19771,-100.99566,-115.49395,-122.91462], "fy":[-52.45281,-68.17297,-28.09499,-19.37482]},
+ {"t":6.69196, "x":8.02437, "y":5.28898, "heading":1.55359, "vx":0.39603, "vy":4.15782, "omega":0.62091, "ax":-8.9724, "ay":-1.68382, "alpha":4.44163, "fx":[-126.78363,-122.92588,-128.47751,-130.53924], "fy":[-33.50279,-40.76821,-11.89328,-9.30643]},
+ {"t":6.73049, "x":8.03297, "y":5.44793, "heading":1.58082, "vx":0.05031, "vy":4.09294, "omega":0.79205, "ax":-9.45045, "ay":0.03335, "alpha":-0.81326, "fx":[-133.85406,-134.04584,-134.06013,-133.87148], "fy":[3.16147,2.99405,-2.11853,-2.14626]},
+ {"t":6.76902, "x":8.02789, "y":5.60566, "heading":1.61073, "vx":-0.31382, "vy":4.09423, "omega":0.76071, "ax":-6.26217, "ay":-0.87009, "alpha":-37.95212, "fx":[-68.01864,-133.02027,-119.02832,-34.99198], "fy":[116.57598,35.15499,-69.20889,-131.85559]},
+ {"t":6.80755, "x":8.01115, "y":5.76277, "heading":1.61187, "vx":-0.55511, "vy":4.0607, "omega":-0.70161, "ax":-4.61902, "ay":-8.13912, "alpha":-10.97963, "fx":[-51.57044,-108.47477,-75.90977,-25.93929], "fy":[-126.61736,-84.50326,-115.1265,-135.23349]},
+ {"t":6.8484, "x":7.98462, "y":5.92188, "heading":1.57404, "vx":-0.74382, "vy":3.72818, "omega":-1.15019, "ax":-4.00121, "ay":-8.54696, "alpha":-9.09611, "fx":[-39.85986,-93.57702,-68.3157,-25.11222], "fy":[-130.37008,-99.97754,-119.29487,-134.96216]},
+ {"t":6.88926, "x":7.95089, "y":6.06706, "heading":1.51946, "vx":-0.90729, "vy":3.37899, "omega":-1.52181, "ax":-3.37909, "ay":-8.79295, "alpha":-8.59623, "fx":[-26.80188,-82.5653,-62.07946,-20.1446], "fy":[-132.95747,-108.28309,-122.04938,-135.26206]},
+ {"t":6.93011, "x":7.911, "y":6.19777, "heading":1.45011, "vx":-1.04534, "vy":3.01975, "omega":-1.87302, "ax":-2.88411, "ay":-8.91276, "alpha":-8.47516, "fx":[-15.76566,-73.83123,-57.85109,-16.07847], "fy":[-133.75722,-113.15545,-123.31546,-135.11671]},
+ {"t":6.97097, "x":7.86589, "y":6.31371, "heading":1.36651, "vx":-1.16317, "vy":2.65562, "omega":-2.21927, "ax":-2.68017, "ay":-8.88121, "alpha":-8.61289, "fx":[-9.45993,-69.53094,-57.51745,-15.4547], "fy":[-132.95685,-114.043,-122.34295,-134.21331]},
+ {"t":7.01182, "x":7.81613, "y":6.41479, "heading":1.26866, "vx":-1.27267, "vy":2.29277, "omega":-2.57115, "ax":-2.90557, "ay":-8.64272, "alpha":-8.89737, "fx":[-10.44927,-71.43893,-62.46341,-20.39169], "fy":[-130.44035,-109.76106,-117.95715,-131.8752]},
+ {"t":7.05268, "x":7.76171, "y":6.50125, "heading":1.15619, "vx":-1.39138, "vy":1.93967, "omega":-2.93466, "ax":-3.67428, "ay":-7.95115, "alpha":-9.04073, "fx":[-22.48388,-79.68292,-72.9628,-33.1986], "fy":[-122.75977,-96.24776,-106.82862,-124.98668]},
+ {"t":7.09353, "x":7.7018, "y":6.57386, "heading":1.02874, "vx":-1.54149, "vy":1.61483, "omega":-3.30402, "ax":-5.43063, "ay":-3.38271, "alpha":-4.32389, "fx":[-69.67296,-82.91526,-83.86731,-71.45626], "fy":[-49.1968,-34.9767,-47.39879,-60.22402]},
+ {"t":7.13439, "x":7.63429, "y":6.63701, "heading":0.89015, "vx":-1.76336, "vy":1.47662, "omega":-3.48067, "ax":1.75517, "ay":8.24231, "alpha":5.39004, "fx":[3.12751,32.18677,44.43505,19.76699], "fy":[119.83576,112.5987,113.32788,121.56868]},
+ {"t":7.17524, "x":7.56371, "y":6.70422, "heading":0.75244, "vx":-1.69166, "vy":1.81337, "omega":-3.26046, "ax":3.94241, "ay":7.76585, "alpha":4.74674, "fx":[38.22956,63.69571,71.29188,50.31387], "fy":[117.45146,104.1378,103.08152,115.64572]},
+ {"t":7.2161, "x":7.49789, "y":6.78478, "heading":0.6232, "vx":-1.53059, "vy":2.13064, "omega":-3.06653, "ax":4.98234, "ay":6.86254, "alpha":4.38996, "fx":[56.03853,76.78255,83.43502,66.23766], "fy":[106.05992,90.25164,89.19148,103.59666]},
+ {"t":7.25696, "x":7.43951, "y":6.87756, "heading":0.50158, "vx":-1.32703, "vy":2.41101, "omega":-2.88718, "ax":-6.7022, "ay":2.92822, "alpha":13.14868, "fx":[-110.76898,-111.59741,-79.9178,-77.7244], "fy":[42.80162,-5.31729,45.24764,83.29537]},
+ {"t":7.29781, "x":7.3797, "y":6.9785, "heading":0.39459, "vx":-1.60085, "vy":2.53065, "omega":-2.34999, "ax":-8.8831, "ay":-2.35333, "alpha":8.38764, "fx":[-133.03862,-121.00231,-117.55658,-132.06579], "fy":[-16.54922,-57.3138,-59.02594,-0.54263]},
+ {"t":7.33867, "x":7.30688, "y":7.07993, "heading":0.30559, "vx":-1.96377, "vy":2.4345, "omega":-2.00731, "ax":-8.87044, "ay":-3.2713, "alpha":6.78097, "fx":[-133.1957,-120.80655,-115.79136,-133.15199], "fy":[-28.60271,-63.03103,-70.05401,-23.792]},
+ {"t":7.37952, "x":7.21925, "y":7.17666, "heading":0.22924, "vx":-2.32618, "vy":2.30085, "omega":-1.73027, "ax":-8.72069, "ay":-3.91588, "alpha":5.98324, "fx":[-131.95578,-119.26366,-112.43119,-130.80442], "fy":[-37.29981,-67.81824,-77.75476,-39.15374]},
+ {"t":7.42038, "x":7.11694, "y":7.2674, "heading":0.16354, "vx":-2.68247, "vy":2.14087, "omega":-1.48582, "ax":-8.51384, "ay":-4.45766, "alpha":5.69135, "fx":[-130.31855,-116.98675,-107.98761,-127.43384], "fy":[-44.25253,-72.64742,-84.94775,-50.89736]},
+ {"t":7.46123, "x":7.00024, "y":7.35114, "heading":0.10759, "vx":-3.0303, "vy":1.95875, "omega":-1.2533, "ax":-8.28334, "ay":-4.91499, "alpha":5.81142, "fx":[-128.82805,-114.40179,-102.6243,-123.80361], "fy":[-49.32545,-77.23093,-91.97988,-60.13891]},
+ {"t":7.50209, "x":6.86952, "y":7.42707, "heading":0.06123, "vx":-3.36872, "vy":1.75794, "omega":-1.01587, "ax":-8.04531, "ay":-5.29599, "alpha":6.30101, "fx":[-127.84803,-111.78473,-96.3215,-120.20693], "fy":[-52.39779,-81.34672,-98.94228,-67.59074]},
+ {"t":7.54294, "x":6.72518, "y":7.49447, "heading":0.02499, "vx":-3.69741, "vy":1.54157, "omega":-0.75844, "ax":-7.80107, "ay":-5.61136, "alpha":7.1896, "fx":[-127.64353,-109.24042,-88.71968,-116.70987], "fy":[-53.30855,-84.99019,-106.06086,-73.79932]},
+ {"t":7.5838, "x":6.56761, "y":7.55277, "heading":0.0, "vx":-4.01613, "vy":1.31232, "omega":-0.46471, "ax":-6.63765, "ay":-6.89783, "alpha":8.1628, "fx":[-119.85479,-96.47654,-64.95629,-95.06094], "fy":[-69.21758,-99.37768,-122.22247,-100.28266]},
+ {"t":7.64503, "x":6.30926, "y":7.62019, "heading":-0.01315, "vx":-4.42254, "vy":0.88998, "omega":0.03509, "ax":-1.68634, "ay":-9.56147, "alpha":-0.54314, "fx":[-21.90748,-22.42537,-25.94122,-25.34], "fy":[-135.89411,-135.77638,-135.15433,-135.30128]},
+ {"t":7.70626, "x":6.03532, "y":7.65676, "heading":-0.01202, "vx":-4.52579, "vy":0.30454, "omega":0.00183, "ax":-0.04471, "ay":-9.37617, "alpha":-0.01882, "fx":[-0.57436,-0.57342,-0.69313,-0.69397], "fy":[-132.90786,-132.90257,-132.90197,-132.90725]},
+ {"t":7.76748, "x":5.75812, "y":7.65783, "heading":-0.01195, "vx":-4.52853, "vy":-0.26955, "omega":0.00068, "ax":-0.12016, "ay":3.10392, "alpha":-0.0107, "fx":[-1.68413,-1.68339,-1.72247,-1.72322], "fy":[43.9813,44.01328,44.01338,43.98141]},
+ {"t":7.82871, "x":5.48062, "y":7.64714, "heading":-0.01192, "vx":-4.53589, "vy":-0.0795, "omega":0.00002, "ax":-0.00839, "ay":0.68017, "alpha":-0.0001, "fx":[-0.11875,-0.11874,-0.11906,-0.11906], "fy":[9.64106,9.64137,9.64138,9.64106]},
+ {"t":7.88994, "x":5.20288, "y":7.64355, "heading":-0.01192, "vx":-4.5364, "vy":-0.03786, "omega":0.00002, "ax":5.61608, "ay":0.86237, "alpha":0.86652, "fx":[78.49185,78.76359,80.71655,80.45452], "fy":[14.31125,10.47171,10.17999,13.93262]},
+ {"t":7.95117, "x":4.93565, "y":7.64285, "heading":-0.0103, "vx":-4.19254, "vy":0.01495, "omega":0.05307, "ax":9.75183, "ay":0.0505, "alpha":-0.14628, "fx":[138.23465,138.22979,138.22488,138.22988], "fy":[0.22686,1.18999,1.20829,0.23843]},
+ {"t":8.0124, "x":4.69723, "y":7.64386, "heading":-0.00732, "vx":-3.59545, "vy":0.01804, "omega":0.04412, "ax":9.77586, "ay":0.01986, "alpha":-0.15098, "fx":[138.57269,138.57072,138.56819,138.57024], "fy":[-0.22146,0.77504,0.78799,-0.21571]},
+ {"t":8.07363, "x":4.49541, "y":7.645, "heading":-0.0049, "vx":-2.99688, "vy":0.01925, "omega":0.03487, "ax":9.7839, "ay":-0.00738, "alpha":-0.13894, "fx":[138.68481,138.68552,138.68399,138.6833], "fy":[-0.56552,0.35256,0.35944,-0.56475]},
+ {"t":8.13486, "x":4.33025, "y":7.64616, "heading":-0.00303, "vx":-2.39783, "vy":0.0188, "omega":0.02637, "ax":9.78786, "ay":-0.03443, "alpha":-0.12481, "fx":[138.73964,138.74255,138.74153,138.73861], "fy":[-0.90048,-0.07518,-0.07311,-0.90335]},
+ {"t":8.19608, "x":4.20178, "y":7.64725, "heading":-0.00165, "vx":-1.79853, "vy":0.01669, "omega":0.01873, "ax":9.79014, "ay":-0.06207, "alpha":-0.11189, "fx":[138.77094,138.77562,138.77488,138.77016], "fy":[-1.24823,-0.50793,-0.50943,-1.25369]},
+ {"t":8.25731, "x":4.11001, "y":7.64816, "heading":-0.00071, "vx":-1.1991, "vy":0.01289, "omega":0.01187, "ax":9.79153, "ay":-0.09058, "alpha":-0.10117, "fx":[138.78981,138.79599,138.79541,138.78918], "fy":[-1.61587,-0.94628,-0.95039,-1.62322]},
+ {"t":8.31854, "x":4.05495, "y":7.64878, "heading":-0.00017, "vx":-0.59957, "vy":0.00735, "omega":0.00568, "ax":9.79236, "ay":-0.12002, "alpha":-0.09276, "fx":[138.80094,138.80845,138.80795,138.80038], "fy":[-2.00457,-1.39044,-1.3965,-2.01337]},
+ {"t":8.37977, "x":4.03659, "y":7.649, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":8.44251, "ay":-4.14519, "alpha":-11.72858, "fx":[117.99677,137.43852,133.88147,89.36535], "fy":[-73.12964,-19.43257,-36.28929,-106.1766]},
+ {"t":8.42931, "x":4.04695, "y":7.64391, "heading":-0.01439, "vx":0.41827, "vy":-0.20537, "omega":-0.58108, "ax":8.64976, "ay":-3.68316, "alpha":-11.83524, "fx":[120.54354,138.03394,136.43553,95.42007], "fy":[-68.81304,-14.43551,-24.85477,-100.72831]},
+ {"t":8.47886, "x":4.07829, "y":7.62922, "heading":-0.05771, "vx":0.84681, "vy":-0.38784, "omega":-1.16744, "ax":8.90605, "ay":-3.00562, "alpha":-11.93987, "fx":[123.69537,138.52756,138.40792,104.33354], "fy":[-62.90945,-8.0452,-8.07102,-91.39023]},
+ {"t":8.5284, "x":4.13118, "y":7.60632, "heading":-0.1302, "vx":1.28805, "vy":-0.53675, "omega":-1.75898, "ax":9.22763, "ay":-1.93099, "alpha":-11.52698, "fx":[128.36376,138.716,137.8754,118.24294], "fy":[-52.61081,1.33064,14.13732,-72.34243]},
+ {"t":8.57794, "x":4.20631, "y":7.57735, "heading":-0.23149, "vx":1.74522, "vy":-0.63242, "omega":-2.33007, "ax":9.54167, "ay":0.01615, "alpha":-9.27326, "fx":[135.47254,137.51993,132.40865,135.60251], "fy":[-29.46274,17.71909,40.77603,-28.11681]},
+ {"t":8.62749, "x":4.30449, "y":7.54604, "heading":-0.35831, "vx":2.21795, "vy":-0.63162, "omega":-2.7895, "ax":9.53416, "ay":1.8961, "alpha":4.05946, "fx":[130.78112,136.60715,138.15371,135.03585], "fy":[45.39621,22.04708,9.27287,30.79068]},
+ {"t":8.67703, "x":4.42608, "y":7.51707, "heading":-0.49153, "vx":2.69031, "vy":-0.53768, "omega":-2.58838, "ax":8.9405, "ay":2.17345, "alpha":13.65581, "fx":[103.09251,135.08852,134.93184,133.80471], "fy":[92.19949,25.86382,-30.47929,35.64836]},
+ {"t":8.72658, "x":4.57034, "y":7.4931, "heading":-0.60301, "vx":3.13325, "vy":-0.43, "omega":-1.91182, "ax":8.9363, "ay":2.47155, "alpha":12.53397, "fx":[107.1544,129.46298,135.74821,134.31383], "fy":[87.04589,45.1772,-24.8217,32.73338]},
+ {"t":8.77612, "x":4.73654, "y":7.47483, "heading":-0.68235, "vx":3.57599, "vy":-0.30755, "omega":-1.29084, "ax":8.7837, "ay":2.96539, "alpha":11.82554, "fx":[107.19024,121.31496,136.38628,133.13576], "fy":[85.84913,61.18613,-13.69134,34.79104]},
+ {"t":8.82566, "x":4.92448, "y":7.46323, "heading":-0.73179, "vx":4.01116, "vy":-0.16064, "omega":-0.70496, "ax":3.56519, "ay":8.19604, "alpha":4.94088, "fx":[43.70204,31.4828,60.0362,66.92184], "fy":[121.45329,122.97146,110.58782,109.69516]},
+ {"t":8.87521, "x":5.12759, "y":7.46534, "heading":-0.76065, "vx":4.18779, "vy":0.24543, "omega":-0.46017, "ax":-9.18028, "ay":0.62413, "alpha":-12.29732, "fx":[-127.44154,-133.68728,-122.10567,-137.27853], "fy":[-49.74758,18.16911,61.99134,4.9746]},
+ {"t":8.92475, "x":5.3238, "y":7.47826, "heading":-0.79854, "vx":3.73297, "vy":0.27635, "omega":-1.06943, "ax":-9.24998, "ay":-0.24583, "alpha":-12.65582, "fx":[-123.80613,-136.65818,-125.83407,-138.16645], "fy":[-60.8428,-7.85599,56.46101,-1.70061]},
+ {"t":8.97429, "x":5.49739, "y":7.49165, "heading":-0.86706, "vx":3.27469, "vy":0.26417, "omega":-1.69644, "ax":-9.22891, "ay":-0.68111, "alpha":-13.05744, "fx":[-122.73278,-134.9803,-127.14899,-138.40855], "fy":[-63.75654,-26.65506,54.23272,-2.43953]},
+ {"t":9.02384, "x":5.6483, "y":7.5039, "heading":-0.96713, "vx":2.81746, "vy":0.23042, "omega":-2.34336, "ax":-8.79796, "ay":-4.13602, "alpha":-4.17038, "fx":[-118.54002,-119.40424,-131.56873,-129.32304], "fy":[-71.67062,-70.05148,-43.11959,-49.66687]},
+ {"t":9.07338, "x":5.77709, "y":7.51024, "heading":-1.08835, "vx":2.38158, "vy":0.02551, "omega":-2.54997, "ax":-7.32129, "ay":-6.40359, "alpha":4.49784, "fx":[-112.10677,-114.43684,-97.79827,-90.76854], "fy":[-81.56097,-78.3529,-98.35099,-104.81281]},
+ {"t":9.12292, "x":5.8861, "y":7.50365, "heading":-1.20916, "vx":2.01885, "vy":-0.29175, "omega":-2.32713, "ax":-6.52885, "ay":-7.06824, "alpha":7.69882, "fx":[-104.49587,-114.41304,-87.10755,-64.16317], "fy":[-91.19611,-78.51781,-108.03839,-123.01034]},
+ {"t":9.17247, "x":5.97811, "y":7.48052, "heading":-1.31501, "vx":1.69539, "vy":-0.64193, "omega":-1.9457, "ax":-6.08798, "ay":-7.36723, "alpha":9.0618, "fx":[-95.75876,-115.16828,-84.06158,-50.19393], "fy":[-100.38223,-77.4682,-110.47155,-129.39316]},
+ {"t":9.22201, "x":6.05463, "y":7.43967, "heading":-1.40028, "vx":1.39377, "vy":-1.00693, "omega":-1.49675, "ax":-5.94399, "ay":-7.46752, "alpha":9.29044, "fx":[-89.30513,-115.61265,-84.8342,-47.26676], "fy":[-106.18191,-76.819,-109.89223,-130.50783]},
+ {"t":9.2739, "x":6.11895, "y":7.37737, "heading":-1.46544, "vx":1.08533, "vy":-1.39443, "omega":-1.01465, "ax":-5.97595, "ay":-7.46729, "alpha":8.89585, "fx":[-86.4331,-115.32639,-86.87398,-50.19724], "fy":[-108.50721,-77.2164,-108.27016,-129.3943]},
+ {"t":9.32579, "x":6.16723, "y":7.29495, "heading":-1.50612, "vx":0.77523, "vy":-1.78192, "omega":-0.55303, "ax":-6.02325, "ay":-7.46012, "alpha":8.39255, "fx":[-85.30102,-114.47455,-88.24185,-53.49535], "fy":[-109.36803,-78.43398,-107.13509,-128.04472]},
+ {"t":9.37769, "x":6.19935, "y":7.19244, "heading":-1.52352, "vx":0.46267, "vy":-2.16904, "omega":-0.11753, "ax":-6.09156, "ay":-7.44539, "alpha":7.67076, "fx":[-85.78361,-112.94966,-89.08617,-57.56617], "fy":[-108.94878,-80.56236,-106.40037,-126.23513]},
+ {"t":9.42958, "x":6.21515, "y":7.06986, "heading":-1.51929, "vx":0.14657, "vy":-2.5554, "omega":0.28052, "ax":-6.19078, "ay":-7.41827, "alpha":6.56379, "fx":[-87.62792,-110.42204,-89.65565,-63.30566], "fy":[-107.41411,-83.92058,-105.86783,-123.40618]},
+ {"t":9.48147, "x":6.21442, "y":6.92727, "heading":-1.49589, "vx":-0.17468, "vy":-2.94034, "omega":0.62112, "ax":-6.34244, "ay":-7.36173, "alpha":4.69259, "fx":[-90.45288,-105.98915,-90.49364,-72.67474], "fy":[-104.96271,-89.34159,-105.0563,-118.04231]},
+ {"t":9.53336, "x":6.19682, "y":6.76478, "heading":-1.45734, "vx":-0.5038, "vy":-3.32235, "omega":0.86463, "ax":-6.59528, "ay":-7.20356, "alpha":0.93124, "fx":[-93.73018,-96.66491,-93.30223,-90.24865], "fy":[-101.91263,-99.15479,-102.34174,-105.02586]},
+ {"t":9.58525, "x":6.1618, "y":6.58268, "heading":-1.41122, "vx":-0.84604, "vy":-3.69616, "omega":0.91295, "ax":-6.9925, "ay":-6.4618, "alpha":-8.81654, "fx":[-96.93666,-67.86891,-108.60303,-123.05944], "fy":[-98.60915,-120.24319,-84.71604,-62.80957]},
+ {"t":9.63714, "x":6.10848, "y":6.38218, "heading":-1.37572, "vx":-1.20889, "vy":-4.03147, "omega":0.45545, "ax":-8.61173, "ay":-3.66963, "alpha":-8.74279, "fx":[-117.69287,-105.21832,-131.95339,-133.41251], "fy":[-68.74191,-84.91895,-27.25504,-27.1485]},
+ {"t":9.68904, "x":6.03415, "y":6.16804, "heading":-1.36386, "vx":-1.65577, "vy":-4.2219, "omega":0.00177, "ax":8.49168, "ay":-2.84524, "alpha":-0.00704, "fx":[120.35761,120.36766,120.37757,120.36753], "fy":[-40.35814,-40.33863,-40.30292,-40.32243]},
+ {"t":9.74093, "x":5.95967, "y":5.94513, "heading":-1.36377, "vx":-1.21512, "vy":-4.36954, "omega":0.00141, "ax":9.63362, "ay":-0.41315, "alpha":2.39593, "fx":[136.95164,136.76062,135.92034,136.58431], "fy":[3.25172,0.18987,-15.72595,-11.14078]},
+ {"t":9.79282, "x":5.90958, "y":5.71783, "heading":-1.36048, "vx":-0.71522, "vy":-4.39098, "omega":0.12573, "ax":8.31438, "ay":4.68915, "alpha":8.50351, "fx":[112.66563,96.8496,129.78532,132.11694], "fy":[80.19803,98.38482,46.59541,40.69187]},
+ {"t":9.84471, "x":5.88366, "y":5.49629, "heading":-1.3425, "vx":-0.28377, "vy":-4.14765, "omega":0.567, "ax":7.6677, "ay":5.67259, "alpha":9.08114, "fx":[103.6142,80.71492,122.30195,128.12042], "fy":[91.99252,112.45598,64.53576,52.64613]},
+ {"t":9.8966, "x":5.87926, "y":5.2887, "heading":-1.30085, "vx":0.11412, "vy":-3.85329, "omega":1.03823, "ax":7.3819, "ay":6.09106, "alpha":8.61048, "fx":[99.27283,76.60639,117.92093,124.74642], "fy":[96.80959,115.47142,72.60296,60.47328]},
+ {"t":9.94849, "x":5.89512, "y":5.09694, "heading":-1.23538, "vx":0.49718, "vy":-3.53722, "omega":1.48504, "ax":7.22839, "ay":6.34347, "alpha":7.71069, "fx":[96.3923,77.21724,115.10419,121.12914], "fy":[99.75299,115.15816,77.19863,67.55915]},
+ {"t":10.00039, "x":5.93065, "y":4.92193, "heading":-1.14794, "vx":0.87227, "vy":-3.20804, "omega":1.88516, "ax":7.04144, "ay":6.61802, "alpha":6.49951, "fx":[93.14658,78.66791,111.3478,116.08086], "fy":[102.74987,114.13445,82.49504,75.85623]},
+ {"t":10.04718, "x":5.97918, "y":4.77907, "heading":-1.05262, "vx":1.20175, "vy":-2.89837, "omega":2.18929, "ax":6.74549, "ay":6.96319, "alpha":5.36528, "fx":[88.74583,78.10753,105.95496,109.6545], "fy":[106.46816,114.40756,89.18175,84.74851]},
+ {"t":10.09397, "x":6.04279, "y":4.65107, "heading":-0.9443, "vx":1.51739, "vy":-2.57255, "omega":2.44034, "ax":6.41377, "ay":7.29902, "alpha":4.12302, "fx":[84.35072,77.60363,99.61406,102.08604], "fy":[109.79534,114.55427,95.98925,93.50883]},
+ {"t":10.14076, "x":6.12082, "y":4.53868, "heading":-0.8256, "vx":1.81751, "vy":-2.23101, "omega":2.63327, "ax":6.11326, "ay":7.55711, "alpha":2.6306, "fx":[81.46441,78.53609,92.73709,93.87828], "fy":[111.52875,113.48483,102.18321,101.28387]},
+ {"t":10.18755, "x":6.21256, "y":4.44256, "heading":-0.6995, "vx":2.10356, "vy":-1.87739, "omega":2.75636, "ax":6.36116, "ay":7.24719, "alpha":-0.18425, "fx":[90.6253,90.6154,89.71426,89.71646], "fy":[102.31294,102.34766,103.13795,103.11027]},
+ {"t":10.23435, "x":6.31795, "y":4.36265, "heading":-0.57073, "vx":2.40121, "vy":-1.53828, "omega":2.74774, "ax":7.90168, "ay":4.95236, "alpha":-6.7365, "fx":[126.45454,118.60462,97.84866,105.10971], "fy":[43.46831,64.1638,92.15936,81.00241]},
+ {"t":10.28114, "x":6.43896, "y":4.29609, "heading":-0.44953, "vx":2.77095, "vy":-1.30655, "omega":2.43252, "ax":8.17339, "ay":3.98259, "alpha":-8.90414, "fx":[130.82853,121.79132,97.73414,113.0695], "fy":[18.72575,55.07174,89.5886,62.42289]},
+ {"t":10.32793, "x":6.57757, "y":4.23931, "heading":-0.34545, "vx":3.1534, "vy":-1.1202, "omega":2.01588, "ax":7.80582, "ay":3.95626, "alpha":-9.78684, "fx":[127.27362,117.00114,89.82844,108.47941], "fy":[17.13177,58.16047,92.02368,57.00011]},
+ {"t":10.37472, "x":6.73367, "y":4.19123, "heading":-0.26184, "vx":3.51865, "vy":-0.93507, "omega":1.55793, "ax":6.87574, "ay":3.54784, "alpha":-9.65759, "fx":[113.52579,105.34479,78.88264,92.09464], "fy":[17.66918,56.00898,82.67783,44.80328]},
+ {"t":10.42152, "x":6.90584, "y":4.15136, "heading":-0.19951, "vx":3.84039, "vy":-0.76906, "omega":1.10603, "ax":2.6666, "ay":3.60693, "alpha":-3.38337, "fx":[43.95204,43.78285,31.98868,31.46993], "fy":[44.00251,53.96146,58.17489,48.37049]},
+ {"t":10.46831, "x":7.08846, "y":4.11932, "heading":-0.15146, "vx":3.96516, "vy":-0.60029, "omega":0.94771, "ax":-4.78671, "ay":5.06474, "alpha":8.98184, "fx":[-72.84798,-90.72647,-64.61433,-43.21324], "fy":[81.93143,51.5668,61.0111,92.65638]},
+ {"t":10.5151, "x":7.26876, "y":4.09678, "heading":-0.09728, "vx":3.74118, "vy":-0.36329, "omega":1.36799, "ax":-6.27453, "ay":6.14606, "alpha":10.56397, "fx":[-91.16776,-117.84898,-93.34888,-53.39443], "fy":[94.95683,54.71771,81.02546,117.77587]},
+ {"t":10.56189, "x":7.43695, "y":4.08651, "heading":-0.02171, "vx":3.44758, "vy":-0.07571, "omega":1.86231, "ax":-6.73859, "ay":6.26802, "alpha":9.84281, "fx":[-97.79234,-122.96406,-99.30047,-62.01457], "fy":[93.86832,55.0429,87.04728,119.43219]},
+ {"t":10.60869, "x":7.59089, "y":4.08983, "heading":0.07621, "vx":3.13227, "vy":0.21759, "omega":2.32287, "ax":-8.15078, "ay":4.43716, "alpha":9.97756, "fx":[-115.9313,-133.35599,-123.6466,-89.2075], "fy":[71.87755,26.30538,51.37951,102.02049]},
+ {"t":10.65548, "x":7.72853, "y":4.10486, "heading":0.19582, "vx":2.75087, "vy":0.42521, "omega":2.78975, "ax":-8.7064, "ay":3.42178, "alpha":9.48389, "fx":[-124.43972,-136.00596,-129.90347,-103.29544], "fy":[57.00016,11.59116,36.68114,88.73921]},
+ {"t":10.70227, "x":7.84772, "y":4.12851, "heading":0.33675, "vx":2.34348, "vy":0.58533, "omega":3.23352, "ax":-8.68498, "ay":3.65948, "alpha":8.68378, "fx":[-125.75831,-135.99024,-126.10707,-104.57424], "fy":[54.78562,14.62331,50.01672,88.06353]},
+ {"t":10.74906, "x":7.94787, "y":4.1599, "heading":0.49756, "vx":1.93709, "vy":0.75656, "omega":3.63985, "ax":-8.0479, "ay":5.12513, "alpha":6.67497, "fx":[-119.53383,-129.22246,-108.99162,-98.56061], "fy":[67.66957,45.34258,82.1819,95.39619]},
+ {"t":10.79585, "x":8.0297, "y":4.20091, "heading":0.67518, "vx":1.56051, "vy":0.99638, "omega":3.95219, "ax":-6.52613, "ay":7.16135, "alpha":1.97459, "fx":[-96.82824,-97.74278,-87.7684,-87.68588], "fy":[97.71058,96.60901,105.74015,105.98219]},
+ {"t":10.84265, "x":8.09558, "y":4.25538, "heading":0.86227, "vx":1.25514, "vy":1.33148, "omega":4.04459, "ax":-4.8138, "ay":8.40968, "alpha":-3.97354, "fx":[-52.73399,-62.37421,-81.72031,-76.10922], "fy":[127.45053,123.18852,111.244,114.93752]},
+ {"t":10.88944, "x":8.14904, "y":4.32689, "heading":1.04718, "vx":1.02989, "vy":1.72498, "omega":3.85866, "ax":-3.86487, "ay":8.74767, "alpha":-8.26636, "fx":[-17.32511,-51.43728,-87.09422,-63.27763], "fy":[137.22129,128.53513,107.50538,122.72256]},
+ {"t":10.93623, "x":8.193, "y":4.41718, "heading":1.21869, "vx":0.84904, "vy":2.13431, "omega":3.47185, "ax":-3.43841, "ay":8.78705, "alpha":-10.76285, "fx":[-1.63868,-52.83794,-94.71284,-45.76496], "fy":[138.52909,128.15263,101.08556,130.45035]},
+ {"t":10.98302, "x":8.22896, "y":4.52667, "heading":1.36936, "vx":0.68815, "vy":2.54547, "omega":2.96824, "ax":-3.20308, "ay":8.33336, "alpha":-17.04936, "fx":[14.43464,-63.06787,-117.39996,-15.57811], "fy":[137.92237,123.56164,73.73219,137.27738]},
+ {"t":11.02982, "x":8.25765, "y":4.6549, "heading":1.48958, "vx":0.53827, "vy":2.93541, "omega":2.17046, "ax":-0.88076, "ay":4.65024, "alpha":-46.14214, "fx":[31.02491,-80.3783,-137.99149,137.40687], "fy":[135.18135,113.09063,13.51936,1.87271]},
+ {"t":11.07104, "x":8.27909, "y":4.77985, "heading":1.53985, "vx":0.50197, "vy":3.1271, "omega":0.26841, "ax":-2.87826, "ay":7.12216, "alpha":-27.57496, "fx":[10.50025,-84.44,-135.85372,46.59868], "fy":[138.19664,109.99215,27.09524,128.53571]},
+ {"t":11.11226, "x":8.29734, "y":4.91481, "heading":1.52749, "vx":0.38332, "vy":3.42068, "omega":-0.86826, "ax":-5.64609, "ay":7.46735, "alpha":-11.62094, "fx":[-36.37233,-86.84269,-120.8624,-76.05043], "fy":[133.46087,107.82979,67.20226,114.89873]},
+ {"t":11.15348, "x":8.30834, "y":5.06216, "heading":1.48182, "vx":0.15058, "vy":3.7285, "omega":-1.3473, "ax":-6.76775, "ay":6.42734, "alpha":11.64738, "fx":[-127.98677,-106.29109,-52.83532,-96.61193], "fy":[51.58467,86.65308,127.39373,98.7927]},
+ {"t":11.1947, "x":8.3088, "y":5.22131, "heading":1.43618, "vx":-0.12839, "vy":3.99344, "omega":-0.86717, "ax":-7.14588, "ay":6.04342, "alpha":10.83593, "fx":[-127.95432,-114.20901,-63.60487,-99.39649], "fy":[50.58333,74.83374,121.78073,95.45843]},
+ {"t":11.23592, "x":8.29744, "y":5.39106, "heading":1.40964, "vx":-0.42296, "vy":4.24256, "omega":-0.4205, "ax":-8.0893, "ay":4.7627, "alpha":8.95454, "fx":[-130.66693,-126.03356,-91.25212,-110.70289], "fy":[39.72063,49.11915,100.83113,80.36967]},
+ {"t":11.27714, "x":8.27313, "y":5.56999, "heading":1.39992, "vx":-0.75641, "vy":4.43889, "omega":-0.05138, "ax":-9.26268, "ay":-1.25625, "alpha":1.0685, "fx":[-131.22797,-130.5902,-131.42521,-131.94191], "fy":[-19.96259,-22.06718,-15.52325,-13.6749]},
+ {"t":11.31837, "x":8.23408, "y":5.7519, "heading":1.39871, "vx":-1.13823, "vy":4.3871, "omega":-0.00734, "ax":-8.56976, "ay":-2.58672, "alpha":0.02155, "fx":[-121.47343,-121.44625,-121.47508,-121.50221], "fy":[-36.69637,-36.74971,-36.63585,-36.58257]},
+ {"t":11.35959, "x":8.17988, "y":5.93054, "heading":1.39842, "vx":-1.49149, "vy":4.28047, "omega":-0.00645, "ax":-8.08697, "ay":-3.18006, "alpha":0.00449, "fx":[-114.63133,-114.62372,-114.6305,-114.63811], "fy":[-45.08151,-45.09341,-45.07162,-45.05972]},
+ {"t":11.40081, "x":8.11153, "y":6.10429, "heading":1.39816, "vx":-1.82484, "vy":4.14939, "omega":-0.00627, "ax":-8.13654, "ay":-3.99143, "alpha":-0.00407, "fx":[-115.33348,-115.3414,-115.3335,-115.32558], "fy":[-56.57454,-56.56198,-56.58071,-56.59327]},
+ {"t":11.44203, "x":8.02939, "y":6.27194, "heading":1.3979, "vx":-2.16024, "vy":3.98485, "omega":-0.00643, "ax":-7.99635, "ay":-4.96451, "alpha":-0.19762, "fx":[-113.3438,-113.81649,-113.35136,-112.87355], "fy":[-70.30907,-69.61479,-70.43317,-71.12581]},
+ {"t":11.48325, "x":7.93355, "y":6.43199, "heading":1.39747, "vx":-2.48986, "vy":3.78021, "omega":-0.01458, "ax":-4.86953, "ay":-8.19219, "alpha":-5.12375, "fx":[-59.3612,-88.00514,-76.79248,-51.93892], "fy":[-121.94393,-103.63328,-112.8573,-126.05466]},
+ {"t":11.52447, "x":7.82678, "y":6.58085, "heading":1.39251, "vx":-2.69059, "vy":3.44252, "omega":-0.22579, "ax":-2.13898, "ay":-9.28514, "alpha":-7.82548, "fx":[-2.76878,-59.5276,-49.65821,-9.32382], "fy":[-137.10736,-123.58921,-128.4131,-137.34915]},
+ {"t":11.56569, "x":7.71405, "y":6.71487, "heading":1.37656, "vx":-2.77876, "vy":3.05977, "omega":-0.54836, "ax":-1.21548, "ay":-9.43285, "alpha":-8.37647, "fx":[15.64488,-46.66105,-40.49726,2.5967], "fy":[-136.38307,-129.03954,-131.66417,-137.74704]},
+ {"t":11.60532, "x":7.60298, "y":6.82872, "heading":1.34825, "vx":-2.82693, "vy":2.68596, "omega":-0.88031, "ax":-0.49412, "ay":-9.4377, "alpha":-8.95825, "fx":[30.55388,-35.85182,-34.09718,11.37871], "fy":[-133.32684,-131.76378,-133.05989,-136.95826]},
+ {"t":11.64495, "x":7.49056, "y":6.92775, "heading":1.30633, "vx":-2.84651, "vy":2.31196, "omega":-1.23531, "ax":0.32761, "ay":-9.33182, "alpha":-10.03344, "fx":[48.93036,-23.26918,-28.29998,21.21396], "fy":[-126.86782,-133.28061,-133.73041,-135.22664]},
+ {"t":11.68458, "x":7.37802, "y":7.01204, "heading":1.2495, "vx":-2.83353, "vy":1.94215, "omega":-1.63293, "ax":1.62849, "ay":-8.93509, "alpha":-12.05331, "fx":[76.98615,0.31555,-20.97885,36.01088], "fy":[-110.21392,-131.87815,-133.58461,-130.93426]},
+ {"t":11.72421, "x":7.26701, "y":7.08199, "heading":1.17532, "vx":-2.769, "vy":1.58807, "omega":-2.11058, "ax":6.24572, "ay":-5.74325, "alpha":-15.51519, "fx":[121.36207,113.54298,36.77721,82.44427], "fy":[-50.8369,-47.75804,-123.35681,-103.68495]},
+ {"t":11.76384, "x":7.16218, "y":7.14041, "heading":1.0795, "vx":-2.52149, "vy":1.36047, "omega":-2.72543, "ax":8.32525, "ay":3.80465, "alpha":-7.24182, "fx":[122.6384,104.20672,115.41699,129.77144], "fy":[52.04597,81.44296,58.98811,23.2427]},
+ {"t":11.80347, "x":7.06879, "y":7.19731, "heading":0.96581, "vx":-2.19157, "vy":1.51124, "omega":-3.01241, "ax":8.62953, "ay":2.84284, "alpha":-6.64977, "fx":[126.46099,112.74217,120.07379,130.00939], "fy":[38.05507,66.68332,44.97008,11.47767]},
+ {"t":11.84309, "x":6.98872, "y":7.25943, "heading":0.84121, "vx":-1.84959, "vy":1.6239, "omega":-3.27593, "ax":8.2577, "ay":-1.4505, "alpha":-7.43767, "fx":[123.4044,120.1586,112.1373,112.50336], "fy":[-15.72159,10.26842,-28.66037,-48.12851]},
+ {"t":11.88272, "x":6.92191, "y":7.32265, "heading":0.70555, "vx":-1.52235, "vy":1.56642, "omega":-3.57067, "ax":-4.27732, "ay":-6.6554, "alpha":4.99802, "fx":[-74.99194,-55.81335,-44.22384,-67.49083], "fy":[-87.24634,-101.93693,-102.28602,-85.88552]},
+ {"t":11.92235, "x":6.85822, "y":7.37949, "heading":0.56797, "vx":-1.69186, "vy":1.30268, "omega":-3.37261, "ax":-8.00206, "ay":-3.85164, "alpha":9.22918, "fx":[-125.20606,-104.62595,-98.15746,-125.71947], "fy":[-38.66916,-78.46661,-80.02274,-21.22606]},
+ {"t":11.96198, "x":6.78489, "y":7.42809, "heading":0.44157, "vx":-2.00897, "vy":1.15004, "omega":-3.00687, "ax":-8.74627, "ay":-2.64994, "alpha":9.44414, "fx":[-132.69227,-117.76146,-113.4561,-131.99562], "fy":[-19.86582,-63.87952,-66.12794,-0.37559]},
+ {"t":12.00161, "x":6.69841, "y":7.47159, "heading":0.32983, "vx":-2.35557, "vy":1.04503, "omega":-2.63261, "ax":-9.02112, "ay":-2.0897, "alpha":9.43549, "fx":[-135.14788,-123.58356,-119.0551,-133.70255], "fy":[-9.96252,-55.35833,-60.55777,7.39445]},
+ {"t":12.04124, "x":6.59798, "y":7.51136, "heading":0.23291, "vx":-2.71306, "vy":0.96222, "omega":-2.2587, "ax":-9.10791, "ay":-2.00472, "alpha":9.55584, "fx":[-136.16936,-125.79896,-119.64747,-134.79421], "fy":[-6.08115,-52.44448,-62.41568,7.2758]},
+ {"t":12.08087, "x":6.48332, "y":7.54792, "heading":0.1509, "vx":-3.074, "vy":0.88277, "omega":-1.88001, "ax":-9.07162, "ay":-2.29902, "alpha":9.89383, "fx":[-136.69596,-125.65732,-116.28421,-135.71461], "fy":[-6.73998,-54.37862,-70.37825,1.14465]},
+ {"t":12.12049, "x":6.35437, "y":7.58109, "heading":0.08417, "vx":-3.43349, "vy":0.79166, "omega":-1.48793, "ax":-8.91661, "ay":-2.85298, "alpha":10.50515, "fx":[-136.91881,-123.70679,-108.99018,-135.94773], "fy":[-10.21267,-59.80712,-82.46108,-9.28012]},
+ {"t":12.16012, "x":6.21131, "y":7.61023, "heading":0.03345, "vx":-3.78684, "vy":0.67861, "omega":-1.07163, "ax":-8.63934, "ay":-3.52345, "alpha":11.48074, "fx":[-136.90734,-120.35845,-97.65504,-134.92175], "fy":[-14.36716,-67.09088,-96.45239,-21.86604]},
+ {"t":12.19975, "x":6.05446, "y":7.63435, "heading":0.0, "vx":-4.12921, "vy":0.53898, "omega":-0.61667, "ax":-6.54556, "ay":-6.67273, "alpha":9.98177, "fx":[-123.01507,-96.32322,-57.54267,-94.24613], "fy":[-59.60507,-97.67261,-124.00764,-97.05213]},
+ {"t":12.26161, "x":5.78652, "y":7.65492, "heading":-0.01905, "vx":-4.5341, "vy":0.12622, "omega":0.00077, "ax":0.00015, "ay":-4.48929, "alpha":-0.01036, "fx":[0.0225,0.0233,-0.01833,-0.01911], "fy":[-63.64841,-63.62123,-63.62071,-63.6479]},
+ {"t":12.32346, "x":5.50605, "y":7.65414, "heading":-0.01902, "vx":-4.53409, "vy":-0.15147, "omega":0.00013, "ax":-0.03397, "ay":1.46294, "alpha":-0.00152, "fx":[-0.47899,-0.47889,-0.48405,-0.48415], "fy":[20.73432,20.73926,20.73935,20.73441]},
+ {"t":12.38532, "x":5.22552, "y":7.64757, "heading":-0.01902, "vx":-4.53619, "vy":-0.06098, "omega":0.00004, "ax":4.85804, "ay":1.01266, "alpha":1.43588, "fx":[66.82078,67.24701,70.895,70.48338], "fy":[17.64004,11.66747,11.1626,16.94668]},
+ {"t":12.44718, "x":4.95422, "y":7.64574, "heading":-0.01627, "vx":-4.23569, "vy":0.00166, "omega":0.08886, "ax":9.7523, "ay":0.08059, "alpha":-0.27159, "fx":[138.24814,138.2339,138.22443,138.23943], "fy":[0.22899,2.01162,2.06713,0.26136]},
+ {"t":12.50903, "x":4.71087, "y":7.646, "heading":-0.01129, "vx":-3.63244, "vy":0.00665, "omega":0.07206, "ax":9.77606, "ay":0.04849, "alpha":-0.27485, "fx":[138.57998,138.57127,138.56641,138.57562], "fy":[-0.23167,1.57687,1.61835,-0.21403]},
+ {"t":12.57089, "x":4.50489, "y":7.6465, "heading":-0.00736, "vx":-3.02772, "vy":0.00965, "omega":0.05506, "ax":9.78407, "ay":0.02132, "alpha":-0.24271, "fx":[138.68994,138.68656,138.68378,138.68737], "fy":[-0.50529,1.09448,1.11913,-0.49926]},
+ {"t":12.63275, "x":4.33632, "y":7.64714, "heading":-0.00442, "vx":-2.42251, "vy":0.01097, "omega":0.04004, "ax":9.78808, "ay":-0.00474, "alpha":-0.20648, "fx":[138.74416,138.74485,138.74315,138.7425], "fy":[-0.75129,0.61147,0.6236,-0.75266]},
+ {"t":12.69461, "x":4.2052, "y":7.64781, "heading":-0.00233, "vx":-1.81705, "vy":0.01067, "omega":0.02727, "ax":9.79043, "ay":-0.0308, "alpha":-0.17344, "fx":[138.77574,138.77934,138.77822,138.77459], "fy":[-1.00899,0.13686,0.1407,-1.01468]},
+ {"t":12.75646, "x":4.11153, "y":7.64841, "heading":-0.00098, "vx":-1.21145, "vy":0.00877, "omega":0.01654, "ax":9.79191, "ay":-0.0573, "alpha":-0.14535, "fx":[138.79555,138.80116,138.80038,138.7947], "fy":[-1.29043,-0.32938,-0.33072,-1.29848]},
+ {"t":12.81832, "x":4.05533, "y":7.64884, "heading":-0.00023, "vx":-0.60575, "vy":0.00522, "omega":0.00755, "ax":9.79285, "ay":-0.08443, "alpha":-0.1221, "fx":[138.80819,138.81513,138.81455,138.80753], "fy":[-1.59728,-0.78944,-0.79386,-1.60643]},
+ {"t":12.88018, "x":4.03659, "y":7.649, "heading":0.0, "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,186]
+ },
+ "events":[]
+}
--- /dev/null
+{
+ "name":"liberal",
+ "version":3,
+ "snapshot":{
+ "waypoints":[
+ {"x":4.420742511749268, "y":7.652679920196533, "heading":-1.5707963267948966, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":8.613431930541992, "y":6.143284797668457, "heading":-2.309870115554166, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":7.056009769439697, "y":5.0790486335754395, "heading":2.511558587727175, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":5.9137725830078125, "y":7.295578479766846, "heading":0.1300731703999055, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":4.036591053009033, "y":7.649001598358154, "heading":0.0, "intervals":18, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":5.9600324630737305, "y":6.425201892852783, "heading":-1.5493711192251445, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":5.909896373748779, "y":4.600945472717285, "heading":-0.7175417284567359, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":7.224385738372803, "y":4.976198196411133, "heading":1.3569870388467742, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":7.079377174377441, "y":6.611003398895264, "heading":1.1902896090630062, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":4.036591053009033, "y":7.649001598358154, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true},
+ {"from":"first", "to":4, "data":{"type":"KeepOutCircle", "props":{"x":4.889016749337316, "y":5.995364322792739, "r":1.0}}, "enabled":true},
+ {"from":4, "to":"last", "data":{"type":"KeepOutCircle", "props":{"x":4.492599055171013, "y":6.150994903218816, "r":1.0331388199726266}}, "enabled":true},
+ {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":5, "to":7, "data":{"type":"KeepInRectangle", "props":{"x":5.236986309289932, "y":3.979900181293487, "w":3.833156108856201, "h":3.509829044342041}}, "enabled":true}],
+ "targetDt":0.05
+ },
+ "params":{
+ "waypoints":[
+ {"x":{"exp":"4.420742511749268 m", "val":4.420742511749268}, "y":{"exp":"7.652679920196533 m", "val":7.652679920196533}, "heading":{"exp":"-90 deg", "val":-1.5707963267948966}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"8.613431930541992 m", "val":8.613431930541992}, "y":{"exp":"6.143284797668457 m", "val":6.143284797668457}, "heading":{"exp":"-2.309870115554166 rad", "val":-2.309870115554166}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"7.056009769439697 m", "val":7.056009769439697}, "y":{"exp":"5.0790486335754395 m", "val":5.0790486335754395}, "heading":{"exp":"2.511558587727175 rad", "val":2.511558587727175}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"5.9137725830078125 m", "val":5.9137725830078125}, "y":{"exp":"7.295578479766846 m", "val":7.295578479766846}, "heading":{"exp":"0.1300731703999055 rad", "val":0.1300731703999055}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"shootPos.x", "val":4.036591053009033}, "y":{"exp":"shootPos.y", "val":7.649001598358154}, "heading":{"exp":"shootPos.heading", "val":0.0}, "intervals":18, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"5.9600324630737305 m", "val":5.9600324630737305}, "y":{"exp":"6.425201892852783 m", "val":6.425201892852783}, "heading":{"exp":"-1.5493711192251445 rad", "val":-1.5493711192251445}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"5.909896373748779 m", "val":5.909896373748779}, "y":{"exp":"4.600945472717285 m", "val":4.600945472717285}, "heading":{"exp":"-0.7175417284567359 rad", "val":-0.7175417284567359}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"7.224385738372803 m", "val":7.224385738372803}, "y":{"exp":"4.976198196411133 m", "val":4.976198196411133}, "heading":{"exp":"1.3569870388467742 rad", "val":1.3569870388467742}, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"7.079377174377441 m", "val":7.079377174377441}, "y":{"exp":"6.611003398895264 m", "val":6.611003398895264}, "heading":{"exp":"1.1902896090630062 rad", "val":1.1902896090630062}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"shootPos.x", "val":4.036591053009033}, "y":{"exp":"shootPos.y", "val":7.649001598358154}, "heading":{"exp":"shootPos.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true},
+ {"from":"first", "to":4, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"4.889016749337316 m", "val":4.889016749337316}, "y":{"exp":"5.995364322792739 m", "val":5.995364322792739}, "r":{"exp":"1 m", "val":1.0}}}, "enabled":true},
+ {"from":4, "to":"last", "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"4.492599055171013 m", "val":4.492599055171013}, "y":{"exp":"6.150994903218816 m", "val":6.150994903218816}, "r":{"exp":"1.0331388199726266 m", "val":1.0331388199726266}}}, "enabled":true},
+ {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":5, "to":7, "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"5.236986309289932 m", "val":5.236986309289932}, "y":{"exp":"3.9799001812934875 m", "val":3.979900181293487}, "w":{"exp":"3.833156108856201 m", "val":3.833156108856201}, "h":{"exp":"3.509829044342041 m", "val":3.509829044342041}}}, "enabled":true}],
+ "targetDt":{
+ "exp":"0.05 s",
+ "val":0.05
+ }
+ },
+ "trajectory":{
+ "config":{
+ "frontLeft":{
+ "x":0.52705,
+ "y":0.52705
+ },
+ "backLeft":{
+ "x":-0.52705,
+ "y":0.52705
+ },
+ "mass":56.69904625,
+ "inertia":7.0,
+ "gearing":7.03125,
+ "radius":0.0508,
+ "vmax":628.3185307179587,
+ "tmax":7.0,
+ "cof":1.0,
+ "bumper":{
+ "front":0.4064,
+ "side":0.4064,
+ "back":0.4064
+ },
+ "differentialTrackWidth":0.5588
+ },
+ "sampleType":"Swerve",
+ "waypoints":[0.0,1.34621,2.07615,3.01406,3.68029,4.64137,5.19971,5.85499,6.30045,7.43645],
+ "samples":[
+ {"t":0.0, "x":4.42074, "y":7.65268, "heading":-1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.66812, "ay":-1.34927, "alpha":-3.29992, "fx":[135.04281,136.04726,138.59088,138.49219], "fy":[-32.05199,-27.56147,-7.78018,-9.10859]},
+ {"t":0.05385, "x":4.43476, "y":7.65072, "heading":-1.57558, "vx":0.52061, "vy":-0.07266, "omega":-0.1777, "ax":9.67123, "ay":-1.33091, "alpha":-3.20645, "fx":[135.18875,136.11277,138.57022,138.47754], "fy":[-31.35558,-27.15921,-7.87652,-9.06976]},
+ {"t":0.1077, "x":4.47682, "y":7.64488, "heading":-1.5898, "vx":1.0414, "vy":-0.14432, "omega":-0.35036, "ax":9.67437, "ay":-1.31629, "alpha":-3.05426, "fx":[135.38308,136.17127,138.52552,138.44768], "fy":[-30.40224,-26.75866,-8.2923,-9.17907]},
+ {"t":0.16155, "x":4.54692, "y":7.6352, "heading":-1.61309, "vx":1.56235, "vy":-0.2152, "omega":-0.51483, "ax":9.67799, "ay":-1.30518, "alpha":-2.794, "fx":[135.65542,136.2501,138.44169,138.38586], "fy":[-29.01223,-26.19984,-9.16104,-9.6293]},
+ {"t":0.21539, "x":4.64508, "y":7.62172, "heading":-1.64487, "vx":2.08349, "vy":-0.28549, "omega":-0.66528, "ax":9.68226, "ay":-1.29777, "alpha":-2.34185, "fx":[136.0389,136.39446,138.28544,138.25624], "fy":[-26.91728,-25.18961,-10.71233,-10.76338]},
+ {"t":0.26924, "x":4.77131, "y":7.60447, "heading":-1.68409, "vx":2.60487, "vy":-0.35537, "omega":-0.79138, "ax":9.68619, "ay":-1.29441, "alpha":-1.51716, "fx":[136.57699,136.69241,137.96696,137.96137], "fy":[-23.59632,-23.04935,-13.45724,-13.28875]},
+ {"t":0.32309, "x":4.92562, "y":7.58345, "heading":-1.7289, "vx":3.12645, "vy":-0.42507, "omega":-0.87308, "ax":9.68124, "ay":-1.29531, "alpha":0.21315, "fx":[137.32246,137.32327,137.13535,137.13609], "fy":[-17.68319,-17.64036,-19.04535,-19.07394]},
+ {"t":0.37694, "x":5.10801, "y":7.55869, "heading":-1.77561, "vx":3.64777, "vy":-0.49482, "omega":-0.8616, "ax":9.55521, "ay":-1.29481, "alpha":5.70761, "fx":[138.03039,137.78975,132.53843,133.41284], "fy":[-3.71583,3.60612,-37.75353,-35.55131]},
+ {"t":0.43079, "x":5.31829, "y":7.53016, "heading":-1.81373, "vx":4.16231, "vy":-0.56454, "omega":-0.55426, "ax":6.21196, "ay":-0.58364, "alpha":10.28588, "fx":[101.09067,82.37991,74.31064,94.43107], "fy":[7.56631,22.92404,-31.01584,-32.56646]},
+ {"t":0.48464, "x":5.55144, "y":7.49892, "heading":-1.82866, "vx":4.49681, "vy":-0.59597, "omega":-0.00038, "ax":0.00461, "ay":0.02765, "alpha":0.00042, "fx":[0.06618,0.06483,0.06447,0.06582], "fy":[0.39247,0.39282,0.39147,0.39111]},
+ {"t":0.53849, "x":5.79359, "y":7.46687, "heading":-1.82868, "vx":4.49706, "vy":-0.59448, "omega":-0.00036, "ax":-0.00114, "ay":-0.0084, "alpha":0.00001, "fx":[-0.01611,-0.01614,-0.01615,-0.01612], "fy":[-0.11913,-0.11912,-0.11915,-0.11915]},
+ {"t":0.59233, "x":6.03575, "y":7.43484, "heading":-1.8287, "vx":4.497, "vy":-0.59494, "omega":-0.00035, "ax":-0.00082, "ay":-0.00601, "alpha":0.0, "fx":[-0.01165,-0.01166,-0.01166,-0.01165], "fy":[-0.08512,-0.08512,-0.08513,-0.08513]},
+ {"t":0.64618, "x":6.2779, "y":7.4028, "heading":-1.82872, "vx":4.49696, "vy":-0.59526, "omega":-0.00035, "ax":-0.00125, "ay":-0.00923, "alpha":0.0, "fx":[-0.01772,-0.01771,-0.0177,-0.01771], "fy":[-0.13085,-0.13085,-0.13084,-0.13084]},
+ {"t":0.70003, "x":6.52006, "y":7.37073, "heading":-1.82874, "vx":4.49689, "vy":-0.59576, "omega":-0.00035, "ax":-0.01445, "ay":-0.10833, "alpha":-0.00001, "fx":[-0.20483,-0.20481,-0.20481,-0.20483], "fy":[-1.53556,-1.53556,-1.53554,-1.53554]},
+ {"t":0.75388, "x":6.76219, "y":7.33849, "heading":-1.82876, "vx":4.49611, "vy":-0.60159, "omega":-0.00035, "ax":-0.17448, "ay":-1.23395, "alpha":-0.00002, "fx":[-2.47332,-2.47326,-2.47324,-2.4733], "fy":[-17.49095,-17.49096,-17.4909,-17.49089]},
+ {"t":0.80773, "x":7.00404, "y":7.30431, "heading":-1.82877, "vx":4.48672, "vy":-0.66804, "omega":-0.00036, "ax":-1.30592, "ay":-6.82165, "alpha":-0.00016, "fx":[-18.5115,-18.51073,-18.51062,-18.5114], "fy":[-96.69527,-96.69544,-96.69519,-96.69502]},
+ {"t":0.86158, "x":7.24375, "y":7.25844, "heading":-1.82879, "vx":4.41639, "vy":-1.03537, "omega":-0.00036, "ax":-2.61611, "ay":-8.90737, "alpha":-0.00053, "fx":[-37.08432,-37.08112,-37.08116,-37.08436], "fy":[-126.25947,-126.26041,-126.26021,-126.25927]},
+ {"t":0.91542, "x":7.47777, "y":7.18978, "heading":-1.82881, "vx":4.27552, "vy":-1.51502, "omega":-0.00039, "ax":-3.74539, "ay":-8.85101, "alpha":-0.01439, "fx":[-53.12701,-53.04151,-53.05302,-53.13857], "fy":[-125.44649,-125.48238,-125.47556,-125.43963]},
+ {"t":0.96927, "x":7.70257, "y":7.09536, "heading":-1.82886, "vx":4.07384, "vy":-1.99164, "omega":-0.00117, "ax":-6.99765, "ay":-6.65885, "alpha":-4.22989, "fx":[-102.55171,-86.12825,-95.83977,-112.24039], "fy":[-92.02936,-107.4214,-98.49518,-79.60468]},
+ {"t":1.02312, "x":7.9118, "y":6.97846, "heading":-1.83505, "vx":3.69702, "vy":-2.3502, "omega":-0.22894, "ax":-9.15156, "ay":-2.76516, "alpha":-8.33781, "fx":[-130.26028,-115.21601,-135.15136,-138.25732], "fy":[-46.86548,-76.47313,-28.2187,-5.22464]},
+ {"t":1.07697, "x":8.09761, "y":6.8479, "heading":-1.85947, "vx":3.20422, "vy":-2.4991, "omega":-0.67792, "ax":-9.46213, "ay":-1.45044, "alpha":-8.46445, "fx":[-134.97503,-125.10595,-138.35826,-138.05445], "fy":[-31.55057,-59.44129,-3.24999,12.0033]},
+ {"t":1.13082, "x":8.25643, "y":6.71122, "heading":-1.90825, "vx":2.6947, "vy":-2.57721, "omega":-1.13372, "ax":-9.5728, "ay":-0.84121, "alpha":-7.82377, "fx":[-136.85104,-129.99581,-138.47749,-137.44429], "fy":[-22.53841,-48.13909,4.59082,18.39106]},
+ {"t":1.18467, "x":8.38766, "y":6.57122, "heading":-1.98064, "vx":2.17922, "vy":-2.62251, "omega":-1.55502, "ax":-9.6482, "ay":-0.47018, "alpha":-6.69206, "fx":[-137.91355,-133.40775,-138.51925,-137.20303], "fy":[-15.11529,-37.91605,5.91396,20.45842]},
+ {"t":1.23852, "x":8.49102, "y":6.42932, "heading":-2.07408, "vx":1.65968, "vy":-2.64782, "omega":-1.91537, "ax":-9.7052, "ay":-0.20039, "alpha":-5.35969, "fx":[-138.50971,-135.86875,-138.61488,-137.28196], "fy":[-8.45097,-28.06044,5.00895,20.1406]},
+ {"t":1.29236, "x":8.56632, "y":6.28645, "heading":-2.18499, "vx":1.13707, "vy":-2.65862, "omega":-2.20398, "ax":-9.73843, "ay":-0.00281, "alpha":-4.27814, "fx":[-138.75331,-137.27925,-138.68842,-137.43858], "fy":[-3.05772,-20.2644,3.95888,19.20392]},
+ {"t":1.34621, "x":8.61343, "y":6.14328, "heading":-2.30987, "vx":0.61267, "vy":-2.65877, "omega":-2.43436, "ax":-9.77064, "ay":0.25174, "alpha":-2.26346, "fx":[-138.72832,-138.56484,-138.6623,-138.03026], "fy":[2.86145,-7.07897,4.40255,14.08833]},
+ {"t":1.39183, "x":8.63121, "y":6.02225, "heading":-2.42328, "vx":0.16692, "vy":-2.64728, "omega":-2.53762, "ax":-9.77157, "ay":0.53855, "alpha":-0.28634, "fx":[-138.51381,-138.58031,-138.51193,-138.43248], "fy":[7.65014,6.29325,7.61768,8.97448]},
+ {"t":1.43746, "x":8.62866, "y":5.90204, "heading":-2.53935, "vx":-0.27887, "vy":-2.62271, "omega":-2.55068, "ax":-9.74188, "ay":0.85083, "alpha":1.41944, "fx":[-138.21432,-137.42967,-138.12401,-138.58717], "fy":[11.39802,18.68223,12.67553,5.4856]},
+ {"t":1.48308, "x":8.6058, "y":5.78327, "heading":-2.65424, "vx":-0.72331, "vy":-2.5839, "omega":-2.48593, "ax":-9.68072, "ay":1.22366, "alpha":2.94274, "fx":[-137.84085,-135.14509,-137.29806,-138.60373], "fy":[14.77432,30.99984,19.53522,4.07092]},
+ {"t":1.5287, "x":8.56273, "y":5.66667, "heading":-2.76459, "vx":-1.16495, "vy":-2.52807, "omega":-2.35167, "ax":-9.58101, "ay":1.69486, "alpha":4.17661, "fx":[-137.26154,-131.70075,-135.77305,-138.49903], "fy":[18.94713,43.19691,28.08675,5.8659]},
+ {"t":1.57432, "x":8.49961, "y":5.5531, "heading":-2.86753, "vx":-1.60205, "vy":-2.45075, "omega":-2.16113, "ax":-9.41919, "ay":2.33245, "alpha":5.01844, "fx":[-136.01401,-126.86503,-133.13486,-138.04496], "fy":[26.0089,55.65881,38.53895,12.04084]},
+ {"t":1.61994, "x":8.41672, "y":5.44372, "heading":-2.9609, "vx":-2.03177, "vy":-2.34434, "omega":-1.93218, "ax":-9.12712, "ay":3.24943, "alpha":5.34519, "fx":[-132.78287,-119.84192,-128.5273,-136.34704], "fy":[38.93169,69.33197,51.69347,24.28228]},
+ {"t":1.66556, "x":8.31453, "y":5.34015, "heading":-3.04349, "vx":-2.44816, "vy":-2.1961, "omega":-1.68833, "ax":-8.5288, "ay":4.60374, "alpha":4.94716, "fx":[-124.08862,-108.899,-119.9461,-130.64098], "fy":[60.94666,85.32838,69.11978,45.63286]},
+ {"t":1.71118, "x":8.19397, "y":5.24475, "heading":-3.11536, "vx":-2.83725, "vy":-1.98607, "omega":-1.46263, "ax":-7.22916, "ay":6.49483, "alpha":3.45515, "fx":[-103.09115,-91.30085,-102.61297,-112.88159], "fy":[91.94702,103.78823,92.72312,79.79206]},
+ {"t":1.7568, "x":8.05701, "y":5.1609, "heading":3.10469, "vx":-3.16705, "vy":-1.68977, "omega":-1.30501, "ax":-4.69841, "ay":8.53636, "alpha":0.57366, "fx":[-66.01624,-64.3297,-67.1709,-68.87837], "fy":[121.3175,122.24132,120.71559,119.72884]},
+ {"t":1.80243, "x":7.90763, "y":5.09269, "heading":3.04575, "vx":-3.3814, "vy":-1.30033, "omega":-1.27883, "ax":-1.26815, "ay":9.63973, "alpha":-2.87138, "fx":[-24.48769,-29.72792,-10.35959,-7.32779], "fy":[136.01659,134.8687,137.68832,137.98963]},
+ {"t":1.84805, "x":7.75205, "y":5.0434, "heading":2.98442, "vx":-3.43926, "vy":-0.86055, "omega":-1.40983, "ax":1.66407, "ay":9.53163, "alpha":-5.29128, "fx":[8.47742,4.76564,42.79982,38.30826], "fy":[138.06588,138.08064,131.37036,132.91735]},
+ {"t":1.89367, "x":7.59688, "y":5.01406, "heading":2.9146, "vx":-3.36334, "vy":-0.42571, "omega":-1.65123, "ax":3.54625, "ay":8.97658, "alpha":-6.31162, "fx":[31.40465,31.45403,74.04725,64.16304], "fy":[134.82501,134.64245,116.81189,122.68419]},
+ {"t":1.93929, "x":7.44713, "y":5.00398, "heading":2.8327, "vx":-3.20155, "vy":-0.01618, "omega":-1.93917, "ax":4.67106, "ay":8.45244, "alpha":-6.34485, "fx":[47.57056,49.43954,89.15135,78.68295], "fy":[130.09359,129.24541,105.88482,114.02159]},
+ {"t":1.98491, "x":7.30593, "y":5.01204, "heading":2.73763, "vx":-2.98846, "vy":0.36943, "omega":-2.22863, "ax":5.35781, "ay":8.06548, "alpha":-5.73091, "fx":[59.82918,61.45113,95.42151,87.08081], "fy":[125.00096,124.09528,100.38923,107.81967]},
+ {"t":2.03053, "x":7.17517, "y":5.03729, "heading":2.62999, "vx":-2.74403, "vy":0.73738, "omega":-2.49008, "ax":5.79163, "ay":7.8036, "alpha":-4.64543, "fx":[69.75996,70.19007,96.74097,91.68896], "fy":[119.79723,119.46422,99.21108,103.98415]},
+ {"t":2.07615, "x":7.05601, "y":5.07905, "heading":2.51156, "vx":-2.4798, "vy":1.09339, "omega":-2.70201, "ax":6.13728, "ay":7.56957, "alpha":-3.32105, "fx":[78.77776,78.24996,96.47468,94.4754], "fy":[114.02694,114.32373,99.43128,101.40555]},
+ {"t":2.11879, "x":6.95587, "y":5.13254, "heading":2.39335, "vx":-2.21816, "vy":1.4161, "omega":-2.84359, "ax":6.53048, "ay":7.25037, "alpha":-2.01282, "fx":[87.96613,87.29303,97.64707,97.36582], "fy":[106.97236,107.46857,98.15507,98.49301]},
+ {"t":2.16142, "x":6.86724, "y":5.1995, "heading":2.27029, "vx":-1.93975, "vy":1.7252, "omega":-2.9294, "ax":6.93546, "ay":6.86377, "alpha":-0.68019, "fx":[96.89436,96.5707,99.77729,99.99168], "fy":[98.74049,99.0309,95.79702,95.60055]},
+ {"t":2.20405, "x":6.79084, "y":5.27929, "heading":2.14479, "vx":-1.64408, "vy":2.01782, "omega":-2.9584, "ax":7.3447, "ay":6.3983, "alpha":0.61956, "fx":[105.25938,105.61558,103.00358,102.55891], "fy":[89.35912,88.97545,91.99163,92.45138]},
+ {"t":2.24668, "x":6.72743, "y":5.37113, "heading":2.01923, "vx":-1.33096, "vy":2.29059, "omega":-2.93199, "ax":7.75054, "ay":5.83366, "alpha":1.79911, "fx":[112.77594,113.90625,107.30229,105.46391], "fy":[78.77769,77.33013,86.29017,88.365]},
+ {"t":2.28931, "x":6.67773, "y":5.47408, "heading":1.89586, "vx":-1.00053, "vy":2.53929, "omega":-2.85529, "ax":8.14186, "ay":5.11682, "alpha":2.73036, "fx":[119.11523,120.85688,112.44922,109.21433], "fy":[66.48036,63.90847,77.85645,81.87332]},
+ {"t":2.33195, "x":6.64247, "y":5.58699, "heading":1.77662, "vx":-0.65343, "vy":2.75743, "omega":-2.73889, "ax":8.46231, "ay":4.04262, "alpha":3.10188, "fx":[123.00757,124.99505,117.6899,114.11216], "fy":[50.02553,47.21976,63.59364,68.37364]},
+ {"t":2.37458, "x":6.62231, "y":5.70822, "heading":1.66267, "vx":-0.29266, "vy":2.92978, "omega":-2.60665, "ax":8.05375, "ay":0.22947, "alpha":-0.31161, "fx":[114.29039,113.95908,114.02951,114.36099], "fy":[4.17168,4.0629,2.32314,2.453]},
+ {"t":2.41721, "x":6.61715, "y":5.83333, "heading":1.55126, "vx":0.05069, "vy":2.93956, "omega":-2.61993, "ax":6.18291, "ay":-5.32627, "alpha":-6.35086, "fx":[103.64298,86.66971,70.03594,90.21633], "fy":[-56.61346,-70.79944,-94.14593,-80.43584]},
+ {"t":2.45984, "x":6.62493, "y":5.95381, "heading":1.4338, "vx":0.31428, "vy":2.71249, "omega":-2.89068, "ax":0.46249, "ay":-7.66607, "alpha":-7.29546, "fx":[31.45033,-11.66587,-13.56988,20.00832], "fy":[-103.11692,-104.48817,-113.46621,-113.58779]},
+ {"t":2.50247, "x":6.63875, "y":6.06248, "heading":1.30393, "vx":0.334, "vy":2.38567, "omega":-3.20171, "ax":-4.5355, "ay":-6.15388, "alpha":-4.32774, "fx":[-56.27391,-75.77739,-71.36271,-53.74429], "fy":[-88.8021,-76.54151,-86.70249,-96.87328]},
+ {"t":2.54511, "x":6.64886, "y":6.15859, "heading":1.1635, "vx":0.14064, "vy":2.12332, "omega":-3.38621, "ax":-7.21794, "ay":-2.99147, "alpha":-0.76435, "fx":[-101.69874,-103.42068,-102.93988,-101.19112], "fy":[-42.19703,-39.68459,-42.61732,-45.11477]},
+ {"t":2.58774, "x":6.6483, "y":6.2464, "heading":1.01845, "vx":-0.16708, "vy":1.99578, "omega":-3.41879, "ax":-7.98241, "ay":0.29255, "alpha":2.10832, "fx":[-114.88085,-112.94026,-111.52164,-113.25203], "fy":[2.12742,-3.92782,6.41625,11.97122]},
+ {"t":2.63037, "x":6.63392, "y":6.33175, "heading":0.87461, "vx":-0.50739, "vy":2.00826, "omega":-3.32891, "ax":-7.49703, "ay":3.16016, "alpha":4.38438, "fx":[-111.88483,-110.96163,-100.29544,-101.93267], "fy":[39.18398,29.21575,52.16571,58.61271]},
+ {"t":2.673, "x":6.60548, "y":6.42023, "heading":0.73668, "vx":-0.827, "vy":2.14298, "omega":-3.14199, "ax":-6.34427, "ay":5.33986, "alpha":6.23278, "fx":[-101.10936,-101.8764,-76.32838,-80.39987], "fy":[67.86882,57.11395,86.96138,90.82094]},
+ {"t":2.71564, "x":6.56446, "y":6.51645, "heading":0.60839, "vx":-1.09747, "vy":2.37063, "omega":-2.87628, "ax":-5.30492, "ay":6.61767, "alpha":7.68375, "fx":[-91.06206,-94.04991,-54.30724,-61.3649], "fy":[85.79406,74.40903,106.24055,108.7717]},
+ {"t":2.75827, "x":6.51285, "y":6.62353, "heading":0.49275, "vx":-1.32363, "vy":2.65276, "omega":-2.5487, "ax":-5.54904, "ay":6.32523, "alpha":13.67994, "fx":[-100.93914,-114.69127,-44.74668,-54.24815], "fy":[81.50898,48.08402,111.9966,117.04488]},
+ {"t":2.8009, "x":6.45138, "y":6.74237, "heading":0.39653, "vx":-1.5602, "vy":2.92241, "omega":-1.9655, "ax":-9.20395, "ay":-2.1297, "alpha":8.00704, "fx":[-136.12896,-125.45957,-124.2402,-136.02627], "fy":[-14.08434,-54.44696,-54.7978,2.57694]},
+ {"t":2.84353, "x":6.3765, "y":6.86502, "heading":0.32001, "vx":-1.95258, "vy":2.83162, "omega":-1.62414, "ax":-9.06372, "ay":-3.16563, "alpha":6.28852, "fx":[-134.85368,-123.45601,-120.12544,-135.4689], "fy":[-28.34629,-61.22671,-66.77945,-23.13599]},
+ {"t":2.88616, "x":6.28502, "y":6.98286, "heading":0.25648, "vx":-2.33899, "vy":2.69666, "omega":-1.35605, "ax":-8.90666, "ay":-3.71435, "alpha":5.72954, "fx":[-133.50735,-121.6618,-116.59118,-133.23876], "fy":[-35.58358,-65.533,-73.7934,-35.6901]},
+ {"t":2.9288, "x":6.17721, "y":7.09445, "heading":0.20388, "vx":-2.7187, "vy":2.53831, "omega":-1.11178, "ax":-8.74338, "ay":-4.12416, "alpha":5.69213, "fx":[-132.38252,-119.70166,-112.65529,-131.00199], "fy":[-40.24791,-69.45287,-80.13566,-43.99943]},
+ {"t":2.97143, "x":6.05336, "y":7.19892, "heading":0.16165, "vx":-3.09145, "vy":2.36249, "omega":-0.86911, "ax":-8.57366, "ay":-4.46394, "alpha":6.02112, "fx":[-131.5764,-117.57685,-108.01767,-128.94771], "fy":[-43.20877,-73.23072,-86.53943,-50.122]},
+ {"t":3.01406, "x":5.91377, "y":7.29558, "heading":0.13007, "vx":-3.45696, "vy":2.17218, "omega":-0.61242, "ax":-8.13376, "ay":-5.21589, "alpha":6.28698, "fx":[-128.34604,-111.94306,-98.18132,-122.70628], "fy":[-52.22096,-81.71589,-97.70276,-64.0966]},
+ {"t":3.06958, "x":5.70931, "y":7.40814, "heading":0.10576, "vx":-3.90854, "vy":1.8826, "omega":-0.26337, "ax":-6.58789, "ay":-7.11884, "alpha":4.57847, "fx":[-108.36103,-93.07448,-76.9679,-95.1237], "fy":[-85.99279,-102.4326,-114.93269,-100.27309]},
+ {"t":3.1251, "x":5.48216, "y":7.50169, "heading":0.0982, "vx":-4.27429, "vy":1.48737, "omega":-0.00918, "ax":-2.77928, "ay":-9.32794, "alpha":0.13955, "fx":[-39.97113,-39.63747,-38.82196,-39.15205], "fy":[-132.04745,-132.15405,-132.39345,-132.29011]},
+ {"t":3.18062, "x":5.24057, "y":7.56989, "heading":0.0979, "vx":-4.4286, "vy":0.96949, "omega":-0.00143, "ax":2.68093, "ay":-9.26008, "alpha":-4.97444, "fx":[49.07797,57.50787,23.77874,21.64138], "fy":[-128.74468,-124.90416,-135.35704,-136.0321]},
+ {"t":3.23614, "x":4.99883, "y":7.60944, "heading":0.09015, "vx":-4.27976, "vy":0.45538, "omega":-0.27761, "ax":8.86796, "ay":-4.04499, "alpha":-1.38901, "fx":[125.10756,128.21792,126.47641,123.00289], "fy":[-58.86975,-51.69228,-55.71671,-63.06827]},
+ {"t":3.29166, "x":4.77489, "y":7.62849, "heading":0.0726, "vx":-3.78742, "vy":0.23081, "omega":-0.35473, "ax":9.58705, "ay":-1.89096, "alpha":0.44001, "fx":[136.09283,135.53843,135.70808,136.23712], "fy":[-25.78981,-28.57148,-27.79385,-25.06077]},
+ {"t":3.34717, "x":4.57939, "y":7.63839, "heading":0.05359, "vx":-3.25515, "vy":0.12582, "omega":-0.3303, "ax":9.72419, "ay":-1.02541, "alpha":0.97994, "fx":[138.13114,137.42187,137.58358,138.21582], "fy":[-11.78531,-18.30117,-17.14374,-10.90938]},
+ {"t":3.40269, "x":4.41366, "y":7.64379, "heading":0.03676, "vx":-2.71528, "vy":0.06889, "omega":-0.27589, "ax":9.76396, "ay":-0.59486, "alpha":1.08381, "fx":[138.60446,138.14143,138.22821,138.63333], "fy":[-5.12258,-12.43777,-11.56094,-4.60681]},
+ {"t":3.45821, "x":4.27796, "y":7.6467, "heading":0.02311, "vx":-2.17319, "vy":0.03587, "omega":-0.21572, "ax":9.77888, "ay":-0.34764, "alpha":1.05575, "fx":[138.73185,138.4674,138.51126,138.74274], "fy":[-1.554,-8.7102,-8.12708,-1.31974]},
+ {"t":3.51373, "x":4.17237, "y":7.64816, "heading":0.01276, "vx":-1.63028, "vy":0.01657, "omega":-0.15711, "ax":9.78543, "ay":-0.19195, "alpha":0.99729, "fx":[138.76762,138.62996,138.65271,138.77408], "fy":[0.55466,-6.20969,-5.83949,0.6113]},
+ {"t":3.56925, "x":4.09694, "y":7.64878, "heading":0.00558, "vx":-1.087, "vy":0.00591, "omega":-0.10174, "ax":9.7886, "ay":-0.08852, "alpha":0.93984, "fx":[138.77623,138.71661,138.72929,138.78231], "fy":[1.88556,-4.48582,-4.2553,1.83682]},
+ {"t":3.62477, "x":4.05168, "y":7.64897, "heading":0.00138, "vx":-0.54355, "vy":0.001, "omega":-0.04956, "ax":9.79027, "ay":-0.01795, "alpha":0.89266, "fx":[138.7769,138.76541,138.77329,138.78351], "fy":[2.75947,-3.28731,-3.14226,2.65235]},
+ {"t":3.68029, "x":4.03659, "y":7.649, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.74711, "ay":-0.78424, "alpha":-2.20658, "fx":[137.71087,138.75932,138.73632,137.44551], "fy":[-17.42044,-3.65865,-4.06509,-19.32161]},
+ {"t":3.73368, "x":4.05048, "y":7.64788, "heading":-0.00315, "vx":0.52043, "vy":-0.04187, "omega":-0.11782, "ax":9.73705, "ay":-0.82409, "alpha":-2.60433, "fx":[137.4802,138.75781,138.73395,137.10966], "fy":[-19.02677,-2.96542,-3.29986,-21.43306]},
+ {"t":3.78708, "x":4.09215, "y":7.64447, "heading":-0.01315, "vx":1.04032, "vy":-0.08587, "omega":-0.25687, "ax":9.72093, "ay":-0.88637, "alpha":-3.14035, "fx":[137.11795,138.74573,138.72062,136.58313], "fy":[-21.31882,-2.2444,-2.31181,-24.38116]},
+ {"t":3.84047, "x":4.16155, "y":7.63862, "heading":-0.03134, "vx":1.55935, "vy":-0.1332, "omega":-0.42454, "ax":9.69285, "ay":-0.98744, "alpha":-3.90946, "fx":[136.50215,138.71625,138.68309,135.67389], "fy":[-24.74371,-1.52656,-0.95115,-28.76573]},
+ {"t":3.89386, "x":4.25863, "y":7.63011, "heading":-0.05958, "vx":2.07689, "vy":-0.18592, "omega":-0.63328, "ax":9.63572, "ay":-1.169, "alpha":-5.14754, "fx":[135.29942,138.65366,138.58234,133.80073], "fy":[-30.34064,-0.91194,1.10055,-36.12913]},
+ {"t":3.94725, "x":4.38326, "y":7.61851, "heading":-0.10073, "vx":2.59137, "vy":-0.24834, "omega":-0.90813, "ax":9.47468, "ay":-1.57748, "alpha":-7.67583, "fx":[132.21791,138.50594,138.25951,128.22196], "fy":[-41.28961,-0.85567,4.68186,-51.97807]},
+ {"t":4.00065, "x":4.53512, "y":7.603, "heading":-0.16016, "vx":3.09725, "vy":-0.33257, "omega":-1.31796, "ax":8.52474, "ay":-4.53769, "alpha":-4.33188, "fx":[117.75175,128.54212,125.74201,111.30888], "fy":[-71.54016,-49.42575,-55.49479,-80.8222]},
+ {"t":4.05404, "x":4.71265, "y":7.57878, "heading":-0.2367, "vx":3.55241, "vy":-0.57485, "omega":-1.54926, "ax":0.65043, "ay":-9.45815, "alpha":3.3787, "fx":[1.38808,-3.63197,15.70272,23.42014], "fy":[-134.38578,-135.00464,-134.29331,-132.58408]},
+ {"t":4.10743, "x":4.90325, "y":7.5346, "heading":-0.31461, "vx":3.58714, "vy":-1.07985, "omega":-1.36886, "ax":0.25024, "ay":-6.83816, "alpha":3.74509, "fx":[-2.576,-7.77976,8.79241,15.75147], "fy":[-93.09788,-99.21834,-100.86342,-94.53768]},
+ {"t":4.16083, "x":5.09513, "y":7.4672, "heading":-0.38236, "vx":3.6005, "vy":-1.44496, "omega":-1.16889, "ax":-5.88231, "ay":-2.93712, "alpha":-6.85119, "fx":[-73.52772,-74.74654,-93.2897,-91.95727], "fy":[-62.41689,-39.19329,-20.38507,-44.5367]},
+ {"t":4.21422, "x":5.27899, "y":7.38586, "heading":-0.45453, "vx":3.28643, "vy":-1.60178, "omega":-1.5347, "ax":-8.23173, "ay":-2.47376, "alpha":-9.4186, "fx":[-102.85832,-114.8802,-126.17506,-122.81772], "fy":[-72.64987,-32.21561,2.85703,-38.25146]},
+ {"t":4.26761, "x":5.44273, "y":7.29681, "heading":-0.5499, "vx":2.84691, "vy":-1.73386, "omega":-2.03759, "ax":-8.92461, "ay":-1.04287, "alpha":-9.53507, "fx":[-118.09254,-127.11296,-129.2859,-131.52564], "fy":[-57.09674,-7.53993,25.02119,-19.51403]},
+ {"t":4.32101, "x":5.58202, "y":7.20275, "heading":-0.67229, "vx":2.3704, "vy":-1.78955, "omega":-2.5467, "ax":-9.10134, "ay":0.23811, "alpha":-9.04363, "fx":[-126.83166,-129.00518,-126.31403,-133.88633], "fy":[-37.98474,11.33879,41.25745,-1.11087]},
+ {"t":4.3744, "x":5.6956, "y":7.10754, "heading":-0.82115, "vx":1.88445, "vy":-1.77683, "omega":-3.02957, "ax":-8.99312, "ay":-2.4509, "alpha":-5.82961, "fx":[-120.97712,-124.77882,-132.94767,-131.198], "fy":[-57.2711,-43.8439,-9.01567,-28.83289]},
+ {"t":4.42779, "x":5.7834, "y":7.00917, "heading":-0.99122, "vx":1.40428, "vy":-1.90769, "omega":-3.34083, "ax":-6.46628, "ay":-7.05636, "alpha":3.11775, "fx":[-97.45448,-100.24915,-87.03276,-81.89539], "fy":[-94.54115,-92.20698,-104.88152,-108.45926]},
+ {"t":4.48119, "x":5.84916, "y":6.89726, "heading":-1.16516, "vx":1.05902, "vy":-2.28446, "omega":-3.17436, "ax":-4.75712, "ay":-8.22482, "alpha":8.60117, "fx":[-75.61759,-98.90012,-64.92066,-30.28561], "fy":[-114.4167,-95.90412,-121.72137,-134.29732]},
+ {"t":4.53458, "x":5.89893, "y":6.76356, "heading":-1.32238, "vx":0.80502, "vy":-2.72361, "omega":-2.71512, "ax":-4.36124, "ay":-8.34683, "alpha":10.77405, "fx":[-59.79124,-104.8947,-66.14542,-16.44679], "fy":[-124.1966,-90.11584,-121.61494,-137.32967]},
+ {"t":4.58797, "x":5.93569, "y":6.60624, "heading":-1.452, "vx":0.57216, "vy":-3.16927, "omega":-2.13986, "ax":-4.35738, "ay":-8.29318, "alpha":11.8408, "fx":[-49.22294,-109.54717,-71.57171,-16.71724], "fy":[-129.16916,-84.77117,-118.72407,-137.55107]},
+ {"t":4.64137, "x":5.96003, "y":6.4252, "heading":-1.54937, "vx":0.33951, "vy":-3.61207, "omega":-1.50764, "ax":-4.5025, "ay":-7.70908, "alpha":16.95324, "fx":[-35.76577,-127.11915,-82.65842,-9.74399], "fy":[-132.92764,-54.77659,-111.23631,-138.15701]},
+ {"t":4.68125, "x":5.96999, "y":6.27502, "heading":-1.59602, "vx":0.15994, "vy":-3.91952, "omega":-0.83152, "ax":-5.25452, "ay":-7.47507, "alpha":14.46527, "fx":[-57.87965,-125.93902,-88.22971,-25.87811], "fy":[-124.54738,-56.82546,-106.63047,-135.82629]},
+ {"t":4.72113, "x":5.97219, "y":6.11276, "heading":-1.61767, "vx":-0.04962, "vy":-4.21763, "omega":-0.25462, "ax":-6.90845, "ay":-6.53835, "alpha":8.04401, "fx":[-98.71228,-121.86123,-100.47272,-70.65654], "fy":[-94.81796,-63.67351,-94.26038,-117.96612]},
+ {"t":4.76101, "x":5.96472, "y":5.93935, "heading":-1.62143, "vx":-0.32513, "vy":-4.47839, "omega":0.06618, "ax":-9.50083, "ay":0.00402, "alpha":-1.56211, "fx":[-134.84854,-134.51451,-134.51247,-134.81271], "fy":[-4.53254,-5.39667,5.00791,5.14951]},
+ {"t":4.80089, "x":5.9442, "y":5.76075, "heading":-1.62003, "vx":-0.70404, "vy":-4.47823, "omega":0.00388, "ax":-4.30428, "ay":7.65081, "alpha":10.95253, "fx":[-24.23763,-71.89193,-101.21582,-46.70338], "fy":[128.16746,110.48095,80.23733,114.9076]},
+ {"t":4.84077, "x":5.9127, "y":5.58824, "heading":-1.61117, "vx":-0.8757, "vy":-4.17311, "omega":0.44068, "ax":3.08021, "ay":8.59951, "alpha":13.98346, "fx":[65.76708,4.93386,4.28108,99.66289], "fy":[120.80479,137.30193,135.83211,93.645]},
+ {"t":4.88065, "x":5.88022, "y":5.42865, "heading":-1.58247, "vx":-0.75286, "vy":-3.83015, "omega":0.99836, "ax":4.26092, "ay":8.21694, "alpha":12.94298, "fx":[76.06826,18.51318,35.67054,111.33839], "fy":[115.36472,136.80894,132.45992,81.2592]},
+ {"t":4.92054, "x":5.85358, "y":5.28243, "heading":-1.53236, "vx":-0.58293, "vy":-3.50244, "omega":1.51455, "ax":5.039, "ay":7.87104, "alpha":11.85072, "fx":[81.16388,28.14118,60.24533,116.15633], "fy":[112.12136,135.40604,123.8379,74.91536]},
+ {"t":4.96042, "x":5.83434, "y":5.14901, "heading":-1.46254, "vx":-0.38196, "vy":-3.18854, "omega":1.98717, "ax":5.47289, "ay":7.66543, "alpha":10.89382, "fx":[82.43903,34.77026,76.30259,116.79599], "fy":[111.33186,133.98973,115.03265,74.26808]},
+ {"t":5.0003, "x":5.82346, "y":5.02794, "heading":-1.37462, "vx":-0.1637, "vy":-2.88283, "omega":2.42163, "ax":5.75913, "ay":7.53161, "alpha":10.01487, "fx":[82.22698,41.03629,87.608,115.66614], "fy":[111.57432,132.28697,106.95326,76.22031]},
+ {"t":5.04018, "x":5.82151, "y":4.91896, "heading":-1.27008, "vx":0.06598, "vy":-2.58246, "omega":2.82104, "ax":5.97167, "ay":7.43846, "alpha":9.11682, "fx":[81.53451,48.01705,95.45783,113.57868], "fy":[112.13691,129.97238,100.21848,79.42596]},
+ {"t":5.08006, "x":5.82889, "y":4.82188, "heading":-1.15032, "vx":0.30414, "vy":-2.2858, "omega":3.18463, "ax":6.14764, "ay":7.36969, "alpha":8.05514, "fx":[80.99669,56.23326,100.48394,110.85125], "fy":[112.56412,126.67806,95.3369,83.27531]},
+ {"t":5.11994, "x":5.84591, "y":4.73658, "heading":-1.01691, "vx":0.54932, "vy":-1.99189, "omega":3.50588, "ax":6.30516, "ay":7.31679, "alpha":6.65985, "fx":[81.20222,65.8604,102.92336,107.51052], "fy":[112.44296,121.98944,92.81884,87.60358]},
+ {"t":5.15982, "x":5.87284, "y":4.66296, "heading":-0.87179, "vx":0.80078, "vy":-1.70008, "omega":3.77148, "ax":6.44428, "ay":7.2758, "alpha":4.82959, "fx":[82.85933,76.3476,102.7102,103.4674], "fy":[111.24704,115.75569,93.14534,92.38281]},
+ {"t":5.19971, "x":5.9099, "y":4.60095, "heading":-0.71754, "vx":1.05778, "vy":-1.40991, "omega":3.9641, "ax":6.52391, "ay":7.26842, "alpha":2.47074, "fx":[86.69578,86.1067,98.95495,98.14187], "fy":[108.26238,108.69727,97.14681,98.00592]},
+ {"t":5.24066, "x":5.95869, "y":4.5493, "heading":-0.55312, "vx":1.32497, "vy":-1.11224, "omega":4.06528, "ax":6.6304, "ay":7.18763, "alpha":-0.51152, "fx":[95.45402,94.96098,92.53756,92.9847], "fy":[100.51779,100.9932,103.21653,102.80443]},
+ {"t":5.28162, "x":6.01851, "y":4.50977, "heading":-0.38705, "vx":1.59652, "vy":-0.81787, "omega":4.04434, "ax":6.64429, "ay":7.11926, "alpha":-3.543, "fx":[105.42762,98.91989,83.56669,88.81063], "fy":[89.84212,97.03896,110.51388,106.26036]},
+ {"t":5.32257, "x":6.08947, "y":4.48225, "heading":-0.22439, "vx":1.86864, "vy":-0.5263, "omega":3.89923, "ax":6.47006, "ay":7.17456, "alpha":-6.05362, "fx":[111.8502,97.4951,72.22168,85.27902], "fy":[81.48296,98.35294,118.10558,108.84947]},
+ {"t":5.36353, "x":6.17143, "y":4.46671, "heading":-0.06977, "vx":2.13362, "vy":-0.23246, "omega":3.65131, "ax":6.33957, "ay":7.14997, "alpha":-8.21644, "fx":[117.27191,94.80893,61.20338,86.16315], "fy":[73.07579,100.73927,123.93663,107.64473]},
+ {"t":5.40448, "x":6.26413, "y":4.46319, "heading":0.07287, "vx":2.39326, "vy":0.06037, "omega":3.3148, "ax":6.31702, "ay":6.99667, "alpha":-9.98379, "fx":[121.16672,91.86306,52.15569,92.98355], "fy":[65.56923,102.9978,127.49978,100.63754]},
+ {"t":5.44544, "x":6.36744, "y":4.47153, "heading":0.20026, "vx":2.65197, "vy":0.34691, "omega":2.90591, "ax":6.51418, "ay":6.53439, "alpha":-11.47642, "fx":[123.50694,90.56983,49.49257,105.77835], "fy":[58.10167,102.68703,126.83213,82.87275]},
+ {"t":5.48639, "x":6.48152, "y":4.49122, "heading":0.30965, "vx":2.91876, "vy":0.61453, "omega":2.4359, "ax":0.95694, "ay":-2.98735, "alpha":-3.8263, "fx":[21.77768,18.69524,4.96101,8.82375], "fy":[-45.70069,-34.71895,-39.11398,-49.84616]},
+ {"t":5.52735, "x":6.60186, "y":4.51388, "heading":0.4062, "vx":2.95795, "vy":0.49218, "omega":2.27919, "ax":-9.07956, "ay":-0.61944, "alpha":10.80219, "fx":[-134.68597,-126.93537,-126.02143,-127.15967], "fy":[6.86152,-44.21614,-36.78792,39.02083]},
+ {"t":5.5683, "x":6.71538, "y":4.53352, "heading":0.5086, "vx":2.5861, "vy":0.46681, "omega":2.72159, "ax":-7.66744, "ay":5.47004, "alpha":8.32549, "fx":[-117.08914,-128.6082,-99.47873,-89.56073], "fy":[70.76924,44.53337,91.76628,103.07715]},
+ {"t":5.60926, "x":6.81487, "y":4.55722, "heading":0.62705, "vx":2.27208, "vy":0.69084, "omega":3.06257, "ax":-6.05644, "ay":7.49878, "alpha":4.48234, "fx":[-95.67629,-98.65904,-73.7899,-75.26933], "fy":[98.85943,95.44445,115.7245,115.14501]},
+ {"t":5.65021, "x":6.90284, "y":4.5918, "heading":0.75624, "vx":2.02403, "vy":0.99795, "omega":3.24614, "ax":-5.00959, "ay":8.34308, "alpha":0.78428, "fx":[-73.61376,-72.73318,-68.32053,-69.37141], "fy":[116.70289,117.21262,119.84748,119.28172]},
+ {"t":5.69117, "x":6.98153, "y":4.63967, "heading":0.88984, "vx":1.81887, "vy":1.33964, "omega":3.27826, "ax":-4.355, "ay":8.70805, "alpha":-2.53117, "fx":[-51.45185,-58.39714,-71.25166,-65.82378], "fy":[128.31535,125.38,118.51577,121.52701]},
+ {"t":5.73212, "x":7.05237, "y":4.70184, "heading":1.02198, "vx":1.64051, "vy":1.69628, "omega":3.1746, "ax":-3.96977, "ay":8.8381, "alpha":-5.26966, "fx":[-33.03366,-53.14255,-77.21332,-61.69277], "fy":[134.4358,127.91367,114.93712,123.82533]},
+ {"t":5.77308, "x":7.11623, "y":4.77872, "heading":1.14757, "vx":1.47792, "vy":2.05825, "omega":2.95878, "ax":-3.76723, "ay":8.8508, "alpha":-7.39998, "fx":[-20.3317,-53.21576,-84.09605,-55.95507], "fy":[137.06534,128.00924,110.13621,126.62141]},
+ {"t":5.81403, "x":7.1736, "y":4.87044, "heading":1.26254, "vx":1.32364, "vy":2.42073, "omega":2.65571, "ax":-4.08365, "ay":7.88615, "alpha":-17.07788, "fx":[18.74838,-63.75178,-122.98928,-63.54611], "fy":[137.4037,123.22926,64.06984,122.43452]},
+ {"t":5.85499, "x":7.22439, "y":4.9762, "heading":1.35699, "vx":1.15639, "vy":2.74371, "omega":1.95628, "ax":-2.83267, "ay":6.17709, "alpha":-30.70994, "fx":[29.45276,-74.94009,-135.48801,20.36545], "fy":[135.5445,116.79203,29.70945,68.18939]},
+ {"t":5.89548, "x":7.26889, "y":5.09237, "heading":1.41103, "vx":1.04168, "vy":2.99386, "omega":0.71263, "ax":-5.14081, "ay":6.86454, "alpha":-19.39976, "fx":[10.5279,-81.6906,-134.8442,-85.4723], "fy":[138.2504,112.12472,32.27354,106.56437]},
+ {"t":5.93598, "x":7.30686, "y":5.21924, "heading":1.42398, "vx":0.83349, "vy":3.27185, "omega":-0.07299, "ax":-6.07558, "ay":6.59309, "alpha":-16.3427, "fx":[-18.08231,-88.36554,-132.28084,-105.75089], "fy":[137.3624,106.85365,41.28888,88.3172]},
+ {"t":5.97648, "x":7.33563, "y":5.35715, "heading":1.40762, "vx":0.58745, "vy":3.53885, "omega":-0.73482, "ax":-7.25971, "ay":6.17888, "alpha":-9.01853, "fx":[-72.09202,-99.97195,-125.63982,-113.91497], "fy":[118.11484,95.87206,58.15357,78.19606]},
+ {"t":6.01697, "x":7.35347, "y":5.50553, "heading":1.37047, "vx":0.29346, "vy":3.78908, "omega":-1.10004, "ax":-8.51691, "ay":4.54633, "alpha":5.95903, "fx":[-130.74004,-128.41449,-107.33644,-116.40959], "fy":[45.13585,50.839,87.03864,74.75901]},
+ {"t":6.05747, "x":7.35837, "y":5.6627, "heading":1.33081, "vx":-0.05145, "vy":3.97319, "omega":-0.85872, "ax":-9.15016, "ay":3.03223, "alpha":6.17753, "fx":[-135.88354,-136.1067,-121.37154,-125.44357], "fy":[25.3444,22.76891,65.72638,58.08472]},
+ {"t":6.09797, "x":7.34878, "y":5.82609, "heading":1.3011, "vx":-0.422, "vy":4.09598, "omega":-0.60855, "ax":-9.65183, "ay":0.92672, "alpha":3.82929, "fx":[-138.04804,-137.93567,-135.74776,-135.51788], "fy":[4.02831,-2.36811,24.37947,26.50441]},
+ {"t":6.13846, "x":7.32378, "y":5.99272, "heading":1.27959, "vx":-0.81287, "vy":4.13351, "omega":-0.45347, "ax":-9.55799, "ay":-1.82394, "alpha":0.97799, "fx":[-135.29217,-134.61888,-135.74783,-136.27017], "fy":[-27.17258,-30.15985,-24.46227,-21.62118]},
+ {"t":6.17896, "x":7.28302, "y":6.15862, "heading":1.26203, "vx":-1.19994, "vy":4.05965, "omega":-0.41387, "ax":-8.60763, "ay":-4.52965, "alpha":-1.82343, "fx":[-122.0425,-125.85983,-122.20364,-117.93867], "fy":[-64.30301,-56.61552,-64.23738,-71.67093]},
+ {"t":6.21946, "x":7.22737, "y":6.31931, "heading":1.24378, "vx":-1.54852, "vy":3.87621, "omega":-0.48771, "ax":-7.2085, "ay":-6.49179, "alpha":-3.91871, "fx":[-98.65595,-113.88079,-105.60723,-90.57099], "fy":[-96.49559,-78.11038,-89.15355,-104.31871]},
+ {"t":6.25995, "x":7.15875, "y":6.47096, "heading":1.22081, "vx":-1.84044, "vy":3.61332, "omega":-0.64641, "ax":-5.90622, "ay":-7.66222, "alpha":-5.30048, "fx":[-73.41303,-101.67249,-91.9819,-67.80971], "fy":[-116.99322,-93.59272,-103.32192,-120.53253]},
+ {"t":6.30045, "x":7.07938, "y":6.611, "heading":1.19029, "vx":-2.07962, "vy":3.30302, "omega":-0.86106, "ax":-5.13349, "ay":-8.19415, "alpha":-5.73638, "fx":[-57.94125,-92.55498,-84.131,-56.43697], "fy":[-125.52793,-102.75682,-109.93168,-126.38379]},
+ {"t":6.35455, "x":6.95937, "y":6.77769, "heading":1.13532, "vx":-2.35732, "vy":2.85976, "omega":-1.17137, "ax":-4.63459, "ay":-8.44495, "alpha":-6.09976, "fx":[-46.98154,-86.07406,-79.8377,-49.8836], "fy":[-129.60052,-107.71247,-112.71548,-128.7919]},
+ {"t":6.40864, "x":6.82507, "y":6.92004, "heading":1.06303, "vx":-2.60803, "vy":2.40292, "omega":-1.50134, "ax":-3.83913, "ay":-8.68257, "alpha":-7.61407, "fx":[-26.27213,-77.9902,-75.00123,-38.4112], "fy":[-133.93771,-111.8789,-114.79813,-131.6789]},
+ {"t":6.46274, "x":6.67837, "y":7.03732, "heading":0.97067, "vx":-2.81571, "vy":1.93324, "omega":-1.91322, "ax":1.00832, "ay":-6.2745, "alpha":-2.72728, "fx":[23.26418,13.4557,5.34855,15.1026], "fy":[-87.39788,-85.31471,-90.47263,-92.57319]},
+ {"t":6.51683, "x":6.52753, "y":7.13272, "heading":0.86318, "vx":-2.76116, "vy":1.59382, "omega":-2.06076, "ax":0.14333, "ay":-3.68083, "alpha":-0.3258, "fx":[2.90261,1.97212,1.16122,2.09099], "fy":[-52.11639,-51.51936,-52.23528,-52.82848]},
+ {"t":6.57093, "x":6.37837, "y":7.21355, "heading":0.75123, "vx":-2.75341, "vy":1.3947, "omega":-2.07838, "ax":-0.99939, "ay":-3.05881, "alpha":0.96983, "fx":[-16.64002,-14.11041,-11.68295,-14.23115], "fy":[-43.15115,-45.42162,-43.58292,-41.27595]},
+ {"t":6.62502, "x":6.22796, "y":7.28452, "heading":0.64022, "vx":-2.80747, "vy":1.22923, "omega":-2.02592, "ax":-1.90695, "ay":-2.54292, "alpha":1.84432, "fx":[-31.48162,-27.26477,-22.5345,-26.8411], "fy":[-35.04231,-40.10324,-37.1237,-31.91166]},
+ {"t":6.67912, "x":6.0733, "y":7.3473, "heading":0.53332, "vx":-2.91063, "vy":1.09167, "omega":-1.92615, "ax":-2.58211, "ay":-1.8529, "alpha":2.57127, "fx":[-42.35416,-37.51767,-30.75689,-35.77453], "fy":[-24.20632,-32.07687,-28.48521,-20.28925]},
+ {"t":6.73321, "x":5.91207, "y":7.40364, "heading":0.43289, "vx":-3.05031, "vy":0.99144, "omega":-1.78706, "ax":-3.07466, "ay":-1.0308, "alpha":3.2681, "fx":[-50.26168,-45.59151,-36.76701,-41.71032], "fy":[-11.38987,-22.1289,-18.10719,-6.81942]},
+ {"t":6.78731, "x":5.74257, "y":7.45576, "heading":0.341, "vx":-3.21663, "vy":0.93568, "omega":-1.61027, "ax":-3.43744, "ay":-0.22033, "alpha":3.9701, "fx":[-56.08663,-52.19424,-41.18418,-45.43431], "fy":[1.31153,-12.29689,-7.96683,6.45972]},
+ {"t":6.8414, "x":5.56353, "y":7.50606, "heading":0.2597, "vx":-3.40258, "vy":0.92376, "omega":-1.3955, "ax":-3.96925, "ay":-0.19393, "alpha":4.73321, "fx":[-64.29389,-60.86725,-48.03118,-51.86043], "fy":[3.53357,-13.45863,-9.78686,8.71603]},
+ {"t":6.8955, "x":5.37366, "y":7.55574, "heading":0.19114, "vx":-3.6173, "vy":0.91327, "omega":-1.13946, "ax":-5.27649, "ay":-4.36642, "alpha":5.20503, "fx":[-86.81697,-76.55296,-61.85954,-73.94231], "fy":[-50.0349,-69.3304,-74.57109,-53.63523]},
+ {"t":6.94959, "x":5.17026, "y":7.59876, "heading":0.13711, "vx":-3.90273, "vy":0.67707, "omega":-0.85789, "ax":-5.3239, "ay":-7.42067, "alpha":5.73426, "fx":[-95.5008,-77.4534,-54.45591,-74.4497], "fy":[-89.49697,-106.98211,-119.09913,-105.16677]},
+ {"t":7.00369, "x":4.95135, "y":7.62453, "heading":0.0991, "vx":-4.19073, "vy":0.27564, "omega":-0.54769, "ax":9.07371, "ay":-3.25172, "alpha":0.62611, "fx":[128.91982,127.66615,128.34953,129.53507], "fy":[-45.14902,-48.66547,-47.00737,-43.54738]},
+ {"t":7.05778, "x":4.73793, "y":7.63468, "heading":0.07038, "vx":-3.69988, "vy":0.09974, "omega":-0.51382, "ax":9.71732, "ay":-0.57356, "alpha":2.83761, "fx":[138.24917,136.92927,137.46059,138.32358], "fy":[0.5343,-19.15959,-15.59858,1.7034]},
+ {"t":7.11188, "x":4.552, "y":7.63924, "heading":0.04674, "vx":-3.17422, "vy":0.06871, "omega":-0.36032, "ax":9.76007, "ay":-0.31974, "alpha":1.87871, "fx":[138.53585,138.06963,138.21892,138.56236], "fy":[1.43861,-11.48128,-9.9582,1.87174]},
+ {"t":7.16598, "x":4.39457, "y":7.64249, "heading":0.03, "vx":-2.64625, "vy":0.05142, "omega":-0.25869, "ax":9.77449, "ay":-0.23371, "alpha":1.37912, "fx":[138.65264,138.41184,138.47272,138.66718], "fy":[1.14944,-8.26471,-7.47751,1.34162]},
+ {"t":7.22007, "x":4.26572, "y":7.64493, "heading":0.01802, "vx":-2.11749, "vy":0.03877, "omega":-0.18409, "ax":9.78141, "ay":-0.19593, "alpha":1.07592, "fx":[138.71583,138.56275,138.59316,138.72509], "fy":[0.7406,-6.56547,-6.11297,0.82884]},
+ {"t":7.27417, "x":4.16549, "y":7.64674, "heading":0.00964, "vx":-1.58836, "vy":0.02817, "omega":-0.12589, "ax":9.78532, "ay":-0.17838, "alpha":0.88244, "fx":[138.7546,138.64245,138.66016,138.76099], "fy":[0.37557,-5.59506,-5.30959,0.41531]},
+ {"t":7.32826, "x":4.09388, "y":7.648, "heading":0.00412, "vx":-1.05902, "vy":0.01853, "omega":-0.07815, "ax":9.78772, "ay":-0.17143, "alpha":0.75962, "fx":[138.78036,138.68865,138.70049,138.78507], "fy":[0.07932,-5.04802,-4.8481,0.09687]},
+ {"t":7.38236, "x":4.05091, "y":7.64875, "heading":0.001, "vx":-0.52955, "vy":0.00925, "omega":-0.03706, "ax":9.78928, "ay":-0.17103, "alpha":0.68505, "fx":[138.79854,138.71655,138.72556,138.80222], "fy":[-0.15767,-4.7748,-4.61681,-0.14811]},
+ {"t":7.43645, "x":4.03659, "y":7.649, "heading":0.0, "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,75]
+ },
+ "events":[]
+}
--- /dev/null
+{
+ "name":"loopBumpDepot",
+ "version":3,
+ "snapshot":{
+ "waypoints":[
+ {"x":4.408560752868652, "y":7.650118350982666, "heading":0.0, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":5.938620567321777, "y":4.880990982055664, "heading":-1.5273453115960398, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":6.895162582397461, "y":4.453661918640137, "heading":0.0, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":7.397093296051025, "y":5.255403518676758, "heading":1.4745043905199968, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":3.3102965354919434, "y":5.603780269622803, "heading":1.5815484919422391, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":true},
+ {"x":0.4285269379615784, "y":4.993633270263672, "heading":1.5707963267948966, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":0.4379138052463531, "y":7.039971351623535, "heading":1.5707963267948966, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":true},
+ {"x":3.5649924278259277, "y":7.61433219909668, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":6.540783405303955, "y":7.388168811798096, "heading":-0.7517029687773807, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":8.491996765136719, "y":4.8637166023254395, "heading":-0.859887011106121, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true},
+ {"from":0, "to":1, "data":{"type":"KeepOutCircle", "props":{"x":4.605355881154537, "y":6.080103725194931, "r":1.0}}, "enabled":true},
+ {"from":4, "to":4, "data":{"type":"KeepOutCircle", "props":{"x":0.0, "y":0.0, "r":1.0}}, "enabled":true},
+ {"from":4, "to":5, "data":{"type":"KeepOutCircle", "props":{"x":0.9606258273124696, "y":3.940527379512787, "r":0.5301561610110335}}, "enabled":true},
+ {"from":5, "to":6, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true},
+ {"from":4, "to":5, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true},
+ {"from":6, "to":7, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}],
+ "targetDt":0.05
+ },
+ "params":{
+ "waypoints":[
+ {"x":{"exp":"4.408560752868652 m", "val":4.408560752868652}, "y":{"exp":"7.650118350982666 m", "val":7.650118350982666}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"5.938620567321777 m", "val":5.938620567321777}, "y":{"exp":"4.880990982055664 m", "val":4.880990982055664}, "heading":{"exp":"-1.5273453115960398 rad", "val":-1.5273453115960398}, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"6.895162582397461 m", "val":6.895162582397461}, "y":{"exp":"4.453661918640137 m", "val":4.453661918640137}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"7.397093296051025 m", "val":7.397093296051025}, "y":{"exp":"5.255403518676758 m", "val":5.255403518676758}, "heading":{"exp":"1.4745043905199968 rad", "val":1.4745043905199968}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"3.3102965354919434 m", "val":3.3102965354919434}, "y":{"exp":"5.603780269622803 m", "val":5.603780269622803}, "heading":{"exp":"1.5815484919422391 rad", "val":1.5815484919422391}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":true},
+ {"x":{"exp":"0.42852693796157837 m", "val":0.4285269379615784}, "y":{"exp":"4.993633270263672 m", "val":4.993633270263672}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"0.43791380524635315 m", "val":0.4379138052463531}, "y":{"exp":"7.039971351623535 m", "val":7.039971351623535}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":true},
+ {"x":{"exp":"3.5649924278259277 m", "val":3.5649924278259277}, "y":{"exp":"7.61433219909668 m", "val":7.61433219909668}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"6.540783405303955 m", "val":6.540783405303955}, "y":{"exp":"7.388168811798096 m", "val":7.388168811798096}, "heading":{"exp":"-0.7517029687773807 rad", "val":-0.7517029687773807}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"8.491996765136719 m", "val":8.491996765136719}, "y":{"exp":"4.8637166023254395 m", "val":4.8637166023254395}, "heading":{"exp":"-0.859887011106121 rad", "val":-0.859887011106121}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true},
+ {"from":0, "to":1, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"4.605355881154537 m", "val":4.605355881154537}, "y":{"exp":"6.080103725194931 m", "val":6.080103725194931}, "r":{"exp":"1 m", "val":1.0}}}, "enabled":true},
+ {"from":4, "to":4, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "r":{"exp":"1 m", "val":1.0}}}, "enabled":true},
+ {"from":4, "to":5, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"0.9606258273124695 m", "val":0.9606258273124696}, "y":{"exp":"3.940527379512787 m", "val":3.940527379512787}, "r":{"exp":"0.5301561610110335 m", "val":0.5301561610110335}}}, "enabled":true},
+ {"from":5, "to":6, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true},
+ {"from":4, "to":5, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true},
+ {"from":6, "to":7, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}],
+ "targetDt":{
+ "exp":"0.05 s",
+ "val":0.05
+ }
+ },
+ "trajectory":{
+ "config":{
+ "frontLeft":{
+ "x":0.52705,
+ "y":0.52705
+ },
+ "backLeft":{
+ "x":-0.52705,
+ "y":0.52705
+ },
+ "mass":56.69904625,
+ "inertia":7.0,
+ "gearing":7.03125,
+ "radius":0.0508,
+ "vmax":628.3185307179587,
+ "tmax":7.0,
+ "cof":1.0,
+ "bumper":{
+ "front":0.4064,
+ "side":0.4064,
+ "back":0.4064
+ },
+ "differentialTrackWidth":0.5588
+ },
+ "sampleType":"Swerve",
+ "waypoints":[0.0,1.2302,1.73128,2.21329,3.38389,6.38345,7.48512,10.6974,11.51115,12.47044],
+ "samples":[
+ {"t":0.0, "x":4.40856, "y":7.65012, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.16322, "ay":-2.83718, "alpha":-8.50869, "fx":[127.5421,138.3054,137.5879,116.11032], "fy":[-54.82407,-11.92691,-18.07633,-76.03783]},
+ {"t":0.05592, "x":4.42289, "y":7.64568, "heading":-0.0133, "vx":0.51239, "vy":-0.15865, "omega":-0.47579, "ax":9.18032, "ay":-2.71079, "alpha":-8.91197, "fx":[127.74571,138.46057,138.03614,116.27283], "fy":[-54.30306,-9.7046,-13.95152,-75.73994]},
+ {"t":0.11184, "x":4.46589, "y":7.63257, "heading":-0.05384, "vx":1.02574, "vy":-0.31023, "omega":-0.97413, "ax":9.19913, "ay":-2.5273, "alpha":-9.53512, "fx":[127.77123,138.58868,138.51817,116.70395], "fy":[-54.17418,-7.15541,-6.97233,-74.99354]},
+ {"t":0.16776, "x":4.53763, "y":7.61127, "heading":-0.12322, "vx":1.54014, "vy":-0.45156, "omega":-1.50732, "ax":9.22055, "ay":-2.23491, "alpha":-10.48237, "fx":[127.73257,138.67713,138.55226,117.83466], "fy":[-54.14699,-3.79943,4.28152,-73.05235]},
+ {"t":0.22367, "x":4.63817, "y":7.58253, "heading":-0.2239, "vx":2.05574, "vy":-0.57653, "omega":-2.09348, "ax":9.33042, "ay":-2.16426, "alpha":-8.51227, "fx":[128.33885,138.31575,138.49038,123.88107], "fy":[-52.40243,-9.24896,0.85985,-61.91986]},
+ {"t":0.27959, "x":4.76771, "y":7.54691, "heading":-0.35427, "vx":2.57748, "vy":-0.69755, "omega":-2.56947, "ax":8.68025, "ay":-3.87793, "alpha":9.48588, "fx":[136.06978,105.88216,115.62597,134.58426], "fy":[-23.24912,-88.62555,-75.97459,-32.02593]},
+ {"t":0.33551, "x":4.92541, "y":7.50184, "heading":-0.48312, "vx":3.06287, "vy":-0.9144, "omega":-2.03904, "ax":3.65123, "ay":-8.73868, "alpha":9.11077, "fx":[58.13099,10.44413,50.1528,88.29341], "fy":[-124.06529,-137.24488,-128.48681,-105.67756]},
+ {"t":0.39143, "x":5.10239, "y":7.43704, "heading":-0.58289, "vx":3.26704, "vy":-1.40305, "omega":-1.52958, "ax":-2.49729, "ay":-9.33407, "alpha":2.23521, "fx":[-40.18134,-44.02153,-31.26862,-26.12249], "fy":[-131.01176,-129.96732,-133.66198,-134.59178]},
+ {"t":0.44735, "x":5.28117, "y":7.344, "heading":-0.66493, "vx":3.12739, "vy":-1.925, "omega":-1.40459, "ax":-4.4974, "ay":-8.37014, "alpha":0.92566, "fx":[-65.89641,-66.76281,-61.71728,-60.62201], "fy":[-117.38117,-117.10066,-119.86969,-120.22753]},
+ {"t":0.50327, "x":5.44902, "y":7.22327, "heading":-0.74203, "vx":2.8759, "vy":-2.39304, "omega":-1.35283, "ax":-7.46264, "ay":-4.43252, "alpha":-4.57788, "fx":[-98.52951,-98.99023,-113.65939,-111.94527], "fy":[-76.80129,-70.83826,-46.77503,-56.90486]},
+ {"t":0.55918, "x":5.59817, "y":7.08252, "heading":-0.82483, "vx":2.45861, "vy":-2.6409, "omega":-1.60881, "ax":-7.88045, "ay":-0.64979, "alpha":-7.17947, "fx":[-110.08642,-104.90961,-112.94115,-118.87681], "fy":[-35.12368,-14.30738,18.15474,-5.5663]},
+ {"t":0.6151, "x":5.72333, "y":6.93383, "heading":-0.92602, "vx":2.01794, "vy":-2.67723, "omega":-2.01028, "ax":-7.5282, "ay":-1.38409, "alpha":-4.56064, "fx":[-105.24258,-101.50455,-108.23405,-111.86046], "fy":[-35.02067,-25.15389,-3.10464,-15.19748]},
+ {"t":0.67102, "x":5.8244, "y":6.78196, "heading":-1.04556, "vx":1.59698, "vy":-2.75463, "omega":-2.2653, "ax":-6.84974, "ay":-2.54464, "alpha":-1.14504, "fx":[-96.56241,-95.37799,-97.66695,-98.76645], "fy":[-39.52615,-38.00948,-32.49336,-34.24965]},
+ {"t":0.72694, "x":5.90299, "y":6.62395, "heading":-1.17402, "vx":1.21395, "vy":-2.89692, "omega":-2.32933, "ax":-6.6087, "ay":-3.12941, "alpha":1.19235, "fx":[-94.49969,-95.73653,-92.96729,-91.50364], "fy":[-41.22987,-41.48633,-47.29282,-47.4255]},
+ {"t":0.78286, "x":5.96054, "y":6.45706, "heading":-1.30241, "vx":0.84441, "vy":-3.07191, "omega":-2.26266, "ax":-6.03525, "ay":-5.07641, "alpha":5.64074, "fx":[-88.29428,-98.89296,-84.48016,-70.52547], "fy":[-62.01339,-58.40416,-80.60557,-86.80431]},
+ {"t":0.83878, "x":5.99832, "y":6.27735, "heading":-1.42011, "vx":0.50693, "vy":-3.35578, "omega":-1.94724, "ax":-5.47345, "ay":-6.48128, "alpha":9.34628, "fx":[-79.42919,-105.33278,-79.74288,-45.83453], "fy":[-85.84535,-66.84723,-99.30761,-115.48225]},
+ {"t":0.89469, "x":6.01811, "y":6.07957, "heading":-1.51439, "vx":0.20086, "vy":-3.7182, "omega":-1.42461, "ax":-7.25088, "ay":-5.46429, "alpha":9.59324, "fx":[-110.17885,-124.94751,-102.73651,-73.25519], "fy":[-67.86435,-45.7235,-86.00439,-110.22764]},
+ {"t":0.95061, "x":6.01801, "y":5.86311, "heading":-1.57905, "vx":-0.2046, "vy":-4.02375, "omega":-0.88817, "ax":-8.84918, "ay":-3.23716, "alpha":7.61681, "fx":[-131.8322,-134.76168,-123.61838,-111.52772], "fy":[-28.96883,-19.71875,-57.72021,-77.13612]},
+ {"t":1.00653, "x":5.99273, "y":5.63305, "heading":-1.61681, "vx":-0.69943, "vy":-4.20477, "omega":-0.46225, "ax":3.02286, "ay":8.6345, "alpha":-11.05513, "fx":[13.12018,86.72662,61.20192,10.34471], "fy":[131.91428,101.96729,120.89838,134.78808]},
+ {"t":1.06245, "x":5.95835, "y":5.41142, "heading":-1.65994, "vx":-0.5304, "vy":-3.72194, "omega":-1.08043, "ax":5.02339, "ay":6.73703, "alpha":21.36735, "fx":[99.10885,15.85176,32.21704,137.64382], "fy":[96.88748,137.59038,132.67917,14.82621]},
+ {"t":1.11837, "x":5.93654, "y":5.21383, "heading":-1.68695, "vx":-0.2495, "vy":-3.34522, "omega":0.11439, "ax":4.69585, "ay":6.60567, "alpha":23.99895, "fx":[101.98721,15.31764,10.39016,138.55528], "fy":[94.07882,137.85298,136.97157,5.63163]},
+ {"t":1.17429, "x":5.92993, "y":5.0371, "heading":-1.64303, "vx":0.01309, "vy":-2.97584, "omega":1.45637, "ax":5.09008, "ay":6.58517, "alpha":21.90687, "fx":[100.61847,13.05734,36.3438,138.58301], "fy":[95.61668,138.15043,132.74163,6.86393]},
+ {"t":1.2302, "x":5.93862, "y":4.88099, "heading":-1.52735, "vx":0.29772, "vy":-2.60761, "omega":2.68137, "ax":6.02533, "ay":6.76631, "alpha":15.44596, "fx":[92.98433,25.54374,90.20143,132.90113], "fy":[102.97663,136.31126,104.73432,39.62131]},
+ {"t":1.26875, "x":5.95457, "y":4.78551, "heading":-1.41252, "vx":0.52996, "vy":-2.34681, "omega":3.27672, "ax":6.065, "ay":7.47896, "alpha":7.2353, "fx":[86.13374,57.95864,89.14206,110.64515], "fy":[108.66438,125.9039,105.99022,83.49135]},
+ {"t":1.30729, "x":5.9795, "y":4.70061, "heading":-1.28085, "vx":0.76373, "vy":-2.05854, "omega":3.5556, "ax":6.29051, "ay":7.39999, "alpha":4.65977, "fx":[86.70066,71.84679,93.70569,104.41304], "fy":[108.11631,118.43956,101.9424,91.07384]},
+ {"t":1.34584, "x":6.01361, "y":4.62676, "heading":-1.14034, "vx":1.00619, "vy":-1.77331, "omega":3.73521, "ax":6.56654, "ay":7.22023, "alpha":1.71965, "fx":[91.04256,87.37571,95.46636,98.43165], "fy":[104.34749,107.39454,100.24844,97.38966]},
+ {"t":1.38438, "x":6.05727, "y":4.56377, "heading":-0.99509, "vx":1.2593, "vy":-1.49501, "omega":3.80149, "ax":6.85691, "ay":6.94102, "alpha":-1.2836, "fx":[99.58513,100.69727,95.00994,93.48787], "fy":[96.03203,94.91382,100.61559,101.98792]},
+ {"t":1.42292, "x":6.11091, "y":4.5113, "heading":-0.84952, "vx":1.52359, "vy":-1.22747, "omega":3.75202, "ax":7.11585, "ay":6.60737, "alpha":-3.94557, "fx":[110.13239,109.18856,93.25924,90.88185], "fy":[83.47285,84.9029,102.1472,104.10837]},
+ {"t":1.46147, "x":6.17492, "y":4.4689, "heading":-0.70783, "vx":1.79787, "vy":-0.9728, "omega":3.59994, "ax":7.28743, "ay":6.31865, "alpha":-5.99966, "fx":[118.71436,113.07963,90.64543,90.75081], "fy":[70.44219,79.52108,104.34592,103.95212]},
+ {"t":1.50001, "x":6.24963, "y":4.4361, "heading":-0.57353, "vx":2.07875, "vy":-0.72925, "omega":3.36869, "ax":7.30752, "ay":6.19869, "alpha":-7.40004, "fx":[123.37475,113.40217,86.50778,91.04472], "fy":[61.57561,78.89665,107.63504,103.35262]},
+ {"t":1.53856, "x":6.33518, "y":4.41259, "heading":-0.44918, "vx":2.36042, "vy":-0.49033, "omega":3.08346, "ax":7.09985, "ay":6.35878, "alpha":-8.24557, "fx":[123.93993,110.31239,79.62567,88.67674], "fy":[60.00465,82.94513,112.61159,104.9754]},
+ {"t":1.5771, "x":6.43144, "y":4.39842, "heading":-0.33646, "vx":2.63408, "vy":-0.24523, "omega":2.76564, "ax":6.51489, "ay":6.91027, "alpha":-8.57552, "fx":[119.19916,102.51028,68.21979,79.45881], "fy":[68.41566,92.12854,119.60645,111.65497]},
+ {"t":1.61565, "x":6.5378, "y":4.3941, "heading":-0.23623, "vx":2.88519, "vy":0.02112, "omega":2.4351, "ax":5.20318, "ay":7.94365, "alpha":-8.12752, "fx":[103.03788,85.80709,49.14857,57.02175], "fy":[90.36534,107.49364,128.32246,124.21584]},
+ {"t":1.65419, "x":6.65288, "y":4.40081, "heading":-0.14841, "vx":3.08574, "vy":0.3273, "omega":2.12183, "ax":2.50537, "ay":9.22026, "alpha":-6.0983, "fx":[58.66449,49.94481,17.26065,16.18223], "fy":[123.40627,127.75772,136.03872,135.57727]},
+ {"t":1.69273, "x":6.77368, "y":4.42028, "heading":-0.07115, "vx":3.18231, "vy":0.68269, "omega":1.88678, "ax":-1.57838, "ay":9.51596, "alpha":-2.11768, "fx":[-17.00613,-14.45792,-27.16064,-30.8678], "fy":[135.71843,136.21693,134.30372,133.30695]},
+ {"t":1.73128, "x":6.89516, "y":4.45366, "heading":0.0, "vx":3.12147, "vy":1.04948, "omega":1.80515, "ax":-5.25676, "ay":8.10206, "alpha":1.9573, "fx":[-76.14293,-81.96073,-72.82407,-67.12556], "fy":[114.08928,109.81654,115.95199,119.52109]},
+ {"t":1.77145, "x":7.0163, "y":4.50235, "heading":0.07409, "vx":2.91032, "vy":1.37491, "omega":1.88377, "ax":-8.06353, "ay":5.16653, "alpha":6.0385, "fx":[-113.91834,-127.8379,-117.77003,-97.66842], "fy":[76.99268,50.05135,69.54493,96.34827]},
+ {"t":1.81161, "x":7.1267, "y":4.56175, "heading":0.15462, "vx":2.58643, "vy":1.58244, "omega":2.12632, "ax":-9.14593, "ay":2.64625, "alpha":8.16327, "fx":[-129.14562,-137.56684,-135.4619,-116.39133], "fy":[48.27304,7.00617,21.51301,73.24755]},
+ {"t":1.85178, "x":7.22321, "y":4.62744, "heading":0.24662, "vx":2.21906, "vy":1.68873, "omega":2.45422, "ax":-9.43951, "ay":1.25512, "alpha":8.65483, "fx":[-134.69406,-137.3711,-137.57088,-125.5749], "fy":[30.6664,-13.73927,-2.63734,56.87432]},
+ {"t":1.89195, "x":7.30473, "y":4.69629, "heading":0.35218, "vx":1.8399, "vy":1.73915, "omega":2.80186, "ax":-9.54385, "ay":0.56876, "alpha":8.17576, "fx":[-136.95526,-136.32036,-137.56101,-130.2904], "fy":[19.3813,-23.02637,-9.93505,45.82809]},
+ {"t":1.93212, "x":7.37093, "y":4.7666, "heading":0.47132, "vx":1.45655, "vy":1.76199, "omega":3.13026, "ax":-9.61918, "ay":0.21923, "alpha":6.99015, "fx":[-138.00619,-135.98102,-137.87678,-133.53415], "fy":[10.93642,-25.65118,-8.86982,36.01492]},
+ {"t":1.97228, "x":7.42168, "y":4.83755, "heading":0.60269, "vx":1.07018, "vy":1.7708, "omega":3.41103, "ax":-9.69028, "ay":-0.00222, "alpha":5.23022, "fx":[-138.46535,-136.507,-138.24314,-136.21394], "fy":[3.8209,-23.23073,-5.44714,24.73105]},
+ {"t":2.01245, "x":7.45685, "y":4.90868, "heading":0.74392, "vx":0.68095, "vy":1.77071, "omega":3.62111, "ax":-9.74553, "ay":-0.19682, "alpha":3.03772, "fx":[-138.55666,-137.50736,-138.44122,-138.0573], "fy":[-1.99789,-16.8856,-3.77679,11.50105]},
+ {"t":2.05262, "x":7.47634, "y":4.97965, "heading":0.89182, "vx":0.2895, "vy":1.7628, "omega":3.74313, "ax":-9.76893, "ay":-0.39711, "alpha":0.60448, "fx":[-138.48539,-138.34256,-138.48783,-138.57311], "fy":[-5.81218,-8.45578,-5.43933,-2.80856]},
+ {"t":2.09278, "x":7.48009, "y":5.05013, "heading":1.04266, "vx":-0.1029, "vy":1.74685, "omega":3.76741, "ax":-9.75327, "ay":-0.60629, "alpha":-1.83207, "fx":[-138.46514,-138.65369,-138.29187,-137.59041], "fy":[-6.82941,-0.28459,-10.17181,-17.09005]},
+ {"t":2.13295, "x":7.46808, "y":5.11981, "heading":1.19251, "vx":-0.49466, "vy":1.7225, "omega":3.69382, "ax":-9.70405, "ay":-0.81304, "alpha":-4.05794, "fx":[-138.57691,-138.58621,-137.6785,-135.36888], "fy":[-4.65502,5.60015,-16.91973,-30.12418]},
+ {"t":2.17312, "x":7.44039, "y":5.18834, "heading":1.33761, "vx":-0.88444, "vy":1.68984, "omega":3.53083, "ax":-9.62958, "ay":-1.01034, "alpha":-6.10624, "fx":[-138.6748,-138.46494,-136.51949,-132.3289], "fy":[0.37995,8.6707,-24.77093,-41.56498]},
+ {"t":2.21329, "x":7.39709, "y":5.2554, "heading":1.4745, "vx":-1.27124, "vy":1.64926, "omega":3.28556, "ax":-9.5263, "ay":-1.18463, "alpha":-8.32222, "fx":[-138.47291,-138.42516,-134.73842,-128.49543], "fy":[8.44617,10.05836,-33.31931,-52.35237]},
+ {"t":2.26903, "x":7.31143, "y":5.3455, "heading":1.64472, "vx":-1.80226, "vy":1.58322, "omega":2.82165, "ax":-9.48054, "ay":-1.28114, "alpha":-9.02177, "fx":[-137.94945,-138.63353,-132.95306,-128.00171], "fy":[14.40158,6.15844,-39.76577,-53.43363]},
+ {"t":2.32477, "x":7.19624, "y":5.43176, "heading":1.78799, "vx":-2.33074, "vy":1.51181, "omega":2.31875, "ax":-9.41789, "ay":-1.42839, "alpha":-9.78834, "fx":[-137.3776,-138.72571,-130.72977,-127.15223], "fy":[18.7547,1.96268,-46.44856,-55.257]},
+ {"t":2.38052, "x":7.05168, "y":5.51382, "heading":1.90204, "vx":-2.85572, "vy":1.43219, "omega":1.77311, "ax":-9.34117, "ay":-1.65736, "alpha":-10.39623, "fx":[-137.09147,-138.65403,-128.07604,-125.81388], "fy":[20.2026,-3.0176,-53.175,-57.98057]},
+ {"t":2.43626, "x":6.87798, "y":5.59108, "heading":1.98473, "vx":-3.37643, "vy":1.3398, "omega":1.19359, "ax":-9.22894, "ay":-2.08039, "alpha":-10.67668, "fx":[-137.47884,-138.21463,-124.65195,-122.92663], "fy":[16.01452,-10.03454,-60.50016,-63.43598]},
+ {"t":2.492, "x":6.67543, "y":5.66253, "heading":2.03467, "vx":-3.89088, "vy":1.22383, "omega":0.59844, "ax":-8.88064, "ay":-3.24724, "alpha":-10.27035, "fx":[-137.79804,-135.77719,-117.27353,-112.6749], "fy":[-5.53925,-26.03366,-73.17003,-79.37255]},
+ {"t":2.54775, "x":6.44474, "y":5.7257, "heading":2.05208, "vx":-4.38592, "vy":1.04282, "omega":0.02594, "ax":-2.15153, "ay":-9.32351, "alpha":-0.4683, "fx":[-30.32398,-32.58382,-30.66935,-28.41233], "fy":[-132.15424,-131.66497,-132.17922,-132.63579]},
+ {"t":2.60349, "x":6.19691, "y":5.76935, "heading":2.05279, "vx":-4.50585, "vy":0.5231, "omega":-0.00017, "ax":-0.54459, "ay":-9.01822, "alpha":0.0001, "fx":[-7.67213,-7.70713,-7.76673,-7.7315], "fy":[-127.83406,-127.83193,-127.82828,-127.83044]},
+ {"t":2.65923, "x":5.9449, "y":5.7845, "heading":2.05278, "vx":-4.53621, "vy":0.02039, "omega":-0.00016, "ax":0.11173, "ay":-4.65132, "alpha":0.00004, "fx":[1.75968,1.64013,1.40768,1.52754], "fy":[-65.969,-65.80805,-65.89379,-66.05476]},
+ {"t":2.71498, "x":5.69221, "y":5.77841, "heading":2.05278, "vx":-4.52998, "vy":-0.23889, "omega":-0.00016, "ax":0.03712, "ay":-0.65409, "alpha":0.0, "fx":[0.57932,0.54284,0.47298,0.50945], "fy":[-9.28734,-9.22082,-9.25565,-9.32231]},
+ {"t":2.77072, "x":5.43975, "y":5.76407, "heading":2.05277, "vx":-4.52791, "vy":-0.27535, "omega":-0.00016, "ax":-0.00038, "ay":0.00616, "alpha":0.0, "fx":[0.04572,0.01058,-0.05634,-0.02123], "fy":[0.07159,0.13724,0.10302,0.03732]},
+ {"t":2.82646, "x":5.18735, "y":5.74873, "heading":2.05276, "vx":-4.52793, "vy":-0.27501, "omega":-0.00016, "ax":-0.00197, "ay":0.03259, "alpha":0.0, "fx":[0.09245,0.00978,-0.14838,-0.06579], "fy":[0.42484,0.5809,0.49913,0.3431]},
+ {"t":2.88221, "x":4.93494, "y":5.73345, "heading":2.05275, "vx":-4.52804, "vy":-0.27319, "omega":-0.00016, "ax":0.00017, "ay":-0.00287, "alpha":0.0, "fx":[0.33866,0.1087,-0.33345,-0.10415], "fy":[-0.14407,0.29275,0.06229,-0.37398]},
+ {"t":2.93795, "x":4.68254, "y":5.71822, "heading":2.05274, "vx":-4.52803, "vy":-0.27335, "omega":-0.00016, "ax":0.01541, "ay":-0.23952, "alpha":-0.00017, "fx":[0.73899,0.39092,-0.29873,0.04252], "fy":[-3.54584,-2.87676,-3.24726,-3.91061]},
+ {"t":2.99369, "x":4.43015, "y":5.70261, "heading":2.05273, "vx":-4.52717, "vy":-0.2867, "omega":-0.00017, "ax":5.41951, "ay":-0.98121, "alpha":-7.59148, "fx":[82.82411,63.28184,72.08263,89.09264], "fy":[6.37852,-7.1311,-35.23753,-19.64335]},
+ {"t":3.04943, "x":4.18621, "y":5.68511, "heading":2.04093, "vx":-4.22507, "vy":-0.3414, "omega":-0.42334, "ax":9.3241, "ay":0.6058, "alpha":-11.87064, "fx":[128.85767,132.08785,129.63181,138.09027], "fy":[49.80718,38.19516,-47.11447,-6.53943]},
+ {"t":3.10518, "x":3.96518, "y":5.66702, "heading":1.99889, "vx":-3.70531, "vy":-0.30763, "omega":-1.08505, "ax":9.67859, "ay":0.57511, "alpha":-5.12804, "fx":[135.59517,137.00764,137.62926,138.53452], "fy":[28.25231,19.54099,-15.03539,-0.14956]},
+ {"t":3.16092, "x":3.77367, "y":5.65076, "heading":1.93043, "vx":-3.1658, "vy":-0.27557, "omega":-1.3709, "ax":9.74291, "ay":0.58001, "alpha":-2.64793, "fx":[137.37464,137.84375,138.57837,138.61667], "fy":[18.67324,14.47284,-3.15454,2.89455]},
+ {"t":3.21666, "x":3.61234, "y":5.6363, "heading":1.8499, "vx":-2.6227, "vy":-0.24324, "omega":-1.51851, "ax":9.76201, "ay":0.58255, "alpha":-1.4128, "fx":[138.02791,138.17511,138.67082,138.62271], "fy":[13.68781,11.94667,2.53871,4.8569]},
+ {"t":3.27241, "x":3.48131, "y":5.62365, "heading":1.76306, "vx":-2.07853, "vy":-0.21076, "omega":-1.59726, "ax":9.76933, "ay":0.5838, "alpha":-0.67562, "fx":[138.32818,138.36221,138.62243,138.59892], "fy":[10.75217,10.23119,5.72819,6.38944]},
+ {"t":3.32815, "x":3.38062, "y":5.61281, "heading":1.67297, "vx":-1.53396, "vy":-0.17822, "omega":-1.63492, "ax":9.77251, "ay":0.58445, "alpha":-0.18603, "fx":[138.48531,138.48707,138.56082,138.55887], "fy":[8.92325,8.87638,7.64015,7.69796]},
+ {"t":3.38389, "x":3.3103, "y":5.60378, "heading":1.58155, "vx":-0.98921, "vy":-0.14564, "omega":-1.64529, "ax":0.11153, "ay":-0.58052, "alpha":2.05936, "fx":[-1.8042,5.03553,4.97405,-1.88193], "fy":[-11.6816,-11.60281,-4.77715,-4.85324]},
+ {"t":3.53387, "x":3.16319, "y":5.57541, "heading":1.35795, "vx":-0.97248, "vy":-0.23271, "omega":-1.33644, "ax":0.00005, "ay":-0.00023, "alpha":1.38568, "fx":[-2.73913,1.755,2.74068,-1.75349], "fy":[-1.75746,-2.74312,1.75103,2.73671]},
+ {"t":3.68385, "x":3.01734, "y":5.54051, "heading":1.1731, "vx":-0.97247, "vy":-0.23274, "omega":-1.12862, "ax":0.00003, "ay":-0.00011, "alpha":1.72346, "fx":[-3.69121,1.66366,3.69198,-1.6629], "fy":[-1.66487,-3.69318,1.66168,3.69]},
+ {"t":3.83383, "x":2.87149, "y":5.5056, "heading":1.02322, "vx":-0.97247, "vy":-0.23276, "omega":-0.87014, "ax":0.00003, "ay":-0.00014, "alpha":2.1822, "fx":[-4.89727,1.54381,4.89824,-1.54287], "fy":[-1.54534,-4.89973,1.54134,4.89577]},
+ {"t":3.98381, "x":2.72565, "y":5.47069, "heading":0.91726, "vx":-0.97246, "vy":-0.23278, "omega":-0.54285, "ax":0.00004, "ay":-0.00017, "alpha":2.13901, "fx":[-4.91453,1.13879,4.91568,-1.13767], "fy":[-1.14062,-4.91746,1.13584,4.91274]},
+ {"t":4.13378, "x":2.5798, "y":5.43577, "heading":0.8599, "vx":-0.97246, "vy":-0.2328, "omega":-0.22205, "ax":0.00005, "ay":-0.0002, "alpha":1.58759, "fx":[-3.67875,0.78289,3.6801,-0.78158], "fy":[-0.78504,-3.68219,0.77943,3.67665]},
+ {"t":4.28376, "x":2.43395, "y":5.40086, "heading":0.84445, "vx":-0.97245, "vy":-0.23283, "omega":0.01605, "ax":0.00006, "ay":-0.00023, "alpha":1.03072, "fx":[-2.3881,0.59807,2.3897,-0.5965], "fy":[-0.60061,-2.39218,0.59396,2.38559]},
+ {"t":4.43374, "x":2.28811, "y":5.36593, "heading":0.85845, "vx":-0.97244, "vy":-0.23287, "omega":0.17064, "ax":0.00007, "ay":-0.00028, "alpha":0.67664, "fx":[-1.55544,0.49988,1.55736,-0.49799], "fy":[-0.50293,-1.56036,0.49494,1.55242]},
+ {"t":4.58372, "x":2.14226, "y":5.33101, "heading":0.89165, "vx":-0.97243, "vy":-0.23291, "omega":0.27212, "ax":0.00008, "ay":-0.00034, "alpha":0.50758, "fx":[-1.15057,0.43946,1.15289,-0.43716], "fy":[-0.44315,-1.15653,0.43347,1.1469]},
+ {"t":4.73369, "x":1.99642, "y":5.29607, "heading":0.93817, "vx":-0.97242, "vy":-0.23296, "omega":0.34825, "ax":0.0001, "ay":-0.00041, "alpha":0.44939, "fx":[-1.00397,0.40519,1.00679,-0.40239], "fy":[-0.40968,-1.01123,0.3979,0.9995]},
+ {"t":4.88367, "x":1.85058, "y":5.26113, "heading":0.99546, "vx":-0.9724, "vy":-0.23302, "omega":0.41564, "ax":0.00012, "ay":-0.00051, "alpha":0.44537, "fx":[-0.9839,0.39352,0.98736,-0.39009], "fy":[-0.39901,-0.99278,0.38462,0.97844]},
+ {"t":5.03365, "x":1.70474, "y":5.22617, "heading":1.0628, "vx":-0.97239, "vy":-0.2331, "omega":0.48244, "ax":0.00015, "ay":-0.00062, "alpha":0.46374, "fx":[-1.01116,0.41932,1.0154,-0.41511], "fy":[-0.42604,-1.02206,0.4084,1.00445]},
+ {"t":5.18363, "x":1.55891, "y":5.19121, "heading":1.14037, "vx":-0.97236, "vy":-0.23319, "omega":0.55199, "ax":0.00018, "ay":-0.00076, "alpha":0.49332, "fx":[-1.04752,0.5021,1.05272,-0.49692], "fy":[-0.51034,-1.06091,0.48868,1.03928]},
+ {"t":5.33361, "x":1.41308, "y":5.15623, "heading":1.22871, "vx":-0.97234, "vy":-0.23331, "omega":0.62598, "ax":0.00022, "ay":-0.00094, "alpha":0.55104, "fx":[-1.12261,0.64847,1.12899,-0.64212], "fy":[-0.65857,-1.13905,0.63202,1.11252]},
+ {"t":5.48358, "x":1.26725, "y":5.12122, "heading":1.32879, "vx":-0.9723, "vy":-0.23345, "omega":0.70862, "ax":0.00027, "ay":-0.00114, "alpha":0.61781, "fx":[-1.19935,0.81733,1.20711,-0.80961], "fy":[-0.82959,-1.21936,0.79734,1.18714]},
+ {"t":5.63356, "x":1.12143, "y":5.0862, "heading":1.44201, "vx":-0.97226, "vy":-0.23362, "omega":0.80128, "ax":0.00032, "ay":-0.00135, "alpha":0.39063, "fx":[-0.71975,0.56708,0.72899,-0.5579], "fy":[-0.58166,-0.74357,0.54334,0.70529]},
+ {"t":5.78354, "x":0.97562, "y":5.05115, "heading":1.56658, "vx":-0.97221, "vy":-0.23382, "omega":0.85986, "ax":0.00037, "ay":-0.00153, "alpha":-1.17979, "fx":[1.93237,-1.98546,-1.92179,1.99579], "fy":[1.96881,1.90531,-2.01229,-1.94871]},
+ {"t":5.93352, "x":0.82981, "y":5.01606, "heading":1.68227, "vx":-0.97216, "vy":-0.23405, "omega":0.68292, "ax":0.0004, "ay":-0.00164, "alpha":-5.08992, "fx":[7.53313,-9.27739,-7.52144,9.28816], "fy":[9.25877,7.50349,-9.30541,-7.55009]},
+ {"t":6.08349, "x":0.68401, "y":4.98094, "heading":1.72745, "vx":-0.9721, "vy":-0.2343, "omega":-0.08045, "ax":-0.00073, "ay":0.00303, "alpha":-6.15523, "fx":[8.69997,-11.54609,-8.72427,11.52903], "fy":[11.57112,8.75608,-11.48592,-8.66946]},
+ {"t":6.23347, "x":0.53821, "y":4.94584, "heading":1.64616, "vx":-0.97221, "vy":-0.23384, "omega":-1.0036, "ax":3.21211, "ay":7.36832, "alpha":6.68237, "fx":[35.2382,68.62585,53.91704,24.3426], "fy":[104.09074,91.97872,106.7856,114.92161]},
+ {"t":6.38345, "x":0.42853, "y":4.99363, "heading":1.5708, "vx":-0.49046, "vy":0.87124, "omega":-0.00139, "ax":5.73923, "ay":7.92791, "alpha":0.02997, "fx":[81.33415,81.46464,81.37036,81.23987], "fy":[112.38911,112.29475,112.36334,112.45754]},
+ {"t":6.43353, "x":0.41116, "y":5.0472, "heading":1.57076, "vx":-0.20306, "vy":1.26824, "omega":0.00011, "ax":4.34688, "ay":8.75187, "alpha":0.0214, "fx":[61.58725,61.70101,61.6447,61.531], "fy":[124.06967,124.01334,124.04175,124.09799]},
+ {"t":6.4836, "x":0.40644, "y":5.12168, "heading":1.5708, "vx":0.01461, "vy":1.7065, "omega":0.00118, "ax":1.22327, "ay":5.82442, "alpha":-0.04252, "fx":[17.38573,17.13482,17.2917,17.54628], "fy":[82.52116,82.628,82.59853,82.49139]},
+ {"t":6.53368, "x":0.40871, "y":5.21444, "heading":1.5708, "vx":0.07587, "vy":1.99816, "omega":-0.00095, "ax":-0.42875, "ay":0.01402, "alpha":0.02534, "fx":[-6.1204,-6.03604,-6.03462,-6.11893], "fy":[0.15728,0.15622,0.24013,0.24121]},
+ {"t":6.58375, "x":0.41197, "y":5.31452, "heading":1.57079, "vx":0.0544, "vy":1.99887, "omega":0.00032, "ax":-0.24706, "ay":0.00596, "alpha":-0.00311, "fx":[-3.49671,-3.50706,-3.5073,-3.49694], "fy":[0.08941,0.08976,0.07948,0.07914]},
+ {"t":6.63383, "x":0.41439, "y":5.41462, "heading":1.5708, "vx":0.04203, "vy":1.99916, "omega":0.00016, "ax":-0.18306, "ay":0.00343, "alpha":-0.00454, "fx":[-2.58683,-2.60204,-2.60287,-2.58765], "fy":[0.05565,0.05646,0.04153,0.04072]},
+ {"t":6.68391, "x":0.41626, "y":5.51474, "heading":1.5708, "vx":0.03286, "vy":1.99934, "omega":-0.00007, "ax":-0.15382, "ay":0.00223, "alpha":-0.00034, "fx":[-2.17932,-2.18056,-2.18155,-2.1803], "fy":[0.03171,0.03258,0.03155,0.03067]},
+ {"t":6.73398, "x":0.41771, "y":5.61486, "heading":1.5708, "vx":0.02516, "vy":1.99945, "omega":-0.00008, "ax":-0.13707, "ay":0.00149, "alpha":0.00133, "fx":[-1.94468,-1.94032,-1.94128,-1.94564], "fy":[0.01848,0.01929,0.02374,0.02293]},
+ {"t":6.78406, "x":0.4188, "y":5.71498, "heading":1.57079, "vx":0.01829, "vy":1.99952, "omega":-0.00002, "ax":-0.12725, "ay":0.00096, "alpha":0.00089, "fx":[-1.80478,-1.80183,-1.80271,-1.80566], "fy":[0.01178,0.01252,0.01547,0.01473]},
+ {"t":6.83413, "x":0.41956, "y":5.81511, "heading":1.57079, "vx":0.01192, "vy":1.99957, "omega":0.00003, "ax":-0.12162, "ay":0.00054, "alpha":0.00005, "fx":[-1.72364,-1.72345,-1.72424,-1.72442], "fy":[0.00724,0.00793,0.00806,0.00737]},
+ {"t":6.88421, "x":0.42, "y":5.91524, "heading":1.5708, "vx":0.00583, "vy":1.9996, "omega":0.00003, "ax":-0.11905, "ay":0.00017, "alpha":-0.00037, "fx":[-1.6865,-1.68769,-1.68839,-1.68721], "fy":[0.00271,0.00336,0.0021,0.00145]},
+ {"t":6.93429, "x":0.42014, "y":6.01538, "heading":1.5708, "vx":-0.00013, "vy":1.99961, "omega":0.00001, "ax":-0.11911, "ay":-0.00019, "alpha":-0.00033, "fx":[-1.68751,-1.68857,-1.68921,-1.68814], "fy":[-0.00237,-0.00174,-0.00288,-0.00351]},
+ {"t":6.98436, "x":0.41999, "y":6.11551, "heading":1.5708, "vx":-0.0061, "vy":1.9996, "omega":0.0, "ax":-0.12178, "ay":-0.00056, "alpha":-0.00015, "fx":[-1.72569,-1.72615,-1.72674,-1.72627], "fy":[-0.00793,-0.00733,-0.00785,-0.00846]},
+ {"t":7.03444, "x":0.41953, "y":6.21564, "heading":1.5708, "vx":-0.0122, "vy":1.99957, "omega":-0.00001, "ax":-0.12743, "ay":-0.00098, "alpha":-0.00002, "fx":[-1.80599,-1.80605,-1.80659,-1.80653], "fy":[-0.01413,-0.01355,-0.01366,-0.01423]},
+ {"t":7.08452, "x":0.41876, "y":6.31577, "heading":1.5708, "vx":-0.01858, "vy":1.99952, "omega":-0.00001, "ax":-0.13758, "ay":-0.00151, "alpha":0.00006, "fx":[-1.95007,-1.94987,-1.95038,-1.95058], "fy":[-0.02182,-0.02128,-0.02112,-0.02166]},
+ {"t":7.13459, "x":0.41766, "y":6.41589, "heading":1.5708, "vx":-0.02547, "vy":1.99944, "omega":-0.00001, "ax":-0.15565, "ay":-0.00229, "alpha":0.00025, "fx":[-2.2065,-2.20566,-2.20614,-2.20698], "fy":[-0.03304,-0.03255,-0.03174,-0.03223]},
+ {"t":7.18467, "x":0.41619, "y":6.51602, "heading":1.5708, "vx":-0.03326, "vy":1.99933, "omega":0.0, "ax":-0.18303, "ay":-0.00346, "alpha":0.00054, "fx":[-2.59509,-2.59327,-2.59372,-2.59554], "fy":[-0.05021,-0.04976,-0.04798,-0.04843]},
+ {"t":7.23474, "x":0.41429, "y":6.61613, "heading":1.5708, "vx":-0.04243, "vy":1.99916, "omega":0.00003, "ax":-0.28902, "ay":-0.00718, "alpha":0.00018, "fx":[-4.09685,-4.0962,-4.09662,-4.09727], "fy":[-0.10225,-0.10181,-0.10126,-0.10171]},
+ {"t":7.28482, "x":0.4118, "y":6.71623, "heading":1.5708, "vx":-0.0569, "vy":1.9988, "omega":0.00004, "ax":-1.71769, "ay":-0.08599, "alpha":-0.00287, "fx":[-24.34302,-24.35247,-24.35275,-24.3433], "fy":[-1.21429,-1.21376,-1.2234,-1.22393]},
+ {"t":7.3349, "x":0.4068, "y":6.81621, "heading":1.5708, "vx":-0.14291, "vy":1.99449, "omega":-0.00011, "ax":5.45298, "ay":-4.86242, "alpha":0.00426, "fx":[77.28224,77.29762,77.30721,77.29183], "fy":[-68.937,-68.92315,-68.91022,-68.92406]},
+ {"t":7.38497, "x":0.40648, "y":6.90999, "heading":1.5708, "vx":0.13015, "vy":1.751, "omega":0.00011, "ax":3.44418, "ay":-9.14355, "alpha":-0.00192, "fx":[48.82805,48.81691,48.81273,48.82387], "fy":[-129.60479,-129.60897,-129.61059,-129.6064]},
+ {"t":7.43505, "x":0.41732, "y":6.98621, "heading":1.5708, "vx":0.30262, "vy":1.29313, "omega":0.00001, "ax":4.34042, "ay":-8.7707, "alpha":-0.00288, "fx":[61.53585,61.52052,61.51295,61.52828], "fy":[-124.31684,-124.32441,-124.32819,-124.32062]},
+ {"t":7.48512, "x":0.43791, "y":7.03997, "heading":1.5708, "vx":0.51997, "vy":0.85392, "omega":-0.00013, "ax":2.89745, "ay":-4.25676, "alpha":-2.57547, "fx":[46.58006,37.24015,35.70848,44.75378], "fy":[-55.58705,-57.17951,-65.1615,-63.42591]},
+ {"t":7.64574, "x":0.5588, "y":7.12222, "heading":1.53756, "vx":0.98534, "vy":0.17023, "omega":-0.41379, "ax":0.00014, "ay":-0.0008, "alpha":-1.97152, "fx":[3.36978,-3.17599,-3.36541,3.17947], "fy":[3.16755,3.35155,-3.19052,-3.37404]},
+ {"t":7.80635, "x":0.71706, "y":7.14955, "heading":1.44567, "vx":0.98536, "vy":0.1701, "omega":-0.73044, "ax":-0.00008, "ay":0.00044, "alpha":-1.2345, "fx":[2.23805,-1.84398,-2.24013,1.84173], "fy":[1.84898,2.24459,-1.83649,-2.23203]},
+ {"t":7.96696, "x":0.87532, "y":7.17688, "heading":1.31242, "vx":0.98535, "vy":0.17017, "omega":-0.92872, "ax":-0.00008, "ay":0.00045, "alpha":-0.9216, "fx":[1.79843,-1.21095,-1.8006,1.20875], "fy":[1.21615,1.80571,-1.2035,-1.79304]},
+ {"t":8.12758, "x":1.03358, "y":7.20421, "heading":1.15137, "vx":0.98534, "vy":0.17024, "omega":-1.07674, "ax":-0.00008, "ay":0.00047, "alpha":-0.65182, "fx":[1.36484,-0.71335,-1.36714,0.71107], "fy":[0.71884,1.37259,-0.70556,-1.35932]},
+ {"t":8.28819, "x":1.19184, "y":7.23156, "heading":0.97002, "vx":0.98533, "vy":0.17032, "omega":-1.18143, "ax":-0.00009, "ay":0.0005, "alpha":-0.21214, "fx":[0.45917,-0.24916,-0.46162,0.24672], "fy":[0.25501,0.46745,-0.24086,-0.45331]},
+ {"t":8.44881, "x":1.3501, "y":7.25893, "heading":0.77753, "vx":0.98531, "vy":0.1704, "omega":-1.21551, "ax":-0.00009, "ay":0.00053, "alpha":0.40687, "fx":[-0.9561,-0.06488,0.9535,0.06229], "fy":[0.07108,-0.94732,-0.05609,0.96231]},
+ {"t":8.60942, "x":1.50835, "y":7.2863, "heading":0.58755, "vx":0.9853, "vy":0.17049, "omega":-1.15016, "ax":-0.0001, "ay":0.00055, "alpha":1.00677, "fx":[-2.34365,-0.34288,2.34095,0.34018], "fy":[0.34933,-2.33452,-0.33374,2.35009]},
+ {"t":8.77003, "x":1.6666, "y":7.31369, "heading":0.41581, "vx":0.98528, "vy":0.17057, "omega":-0.98846, "ax":-0.0001, "ay":0.00057, "alpha":1.35381, "fx":[-3.07943,-0.855,3.07667,0.85221], "fy":[0.86162,-3.07004,-0.84558,3.08605]},
+ {"t":8.93065, "x":1.82485, "y":7.34109, "heading":0.27451, "vx":0.98527, "vy":0.17066, "omega":-0.77102, "ax":-0.0001, "ay":0.00059, "alpha":1.37815, "fx":[-3.03765,-1.20232,3.0348,1.19942], "fy":[1.20918,-3.02793,-1.19256,3.0445]},
+ {"t":9.09126, "x":1.9831, "y":7.36851, "heading":0.16845, "vx":0.98525, "vy":0.17076, "omega":-0.54967, "ax":-0.00011, "ay":0.00062, "alpha":1.17969, "fx":[-2.52939,-1.22506,2.52639,1.222], "fy":[1.23227,-2.51915,-1.21476,2.5366]},
+ {"t":9.25188, "x":2.14134, "y":7.39595, "heading":0.09538, "vx":0.98523, "vy":0.17086, "omega":-0.36019, "ax":-0.00011, "ay":0.00066, "alpha":0.90349, "fx":[-1.90831,-1.02395,1.90509,1.02066], "fy":[1.03168,-1.89732,-1.0129,1.91606]},
+ {"t":9.41249, "x":2.29958, "y":7.4234, "heading":0.04919, "vx":0.98521, "vy":0.17096, "omega":-0.21508, "ax":-0.00013, "ay":0.00072, "alpha":0.64971, "fx":[-1.37243,-0.76091,1.36891,0.75732], "fy":[0.76934,-1.36043,-0.74885,1.38089]},
+ {"t":9.5731, "x":2.45782, "y":7.45087, "heading":0.02302, "vx":0.98519, "vy":0.17108, "omega":-0.11073, "ax":-0.00014, "ay":0.0008, "alpha":0.43514, "fx":[-0.93736,-0.50179,0.93344,0.49782], "fy":[0.51115,-0.92404,-0.48841,0.94675]},
+ {"t":9.73372, "x":2.61605, "y":7.47835, "heading":0.01085, "vx":0.98517, "vy":0.17121, "omega":-0.04084, "ax":-0.00016, "ay":0.00091, "alpha":0.17947, "fx":[-0.44825,-0.14892,0.4438,0.14442], "fy":[0.15951,-0.43317,-0.13379,0.45887]},
+ {"t":9.89433, "x":2.77428, "y":7.50586, "heading":0.00661, "vx":0.98515, "vy":0.17135, "omega":-0.01201, "ax":-0.00018, "ay":0.00105, "alpha":-0.26055, "fx":[0.34794,0.51307,-0.35312,-0.51828], "fy":[-0.50078,0.36545,0.53061,-0.33562]},
+ {"t":10.05494, "x":2.93251, "y":7.5334, "heading":0.00131, "vx":0.98512, "vy":0.17152, "omega":-0.05386, "ax":-0.00022, "ay":0.00127, "alpha":-0.73601, "fx":[1.20011,1.2375,-1.20636,-1.24379], "fy":[-1.22266,1.22123,1.25864,-1.18524]},
+ {"t":10.21556, "x":3.09073, "y":7.56096, "heading":-0.01683, "vx":0.98508, "vy":0.17173, "omega":-0.17207, "ax":-0.00029, "ay":0.00165, "alpha":0.67076, "fx":[-1.04949,-1.18389,1.04148,1.17557], "fy":[1.20311,-1.02206,-1.15633,1.06892]},
+ {"t":10.37617, "x":3.24894, "y":7.58857, "heading":-0.03581, "vx":0.98504, "vy":0.17199, "omega":-0.06434, "ax":-0.00045, "ay":0.00259, "alpha":9.57593, "fx":[-15.43246,-16.36314,15.42156,16.3484], "fy":[16.39212,-15.39011,-16.31925,15.46387]},
+ {"t":10.53679, "x":3.40715, "y":7.61623, "heading":0.07737, "vx":0.98496, "vy":0.17241, "omega":1.47369, "ax":-0.02756, "ay":-2.29363, "alpha":-24.34872, "fx":[41.84027,40.87599,-49.32474,-34.95388], "fy":[-68.3666,10.47874,2.03164,-74.19018]},
+ {"t":10.6974, "x":3.56499, "y":7.61433, "heading":0.0, "vx":0.98054, "vy":-0.19598, "omega":-2.43706, "ax":8.34523, "ay":-5.10916, "alpha":-0.63599, "fx":[117.95062,119.78687,118.67216,116.75702], "fy":[-73.01344,-69.95417,-71.81889,-74.89813]},
+ {"t":10.74023, "x":3.61464, "y":7.60125, "heading":-0.10496, "vx":1.33796, "vy":-0.4148, "omega":-2.46429, "ax":8.97576, "ay":-3.85144, "alpha":-2.31503, "fx":[125.50394,130.78719,129.44598,123.17968], "fy":[-58.9906,-46.08567,-49.65046,-63.64649]},
+ {"t":10.78306, "x":3.68018, "y":7.57995, "heading":-0.21263, "vx":1.72238, "vy":-0.57975, "omega":-2.56345, "ax":9.58302, "ay":-1.52273, "alpha":-5.13245, "fx":[133.56206,138.38797,138.51929,132.87902], "fy":[-37.07295,-7.92864,-2.17278,-39.16274]},
+ {"t":10.82589, "x":3.76273, "y":7.55373, "heading":-0.32712, "vx":2.13281, "vy":-0.64497, "omega":-2.78326, "ax":9.72419, "ay":0.91108, "alpha":-0.76771, "fx":[138.13602,137.74953,137.50017,137.96672], "fy":[9.60271,14.16341,16.28454,11.60688]},
+ {"t":10.86872, "x":3.863, "y":7.52694, "heading":-0.44703, "vx":2.54929, "vy":-0.60595, "omega":-2.81614, "ax":9.00424, "ay":1.68041, "alpha":14.27045, "fx":[104.76719,137.51319,133.92262,134.32889], "fy":[90.33715,6.07958,-34.91635,33.7771]},
+ {"t":10.91155, "x":3.98044, "y":7.50253, "heading":-0.55456, "vx":2.93493, "vy":-0.53398, "omega":-2.20495, "ax":9.03114, "ay":1.9007, "alpha":13.22997, "fx":[107.9004,134.98441,133.99708,135.17535], "fy":[86.38814,25.63167,-33.96255,29.71041]},
+ {"t":10.95437, "x":4.11442, "y":7.4814, "heading":-0.63686, "vx":3.32173, "vy":-0.45258, "omega":-1.63833, "ax":9.04939, "ay":2.01327, "alpha":12.49974, "fx":[111.09049,131.99688,134.38524,135.61906], "fy":[81.88934,36.81827,-31.21973,26.66248]},
+ {"t":10.9972, "x":4.26499, "y":7.46387, "heading":-0.69556, "vx":3.7093, "vy":-0.36635, "omega":-1.10298, "ax":9.02124, "ay":2.11758, "alpha":12.11197, "fx":[112.5128,128.95578,134.50699,135.5199], "fy":[79.15857,43.91515,-28.19256,25.18343]},
+ {"t":11.04003, "x":4.43213, "y":7.45012, "heading":-0.73169, "vx":4.09567, "vy":-0.27566, "omega":-0.58423, "ax":8.8491, "ay":2.45305, "alpha":11.76841, "fx":[110.90344,122.73357,134.207,133.89134], "fy":[79.06754,53.15001,-20.58501,27.453]},
+ {"t":11.08286, "x":4.61566, "y":7.44056, "heading":-0.74592, "vx":4.47467, "vy":-0.17059, "omega":-0.0802, "ax":1.31343, "ay":7.13457, "alpha":1.72399, "fx":[16.81616,13.31266,20.6967,23.64493], "fy":[103.9141,101.62145,98.16473,100.82283]},
+ {"t":11.12569, "x":4.80851, "y":7.4398, "heading":-0.74778, "vx":4.53092, "vy":0.13497, "omega":-0.00636, "ax":-0.11392, "ay":2.72195, "alpha":0.00288, "fx":[-1.9663,-1.63334,-1.24506,-1.61437], "fy":[38.56184,38.38124,38.59638,38.79267]},
+ {"t":11.16852, "x":5.00246, "y":7.44808, "heading":-0.74805, "vx":4.52605, "vy":0.25155, "omega":-0.00624, "ax":-0.04568, "ay":0.77127, "alpha":-0.00002, "fx":[-0.56829,-0.65116,-0.72498,-0.64577], "fy":[10.9357,11.01958,10.92892,10.84613]},
+ {"t":11.21135, "x":5.19627, "y":7.45956, "heading":-0.74831, "vx":4.52409, "vy":0.28458, "omega":-0.00624, "ax":-0.01546, "ay":0.24142, "alpha":-0.00001, "fx":[0.0989,-0.23024,-0.53699,-0.20803], "fy":[3.43463,3.73905,3.40976,3.1046]},
+ {"t":11.25418, "x":5.39001, "y":7.47197, "heading":-0.74858, "vx":4.52343, "vy":0.29492, "omega":-0.00624, "ax":0.01156, "ay":-0.17924, "alpha":0.00003, "fx":[0.61378,0.1481,-0.28658,0.17997], "fy":[-2.5235,-2.09452,-2.55792,-2.98698]},
+ {"t":11.29701, "x":5.58376, "y":7.48443, "heading":-0.74885, "vx":4.52392, "vy":0.28725, "omega":-0.00624, "ax":0.11204, "ay":-2.15959, "alpha":-0.00442, "fx":[2.02277,1.60604,1.15113,1.57258], "fy":[-30.58666,-30.24981,-30.63689,-30.97359]},
+ {"t":11.33984, "x":5.77762, "y":7.49475, "heading":-0.74912, "vx":4.52872, "vy":0.19475, "omega":-0.00643, "ax":-0.05801, "ay":-9.46716, "alpha":-0.0549, "fx":[-0.80977,-0.57338,-0.83503,-1.07114], "fy":[-134.20357,-134.19586,-134.18623,-134.19354]},
+ {"t":11.38266, "x":5.97152, "y":7.49441, "heading":-0.74944, "vx":4.52624, "vy":-0.21072, "omega":-0.00878, "ax":-1.01904, "ay":-9.64634, "alpha":-0.16964, "fx":[-14.33588,-13.66748,-14.55492,-15.22041], "fy":[-136.75519,-136.8145,-136.71597,-136.65274]},
+ {"t":11.42549, "x":6.16444, "y":7.47654, "heading":-0.74998, "vx":4.48259, "vy":-0.62386, "omega":-0.01605, "ax":-1.93299, "ay":-9.55202, "alpha":-0.21213, "fx":[-27.17639,-26.45329,-27.6266,-28.34219], "fy":[-135.44988,-135.58626,-135.34786,-135.20665]},
+ {"t":11.46832, "x":6.35466, "y":7.44106, "heading":-0.75086, "vx":4.3998, "vy":-1.03296, "omega":-0.02513, "ax":-2.52018, "ay":-9.4328, "alpha":0.25258, "fx":[-36.05775,-36.81216,-35.39468,-34.62731], "fy":[-133.61602,-133.41585,-133.80184,-133.9969]},
+ {"t":11.51115, "x":6.54078, "y":7.38817, "heading":-0.7517, "vx":4.29187, "vy":-1.43696, "omega":-0.01432, "ax":-3.53805, "ay":-9.1097, "alpha":0.24349, "fx":[-50.5716,-51.12488,-49.73778,-49.16955], "fy":[-128.96304,-128.74921,-129.29316,-129.5059]},
+ {"t":11.56758, "x":6.77734, "y":7.29258, "heading":-0.75212, "vx":4.09222, "vy":-1.95101, "omega":-0.00058, "ax":-4.70686, "ay":-8.55174, "alpha":0.00532, "fx":[-66.72965,-66.73731,-66.70746,-66.69979], "fy":[-121.21274,-121.20868,-121.22515,-121.22921]},
+ {"t":11.62401, "x":7.00076, "y":7.16887, "heading":-0.75215, "vx":3.82661, "vy":-2.43358, "omega":-0.00027, "ax":-5.7065, "ay":-7.89449, "alpha":0.00128, "fx":[-80.8912,-80.89209,-80.88528,-80.88438], "fy":[-111.90039,-111.89981,-111.90474,-111.90533]},
+ {"t":11.68044, "x":7.20761, "y":7.01898, "heading":-0.75216, "vx":3.5046, "vy":-2.87906, "omega":-0.0002, "ax":-6.59191, "ay":-7.11314, "alpha":0.00066, "fx":[-93.44025,-93.4403,-93.43714,-93.43709], "fy":[-100.82563,-100.82564,-100.82857,-100.82855]},
+ {"t":11.73687, "x":7.39487, "y":6.84519, "heading":-0.75217, "vx":3.13263, "vy":-3.28044, "omega":-0.00017, "ax":-7.31022, "ay":-6.19632, "alpha":0.00034, "fx":[-103.62143,-103.62132,-103.61988,-103.61999], "fy":[-87.83044,-87.83065,-87.83234,-87.83214]},
+ {"t":11.7933, "x":7.56001, "y":6.65021, "heading":-0.75218, "vx":2.72012, "vy":-3.6301, "omega":-0.00015, "ax":-7.57947, "ay":-5.03794, "alpha":0.00015, "fx":[-107.43362,-107.44348,-107.44061,-107.43075], "fy":[-71.41686,-71.40215,-71.40643,-71.42114]},
+ {"t":11.84973, "x":7.70143, "y":6.43735, "heading":-0.75219, "vx":2.29242, "vy":-3.91438, "omega":-0.00014, "ax":-4.38761, "ay":-2.39009, "alpha":0.00003, "fx":[-62.13467,-62.21718,-62.25192,-62.16929], "fy":[-33.89783,-33.79236,-33.85991,-33.96557]},
+ {"t":11.90615, "x":7.82381, "y":6.21266, "heading":-0.7522, "vx":2.04483, "vy":-4.04925, "omega":-0.00014, "ax":-0.62888, "ay":-0.31397, "alpha":-0.00003, "fx":[-8.86809,-8.92526,-8.96123,-8.90232], "fy":[-4.45886,-4.40153,-4.44195,-4.49933]},
+ {"t":11.96258, "x":7.93819, "y":5.98366, "heading":-0.7522, "vx":2.00934, "vy":-4.06697, "omega":-0.00014, "ax":-0.94243, "ay":1.984, "alpha":-2.78808, "fx":[-13.40582,-6.81788,-13.35266,-19.85845], "fy":[21.52606,28.57379,34.65779,27.73309]},
+ {"t":12.01901, "x":8.05008, "y":5.75733, "heading":-0.75665, "vx":1.95616, "vy":-3.95502, "omega":-0.15747, "ax":-4.30965, "ay":8.70619, "alpha":-2.96603, "fx":[-66.81389,-49.2689,-56.48849,-71.78159], "fy":[120.70602,128.92183,126.04185,117.96323]},
+ {"t":12.07544, "x":8.1536, "y":5.54801, "heading":-0.77026, "vx":1.71297, "vy":-3.46373, "omega":-0.32484, "ax":-4.33174, "ay":8.75688, "alpha":-0.45794, "fx":[-62.23848,-59.6514,-60.59194,-63.12381], "fy":[123.7118,124.98332,124.5396,123.27227]},
+ {"t":12.13187, "x":8.24337, "y":5.3665, "heading":-0.78932, "vx":1.46854, "vy":-2.96959, "omega":-0.35068, "ax":-4.33566, "ay":8.7664, "alpha":0.37932, "fx":[-60.75597,-62.87333,-62.17712,-60.02112], "fy":[124.61574,123.55923,123.90564,124.96608]},
+ {"t":12.1883, "x":8.31933, "y":5.21288, "heading":-0.8085, "vx":1.22388, "vy":-2.47491, "omega":-0.32927, "ax":-4.33698, "ay":8.76976, "alpha":0.79849, "fx":[-59.96438,-64.40559,-63.07378,-58.45861], "fy":[125.0787,122.84831,123.52875,125.78131]},
+ {"t":12.24473, "x":8.38149, "y":5.08719, "heading":-0.82581, "vx":0.97915, "vy":-1.98004, "omega":-0.28421, "ax":-4.33758, "ay":8.77132, "alpha":1.05034, "fx":[-59.44673,-65.28349,-63.67396,-57.5324], "fy":[125.37225,122.43192,123.2675,126.2536]},
+ {"t":12.30116, "x":8.42983, "y":4.98942, "heading":-0.84018, "vx":0.73438, "vy":-1.48509, "omega":-0.22494, "ax":-4.33791, "ay":8.77216, "alpha":1.21847, "fx":[-59.0753,-65.8456,-64.10946,-56.92515], "fy":[125.57826,122.16218,123.07325,126.55917]},
+ {"t":12.35759, "x":8.46437, "y":4.91959, "heading":-0.85093, "vx":0.4896, "vy":-0.99008, "omega":-0.15619, "ax":-4.33812, "ay":8.77265, "alpha":1.33875, "fx":[-58.79897,-66.2365,-64.43621,-56.49579], "fy":[125.72944,121.97314,122.92527,126.77328]},
+ {"t":12.41402, "x":8.48509, "y":4.87768, "heading":-0.85761, "vx":0.2448, "vy":-0.49505, "omega":-0.08064, "ax":-4.33827, "ay":8.77297, "alpha":1.4291, "fx":[-58.5938,-66.52886,-64.68168,-56.17151], "fy":[125.84122,121.83067,122.81345,126.93371]},
+ {"t":12.47044, "x":8.492, "y":4.86372, "heading":-0.85989, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+ "splits":[0]
+ },
+ "events":[]
+}
--- /dev/null
+{
+ "name":"project",
+ "version":2,
+ "type":"Swerve",
+ "variables":{
+ "expressions":{},
+ "poses":{
+ "shootPos":{
+ "x":{
+ "exp":"4.036591053009033 m",
+ "val":4.036591053009033
+ },
+ "y":{
+ "exp":"7.649001598358154 m",
+ "val":7.649001598358154
+ },
+ "heading":{
+ "exp":"0 rad",
+ "val":0.0
+ }
+ }
+ }
+ },
+ "config":{
+ "frontLeft":{
+ "x":{
+ "exp":"20.75 in",
+ "val":0.52705
+ },
+ "y":{
+ "exp":"20.75 in",
+ "val":0.52705
+ }
+ },
+ "backLeft":{
+ "x":{
+ "exp":"-20.75 in",
+ "val":-0.52705
+ },
+ "y":{
+ "exp":"20.75 in",
+ "val":0.52705
+ }
+ },
+ "mass":{
+ "exp":"125 lbs",
+ "val":56.69904625
+ },
+ "inertia":{
+ "exp":"7 kg m ^ 2",
+ "val":7.0
+ },
+ "gearing":{
+ "exp":"7.03125",
+ "val":7.03125
+ },
+ "radius":{
+ "exp":"2 in",
+ "val":0.0508
+ },
+ "vmax":{
+ "exp":"6000 RPM",
+ "val":628.3185307179587
+ },
+ "tmax":{
+ "exp":"7 N * m",
+ "val":7.0
+ },
+ "cof":{
+ "exp":"1",
+ "val":1.0
+ },
+ "bumper":{
+ "front":{
+ "exp":"16 in",
+ "val":0.4064
+ },
+ "side":{
+ "exp":"16 in",
+ "val":0.4064
+ },
+ "back":{
+ "exp":"16 in",
+ "val":0.4064
+ }
+ },
+ "differentialTrackWidth":{
+ "exp":"22 in",
+ "val":0.5588
+ }
+ },
+ "generationFeatures":[],
+ "codegen":{
+ "root":"home/moo/Projects/robotics/FRC2026/src/main/java/choreo",
+ "genVars":true,
+ "genTrajData":true,
+ "useChoreoLib":true
+ }
+}
--- /dev/null
+{
+ "name":"shallowSwipe",
+ "version":3,
+ "snapshot":{
+ "waypoints":[
+ {"x":3.5895698070526123, "y":7.62680196762085, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":6.036516189575195, "y":6.847882270812988, "heading":-1.5313432532484534, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":5.826457977294922, "y":4.701357364654541, "heading":-0.9827935145707228, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":7.098229885101318, "y":4.711697101593018, "heading":1.1270129868473104, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":6.83792781829834, "y":6.324362754821777, "heading":1.4627850621263598, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":5.888542652130127, "y":7.473618507385254, "heading":0.06807331739317207, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":3.5895698070526123, "y":7.62680196762085, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepOutCircle", "props":{"x":4.744918951764703, "y":6.406979292631149, "r":0.6145389142105349}}, "enabled":true}],
+ "targetDt":0.05
+ },
+ "params":{
+ "waypoints":[
+ {"x":{"exp":"3.5895698070526123 m", "val":3.5895698070526123}, "y":{"exp":"7.62680196762085 m", "val":7.62680196762085}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"6.036516189575195 m", "val":6.036516189575195}, "y":{"exp":"6.847882270812988 m", "val":6.847882270812988}, "heading":{"exp":"-1.5313432532484534 rad", "val":-1.5313432532484534}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"5.826457977294922 m", "val":5.826457977294922}, "y":{"exp":"4.701357364654541 m", "val":4.701357364654541}, "heading":{"exp":"-0.9827935145707227 rad", "val":-0.9827935145707228}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"7.098229885101318 m", "val":7.098229885101318}, "y":{"exp":"4.711697101593018 m", "val":4.711697101593018}, "heading":{"exp":"1.1270129868473104 rad", "val":1.1270129868473104}, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"6.83792781829834 m", "val":6.83792781829834}, "y":{"exp":"6.324362754821777 m", "val":6.324362754821777}, "heading":{"exp":"1.4627850621263598 rad", "val":1.4627850621263598}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"5.888542652130127 m", "val":5.888542652130127}, "y":{"exp":"7.473618507385254 m", "val":7.473618507385254}, "heading":{"exp":"68.07331739317208 mrad", "val":0.06807331739317207}, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"3.5895698070526123 m", "val":3.5895698070526123}, "y":{"exp":"7.62680196762085 m", "val":7.62680196762085}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"4.744918951764703 m", "val":4.744918951764703}, "y":{"exp":"6.406979292631149 m", "val":6.406979292631149}, "r":{"exp":"0.6145389142105349 m", "val":0.6145389142105349}}}, "enabled":true}],
+ "targetDt":{
+ "exp":"0.05 s",
+ "val":0.05
+ }
+ },
+ "trajectory":{
+ "config":{
+ "frontLeft":{
+ "x":0.52705,
+ "y":0.52705
+ },
+ "backLeft":{
+ "x":-0.52705,
+ "y":0.52705
+ },
+ "mass":56.69904625,
+ "inertia":7.0,
+ "gearing":7.03125,
+ "radius":0.0508,
+ "vmax":628.3185307179587,
+ "tmax":7.0,
+ "cof":1.0,
+ "bumper":{
+ "front":0.4064,
+ "side":0.4064,
+ "back":0.4064
+ },
+ "differentialTrackWidth":0.5588
+ },
+ "sampleType":"Swerve",
+ "waypoints":[0.0,1.02437,1.62837,2.30231,2.72798,3.28679,4.04196],
+ "samples":[
+ {"t":0.0, "x":3.58957, "y":7.6268, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.64948, "ay":-1.17974, "alpha":-5.04028, "fx":[135.68641,138.80718,138.77846,133.84413], "fy":[-29.28221,-0.40058,-0.50883,-36.69856]},
+ {"t":0.05391, "x":3.60359, "y":7.62509, "heading":-0.00733, "vx":0.52024, "vy":-0.0636, "omega":-0.27174, "ax":9.66111, "ay":-1.11414, "alpha":-4.8535, "fx":[135.90216,138.79098,138.75964,134.32288], "fy":[-28.18204,-0.1884,0.0261,-34.82645]},
+ {"t":0.10783, "x":3.64568, "y":7.62004, "heading":-0.02903, "vx":1.04111, "vy":-0.12367, "omega":-0.53341, "ax":9.67568, "ay":-1.02774, "alpha":-4.59548, "fx":[136.1473,138.76826,138.73066,134.95583], "fy":[-26.85305,-0.10172,0.85184,-32.16918]},
+ {"t":0.16174, "x":3.71588, "y":7.61188, "heading":-0.06447, "vx":1.56277, "vy":-0.17908, "omega":-0.78118, "ax":9.69471, "ay":-0.90602, "alpha":-4.20909, "fx":[136.48563,138.73488,138.68314,135.7773], "fy":[-24.88521,-0.06156,1.88718,-28.31102]},
+ {"t":0.21566, "x":3.81422, "y":7.60091, "heading":-0.1127, "vx":2.08545, "vy":-0.22793, "omega":-1.00811, "ax":9.7201, "ay":-0.71813, "alpha":-3.57374, "fx":[137.02265,138.68076,138.60668,136.81035], "fy":[-21.37283,0.10426,2.97913,-22.42812]},
+ {"t":0.26957, "x":3.94078, "y":7.58757, "heading":-0.17225, "vx":2.6095, "vy":-0.26665, "omega":-1.20078, "ax":9.75145, "ay":-0.38486, "alpha":-2.3767, "fx":[137.88083,138.57475,138.48688,137.95544], "fy":[-13.83209,0.86016,3.7776,-12.6267]},
+ {"t":0.32348, "x":4.09565, "y":7.57264, "heading":-0.24044, "vx":3.13524, "vy":-0.2874, "omega":-1.32892, "ax":9.74931, "ay":0.37526, "alpha":0.57684, "fx":[138.08964,138.24168,138.28314,138.16196], "fy":[7.69699,3.98942,2.98524,6.60513]},
+ {"t":0.3774, "x":4.27885, "y":7.55769, "heading":-0.31125, "vx":3.66087, "vy":-0.26717, "omega":-1.29782, "ax":9.06933, "ay":1.35398, "alpha":13.47677, "fx":[110.87396,135.52165,134.67827,133.14839], "fy":[80.71545,-11.37992,-27.78628,35.22037]},
+ {"t":0.43131, "x":4.4894, "y":7.54525, "heading":-0.36163, "vx":4.14983, "vy":-0.19417, "omega":-0.57123, "ax":6.91538, "ay":3.63136, "alpha":10.16576, "fx":[79.53346,90.1184,114.68991,107.75377], "fy":[85.10537,51.07773,15.34719,54.36434]},
+ {"t":0.48523, "x":4.72319, "y":7.54006, "heading":-0.37766, "vx":4.52267, "vy":0.00162, "omega":-0.02315, "ax":-7.09546, "ay":-4.1616, "alpha":-10.2868, "fx":[-79.99123,-92.67983,-119.42966,-110.20522], "fy":[-92.68004,-62.89922,-20.64072,-59.73869]},
+ {"t":0.53914, "x":4.95671, "y":7.5341, "heading":-0.39385, "vx":4.14012, "vy":-0.22275, "omega":-0.57776, "ax":-8.44503, "ay":-3.48825, "alpha":-11.81163, "fx":[-94.6525,-122.56505,-135.56506,-126.04246], "fy":[-97.07133,-50.64228,1.98729,-52.05435]},
+ {"t":0.59306, "x":5.16765, "y":7.51702, "heading":-0.44217, "vx":3.68482, "vy":-0.41082, "omega":-1.21457, "ax":-8.52103, "ay":-3.73245, "alpha":-11.10439, "fx":[-97.26158,-121.77975,-136.9983,-127.09466], "fy":[-96.64418,-59.70274,-2.82437,-52.45511]},
+ {"t":0.64697, "x":5.35393, "y":7.48945, "heading":-0.52379, "vx":3.22541, "vy":-0.61205, "omega":-1.81325, "ax":-8.41458, "ay":-4.24982, "alpha":-9.89126, "fx":[-98.81522,-115.31899,-136.62939,-126.3352], "fy":[-95.92295,-73.782,-15.9515,-55.30437]},
+ {"t":0.70088, "x":5.51559, "y":7.45027, "heading":-0.63593, "vx":2.77175, "vy":-0.84118, "omega":-2.34653, "ax":-8.10518, "ay":-5.05436, "alpha":-8.0139, "fx":[-98.63614,-105.65544,-132.19245,-123.0718], "fy":[-96.57878,-88.18957,-39.10067,-62.70826]},
+ {"t":0.7548, "x":5.65325, "y":7.39758, "heading":-0.77409, "vx":2.33477, "vy":-1.11368, "omega":-2.77859, "ax":-7.54607, "ay":-6.05813, "alpha":-5.07629, "fx":[-96.73596,-96.63457,-119.19414,-115.29055], "fy":[-98.77573,-98.62486,-69.75695,-76.3324]},
+ {"t":0.80871, "x":5.76816, "y":7.32873, "heading":-0.93127, "vx":1.92793, "vy":-1.4403, "omega":-3.05228, "ax":-6.83014, "ay":-6.96971, "alpha":-1.15025, "fx":[-94.64912,-93.6196,-99.14571,-99.84827], "fy":[-100.96607,-101.88248,-96.5061,-95.8215]},
+ {"t":0.86263, "x":5.86218, "y":7.24095, "heading":-1.0975, "vx":1.55968, "vy":-1.81606, "omega":-3.11429, "ax":-6.16998, "ay":-7.54759, "alpha":2.92364, "fx":[-91.82378,-96.81909,-84.10759,-77.08157], "fy":[-103.66363,-99.08986,-110.10736,-115.08036]},
+ {"t":0.91654, "x":5.9373, "y":7.13207, "heading":-1.26116, "vx":1.22704, "vy":-2.22298, "omega":-2.95667, "ax":-5.70449, "ay":-7.80611, "alpha":6.28206, "fx":[-86.36753,-102.71765,-78.67105,-55.68317], "fy":[-108.3346,-93.11504,-114.20174,-126.94785]},
+ {"t":0.97045, "x":5.99516, "y":7.00087, "heading":-1.41143, "vx":0.91948, "vy":-2.64384, "omega":-2.61798, "ax":-5.65514, "ay":-7.18807, "alpha":14.61123, "fx":[-90.00929,-127.58651,-83.85356,-19.19177], "fy":[-105.14147,-54.45976,-110.57188,-137.38351]},
+ {"t":1.02437, "x":6.03652, "y":6.84788, "heading":-1.53134, "vx":0.61459, "vy":-3.03138, "omega":-1.83022, "ax":-6.05325, "ay":-3.48345, "alpha":31.07952, "fx":[-113.60073,-138.63317,-94.96806,3.98855], "fy":[47.13195,-4.79704,-101.18746,-138.65573]},
+ {"t":1.06751, "x":6.0574, "y":6.71386, "heading":-1.58138, "vx":0.35344, "vy":-3.18167, "omega":-0.48937, "ax":-6.12696, "ay":-6.0776, "alpha":19.00838, "fx":[-96.01717,-138.12813,-98.95057,-14.29669], "fy":[-97.56771,-11.9038,-97.22055,-137.90186]},
+ {"t":1.11065, "x":6.06694, "y":6.57094, "heading":-1.5848, "vx":0.0891, "vy":-3.44387, "omega":0.3307, "ax":-6.89944, "ay":-6.0441, "alpha":14.13022, "fx":[-108.07603,-134.50429,-102.26011,-46.3511], "fy":[-85.61577,-33.02487,-93.55868,-130.49544]},
+ {"t":1.1538, "x":6.06437, "y":6.41673, "heading":-1.55739, "vx":-0.20856, "vy":-3.70463, "omega":0.94032, "ax":-7.9565, "ay":-5.61867, "alpha":-1.65751, "fx":[-112.1213,-108.25677,-113.69402,-117.05402], "fy":[-80.8226,-85.85777,-78.45204,-73.44103]},
+ {"t":1.19694, "x":6.04796, "y":6.25168, "heading":-1.51836, "vx":-0.55182, "vy":-3.94704, "omega":0.86881, "ax":-8.06462, "ay":-4.87122, "alpha":-10.21041, "fx":[-112.14653,-85.22283,-126.38607,-133.50068], "fy":[-80.44322,-108.11101,-53.21508,-34.42434]},
+ {"t":1.24008, "x":6.01665, "y":6.07686, "heading":-1.49038, "vx":-0.89975, "vy":-4.15719, "omega":0.4283, "ax":-8.36222, "ay":-4.23769, "alpha":-9.32008, "fx":[-115.66903,-96.07808,-129.04101,-133.34161], "fy":[-73.21467,-96.39932,-40.74669,-29.91228]},
+ {"t":1.28322, "x":5.97005, "y":5.89356, "heading":-1.48058, "vx":-1.26052, "vy":-4.34002, "omega":0.02621, "ax":-3.43793, "ay":2.97204, "alpha":2.59145, "fx":[-44.00855,-52.38951,-53.54292,-44.98643], "fy":[47.80231,45.12386,36.51535,39.0701]},
+ {"t":1.32637, "x":5.91247, "y":5.70909, "heading":-1.47703, "vx":-1.40884, "vy":-4.2118, "omega":0.13801, "ax":7.24709, "ay":5.80201, "alpha":11.12545, "fx":[101.72754,65.16,114.88953,129.12625], "fy":[92.12838,120.06363,71.31724,45.45915]},
+ {"t":1.36951, "x":5.85843, "y":5.53278, "heading":-1.46072, "vx":-1.09618, "vy":-3.96148, "omega":0.61799, "ax":7.37216, "ay":5.81419, "alpha":10.7855, "fx":[102.59003,68.16056,117.49293,129.75106], "fy":[92.42756,119.7239,70.739,46.7686]},
+ {"t":1.41265, "x":5.818, "y":5.36728, "heading":-1.42403, "vx":-0.77813, "vy":-3.71064, "omega":1.08331, "ax":7.46032, "ay":5.81091, "alpha":10.14967, "fx":[102.69252,72.18434,119.03726,129.07878], "fy":[92.71918,117.78468,69.42327,49.54575]},
+ {"t":1.4558, "x":5.79138, "y":5.2126, "heading":-1.36784, "vx":-0.45627, "vy":-3.45994, "omega":1.52119, "ax":7.55153, "ay":5.79673, "alpha":9.28032, "fx":[102.63782,77.39967,120.32104,127.80612], "fy":[92.97687,114.64978,67.85947,53.18299]},
+ {"t":1.49894, "x":5.77872, "y":5.06873, "heading":-1.29358, "vx":-0.13047, "vy":-3.20986, "omega":1.92157, "ax":7.65302, "ay":5.76753, "alpha":8.18817, "fx":[102.75608,83.73388,121.28705,126.142], "fy":[92.96158,110.2501,66.53924,57.2628]},
+ {"t":1.54208, "x":5.78021, "y":4.93561, "heading":-1.20306, "vx":0.1997, "vy":-2.96103, "omega":2.27483, "ax":7.76518, "ay":5.72097, "alpha":6.84696, "fx":[103.38059,91.01062,121.72165,124.16552], "fy":[92.34202,104.42846,66.02212,61.58114]},
+ {"t":1.58522, "x":5.79605, "y":4.81319, "heading":-1.09854, "vx":0.53471, "vy":-2.71421, "omega":2.57023, "ax":7.88184, "ay":5.6595, "alpha":5.22373, "fx":[104.89184,98.79504,121.33668,121.86907], "fy":[90.675,97.18534,66.92164,66.10643]},
+ {"t":1.62837, "x":5.82646, "y":4.70136, "heading":-0.98279, "vx":0.87475, "vy":-2.47005, "omega":2.79559, "ax":7.81175, "ay":5.8182, "alpha":3.67775, "fx":[104.63666,102.38805,118.14615,117.74809], "fy":[90.93364,93.3795,72.41214,73.16105]},
+ {"t":1.67049, "x":5.87023, "y":4.60248, "heading":-0.86178, "vx":1.20379, "vy":-2.22498, "omega":2.9505, "ax":7.51488, "ay":6.22309, "alpha":2.36203, "fx":[101.77166,101.32785,111.81532,111.17176], "fy":[94.00774,94.42204,81.73394,82.67975]},
+ {"t":1.71261, "x":5.9276, "y":4.51428, "heading":-0.7354, "vx":1.52033, "vy":-1.96285, "omega":3.04999, "ax":7.1432, "ay":6.65333, "alpha":1.01229, "fx":[98.84438,99.11455,103.7602,103.29344], "fy":[96.90471,96.59362,91.58852,92.1506]},
+ {"t":1.75473, "x":5.99798, "y":4.4375, "heading":-0.60604, "vx":1.82121, "vy":-1.68261, "omega":3.09263, "ax":6.67277, "ay":7.11128, "alpha":-0.2883, "fx":[95.37534,95.16968,93.80243,93.99244], "fy":[100.05226,100.26063,101.53912,101.3506]},
+ {"t":1.79685, "x":6.08061, "y":4.37294, "heading":-0.47603, "vx":2.10227, "vy":-1.38307, "omega":3.08049, "ax":6.0525, "ay":7.61175, "alpha":-1.39643, "fx":[90.09547,88.5604,81.67461,82.84061], "fy":[104.39687,105.78458,111.17277,110.22453]},
+ {"t":1.83897, "x":6.17453, "y":4.32143, "heading":-0.34751, "vx":2.35721, "vy":-1.06246, "omega":3.02167, "ax":5.14163, "ay":8.19819, "alpha":-2.04087, "fx":[79.78238,77.07269,66.4075,68.26309], "fy":[111.71428,113.77517,120.28032,119.05995]},
+ {"t":1.88109, "x":6.27838, "y":4.28395, "heading":-0.22204, "vx":2.57378, "vy":-0.71714, "omega":2.93571, "ax":3.47246, "ay":8.94046, "alpha":-1.52599, "fx":[54.58745,53.0071,44.15201,45.13844], "fy":[124.51749,125.41674,128.77202,128.20945]},
+ {"t":1.92321, "x":6.38987, "y":4.26168, "heading":-0.09974, "vx":2.72005, "vy":-0.34055, "omega":2.87143, "ax":-0.56418, "ay":9.36098, "alpha":2.15265, "fx":[-13.36522,-16.11894,-1.99896,-0.50547], "fy":[132.76292,131.89204,132.75058,133.35312]},
+ {"t":1.96534, "x":6.50394, "y":4.25564, "heading":0.02311, "vx":2.69628, "vy":0.05374, "omega":2.9621, "ax":-5.24969, "ay":7.63909, "alpha":6.29441, "fx":[-79.89655,-97.08275,-68.34198,-52.33134], "fy":[107.55884,90.67554,112.27149,122.62346]},
+ {"t":2.00746, "x":6.61286, "y":4.26468, "heading":0.15347, "vx":2.47516, "vy":0.37551, "omega":3.22723, "ax":-6.389, "ay":6.87658, "alpha":6.3267, "fx":[-95.48644,-110.8178,-85.73637,-70.20943], "fy":[95.98493,76.35548,102.36829,115.18694]},
+ {"t":2.04958, "x":6.71145, "y":4.2866, "heading":0.29501, "vx":2.20605, "vy":0.66516, "omega":3.49372, "ax":-5.76467, "ay":7.66437, "alpha":2.69711, "fx":[-85.82485,-90.8332,-77.13017,-73.06325], "fy":[106.05682,101.39745,112.00386,115.10431]},
+ {"t":2.0917, "x":6.79925, "y":4.32141, "heading":0.44457, "vx":1.96323, "vy":0.98799, "omega":3.60732, "ax":-4.86374, "ay":8.41948, "alpha":-1.49826, "fx":[-64.85639,-64.50936,-72.74287,-73.6608], "fy":[121.66941,121.93454,117.21501,116.55732]},
+ {"t":2.13382, "x":6.87763, "y":4.3705, "heading":0.59518, "vx":1.75837, "vy":1.34263, "omega":3.54422, "ax":-5.04913, "ay":8.31302, "alpha":-3.41712, "fx":[-60.67206,-63.2991,-80.8943,-81.41526], "fy":[124.26179,123.04869,112.26245,111.76766]},
+ {"t":2.17594, "x":6.94722, "y":4.43442, "heading":0.74144, "vx":1.54569, "vy":1.69279, "omega":3.40028, "ax":-5.37242, "ay":8.08328, "alpha":-4.86321, "fx":[-59.35891,-66.71099,-89.80317,-88.73794], "fy":[125.10185,121.43891,105.5004,106.27327]},
+ {"t":2.21806, "x":7.00756, "y":4.5129, "heading":0.88035, "vx":1.3194, "vy":2.03326, "omega":3.19544, "ax":-5.69695, "ay":7.8214, "alpha":-6.06928, "fx":[-58.80323,-71.5126,-98.00098,-94.69479], "fy":[125.48298,118.78772,98.06323,101.13195]},
+ {"t":2.26018, "x":7.05808, "y":4.60548, "heading":1.00956, "vx":1.07944, "vy":2.36271, "omega":2.93979, "ax":-5.99246, "ay":7.55027, "alpha":-7.18405, "fx":[-58.24855,-76.62299,-105.39722,-99.49816], "fy":[125.81799,115.62843,90.15366,96.49321]},
+ {"t":2.30231, "x":7.09823, "y":4.7117, "heading":1.12701, "vx":0.82703, "vy":2.68074, "omega":2.63719, "ax":-6.17905, "ay":7.30296, "alpha":-8.77938, "fx":[-54.05039,-80.47307,-112.4475,-103.37527], "fy":[127.66426,112.97921,81.17067,92.25684]},
+ {"t":2.341, "x":7.12561, "y":4.8209, "heading":1.22249, "vx":0.58791, "vy":2.96335, "omega":2.29745, "ax":-6.27503, "ay":7.19094, "alpha":-9.1241, "fx":[-53.6451,-83.82497,-115.50922,-102.8087], "fy":[127.79269,110.47671,76.67574,92.77443]},
+ {"t":2.3797, "x":7.14366, "y":4.94096, "heading":1.30457, "vx":0.34508, "vy":3.24162, "omega":1.94436, "ax":-6.40826, "ay":7.01996, "alpha":-9.73243, "fx":[-53.22798,-87.37995,-119.19835,-103.53603], "fy":[127.90576,107.63309,70.69388,91.79218]},
+ {"t":2.4184, "x":7.15222, "y":5.07167, "heading":1.37252, "vx":0.09709, "vy":3.51328, "omega":1.56774, "ax":-6.60108, "ay":6.77754, "alpha":-10.35726, "fx":[-54.18469,-91.39336,-122.99868,-105.69813], "fy":[127.40916,104.16326,63.67429,89.03311]},
+ {"t":2.4571, "x":7.15103, "y":5.2127, "heading":1.42544, "vx":-0.15836, "vy":3.77556, "omega":1.16693, "ax":-6.90731, "ay":6.41385, "alpha":-10.75, "fx":[-58.55279,-96.31907,-126.58137,-110.18453], "fy":[125.29782,99.47829,55.89155,82.9917]},
+ {"t":2.4958, "x":7.13973, "y":5.36361, "heading":1.46255, "vx":-0.42566, "vy":4.02376, "omega":0.75093, "ax":-7.46115, "ay":5.75118, "alpha":-10.5514, "fx":[-70.70344,-103.63337,-130.07173,-118.63137], "fy":[118.51222,91.50929,46.48093,69.58408]},
+ {"t":2.53449, "x":7.11767, "y":5.52362, "heading":1.4837, "vx":-0.71439, "vy":4.24632, "omega":0.34261, "ax":-8.63654, "ay":3.91997, "alpha":-8.56153, "fx":[-103.85882,-119.12367,-134.68837,-132.01247], "fy":[89.6066,68.96876,27.79826,35.88466]},
+ {"t":2.57319, "x":7.08356, "y":5.69088, "heading":1.49055, "vx":-1.04861, "vy":4.39801, "omega":0.0113, "ax":-9.28872, "ay":-2.26609, "alpha":-0.29022, "fx":[-131.78149,-131.9584,-131.55523,-131.36647], "fy":[-31.52932,-30.97491,-32.70408,-33.27663]},
+ {"t":2.61189, "x":7.03602, "y":5.85938, "heading":1.49077, "vx":-1.40806, "vy":4.31032, "omega":0.00006, "ax":-8.71661, "ay":-3.23544, "alpha":-0.00207, "fx":[-123.55649,-123.55899,-123.55514,-123.55264], "fy":[-45.85845,-45.8535,-45.86471,-45.86966]},
+ {"t":2.65059, "x":6.97501, "y":6.02376, "heading":1.49077, "vx":-1.74538, "vy":4.18512, "omega":-0.00002, "ax":-2.43204, "ay":-7.72995, "alpha":-8.1517, "fx":[-13.34932,-62.01011,-49.76758,-12.76708], "fy":[-111.32636,-96.76815,-110.59229,-119.59419]},
+ {"t":2.68929, "x":6.90565, "y":6.17993, "heading":1.48467, "vx":-1.83949, "vy":3.88598, "omega":-0.31547, "ax":4.63035, "ay":-7.93785, "alpha":-12.92198, "fx":[115.28001,52.6225,18.21846,76.41556], "fy":[-74.34961,-125.02287,-136.16494,-114.53135]},
+ {"t":2.72798, "x":6.83793, "y":6.32436, "heading":1.46279, "vx":-1.66031, "vy":3.5788, "omega":-0.81552, "ax":4.66365, "ay":-8.00064, "alpha":-12.44351, "fx":[114.24825,55.67733,18.98406,75.51486], "fy":[-76.82954,-124.77152,-136.48942,-115.53831]},
+ {"t":2.7679, "x":6.77537, "y":6.46084, "heading":1.42032, "vx":-1.47416, "vy":3.25946, "omega":-1.31221, "ax":4.19234, "ay":-8.25672, "alpha":-12.2136, "fx":[108.6306,47.38785,12.62814,69.05522], "fy":[-84.14811,-127.71698,-136.99363,-119.28969]},
+ {"t":2.80781, "x":6.71987, "y":6.58436, "heading":1.35821, "vx":-1.30682, "vy":2.92989, "omega":-1.79971, "ax":3.49932, "ay":-8.55374, "alpha":-12.10128, "fx":[100.68216,35.35151,2.71971,59.65474], "fy":[-92.92277,-130.91043,-137.2115,-123.94429]},
+ {"t":2.84773, "x":6.6705, "y":6.69449, "heading":1.27674, "vx":-1.16714, "vy":2.58847, "omega":-2.28274, "ax":2.52432, "ay":-8.84109, "alpha":-12.16025, "fx":[89.54749,18.21581,-11.28087,46.64405], "fy":[-102.81833,-133.31326,-136.23273,-128.91683]},
+ {"t":2.88764, "x":6.62592, "y":6.79077, "heading":1.17594, "vx":-1.06638, "vy":2.23558, "omega":-2.76811, "ax":1.19343, "ay":-9.0125, "alpha":-12.43466, "fx":[73.74897,-5.48102,-29.71382,29.11227], "fy":[-113.12762,-132.41533,-132.38668,-133.07041]},
+ {"t":2.92756, "x":6.58431, "y":6.87282, "heading":1.05554, "vx":-1.01875, "vy":1.87584, "omega":-3.26444, "ax":-0.63715, "ay":-8.80907, "alpha":-12.7967, "fx":[48.59751,-37.16173,-52.45909,4.89751], "fy":[-121.98527,-121.70076,-122.11743,-133.66258]},
+ {"t":2.96747, "x":6.54314, "y":6.94068, "heading":0.91505, "vx":-1.04418, "vy":1.52423, "omega":-3.77522, "ax":-5.61733, "ay":-2.36693, "alpha":-4.54862, "fx":[-72.08231,-83.70755,-86.83588,-75.87152], "fy":[-35.08982,-19.41516,-32.66129,-47.03614]},
+ {"t":3.00739, "x":6.49698, "y":6.99963, "heading":0.76074, "vx":-1.2684, "vy":1.42975, "omega":-3.95678, "ax":0.40805, "ay":9.06528, "alpha":7.70466, "fx":[-28.41286,6.69977,39.46526,5.38395], "fy":[128.13079,128.07569,125.24751,132.53861]},
+ {"t":3.0473, "x":6.44668, "y":7.06392, "heading":0.60894, "vx":-1.25211, "vy":1.79159, "omega":-3.64925, "ax":-8.49659, "ay":2.1304, "alpha":15.46223, "fx":[-132.88587,-128.50742,-121.6132,-98.74207], "fy":[28.96214,-40.85397,40.54897,92.13467]},
+ {"t":3.08722, "x":6.38993, "y":7.13713, "heading":0.4756, "vx":-1.59125, "vy":1.87663, "omega":-3.03208, "ax":-9.2265, "ay":-1.26493, "alpha":11.42472, "fx":[-137.55138,-125.78611,-127.19846,-132.59789], "fy":[-1.6649,-55.30105,-48.83803,34.08348]},
+ {"t":3.12713, "x":6.31907, "y":7.21103, "heading":0.36367, "vx":-1.95953, "vy":1.82614, "omega":-2.57606, "ax":-9.26497, "ay":-1.96656, "alpha":9.58954, "fx":[-137.74677,-126.2903,-124.18039,-137.09762], "fy":[-8.80761,-55.60105,-58.97485,11.88172]},
+ {"t":3.16705, "x":6.23347, "y":7.28235, "heading":0.26849, "vx":-2.32934, "vy":1.74764, "omega":-2.19329, "ax":-9.2433, "ay":-2.34291, "alpha":8.7466, "fx":[-137.71999,-126.47829,-121.93624,-137.95189], "fy":[-12.16823,-55.83107,-64.4923,-0.34914]},
+ {"t":3.20696, "x":6.13313, "y":7.35025, "heading":0.18791, "vx":-2.69828, "vy":1.65412, "omega":-1.84417, "ax":-9.18371, "ay":-2.64984, "alpha":8.52461, "fx":[-137.65258,-126.09776,-119.0839,-137.87325], "fy":[-14.32667,-57.06435,-70.10906,-8.74313]},
+ {"t":3.24688, "x":6.01812, "y":7.41416, "heading":0.12109, "vx":-3.06485, "vy":1.54836, "omega":-1.50391, "ax":-9.08878, "ay":-2.94169, "alpha":8.80262, "fx":[-137.60537,-125.18556,-115.08171,-137.45247], "fy":[-15.65215,-59.28526,-76.7813,-15.07208]},
+ {"t":3.28679, "x":5.88854, "y":7.47362, "heading":0.06807, "vx":-3.42763, "vy":1.43094, "omega":-1.15255, "ax":-8.87918, "ay":-3.40647, "alpha":9.77166, "fx":[-137.45026,-122.65204,-106.83434,-136.50414], "fy":[-17.70839,-64.57722,-88.11259,-22.7456]},
+ {"t":3.34488, "x":5.67445, "y":7.55099, "heading":0.01761, "vx":-3.94342, "vy":1.23306, "omega":-0.58492, "ax":-8.16388, "ay":-4.81475, "alpha":9.77548, "fx":[-134.06693,-113.94956,-88.24519,-126.62257], "fy":[-33.79518,-78.46967,-106.28999,-54.43699]},
+ {"t":3.40297, "x":5.4316, "y":7.6145, "heading":0.00012, "vx":-4.41766, "vy":0.95337, "omega":-0.01706, "ax":-1.73369, "ay":-9.47751, "alpha":0.29542, "fx":[-25.68225,-25.33453,-23.47891,-23.80281], "fy":[-134.12429,-134.22306,-134.55419,-134.46444]},
+ {"t":3.46106, "x":5.17206, "y":7.65389, "heading":-0.00037, "vx":-4.51837, "vy":0.40282, "omega":0.0001, "ax":-0.28191, "ay":-9.0759, "alpha":-0.00082, "fx":[-3.99343,-3.99358,-3.99864,-3.9985], "fy":[-128.64906,-128.64867,-128.64852,-128.64891]},
+ {"t":3.51915, "x":4.90911, "y":7.66198, "heading":-0.00036, "vx":-4.53475, "vy":-0.1244, "omega":0.00005, "ax":0.02615, "ay":-0.26334, "alpha":-0.00068, "fx":[0.37174,0.37174,0.36949,0.36949], "fy":[-3.73388,-3.73164,-3.73164,-3.73388]},
+ {"t":3.57724, "x":4.64573, "y":7.65431, "heading":-0.00036, "vx":-4.53323, "vy":-0.13969, "omega":0.00001, "ax":9.58468, "ay":0.52561, "alpha":0.06705, "fx":[135.84362,135.86705,135.8773,135.85402], "fy":[7.67991,7.24428,7.22144,7.65572]},
+ {"t":3.63533, "x":4.39857, "y":7.64708, "heading":-0.00025, "vx":-3.97646, "vy":-0.10916, "omega":0.00391, "ax":9.75189, "ay":0.34374, "alpha":-0.02654, "fx":[138.23431,138.22816,138.22718,138.23334], "fy":[4.78181,4.95696,4.96327,4.7879]},
+ {"t":3.69342, "x":4.18403, "y":7.64132, "heading":-0.00007, "vx":-3.40997, "vy":-0.08919, "omega":0.00236, "ax":9.7718, "ay":0.30449, "alpha":-0.02349, "fx":[138.51561,138.51078,138.51026,138.51509], "fy":[4.23588,4.39128,4.39621,4.24064]},
+ {"t":3.75151, "x":4.00243, "y":7.63665, "heading":0.00003, "vx":-2.84233, "vy":-0.07151, "omega":0.001, "ax":9.77973, "ay":0.28, "alpha":-0.01525, "fx":[138.62692,138.62403,138.62378,138.62667], "fy":[3.91698,4.01798,4.0209,3.91983]},
+ {"t":3.8096, "x":3.85382, "y":7.63297, "heading":0.00006, "vx":-2.27422, "vy":-0.05524, "omega":0.00011, "ax":9.78408, "ay":0.26088, "alpha":-0.00792, "fx":[138.68773,138.68633,138.68623,138.68763], "fy":[3.67099,3.72343,3.72484,3.67237]},
+ {"t":3.86769, "x":3.73822, "y":7.6302, "heading":0.00006, "vx":-1.70587, "vy":-0.04009, "omega":-0.00035, "ax":9.78687, "ay":0.24447, "alpha":-0.00214, "fx":[138.72672,138.72636,138.72634,138.72669], "fy":[3.45809,3.47229,3.47264,3.45844]},
+ {"t":3.92578, "x":3.65564, "y":7.62828, "heading":0.00003, "vx":-1.13735, "vy":-0.02589, "omega":-0.00047, "ax":9.78884, "ay":0.22968, "alpha":0.00231, "fx":[138.75424,138.7546,138.75462,138.75426], "fy":[3.26349,3.24816,3.2478,3.26313]},
+ {"t":3.98387, "x":3.60609, "y":7.62717, "heading":0.00001, "vx":-0.56872, "vy":-0.01254, "omega":-0.00034, "ax":9.79032, "ay":0.21592, "alpha":0.00578, "fx":[138.77495,138.7758,138.77584,138.775], "fy":[3.08026,3.04193,3.04109,3.07941]},
+ {"t":4.04196, "x":3.58957, "y":7.6268, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+ "splits":[0]
+ },
+ "events":[]
+}
--- /dev/null
+{
+ "name":"stealOppHub",
+ "version":3,
+ "snapshot":{
+ "waypoints":[
+ {"x":3.573173999786377, "y":7.658982753753662, "heading":0.0, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":5.909358024597168, "y":7.412297248840332, "heading":-0.5836359716876055, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":10.596938133239746, "y":5.529364585876465, "heading":-1.1853205735433847, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":10.399354934692385, "y":3.262010097503662, "heading":-2.998884946210841, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":7.633565902709961, "y":4.615322113037109, "heading":2.4102064162028483, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":6.1281585693359375, "y":7.472915172576904, "heading":-1.3374351627371277, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":4.036591053009033, "y":7.649001598358154, "heading":0.0, "intervals":25, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":7.925092697143555, "y":7.378946304321289, "heading":-1.5707966850768345, "intervals":58, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":7.809670448303223, "y":2.0913589000701904, "heading":-2.4676511874322324, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":6.394168853759766, "y":2.0913589000701904, "heading":1.815774798656562, "intervals":56, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":6.336783409118652, "y":7.2082061767578125, "heading":1.1406287746226669, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":8.618081092834473, "y":4.6805739402771, "heading":-0.8534745074490971, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true},
+ {"from":2, "to":3, "data":{"type":"KeepOutCircle", "props":{"x":12.306512206792831, "y":4.034490287303925, "r":1.0}}, "enabled":true},
+ {"from":7, "to":8, "data":{"type":"KeepInLane", "props":{"tolerance":0.6257709589078365}}, "enabled":true},
+ {"from":7, "to":10, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true},
+ {"from":6, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}],
+ "targetDt":0.05
+ },
+ "params":{
+ "waypoints":[
+ {"x":{"exp":"3.573173999786377 m", "val":3.573173999786377}, "y":{"exp":"7.658982753753662 m", "val":7.658982753753662}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"5.909358024597168 m", "val":5.909358024597168}, "y":{"exp":"7.412297248840332 m", "val":7.412297248840332}, "heading":{"exp":"-0.5836359716876055 rad", "val":-0.5836359716876055}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"10.596938133239746 m", "val":10.596938133239746}, "y":{"exp":"5.529364585876465 m", "val":5.529364585876465}, "heading":{"exp":"-1.1853205735433847 rad", "val":-1.1853205735433847}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"10.399354934692383 m", "val":10.399354934692385}, "y":{"exp":"3.262010097503662 m", "val":3.262010097503662}, "heading":{"exp":"-2.998884946210841 rad", "val":-2.998884946210841}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"7.633565902709961 m", "val":7.633565902709961}, "y":{"exp":"4.615322113037109 m", "val":4.615322113037109}, "heading":{"exp":"2.4102064162028483 rad", "val":2.4102064162028483}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"6.1281585693359375 m", "val":6.1281585693359375}, "y":{"exp":"7.472915172576904 m", "val":7.472915172576904}, "heading":{"exp":"-1.3374351627371275 rad", "val":-1.3374351627371277}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"shootPos.x", "val":4.036591053009033}, "y":{"exp":"shootPos.y", "val":7.649001598358154}, "heading":{"exp":"shootPos.heading", "val":0.0}, "intervals":25, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"7.925092697143555 m", "val":7.925092697143555}, "y":{"exp":"7.378946304321289 m", "val":7.378946304321289}, "heading":{"exp":"-1.5707966850768345 rad", "val":-1.5707966850768345}, "intervals":58, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"7.809670448303223 m", "val":7.809670448303223}, "y":{"exp":"2.0913589000701904 m", "val":2.0913589000701904}, "heading":{"exp":"-2.4676511874322324 rad", "val":-2.4676511874322324}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"6.394168853759766 m", "val":6.394168853759766}, "y":{"exp":"2.0913589000701904 m", "val":2.0913589000701904}, "heading":{"exp":"1.815774798656562 rad", "val":1.815774798656562}, "intervals":56, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"6.336783409118652 m", "val":6.336783409118652}, "y":{"exp":"7.2082061767578125 m", "val":7.2082061767578125}, "heading":{"exp":"1.1406287746226669 rad", "val":1.1406287746226669}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"8.618081092834473 m", "val":8.618081092834473}, "y":{"exp":"4.6805739402771 m", "val":4.6805739402771}, "heading":{"exp":"-0.8534745074490971 rad", "val":-0.8534745074490971}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true},
+ {"from":2, "to":3, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"12.306512206792831 m", "val":12.306512206792831}, "y":{"exp":"4.034490287303925 m", "val":4.034490287303925}, "r":{"exp":"1 m", "val":1.0}}}, "enabled":true},
+ {"from":7, "to":8, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.6257709589078365 m", "val":0.6257709589078365}}}, "enabled":true},
+ {"from":7, "to":10, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true},
+ {"from":6, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}],
+ "targetDt":{
+ "exp":"0.05 s",
+ "val":0.05
+ }
+ },
+ "trajectory":{
+ "config":{
+ "frontLeft":{
+ "x":0.52705,
+ "y":0.52705
+ },
+ "backLeft":{
+ "x":-0.52705,
+ "y":0.52705
+ },
+ "mass":56.69904625,
+ "inertia":7.0,
+ "gearing":7.03125,
+ "radius":0.0508,
+ "vmax":628.3185307179587,
+ "tmax":7.0,
+ "cof":1.0,
+ "bumper":{
+ "front":0.4064,
+ "side":0.4064,
+ "back":0.4064
+ },
+ "differentialTrackWidth":0.5588
+ },
+ "sampleType":"Swerve",
+ "waypoints":[0.0,0.75975,1.97565,2.76838,3.57985,4.70066,5.494,6.73244,9.42401,10.16269,12.80034,13.99828],
+ "samples":[
+ {"t":0.0, "x":3.57317, "y":7.65898, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.0757, "ay":-3.20635, "alpha":-7.73704, "fx":[126.15371,137.54875,136.05221,114.82907], "fy":[-57.92473,-18.65375,-27.2757,-77.94288]},
+ {"t":0.05065, "x":3.58482, "y":7.65487, "heading":-0.00992, "vx":0.45968, "vy":-0.1624, "omega":-0.39188, "ax":9.17218, "ay":-2.87412, "alpha":-8.03252, "fx":[127.52805,138.09635,137.30463,117.12467], "fy":[-54.79479,-13.88609,-19.87245,-74.40646]},
+ {"t":0.1013, "x":3.61986, "y":7.64296, "heading":-0.04008, "vx":0.92425, "vy":-0.30797, "omega":-0.79872, "ax":9.28308, "ay":-2.43738, "alpha":-8.33461, "fx":[129.07461,138.51595,138.35482,120.39664], "fy":[-50.98862,-8.44184,-9.84546,-68.92117]},
+ {"t":0.15195, "x":3.67858, "y":7.62423, "heading":-0.09122, "vx":1.39444, "vy":-0.43143, "omega":-1.22087, "ax":9.41087, "ay":-1.8288, "alpha":-8.5422, "fx":[131.04631,138.7335,138.62378,125.18399], "fy":[-45.58949,-1.82277,3.3936,-59.6728]},
+ {"t":0.2026, "x":3.76128, "y":7.60004, "heading":-0.16402, "vx":1.87109, "vy":-0.52406, "omega":-1.65353, "ax":9.54873, "ay":-0.92105, "alpha":-8.3244, "fx":[133.83054,138.53228,137.16705,131.87384], "fy":[-36.42881,6.89346,19.9727,-42.65972]},
+ {"t":0.25325, "x":3.8683, "y":7.57231, "heading":-0.25844, "vx":2.35473, "vy":-0.57071, "omega":-2.07516, "ax":9.73306, "ay":0.01853, "alpha":-3.91674, "fx":[137.84024,138.34769,137.50032,138.1669], "fy":[-14.42929,8.45684,17.01501,-9.99172]},
+ {"t":0.3039, "x":4.00005, "y":7.54343, "heading":-0.36857, "vx":2.84771, "vy":-0.56977, "omega":-2.27354, "ax":9.40506, "ay":0.46356, "alpha":11.15961, "fx":[125.29759,136.67752,134.36645,136.91628], "fy":[58.68464,-20.00532,-33.48443,21.08845]},
+ {"t":0.35455, "x":4.15635, "y":7.51516, "heading":-0.46941, "vx":3.32408, "vy":-0.54629, "omega":-1.70831, "ax":9.19245, "ay":0.60889, "alpha":13.63591, "fx":[116.8077,136.34506,131.35388,136.69624], "fy":[73.71922,-17.54266,-43.14341,21.4905]},
+ {"t":0.4052, "x":4.33651, "y":7.48828, "heading":-0.53845, "vx":3.78967, "vy":-0.51545, "omega":-1.01765, "ax":9.15522, "ay":0.97263, "alpha":12.90253, "fx":[115.37633,136.06868,131.46148,136.18556], "fy":[74.69351,-0.39171,-40.59497,21.44051]},
+ {"t":0.45585, "x":4.5402, "y":7.46342, "heading":-0.57344, "vx":4.25338, "vy":-0.46618, "omega":-0.36414, "ax":5.51347, "ay":6.26826, "alpha":7.12444, "fx":[64.73142,60.83346,95.31567,91.72772], "fy":[104.18203,99.48989,69.50321,82.22938]},
+ {"t":0.5065, "x":4.7627, "y":7.44784, "heading":-0.58275, "vx":4.53263, "vy":-0.1487, "omega":-0.00329, "ax":0.05216, "ay":2.77773, "alpha":0.00535, "fx":[0.73662,0.7261,0.74215,0.75267], "fy":[39.38502,39.37141,39.36238,39.37599]},
+ {"t":0.55715, "x":4.99234, "y":7.44388, "heading":-0.58291, "vx":4.53528, "vy":-0.00801, "omega":-0.00302, "ax":-0.00001, "ay":0.32587, "alpha":0.00003, "fx":[-0.00015,-0.00021,-0.00012,-0.00006], "fy":[4.6192,4.61911,4.61905,4.61914]},
+ {"t":0.6078, "x":5.22205, "y":7.44389, "heading":-0.58306, "vx":4.53528, "vy":0.0085, "omega":-0.00302, "ax":-0.00066, "ay":-0.53703, "alpha":-0.00012, "fx":[-0.0093,-0.00907,-0.00942,-0.00964], "fy":[-7.61251,-7.61217,-7.61194,-7.61228]},
+ {"t":0.65845, "x":5.45176, "y":7.44363, "heading":-0.58321, "vx":4.53524, "vy":-0.0187, "omega":-0.00303, "ax":-0.1446, "ay":-4.71629, "alpha":-0.00148, "fx":[-2.04879,-2.04552,-2.0506,-2.05386], "fy":[-66.85493,-66.85182,-66.84968,-66.85279]},
+ {"t":0.7091, "x":5.68129, "y":7.43663, "heading":-0.58337, "vx":4.52792, "vy":-0.25758, "omega":-0.0031, "ax":-0.98835, "ay":-8.80123, "alpha":-0.08516, "fx":[-13.90242,-13.66479,-14.11752,-14.35398], "fy":[-124.80259,-124.78252,-124.70829,-124.72789]},
+ {"t":0.75975, "x":5.90936, "y":7.4123, "heading":-0.58364, "vx":4.47786, "vy":-0.70336, "omega":-0.00742, "ax":-2.01013, "ay":-8.63103, "alpha":-0.40004, "fx":[-27.84261,-26.95764,-29.16029,-30.01198], "fy":[-122.65681,-122.62427,-122.0305,-122.05977]},
+ {"t":0.80478, "x":6.10897, "y":7.37187, "heading":-0.58438, "vx":4.38734, "vy":-1.09205, "omega":-0.02543, "ax":-1.64448, "ay":-5.65279, "alpha":-0.10585, "fx":[-23.19857,-23.0018,-23.42232,-23.61796], "fy":[-80.30584,-80.13971,-79.94792,-80.11445]},
+ {"t":0.84981, "x":6.30488, "y":7.31696, "heading":-0.58563, "vx":4.31328, "vy":-1.34661, "omega":-0.0302, "ax":-0.3967, "ay":-1.18511, "alpha":-0.03152, "fx":[-5.60757,-5.54982,-5.63875,-5.69649], "fy":[-16.87039,-16.78487,-16.72691,-16.81245]},
+ {"t":0.89485, "x":6.49872, "y":7.25512, "heading":-0.58702, "vx":4.29541, "vy":-1.39998, "omega":-0.03162, "ax":-0.04935, "ay":-0.14278, "alpha":-0.00399, "fx":[-0.6976,-0.69026,-0.7013,-0.70865], "fy":[-2.0331,-2.02206,-2.01472,-2.02575]},
+ {"t":0.93988, "x":6.69211, "y":7.19192, "heading":-0.58845, "vx":4.29319, "vy":-1.40641, "omega":-0.0318, "ax":0.00066, "ay":0.00193, "alpha":0.00059, "fx":[0.00901,0.00792,0.00956,0.01065], "fy":[0.02877,0.02713,0.02603,0.02767]},
+ {"t":0.98491, "x":6.88545, "y":7.12859, "heading":-0.58988, "vx":4.29322, "vy":-1.40632, "omega":-0.03177, "ax":0.00688, "ay":0.01976, "alpha":0.00123, "fx":[0.0969,0.09464,0.09802,0.10028], "fy":[0.28292,0.27954,0.27727,0.28066]},
+ {"t":1.02995, "x":7.07879, "y":7.06528, "heading":-0.59131, "vx":4.29353, "vy":-1.40543, "omega":-0.03171, "ax":0.00712, "ay":0.02037, "alpha":0.00129, "fx":[0.10027,0.09788,0.10144,0.10383], "fy":[0.29165,0.28809,0.2857,0.28926]},
+ {"t":1.07498, "x":7.27215, "y":7.00201, "heading":-0.59274, "vx":4.29385, "vy":-1.40452, "omega":-0.03166, "ax":0.00664, "ay":0.01891, "alpha":0.00128, "fx":[0.09349,0.09112,0.09464,0.09702], "fy":[0.27106,0.26753,0.26516,0.26868]},
+ {"t":1.12001, "x":7.46553, "y":6.93878, "heading":-0.59416, "vx":4.29415, "vy":-1.40366, "omega":-0.0316, "ax":0.00614, "ay":0.01743, "alpha":0.00126, "fx":[0.0865,0.08415,0.08762,0.08997], "fy":[0.24998,0.24651,0.24416,0.24763]},
+ {"t":1.16505, "x":7.65891, "y":6.87558, "heading":-0.59558, "vx":4.29443, "vy":-1.40288, "omega":-0.03154, "ax":0.00571, "ay":0.01613, "alpha":0.00124, "fx":[0.08037,0.07806,0.08147,0.08379], "fy":[0.23154,0.22812,0.2258,0.22922]},
+ {"t":1.21008, "x":7.85231, "y":6.81242, "heading":-0.597, "vx":4.29468, "vy":-1.40215, "omega":-0.03149, "ax":0.00533, "ay":0.01501, "alpha":0.00123, "fx":[0.07507,0.07278,0.07615,0.07844], "fy":[0.21555,0.21218,0.20989,0.21326]},
+ {"t":1.25511, "x":8.04572, "y":6.74929, "heading":-0.59842, "vx":4.29492, "vy":-1.40148, "omega":-0.03143, "ax":0.00501, "ay":0.01402, "alpha":0.00121, "fx":[0.07043,0.06816,0.07149,0.07376], "fy":[0.20156,0.19823,0.19596,0.19929]},
+ {"t":1.30015, "x":8.23914, "y":6.6862, "heading":-0.59983, "vx":4.29515, "vy":-1.40085, "omega":-0.03138, "ax":0.00471, "ay":0.01314, "alpha":0.0012, "fx":[0.06629,0.06404,0.06733,0.06958], "fy":[0.18906,0.18578,0.18353,0.18682]},
+ {"t":1.34518, "x":8.43257, "y":6.62312, "heading":-0.60124, "vx":4.29536, "vy":-1.40025, "omega":-0.03132, "ax":0.00443, "ay":0.01229, "alpha":0.00119, "fx":[0.06227,0.06005,0.06329,0.06552], "fy":[0.17694,0.1737,0.17147,0.17471]},
+ {"t":1.39021, "x":8.62601, "y":6.56008, "heading":-0.60265, "vx":4.29556, "vy":-1.3997, "omega":-0.03127, "ax":0.004, "ay":0.01103, "alpha":0.00116, "fx":[0.05626,0.05408,0.05725,0.05943], "fy":[0.159,0.15583,0.15366,0.15682]},
+ {"t":1.43525, "x":8.81946, "y":6.49706, "heading":-0.60406, "vx":4.29574, "vy":-1.3992, "omega":-0.03122, "ax":0.0024, "ay":0.00635, "alpha":0.00103, "fx":[0.03365,0.03172,0.03452,0.03646], "fy":[0.09237,0.08957,0.08763,0.09044]},
+ {"t":1.48028, "x":9.01291, "y":6.43405, "heading":-0.60546, "vx":4.29585, "vy":-1.39892, "omega":-0.03117, "ax":-0.00774, "ay":-0.02319, "alpha":0.00015, "fx":[-0.10976,-0.11004,-0.10963,-0.10935], "fy":[-0.32831,-0.32871,-0.329,-0.32859]},
+ {"t":1.52531, "x":9.20636, "y":6.37103, "heading":-0.60687, "vx":4.2955, "vy":-1.39996, "omega":-0.03116, "ax":-0.08017, "ay":-0.23028, "alpha":-0.00738, "fx":[-1.13337,-1.11939,-1.13954,-1.15351], "fy":[-3.28118,-3.26106,-3.24708,-3.2672]},
+ {"t":1.57035, "x":9.39972, "y":6.30775, "heading":-0.60828, "vx":4.29189, "vy":-1.41033, "omega":-0.0315, "ax":-0.69093, "ay":-1.61386, "alpha":-0.21725, "fx":[-9.6905,-9.28239,-9.89754,-10.30467], "fy":[-23.36893,-22.80011,-22.3828,-22.95256]},
+ {"t":1.61538, "x":9.5923, "y":6.2426, "heading":-0.60992, "vx":4.26078, "vy":-1.48301, "omega":-0.04128, "ax":-6.3158, "ay":-3.45254, "alpha":-7.34297, "fx":[-80.31613,-78.25812,-99.63089,-99.89484], "fy":[-70.72513,-54.47209,-24.29457,-46.26378]},
+ {"t":1.66042, "x":9.77777, "y":6.17232, "heading":-0.61922, "vx":3.97635, "vy":-1.63849, "omega":-0.37196, "ax":-8.60205, "ay":-1.78131, "alpha":-11.20336, "fx":[-109.20605,-120.6819,-128.04562,-129.79462], "fy":[-71.28128,-27.9809,23.5784,-25.31491]},
+ {"t":1.70545, "x":9.94812, "y":6.09672, "heading":-0.64733, "vx":3.58897, "vy":-1.71871, "omega":-0.87649, "ax":-8.92961, "ay":-1.68327, "alpha":-11.01281, "fx":[-114.08641,-127.52785,-131.45809,-133.22818], "fy":[-70.68898,-27.93345,26.28741,-23.10469]},
+ {"t":1.75048, "x":10.10069, "y":6.01762, "heading":-0.69797, "vx":3.18684, "vy":-1.79451, "omega":-1.37243, "ax":-9.03811, "ay":-2.06071, "alpha":-9.96075, "fx":[-116.24712,-127.8914,-134.36731,-133.94629], "fy":[-70.11007,-38.71282,17.32314,-25.34053]},
+ {"t":1.79552, "x":10.23504, "y":5.93471, "heading":-0.76988, "vx":2.77982, "vy":-1.88731, "omega":-1.821, "ax":-8.97382, "ay":-2.90535, "alpha":-8.15733, "fx":[-116.29841,-123.61825,-136.17513,-132.71521], "fy":[-71.67329,-55.3878,-4.12168,-33.54774]},
+ {"t":1.84055, "x":10.35113, "y":5.84678, "heading":-0.86015, "vx":2.3757, "vy":-2.01815, "omega":-2.18835, "ax":-8.62792, "ay":-4.17612, "alpha":-5.3537, "fx":[-113.87779,-116.07831,-131.42228,-127.81628], "fy":[-76.47633,-72.13637,-38.13611,-50.03333]},
+ {"t":1.88558, "x":10.44936, "y":5.75166, "heading":-0.96413, "vx":1.98716, "vy":-2.20622, "omega":-2.42945, "ax":-7.94692, "ay":-5.55321, "alpha":-1.57988, "fx":[-109.84455,-109.50556,-115.66908,-115.56337], "fy":[-82.829,-83.10447,-74.28581,-74.64244]},
+ {"t":1.93062, "x":10.53079, "y":5.64667, "heading":-1.07514, "vx":1.62928, "vy":-2.4563, "omega":-2.50059, "ax":-7.12793, "ay":-6.6006, "alpha":2.39626, "fx":[-105.18559,-107.25341,-97.57362,-94.13405], "fy":[-89.11273,-86.79227,-97.58114,-100.76163]},
+ {"t":1.97565, "x":10.59694, "y":5.52936, "heading":-1.18532, "vx":1.30829, "vy":-2.75354, "omega":-2.39268, "ax":-6.78933, "ay":-6.86477, "alpha":4.95931, "fx":[-103.66538,-110.12096,-91.60625,-79.55602], "fy":[-90.54205,-83.01505,-103.18157,-112.4872]},
+ {"t":2.01737, "x":10.64561, "y":5.4085, "heading":-1.28083, "vx":1.02502, "vy":-3.03996, "omega":-2.18576, "ax":-6.66895, "ay":-6.8357, "alpha":6.68957, "fx":[-102.84367,-113.68509,-90.77883,-70.81571], "fy":[-90.00709,-76.96458,-103.23057,-117.37567]},
+ {"t":2.05909, "x":10.68258, "y":5.27572, "heading":-1.36621, "vx":0.74677, "vy":-3.32517, "omega":-1.90666, "ax":-5.99432, "ay":-7.14461, "alpha":9.02836, "fx":[-91.32885,-113.59505,-84.52602,-50.42208], "fy":[-98.00821,-74.33643,-106.93445,-125.8135]},
+ {"t":2.10082, "x":10.70852, "y":5.13076, "heading":-1.4379, "vx":0.49667, "vy":-3.62326, "omega":-1.52997, "ax":-4.33839, "ay":-7.70005, "alpha":11.79954, "fx":[-51.39196,-105.31894,-70.47347,-18.79795], "fy":[-114.69967,-78.24442,-113.096,-130.5456]},
+ {"t":2.14254, "x":10.72546, "y":4.97289, "heading":-1.49146, "vx":0.31566, "vy":-3.94453, "omega":-1.03766, "ax":-2.89713, "ay":-7.31303, "alpha":12.3746, "fx":[-17.31903,-84.83937,-56.75053,-5.35548], "fy":[-104.59017,-79.91638,-109.43594,-120.69959]},
+ {"t":2.18426, "x":10.73611, "y":4.80195, "heading":-1.52399, "vx":0.19478, "vy":-4.24965, "omega":-0.52136, "ax":-1.75949, "ay":-3.98701, "alpha":7.12827, "fx":[-13.15368,-41.27304,-35.26784,-10.06664], "fy":[-47.36295,-45.03707,-65.55663,-68.10283]},
+ {"t":2.22599, "x":10.74271, "y":4.62117, "heading":-1.53954, "vx":0.12137, "vy":-4.416, "omega":-0.22395, "ax":-0.99349, "ay":0.92804, "alpha":-1.75109, "fx":[-16.96264,-11.13219,-11.22552,-17.0097], "fy":[10.11123,10.38262,16.22004,15.9049]},
+ {"t":2.26771, "x":10.74691, "y":4.43773, "heading":-1.5504, "vx":0.07992, "vy":-4.37728, "omega":-0.29701, "ax":-2.22966, "ay":6.16566, "alpha":-11.04376, "fx":[-63.91988,-7.47065,-6.08815,-48.94072], "fy":[68.60489,82.54904,103.54363,94.88924]},
+ {"t":2.30943, "x":10.7483, "y":4.26047, "heading":-1.57241, "vx":-0.01311, "vy":-4.12003, "omega":-0.75778, "ax":-3.82824, "ay":7.59372, "alpha":-12.70292, "fx":[-100.34154,-30.35002,-16.2969,-70.06893], "fy":[77.00415,115.78924,127.85162,109.91192]},
+ {"t":2.35115, "x":10.74442, "y":4.09518, "heading":-1.61508, "vx":-0.17283, "vy":-3.8032, "omega":-1.28778, "ax":-4.77672, "ay":7.67658, "alpha":-11.70335, "fx":[-110.76576,-55.39553,-26.73687,-77.93724], "fy":[75.04302,118.01493,131.70353,110.49351]},
+ {"t":2.39288, "x":10.73305, "y":3.94318, "heading":-1.679, "vx":-0.37213, "vy":-3.48291, "omega":-1.77608, "ax":-3.8913, "ay":8.35179, "alpha":-11.19003, "fx":[-100.55168,-39.09099,-14.66275,-66.32766], "fy":[90.97681,127.92124,135.28996,119.35025]},
+ {"t":2.4346, "x":10.71414, "y":3.80513, "heading":-1.76284, "vx":-0.53448, "vy":-3.13445, "omega":-2.24296, "ax":-2.42666, "ay":8.98399, "alpha":-10.69508, "fx":[-81.28118,-12.78764,4.0641,-47.58462], "fy":[109.44119,134.63776,136.74345,128.56142]},
+ {"t":2.47632, "x":10.68973, "y":3.68217, "heading":-1.86573, "vx":-0.63573, "vy":-2.75961, "omega":-2.68919, "ax":-1.65048, "ay":9.22914, "alpha":-10.0905, "fx":[-69.2712,-3.65903,14.61481,-35.26505], "fy":[118.02888,135.99342,136.42705,132.83433]},
+ {"t":2.51805, "x":10.66177, "y":3.57507, "heading":-1.98671, "vx":-0.70459, "vy":-2.37454, "omega":-3.11019, "ax":-1.4748, "ay":9.30271, "alpha":-9.65838, "fx":[-65.74662,-7.70846,18.88352,-29.04801], "fy":[120.4735,136.2664,136.13909,134.57557]},
+ {"t":2.55977, "x":10.63108, "y":3.48409, "heading":-2.12489, "vx":-0.76613, "vy":-1.98641, "omega":-3.51317, "ax":-1.89359, "ay":9.26623, "alpha":-9.18277, "fx":[-68.27813,-25.54617,15.08299,-28.62372], "fy":[119.40574,134.40427,136.74257,134.83391]},
+ {"t":2.60149, "x":10.59747, "y":3.40928, "heading":-2.27946, "vx":-0.84513, "vy":-1.5998, "omega":-3.8963, "ax":-2.8967, "ay":9.0815, "alpha":-8.00501, "fx":[-72.52792,-52.57235,-3.97196,-35.16778], "fy":[117.15833,126.77421,137.59451,133.38546]},
+ {"t":2.64321, "x":10.55969, "y":3.35043, "heading":-2.44899, "vx":-0.96599, "vy":-1.22089, "omega":-4.23029, "ax":-4.31877, "ay":8.63208, "alpha":-5.2182, "fx":[-77.68843,-74.43045,-41.06259,-51.6888], "fy":[114.03521,115.87468,131.54094,127.97961]},
+ {"t":2.68494, "x":10.51563, "y":3.30701, "heading":-2.63003, "vx":-1.14618, "vy":-0.86074, "omega":-4.448, "ax":-5.71015, "ay":7.89374, "alpha":-0.62957, "fx":[-82.4056,-82.82113,-79.42943,-79.10379], "fy":[110.85292,110.51322,112.97205,113.2295]},
+ {"t":2.72666, "x":10.46283, "y":3.27797, "heading":-2.81616, "vx":-1.38443, "vy":-0.53139, "omega":-4.47427, "ax":-6.56757, "ay":7.13873, "alpha":4.54583, "fx":[-86.88674,-79.14446,-98.50539,-107.83848], "fy":[107.49643,113.4811,97.2073,86.57415]},
+ {"t":2.76838, "x":10.39935, "y":3.26201, "heading":-2.99888, "vx":-1.65844, "vy":-0.23354, "omega":-4.28461, "ax":-6.21935, "ay":7.32805, "alpha":7.33429, "fx":[-81.53368,-63.45407,-94.35024,-113.29312], "fy":[111.61754,123.03316,101.38515,79.45762]},
+ {"t":2.80896, "x":10.32695, "y":3.25857, "heading":3.1165, "vx":-1.91078, "vy":0.06378, "omega":-3.98703, "ax":-5.68723, "ay":7.6949, "alpha":8.15271, "fx":[-76.69029,-50.15535,-85.38827,-110.22652], "fy":[114.82164,128.94168,108.97571,83.55426]},
+ {"t":2.84953, "x":10.24474, "y":3.26749, "heading":2.96144, "vx":-2.14153, "vy":0.37599, "omega":-3.65625, "ax":-5.67357, "ay":7.51854, "alpha":10.60496, "fx":[-85.49852,-37.51354,-81.98087,-116.69323], "fy":[107.86566,132.97727,111.43554,74.01581]},
+ {"t":2.8901, "x":10.15318, "y":3.28893, "heading":2.82182, "vx":-2.37173, "vy":0.68104, "omega":-3.22597, "ax":-6.03055, "ay":7.07, "alpha":12.30101, "fx":[-104.50592,-35.71101,-81.17357,-120.53571], "fy":[88.73888,133.12806,111.79761,67.19772]},
+ {"t":2.93067, "x":10.05199, "y":3.32238, "heading":2.70106, "vx":-2.6164, "vy":0.96789, "omega":-2.72688, "ax":-6.53126, "ay":6.48564, "alpha":13.38665, "fx":[-119.80967,-44.52185,-83.2684,-122.71612], "fy":[65.53379,129.84647,109.85616,62.49318]},
+ {"t":2.97125, "x":9.94046, "y":3.36699, "heading":2.60144, "vx":-2.8814, "vy":1.23104, "omega":-2.18374, "ax":-7.04631, "ay":5.81658, "alpha":13.87985, "fx":[-128.49466,-58.62837,-87.48755,-124.90857], "fy":[44.05681,123.03393,105.8488,56.85523]},
+ {"t":3.01182, "x":9.81775, "y":3.42173, "heading":2.52426, "vx":-3.16729, "vy":1.46703, "omega":-1.62059, "ax":-7.50926, "ay":5.05054, "alpha":13.9417, "fx":[-132.29853,-73.85711,-92.56461,-127.04784], "fy":[24.40208,112.30223,100.10896,49.54741]},
+ {"t":3.05239, "x":9.68306, "y":3.48541, "heading":2.46999, "vx":-3.47197, "vy":1.67195, "omega":-1.05493, "ax":-7.82429, "ay":4.13872, "alpha":13.64145, "fx":[-131.14394,-87.26968,-97.25981,-127.95645], "fy":[6.23,95.94972,92.02069,40.461]},
+ {"t":3.09297, "x":9.53575, "y":3.55665, "heading":2.43841, "vx":-3.78942, "vy":1.83987, "omega":-0.50145, "ax":-7.23606, "ay":2.24232, "alpha":11.54117, "fx":[-110.1094,-87.41429,-95.83316,-116.92108], "fy":[-11.87041,50.20285,66.27423,22.53075]},
+ {"t":3.13354, "x":9.37605, "y":3.63314, "heading":2.42757, "vx":-4.08301, "vy":1.93085, "omega":-0.03319, "ax":-0.41708, "ay":-0.5018, "alpha":0.25541, "fx":[-5.86677,-5.31348,-5.95758,-6.51035], "fy":[-7.71066,-7.07294,-6.51499,-7.15318]},
+ {"t":3.17411, "x":9.21004, "y":3.71107, "heading":2.42643, "vx":-4.09994, "vy":1.91049, "omega":-0.02283, "ax":-0.04835, "ay":-0.09979, "alpha":0.00324, "fx":[-0.68475,-0.6777,-0.68582,-0.69287], "fy":[-1.42207,-1.41396,-1.40691,-1.41502]},
+ {"t":3.21469, "x":9.04366, "y":3.7885, "heading":2.42551, "vx":-4.1019, "vy":1.90644, "omega":-0.0227, "ax":-0.00657, "ay":-0.01352, "alpha":0.00071, "fx":[-0.09305,-0.09149,-0.09328,-0.09483], "fy":[-0.19336,-0.19158,-0.19002,-0.19181]},
+ {"t":3.25526, "x":8.87722, "y":3.86584, "heading":2.42459, "vx":-4.10216, "vy":1.90589, "omega":-0.02267, "ax":0.01043, "ay":0.02238, "alpha":0.00016, "fx":[0.1479,0.14824,0.14785,0.14751], "fy":[0.31688,0.31727,0.31762,0.31722]},
+ {"t":3.29583, "x":8.71079, "y":3.94319, "heading":2.42367, "vx":-4.10174, "vy":1.9068, "omega":-0.02266, "ax":0.06942, "ay":0.14656, "alpha":-0.00179, "fx":[0.98367,0.97976,0.98423,0.98814], "fy":[2.08164,2.07717,2.07327,2.07774]},
+ {"t":3.33641, "x":8.54443, "y":4.02068, "heading":2.42275, "vx":-4.09892, "vy":1.91275, "omega":-0.02273, "ax":0.40071, "ay":0.83205, "alpha":-0.0138, "fx":[5.67755,5.64746,5.68234,5.71243], "fy":[11.82631,11.79223,11.76199,11.79607]},
+ {"t":3.37698, "x":8.37845, "y":4.09897, "heading":2.42181, "vx":-4.08267, "vy":1.94651, "omega":-0.02329, "ax":1.96268, "ay":3.80787, "alpha":-0.09241, "fx":[27.77121,27.58217,27.87022,28.05877], "fy":[54.17026,53.99679,53.78082,53.95469]},
+ {"t":3.41755, "x":8.21442, "y":4.18108, "heading":2.42079, "vx":-4.00303, "vy":2.101, "omega":-0.02704, "ax":4.27213, "ay":6.93426, "alpha":-0.46567, "fx":[59.80111,59.15965,61.33234,61.93243], "fy":[99.08225,98.92082,97.49085,97.67212]},
+ {"t":3.45813, "x":8.05552, "y":4.27203, "heading":2.41931, "vx":-3.8297, "vy":2.38235, "omega":-0.04594, "ax":5.7986, "ay":7.15018, "alpha":-1.41159, "fx":[79.2038,78.35338,85.40603,85.81201], "fy":[104.15424,104.11763,98.38258,98.75406]},
+ {"t":3.4987, "x":7.90491, "y":4.37457, "heading":2.41628, "vx":-3.59443, "vy":2.67245, "omega":-0.10321, "ax":6.19654, "ay":7.21691, "alpha":-0.63764, "fx":[86.36366,86.18751,89.35191,89.43467], "fy":[103.64364,103.63164,100.91331,101.0031]},
+ {"t":3.53927, "x":7.76417, "y":4.48894, "heading":2.41157, "vx":-3.34302, "vy":2.96527, "omega":-0.12908, "ax":6.10911, "ay":7.37199, "alpha":4.7042, "fx":[98.85744,97.69394,76.91171,72.91757], "fy":[93.83946,95.78491,113.16923,115.19117]},
+ {"t":3.57985, "x":7.63357, "y":4.61532, "heading":2.41021, "vx":-3.09515, "vy":3.26437, "omega":0.06178, "ax":9.39955, "ay":0.45133, "alpha":9.62102, "fx":[131.85969,137.43992,127.52097,136.1249], "fy":[-38.12968,6.82167,50.77019,6.12766]},
+ {"t":3.62295, "x":7.50887, "y":4.75646, "heading":2.42181, "vx":-2.68995, "vy":3.28383, "omega":0.47653, "ax":9.40028, "ay":-0.20959, "alpha":8.09585, "fx":[130.42354,136.57688,131.42101,134.56538], "fy":[-38.89786,-0.33131,34.87846,-7.53289]},
+ {"t":3.66606, "x":7.40165, "y":4.89783, "heading":2.44987, "vx":-2.28473, "vy":3.27479, "omega":0.82552, "ax":9.14725, "ay":-1.35545, "alpha":7.67635, "fx":[124.24217,133.90854,132.3175,128.17236], "fy":[-49.89403,-12.94041,15.93896,-29.95747]},
+ {"t":3.70917, "x":7.31166, "y":5.03774, "heading":2.49259, "vx":-1.8904, "vy":3.21636, "omega":1.15644, "ax":8.32731, "ay":-3.12094, "alpha":8.26048, "fx":[109.65894,126.51516,127.18885,108.78748], "fy":[-69.89688,-32.72523,-11.10073,-63.23171]},
+ {"t":3.75228, "x":7.2379, "y":5.17349, "heading":2.55012, "vx":-1.53143, "vy":3.08182, "omega":1.51253, "ax":7.02112, "ay":-4.61744, "alpha":8.91083, "fx":[91.24258,114.47634,112.57818,79.79375], "fy":[-85.47559,-50.35173,-38.0614,-87.91572]},
+ {"t":3.79539, "x":7.17841, "y":5.30205, "heading":2.6236, "vx":-1.22876, "vy":2.88278, "omega":1.89666, "ax":6.31822, "ay":-4.72282, "alpha":8.71823, "fx":[84.05644,106.61011,99.87875,67.69165], "fy":[-84.28308,-51.50717,-43.97478,-88.01441]},
+ {"t":3.8385, "x":7.13131, "y":5.42193, "heading":2.71346, "vx":-0.95639, "vy":2.67918, "omega":2.27249, "ax":6.40073, "ay":-3.36281, "alpha":7.56038, "fx":[88.77064,103.81684,95.07877,75.249], "fy":[-64.49985,-34.38708,-26.46871,-65.3124]},
+ {"t":3.8816, "x":7.09603, "y":5.5343, "heading":2.81845, "vx":-0.68047, "vy":2.53422, "omega":2.5984, "ax":6.19379, "ay":-0.37945, "alpha":4.63718, "fx":[90.30197,93.8549,85.38548,81.63984], "fy":[-18.08161,1.28076,8.62543,-13.33898]},
+ {"t":3.92471, "x":7.07245, "y":5.6432, "heading":2.93477, "vx":-0.41347, "vy":2.51786, "omega":2.7983, "ax":3.62598, "ay":4.12168, "alpha":-1.41114, "fx":[48.6783,49.04754,54.17189,53.69169], "fy":[61.63633,57.56192,55.18351,59.31331]},
+ {"t":3.96782, "x":7.058, "y":5.75557, "heading":3.05409, "vx":-0.25716, "vy":2.69554, "omega":2.73747, "ax":-0.47645, "ay":6.44795, "alpha":-6.20149, "fx":[-18.15236,-24.59176,7.34054,8.38946], "fy":[97.25247,85.22857,85.76495,97.34658]},
+ {"t":4.01093, "x":7.04647, "y":5.87776, "heading":-3.11685, "vx":-0.2777, "vy":2.9735, "omega":2.47014, "ax":-3.99591, "ay":6.07688, "alpha":-8.02136, "fx":[-66.151,-80.02929,-45.40593,-34.97814], "fy":[90.27918,67.76804,84.17408,102.33204]},
+ {"t":4.05404, "x":7.03078, "y":6.01159, "heading":-3.01782, "vx":-0.44995, "vy":3.23546, "omega":2.12435, "ax":-6.5257, "ay":4.12458, "alpha":-7.6533, "fx":[-97.49408,-107.70574,-89.33452,-75.46685], "fy":[64.51801,34.49071,52.15467,82.69619]},
+ {"t":4.09714, "x":7.00532, "y":6.15489, "heading":-2.93335, "vx":-0.73126, "vy":3.41326, "omega":1.79443, "ax":-6.59504, "ay":2.50886, "alpha":-6.37181, "fx":[-98.76559,-102.3804,-89.11339,-83.673], "fy":[42.11067,16.05519,27.78343,56.30077]},
+ {"t":4.14025, "x":6.96767, "y":6.30437, "heading":-2.86192, "vx":-1.01556, "vy":3.52142, "omega":1.51975, "ax":-5.55528, "ay":-0.117, "alpha":-2.57013, "fx":[-82.35039,-80.67582,-75.10356,-76.84927], "fy":[2.11112,-8.41916,-5.76831,5.44265]},
+ {"t":4.18336, "x":6.91873, "y":6.45606, "heading":-2.79879, "vx":-1.25504, "vy":3.51637, "omega":1.40896, "ax":-4.25399, "ay":-3.86719, "alpha":3.29712, "fx":[-52.26108,-59.63875,-68.05442,-61.24279], "fy":[-60.82157,-48.51926,-49.20081,-60.72441]},
+ {"t":4.22647, "x":6.86068, "y":6.60405, "heading":-2.73499, "vx":-1.43842, "vy":3.34967, "omega":1.55109, "ax":-3.43275, "ay":-6.57911, "alpha":7.04462, "fx":[-24.73514,-48.52115,-71.30946,-50.06783], "fy":[-104.36835,-85.9033,-81.81427,-100.94322]},
+ {"t":4.26958, "x":6.79548, "y":6.74233, "heading":-2.66158, "vx":-1.5864, "vy":3.06605, "omega":1.85477, "ax":-2.27689, "ay":-7.78428, "alpha":8.26528, "fx":[-0.65125,-30.04591,-63.56459,-34.83594], "fy":[-118.54383,-105.96871,-99.8183,-117.03059]},
+ {"t":4.31269, "x":6.72498, "y":6.86727, "heading":-2.57395, "vx":-1.68456, "vy":2.73049, "omega":2.21107, "ax":1.24606, "ay":-7.90063, "alpha":8.97543, "fx":[46.99522,33.16231,-17.78522,8.27794], "fy":[-109.48239,-103.9083,-113.68483,-120.88247]},
+ {"t":4.35579, "x":6.65352, "y":6.97764, "heading":-2.47029, "vx":-1.63084, "vy":2.3899, "omega":2.59799, "ax":4.20276, "ay":-6.79208, "alpha":8.18835, "fx":[79.91638,76.80974,33.28768,48.27883], "fy":[-88.36996,-79.85716,-107.34099,-109.53648]},
+ {"t":4.3989, "x":6.58712, "y":7.07435, "heading":-2.35069, "vx":-1.44967, "vy":2.09711, "omega":2.95097, "ax":5.83294, "ay":-5.45098, "alpha":7.23597, "fx":[97.17287,95.50022,64.55621,73.4929], "fy":[-68.01024,-57.59254,-90.53244,-92.93012]},
+ {"t":4.44201, "x":6.53005, "y":7.15969, "heading":-2.21675, "vx":-1.19822, "vy":1.86213, "omega":3.2629, "ax":5.73804, "ay":-5.00877, "alpha":7.35896, "fx":[96.7177,91.10998,62.46021,75.05353], "fy":[-60.2025,-52.0071,-85.54859,-86.23422]},
+ {"t":4.48512, "x":6.48372, "y":7.23531, "heading":-2.06926, "vx":-0.95086, "vy":1.64621, "omega":3.58013, "ax":-2.99454, "ay":-6.53292, "alpha":4.87615, "fx":[-27.99734,-52.26073,-54.98684,-34.54242], "fy":[-95.35893,-83.73357,-90.67342,-100.64437]},
+ {"t":4.52823, "x":6.43995, "y":7.3002, "heading":-1.91039, "vx":-1.07995, "vy":1.36459, "omega":3.79034, "ax":-8.44137, "ay":-3.93831, "alpha":-3.71482, "fx":[-120.72356,-112.18977,-119.41859,-126.28556], "fy":[-56.47689,-70.73477,-55.51985,-40.56677]},
+ {"t":4.57133, "x":6.38555, "y":7.35537, "heading":-1.75045, "vx":-1.44384, "vy":1.19481, "omega":3.6302, "ax":-8.57259, "ay":-4.17215, "alpha":-6.00669, "fx":[-121.54449,-107.83728,-124.51096,-132.16483], "fy":[-63.16211,-83.78533,-54.70197,-34.90736]},
+ {"t":4.61444, "x":6.31535, "y":7.403, "heading":-1.59954, "vx":-1.81339, "vy":1.01496, "omega":3.37126, "ax":-8.36484, "ay":-4.62668, "alpha":-7.42774, "fx":[-117.10652,-99.47144,-125.21384,-132.48663], "fy":[-72.89632,-95.16568,-56.36132,-37.90502]},
+ {"t":4.65755, "x":6.2294, "y":7.44245, "heading":-1.46111, "vx":-2.17399, "vy":0.81551, "omega":3.05106, "ax":-8.10132, "ay":-5.05184, "alpha":-8.44586, "fx":[-111.48496,-91.27929,-125.25376,-131.31905], "fy":[-81.88121,-103.65909,-57.67829,-43.21606]},
+ {"t":4.70066, "x":6.12816, "y":7.47292, "heading":-1.33744, "vx":-2.52322, "vy":0.59774, "omega":2.68698, "ax":-8.05233, "ay":-5.03866, "alpha":-9.61841, "fx":[-108.46346,-87.977,-128.69455,-131.42453], "fy":[-85.9811,-106.57416,-49.87923,-43.25265]},
+ {"t":4.74733, "x":6.00164, "y":7.49532, "heading":-1.22252, "vx":-2.899, "vy":0.3626, "omega":2.23811, "ax":-8.24549, "ay":-4.56017, "alpha":-10.81132, "fx":[-108.92906,-91.90961,-133.88454,-132.78808], "fy":[-85.10574,-102.77464,-32.38069,-38.29642]},
+ {"t":4.79399, "x":5.85737, "y":7.50728, "heading":-1.12984, "vx":-3.28379, "vy":0.14979, "omega":1.73358, "ax":-8.42603, "ay":-4.06354, "alpha":-11.55391, "fx":[-110.0385,-97.73099,-136.26638,-133.71209], "fy":[-83.16341,-96.47137,-16.92626,-33.83758]},
+ {"t":4.84066, "x":5.69495, "y":7.50984, "heading":-1.06152, "vx":-3.67701, "vy":-0.03985, "omega":1.1944, "ax":-8.55665, "ay":-3.57211, "alpha":-11.97876, "fx":[-111.24699,-103.36601,-136.37424,-134.16646], "fy":[-80.46299,-88.65186,-4.00075,-29.41958]},
+ {"t":4.88733, "x":5.51404, "y":7.5041, "heading":-1.01883, "vx":-4.07632, "vy":-0.20654, "omega":0.63538, "ax":-8.51082, "ay":-3.15701, "alpha":-12.09356, "fx":[-110.93018,-105.34682,-133.37566,-132.90294], "fy":[-77.24039,-80.22514,4.36533,-25.89925]},
+ {"t":4.93399, "x":5.31454, "y":7.49102, "heading":-1.00234, "vx":-4.4735, "vy":-0.35387, "omega":0.07101, "ax":4.09411, "ay":0.89616, "alpha":5.77115, "fx":[59.7441,46.45901,56.90697,69.02224], "fy":[27.4225,17.60713,-2.92805,8.70956]},
+ {"t":4.98066, "x":5.11024, "y":7.47548, "heading":-0.99275, "vx":-4.28244, "vy":-0.31205, "omega":0.34034, "ax":8.62102, "ay":3.07148, "alpha":11.92083, "fx":[111.98119,108.703,134.45985,133.65957], "fy":[76.97917,77.71719,-6.25631,25.71]},
+ {"t":5.02733, "x":4.91978, "y":7.46426, "heading":-0.96388, "vx":-3.88012, "vy":-0.16872, "omega":0.89665, "ax":8.64236, "ay":3.47065, "alpha":11.4837, "fx":[110.86366,108.51804,136.78036,133.85156], "fy":[81.19837,82.55706,1.46734,31.55972]},
+ {"t":5.074, "x":4.74811, "y":7.46017, "heading":-0.90953, "vx":-3.47681, "vy":-0.00675, "omega":1.43256, "ax":8.57452, "ay":3.79145, "alpha":11.18395, "fx":[108.3996,107.53373,137.35161,132.88213], "fy":[85.28847,85.27071,7.16948,37.24283]},
+ {"t":5.12066, "x":4.5952, "y":7.46398, "heading":-0.8305, "vx":-3.07666, "vy":0.17019, "omega":1.95448, "ax":8.46874, "ay":4.03971, "alpha":11.3888, "fx":[104.705,106.23769,137.61995,131.60669], "fy":[90.17321,87.51038,9.02968,42.33443]},
+ {"t":5.16733, "x":4.46084, "y":7.47632, "heading":-0.72689, "vx":-2.68145, "vy":0.35871, "omega":2.48596, "ax":8.32448, "ay":4.2064, "alpha":12.37989, "fx":[99.06109,104.61937,138.03425,130.27519], "fy":[96.553,89.71153,5.50969,46.72488]},
+ {"t":5.214, "x":4.34477, "y":7.49764, "heading":-0.5974, "vx":-2.29297, "vy":0.55501, "omega":3.06369, "ax":8.33864, "ay":4.21614, "alpha":12.10068, "fx":[95.74418,110.20883,138.22703,128.61278], "fy":[99.95843,82.9644,4.75383,51.37433]},
+ {"t":5.26066, "x":4.24684, "y":7.52813, "heading":-0.44125, "vx":-1.90383, "vy":0.75176, "omega":3.62839, "ax":9.41729, "ay":0.88489, "alpha":-10.47736, "fx":[135.36153,136.75203,123.67063,138.1669], "fy":[-29.51152,22.55146,62.30351,-5.17098]},
+ {"t":5.30733, "x":4.16825, "y":7.56418, "heading":-0.28333, "vx":-1.46435, "vy":0.79306, "omega":3.13945, "ax":8.60324, "ay":-2.55088, "alpha":-17.74953, "fx":[118.44989,138.64833,134.91831,95.77923], "fy":[-72.15901,-3.71807,31.29093,-100.04615]},
+ {"t":5.354, "x":4.10928, "y":7.59841, "heading":-0.15615, "vx":-1.06286, "vy":0.67401, "omega":2.31113, "ax":7.98999, "ay":-3.98466, "alpha":-17.91087, "fx":[110.46413,137.95215,138.41713,66.19118], "fy":[-83.98883,-14.84062,-5.24419,-121.85261]},
+ {"t":5.40066, "x":4.06838, "y":7.62553, "heading":-0.0678, "vx":-0.69, "vy":0.48806, "omega":1.47528, "ax":7.57976, "ay":-4.90877, "alpha":-16.287, "fx":[106.70065,137.18501,132.4943,53.38543], "fy":[-88.77714,-20.96951,-40.51144,-128.06424]},
+ {"t":5.44733, "x":4.04444, "y":7.64296, "heading":-0.01669, "vx":-0.33627, "vy":0.25899, "omega":0.71522, "ax":7.20575, "ay":-5.54964, "alpha":-15.32596, "fx":[104.24602,136.46958,121.39369,46.45009], "fy":[-91.67994,-25.32497,-66.87209,-130.78232]},
+ {"t":5.494, "x":4.03659, "y":7.649, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":7.78206, "ay":-4.93089, "alpha":-14.15756, "fx":[110.58707,137.0976,128.89947,64.65097], "fy":[-83.92676,-21.72118,-51.11765,-122.81104]},
+ {"t":5.54354, "x":4.04614, "y":7.64295, "heading":-0.01737, "vx":0.3855, "vy":-0.24426, "omega":-0.70133, "ax":8.083, "ay":-4.36486, "alpha":-14.61395, "fx":[113.8077,137.86778,134.51473,72.10828], "fy":[-79.47112,-15.95527,-33.50973,-118.54711]},
+ {"t":5.59307, "x":4.07515, "y":7.6255, "heading":-0.07004, "vx":0.78592, "vy":-0.46049, "omega":-1.42527, "ax":8.44404, "ay":-3.50979, "alpha":-15.39791, "fx":[117.90037,138.5039,138.4103,83.95435], "fy":[-73.20475,-8.44002,-6.95627,-110.40093]},
+ {"t":5.64261, "x":4.12445, "y":7.59838, "heading":-0.15954, "vx":1.20421, "vy":-0.63436, "omega":-2.18805, "ax":8.91868, "ay":-2.14178, "alpha":-15.28702, "fx":[124.5776,138.68511,136.18027,106.23782], "fy":[-61.03149,3.06325,25.51753,-88.98596]},
+ {"t":5.69215, "x":4.19504, "y":7.56433, "heading":-0.28669, "vx":1.64602, "vy":-0.74045, "omega":-2.94533, "ax":9.59024, "ay":0.17311, "alpha":-8.13334, "fx":[136.1232,137.6415,132.92851,137.06447], "fy":[-26.09103,16.48394,38.95411,-19.53198]},
+ {"t":5.74169, "x":4.28835, "y":7.52786, "heading":-0.44257, "vx":2.1211, "vy":-0.73188, "omega":-3.34824, "ax":9.00527, "ay":2.0599, "alpha":13.38354, "fx":[104.65235,136.78921,135.5862,133.56264], "fy":[90.60179,17.38685,-28.08058,36.88654]},
+ {"t":5.79122, "x":4.40448, "y":7.49413, "heading":-0.59202, "vx":2.5672, "vy":-0.62984, "omega":-2.68525, "ax":8.95544, "ay":2.27409, "alpha":13.20609, "fx":[106.34363,131.68239,134.83889,134.89986], "fy":[88.47988,40.00834,-30.81439,31.26463]},
+ {"t":5.84076, "x":4.54264, "y":7.46572, "heading":-0.70883, "vx":3.01083, "vy":-0.51718, "omega":-2.03105, "ax":9.02167, "ay":2.28084, "alpha":12.25381, "fx":[112.03146,128.27197,135.40708,135.80966], "fy":[80.86446,49.26097,-26.99338,26.18938]},
+ {"t":5.8903, "x":4.70286, "y":7.4429, "heading":-0.79441, "vx":3.45775, "vy":-0.4042, "omega":-1.42402, "ax":9.03585, "ay":2.22379, "alpha":11.911, "fx":[114.90897,126.03068,135.21436,136.17033], "fy":[76.08056,53.18514,-25.55079,22.37208]},
+ {"t":5.93984, "x":4.88523, "y":7.4256, "heading":-0.85034, "vx":3.90536, "vy":-0.29403, "omega":-0.83398, "ax":8.94861, "ay":2.25509, "alpha":11.80465, "fx":[115.18729,122.45234,134.25881,135.47937], "fy":[73.72273,56.55485,-23.26505,20.84912]},
+ {"t":5.98937, "x":5.08967, "y":7.4138, "heading":-0.87717, "vx":4.34865, "vy":-0.18232, "omega":-0.24921, "ax":3.67662, "ay":6.31676, "alpha":4.88251, "fx":[48.635,36.30148,57.1635,66.36093], "fy":[97.03659,96.07285,81.40827,83.63637]},
+ {"t":6.03891, "x":5.30961, "y":7.41252, "heading":-0.88352, "vx":4.53078, "vy":0.13059, "omega":-0.00734, "ax":-0.05895, "ay":1.66711, "alpha":0.00536, "fx":[-0.83425,-0.84844,-0.83684,-0.82266], "fy":[23.64311,23.63211,23.61876,23.62976]},
+ {"t":6.08845, "x":5.53398, "y":7.42104, "heading":-0.88388, "vx":4.52786, "vy":0.21318, "omega":-0.00707, "ax":-0.01284, "ay":0.26384, "alpha":-0.00011, "fx":[-0.18199,-0.1817,-0.18194,-0.18223], "fy":[3.73954,3.73978,3.74007,3.73983]},
+ {"t":6.13799, "x":5.75826, "y":7.43192, "heading":-0.88423, "vx":4.52723, "vy":0.22625, "omega":-0.00708, "ax":-0.00569, "ay":0.11238, "alpha":-0.00004, "fx":[-0.08066,-0.08057,-0.08065,-0.08074], "fy":[1.59286,1.59294,1.59304,1.59296]},
+ {"t":6.18753, "x":5.98252, "y":7.44327, "heading":-0.88458, "vx":4.52695, "vy":0.23182, "omega":-0.00708, "ax":-0.00575, "ay":0.11097, "alpha":-0.00003, "fx":[-0.08146,-0.0814,-0.08145,-0.08152], "fy":[1.57291,1.57296,1.57303,1.57298]},
+ {"t":6.23706, "x":6.20677, "y":7.45489, "heading":-0.88493, "vx":4.52666, "vy":0.23731, "omega":-0.00708, "ax":-0.00666, "ay":0.12558, "alpha":-0.00002, "fx":[-0.09436,-0.09432,-0.09435,-0.0944], "fy":[1.78003,1.78006,1.7801,1.78007]},
+ {"t":6.2866, "x":6.431, "y":7.4668, "heading":-0.88528, "vx":4.52633, "vy":0.24353, "omega":-0.00708, "ax":-0.00259, "ay":0.04723, "alpha":-0.00006, "fx":[-0.03675,-0.03659,-0.03672,-0.03688], "fy":[0.66929,0.66942,0.66958,0.66945]},
+ {"t":6.33614, "x":6.65522, "y":7.47892, "heading":-0.88563, "vx":4.5262, "vy":0.24587, "omega":-0.00709, "ax":-0.1379, "ay":-1.1262, "alpha":-0.25845, "fx":[-2.01426,-1.34295,-1.89515,-2.56655], "fy":[-16.55905,-16.02582,-15.36765,-15.90206]},
+ {"t":6.38568, "x":6.87927, "y":7.48972, "heading":-0.8863, "vx":4.51937, "vy":0.19008, "omega":-0.01989, "ax":-8.98219, "ay":-1.2271, "alpha":-12.2232, "fx":[-119.26492,-126.04209,-128.39862,-135.57614], "fy":[-63.63599,-37.93083,40.11702,-8.1255]},
+ {"t":6.43521, "x":7.09213, "y":7.49763, "heading":-0.90229, "vx":4.07442, "vy":0.1293, "omega":-0.6254, "ax":-9.08544, "ay":-1.7354, "alpha":-12.41639, "fx":[-118.93613,-126.22614,-132.8052,-137.1681], "fy":[-69.06946,-51.20674,34.49409,-12.61319]},
+ {"t":6.48475, "x":7.28282, "y":7.5019, "heading":-0.9485, "vx":3.62434, "vy":0.04333, "omega":-1.24048, "ax":-8.78349, "ay":-3.1884, "alpha":-11.96317, "fx":[-112.60493,-112.39321,-137.68316,-135.33443], "fy":[-80.02679,-79.2894,6.58952,-28.05261]},
+ {"t":6.53429, "x":7.45158, "y":7.50014, "heading":-1.02463, "vx":3.18923, "vy":-0.11462, "omega":-1.8331, "ax":-8.32377, "ay":-4.45027, "alpha":-10.87212, "fx":[-106.39369,-97.68775,-136.08926,-131.77931], "fy":[-88.54116,-97.63876,-23.771,-42.37529]},
+ {"t":6.58383, "x":7.59936, "y":7.489, "heading":-1.12878, "vx":2.77689, "vy":-0.33507, "omega":-2.37168, "ax":-7.98923, "ay":-5.19509, "alpha":-9.36707, "fx":[-103.87953,-90.13996,-130.22821,-128.73384], "fy":[-91.68554,-105.00282,-46.67324,-51.19517]},
+ {"t":6.63336, "x":7.72711, "y":7.46603, "heading":-1.25776, "vx":2.38112, "vy":-0.59242, "omega":-2.8357, "ax":-7.75687, "ay":-5.70547, "alpha":-7.27103, "fx":[-103.88301,-89.0259,-121.64027,-125.25811], "fy":[-91.80094,-106.15044,-66.18694,-59.35638]},
+ {"t":6.6829, "x":7.83555, "y":7.42968, "heading":-1.40715, "vx":1.99687, "vy":-0.87506, "omega":-3.19589, "ax":-7.6431, "ay":-6.01846, "alpha":-4.34103, "fx":[-105.7966,-95.2357,-112.84275,-119.48152], "fy":[-89.65851,-100.74793,-80.47804,-70.35667]},
+ {"t":6.73244, "x":7.92509, "y":7.37895, "heading":-1.5708, "vx":1.61825, "vy":-1.1732, "omega":-3.41094, "ax":-6.59045, "ay":-7.21203, "alpha":1.54877, "fx":[-93.21509,-98.71632,-93.72262,-88.01828], "fy":[-102.51771,-97.2576,-102.10149,-107.03816]},
+ {"t":6.77885, "x":7.99309, "y":7.31674, "heading":-1.72742, "vx":1.31241, "vy":-1.50788, "omega":-3.33907, "ax":-8.02861, "ay":-5.53501, "alpha":1.52891, "fx":[-113.96131,-117.78063,-113.80659,-109.66612], "fy":[-78.3353,-72.53049,-78.6678,-84.2963]},
+ {"t":6.82525, "x":8.04535, "y":7.2408, "heading":-1.88073, "vx":0.93983, "vy":-1.76475, "omega":-3.26811, "ax":-9.01237, "ay":-3.56732, "alpha":1.95701, "fx":[-128.15033,-130.93733,-127.65563,-124.24963], "fy":[-49.69317,-42.15431,-51.43529,-58.98089]},
+ {"t":6.87166, "x":8.07926, "y":7.15506, "heading":-2.03028, "vx":0.5216, "vy":-1.93029, "omega":-3.1773, "ax":-9.21691, "ay":-1.4452, "alpha":4.221, "fx":[-130.93073,-133.12284,-131.59163,-126.94503], "fy":[-16.751,-2.42847,-23.52939,-39.23268]},
+ {"t":6.91806, "x":8.09354, "y":7.06393, "heading":-2.17318, "vx":0.09387, "vy":-1.99736, "omega":-2.98141, "ax":-3.33149, "ay":-0.02776, "alpha":23.93327, "fx":[9.19807,-61.51797,-92.66466,-43.90793], "fy":[14.8844,56.11099,-9.78289,-62.78619]},
+ {"t":6.96447, "x":8.09431, "y":6.97121, "heading":-2.28577, "vx":-0.06073, "vy":-1.99865, "omega":-1.87076, "ax":-0.17211, "ay":0.00557, "alpha":18.60952, "fx":[41.37245,-5.74954,-45.77639,0.39482], "fy":[3.19749,43.63119,-2.95626,-43.5565]},
+ {"t":7.01088, "x":8.09131, "y":6.87847, "heading":-2.35255, "vx":-0.06872, "vy":-1.99839, "omega":-1.00716, "ax":-0.00585, "ay":0.0002, "alpha":11.31336, "fx":[26.48208,-0.18284,-26.64181,0.01099], "fy":[0.09991,26.5647,-0.09393,-26.55923]},
+ {"t":7.05728, "x":8.08811, "y":6.78573, "heading":-2.3871, "vx":-0.06899, "vy":-1.99838, "omega":-0.48214, "ax":0.00049, "ay":-0.00002, "alpha":6.66901, "fx":[15.6573,0.49096,-15.64349,-0.47679], "fy":[-0.4841,15.65016,0.48364,-15.65063]},
+ {"t":7.10369, "x":8.08491, "y":6.69299, "heading":-2.4023, "vx":-0.06897, "vy":-1.99838, "omega":-0.17266, "ax":0.00071, "ay":-0.00002, "alpha":3.87555, "fx":[9.09957,0.42943,-9.07955,-0.40924], "fy":[-0.41967,9.08922,0.419,-9.0899]},
+ {"t":7.1501, "x":8.08171, "y":6.60025, "heading":-2.40613, "vx":-0.06893, "vy":-1.99838, "omega":0.00719, "ax":0.00072, "ay":-0.00002, "alpha":2.23876, "fx":[5.25988,0.27257,-5.23958,-0.25221], "fy":[-0.26274,5.24938,0.26205,-5.25007]},
+ {"t":7.1965, "x":8.07851, "y":6.50751, "heading":-2.40339, "vx":-0.0689, "vy":-1.99838, "omega":0.11109, "ax":0.00072, "ay":-0.00002, "alpha":1.28999, "fx":[3.0355,0.15307,-3.01516,-0.13271], "fy":[-0.14323,3.02499,0.14255,-3.02568]},
+ {"t":7.24291, "x":8.07532, "y":6.41478, "heading":-2.39685, "vx":-0.06887, "vy":-1.99838, "omega":0.17095, "ax":0.00072, "ay":-0.00002, "alpha":0.74199, "fx":[1.75082,0.08097,-1.73049,-0.06063], "fy":[-0.07114,1.74031,0.07046,-1.741]},
+ {"t":7.28932, "x":8.07212, "y":6.32204, "heading":-2.38811, "vx":-0.06883, "vy":-1.99838, "omega":0.20538, "ax":0.00072, "ay":-0.00002, "alpha":0.42692, "fx":[1.01198,0.04214,-0.99168,-0.02184], "fy":[-0.03233,1.00149,0.03165,-1.00217]},
+ {"t":7.33572, "x":8.06893, "y":6.2293, "heading":-2.37812, "vx":-0.0688, "vy":-1.99839, "omega":0.2252, "ax":0.00071, "ay":-0.00002, "alpha":0.24513, "fx":[0.58552,0.02274,-0.56527,-0.0025], "fy":[-0.01296,0.57505,0.01228,-0.57574]},
+ {"t":7.38213, "x":8.06574, "y":6.13656, "heading":-2.36741, "vx":-0.06877, "vy":-1.99839, "omega":0.23657, "ax":0.00071, "ay":-0.00002, "alpha":0.14057, "fx":[0.3401,0.01379,-0.31991,0.00639], "fy":[-0.00404,0.32966,0.00336,-0.33035]},
+ {"t":7.42854, "x":8.06255, "y":6.04382, "heading":-2.35628, "vx":-0.06874, "vy":-1.99839, "omega":0.24309, "ax":0.00071, "ay":-0.00002, "alpha":0.08036, "fx":[0.19874,0.01007,-0.17863,0.01004], "fy":[-0.00036,0.18834,-0.00033,-0.18902]},
+ {"t":7.47494, "x":8.05936, "y":5.95109, "heading":-2.34491, "vx":-0.0687, "vy":-1.99839, "omega":0.24682, "ax":0.00071, "ay":-0.00002, "alpha":0.04526, "fx":[0.11627,0.00882,-0.09624,0.01121], "fy":[0.00086,0.10592,-0.00154,-0.1066]},
+ {"t":7.52135, "x":8.05617, "y":5.85835, "heading":-2.33341, "vx":-0.06867, "vy":-1.99839, "omega":0.24892, "ax":0.0007, "ay":-0.00002, "alpha":0.02647, "fx":[0.0721,0.00855,-0.05216,0.01138], "fy":[0.00108,0.06179,-0.00175,-0.06247]},
+ {"t":7.56775, "x":8.05298, "y":5.76561, "heading":-2.32183, "vx":-0.06864, "vy":-1.99839, "omega":0.25015, "ax":0.0007, "ay":-0.00002, "alpha":0.0147, "fx":[0.04441,0.00873,-0.02457,0.01111], "fy":[0.00085,0.03415,-0.00152,-0.03483]},
+ {"t":7.61416, "x":8.0498, "y":5.67287, "heading":-2.3102, "vx":-0.0686, "vy":-1.99839, "omega":0.25083, "ax":0.0007, "ay":-0.00002, "alpha":0.00808, "fx":[0.02883,0.009,-0.00909,0.01074], "fy":[0.00054,0.01862,-0.00121,-0.01929]},
+ {"t":7.66057, "x":8.04662, "y":5.58013, "heading":-2.29855, "vx":-0.06857, "vy":-1.99839, "omega":0.25121, "ax":0.00069, "ay":-0.00002, "alpha":0.00416, "fx":[0.01956,0.00925,0.00007,0.01038], "fy":[0.00023,0.00941,-0.0009,-0.01008]},
+ {"t":7.70697, "x":8.04343, "y":5.4874, "heading":-2.28689, "vx":-0.06854, "vy":-1.99839, "omega":0.2514, "ax":0.00069, "ay":-0.00002, "alpha":0.00181, "fx":[0.01401,0.00947,0.00551,0.01006], "fy":[-0.00004,0.00391,-0.00063,-0.00458]},
+ {"t":7.75338, "x":8.04025, "y":5.39466, "heading":-2.27522, "vx":-0.06851, "vy":-1.9984, "omega":0.25149, "ax":0.00068, "ay":-0.00002, "alpha":0.00051, "fx":[0.01089,0.00961,0.00852,0.0098], "fy":[-0.00023,0.00085,-0.00043,-0.00151]},
+ {"t":7.79979, "x":8.03708, "y":5.30192, "heading":-2.26355, "vx":-0.06848, "vy":-1.9984, "omega":0.25151, "ax":0.00068, "ay":-0.00002, "alpha":-0.00015, "fx":[0.00929,0.00968,0.01,0.00961], "fy":[-0.00036,-0.00068,-0.0003,0.00003]},
+ {"t":7.84619, "x":8.0339, "y":5.20918, "heading":-2.25188, "vx":-0.06844, "vy":-1.9984, "omega":0.2515, "ax":0.00068, "ay":-0.00002, "alpha":-0.00042, "fx":[0.00859,0.00969,0.01058,0.00948], "fy":[-0.00043,-0.00132,-0.00022,0.00067]},
+ {"t":7.8926, "x":8.03072, "y":5.11644, "heading":-2.24021, "vx":-0.06841, "vy":-1.9984, "omega":0.25148, "ax":0.00067, "ay":-0.00002, "alpha":-0.00051, "fx":[0.00835,0.00966,0.0107,0.00939], "fy":[-0.00046,-0.0015,-0.00019,0.00085]},
+ {"t":7.93901, "x":8.02755, "y":5.0237, "heading":-2.22854, "vx":-0.06838, "vy":-1.9984, "omega":0.25146, "ax":0.00067, "ay":-0.00002, "alpha":0.00027, "fx":[0.0101,0.00938,0.00882,0.00954], "fy":[-0.00024,0.00032,-0.0004,-0.00096]},
+ {"t":7.98541, "x":8.02438, "y":4.93096, "heading":-2.21687, "vx":-0.06835, "vy":-1.9984, "omega":0.25147, "ax":0.00066, "ay":-0.00002, "alpha":-0.00145, "fx":[0.00604,0.00987,0.01276,0.00893], "fy":[-0.00079,-0.00368,0.00015,0.00304]},
+ {"t":8.03182, "x":8.02121, "y":4.83823, "heading":-2.2052, "vx":-0.06832, "vy":-1.9984, "omega":0.25141, "ax":0.00066, "ay":-0.00002, "alpha":-0.00141, "fx":[0.00605,0.00984,0.01262,0.00884], "fy":[-0.00082,-0.0036,0.00018,0.00296]},
+ {"t":8.07823, "x":8.01804, "y":4.74549, "heading":-2.19354, "vx":-0.06829, "vy":-1.9984, "omega":0.25134, "ax":0.00065, "ay":-0.00002, "alpha":-0.00114, "fx":[0.00664,0.00971,0.01191,0.00884], "fy":[-0.00075,-0.00295,0.00012,0.00232]},
+ {"t":8.12463, "x":8.01487, "y":4.65275, "heading":-2.18187, "vx":-0.06826, "vy":-1.9984, "omega":0.25129, "ax":0.00065, "ay":-0.00002, "alpha":-0.00111, "fx":[0.00665,0.00966,0.01176,0.00876], "fy":[-0.00076,-0.00287,0.00014,0.00224]},
+ {"t":8.17104, "x":8.0117, "y":4.56001, "heading":-2.17021, "vx":-0.06823, "vy":-1.9984, "omega":0.25124, "ax":0.00065, "ay":-0.00002, "alpha":-0.00099, "fx":[0.00685,0.00958,0.01144,0.00871], "fy":[-0.00074,-0.0026,0.00012,0.00198]},
+ {"t":8.21745, "x":8.00853, "y":4.46727, "heading":-2.15856, "vx":-0.0682, "vy":-1.99841, "omega":0.25119, "ax":0.00064, "ay":-0.00002, "alpha":-0.00129, "fx":[0.00612,0.00967,0.01204,0.00849], "fy":[-0.0009,-0.00327,0.00028,0.00265]},
+ {"t":8.26385, "x":8.00537, "y":4.37453, "heading":-2.1469, "vx":-0.06817, "vy":-1.99841, "omega":0.25113, "ax":0.00064, "ay":-0.00002, "alpha":-0.0014, "fx":[0.0058,0.0097,0.01223,0.00833], "fy":[-0.00099,-0.00353,0.00038,0.00291]},
+ {"t":8.31026, "x":8.00221, "y":4.28179, "heading":-2.13525, "vx":-0.06814, "vy":-1.99841, "omega":0.25106, "ax":0.00063, "ay":-0.00002, "alpha":-0.00133, "fx":[0.00591,0.00963,0.01199,0.00827], "fy":[-0.00099,-0.00334,0.00038,0.00273]},
+ {"t":8.35666, "x":7.99905, "y":4.18905, "heading":-2.1236, "vx":-0.06811, "vy":-1.99841, "omega":0.251, "ax":0.00063, "ay":-0.00002, "alpha":-0.00214, "fx":[0.004,0.01004,0.01377,0.00773], "fy":[-0.00146,-0.00519,0.00085,0.00458]},
+ {"t":8.40307, "x":7.99589, "y":4.09631, "heading":-2.11195, "vx":-0.06808, "vy":-1.99841, "omega":0.2509, "ax":0.00062, "ay":-0.00002, "alpha":-0.00252, "fx":[0.00309,0.01025,0.01455,0.00739], "fy":[-0.00173,-0.00603,0.00113,0.00543]},
+ {"t":8.44948, "x":7.99273, "y":4.00358, "heading":-2.10031, "vx":-0.06805, "vy":-1.99841, "omega":0.25079, "ax":0.00062, "ay":-0.00002, "alpha":-0.00406, "fx":[-0.00047,0.01117,0.01798,0.00635], "fy":[-0.00271,-0.00952,0.00212,0.00893]},
+ {"t":8.49588, "x":7.98957, "y":3.91084, "heading":-2.08868, "vx":-0.06802, "vy":-1.99841, "omega":0.2506, "ax":0.00061, "ay":-0.00002, "alpha":-0.00522, "fx":[-0.00313,0.01194,0.02052,0.00545], "fy":[-0.00354,-0.01212,0.00294,0.01153]},
+ {"t":8.54229, "x":7.98641, "y":3.8181, "heading":-2.07705, "vx":-0.068, "vy":-1.99841, "omega":0.25036, "ax":0.00061, "ay":-0.00002, "alpha":-0.00804, "fx":[-0.00951,0.01383,0.02677,0.00343], "fy":[-0.00549,-0.01843,0.00491,0.01785]},
+ {"t":8.5887, "x":7.98326, "y":3.72536, "heading":-2.06545, "vx":-0.06797, "vy":-1.99841, "omega":0.24998, "ax":0.0006, "ay":-0.00002, "alpha":-0.01313, "fx":[-0.02097,0.01741,0.03811,-0.00027], "fy":[-0.00913,-0.02983,0.00855,0.02925]},
+ {"t":8.6351, "x":7.98011, "y":3.63262, "heading":-2.05386, "vx":-0.06794, "vy":-1.99841, "omega":0.24937, "ax":0.0006, "ay":-0.00002, "alpha":-0.02188, "fx":[-0.04053,0.0238,0.05754,-0.00679], "fy":[-0.01558,-0.04932,0.015,0.04874]},
+ {"t":8.68151, "x":7.97695, "y":3.53988, "heading":-2.04231, "vx":-0.06791, "vy":-1.99842, "omega":0.24836, "ax":0.0006, "ay":-0.00002, "alpha":-0.03735, "fx":[-0.07497,0.03552,0.09186,-0.01863], "fy":[-0.02737,-0.0837,0.02679,0.08313]},
+ {"t":8.72792, "x":7.9738, "y":3.44714, "heading":-2.03082, "vx":-0.06788, "vy":-1.99842, "omega":0.24663, "ax":0.00059, "ay":-0.00002, "alpha":-0.0643, "fx":[-0.13467,0.05665,0.15144,-0.03987], "fy":[-0.04855,-0.14334,0.04797,0.14277]},
+ {"t":8.77432, "x":7.97065, "y":3.3544, "heading":-2.01945, "vx":-0.06786, "vy":-1.99842, "omega":0.24364, "ax":0.00059, "ay":-0.00002, "alpha":-0.11132, "fx":[-0.23836,0.09469,0.25501,-0.07803], "fy":[-0.08664,-0.24697,0.08608,0.2464]},
+ {"t":8.82073, "x":7.9675, "y":3.26166, "heading":-2.00826, "vx":-0.06783, "vy":-1.99842, "omega":0.23848, "ax":0.00058, "ay":-0.00002, "alpha":-0.19311, "fx":[-0.41797,0.16286,0.4345,-0.14632], "fy":[-0.15487,-0.42652,0.15431,0.42595]},
+ {"t":8.86714, "x":7.96436, "y":3.16892, "heading":-1.9974, "vx":-0.0678, "vy":-1.99842, "omega":0.22951, "ax":0.00058, "ay":-0.00002, "alpha":-0.33501, "fx":[-0.72826,0.2844,0.74468,-0.26799], "fy":[-0.27648,-0.73675,0.27591,0.73619]},
+ {"t":8.91354, "x":7.96121, "y":3.07618, "heading":-1.98711, "vx":-0.06778, "vy":-1.99842, "omega":0.21397, "ax":0.00058, "ay":-0.00002, "alpha":-0.58163, "fx":[-1.26546,0.5008,1.28177,-0.48449], "fy":[-0.49292,-1.27389,0.49236,1.27334]},
+ {"t":8.95995, "x":7.95807, "y":2.98344, "heading":-1.97781, "vx":-0.06775, "vy":-1.99842, "omega":0.18698, "ax":0.00057, "ay":-0.00002, "alpha":-1.01044, "fx":[-2.19646,0.8845,2.21265,-0.8683], "fy":[-0.87668,-2.20483,0.87612,2.20428]},
+ {"t":9.00635, "x":7.95492, "y":2.8907, "heading":-1.97022, "vx":-0.06772, "vy":-1.99842, "omega":0.14008, "ax":0.00057, "ay":-0.00002, "alpha":-1.75401, "fx":[-3.80715,1.55837,3.82323,-1.54228], "fy":[-1.55061,-3.81546,1.55005,3.81492]},
+ {"t":9.05276, "x":7.95178, "y":2.79796, "heading":-1.96561, "vx":-0.0677, "vy":-1.99842, "omega":0.05869, "ax":0.00056, "ay":-0.00002, "alpha":-3.04153, "fx":[-6.59537,2.72674,6.61114,-2.71091], "fy":[-2.71911,-6.60351,2.71854,6.603]},
+ {"t":9.09917, "x":7.94864, "y":2.70522, "heading":-1.96616, "vx":-0.06767, "vy":-1.99842, "omega":-0.08246, "ax":0.00035, "ay":-0.00001, "alpha":-5.2527, "fx":[-11.40145,4.69404,11.41125,-4.68414], "fy":[-4.68929,-11.40649,4.6889,11.40621]},
+ {"t":9.14557, "x":7.9455, "y":2.61248, "heading":-1.97564, "vx":-0.06765, "vy":-1.99842, "omega":-0.32622, "ax":-0.00596, "ay":0.0002, "alpha":-8.9837, "fx":[-19.6666,7.74853,19.5003,-7.92029], "fy":[-7.83016,-19.58199,7.83867,19.58494]},
+ {"t":9.19198, "x":7.94235, "y":2.51974, "heading":-2.00045, "vx":-0.06793, "vy":-1.99841, "omega":-0.74312, "ax":-0.18312, "ay":0.00662, "alpha":-15.01333, "fx":[-35.50537,9.55232,30.55916,-14.98867], "fy":[-12.07537,-33.072,12.49043,33.03209]},
+ {"t":9.23839, "x":7.93901, "y":2.42701, "heading":-2.05111, "vx":-0.07643, "vy":-1.99811, "omega":-1.43984, "ax":-3.93655, "ay":0.33196, "alpha":-18.01085, "fx":[-88.11782,-48.99808,-18.3019,-67.78093], "fy":[-8.18311,-44.74291,25.48896,46.25881]},
+ {"t":9.28479, "x":7.93122, "y":2.33464, "heading":-2.13732, "vx":-0.25911, "vy":-1.9827, "omega":-2.27566, "ax":-9.15922, "ay":2.23835, "alpha":-1.99031, "fx":[-131.1032,-131.56107,-128.60496,-128.04978], "fy":[28.16471,23.61543,35.77858,39.35333]},
+ {"t":9.3312, "x":7.90933, "y":2.24504, "heading":-2.24507, "vx":-0.68416, "vy":-1.87883, "omega":-2.36802, "ax":-8.68943, "ay":4.33136, "alpha":-0.72474, "fx":[-124.00138,-124.42742,-122.32139,-121.93234], "fy":[59.8072,58.81691,63.05894,63.90103]},
+ {"t":9.37761, "x":7.86823, "y":2.16252, "heading":-2.35574, "vx":-1.0874, "vy":-1.67783, "omega":-2.40165, "ax":-7.51674, "ay":6.22503, "alpha":-0.42829, "fx":[-107.3602,-107.5354,-105.71982,-105.57673], "fy":[87.26979,87.03565,89.23048,89.41753]},
+ {"t":9.42401, "x":7.80967, "y":2.09136, "heading":-2.46765, "vx":-1.43623, "vy":-1.38894, "omega":-2.42153, "ax":-6.23124, "ay":7.49627, "alpha":-0.39386, "fx":[-89.30229,-89.35091,-87.33211,-87.32032], "fy":[105.45615,105.39696,107.07535,107.1027]},
+ {"t":9.45613, "x":7.76033, "y":2.05062, "heading":-2.54562, "vx":-1.63635, "vy":-1.14819, "omega":-2.43418, "ax":-4.94832, "ay":8.36092, "alpha":-0.51255, "fx":[-71.66865,-71.51133,-68.57764,-68.80747], "fy":[117.62476,117.68657,119.422,119.32303]},
+ {"t":9.48825, "x":7.70523, "y":2.01805, "heading":-2.62407, "vx":-1.79527, "vy":-0.87967, "omega":-2.45064, "ax":-3.56111, "ay":8.95625, "alpha":-0.74369, "fx":[-52.99841,-52.42341,-47.87745,-48.61244], "fy":[125.98207,126.13596,127.94311,127.74984]},
+ {"t":9.52036, "x":7.64573, "y":1.99442, "heading":-2.70315, "vx":-1.90964, "vy":-0.59203, "omega":-2.47452, "ax":-2.09606, "ay":9.14179, "alpha":-1.34472, "fx":[-34.57495,-32.9784,-24.60298,-26.68804], "fy":[128.59238,128.62976,130.57842,130.53045]},
+ {"t":9.55248, "x":7.58332, "y":1.98012, "heading":-2.78332, "vx":-1.97696, "vy":-0.29843, "omega":-2.51771, "ax":-0.67416, "ay":7.57345, "alpha":-4.32701, "fx":[-22.99534,-18.0929,5.47007,-2.60608], "fy":[108.08734,103.16772,106.55037,111.60194]},
+ {"t":9.58459, "x":7.51948, "y":1.97444, "heading":-2.86641, "vx":-1.99861, "vy":-0.0552, "omega":-2.65668, "ax":-0.02372, "ay":1.63615, "alpha":-6.97458, "fx":[-14.72717,-8.86213,14.6246,7.62], "fy":[31.0983,9.13732,15.5893,36.94298]},
+ {"t":9.61671, "x":7.45528, "y":1.97351, "heading":-2.95533, "vx":-1.99937, "vy":-0.00265, "omega":-2.88067, "ax":-0.00002, "ay":0.145, "alpha":-2.91746, "fx":[-5.65358,-3.8686,5.66227,3.85866], "fy":[5.91903,-3.60199,-1.80483,7.70892]},
+ {"t":9.64883, "x":7.39107, "y":1.9735, "heading":-3.04935, "vx":-1.99937, "vy":0.002, "omega":-2.97437, "ax":0.00001, "ay":0.01246, "alpha":-1.01847, "fx":[-1.83916,-1.52777,1.83967,1.52808], "fy":[1.70452,-1.66283,-1.35132,2.01599]},
+ {"t":9.68094, "x":7.32685, "y":1.97357, "heading":3.13778, "vx":-1.99937, "vy":0.0024, "omega":-3.00708, "ax":0.0, "ay":0.00142, "alpha":-0.06864, "fx":[-0.11349,-0.11436,0.11355,0.11442], "fy":[0.13452,-0.09339,-0.09426,0.13365]},
+ {"t":9.71306, "x":7.26264, "y":1.97365, "heading":3.04117, "vx":-1.99937, "vy":0.00245, "omega":-3.00928, "ax":0.0, "ay":-0.00008, "alpha":0.47242, "fx":[0.70173,0.85899,-0.70172,-0.85898], "fy":[-0.86018,0.70053,0.85779,-0.70292]},
+ {"t":9.74518, "x":7.19843, "y":1.97373, "heading":2.94477, "vx":-1.99937, "vy":0.00245, "omega":-2.99411, "ax":0.0, "ay":-0.00113, "alpha":0.75066, "fx":[0.97845,1.46587,-0.97848,-1.46589], "fy":[-1.48188,0.96247,1.44989,-0.99447]},
+ {"t":9.77729, "x":7.13422, "y":1.97381, "heading":2.849, "vx":-1.99937, "vy":0.00241, "omega":-2.97, "ax":0.0, "ay":-0.00213, "alpha":0.83145, "fx":[0.92351,1.71982,-0.92358,-1.71987], "fy":[-1.75002,0.89336,1.68966,-0.95373]},
+ {"t":9.80941, "x":7.07001, "y":1.97389, "heading":2.75404, "vx":-1.99937, "vy":0.00234, "omega":-2.9433, "ax":0.0, "ay":-0.00295, "alpha":0.7786, "fx":[0.70819,1.68522,-0.70829,-1.6853], "fy":[-1.72702,0.66647,1.6435,-0.75001]},
+ {"t":9.84152, "x":7.00579, "y":1.97396, "heading":2.65991, "vx":-1.99937, "vy":0.00225, "omega":-2.9183, "ax":0.0, "ay":-0.00347, "alpha":0.66254, "fx":[0.46517,1.48431,-0.46528,-1.48441], "fy":[-1.53358,0.416,1.43515,-0.51445]},
+ {"t":9.87364, "x":6.94158, "y":1.97403, "heading":2.56653, "vx":-1.99937, "vy":0.00214, "omega":-2.89702, "ax":0.0, "ay":-0.00298, "alpha":0.58496, "fx":[0.2867,1.3431,-0.2868,-1.34319], "fy":[-1.38542,0.24447,1.30087,-0.32904]},
+ {"t":9.90576, "x":6.87737, "y":1.9741, "heading":2.47379, "vx":-1.99937, "vy":0.00204, "omega":-2.87823, "ax":0.00001, "ay":0.00721, "alpha":0.78943, "fx":[0.21756,1.84076,-0.21736,-1.84058], "fy":[-1.73849,0.31967,1.94284,-0.11525]},
+ {"t":9.93787, "x":6.81316, "y":1.97417, "heading":2.38176, "vx":-1.99937, "vy":0.00227, "omega":-2.85288, "ax":0.00028, "ay":0.12926, "alpha":2.00863, "fx":[0.12464,4.71908,-0.11652,-4.71141], "fy":[-2.88354,1.95481,6.54368,1.71375]},
+ {"t":9.96999, "x":6.74894, "y":1.97431, "heading":2.29117, "vx":-1.99936, "vy":0.00642, "omega":-2.78837, "ax":0.02301, "ay":1.50301, "alpha":5.90882, "fx":[-0.61205,14.51153,1.2152,-13.80999], "fy":[7.58148,20.61222,34.63487,22.39093]},
+ {"t":10.00211, "x":6.68474, "y":1.97529, "heading":2.20467, "vx":-1.99863, "vy":0.05469, "omega":-2.5986, "ax":0.64724, "ay":7.40631, "alpha":4.40354, "fx":[7.62439,25.44259,10.5178,-6.88707], "fy":[100.89824,103.09117,109.44112,106.49999]},
+ {"t":10.03422, "x":6.62089, "y":1.98086, "heading":2.12348, "vx":-1.97784, "vy":0.29256, "omega":-2.45717, "ax":2.06349, "ay":9.13052, "alpha":1.37756, "fx":[29.13533,35.27308,29.38565,23.20406], "fy":[129.22487,127.9945,129.74422,130.72836]},
+ {"t":10.06634, "x":6.55843, "y":1.99497, "heading":2.04528, "vx":-1.91157, "vy":0.58579, "omega":-2.41293, "ax":3.52984, "ay":8.96383, "alpha":0.76074, "fx":[50.23585,53.28458,49.85949,46.75841], "fy":[126.93992,125.76179,127.21588,128.32319]},
+ {"t":10.09845, "x":6.49886, "y":2.0184, "heading":1.96818, "vx":-1.7982, "vy":0.87368, "omega":-2.3885, "ax":4.91929, "ay":8.37609, "alpha":0.52396, "fx":[70.02918,71.80259,69.45086,67.63617], "fy":[118.54039,117.50364,118.9291,119.94328]},
+ {"t":10.13057, "x":6.44365, "y":2.05078, "heading":1.89174, "vx":-1.64021, "vy":1.14269, "omega":-2.37167, "ax":6.20456, "ay":7.51733, "alpha":0.403, "fx":[88.27454,89.35662,87.6376,86.52391], "fy":[106.28154,105.38951,106.83281,107.72149]},
+ {"t":10.16269, "x":6.39417, "y":2.09136, "heading":1.81577, "vx":-1.44095, "vy":1.38412, "omega":-2.35873, "ax":7.50675, "ay":6.23661, "alpha":0.43996, "fx":[106.84588,107.63874,105.98818,105.15284], "fy":[87.87015,86.91442,88.92755,89.89747]},
+ {"t":10.20979, "x":6.33463, "y":2.16347, "heading":1.70516, "vx":-1.08737, "vy":1.67787, "omega":-2.33801, "ax":8.69375, "ay":4.31511, "alpha":0.75858, "fx":[123.92769,124.62408,122.59164,121.78419], "fy":[59.72528,58.36597,62.55405,64.01713]},
+ {"t":10.25689, "x":6.29305, "y":2.24729, "heading":1.59588, "vx":-0.67789, "vy":1.88111, "omega":-2.30228, "ax":9.1185, "ay":2.18613, "alpha":2.27595, "fx":[130.33046,131.35487,128.4697,126.85504], "fy":[25.15933,23.02765,36.18286,39.58164]},
+ {"t":10.30399, "x":6.27124, "y":2.33831, "heading":1.48997, "vx":-0.2484, "vy":1.98408, "omega":-2.19508, "ax":3.32823, "ay":0.28435, "alpha":19.81137, "fx":[15.64994,74.87004,77.15016,21.03749], "fy":[-35.1194,-30.32808,32.12717,49.4429]},
+ {"t":10.35109, "x":6.26323, "y":2.43208, "heading":1.40855, "vx":-0.09163, "vy":1.99747, "omega":-1.26194, "ax":0.14464, "ay":0.00639, "alpha":15.24536, "fx":[-27.05848,22.98386,31.06068,-18.78539], "fy":[-20.93061,-28.85491,20.85751,29.2903]},
+ {"t":10.39819, "x":6.25908, "y":2.52617, "heading":1.36603, "vx":-0.08482, "vy":1.99778, "omega":-0.54387, "ax":0.00465, "ay":0.0002, "alpha":9.1122, "fx":[-17.82283,11.80228,17.9532,-11.66918], "fy":[-11.73435,-17.88382,11.73712,17.89222]},
+ {"t":10.44529, "x":6.25509, "y":2.62027, "heading":1.35052, "vx":-0.0846, "vy":1.99778, "omega":-0.11468, "ax":-0.00015, "ay":-0.00001, "alpha":5.28771, "fx":[-10.48667,6.64622,10.48251,-6.65042], "fy":[-6.6484,-10.4847,6.64824,10.48449]},
+ {"t":10.49239, "x":6.2511, "y":2.71437, "heading":1.35098, "vx":-0.08461, "vy":1.99778, "omega":0.13438, "ax":-0.0003, "ay":-0.00001, "alpha":3.02704, "fx":[-6.0045,3.80453,5.99612,-3.81292], "fy":[-3.8089,-6.0005,3.80856,6.00012]},
+ {"t":10.53949, "x":6.24711, "y":2.80846, "heading":1.36067, "vx":-0.08462, "vy":1.99778, "omega":0.27696, "ax":-0.0003, "ay":-0.00001, "alpha":1.72184, "fx":[-3.39617,2.19522,3.38773,-2.20366], "fy":[-2.19962,-3.39213,2.19926,3.39176]},
+ {"t":10.5866, "x":6.24313, "y":2.90256, "heading":1.37562, "vx":-0.08464, "vy":1.99778, "omega":0.35806, "ax":-0.0003, "ay":-0.00001, "alpha":0.97616, "fx":[-1.90832,1.27135,1.89993,-1.27974], "fy":[-1.27572,-1.90431,1.27536,1.90395]},
+ {"t":10.6337, "x":6.23914, "y":2.99666, "heading":1.39357, "vx":-0.08465, "vy":1.99778, "omega":0.40404, "ax":-0.00029, "ay":-0.00001, "alpha":0.55212, "fx":[-1.06803,0.7365,1.0597,-0.74483], "fy":[-0.74084,-1.06404,0.74049,1.06368]},
+ {"t":10.6808, "x":6.23515, "y":3.09076, "heading":1.41321, "vx":-0.08466, "vy":1.99778, "omega":0.43004, "ax":-0.00029, "ay":-0.00001, "alpha":0.31217, "fx":[-0.5973,0.42636,0.58902,-0.43464], "fy":[-0.43068,-0.59334,0.43032,0.59298]},
+ {"t":10.7279, "x":6.23117, "y":3.18485, "heading":1.43382, "vx":-0.08468, "vy":1.99778, "omega":0.44475, "ax":-0.00029, "ay":-0.00001, "alpha":0.17606, "fx":[-0.33358,0.24553,0.32535,-0.25375], "fy":[-0.24982,-0.32964,0.24946,0.32929]},
+ {"t":10.775, "x":6.22718, "y":3.27895, "heading":1.45496, "vx":-0.08469, "vy":1.99778, "omega":0.45304, "ax":-0.00029, "ay":-0.00001, "alpha":0.09965, "fx":[-0.18754,0.14112,0.17937,-0.1493], "fy":[-0.14539,-0.18363,0.14504,0.18328]},
+ {"t":10.8221, "x":6.22319, "y":3.37305, "heading":1.47641, "vx":-0.0847, "vy":1.99778, "omega":0.45773, "ax":-0.00029, "ay":-0.00001, "alpha":0.05658, "fx":[-0.10644,0.0806,0.09831,-0.08873], "fy":[-0.08484,-0.10255,0.08449,0.1022]},
+ {"t":10.8692, "x":6.2192, "y":3.46715, "heading":1.49803, "vx":-0.08472, "vy":1.99778, "omega":0.4604, "ax":-0.00028, "ay":-0.00001, "alpha":0.03174, "fx":[-0.06043,0.04469,0.05235,-0.05277], "fy":[-0.0489,-0.05656,0.04855,0.05622]},
+ {"t":10.9163, "x":6.21521, "y":3.56124, "heading":1.51975, "vx":-0.08473, "vy":1.99778, "omega":0.46189, "ax":-0.00028, "ay":-0.00001, "alpha":0.01772, "fx":[-0.03489,0.02386,0.02686,-0.03189], "fy":[-0.02805,-0.03105,0.0277,0.0307]},
+ {"t":10.9634, "x":6.21122, "y":3.65534, "heading":1.54153, "vx":-0.08475, "vy":1.99778, "omega":0.46273, "ax":-0.00028, "ay":-0.00001, "alpha":0.00988, "fx":[-0.02086,0.01191,0.01287,-0.0199], "fy":[-0.01608,-0.01704,0.01574,0.0167]},
+ {"t":11.0105, "x":6.20722, "y":3.74944, "heading":1.56333, "vx":-0.08476, "vy":1.99778, "omega":0.46319, "ax":-0.00028, "ay":-0.00001, "alpha":0.00533, "fx":[-0.01288,0.0048,0.00493,-0.01275], "fy":[-0.00894,-0.00908,0.00861,0.00874]},
+ {"t":11.05761, "x":6.20323, "y":3.84354, "heading":1.58515, "vx":-0.08477, "vy":1.99778, "omega":0.46344, "ax":-0.00028, "ay":-0.00001, "alpha":0.00305, "fx":[-0.00893,0.00118,0.00103,-0.00908], "fy":[-0.0053,-0.00515,0.00496,0.00482]},
+ {"t":11.10471, "x":6.19924, "y":3.93763, "heading":1.60699, "vx":-0.08478, "vy":1.99778, "omega":0.46359, "ax":-0.00028, "ay":-0.00001, "alpha":0.00241, "fx":[-0.00779,0.00022,-0.00007,-0.00808], "fy":[-0.00432,-0.00403,0.00398,0.00369]},
+ {"t":11.15181, "x":6.19525, "y":4.03173, "heading":1.62882, "vx":-0.0848, "vy":1.99778, "omega":0.4637, "ax":-0.00028, "ay":-0.00001, "alpha":0.0014, "fx":[-0.00609,-0.00145,-0.00172,-0.00636], "fy":[-0.00262,-0.00235,0.00229,0.00202]},
+ {"t":11.19891, "x":6.19125, "y":4.12583, "heading":1.65067, "vx":-0.08481, "vy":1.99778, "omega":0.46377, "ax":-0.00027, "ay":-0.00001, "alpha":0.00116, "fx":[-0.00565,-0.00182,-0.00212,-0.00595], "fy":[-0.00223,-0.00193,0.0019,0.0016]},
+ {"t":11.24601, "x":6.18726, "y":4.21993, "heading":1.67251, "vx":-0.08482, "vy":1.99778, "omega":0.46382, "ax":-0.00027, "ay":-0.00001, "alpha":0.00077, "fx":[-0.00501,-0.00246,-0.00272,-0.00527], "fy":[-0.00157,-0.00131,0.00124,0.00098]},
+ {"t":11.29311, "x":6.18326, "y":4.31402, "heading":1.69436, "vx":-0.08484, "vy":1.99777, "omega":0.46386, "ax":-0.00027, "ay":-0.00001, "alpha":0.00089, "fx":[-0.00513,-0.00219,-0.00255,-0.0055], "fy":[-0.00182,-0.00145,0.00149,0.00113]},
+ {"t":11.34021, "x":6.17926, "y":4.40812, "heading":1.71621, "vx":-0.08485, "vy":1.99777, "omega":0.4639, "ax":-0.00027, "ay":-0.00001, "alpha":0.0008, "fx":[-0.00493,-0.00232,-0.0027,-0.00532], "fy":[-0.00166,-0.00128,0.00134,0.00095]},
+ {"t":11.38731, "x":6.17527, "y":4.50222, "heading":1.73806, "vx":-0.08486, "vy":1.99777, "omega":0.46394, "ax":-0.00027, "ay":-0.00001, "alpha":0.00109, "fx":[-0.00528,-0.00171,-0.00231,-0.00588], "fy":[-0.00225,-0.00165,0.00193,0.00133]},
+ {"t":11.43441, "x":6.17127, "y":4.59631, "heading":1.75991, "vx":-0.08487, "vy":1.99777, "omega":0.46399, "ax":-0.00027, "ay":-0.00001, "alpha":0.00029, "fx":[-0.00416,-0.0032,-0.00339,-0.00434], "fy":[-0.00073,-0.00054,0.00041,0.00023]},
+ {"t":11.48151, "x":6.16727, "y":4.69041, "heading":1.78177, "vx":-0.08489, "vy":1.99777, "omega":0.464, "ax":-0.00026, "ay":-0.00001, "alpha":0.00244, "fx":[-0.00686,0.00107,-0.00063,-0.00856], "fy":[-0.00497,-0.00327,0.00466,0.00296]},
+ {"t":11.52862, "x":6.16327, "y":4.78451, "heading":1.80362, "vx":-0.0849, "vy":1.99777, "omega":0.46412, "ax":-0.00026, "ay":-0.00001, "alpha":0.00027, "fx":[-0.00406,-0.00318,-0.00339,-0.00427], "fy":[-0.0007,-0.00049,0.00039,0.00018]},
+ {"t":11.57572, "x":6.15927, "y":4.87861, "heading":1.82549, "vx":-0.08491, "vy":1.99777, "omega":0.46413, "ax":-0.00026, "ay":-0.00001, "alpha":0.00071, "fx":[-0.00454,-0.00225,-0.00285,-0.00514], "fy":[-0.0016,-0.001,0.00129,0.00069]},
+ {"t":11.62282, "x":6.15527, "y":4.9727, "heading":1.84735, "vx":-0.08492, "vy":1.99777, "omega":0.46416, "ax":-0.00026, "ay":-0.00001, "alpha":-0.00016, "fx":[-0.00349,-0.00399,-0.00385,-0.00335], "fy":[0.00017,0.00003,-0.00047,-0.00033]},
+ {"t":11.66992, "x":6.15127, "y":5.0668, "heading":1.86921, "vx":-0.08494, "vy":1.99777, "omega":0.46415, "ax":-0.00026, "ay":-0.00001, "alpha":0.003, "fx":[-0.00693,0.00258,-0.00035,-0.00986], "fy":[-0.00637,-0.00344,0.00607,0.00314]},
+ {"t":11.71702, "x":6.14727, "y":5.1609, "heading":1.89107, "vx":-0.08495, "vy":1.99777, "omega":0.4643, "ax":-0.00025, "ay":-0.00001, "alpha":-0.00207, "fx":[-0.00143,-0.00795,-0.00578,0.00073], "fy":[0.00419,0.00202,-0.00449,-0.00233]},
+ {"t":11.76412, "x":6.14327, "y":5.25499, "heading":1.91294, "vx":-0.08496, "vy":1.99777, "omega":0.4642, "ax":-0.00025, "ay":-0.00001, "alpha":0.0, "fx":[-0.00358,-0.00357,-0.00357,-0.00358], "fy":[-0.00015,-0.00015,-0.00015,-0.00015]},
+ {"t":11.81122, "x":6.13927, "y":5.34909, "heading":1.93481, "vx":-0.08497, "vy":1.99777, "omega":0.4642, "ax":-0.00025, "ay":-0.00001, "alpha":-0.00616, "fx":[0.00237,-0.01673,-0.00945,0.00965], "fy":[0.01304,0.00576,-0.01334,-0.00606]},
+ {"t":11.85832, "x":6.13527, "y":5.44319, "heading":1.95666, "vx":-0.08498, "vy":1.99777, "omega":0.46391, "ax":-0.00025, "ay":-0.00001, "alpha":-0.00933, "fx":[0.00502,-0.02368,-0.01203,0.01667], "fy":[0.02003,0.00837,-0.02033,-0.00867]},
+ {"t":11.90542, "x":6.13126, "y":5.53728, "heading":1.9785, "vx":-0.085, "vy":1.99777, "omega":0.46347, "ax":-0.00024, "ay":-0.00001, "alpha":-0.02227, "fx":[0.01582,-0.05208,-0.02275,0.04515], "fy":[0.04847,0.01914,-0.04876,-0.01943]},
+ {"t":11.95252, "x":6.12726, "y":5.63138, "heading":2.00031, "vx":-0.08501, "vy":1.99777, "omega":0.46242, "ax":-0.00024, "ay":-0.00001, "alpha":-0.04142, "fx":[0.03046,-0.09457,-0.0373,0.08772], "fy":[0.091,0.03374,-0.09129,-0.03402]},
+ {"t":11.99963, "x":6.12326, "y":5.72548, "heading":2.02204, "vx":-0.08502, "vy":1.99777, "omega":0.46047, "ax":-0.00024, "ay":-0.00001, "alpha":-0.07688, "fx":[0.05582,-0.17389,-0.06257,0.16713], "fy":[0.17037,0.05906,-0.17065,-0.05934]},
+ {"t":12.04673, "x":6.11925, "y":5.81957, "heading":2.04365, "vx":-0.08503, "vy":1.99777, "omega":0.45685, "ax":-0.00023, "ay":-0.00001, "alpha":-0.13953, "fx":[0.0974,-0.31505,-0.10406,0.30839], "fy":[0.31158,0.10059,-0.31186,-0.10087]},
+ {"t":12.09383, "x":6.11525, "y":5.91367, "heading":2.06501, "vx":-0.08504, "vy":1.99777, "omega":0.45028, "ax":-0.00023, "ay":-0.00001, "alpha":-0.25114, "fx":[0.16601,-0.56809,-0.17255,0.56155], "fy":[0.56469,0.16914,-0.56496,-0.16941]},
+ {"t":12.14093, "x":6.11124, "y":6.00777, "heading":2.08594, "vx":-0.08505, "vy":1.99777, "omega":0.43845, "ax":-0.00023, "ay":-0.00001, "alpha":-0.44964, "fx":[0.27863,-1.02058,-0.28505,1.01416], "fy":[1.01724,0.28171,-1.0175,-0.28198]},
+ {"t":12.18803, "x":6.10723, "y":6.10186, "heading":2.10609, "vx":-0.08506, "vy":1.99777, "omega":0.41727, "ax":-0.00022, "ay":-0.00001, "alpha":-0.80052, "fx":[0.46204,-1.82417,-0.46833,1.81789], "fy":[1.8209,0.46505,-1.82116,-0.46531]},
+ {"t":12.23513, "x":6.10323, "y":6.19596, "heading":2.12486, "vx":-0.08507, "vy":1.99776, "omega":0.37956, "ax":-0.00022, "ay":-0.00001, "alpha":-1.41976, "fx":[0.76121,-3.24767,-0.76735,3.24154], "fy":[3.24448,0.76415,-3.24473,-0.76441]},
+ {"t":12.28223, "x":6.09922, "y":6.29006, "heading":2.14116, "vx":-0.08508, "vy":1.99776, "omega":0.31269, "ax":-0.00021, "ay":-0.00001, "alpha":-2.50756, "fx":[1.25327,-5.75476,-1.25924,5.74882], "fy":[5.75167,1.25613,-5.75191,-1.25638]},
+ {"t":12.32933, "x":6.09521, "y":6.38416, "heading":2.15311, "vx":-0.08509, "vy":1.99776, "omega":0.19458, "ax":-0.00019, "ay":-0.00001, "alpha":-4.40142, "fx":[2.08152,-10.12426,-2.08706,10.11878], "fy":[10.12141,2.08417,-10.12162,-2.0844]},
+ {"t":12.37643, "x":6.0912, "y":6.47825, "heading":2.15739, "vx":-0.0851, "vy":1.99776, "omega":-0.01273, "ax":0.0001, "ay":0.0, "alpha":-7.63856, "fx":[3.54341,-17.57958,-3.54052,17.58238], "fy":[17.58104,3.54204,-17.58092,-3.54189]},
+ {"t":12.42353, "x":6.0872, "y":6.57235, "heading":2.14832, "vx":-0.0851, "vy":1.99776, "omega":-0.37251, "ax":0.00881, "ay":0.00037, "alpha":-12.94916, "fx":[6.40483,-29.6287,-6.14439,29.86767], "fy":[29.75089,6.28258,-29.74557,-6.26667]},
+ {"t":12.47064, "x":6.0832, "y":6.66645, "heading":2.11641, "vx":-0.08468, "vy":1.99778, "omega":-0.98243, "ax":0.23427, "ay":0.00928, "alpha":-20.9627, "fx":[15.35877,-44.83112,-7.9938,50.74907], "fy":[47.75273,12.04014,-47.90391,-11.36293]},
+ {"t":12.51774, "x":6.07947, "y":6.76055, "heading":2.04688, "vx":-0.07365, "vy":1.99822, "omega":-1.96979, "ax":4.26243, "ay":-0.05724, "alpha":-23.25612, "fx":[78.36709,11.90517,51.44689,99.95663], "fy":[52.37137,27.28834,-66.20678,-16.69837]},
+ {"t":12.56484, "x":6.08073, "y":6.85461, "heading":1.92831, "vx":0.12712, "vy":1.99552, "omega":-3.06518, "ax":9.30605, "ay":-1.64795, "alpha":-3.52999, "fx":[134.34291,132.54999,128.58204,132.16916], "fy":[-8.51097,-18.88218,-38.96099,-27.08281]},
+ {"t":12.61194, "x":6.09704, "y":6.94677, "heading":1.78002, "vx":0.56544, "vy":1.9179, "omega":-3.23145, "ax":8.9366, "ay":-3.79557, "alpha":-1.78867, "fx":[129.75575,127.31741,123.31341,126.31004], "fy":[-46.29475,-52.40269,-61.37515,-55.13258]},
+ {"t":12.65904, "x":6.13358, "y":7.0329, "heading":1.62583, "vx":0.98636, "vy":1.73913, "omega":-3.3157, "ax":7.82424, "ay":-5.82891, "alpha":-1.49643, "fx":[114.98677,111.35104,106.6507,110.63841], "fy":[-77.00922,-82.12676,-88.18555,-83.17229]},
+ {"t":12.70614, "x":6.18872, "y":7.10834, "heading":1.468, "vx":1.35489, "vy":1.46458, "omega":-3.38618, "ax":4.98023, "ay":-8.40462, "alpha":-1.89808, "fx":[78.15784,69.44931,63.03393,71.73324], "fy":[-114.48427,-119.94674,-123.46471,-118.63838]},
+ {"t":12.75324, "x":6.25806, "y":7.16801, "heading":1.3064, "vx":1.58947, "vy":1.06871, "omega":-3.47558, "ax":3.47541, "ay":-9.13825, "alpha":-1.86191, "fx":[57.3509,47.93778,41.24226,50.52172], "fy":[-126.30972,-130.16202,-132.45146,-129.20665]},
+ {"t":12.80034, "x":6.33678, "y":7.20821, "heading":1.14063, "vx":1.75316, "vy":0.63829, "omega":-3.56328, "ax":2.93285, "ay":-9.33724, "alpha":0.57208, "fx":[39.01478,41.70506,44.12744,41.44253], "fy":[-133.14324,-132.3284,-131.53688,-132.40393]},
+ {"t":12.84826, "x":6.42416, "y":7.22807, "heading":0.97054, "vx":1.8937, "vy":0.19088, "omega":-3.53587, "ax":2.84204, "ay":-9.32308, "alpha":3.73856, "fx":[23.23,38.79,56.55677,42.56445], "fy":[-136.7528,-133.20229,-126.66418,-131.99075]},
+ {"t":12.89618, "x":6.51816, "y":7.22652, "heading":0.80541, "vx":2.02988, "vy":-0.25586, "omega":-3.35672, "ax":2.82484, "ay":-9.25266, "alpha":6.23068, "fx":[11.43363,34.0296,64.8169,49.88558], "fy":[-138.19514,-134.47759,-122.61539,-129.32872]},
+ {"t":12.94409, "x":6.61867, "y":7.20363, "heading":0.65171, "vx":2.16524, "vy":-0.69922, "omega":-3.05817, "ax":2.87459, "ay":-9.15436, "alpha":8.16066, "fx":[4.41247,29.03039,68.85793,60.68567], "fy":[-138.5324,-135.61245,-120.35789,-124.54075]},
+ {"t":12.99201, "x":6.72572, "y":7.15962, "heading":0.51454, "vx":2.30298, "vy":-1.13788, "omega":-2.66713, "ax":2.95506, "ay":-9.05892, "alpha":9.50688, "fx":[2.14716,24.78521,69.8827,70.73427], "fy":[-138.49287,-136.40221,-119.7154,-119.02141]},
+ {"t":13.03993, "x":6.83947, "y":7.09469, "heading":0.39766, "vx":2.44458, "vy":-1.57195, "omega":-2.21158, "ax":3.02024, "ay":-8.98472, "alpha":10.36735, "fx":[2.56516,21.25906,69.38576,78.03453], "fy":[-138.34751,-136.91899,-119.92585,-114.23248]},
+ {"t":13.08785, "x":6.96007, "y":7.00906, "heading":0.30358, "vx":2.5893, "vy":-2.00248, "omega":-1.71481, "ax":3.03278, "ay":-8.9389, "alpha":10.88675, "fx":[3.50276,18.02097,68.0329,82.39896], "fy":[-138.10409,-137.25515,-120.5662,-110.90177]},
+ {"t":13.13576, "x":7.08763, "y":6.90284, "heading":0.23391, "vx":2.73462, "vy":-2.43081, "omega":-1.19314, "ax":2.92043, "ay":-8.9363, "alpha":11.16424, "fx":[2.63829,14.22816,65.46265,83.25628], "fy":[-137.7023,-137.45189,-121.71748,-109.80829]},
+ {"t":13.18368, "x":7.22202, "y":6.7761, "heading":0.18956, "vx":2.87456, "vy":-2.85901, "omega":-0.65818, "ax":2.34248, "ay":-9.04551, "alpha":11.10066, "fx":[-6.30845,6.21856,57.86039,75.04569], "fy":[-136.46974,-137.35298,-124.75695,-114.29203]},
+ {"t":13.2316, "x":7.36245, "y":6.62872, "heading":0.17076, "vx":2.98681, "vy":-3.29245, "omega":-0.12627, "ax":-5.68357, "ay":-7.08861, "alpha":2.54292, "fx":[-89.09316,-80.70265,-71.71544,-80.74197], "fy":[-93.47113,-101.66337,-107.26471,-99.51795]},
+ {"t":13.27952, "x":7.49904, "y":6.46282, "heading":0.16763, "vx":2.71447, "vy":-3.63211, "omega":-0.00442, "ax":-5.38759, "ay":-3.74814, "alpha":0.0045, "fx":[-76.37769,-76.3694,-76.35802,-76.36631], "fy":[-53.11825,-53.13631,-53.13962,-53.12156]},
+ {"t":13.32743, "x":7.62293, "y":6.28448, "heading":0.16743, "vx":2.45631, "vy":-3.81171, "omega":-0.00421, "ax":-1.20597, "ay":-0.76428, "alpha":-0.00044, "fx":[-17.09344,-17.09371,-17.09514,-17.09487], "fy":[-10.83406,-10.8326,-10.83282,-10.83428]},
+ {"t":13.37535, "x":7.73924, "y":6.10095, "heading":0.16722, "vx":2.39852, "vy":-3.84834, "omega":-0.00423, "ax":-0.16107, "ay":-0.10013, "alpha":-0.00009, "fx":[-2.28295,-2.28299,-2.28327,-2.28323], "fy":[-1.41944,-1.41916,-1.41921,-1.41949]},
+ {"t":13.42327, "x":7.85399, "y":5.91643, "heading":0.16702, "vx":2.3908, "vy":-3.85313, "omega":-0.00423, "ax":-0.13445, "ay":-0.08184, "alpha":-0.00167, "fx":[-1.9026,-1.90352,-1.909,-1.90808], "fy":[-1.16229,-1.15682,-1.15774,-1.16322]},
+ {"t":13.47119, "x":7.96839, "y":5.73171, "heading":0.16682, "vx":2.38436, "vy":-3.85706, "omega":-0.00431, "ax":-2.71106, "ay":2.54498, "alpha":-5.20741, "fx":[-29.28915,-30.25005,-47.04787,-47.12759], "fy":[30.26772,47.19478,41.73511,25.10048]},
+ {"t":13.5191, "x":8.07953, "y":5.54981, "heading":0.16063, "vx":2.25445, "vy":-3.73511, "omega":-0.25384, "ax":-4.77373, "ay":7.84982, "alpha":-13.02045, "fx":[-39.10116,-30.94993,-85.63372,-114.9809], "fy":[129.8968,133.71136,107.52193,73.947]},
+ {"t":13.56702, "x":8.18208, "y":5.37985, "heading":0.13352, "vx":2.02571, "vy":-3.35896, "omega":-0.87774, "ax":-4.70724, "ay":8.01168, "alpha":-12.54507, "fx":[-39.88263,-29.73486,-83.70181,-113.5768], "fy":[131.39293,134.83878,109.95011,78.07292]},
+ {"t":13.61494, "x":8.27374, "y":5.22809, "heading":0.07706, "vx":1.80015, "vy":-2.97507, "omega":-1.47887, "ax":-4.6458, "ay":8.10587, "alpha":-12.10225, "fx":[-42.61569,-27.58839,-80.98425,-112.22434], "fy":[131.09273,135.57288,112.2691,80.66052]},
+ {"t":13.66285, "x":8.35467, "y":5.09484, "heading":-0.0077, "vx":1.57754, "vy":-2.58665, "omega":-2.05878, "ax":-4.59965, "ay":8.14781, "alpha":-11.98795, "fx":[-47.00583,-23.77723,-77.86625,-112.14651], "fy":[129.83725,136.42835,114.60485,81.10289]},
+ {"t":13.71077, "x":8.42498, "y":4.98025, "heading":-0.12011, "vx":1.35713, "vy":-2.19623, "omega":-2.63321, "ax":-4.59007, "ay":8.12622, "alpha":-12.30168, "fx":[-54.70207,-17.74846,-74.26579,-113.53618], "fy":[126.90817,137.42006,117.06174,79.35874]},
+ {"t":13.75869, "x":8.48474, "y":4.88434, "heading":-0.26041, "vx":1.13719, "vy":-1.80685, "omega":-3.22267, "ax":-4.87461, "ay":8.33755, "alpha":-6.48851, "fx":[-70.04219,-41.98726,-69.88906,-94.46717], "fy":[119.48094,132.0855,119.74679,101.41802]},
+ {"t":13.80661, "x":8.53364, "y":4.80733, "heading":-0.42228, "vx":0.90361, "vy":-1.40733, "omega":-3.53358, "ax":-4.93704, "ay":8.32403, "alpha":6.08144, "fx":[-67.15161,-92.57542,-75.93785,-44.26047], "fy":[121.36612,103.25055,115.94304,131.40485]},
+ {"t":13.85452, "x":8.57127, "y":4.74945, "heading":-0.58462, "vx":0.66704, "vy":-1.00847, "omega":-3.24218, "ax":-4.85052, "ay":7.8169, "alpha":14.50012, "fx":[-57.68984,-109.93273,-102.01376,-5.38342], "fy":[126.21341,84.6519,93.75186,138.59385]},
+ {"t":13.90244, "x":8.59766, "y":4.7101, "heading":-0.72333, "vx":0.43462, "vy":-0.6339, "omega":-2.54737, "ax":-4.71139, "ay":6.98305, "alpha":23.06073, "fx":[-47.49856,-114.43137,-129.11334,23.9121], "fy":[130.42639,78.54157,50.31793,136.64631]},
+ {"t":13.95036, "x":8.61308, "y":4.68774, "heading":-0.81892, "vx":0.20886, "vy":-0.29929, "omega":-1.44236, "ax":-4.35878, "ay":6.24604, "alpha":30.101, "fx":[-39.2603,-115.4412,-138.04045,45.60319], "fy":[133.15884,77.0962,12.85689,131.03277]},
+ {"t":13.99828, "x":8.61808, "y":4.68057, "heading":-0.85347, "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,124]
+ },
+ "events":[
+ {"name":"Marker", "from":{"target":0, "targetTimestamp":0.0, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"hoodDown"}}},
+ {"name":"Marker", "from":{"target":1, "targetTimestamp":0.75975, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"hoodUp"}}},
+ {"name":"Marker", "from":{"target":4, "targetTimestamp":3.57985, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"hoodDown"}}},
+ {"name":"Marker", "from":{"target":6, "targetTimestamp":5.494, "offset":{"exp":"-0.2 s", "val":-0.2}}, "event":{"type":"named", "data":{"name":"hoodUp"}}}]
+}
--- /dev/null
+{
+ "name":"superShuttling",
+ "version":3,
+ "snapshot":{
+ "waypoints":[
+ {"x":3.601878881454468, "y":7.661683082580566, "heading":-1.5707963267948966, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":8.518824577331543, "y":6.778850555419922, "heading":-2.067219187672958, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":8.495003700256348, "y":1.8715722560882568, "heading":-2.1521774178641575, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":6.613085746765137, "y":1.38322651386261, "heading":1.8896315430431432, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":5.779325008392334, "y":2.9435503482818604, "heading":1.5707963267948966, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":5.767414093017578, "y":5.9808220863342285, "heading":1.5707963267948966, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":6.898946285247803, "y":6.469167709350586, "heading":-1.403364447950369, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":6.910857200622559, "y":2.657689332962036, "heading":-1.283220421161046, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":7.86372709274292, "y":2.657689332962036, "heading":1.7191242488688685, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":7.875637531280518, "y":6.385791778564453, "heading":1.9055326915376485, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":5.554238796234131, "y":7.590708255767822, "heading":-1.6058694574950845, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":3.601878881454468, "y":7.661683082580566, "heading":-1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true},
+ {"from":4, "to":5, "data":{"type":"KeepInLane", "props":{"tolerance":0.01867918256759085}}, "enabled":true},
+ {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true},
+ {"from":9, "to":11, "data":{"type":"KeepOutCircle", "props":{"x":4.883889600634575, "y":5.9670998603105545, "r":1.0}}, "enabled":true},
+ {"from":1, "to":9, "data":{"type":"MaxVelocity", "props":{"max":2.5}}, "enabled":true}],
+ "targetDt":0.05
+ },
+ "params":{
+ "waypoints":[
+ {"x":{"exp":"3.6018788814544678 m", "val":3.601878881454468}, "y":{"exp":"7.661683082580566 m", "val":7.661683082580566}, "heading":{"exp":"-90 deg", "val":-1.5707963267948966}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"8.518824577331543 m", "val":8.518824577331543}, "y":{"exp":"6.778850555419922 m", "val":6.778850555419922}, "heading":{"exp":"-2.067219187672958 rad", "val":-2.067219187672958}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"8.495003700256348 m", "val":8.495003700256348}, "y":{"exp":"1.8715722560882568 m", "val":1.8715722560882568}, "heading":{"exp":"-2.1521774178641575 rad", "val":-2.1521774178641575}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"6.613085746765137 m", "val":6.613085746765137}, "y":{"exp":"1.3832265138626099 m", "val":1.38322651386261}, "heading":{"exp":"1.8896315430431434 rad", "val":1.8896315430431432}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"5.779325008392334 m", "val":5.779325008392334}, "y":{"exp":"2.9435503482818604 m", "val":2.9435503482818604}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"5.767414093017578 m", "val":5.767414093017578}, "y":{"exp":"5.9808220863342285 m", "val":5.9808220863342285}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"6.898946285247803 m", "val":6.898946285247803}, "y":{"exp":"6.469167709350586 m", "val":6.469167709350586}, "heading":{"exp":"-1.403364447950369 rad", "val":-1.403364447950369}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"6.910857200622559 m", "val":6.910857200622559}, "y":{"exp":"2.657689332962036 m", "val":2.657689332962036}, "heading":{"exp":"-1.283220421161046 rad", "val":-1.283220421161046}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"7.86372709274292 m", "val":7.86372709274292}, "y":{"exp":"2.657689332962036 m", "val":2.657689332962036}, "heading":{"exp":"1.7191242488688683 rad", "val":1.7191242488688685}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"7.875637531280518 m", "val":7.875637531280518}, "y":{"exp":"6.385791778564453 m", "val":6.385791778564453}, "heading":{"exp":"1.9055326915376483 rad", "val":1.9055326915376485}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"5.554238796234131 m", "val":5.554238796234131}, "y":{"exp":"7.590708255767822 m", "val":7.590708255767822}, "heading":{"exp":"-1.6058694574950843 rad", "val":-1.6058694574950845}, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+ {"x":{"exp":"3.6018788814544678 m", "val":3.601878881454468}, "y":{"exp":"7.661683082580566 m", "val":7.661683082580566}, "heading":{"exp":"-90 deg", "val":-1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+ "constraints":[
+ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true},
+ {"from":4, "to":5, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"18.679182567590846 mm", "val":0.01867918256759085}}}, "enabled":true},
+ {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true},
+ {"from":9, "to":11, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"4.883889600634575 m", "val":4.883889600634575}, "y":{"exp":"5.9670998603105545 m", "val":5.9670998603105545}, "r":{"exp":"1 m", "val":1.0}}}, "enabled":true},
+ {"from":1, "to":9, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2.5 m / s", "val":2.5}}}, "enabled":true}],
+ "targetDt":{
+ "exp":"0.05 s",
+ "val":0.05
+ }
+ },
+ "trajectory":{
+ "config":{
+ "frontLeft":{
+ "x":0.52705,
+ "y":0.52705
+ },
+ "backLeft":{
+ "x":-0.52705,
+ "y":0.52705
+ },
+ "mass":56.69904625,
+ "inertia":7.0,
+ "gearing":7.03125,
+ "radius":0.0508,
+ "vmax":628.3185307179587,
+ "tmax":7.0,
+ "cof":1.0,
+ "bumper":{
+ "front":0.4064,
+ "side":0.4064,
+ "back":0.4064
+ },
+ "differentialTrackWidth":0.5588
+ },
+ "sampleType":"Swerve",
+ "waypoints":[0.0,1.51517,3.48043,4.34395,5.06092,6.27815,7.05683,8.60318,9.32403,10.82661,11.82002,12.48424],
+ "samples":[
+ {"t":0.0, "x":3.60188, "y":7.66168, "heading":-1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.78552, "ay":-0.37604, "alpha":-0.00905, "fx":[138.70618,138.70632,138.70862,138.70849], "fy":[-5.36144,-5.35912,-5.29914,-5.30143]},
+ {"t":0.05612, "x":3.61729, "y":7.66109, "heading":-1.57081, "vx":0.54914, "vy":-0.0211, "omega":-0.00051, "ax":9.78773, "ay":-0.27884, "alpha":-0.01423, "fx":[138.73728,138.73744,138.74012,138.73996], "fy":[-4.00105,-3.99833,-3.90403,-3.90668]},
+ {"t":0.11223, "x":3.66351, "y":7.65947, "heading":-1.57086, "vx":1.0984, "vy":-0.03675, "omega":-0.00131, "ax":9.78794, "ay":-0.21547, "alpha":-0.01874, "fx":[138.7402,138.74039,138.74312,138.74294], "fy":[-3.11769,-3.11491,-2.99073,-2.9934]},
+ {"t":0.16835, "x":3.74057, "y":7.65707, "heading":-1.57096, "vx":1.64767, "vy":-0.04884, "omega":-0.00236, "ax":9.78689, "ay":-0.16956, "alpha":-0.02213, "fx":[138.72546,138.72568,138.72822,138.728], "fy":[-2.47809,-2.4755,-2.32886,-2.3313]},
+ {"t":0.22447, "x":3.84844, "y":7.65406, "heading":-1.57113, "vx":2.19689, "vy":-0.05836, "omega":-0.0036, "ax":9.78452, "ay":-0.13348, "alpha":-0.02408, "fx":[138.69207,138.69234,138.69451,138.69425], "fy":[-1.97292,-1.9707,-1.81115,-1.81318]},
+ {"t":0.28059, "x":3.98713, "y":7.65057, "heading":-1.57137, "vx":2.74597, "vy":-0.06585, "omega":-0.00495, "ax":9.78, "ay":-0.10325, "alpha":-0.02378, "fx":[138.62824,138.62857,138.63023,138.6299], "fy":[-1.54317,-1.54151,-1.38403,-1.38551]},
+ {"t":0.3367, "x":4.15663, "y":7.64672, "heading":-1.57169, "vx":3.2948, "vy":-0.07164, "omega":-0.00629, "ax":9.77046, "ay":-0.07601, "alpha":-0.01861, "fx":[138.49322,138.49359,138.49455,138.49418], "fy":[-1.13944,-1.13854,-1.01536,-1.01615]},
+ {"t":0.39282, "x":4.35691, "y":7.64258, "heading":-1.57207, "vx":3.84309, "vy":-0.07591, "omega":-0.00733, "ax":9.74107, "ay":-0.04636, "alpha":0.00365, "fx":[138.07752,138.07738,138.07726,138.07741], "fy":[-0.6451,-0.64518,-0.66927,-0.66918]},
+ {"t":0.44894, "x":4.58791, "y":7.63824, "heading":-1.57247, "vx":4.38974, "vy":-0.07851, "omega":-0.00713, "ax":2.61058, "ay":1.09236, "alpha":0.1269, "fx":[37.19051,36.79385,36.81813,37.21494], "fy":[15.69384,15.71995,15.27364,15.2482]},
+ {"t":0.50506, "x":4.83836, "y":7.63556, "heading":-1.57267, "vx":4.53624, "vy":-0.01721, "omega":0.0, "ax":0.00076, "ay":0.35937, "alpha":-0.00001, "fx":[0.01079,0.01081,0.01081,0.01079], "fy":[5.09397,5.09397,5.09398,5.09398]},
+ {"t":0.56117, "x":5.09292, "y":7.63516, "heading":-1.57267, "vx":4.53628, "vy":0.00296, "omega":0.0, "ax":-0.00072, "ay":0.29211, "alpha":-0.00001, "fx":[-0.01021,-0.01018,-0.01018,-0.01021], "fy":[4.14051,4.14051,4.14053,4.14053]},
+ {"t":0.61729, "x":5.34749, "y":7.63578, "heading":-1.57267, "vx":4.53624, "vy":0.01935, "omega":0.0, "ax":-0.00182, "ay":0.29825, "alpha":-0.00001, "fx":[-0.02585,-0.02582,-0.02582,-0.02585], "fy":[4.22761,4.22761,4.22763,4.22763]},
+ {"t":0.67341, "x":5.60205, "y":7.63734, "heading":-1.57268, "vx":4.53614, "vy":0.03609, "omega":-0.00001, "ax":-0.00322, "ay":0.32319, "alpha":-0.00001, "fx":[-0.04561,-0.04557,-0.04557,-0.04561], "fy":[4.58118,4.58118,4.58122,4.58122]},
+ {"t":0.72953, "x":5.8566, "y":7.63987, "heading":-1.57268, "vx":4.53596, "vy":0.05422, "omega":-0.00001, "ax":-0.0053, "ay":0.3719, "alpha":-0.00001, "fx":[-0.07513,-0.07509,-0.07509,-0.07513], "fy":[5.27161,5.27161,5.27166,5.27166]},
+ {"t":0.78564, "x":6.11114, "y":7.6435, "heading":-1.57268, "vx":4.53566, "vy":0.0751, "omega":-0.00001, "ax":-0.00908, "ay":0.46726, "alpha":-0.00002, "fx":[-0.12877,-0.1287,-0.1287,-0.12877], "fy":[6.6233,6.6233,6.62337,6.62337]},
+ {"t":0.84176, "x":6.36565, "y":7.64845, "heading":-1.57268, "vx":4.53515, "vy":0.10132, "omega":-0.00001, "ax":-0.01548, "ay":0.59534, "alpha":-0.00006, "fx":[-0.21954,-0.21933,-0.21933,-0.21954], "fy":[8.43864,8.43864,8.43885,8.43885]},
+ {"t":0.89788, "x":6.62013, "y":7.65507, "heading":-1.57268, "vx":4.53428, "vy":0.13473, "omega":-0.00001, "ax":0.02149, "ay":-0.89586, "alpha":-0.00161, "fx":[0.30193,0.30731,0.30732,0.30194], "fy":[-12.70128,-12.70128,-12.69599,-12.69599]},
+ {"t":0.954, "x":6.87461, "y":7.66122, "heading":-1.57268, "vx":4.53549, "vy":0.08445, "omega":-0.0001, "ax":-0.32217, "ay":-8.84728, "alpha":-0.00182, "fx":[-4.57195,-4.56096,-4.5613,-4.57229], "fy":[-125.40851,-125.40887,-125.40774,-125.40738]},
+ {"t":1.01011, "x":7.12863, "y":7.65203, "heading":-1.57269, "vx":4.51741, "vy":-0.41204, "omega":-0.0002, "ax":-1.46943, "ay":-9.47187, "alpha":-0.01789, "fx":[-20.87708,-20.76351,-20.78047,-20.89413], "fy":[-134.25548,-134.27269,-134.26749,-134.25025]},
+ {"t":1.06623, "x":7.37982, "y":7.614, "heading":-1.57273, "vx":4.43495, "vy":-0.94357, "omega":-0.00121, "ax":-6.19187, "ay":-7.3539, "alpha":-5.97726, "fx":[-90.40346,-66.18932,-86.11528,-108.36501], "fy":[-104.14046,-120.79987,-107.05165,-84.96724]},
+ {"t":1.12235, "x":7.61895, "y":7.54947, "heading":-1.58221, "vx":4.08748, "vy":-1.35625, "omega":-0.33664, "ax":-8.53247, "ay":-4.30373, "alpha":-8.54153, "fx":[-118.96642,-99.89646,-129.82143,-135.09834], "fy":[-70.88642,-95.69815,-47.2726,-30.16024]},
+ {"t":1.17847, "x":7.83489, "y":7.46658, "heading":-1.61455, "vx":3.60866, "vy":-1.59777, "omega":-0.81597, "ax":-9.13573, "ay":-3.22877, "alpha":-5.57638, "fx":[-127.54733,-119.94869,-134.14267,-136.34854], "fy":[-54.30605,-69.35634,-34.50304,-24.9029]},
+ {"t":1.23458, "x":8.02301, "y":7.37183, "heading":-1.66912, "vx":3.09598, "vy":-1.77896, "omega":-1.1289, "ax":-9.3751, "ay":-2.68548, "alpha":-3.32859, "fx":[-131.68139,-128.43595,-135.01461,-136.42743], "fy":[-43.55009,-52.26735,-31.52606,-24.92084]},
+ {"t":1.2907, "x":8.18199, "y":7.26777, "heading":-1.73771, "vx":2.56988, "vy":-1.92966, "omega":-1.31569, "ax":-9.48393, "ay":-2.37017, "alpha":-2.01034, "fx":[-133.77777,-132.1246,-135.41841,-136.40889], "fy":[-36.76756,-42.27491,-30.06126,-25.28292]},
+ {"t":1.34682, "x":8.31127, "y":7.15575, "heading":-1.81471, "vx":2.03766, "vy":-2.06267, "omega":-1.4285, "ax":-9.5427, "ay":-2.16662, "alpha":-1.15962, "fx":[-134.95007,-134.06245,-135.69224,-136.35731], "fy":[-32.32238,-35.79831,-28.99566,-25.72877]},
+ {"t":1.40294, "x":8.4106, "y":7.03659, "heading":-1.8967, "vx":1.50215, "vy":-2.18425, "omega":-1.49358, "ax":-9.57838, "ay":-2.02502, "alpha":-0.56741, "fx":[-135.65014,-135.22364,-135.91916,-136.29185], "fy":[-29.3424,-31.23612,-28.04641,-26.19152]},
+ {"t":1.45905, "x":8.47981, "y":6.91083, "heading":-1.98141, "vx":0.96464, "vy":-2.29789, "omega":-1.52542, "ax":-9.60187, "ay":-1.9211, "alpha":-0.13236, "fx":[-136.08389,-135.98342,-136.12592,-136.22345], "fy":[-27.33833,-27.8309,-27.12331,-26.63212]},
+ {"t":1.51517, "x":8.51882, "y":6.77885, "heading":-2.06722, "vx":0.4258, "vy":-2.4057, "omega":-1.53285, "ax":-9.57846, "ay":-1.98796, "alpha":0.07064, "fx":[-135.7777,-135.8393,-135.76752,-135.7051], "fy":[-28.15134,-27.85576,-28.2062,-28.50187]},
+ {"t":1.56196, "x":8.52826, "y":6.66411, "heading":-2.13887, "vx":-0.02239, "vy":-2.49872, "omega":-1.52954, "ax":-0.99682, "ay":0.00447, "alpha":17.27848, "fx":[26.22153,-23.81255,-52.35571,-6.57198], "fy":[9.45715,39.48347,-8.29645,-40.39045]},
+ {"t":1.60875, "x":8.52612, "y":6.54719, "heading":-2.19152, "vx":-0.06903, "vy":-2.49851, "omega":-0.72105, "ax":0.428, "ay":-0.01011, "alpha":9.76033, "fx":[28.47891,2.47455,-16.65349,9.96742], "fy":[3.56627,22.55939,-3.96724,-22.7315]},
+ {"t":1.65555, "x":8.52336, "y":6.43027, "heading":-2.21458, "vx":-0.04901, "vy":-2.49898, "omega":-0.26435, "ax":0.22494, "ay":-0.00394, "alpha":5.25568, "fx":[15.37425,1.47195,-9.04532,4.95296], "fy":[1.67961,12.17397,-1.80611,-12.27065]},
+ {"t":1.70234, "x":8.52132, "y":6.31334, "heading":-2.22119, "vx":-0.03848, "vy":-2.49917, "omega":-0.01843, "ax":0.14096, "ay":-0.00198, "alpha":2.7821, "fx":[8.46508,1.12343,-4.47731,2.8813], "fy":[0.85002,6.44697,-0.90873,-6.50072]},
+ {"t":1.74913, "x":8.51967, "y":6.19639, "heading":-2.21901, "vx":-0.03189, "vy":-2.49926, "omega":0.11175, "ax":0.09913, "ay":-0.00117, "alpha":1.46256, "fx":[4.80557,0.9364,-1.99697,1.8755], "fy":[0.45281,3.38559,-0.48652,-3.41833]},
+ {"t":1.79592, "x":8.51829, "y":6.07945, "heading":-2.21218, "vx":-0.02725, "vy":-2.49932, "omega":0.18019, "ax":0.07335, "ay":-0.00075, "alpha":0.76608, "fx":[2.81948,0.78176,-0.74039,1.29801], "fy":[0.24749,1.76957,-0.26882,-1.79069]},
+ {"t":1.84271, "x":8.51709, "y":5.9625, "heading":-2.20291, "vx":-0.02381, "vy":-2.49935, "omega":0.21604, "ax":0.05566, "ay":-0.0005, "alpha":0.40015, "fx":[1.71738,0.64556,-0.1395,0.93245], "fy":[0.13634,0.92141,-0.15056,-0.93559]},
+ {"t":1.88951, "x":8.51604, "y":5.84555, "heading":-2.19236, "vx":-0.02121, "vy":-2.49937, "omega":0.23476, "ax":0.04283, "ay":-0.00035, "alpha":0.20833, "fx":[1.08966,0.52733,0.12451,0.68688], "fy":[0.07488,0.47769,-0.08469,-0.48749]},
+ {"t":1.9363, "x":8.51509, "y":5.7286, "heading":-2.18115, "vx":-0.01921, "vy":-2.49939, "omega":0.24451, "ax":0.03323, "ay":-0.00024, "alpha":0.10801, "fx":[0.72076,0.4269,0.22135,0.51522], "fy":[0.0407,0.24625,-0.04763,-0.25318]},
+ {"t":1.98309, "x":8.51423, "y":5.61165, "heading":-2.16959, "vx":-0.01765, "vy":-2.4994, "omega":0.24956, "ax":0.02593, "ay":-0.00018, "alpha":0.05559, "fx":[0.49575,0.3433,0.23927,0.39173], "fy":[0.02171,0.12574,-0.02671,-0.13074]},
+ {"t":2.02988, "x":8.51343, "y":5.49469, "heading":-2.15785, "vx":-0.01644, "vy":-2.49941, "omega":0.25216, "ax":0.02031, "ay":-0.00013, "alpha":0.02824, "fx":[0.35294,0.27487,0.22293,0.301], "fy":[0.01123,0.06317,-0.0149,-0.06684]},
+ {"t":2.07667, "x":8.51269, "y":5.37774, "heading":-2.14602, "vx":-0.01549, "vy":-2.49942, "omega":0.25348, "ax":0.01598, "ay":-0.0001, "alpha":0.01402, "fx":[0.25867,0.21963,0.19431,0.23336], "fy":[0.0055,0.03082,-0.00823,-0.03355]},
+ {"t":2.12346, "x":8.51198, "y":5.26079, "heading":-2.13414, "vx":-0.01474, "vy":-2.49942, "omega":0.25414, "ax":0.01263, "ay":-0.00007, "alpha":0.00664, "fx":[0.1942,0.17557,0.16381,0.18244], "fy":[0.0024,0.01417,-0.00446,-0.01623]},
+ {"t":2.17026, "x":8.5113, "y":5.14384, "heading":-2.12224, "vx":-0.01415, "vy":-2.49942, "omega":0.25445, "ax":0.01005, "ay":-0.00006, "alpha":0.00284, "fx":[0.14889,0.14087,0.13593,0.14395], "fy":[0.00075,0.00569,-0.00233,-0.00727]},
+ {"t":2.21705, "x":8.51065, "y":5.02688, "heading":-2.11034, "vx":-0.01368, "vy":-2.49943, "omega":0.25458, "ax":0.00807, "ay":-0.00004, "alpha":0.0009, "fx":[0.11646,0.11389,0.11235,0.11492], "fy":[-0.0001,0.00144,-0.00113,-0.00267]},
+ {"t":2.26384, "x":8.51002, "y":4.90993, "heading":-2.09842, "vx":-0.0133, "vy":-2.49943, "omega":0.25463, "ax":0.00658, "ay":-0.00003, "alpha":-0.00007, "fx":[0.09311,0.09331,0.09343,0.09323], "fy":[-0.00053,-0.00065,-0.00045,-0.00033]},
+ {"t":2.31063, "x":8.5094, "y":4.79298, "heading":-2.08651, "vx":-0.01299, "vy":-2.49943, "omega":0.25462, "ax":0.00548, "ay":-0.00003, "alpha":-0.00054, "fx":[0.0765,0.07805,0.07893,0.07738], "fy":[-0.00073,-0.00161,-0.00006,0.00081]},
+ {"t":2.35742, "x":8.5088, "y":4.67603, "heading":-2.07459, "vx":-0.01274, "vy":-2.49943, "omega":0.2546, "ax":0.00471, "ay":-0.00002, "alpha":-0.00077, "fx":[0.06508,0.0673,0.06853,0.06631], "fy":[-0.00084,-0.00206,0.00016,0.00139]},
+ {"t":2.40422, "x":8.50821, "y":4.55907, "heading":-2.06268, "vx":-0.01252, "vy":-2.49943, "omega":0.25456, "ax":0.00422, "ay":-0.00002, "alpha":-0.00082, "fx":[0.05804,0.06044,0.06173,0.05932], "fy":[-0.00085,-0.00214,0.00026,0.00155]},
+ {"t":2.45101, "x":8.50763, "y":4.44212, "heading":-2.05077, "vx":-0.01232, "vy":-2.49943, "omega":0.25452, "ax":0.00399, "ay":-0.00002, "alpha":-0.00099, "fx":[0.05432,0.05723,0.05874,0.05583], "fy":[-0.00097,-0.00249,0.00042,0.00194]},
+ {"t":2.4978, "x":8.50706, "y":4.32517, "heading":-2.03886, "vx":-0.01213, "vy":-2.49944, "omega":0.25448, "ax":0.00399, "ay":-0.00002, "alpha":-0.00105, "fx":[0.05422,0.05732,0.05888,0.05578], "fy":[-0.00104,-0.0026,0.00049,0.00206]},
+ {"t":2.54459, "x":8.5065, "y":4.20821, "heading":-2.02696, "vx":-0.01195, "vy":-2.49944, "omega":0.25443, "ax":0.00423, "ay":-0.00002, "alpha":-0.0012, "fx":[0.05729,0.06085,0.0626,0.05903], "fy":[-0.00119,-0.00294,0.00062,0.00237]},
+ {"t":2.59138, "x":8.50594, "y":4.09126, "heading":-2.01505, "vx":-0.01175, "vy":-2.49944, "omega":0.25437, "ax":0.00472, "ay":-0.00002, "alpha":-0.00146, "fx":[0.06368,0.06806,0.07014,0.06576], "fy":[-0.00146,-0.00354,0.00084,0.00292]},
+ {"t":2.63817, "x":8.5054, "y":3.97431, "heading":-2.00315, "vx":-0.01153, "vy":-2.49944, "omega":0.2543, "ax":0.00549, "ay":-0.00003, "alpha":-0.00204, "fx":[0.07337,0.07953,0.08237,0.07621], "fy":[-0.00201,-0.00486,0.0013,0.00415]},
+ {"t":2.68497, "x":8.50486, "y":3.85735, "heading":-1.99125, "vx":-0.01127, "vy":-2.49944, "omega":0.25421, "ax":0.0066, "ay":-0.00003, "alpha":-0.00304, "fx":[0.08682,0.09603,0.10015,0.09094], "fy":[-0.00296,-0.00708,0.00213,0.00625]},
+ {"t":2.73176, "x":8.50434, "y":3.7404, "heading":-1.97936, "vx":-0.01096, "vy":-2.49944, "omega":0.25407, "ax":0.00809, "ay":-0.00003, "alpha":-0.00499, "fx":[0.1038,0.11901,0.1256,0.11039], "fy":[-0.00481,-0.0114,0.00382,0.0104]},
+ {"t":2.77855, "x":8.50384, "y":3.62345, "heading":-1.96748, "vx":-0.01058, "vy":-2.49944, "omega":0.25383, "ax":0.01007, "ay":-0.00004, "alpha":-0.00851, "fx":[0.12432,0.15037,0.16128,0.13523], "fy":[-0.00816,-0.01907,0.00698,0.01789]},
+ {"t":2.82534, "x":8.50335, "y":3.50649, "heading":-1.95561, "vx":-0.01011, "vy":-2.49944, "omega":0.25343, "ax":0.01266, "ay":-0.00005, "alpha":-0.01526, "fx":[0.14652,0.19349,0.21251,0.16555], "fy":[-0.01468,-0.0337,0.01327,0.03229]},
+ {"t":2.87213, "x":8.5029, "y":3.38954, "heading":-1.94377, "vx":-0.00952, "vy":-2.49945, "omega":0.25272, "ax":0.01603, "ay":-0.00006, "alpha":-0.02829, "fx":[0.16631,0.25378,0.28801,0.20054], "fy":[-0.02746,-0.06168,0.02579,0.06002]},
+ {"t":2.91893, "x":8.50247, "y":3.27259, "heading":-1.93198, "vx":-0.00877, "vy":-2.49945, "omega":0.2514, "ax":0.02038, "ay":-0.00007, "alpha":-0.05299, "fx":[0.17544,0.34002,0.40219,0.23761], "fy":[-0.05217,-0.11434,0.05025,0.11242]},
+ {"t":2.96572, "x":8.50208, "y":3.15563, "heading":-1.92027, "vx":-0.00782, "vy":-2.49945, "omega":0.24892, "ax":0.02601, "ay":-0.00008, "alpha":-0.10005, "fx":[0.15572,0.46786,0.58161,0.26948], "fy":[-0.10026,-0.21401,0.09812,0.21188]},
+ {"t":3.01251, "x":8.50174, "y":3.03868, "heading":-1.90873, "vx":-0.0066, "vy":-2.49946, "omega":0.24424, "ax":0.03334, "ay":-0.00008, "alpha":-0.18968, "fx":[0.07109,0.66527,0.87406,0.2799], "fy":[-0.1938,-0.40259,0.19158,0.4004]},
+ {"t":3.0593, "x":8.50147, "y":2.92172, "heading":-1.89751, "vx":-0.00504, "vy":-2.49946, "omega":0.23536, "ax":0.04297, "ay":-0.00007, "alpha":-0.36051, "fx":[-0.14979,0.98391,1.36804,0.2344], "fy":[-0.37578,-0.75991,0.37377,0.75798]},
+ {"t":3.10609, "x":8.50128, "y":2.80477, "heading":-1.88689, "vx":-0.00303, "vy":-2.49946, "omega":0.21849, "ax":0.05586, "ay":-0.00004, "alpha":-0.6859, "fx":[-0.6445,1.52014,2.22796,0.06356], "fy":[-0.72898,-1.43679,0.7277,1.43586]},
+ {"t":3.15288, "x":8.5012, "y":2.68782, "heading":-1.87742, "vx":-0.00041, "vy":-2.49946, "omega":0.1864, "ax":0.07364, "ay":0.00004, "alpha":-1.30478, "fx":[-1.67532,2.45532,3.76236,-0.36711], "fy":[-1.41125,-2.71819,1.4115,2.7201]},
+ {"t":3.19968, "x":8.50126, "y":2.57086, "heading":-1.87013, "vx":0.00303, "vy":-2.49946, "omega":0.12534, "ax":0.0996, "ay":0.00021, "alpha":-2.4779, "fx":[-3.73294,4.13074,6.55384,-1.30424], "fy":[-2.71707,-5.13939,2.71903,5.14952]},
+ {"t":3.24647, "x":8.50151, "y":2.45391, "heading":-1.86698, "vx":0.00769, "vy":-2.49945, "omega":0.0094, "ax":0.14196, "ay":0.00062, "alpha":-4.68307, "fx":[-7.69827,7.18525,11.70856,-3.14664], "fy":[-5.16969,-9.68818,5.16655,9.72676]},
+ {"t":3.29326, "x":8.50203, "y":2.33695, "heading":-1.87166, "vx":0.01434, "vy":-2.49942, "omega":-0.20973, "ax":0.22726, "ay":0.00179, "alpha":-8.74009, "fx":[-14.9677,12.81449,21.33001,-6.29144], "fy":[-9.59601,-18.08501,9.53132,18.25098]},
+ {"t":3.34005, "x":8.50295, "y":2.22, "heading":-1.89104, "vx":0.02497, "vy":-2.49934, "omega":-0.6187, "ax":0.41546, "ay":0.00577, "alpha":-15.80217, "fx":[-27.4703,22.75016,38.73927,-10.46321], "fy":[-16.92948,-32.79834,16.42529,33.62945]},
+ {"t":3.38684, "x":8.50457, "y":2.10306, "heading":-1.93729, "vx":0.04441, "vy":-2.49907, "omega":-1.35811, "ax":-1.00116, "ay":0.00109, "alpha":-26.62315, "fx":[-68.98609,8.91036,44.36828,-41.05714], "fy":[-23.71327,-59.65747,28.04433,55.38818]},
+ {"t":3.43364, "x":8.50555, "y":1.98613, "heading":-2.02999, "vx":-0.00244, "vy":-2.49902, "omega":-2.60385, "ax":-9.53276, "ay":2.17454, "alpha":-0.32066, "fx":[-135.30241,-135.40738,-134.94714,-134.84148], "fy":[30.06556,29.56752,31.59666,32.06483]},
+ {"t":3.48043, "x":8.495, "y":1.87157, "heading":-2.15218, "vx":-0.44849, "vy":-2.39727, "omega":-2.61886, "ax":-9.45267, "ay":2.52274, "alpha":-0.21352, "fx":[-134.10675,-134.21898,-133.87244,-133.75904], "fy":[35.32922,34.89154,36.19551,36.62091]},
+ {"t":3.51497, "x":8.47387, "y":1.79027, "heading":-2.24276, "vx":-0.775, "vy":-2.31013, "omega":-2.62623, "ax":-9.23851, "ay":3.20871, "alpha":-0.23061, "fx":[-131.10916,-131.27214,-130.7984,-130.63478], "fy":[45.04496,44.55867,45.92753,46.3997]},
+ {"t":3.54951, "x":8.44159, "y":1.71239, "heading":-2.33361, "vx":-1.0941, "vy":-2.1993, "omega":-2.6342, "ax":-8.81433, "ay":4.2246, "alpha":-0.25627, "fx":[-125.17779,-125.40272,-124.7033,-124.48011], "fy":[59.39857,58.91242,60.37563,60.84405]},
+ {"t":3.58405, "x":8.39854, "y":1.63895, "heading":-2.42475, "vx":-1.39856, "vy":-2.05338, "omega":-2.64305, "ax":-7.91421, "ay":5.72189, "alpha":-0.30002, "fx":[-112.61813,-112.88047,-111.74109,-111.48865], "fy":[80.51303,80.13335,81.71196,82.06736]},
+ {"t":3.61859, "x":8.34551, "y":1.57143, "heading":-2.51623, "vx":-1.67193, "vy":-1.85574, "omega":-2.65341, "ax":-6.80903, "ay":6.98212, "alpha":-0.35998, "fx":[-97.23229,-97.48639,-95.78868,-95.55839], "fy":[98.28305,98.01511,99.67251,99.90905]},
+ {"t":3.65313, "x":8.2837, "y":1.5115, "heading":-2.60809, "vx":-1.90712, "vy":-1.61457, "omega":-2.66585, "ax":-5.77848, "ay":7.82714, "alpha":-0.43609, "fx":[-82.93671,-83.18241,-80.85869,-80.65631], "fy":[110.20496,109.99387,111.71049,111.88219]},
+ {"t":3.68767, "x":8.21438, "y":1.4604, "heading":-2.70043, "vx":-2.10671, "vy":-1.34421, "omega":-2.68091, "ax":-4.65323, "ay":8.4902, "alpha":-0.541, "fx":[-67.41019,-67.61475,-64.46849,-64.34031], "fy":[119.57854,119.41404,121.13854,121.25513]},
+ {"t":3.72221, "x":8.13884, "y":1.41904, "heading":-2.79336, "vx":-2.26744, "vy":-1.05095, "omega":-2.6996, "ax":-3.44166, "ay":8.92386, "alpha":-0.70189, "fx":[-50.85067,-50.9638,-46.64984,-46.67454], "fy":[125.75837,125.59407,127.25702,127.36516]},
+ {"t":3.75676, "x":8.05847, "y":1.38806, "heading":-2.88702, "vx":-2.38632, "vy":-0.74271, "omega":-2.72384, "ax":-2.16159, "ay":8.88694, "alpha":-0.99218, "fx":[-33.66455,-33.60113,-27.48266,-27.81164], "fy":[125.44953,125.02829,126.5215,126.88197]},
+ {"t":3.7913, "x":7.97475, "y":1.36771, "heading":-2.9817, "vx":-2.46098, "vy":-0.43575, "omega":-2.75811, "ax":-0.83376, "ay":6.31579, "alpha":-1.39154, "fx":[-15.26753,-14.87275,-8.22847,-8.90438], "fy":[90.37593,87.73812,88.69215,91.29304]},
+ {"t":3.82584, "x":7.88925, "y":1.35642, "heading":-3.0778, "vx":-2.48978, "vy":-0.21759, "omega":-2.80618, "ax":-0.09013, "ay":1.13375, "alpha":0.37884, "fx":[-0.60146,-0.68139,-1.95226,-1.87501], "fy":[15.49227,16.73069,16.64895,15.41041]},
+ {"t":3.86038, "x":7.80319, "y":1.34958, "heading":3.10868, "vx":-2.49289, "vy":-0.17843, "omega":-2.79309, "ax":-0.00859, "ay":0.12148, "alpha":1.5212, "fx":[2.32069,2.48475,-2.56196,-2.73054], "fy":[-0.88492,4.16282,4.32856,-0.71856]},
+ {"t":3.89492, "x":7.71708, "y":1.34349, "heading":3.01312, "vx":-2.49319, "vy":-0.17423, "omega":-2.74055, "ax":-0.00058, "ay":0.00833, "alpha":1.68676, "fx":[2.41034,3.12774,-2.42659,-3.14435], "fy":[-3.01804,2.53652,3.25403,-2.30041]},
+ {"t":3.92946, "x":7.63096, "y":1.33748, "heading":2.91946, "vx":-2.49321, "vy":-0.17395, "omega":-2.68229, "ax":0.00054, "ay":-0.00771, "alpha":1.49383, "fx":[1.88031,2.97319,-1.86513,-2.95776], "fy":[-3.07476,1.76336,2.85618,-1.98208]},
+ {"t":3.964, "x":7.54485, "y":1.33147, "heading":2.8277, "vx":-2.49319, "vy":-0.17421, "omega":-2.63069, "ax":0.00082, "ay":-0.01167, "alpha":1.20366, "fx":[1.2952,2.5293,-1.27213,-2.50602], "fy":[-2.68308,1.11815,2.35223,-1.44918]},
+ {"t":3.99854, "x":7.45873, "y":1.32544, "heading":2.73755, "vx":-2.49316, "vy":-0.17462, "omega":-2.58911, "ax":0.00093, "ay":-0.01327, "alpha":0.89685, "fx":[0.79687,1.96764,-0.77052,-1.94117], "fy":[-2.14253,0.59549,1.76626,-0.9719]},
+ {"t":4.03308, "x":7.37261, "y":1.3194, "heading":2.64866, "vx":-2.49313, "vy":-0.17508, "omega":-2.55813, "ax":0.00095, "ay":-0.01347, "alpha":0.59448, "fx":[0.41583,1.34993,-0.38899,-1.32305], "fy":[-1.5274,0.21146,1.14556,-0.59337]},
+ {"t":4.06762, "x":7.2865, "y":1.31335, "heading":2.56065, "vx":-2.4931, "vy":-0.17554, "omega":-2.5376, "ax":0.00051, "ay":-0.0073, "alpha":0.30107, "fx":[0.15081,0.69944,-0.13623,-0.68486], "fy":[-0.79557,0.0401,0.58873,-0.24694]},
+ {"t":4.10217, "x":7.20039, "y":1.30728, "heading":2.47318, "vx":-2.49308, "vy":-0.17579, "omega":-2.5272, "ax":-0.00402, "ay":0.0573, "alpha":0.0357, "fx":[-0.04718,0.02629,-0.06674,-0.14021], "fy":[0.72904,0.82207,0.89553,0.8025]},
+ {"t":4.13671, "x":7.11427, "y":1.30124, "heading":2.38591, "vx":-2.49322, "vy":-0.17381, "omega":-2.52597, "ax":-0.04459, "ay":0.68671, "alpha":0.06588, "fx":[-0.6275,-0.47666,-0.63654,-0.78739], "fy":[9.58008,9.73862,9.88777,9.72928]},
+ {"t":4.17125, "x":7.02813, "y":1.29565, "heading":2.2987, "vx":-2.49476, "vy":-0.15009, "omega":-2.52369, "ax":-0.12718, "ay":5.03128, "alpha":1.96942, "fx":[-2.21654,4.03964,-1.4206,-7.61347], "fy":[67.88587,71.20815,74.69771,71.47679]},
+ {"t":4.20579, "x":6.94188, "y":1.29346, "heading":2.2127, "vx":-2.49915, "vy":0.02369, "omega":-2.45567, "ax":0.63492, "ay":8.89679, "alpha":2.1324, "fx":[8.24065,18.14251,9.6895,-0.07321], "fy":[125.43112,125.27293,127.03389,126.70146]},
+ {"t":4.24033, "x":6.85593, "y":1.29959, "heading":2.12915, "vx":-2.47722, "vy":0.331, "omega":-2.38201, "ax":1.87865, "ay":9.32517, "alpha":1.52982, "fx":[26.40177,33.46123,26.87011,19.78446], "fy":[132.10417,130.71533,132.42535,133.48338]},
+ {"t":4.27487, "x":6.77149, "y":1.31659, "heading":2.04779, "vx":-2.41233, "vy":0.6531, "omega":-2.32917, "ax":3.14526, "ay":9.13598, "alpha":1.20021, "fx":[44.70284,49.81652,44.50864,39.3051], "fy":[129.43533,127.64359,129.66411,131.25819]},
+ {"t":4.30941, "x":6.69004, "y":1.34459, "heading":1.96805, "vx":-2.30369, "vy":0.96866, "omega":-2.28771, "ax":4.38714, "ay":8.67298, "alpha":1.00065, "fx":[62.5319,66.30612,61.89806,58.01064], "fy":[122.76382,120.8149,123.16761,125.00348]},
+ {"t":4.34395, "x":6.61309, "y":1.38323, "heading":1.88963, "vx":-2.15215, "vy":1.26824, "omega":-2.25315, "ax":5.54884, "ay":7.98851, "alpha":1.05829, "fx":[79.24549,82.62506,78.14867,74.59455], "fy":[112.8314,110.43309,113.67936,115.99689]},
+ {"t":4.38378, "x":6.53176, "y":1.44008, "heading":1.80072, "vx":-1.93113, "vy":1.58643, "omega":-2.211, "ax":6.68451, "ay":6.97005, "alpha":1.5026, "fx":[95.83455,99.57396,93.87745,89.71955], "fy":[97.73364,94.09954,99.87767,103.48453]},
+ {"t":4.42362, "x":6.46015, "y":1.5088, "heading":1.71385, "vx":-1.66488, "vy":1.86406, "omega":-2.15114, "ax":7.47107, "ay":5.72749, "alpha":2.72529, "fx":[107.92659,112.88246,104.5613,98.232], "fy":[78.13634,71.97562,84.04315,90.5882]},
+ {"t":4.46345, "x":6.39976, "y":1.58759, "heading":1.63033, "vx":-1.36729, "vy":2.0922, "omega":-2.04259, "ax":6.45537, "ay":3.69267, "alpha":9.90994, "fx":[91.69604,109.61248,95.35041,69.35428], "fy":[34.67085,26.38877,65.76693,82.54423]},
+ {"t":4.50328, "x":6.35042, "y":1.67386, "heading":1.55683, "vx":-1.11016, "vy":2.23928, "omega":-1.64786, "ax":1.24178, "ay":0.59877, "alpha":19.09112, "fx":[-14.61027,49.64858,48.10098,-12.7313], "fy":[-24.62369,-22.62296,38.01651,43.17985]},
+ {"t":4.54311, "x":6.30718, "y":1.76353, "heading":1.50634, "vx":-1.0607, "vy":2.26313, "omega":-0.88743, "ax":0.1093, "ay":0.0511, "alpha":12.14154, "fx":[-19.89911,20.40507,22.92046,-17.22927], "fy":[-18.15303,-20.63268,19.48014,22.20288]},
+ {"t":4.58294, "x":6.26502, "y":1.85371, "heading":1.48062, "vx":-1.05635, "vy":2.26517, "omega":-0.40381, "ax":0.00698, "ay":0.00325, "alpha":7.21239, "fx":[-12.90558,10.94677,13.1015,-10.7471], "fy":[-10.80215,-12.95613,10.89172,13.05099]},
+ {"t":4.62278, "x":6.22295, "y":1.94394, "heading":1.47026, "vx":-1.05607, "vy":2.2653, "omega":-0.11653, "ax":-0.00086, "ay":-0.0004, "alpha":4.21312, "fx":[-7.67349,6.2449,7.64909,-6.26947], "fy":[-6.26285,-7.66706,6.25153,7.65552]},
+ {"t":4.66261, "x":6.18088, "y":2.03417, "heading":1.46896, "vx":-1.0561, "vy":2.26528, "omega":0.05129, "ax":-0.00137, "ay":-0.00064, "alpha":2.44522, "fx":[-4.47062,3.60631,4.43176,-3.64525], "fy":[-3.63483,-4.46029,3.61673,4.44208]},
+ {"t":4.70244, "x":6.13882, "y":2.1244, "heading":1.47294, "vx":-1.05616, "vy":2.26526, "omega":0.14869, "ax":-0.00135, "ay":-0.00063, "alpha":1.42061, "fx":[-2.59674,2.09763,2.5585,-2.1359], "fy":[-2.12568,-2.58655,2.10785,2.56868]},
+ {"t":4.74227, "x":6.09675, "y":2.21463, "heading":1.47999, "vx":-1.05621, "vy":2.26523, "omega":0.20527, "ax":-0.00129, "ay":-0.0006, "alpha":0.83557, "fx":[-1.52561,1.23735,1.48895,-1.27403], "fy":[-1.26424,-1.51584,1.24714,1.49872]},
+ {"t":4.7821, "x":6.05467, "y":2.30486, "heading":1.48883, "vx":-1.05626, "vy":2.26521, "omega":0.23855, "ax":-0.00123, "ay":-0.00057, "alpha":0.51211, "fx":[-0.9344,0.76029,0.89951,-0.79518], "fy":[-0.78587,-0.9251,0.76959,0.90881]},
+ {"t":4.82193, "x":6.0126, "y":2.39508, "heading":1.49874, "vx":-1.05631, "vy":2.26519, "omega":0.25895, "ax":-0.00105, "ay":-0.00049, "alpha":0.34921, "fx":[-0.63484,0.52166,0.60515,-0.55136], "fy":[-0.54344,-0.62693,0.52958,0.61306]},
+ {"t":4.86177, "x":5.97052, "y":2.48531, "heading":1.50933, "vx":-1.05635, "vy":2.26517, "omega":0.27286, "ax":0.00088, "ay":0.00041, "alpha":0.29642, "fx":[-0.50892,0.47346,0.53392,-0.44846], "fy":[-0.45514,-0.5156,0.46678,0.52724]},
+ {"t":4.9016, "x":5.92845, "y":2.57554, "heading":1.52043, "vx":-1.05632, "vy":2.26518, "omega":0.28467, "ax":0.02743, "ay":0.01278, "alpha":0.33725, "fx":[-0.19855,0.91983,0.97618,-0.14217], "fy":[-0.34981,-0.40617,0.71219,0.76859]},
+ {"t":4.94143, "x":5.8864, "y":2.66577, "heading":1.53204, "vx":-1.05523, "vy":2.26569, "omega":0.2981, "ax":0.40028, "ay":0.18471, "alpha":0.48238, "fx":[4.84474,6.44361,6.50273,4.90466], "fy":[1.84898,1.7851,3.38686,3.45219]},
+ {"t":4.98126, "x":5.84468, "y":2.75616, "heading":1.54429, "vx":-1.03928, "vy":2.27305, "omega":0.31732, "ax":4.3983, "ay":1.81253, "alpha":0.48507, "fx":[61.78708,63.13449,62.90323,61.55485], "fy":[24.91061,24.60149,26.46577,26.79076]},
+ {"t":5.02109, "x":5.80677, "y":2.84814, "heading":1.55732, "vx":-0.86409, "vy":2.34525, "omega":0.33664, "ax":8.78483, "ay":2.51264, "alpha":0.08655, "fx":[124.55009,124.62674,124.49614,124.41855], "fy":[35.43626,35.29821,35.79502,35.93459]},
+ {"t":5.06092, "x":5.77933, "y":2.94355, "heading":1.5708, "vx":-0.51417, "vy":2.44533, "omega":0.34009, "ax":9.00693, "ay":1.11974, "alpha":0.10651, "fx":[127.67864,127.73784,127.6639,127.60402], "fy":[15.58455,15.51174,16.15806,16.23381]},
+ {"t":5.10774, "x":5.76512, "y":3.05926, "heading":1.58683, "vx":-0.0925, "vy":2.49775, "omega":0.34507, "ax":2.6642, "ay":0.03221, "alpha":0.6451, "fx":[36.79062,38.7738,38.73833,36.7548], "fy":[-0.71473,-0.67203,1.61837,1.59495]},
+ {"t":5.15456, "x":5.76371, "y":3.17623, "heading":1.6037, "vx":0.03223, "vy":2.49926, "omega":0.37527, "ax":-0.09363, "ay":0.00112, "alpha":0.38922, "fx":[-1.95169,-0.66015,-0.70267,-1.99418], "fy":[-0.65114,-0.60873,0.68314,0.64051]},
+ {"t":5.20137, "x":5.76512, "y":3.29324, "heading":1.62169, "vx":0.02784, "vy":2.49931, "omega":0.39349, "ax":-0.15598, "ay":0.00151, "alpha":0.20236, "fx":[-2.52927,-1.85839,-1.89256,-2.56344], "fy":[-0.33129,-0.29715,0.37412,0.33988]},
+ {"t":5.24819, "x":5.76625, "y":3.41025, "heading":1.64034, "vx":0.02054, "vy":2.49938, "omega":0.40297, "ax":-0.12166, "ay":0.00086, "alpha":0.10519, "fx":[-1.88659,-1.53822,-1.56248,-1.91085], "fy":[-0.17417,-0.1499,0.19858,0.1743]},
+ {"t":5.29501, "x":5.76708, "y":3.52726, "heading":1.65932, "vx":0.01484, "vy":2.49942, "omega":0.40789, "ax":-0.09822, "ay":0.00049, "alpha":0.05456, "fx":[-1.47438,-1.29396,-1.30997,-1.4904], "fy":[-0.09125,-0.07524,0.10522,0.08921]},
+ {"t":5.34182, "x":5.76767, "y":3.64428, "heading":1.67847, "vx":0.01025, "vy":2.49944, "omega":0.41045, "ax":-0.08276, "ay":0.00027, "alpha":0.02812, "fx":[-1.21454,-1.12171,-1.13175,-1.22458], "fy":[-0.04754,-0.03751,0.05534,0.0453]},
+ {"t":5.38864, "x":5.76806, "y":3.76129, "heading":1.69772, "vx":0.00637, "vy":2.49946, "omega":0.41176, "ax":-0.07204, "ay":0.00013, "alpha":0.01396, "fx":[-1.04122,-0.99524,-1.00111,-1.04709], "fy":[-0.02401,-0.01814,0.02784,0.02197]},
+ {"t":5.43546, "x":5.76828, "y":3.87831, "heading":1.71701, "vx":0.003, "vy":2.49946, "omega":0.41242, "ax":-0.06441, "ay":0.00004, "alpha":0.00655, "fx":[-0.92211,-0.9006,-0.90377,-0.92528], "fy":[-0.0118,-0.00863,0.01289,0.00972]},
+ {"t":5.48227, "x":5.76835, "y":3.99532, "heading":1.73633, "vx":-0.00002, "vy":2.49947, "omega":0.41272, "ax":-0.05899, "ay":-0.00003, "alpha":0.00134, "fx":[-0.83793,-0.83355,-0.83428,-0.83866], "fy":[-0.00302,-0.00229,0.00209,0.00136]},
+ {"t":5.52909, "x":5.76828, "y":4.11234, "heading":1.75565, "vx":-0.00278, "vy":2.49946, "omega":0.41279, "ax":-0.05529, "ay":-0.00009, "alpha":-0.0035, "fx":[-0.77902,-0.79045,-0.78831,-0.77689], "fy":[0.0055,0.00337,-0.00806,-0.00592]},
+ {"t":5.57591, "x":5.76809, "y":4.22935, "heading":1.77497, "vx":-0.00537, "vy":2.49946, "omega":0.41262, "ax":-0.05302, "ay":-0.00014, "alpha":-0.01003, "fx":[-0.73868,-0.77128,-0.76453,-0.73193], "fy":[0.01769,0.01094,-0.02166,-0.01491]},
+ {"t":5.62272, "x":5.76778, "y":4.34637, "heading":1.79428, "vx":-0.00785, "vy":2.49945, "omega":0.41215, "ax":-0.05204, "ay":-0.00019, "alpha":-0.02134, "fx":[-0.71102,-0.78012,-0.76441,-0.69532], "fy":[0.03973,0.02402,-0.04508,-0.02937]},
+ {"t":5.66954, "x":5.76736, "y":4.46339, "heading":1.81355, "vx":-0.01029, "vy":2.49944, "omega":0.41115, "ax":-0.05228, "ay":-0.00024, "alpha":-0.04238, "fx":[-0.68973,-0.82633,-0.7925,-0.65591], "fy":[0.0818,0.04797,-0.08862,-0.0548]},
+ {"t":5.71635, "x":5.76682, "y":4.5804, "heading":1.83275, "vx":-0.01273, "vy":2.49943, "omega":0.40917, "ax":-0.05376, "ay":-0.0003, "alpha":-0.08263, "fx":[-0.66505,-0.93004,-0.85899,-0.594], "fy":[0.16377,0.09271,-0.1723,-0.10124]},
+ {"t":5.76317, "x":5.76616, "y":4.69741, "heading":1.85182, "vx":-0.01525, "vy":2.49942, "omega":0.4053, "ax":-0.05656, "ay":-0.00037, "alpha":-0.15998, "fx":[-0.62028,-1.13062,-0.98331,-0.47296], "fy":[0.32354,0.17621,-0.33416,-0.18685]},
+ {"t":5.80999, "x":5.76539, "y":4.81443, "heading":1.87062, "vx":-0.0179, "vy":2.4994, "omega":0.39781, "ax":-0.0609, "ay":-0.00047, "alpha":-0.30871, "fx":[-0.52501,-1.50427,-1.20156,-0.22225], "fy":[0.6344,0.33161,-0.64771,-0.34499]},
+ {"t":5.8568, "x":5.76448, "y":4.93144, "heading":1.8889, "vx":-0.02075, "vy":2.49938, "omega":0.38336, "ax":-0.06713, "ay":-0.0006, "alpha":-0.59487, "fx":[-0.32242,-2.1983,-1.58066,0.29544], "fy":[1.23857,0.62061,-1.2554,-0.63775]},
+ {"t":5.90362, "x":5.76344, "y":5.04845, "heading":1.9062, "vx":-0.02389, "vy":2.49935, "omega":0.35551, "ax":-0.07585, "ay":-0.00078, "alpha":-1.14419, "fx":[0.09295,-3.49377,-2.24385,1.34385], "fy":[2.40838,1.15719,-2.42982,-1.1799]},
+ {"t":5.95044, "x":5.76223, "y":5.16546, "heading":1.92159, "vx":-0.02744, "vy":2.49932, "omega":0.30194, "ax":-0.08823, "ay":-0.00104, "alpha":-2.19336, "fx":[0.91633,-5.92016,-3.41988,3.42099], "fy":[4.65789,2.15239,-4.68476,-2.18456]},
+ {"t":5.99725, "x":5.76085, "y":5.28247, "heading":1.93332, "vx":-0.03157, "vy":2.49927, "omega":0.19926, "ax":-0.10685, "ay":-0.00146, "alpha":-4.17811, "fx":[2.50572,-10.45404,-5.54461,7.43486], "fy":[8.93162,3.99983,-8.96146,-4.05256]},
+ {"t":6.04407, "x":5.75926, "y":5.39947, "heading":1.93807, "vx":-0.03658, "vy":2.4992, "omega":0.00365, "ax":-0.14313, "ay":-0.00229, "alpha":-7.84649, "fx":[5.42618,-18.83734,-9.53091,14.82653], "fy":[16.83328,7.42289,-16.84435,-7.54145]},
+ {"t":6.09089, "x":5.75739, "y":5.51648, "heading":1.92964, "vx":-0.04328, "vy":2.49909, "omega":-0.36369, "ax":-0.32347, "ay":-0.00658, "alpha":-14.25083, "fx":[9.06808,-34.84585,-18.57905,26.0164], "fy":[30.60149,13.56511,-30.37979,-14.16004]},
+ {"t":6.1377, "x":5.75501, "y":5.63347, "heading":1.897, "vx":-0.05842, "vy":2.49878, "omega":-1.03086, "ax":-2.22604, "ay":-0.09854, "alpha":-22.29898, "fx":[-12.45483,-74.1875,-55.23344,15.66171], "fy":[51.29842,19.94207,-46.43039,-30.39731]},
+ {"t":6.18452, "x":5.74983, "y":5.75034, "heading":1.8243, "vx":-0.16264, "vy":2.49417, "omega":-2.07482, "ax":6.92518, "ay":0.00047, "alpha":-16.62965, "fx":[108.65324,81.93454,86.73568,115.32744], "fy":[39.2192,41.43377,-58.00916,-22.61718]},
+ {"t":6.23134, "x":5.74981, "y":5.86711, "heading":1.70894, "vx":0.16158, "vy":2.49419, "omega":-2.85336, "ax":9.16241, "ay":-2.79098, "alpha":-4.16102, "fx":[134.6027,132.18424,123.73368,128.97916], "fy":[-23.18177,-32.36077,-57.29198,-45.4115]},
+ {"t":6.27815, "x":5.76741, "y":5.98082, "heading":1.5708, "vx":0.59053, "vy":2.36353, "omega":-3.04817, "ax":5.26186, "ay":-7.90422, "alpha":-8.09435, "fx":[104.9592,67.09586,45.05782,81.22948], "fy":[-88.35882,-118.98697,-129.76426,-111.05147]},
+ {"t":6.31355, "x":5.79161, "y":6.05953, "heading":1.45784, "vx":0.77677, "vy":2.08376, "omega":-3.33466, "ax":5.28904, "ay":-7.86294, "alpha":-6.34216, "fx":[98.54036,72.91199,50.62213,77.80885], "fy":[-93.66852,-113.7596,-126.2836,-112.10953]},
+ {"t":6.34894, "x":5.82242, "y":6.12835, "heading":1.33584, "vx":0.96397, "vy":1.80546, "omega":-3.55914, "ax":5.76043, "ay":-7.31852, "alpha":-3.39691, "fx":[93.20926,82.99052,69.34017,81.07073], "fy":[-94.31118,-102.21858,-112.83047,-105.59274]},
+ {"t":6.38433, "x":5.86014, "y":6.18767, "heading":1.20774, "vx":1.16786, "vy":1.54643, "omega":-3.67937, "ax":7.30163, "ay":-4.36612, "alpha":2.51961, "fx":[98.19261,101.69718,108.35852,105.7474], "fy":[-69.02745,-67.32244,-55.31786,-55.88684]},
+ {"t":6.41973, "x":5.90605, "y":6.23967, "heading":1.07909, "vx":1.42629, "vy":1.39189, "omega":-3.59019, "ax":6.74995, "ay":2.19147, "alpha":6.84779, "fx":[88.45638,102.83805,103.38471,88.03663], "fy":[29.3537,7.57261,33.10886,54.21937]},
+ {"t":6.45512, "x":5.96076, "y":6.29031, "heading":0.9563, "vx":1.6652, "vy":1.46946, "omega":-3.34782, "ax":4.13129, "ay":4.1596, "alpha":4.36009, "fx":[48.63412,64.32326,67.64414,53.63845], "fy":[61.24579,47.8924,57.46346,69.24386]},
+ {"t":6.49052, "x":6.02229, "y":6.34493, "heading":0.84054, "vx":1.81143, "vy":1.61668, "omega":-3.19349, "ax":2.14328, "ay":-0.06804, "alpha":-1.09505, "fx":[32.81658,30.26223,27.92659,30.51647], "fy":[-0.80983,1.72633,-1.1224,-3.65214]},
+ {"t":6.52591, "x":6.08775, "y":6.40211, "heading":0.72682, "vx":1.88729, "vy":1.61427, "omega":-3.23225, "ax":4.38548, "ay":-5.35343, "alpha":-0.33764, "fx":[63.00576,62.60472,61.31307,61.72899], "fy":[-75.54161,-75.14429,-76.2317,-76.61704]},
+ {"t":6.56131, "x":6.15729, "y":6.45589, "heading":0.61221, "vx":2.04251, "vy":1.42479, "omega":-3.2442, "ax":4.19942, "ay":-6.70752, "alpha":0.00361, "fx":[59.51593,59.51903,59.53567,59.53257], "fy":[-95.08137,-95.08435,-95.07367,-95.07069]},
+ {"t":6.5967, "x":6.23222, "y":6.50212, "heading":0.49738, "vx":2.19114, "vy":1.18739, "omega":-3.24407, "ax":3.41206, "ay":-7.23805, "alpha":0.0027, "fx":[48.35742,48.35946,48.37287,48.37084], "fy":[-102.59974,-102.60208,-102.59557,-102.59323]},
+ {"t":6.63209, "x":6.31191, "y":6.53961, "heading":0.38256, "vx":2.31191, "vy":0.9312, "omega":-3.24398, "ax":2.50095, "ay":-7.58299, "alpha":-0.05538, "fx":[35.61347,35.57589,35.28685,35.32507], "fy":[-107.46371,-107.41117,-107.51064,-107.563]},
+ {"t":6.66749, "x":6.3953, "y":6.56782, "heading":0.26771, "vx":2.40043, "vy":0.6628, "omega":-3.24594, "ax":-0.08157, "ay":-7.941, "alpha":-2.2062, "fx":[6.10766,3.25749,-8.85917,-5.13116], "fy":[-113.46907,-111.09037,-111.57598,-114.11177]},
+ {"t":6.70288, "x":6.48021, "y":6.5863, "heading":0.15144, "vx":2.39754, "vy":0.38174, "omega":-3.32403, "ax":-7.18345, "ay":-4.39596, "alpha":-7.67956, "fx":[-83.0338,-106.64849,-116.63279,-100.97972], "fy":[-87.39857,-47.85647,-40.51737,-73.47412]},
+ {"t":6.73828, "x":6.56057, "y":6.59706, "heading":0.02898, "vx":2.14329, "vy":0.22615, "omega":-3.59584, "ax":-8.80913, "ay":-1.03214, "alpha":-7.71901, "fx":[-118.92039,-125.37669,-129.46557,-125.70652], "fy":[-42.65531,10.27633,6.24931,-32.39157]},
+ {"t":6.77367, "x":6.63092, "y":6.60442, "heading":-0.10313, "vx":1.8315, "vy":0.18961, "omega":-3.86905, "ax":-9.01048, "ay":-0.37865, "alpha":-8.4523, "fx":[-123.52104,-126.81197,-130.48448,-130.06824], "fy":[-38.36476,21.0994,19.00143,-23.20524]},
+ {"t":6.80907, "x":6.6901, "y":6.61089, "heading":-0.24536, "vx":1.51258, "vy":0.17621, "omega":-4.16821, "ax":-8.89022, "ay":-1.43497, "alpha":-9.9353, "fx":[-114.83049,-128.51512,-131.81517,-128.90622], "fy":[-63.05588,1.53976,13.96664,-33.81205]},
+ {"t":6.84446, "x":6.73806, "y":6.61623, "heading":-0.39912, "vx":1.19791, "vy":0.12542, "omega":-4.51986, "ax":-7.71951, "ay":-4.69471, "alpha":-10.20896, "fx":[-86.07843,-104.0967,-130.0716,-117.4423], "fy":[-101.32591,-75.93725,-24.41315,-64.50928]},
+ {"t":6.87985, "x":6.77563, "y":6.61773, "heading":-0.56549, "vx":0.92469, "vy":-0.04074, "omega":-4.8812, "ax":-4.46978, "ay":-8.2934, "alpha":-5.07841, "fx":[-52.17362,-46.22638,-77.85752,-77.17491], "fy":[-124.57524,-125.85249,-109.02433,-110.77577]},
+ {"t":6.91525, "x":6.80556, "y":6.61109, "heading":-0.74144, "vx":0.76648, "vy":-0.33428, "omega":-5.06095, "ax":-1.97282, "ay":-9.42231, "alpha":1.87468, "fx":[-30.1934,-36.02566,-26.03561,-19.6026], "fy":[-133.06191,-131.7994,-134.22624,-135.14822]},
+ {"t":6.95064, "x":6.83145, "y":6.59336, "heading":-0.91939, "vx":0.69665, "vy":-0.66778, "omega":-4.9946, "ax":-1.18315, "ay":-9.50644, "alpha":6.67173, "fx":[-16.53234,-47.3945,-17.28789,14.1311], "fy":[-136.18246,-129.19261,-136.74117,-136.88997]},
+ {"t":6.98604, "x":6.85537, "y":6.56377, "heading":-1.09199, "vx":0.65478, "vy":-1.00425, "omega":-4.75846, "ax":-1.082, "ay":-9.40946, "alpha":9.70144, "fx":[-3.0117,-61.17432,-22.72735,25.56527], "fy":[-137.6033,-123.75666,-136.38979,-135.7579]},
+ {"t":7.02143, "x":6.87786, "y":6.52233, "heading":-1.25434, "vx":0.61648, "vy":-1.33729, "omega":-4.41508, "ax":-1.17861, "ay":-9.30933, "alpha":11.56435, "fx":[9.38472,-70.2275,-31.10981,25.12671], "fy":[-137.66553,-119.11926,-134.93696,-136.10829]},
+ {"t":7.05683, "x":6.89895, "y":6.46917, "heading":-1.40336, "vx":0.57476, "vy":-1.66679, "omega":-4.00577, "ax":-1.5387, "ay":-9.2029, "alpha":12.55189, "fx":[13.23051,-77.27055,-41.06487,17.86188], "fy":[-137.45914,-114.72156,-132.29576,-137.319]},
+ {"t":7.10369, "x":6.92419, "y":6.38096, "heading":-1.57729, "vx":0.50266, "vy":-2.09803, "omega":-3.4176, "ax":-4.42516, "ay":-8.18912, "alpha":10.9017, "fx":[-44.66302,-105.37019,-75.16041,-25.70849], "fy":[-127.9457,-86.93203,-114.87205,-134.56536]},
+ {"t":7.15054, "x":6.94289, "y":6.27366, "heading":-1.72547, "vx":0.2953, "vy":-2.48177, "omega":-2.90675, "ax":-6.84067, "ay":-0.37499, "alpha":16.63656, "fx":[-83.05716,-110.40959,-112.56625,-81.82669], "fy":[38.60856,31.58487,-30.21329,-61.24174]},
+ {"t":7.1974, "x":6.94921, "y":6.15695, "heading":-1.84341, "vx":-0.02525, "vy":-2.49934, "omega":-2.12717, "ax":-0.82735, "ay":0.01475, "alpha":22.07113, "fx":[33.95824,-37.69647,-55.79533,12.62339], "fy":[27.31574,44.22277,-24.06189,-46.64013]},
+ {"t":7.24426, "x":6.94712, "y":6.03985, "heading":-1.91886, "vx":-0.06401, "vy":-2.49865, "omega":-1.09294, "ax":-0.05058, "ay":0.00132, "alpha":12.6519, "fx":[26.21185,-13.31775,-27.60501,11.84313], "fy":[12.62569,26.90273,-12.53689,-26.91674]},
+ {"t":7.29112, "x":6.94407, "y":5.92277, "heading":-1.95618, "vx":-0.06638, "vy":-2.49859, "omega":-0.50008, "ax":-0.00284, "ay":0.00008, "alpha":6.84988, "fx":[14.77298,-6.30367,-14.8528,6.22239], "fy":[6.26451,14.81357,-6.26156,-14.81222]},
+ {"t":7.33798, "x":6.94095, "y":5.80569, "heading":-1.97209, "vx":-0.06652, "vy":-2.49858, "omega":-0.1791, "ax":-0.00043, "ay":0.00001, "alpha":3.63778, "fx":[7.91252,-3.20669,-7.92478,3.19436], "fy":[3.20071,7.9188,-3.20034,-7.91849]},
+ {"t":7.38484, "x":6.93784, "y":5.6886, "heading":-1.97649, "vx":-0.06654, "vy":-2.49858, "omega":-0.00864, "ax":-0.00032, "ay":0.00001, "alpha":1.91736, "fx":[4.17658,-1.67301,-4.18554,1.66404], "fy":[1.66865,4.18118,-1.6684,-4.18094]},
+ {"t":7.4317, "x":6.93472, "y":5.57152, "heading":-1.97479, "vx":-0.06655, "vy":-2.49858, "omega":0.08121, "ax":-0.00031, "ay":0.00001, "alpha":1.00717, "fx":[2.1904,-0.88454,-2.19911,0.87583], "fy":[0.88031,2.19488,-0.88007,-2.19464]},
+ {"t":7.47856, "x":6.9316, "y":5.45444, "heading":-1.96988, "vx":-0.06657, "vy":-2.49858, "omega":0.12841, "ax":-0.0003, "ay":0.00001, "alpha":0.52802, "fx":[1.14405,-0.4714,-1.15266,0.46279], "fy":[0.46721,1.14847,-0.46698,-1.14823]},
+ {"t":7.52542, "x":6.92848, "y":5.33736, "heading":-1.96328, "vx":-0.06658, "vy":-2.49858, "omega":0.15315, "ax":-0.0003, "ay":0.00001, "alpha":0.27631, "fx":[0.59504,-0.25265,-0.60357,0.24413], "fy":[0.24851,0.59942,-0.24827,-0.59919]},
+ {"t":7.57228, "x":6.92536, "y":5.22028, "heading":-1.9558, "vx":-0.0666, "vy":-2.49858, "omega":0.1661, "ax":-0.0003, "ay":0.00001, "alpha":0.14424, "fx":[0.30765,-0.13623,-0.3161,0.12778], "fy":[0.13212,0.31199,-0.13189,-0.31176]},
+ {"t":7.61914, "x":6.92224, "y":5.1032, "heading":-1.94786, "vx":-0.06661, "vy":-2.49858, "omega":0.17285, "ax":-0.0003, "ay":0.00001, "alpha":0.07516, "fx":[0.15777,-0.07426,-0.16615,0.06589], "fy":[0.07019,0.16207,-0.06996,-0.16185]},
+ {"t":7.666, "x":6.91912, "y":4.98611, "heading":-1.93968, "vx":-0.06662, "vy":-2.49858, "omega":0.17638, "ax":-0.00029, "ay":0.00001, "alpha":0.03913, "fx":[0.07988,-0.04132,-0.08818,0.03302], "fy":[0.03729,0.08414,-0.03706,-0.08391]},
+ {"t":7.71286, "x":6.91599, "y":4.86903, "heading":-1.93137, "vx":-0.06664, "vy":-2.49858, "omega":0.17821, "ax":-0.00029, "ay":0.00001, "alpha":0.02065, "fx":[0.04005,-0.0241,-0.04829,0.01586], "fy":[0.02009,0.04428,-0.01987,-0.04406]},
+ {"t":7.75971, "x":6.91287, "y":4.75195, "heading":-1.923, "vx":-0.06665, "vy":-2.49858, "omega":0.17918, "ax":-0.00029, "ay":0.00001, "alpha":0.01163, "fx":[0.02071,-0.01555,-0.02888,0.00738], "fy":[0.01158,0.0249,-0.01136,-0.02468]},
+ {"t":7.80657, "x":6.90975, "y":4.63487, "heading":-1.91459, "vx":-0.06666, "vy":-2.49858, "omega":0.17972, "ax":-0.00029, "ay":0.00001, "alpha":0.00811, "fx":[0.01316,-0.0122,-0.02127,0.00408], "fy":[0.00825,0.01733,-0.00803,-0.01711]},
+ {"t":7.85343, "x":6.90662, "y":4.51779, "heading":-1.90616, "vx":-0.06668, "vy":-2.49858, "omega":0.1801, "ax":-0.00028, "ay":0.00001, "alpha":0.00863, "fx":[0.01422,-0.01284,-0.02228,0.00479], "fy":[0.00893,0.01836,-0.00871,-0.01814]},
+ {"t":7.90029, "x":6.9035, "y":4.40071, "heading":-1.89771, "vx":-0.06669, "vy":-2.49858, "omega":0.18051, "ax":-0.00028, "ay":0.00001, "alpha":0.01338, "fx":[0.02416,-0.0179,-0.03216,0.0099], "fy":[0.01401,0.02827,-0.01379,-0.02805]},
+ {"t":7.94715, "x":6.90037, "y":4.28363, "heading":-1.88924, "vx":-0.0667, "vy":-2.49858, "omega":0.18113, "ax":-0.00028, "ay":0.00001, "alpha":0.02412, "fx":[0.04659,-0.02946,-0.05453,0.02152], "fy":[0.0256,0.05067,-0.02538,-0.05045]},
+ {"t":7.99401, "x":6.89725, "y":4.16654, "heading":-1.88072, "vx":-0.06672, "vy":-2.49858, "omega":0.18226, "ax":-0.00028, "ay":0.00001, "alpha":0.04578, "fx":[0.09162,-0.05314,-0.0995,0.04526], "fy":[0.04931,0.09566,-0.04909,-0.09545]},
+ {"t":8.04087, "x":6.89412, "y":4.04946, "heading":-1.87213, "vx":-0.06673, "vy":-2.49858, "omega":0.18441, "ax":-0.00028, "ay":0.00001, "alpha":0.08761, "fx":[0.17815,-0.09964,-0.18598,0.09181], "fy":[0.09583,0.18217,-0.09562,-0.18196]},
+ {"t":8.08773, "x":6.89099, "y":3.93238, "heading":-1.86339, "vx":-0.06674, "vy":-2.49858, "omega":0.18852, "ax":-0.00027, "ay":0.00001, "alpha":0.16754, "fx":[0.34266,-0.18998,-0.35044,0.18221], "fy":[0.1862,0.34665,-0.18599,-0.34645]},
+ {"t":8.13459, "x":6.88786, "y":3.8153, "heading":-1.85438, "vx":-0.06676, "vy":-2.49858, "omega":0.19637, "ax":-0.00027, "ay":0.00001, "alpha":0.31994, "fx":[0.65469,-0.36518,-0.66241,0.35747], "fy":[0.36143,0.65866,-0.36122,-0.65845]},
+ {"t":8.18145, "x":6.88474, "y":3.69822, "heading":-1.84482, "vx":-0.06677, "vy":-2.49858, "omega":0.21136, "ax":-0.00027, "ay":0.00001, "alpha":0.6108, "fx":[1.24679,-0.70563,-1.25445,0.69796], "fy":[0.7019,1.25072,-0.70169,-1.25051]},
+ {"t":8.22831, "x":6.88161, "y":3.58114, "heading":-1.83425, "vx":-0.06678, "vy":-2.49858, "omega":0.23998, "ax":-0.00027, "ay":0.00001, "alpha":1.16585, "fx":[2.36901,-1.36849,-2.37657,1.36092], "fy":[1.36481,2.37289,-1.3646,-2.37269]},
+ {"t":8.27517, "x":6.87848, "y":3.46406, "heading":-1.82172, "vx":-0.06679, "vy":-2.49857, "omega":0.29461, "ax":-0.00024, "ay":0.00001, "alpha":2.22585, "fx":[4.49377,-2.66542,-4.50053,2.65865], "fy":[2.66213,4.49723,-2.66194,-4.49706]},
+ {"t":8.32203, "x":6.87535, "y":3.34698, "heading":-1.80547, "vx":-0.06681, "vy":-2.49857, "omega":0.39891, "ax":0.00033, "ay":-0.00001, "alpha":4.2502, "fx":[8.50818,-5.2172,-8.49874,5.22668], "fy":[5.22179,8.50335,-5.22208,-8.50356]},
+ {"t":8.36888, "x":6.87222, "y":3.22989, "heading":-1.78212, "vx":-0.06679, "vy":-2.49857, "omega":0.59807, "ax":0.01232, "ay":-0.00033, "alpha":8.09724, "fx":[16.1369,-10.14828,-15.79011,10.50012], "fy":[10.31658,15.96193,-10.3319,-15.96518]},
+ {"t":8.41574, "x":6.8691, "y":3.11281, "heading":-1.7452, "vx":-0.06621, "vy":-2.49859, "omega":0.9775, "ax":0.24063, "ay":-0.00583, "alpha":15.21329, "fx":[32.57857,-16.99336,-25.90045,23.95884], "fy":[20.20958,29.40444,-20.80265,-29.14185]},
+ {"t":8.4626, "x":6.86626, "y":2.99572, "heading":-1.68269, "vx":-0.05494, "vy":-2.49886, "omega":1.69039, "ax":3.3485, "ay":0.03164, "alpha":22.73661, "fx":[82.96396,19.74267,9.6453,77.50493], "fy":[31.1831,54.28481,-44.90892,-38.7648]},
+ {"t":8.50946, "x":6.86736, "y":2.87866, "heading":-1.57852, "vx":0.10197, "vy":-2.49738, "omega":2.75581, "ax":8.96243, "ay":1.14238, "alpha":7.68481, "fx":[127.58829,120.89189,128.24716,131.43401], "fy":[33.06722,45.41447,-7.82418,-5.88566]},
+ {"t":8.55632, "x":6.88198, "y":2.76289, "heading":-1.44095, "vx":0.52194, "vy":-2.44385, "omega":3.11591, "ax":4.02251, "ay":8.48306, "alpha":10.67405, "fx":[66.61664,16.51802,43.8757,101.06229], "fy":[120.87137,136.83875,129.77948,93.49209]},
+ {"t":8.60318, "x":6.91086, "y":2.65769, "heading":-1.28322, "vx":0.71044, "vy":-2.04634, "omega":3.61609, "ax":2.54403, "ay":9.04967, "alpha":10.69654, "fx":[45.81735,-6.41275,20.72871,84.11063], "fy":[130.32802,137.84696,135.71069,109.22201]},
+ {"t":8.63922, "x":6.93812, "y":2.58981, "heading":-1.14594, "vx":0.80213, "vy":-1.72017, "omega":4.00162, "ax":2.86427, "ay":9.04394, "alpha":8.32203, "fx":[43.19877,3.7722,38.43202,76.9985], "fy":[130.68789,137.30961,131.08782,113.69717]},
+ {"t":8.67527, "x":6.96889, "y":2.53369, "heading":-0.9963, "vx":0.90537, "vy":-1.3942, "omega":4.30157, "ax":1.74822, "ay":9.21383, "alpha":7.31157, "fx":[25.96954,-8.08158,23.91514,57.31947], "fy":[133.34856,134.9822,131.65485,122.42952]},
+ {"t":8.71131, "x":7.00265, "y":2.48942, "heading":-0.83652, "vx":0.96838, "vy":-1.06211, "omega":4.5651, "ax":2.79545, "ay":8.2716, "alpha":2.78906, "fx":[37.28723,28.49987,42.55441,50.15775], "fy":[119.56927,120.4374,115.06352,113.92167]},
+ {"t":8.74735, "x":7.03937, "y":2.45651, "heading":-0.67017, "vx":1.06913, "vy":-0.76398, "omega":4.66562, "ax":8.21261, "ay":1.83131, "alpha":-6.77007, "fx":[121.1291,121.79522,110.14732,112.57543], "fy":[-1.8107,24.61961,52.1912,28.83366]},
+ {"t":8.78339, "x":7.08324, "y":2.43017, "heading":-0.5064, "vx":1.36514, "vy":-0.69797, "omega":4.42161, "ax":8.33605, "ay":2.75307, "alpha":-7.10779, "fx":[126.33585,122.64311,107.68922,115.97804], "fy":[9.56504,39.41777,67.15141,39.96212]},
+ {"t":8.81944, "x":7.13786, "y":2.4068, "heading":-0.35165, "vx":1.66559, "vy":-0.59874, "omega":4.16543, "ax":6.94014, "ay":5.6022, "alpha":-6.24373, "fx":[114.15621,104.42148,82.70328,92.21832], "fy":[57.13968,77.08236,98.58138,84.8358]},
+ {"t":8.85548, "x":7.2024, "y":2.38886, "heading":-0.20557, "vx":1.91573, "vy":-0.39683, "omega":3.94039, "ax":4.90752, "ay":7.13695, "alpha":-4.92466, "fx":[85.26243,76.78317,55.16231,61.04374], "fy":[88.61155,99.48815,111.85804,104.70034]},
+ {"t":8.89152, "x":7.27464, "y":2.37919, "heading":-0.06675, "vx":2.09261, "vy":-0.13959, "omega":3.76289, "ax":-0.37417, "ay":7.86283, "alpha":0.46609, "fx":[-6.42544,-6.71159,-4.15992,-3.91797], "fy":[111.70542,111.14427,111.20438,111.76073]},
+ {"t":8.92756, "x":7.34982, "y":2.37927, "heading":0.06918, "vx":2.07912, "vy":0.14381, "omega":3.77969, "ax":-5.12742, "ay":7.06128, "alpha":5.1707, "fx":[-77.90966,-89.73358,-66.91699,-56.15968], "fy":[99.96558,86.19043,102.04676,112.16491]},
+ {"t":8.96361, "x":7.42142, "y":2.38904, "heading":0.20876, "vx":1.89432, "vy":0.39831, "omega":3.96605, "ax":-7.28119, "ay":5.1268, "alpha":6.58463, "fx":[-106.74117,-118.60218,-101.36748,-86.12551], "fy":[74.06119,48.51332,73.00929,95.10068]},
+ {"t":8.99965, "x":7.48497, "y":2.40672, "heading":0.35599, "vx":1.63189, "vy":0.5831, "omega":4.20338, "ax":-8.68074, "ay":-0.31128, "alpha":7.36682, "fx":[-127.4577,-123.39908,-119.93235,-121.40066], "fy":[7.05517,-29.0321,-21.30612,25.63396]},
+ {"t":9.03569, "x":7.53815, "y":2.42754, "heading":0.51227, "vx":1.31901, "vy":0.57188, "omega":4.4689, "ax":-6.8955, "ay":-5.08776, "alpha":5.85357, "fx":[-109.38193,-92.26505,-83.46724,-105.85433], "fy":[-59.58687,-84.19573,-87.62119,-57.06731]},
+ {"t":9.07174, "x":7.58121, "y":2.44484, "heading":0.67715, "vx":1.07048, "vy":0.3885, "omega":4.67988, "ax":-3.74151, "ay":-7.06687, "alpha":3.99313, "fx":[-65.78523,-49.83392,-39.078,-57.44306], "fy":[-94.57362,-105.34764,-106.01444,-94.7493]},
+ {"t":9.10778, "x":7.61736, "y":2.45426, "heading":0.84842, "vx":0.93562, "vy":0.13379, "omega":4.8238, "ax":5.13163, "ay":3.41583, "alpha":-7.69914, "fx":[86.71439,66.31342,56.74853,81.18246], "fy":[44.42903,68.08396,55.30928,25.85181]},
+ {"t":9.14382, "x":7.65442, "y":2.4613, "heading":1.01728, "vx":1.12058, "vy":0.25691, "omega":4.54631, "ax":2.11215, "ay":8.8954, "alpha":-6.6094, "fx":[52.68884,19.81983,2.18901,45.05914], "fy":[121.12286,131.06799,130.67702,121.49291]},
+ {"t":9.17986, "x":7.69618, "y":2.47633, "heading":1.17685, "vx":1.19671, "vy":0.57752, "omega":4.30808, "ax":0.40352, "ay":9.44175, "alpha":-6.95473, "fx":[31.27352,-5.25634,-25.31317,22.17492], "fy":[132.54207,136.29676,133.24265,133.25682]},
+ {"t":9.21591, "x":7.73957, "y":2.50328, "heading":1.3276, "vx":1.21125, "vy":0.91783, "omega":4.05742, "ax":-0.72044, "ay":9.49176, "alpha":-7.68427, "fx":[17.09092,-23.25175,-44.25962,9.57212], "fy":[136.38343,135.58378,129.75513,136.45156]},
+ {"t":9.25195, "x":7.78276, "y":2.54253, "heading":1.46885, "vx":1.18528, "vy":1.25994, "omega":3.78046, "ax":-1.55678, "ay":9.40275, "alpha":-8.40356, "fx":[5.79437,-37.51779,-58.16637,1.62185], "fy":[137.87863,132.86735,124.84656,137.53431]},
+ {"t":9.28799, "x":7.82447, "y":2.59405, "heading":1.59965, "vx":1.12917, "vy":1.59884, "omega":3.47757, "ax":-2.22094, "ay":9.26175, "alpha":-9.03586, "fx":[-3.97943,-49.30346,-68.6781,-3.96423], "fy":[138.22913,129.23599,119.7723,137.89505]},
+ {"t":9.32403, "x":7.86373, "y":2.65769, "heading":1.71912, "vx":1.04913, "vy":1.93265, "omega":3.15189, "ax":-3.57201, "ay":8.82739, "alpha":-9.03898, "fx":[-23.68992,-67.27292,-86.17832,-25.38854], "fy":[136.22762,120.85029,107.85776,135.56879]},
+ {"t":9.36957, "x":7.90779, "y":2.75484, "heading":1.85327, "vx":0.88648, "vy":2.33459, "omega":2.74033, "ax":-9.18972, "ay":2.64906, "alpha":-3.88739, "fx":[-124.69039,-130.11823,-134.60395,-131.6356], "fy":[54.5732,41.17719,21.20751,33.24117]},
+ {"t":9.4151, "x":7.93863, "y":2.86388, "heading":1.97401, "vx":0.46805, "vy":2.45521, "omega":2.56332, "ax":-8.56755, "ay":0.94494, "alpha":-8.45277, "fx":[-114.47664,-126.04837,-125.08812,-120.15865], "fy":[49.00013,21.86238,-17.83412,0.54875]},
+ {"t":9.46063, "x":7.95106, "y":2.97666, "heading":2.08196, "vx":0.07795, "vy":2.49823, "omega":2.17845, "ax":-2.18128, "ay":0.02476, "alpha":-25.40257, "fx":[-20.35162,-82.09777,-50.30037,29.07316], "fy":[62.53824,14.88283,-55.82493,-20.19221]},
+ {"t":9.50616, "x":7.95235, "y":3.09043, "heading":2.15482, "vx":-0.02137, "vy":2.49936, "omega":1.0218, "ax":-0.168, "ay":-0.00169, "alpha":-17.13811, "fx":[5.48031,-41.61412,-10.60891,37.21712], "fy":[39.49002,7.94747,-39.3776,-8.15579]},
+ {"t":9.5517, "x":7.9512, "y":3.20423, "heading":2.18358, "vx":-0.02902, "vy":2.49928, "omega":0.24146, "ax":-0.00992, "ay":-0.00012, "alpha":-9.76554, "fx":[3.79387,-22.7243,-4.0822,22.45033], "fy":[22.58705,3.93506,-22.58767,-3.94103]},
+ {"t":9.59723, "x":7.94987, "y":3.31803, "heading":2.18445, "vx":-0.02947, "vy":2.49928, "omega":-0.20319, "ax":-0.00059, "ay":-0.00001, "alpha":-5.27006, "fx":[2.10616,-12.1996,-2.123,12.18301], "fy":[12.19123,2.11445,-12.19139,-2.1147]},
+ {"t":9.64276, "x":7.94853, "y":3.43183, "heading":2.16974, "vx":-0.0295, "vy":2.49928, "omega":-0.44315, "ax":-0.0001, "ay":0.0, "alpha":-2.76927, "fx":[1.20389,-6.39054,-1.20669,6.38775], "fy":[6.38913,1.20527,-6.38916,-1.20531]},
+ {"t":9.68829, "x":7.94719, "y":3.54563, "heading":2.14669, "vx":-0.0295, "vy":2.49928, "omega":-0.56924, "ax":-0.00007, "ay":0.0, "alpha":-1.43746, "fx":[0.70087,-3.30218,-0.70294,3.30012], "fy":[3.30114,0.70189,-3.30116,-0.70192]},
+ {"t":9.73383, "x":7.94584, "y":3.65943, "heading":2.11928, "vx":-0.02951, "vy":2.49928, "omega":-0.63469, "ax":-0.00007, "ay":0.0, "alpha":-0.74244, "fx":[0.40812,-1.69544,-0.41012,1.69345], "fy":[1.69443,0.40911,-1.69446,-0.40913]},
+ {"t":9.77936, "x":7.9445, "y":3.77322, "heading":2.08961, "vx":-0.02951, "vy":2.49928, "omega":-0.66849, "ax":-0.00007, "ay":0.0, "alpha":-0.38309, "fx":[0.23597,-0.86865,-0.23792,0.8667], "fy":[0.86766,0.23693,-0.86769,-0.23696]},
+ {"t":9.82489, "x":7.94315, "y":3.88702, "heading":2.05878, "vx":-0.02951, "vy":2.49928, "omega":-0.68594, "ax":-0.00007, "ay":0.0, "alpha":-0.19806, "fx":[0.13531,-0.44557,-0.13724,0.44364], "fy":[0.44459,0.13626,-0.44462,-0.13629]},
+ {"t":9.87042, "x":7.94181, "y":4.00082, "heading":2.02734, "vx":-0.02952, "vy":2.49928, "omega":-0.69496, "ax":-0.00007, "ay":0.0, "alpha":-0.10295, "fx":[0.07712,-0.22971,-0.07901,0.22782], "fy":[0.22875,0.07805,-0.22877,-0.07808]},
+ {"t":9.91596, "x":7.94047, "y":4.11462, "heading":1.99559, "vx":-0.02952, "vy":2.49928, "omega":-0.69964, "ax":-0.00007, "ay":0.0, "alpha":-0.05394, "fx":[0.04376,-0.11943,-0.04562,0.11757], "fy":[0.11849,0.04468,-0.11851,-0.0447]},
+ {"t":9.96149, "x":7.93912, "y":4.22842, "heading":1.96368, "vx":-0.02952, "vy":2.49928, "omega":-0.7021, "ax":-0.00006, "ay":0.0, "alpha":-0.02834, "fx":[0.02454,-0.0624,-0.02637,0.06057], "fy":[0.06148,0.02545,-0.0615,-0.02547]},
+ {"t":10.00702, "x":7.93778, "y":4.34222, "heading":1.93168, "vx":-0.02952, "vy":2.49928, "omega":-0.70339, "ax":-0.00006, "ay":0.0, "alpha":-0.01451, "fx":[0.01313,-0.03194,-0.01493,0.03014], "fy":[0.03103,0.01402,-0.03105,-0.01404]},
+ {"t":10.05255, "x":7.93643, "y":4.45601, "heading":1.89964, "vx":-0.02953, "vy":2.49928, "omega":-0.70405, "ax":-0.00006, "ay":0.0, "alpha":-0.00609, "fx":[0.00542,-0.01372,-0.00719,0.01195], "fy":[0.01283,0.00629,-0.01284,-0.00631]},
+ {"t":10.09809, "x":7.93509, "y":4.56981, "heading":1.86757, "vx":-0.02953, "vy":2.49928, "omega":-0.70433, "ax":-0.00006, "ay":0.0, "alpha":0.00085, "fx":[-0.0018,0.00089,0.00006,-0.00262], "fy":[-0.00176,-0.00094,0.00175,0.00092]},
+ {"t":10.14362, "x":7.93374, "y":4.68361, "heading":1.83551, "vx":-0.02953, "vy":2.49928, "omega":-0.70429, "ax":-0.00006, "ay":0.0, "alpha":0.00946, "fx":[-0.0119,0.01841,0.0102,-0.02012], "fy":[-0.01928,-0.01106,0.01926,0.01104]},
+ {"t":10.18915, "x":7.9324, "y":4.79741, "heading":1.80345, "vx":-0.02954, "vy":2.49928, "omega":-0.70386, "ax":-0.00006, "ay":0.0, "alpha":0.02343, "fx":[-0.02971,0.04598,0.02804,-0.04765], "fy":[-0.04682,-0.02889,0.0468,0.02887]},
+ {"t":10.23468, "x":7.93106, "y":4.91121, "heading":1.77142, "vx":-0.02954, "vy":2.49928, "omega":-0.70279, "ax":-0.00006, "ay":0.0, "alpha":0.04899, "fx":[-0.06431,0.09509,0.06267,-0.09673], "fy":[-0.09592,-0.0635,0.0959,0.06349]},
+ {"t":10.28022, "x":7.92971, "y":5.02501, "heading":1.73947, "vx":-0.02954, "vy":2.49928, "omega":-0.70056, "ax":-0.00006, "ay":0.0, "alpha":0.09793, "fx":[-0.13378,0.18676,0.13217,-0.18836], "fy":[-0.18757,-0.13298,0.18755,0.13297]},
+ {"t":10.32575, "x":7.92837, "y":5.1388, "heading":1.70768, "vx":-0.02954, "vy":2.49928, "omega":-0.6961, "ax":-0.00006, "ay":0.0, "alpha":0.19166, "fx":[-0.27258,0.35785,0.27101,-0.35942], "fy":[-0.35864,-0.2718,0.35863,0.27179]},
+ {"t":10.37128, "x":7.92702, "y":5.2526, "heading":1.67618, "vx":-0.02955, "vy":2.49928, "omega":-0.68738, "ax":-0.00005, "ay":0.0, "alpha":0.37341, "fx":[-0.55204,0.68093,0.55051,-0.68246], "fy":[-0.6817,-0.55128,0.68169,0.55127]},
+ {"t":10.41681, "x":7.92567, "y":5.3664, "heading":1.64527, "vx":-0.02955, "vy":2.49928, "omega":-0.67037, "ax":-0.00005, "ay":0.0, "alpha":0.72434, "fx":[-1.11047,1.28792,1.10898,-1.28942], "fy":[-1.28867,-1.10973,1.28867,1.10972]},
+ {"t":10.46235, "x":7.92433, "y":5.4802, "heading":1.6155, "vx":-0.02955, "vy":2.49928, "omega":-0.63739, "ax":-0.00005, "ay":0.0, "alpha":1.40037, "fx":[-2.2194,2.4257,2.21792,-2.42717], "fy":[-2.42644,-2.21867,2.42643,2.21866]},
+ {"t":10.50788, "x":7.92298, "y":5.594, "heading":1.58793, "vx":-0.02955, "vy":2.49928, "omega":-0.57363, "ax":-0.00006, "ay":0.0, "alpha":2.6919, "fx":[-4.39274,4.54406,4.39096,-4.54583], "fy":[-4.54495,-4.39186,4.54494,4.39184]},
+ {"t":10.55341, "x":7.92164, "y":5.7078, "heading":1.5646, "vx":-0.02956, "vy":2.49928, "omega":-0.45106, "ax":-0.00029, "ay":0.0, "alpha":5.11506, "fx":[-8.54857,8.43497,8.54026,-8.44329], "fy":[-8.43914,-8.54449,8.43912,8.54434]},
+ {"t":10.59894, "x":7.92029, "y":5.82159, "heading":1.54936, "vx":-0.02957, "vy":2.49928, "omega":-0.21816, "ax":-0.00472, "ay":-0.00006, "alpha":9.48271, "fx":[-16.14368,15.33497,16.01001,-15.46902], "fy":[-15.40108,-16.07936,15.40293,16.07435]},
+ {"t":10.64448, "x":7.91894, "y":5.93539, "heading":1.54926, "vx":-0.02978, "vy":2.49927, "omega":0.21361, "ax":-0.08049, "ay":-0.00102, "alpha":16.6997, "fx":[-29.44904,25.97307,27.17729,-28.26508], "fy":[-27.04668,-28.42252,27.19932,28.2121]},
+ {"t":10.69001, "x":7.9175, "y":6.04919, "heading":1.5763, "vx":-0.03345, "vy":2.49923, "omega":0.97399, "ax":-1.07759, "ay":-0.02503, "alpha":26.43371, "fx":[-58.23797,28.19729,27.75747,-58.81497], "fy":[-42.03079,-47.71171,47.46059,40.86249]},
+ {"t":10.73554, "x":7.91486, "y":6.16296, "heading":1.64804, "vx":-0.08251, "vy":2.49809, "omega":2.17758, "ax":-7.15734, "ay":-0.70823, "alpha":16.93769, "fx":[-111.95999,-82.90143,-93.28478,-117.66835], "fy":[-42.22804,-63.22479,44.3947,20.90198]},
+ {"t":10.78107, "x":7.90368, "y":6.27597, "heading":1.76475, "vx":-0.40841, "vy":2.46584, "omega":2.9488, "ax":-9.11711, "ay":-2.36794, "alpha":6.28299, "fx":[-126.30293,-122.19755,-133.83982,-134.59127], "fy":[-49.33048,-56.26773,-12.55832,-16.10334]},
+ {"t":10.82661, "x":7.87564, "y":6.38579, "heading":1.90553, "vx":-0.82353, "vy":2.35802, "omega":3.23488, "ax":-5.59858, "ay":-7.62941, "alpha":6.41059, "fx":[-76.96934,-54.23851,-85.19495,-101.03144], "fy":[-112.52942,-124.42464,-104.63985,-90.98648]},
+ {"t":10.868, "x":7.83675, "y":6.47686, "heading":2.04492, "vx":-1.05527, "vy":2.04222, "omega":3.50023, "ax":-7.06588, "ay":-5.77175, "alpha":0.89624, "fx":[-99.02807,-97.91378,-101.36511,-102.32167], "fy":[-83.58289,-84.30226,-79.97739,-79.39008]},
+ {"t":10.90939, "x":7.78702, "y":6.55645, "heading":2.19057, "vx":-1.34774, "vy":1.80332, "omega":3.53733, "ax":-8.80086, "ay":-1.19516, "alpha":-6.08234, "fx":[-126.55862,-128.75521,-121.72529,-121.96109], "fy":[9.22885,-10.78128,-40.144,-26.06781]},
+ {"t":10.95078, "x":7.7237, "y":6.63007, "heading":2.33178, "vx":-1.71203, "vy":1.75385, "omega":3.28557, "ax":-8.72843, "ay":-2.08763, "alpha":-6.57043, "fx":[-128.59998,-128.3774,-117.49428,-120.42176], "fy":[-1.07678,-24.30907,-54.77706,-38.2037]},
+ {"t":10.99218, "x":7.64535, "y":6.70087, "heading":2.46215, "vx":-2.07331, "vy":1.66744, "omega":3.0136, "ax":-7.92032, "ay":-3.96993, "alpha":-5.77689, "fx":[-121.7121,-118.22175,-102.95352,-106.18746], "fy":[-33.6926,-50.87108,-75.70612,-64.8215]},
+ {"t":11.03357, "x":7.55275, "y":6.76649, "heading":2.58194, "vx":-2.40115, "vy":1.50311, "omega":2.77448, "ax":-5.61856, "ay":-5.77352, "alpha":-3.04633, "fx":[-86.24479,-85.38363,-73.6149,-73.3239], "fy":[-73.55442,-79.05221,-89.39495,-85.35155]},
+ {"t":11.07496, "x":7.44855, "y":6.82376, "heading":2.69417, "vx":-2.63372, "vy":1.26413, "omega":2.64839, "ax":-0.20627, "ay":-8.31683, "alpha":4.23396, "fx":[2.47578,13.74707,-9.71587,-18.20223], "fy":[-120.85618,-117.01696,-115.39123,-118.29189]},
+ {"t":11.11635, "x":7.33936, "y":6.86896, "heading":2.80742, "vx":-2.64226, "vy":0.91988, "omega":2.82364, "ax":2.42249, "ay":-8.47697, "alpha":6.24783, "fx":[39.11487,59.96146,28.38977,9.88675], "fy":[-122.50732,-111.22345,-120.11866,-126.78675]},
+ {"t":11.15774, "x":7.23206, "y":6.89978, "heading":2.92965, "vx":-2.54198, "vy":0.569, "omega":3.08225, "ax":6.68006, "ay":-5.23093, "alpha":8.03418, "fx":[93.35286,112.95196,100.68402,71.76403], "fy":[-84.58194,-52.1796,-61.20757,-98.61946]},
+ {"t":11.19914, "x":7.13257, "y":6.91885, "heading":3.06411, "vx":-2.26548, "vy":0.35248, "omega":3.4148, "ax":8.35717, "ay":0.52724, "alpha":6.25151, "fx":[121.89369,121.01976,114.35105,116.5788], "fy":[-10.32016,20.85958,29.51338,-10.15887]},
+ {"t":11.24053, "x":7.04595, "y":6.93389, "heading":-3.07237, "vx":-1.91956, "vy":0.37431, "omega":3.67357, "ax":6.77116, "ay":5.11447, "alpha":2.7577, "fx":[102.70824,96.04527,88.79535,96.36969], "fy":[63.91788,75.26938,81.19532,69.60318]},
+ {"t":11.28192, "x":6.9723, "y":6.95376, "heading":-2.91795, "vx":-1.63929, "vy":0.58601, "omega":3.78772, "ax":3.83381, "ay":7.65674, "alpha":-0.26271, "fx":[53.38175,54.18349,55.30497,54.50296], "fy":[109.02816,108.45176,108.03339,108.61654]},
+ {"t":11.32331, "x":6.90773, "y":6.98458, "heading":-2.7614, "vx":-1.4806, "vy":0.90294, "omega":3.77684, "ax":0.27289, "ay":8.54611, "alpha":-3.01631, "fx":[-7.33265,-1.13572,15.81326,8.12796], "fy":[121.98859,119.87516,119.97661,122.71616]},
+ {"t":11.36471, "x":6.84668, "y":7.02928, "heading":-2.60765, "vx":-1.4693, "vy":1.25668, "omega":3.65199, "ax":-3.78678, "ay":7.58955, "alpha":-5.97531, "fx":[-69.3364,-68.72916,-34.24146,-42.39998], "fy":[102.34206,97.15357,114.47445,116.35027]},
+ {"t":11.4061, "x":6.78262, "y":7.08779, "heading":-2.4616, "vx":-1.62605, "vy":1.57083, "omega":3.40466, "ax":-7.44013, "ay":4.09916, "alpha":-8.19926, "fx":[-115.32201,-118.86184,-94.5301,-93.13413], "fy":[51.12549,27.09473,71.0746,83.12391]},
+ {"t":11.44749, "x":6.70894, "y":7.15633, "heading":-2.3277, "vx":-1.93401, "vy":1.7405, "omega":3.06527, "ax":-8.67526, "ay":-1.27624, "alpha":-7.55256, "fx":[-128.07437,-117.61131,-120.42286,-125.7704], "fy":[-15.61578,-48.63745,-22.74804,14.63987]},
+ {"t":11.48888, "x":6.62145, "y":7.22727, "heading":-2.20729, "vx":-2.2931, "vy":1.68767, "omega":2.75266, "ax":-7.76514, "ay":-3.96381, "alpha":-6.17292, "fx":[-116.42912,-99.93927,-103.67702,-120.23041], "fy":[-51.45223,-76.91861,-64.1909,-32.18255]},
+ {"t":11.53027, "x":6.51989, "y":7.29374, "heading":-2.09864, "vx":-2.61451, "vy":1.5236, "omega":2.49715, "ax":-5.91667, "ay":-5.24763, "alpha":-4.17623, "fx":[-90.65387,-75.87451,-76.316,-92.62507], "fy":[-71.54038,-85.55912,-78.52519,-61.91085]},
+ {"t":11.57167, "x":6.4066, "y":7.35231, "heading":-1.99886, "vx":-2.85942, "vy":1.30639, "omega":2.32428, "ax":-2.47044, "ay":-6.78679, "alpha":0.3172, "fx":[-34.11667,-35.63469,-35.91072,-34.40971], "fy":[-96.29406,-95.70135,-96.1111,-96.69811]},
+ {"t":11.61306, "x":6.28612, "y":7.40057, "heading":-1.90238, "vx":-2.96167, "vy":1.02547, "omega":2.33741, "ax":-1.79642, "ay":-6.34329, "alpha":-0.14969, "fx":[-25.86405,-25.17628,-25.06199,-25.75308], "fy":[-89.93466,-90.14516,-89.89517,-89.68375]},
+ {"t":11.65445, "x":6.16199, "y":7.43758, "heading":-1.80576, "vx":-3.03603, "vy":0.76291, "omega":2.33122, "ax":-6.55279, "ay":3.00793, "alpha":-13.28577, "fx":[-113.66118,-96.93425,-65.64522,-95.2964], "fy":[16.84372,6.43633,80.53064,66.73585]},
+ {"t":11.69584, "x":6.03071, "y":7.47173, "heading":-1.72064, "vx":-3.30726, "vy":0.88741, "omega":1.78129, "ax":-8.20038, "ay":2.3179, "alpha":-14.06157, "fx":[-130.63544,-123.2625,-94.55845,-116.4974], "fy":[1.70777,-13.16346,82.97824,59.89994]},
+ {"t":11.73723, "x":5.88679, "y":7.51045, "heading":-1.65896, "vx":-3.6467, "vy":0.98336, "omega":1.19925, "ax":-8.94411, "ay":0.11613, "alpha":-13.55669, "fx":[-132.50224,-121.99157,-122.08209,-130.5464], "fy":[-25.77292,-51.31223,49.73907,33.93062]},
+ {"t":11.77863, "x":5.72819, "y":7.55125, "heading":-1.62093, "vx":-4.01691, "vy":0.98816, "omega":0.63811, "ax":-8.9647, "ay":-1.6896, "alpha":-13.2486, "fx":[-128.46743,-110.29198,-133.50986,-136.02061], "fy":[-46.96462,-78.77532,17.33735,12.60415]},
+ {"t":11.82002, "x":5.55424, "y":7.59071, "heading":-1.60587, "vx":-4.38798, "vy":0.91823, "omega":0.08973, "ax":-2.20921, "ay":-9.07384, "alpha":-1.48417, "fx":[-34.79517,-26.09841,-27.56535,-36.80138], "fy":[-128.07726,-130.0451,-129.28587,-127.07008]},
+ {"t":11.8804, "x":5.28525, "y":7.62961, "heading":-1.60316, "vx":-4.52138, "vy":0.37031, "omega":0.00011, "ax":-0.25167, "ay":-6.01349, "alpha":-0.00038, "fx":[-3.56817,-3.56644,-3.56642,-3.56816], "fy":[-85.24016,-85.24022,-85.23944,-85.23937]},
+ {"t":11.94079, "x":5.01177, "y":7.64101, "heading":-1.60315, "vx":-4.53658, "vy":0.00719, "omega":0.00008, "ax":0.00053, "ay":-0.14057, "alpha":-0.00019, "fx":[0.00715,0.00779,0.00781,0.00717], "fy":[-1.99283,-1.99285,-1.99221,-1.99219]},
+ {"t":12.00117, "x":4.73783, "y":7.64119, "heading":-1.60315, "vx":-4.53655, "vy":-0.0013, "omega":0.00007, "ax":6.67, "ay":1.05897, "alpha":2.8459, "fx":[96.60222,91.26331,92.60288,97.71408], "fy":[20.63826,23.04763,8.78848,7.56815]},
+ {"t":12.06156, "x":4.47606, "y":7.64304, "heading":-1.59795, "vx":-4.13378, "vy":0.06265, "omega":0.17192, "ax":9.75045, "ay":-0.00981, "alpha":-0.63317, "fx":[138.19792,138.2179,138.22419,138.20107], "fy":[-2.20657,-2.25302,1.8657,2.0379]},
+ {"t":12.12194, "x":4.24422, "y":7.6468, "heading":-1.58873, "vx":-3.54501, "vy":0.06206, "omega":0.13369, "ax":9.77451, "ay":-0.04981, "alpha":-0.60365, "fx":[138.53605,138.54612,138.56705,138.55609], "fy":[-2.70782,-2.70166,1.23837,1.3472]},
+ {"t":12.18232, "x":4.04797, "y":7.65046, "heading":-1.58176, "vx":-2.95478, "vy":0.05905, "omega":0.09723, "ax":9.7826, "ay":-0.08637, "alpha":-0.52191, "fx":[138.64753,138.65403,138.68429,138.67837], "fy":[-2.97182,-2.93618,0.48023,0.53072]},
+ {"t":12.24271, "x":3.88739, "y":7.65387, "heading":-1.57684, "vx":-2.36407, "vy":0.05383, "omega":0.06572, "ax":9.78647, "ay":-0.12695, "alpha":-0.42694, "fx":[138.70017,138.70482,138.74095,138.73745], "fy":[-3.23929,-3.19088,-0.38866,-0.37939]},
+ {"t":12.30309, "x":3.76248, "y":7.65689, "heading":-1.57365, "vx":-1.77312, "vy":0.04617, "omega":0.03994, "ax":9.78835, "ay":-0.17653, "alpha":-0.32589, "fx":[138.72632,138.72976,138.76815,138.76583], "fy":[-3.60776,-3.55819,-1.41372,-1.42969]},
+ {"t":12.36348, "x":3.67325, "y":7.65935, "heading":-1.57183, "vx":-1.18206, "vy":0.03551, "omega":0.02026, "ax":9.7887, "ay":-0.2433, "alpha":-0.22037, "fx":[138.73317,138.73566,138.77158,138.76983], "fy":[-4.20062,-4.15817,-2.70457,-2.73167]},
+ {"t":12.42386, "x":3.61972, "y":7.66105, "heading":-1.57101, "vx":-0.59098, "vy":0.02081, "omega":0.00695, "ax":9.78692, "ay":-0.34471, "alpha":-0.11515, "fx":[138.71303,138.71466,138.74138,138.74004], "fy":[-5.28213,-5.25333,-4.49222,-4.51683]},
+ {"t":12.48424, "x":3.60188, "y":7.66168, "heading":-1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+ "splits":[0]
+ },
+ "events":[
+ {"name":"Marker", "from":{"target":0, "targetTimestamp":0.0, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"hoodDown"}}},
+ {"name":"Marker", "from":{"target":1, "targetTimestamp":1.51517, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"hoodUp"}}},
+ {"name":"Marker", "from":{"target":10, "targetTimestamp":11.82002, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"hoodDown"}}},
+ {"name":"Marker", "from":{"target":11, "targetTimestamp":12.48424, "offset":{"exp":"-0.2 s", "val":-0.2}}, "event":{"type":"named", "data":{"name":"hoodUp"}}}]
+}
--- /dev/null
+{
+ "version": 1.0,
+ "grid_size": 128,
+ "tabs": [
+ {
+ "name": "Teleoperated",
+ "grid_layout": {
+ "layouts": [],
+ "containers": [
+ {
+ "title": "Autos Result",
+ "x": 1408.0,
+ "y": 512.0,
+ "width": 256.0,
+ "height": 128.0,
+ "type": "Large Text Display",
+ "properties": {
+ "topic": "/SmartDashboard/WON AUTO?",
+ "period": 0.06,
+ "data_type": "string"
+ }
+ },
+ {
+ "title": "Field",
+ "x": 0.0,
+ "y": 0.0,
+ "width": 512.0,
+ "height": 512.0,
+ "type": "Field",
+ "properties": {
+ "topic": "/SmartDashboard/Field",
+ "period": 0.06,
+ "field_game": "Rebuilt",
+ "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,
+ "show_robot_outside_widget": true
+ }
+ },
+ {
+ "title": "Voltage",
+ "x": 0.0,
+ "y": 512.0,
+ "width": 512.0,
+ "height": 128.0,
+ "type": "Voltage View",
+ "properties": {
+ "topic": "/AdvantageKit/PowerDistribution/Voltage",
+ "period": 0.06,
+ "data_type": "double",
+ "min_value": 4.0,
+ "max_value": 13.0,
+ "divisions": 10,
+ "inverted": false,
+ "orientation": "horizontal"
+ }
+ },
+ {
+ "title": "RobotID",
+ "x": 0.0,
+ "y": 768.0,
+ "width": 256.0,
+ "height": 128.0,
+ "type": "Large Text Display",
+ "properties": {
+ "topic": "/SmartDashboard/RobotID",
+ "period": 0.06,
+ "data_type": "string"
+ }
+ },
+ {
+ "title": "Time till Unactive",
+ "x": 1280.0,
+ "y": 0.0,
+ "width": 384.0,
+ "height": 256.0,
+ "type": "Radial Gauge",
+ "properties": {
+ "topic": "/SmartDashboard/Time till Unactive",
+ "period": 0.06,
+ "data_type": "double",
+ "start_angle": -120.0,
+ "end_angle": 120.0,
+ "min_value": 0.0,
+ "max_value": 100.0,
+ "number_of_labels": 6,
+ "wrap_value": false,
+ "show_pointer": true,
+ "show_ticks": true
+ }
+ },
+ {
+ "title": "Time till active",
+ "x": 1280.0,
+ "y": 256.0,
+ "width": 384.0,
+ "height": 256.0,
+ "type": "Radial Gauge",
+ "properties": {
+ "topic": "/SmartDashboard/Time till active",
+ "period": 0.06,
+ "data_type": "double",
+ "start_angle": -120.0,
+ "end_angle": 120.0,
+ "min_value": 0.0,
+ "max_value": 30.0,
+ "number_of_labels": 6,
+ "wrap_value": false,
+ "show_pointer": true,
+ "show_ticks": false
+ }
+ },
+ {
+ "title": "Hub Active",
+ "x": 1280.0,
+ "y": 512.0,
+ "width": 128.0,
+ "height": 128.0,
+ "type": "Boolean Box",
+ "properties": {
+ "topic": "/SmartDashboard/Hub Active",
+ "period": 0.06,
+ "data_type": "boolean",
+ "true_color": 4283215696,
+ "false_color": 4294198070,
+ "true_icon": "None",
+ "false_icon": "None"
+ }
+ },
+ {
+ "title": "Auto chooser",
+ "x": 0.0,
+ "y": 640.0,
+ "width": 512.0,
+ "height": 128.0,
+ "type": "ComboBox Chooser",
+ "properties": {
+ "topic": "/SmartDashboard/Auto chooser",
+ "period": 0.06,
+ "sort_options": true
+ }
+ },
+ {
+ "title": "Spindexer Jamming",
+ "x": 1280.0,
+ "y": 768.0,
+ "width": 384.0,
+ "height": 128.0,
+ "type": "Boolean Box",
+ "properties": {
+ "topic": "/SmartDashboard/Spindexer Jamming",
+ "period": 0.06,
+ "data_type": "boolean",
+ "true_color": 4283215696,
+ "false_color": 4294198070,
+ "true_icon": "None",
+ "false_icon": "None"
+ }
+ }
+ ]
+ }
+ },
+ {
+ "name": "Autonomous",
+ "grid_layout": {
+ "layouts": [],
+ "containers": []
+ }
+ }
+ ]
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Center Drop Back Path"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Left Bump Edge Start"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 0.1
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.0
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Full Depot Outpost Left Path"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": "Week 2 autos",
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Depot Center Path"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 5.5
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "BookItToNeutral"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "W5 Left Trench Start"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "DirectSwipe1"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Intake Seizure"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Left Swipe2"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.0
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.0
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": "Week 5 autos",
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Kousha S1"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Kousha S2"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Left Drop Back Path"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Left Trench Start"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 0.1
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.0
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Full Left Path v3"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": "Week 2 autos",
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "New - Kousha 0416 #2 0.5"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "New - Kousha 0416 #2 0.5"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "New - Kousha 0416 #1"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Left Bump Depot Center Path"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 4.5
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "BookItToNeutral"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "W5 Left Trench Start"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "LeftConservativeSwipe1"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Intake Seizure"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Reset Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Left Swipe2"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.0
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.0
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "W5 Left Trench Start"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Left Swipe1 Bump"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.0
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Intake Seizure"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Reset Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Left Swipe2"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 0.5
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "W5 Left Trench Start"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Left Swipe1"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.0
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Intake Seizure"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Reset Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Left Swipe2"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 0.5
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "W5 Left Trench Start"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Swipe1Translation"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Intake Seizure"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Reset Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Swipe2Translation"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.0
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.0
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": "Week 5 autos",
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "W5 Left Trench Start"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Left Swipe3"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Intake Seizure"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Reset Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Left Swipe2"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.0
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.0
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "LeftConservativeSwipe1"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Left Swipe1"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Left Swipe3"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Left Swipe2"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Left Trench Depot Center Path"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.0
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 4.5
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "BookItToNeutral"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.5
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "New - Kousha 0416 #1"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Right Drop Back Path"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Right Trench Start"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 0.1
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.0
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Full Right Path v3"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": "Week 2 autos",
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "W5 Right Trench Start"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "RightConservativeSwipe1"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Intake Seizure"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Reset Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Right Swipe2"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.0
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.0
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "W5 Right Trench Start"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Right Swipe1"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.0
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Intake Seizure"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Reset Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Right Swipe2"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 0.5
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "W5 Right Trench Start"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Right Swipe3"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.0
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Intake Seizure"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Reset Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Right Swipe2"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 0.5
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "RightConservativeSwipe1"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Right Swipe1"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Right Swipe3"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Right Swipe2"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Left Bump Edge Start"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 0.1
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.0
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": "Week 2 autos",
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Random default auto"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": "Testing",
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Velocity TEST"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": "Week 2 autos",
+ "choreoAuto": false
+}
\ No newline at end of file
--- /dev/null
+{"field_size":{"x":16.54,"y":8.07},"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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,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,true,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,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,true,true,true,true,true,true,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,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,true,true,true,true,true,true,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,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,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,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,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,true,true,true,true,true,true,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,false,false,false,false,false,false,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,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,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,true,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,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,true,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,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,true,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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]]}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 1.5352,
+ "y": 7.408
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 2.5351999999999997,
+ "y": 7.408
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.8611506524317925,
+ "y": 7.408
+ },
+ "prevControl": {
+ "x": 6.8611506524317925,
+ "y": 7.408
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Hood Down",
+ "waypointRelativePos": 0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Stop Intake Seizure",
+ "waypointRelativePos": 0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Intake Seizure"
+ }
+ }
+ },
+ {
+ "name": "Stop Spindexer",
+ "waypointRelativePos": 0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ }
+ },
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 0.6051743532058476,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": null,
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 3.543,
+ "y": 4.217
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 2.044649240494815,
+ "y": 4.241017071760963
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 2.2,
+ "y": 4.217
+ },
+ "prevControl": {
+ "x": 2.5070000000000006,
+ "y": 4.217
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": null,
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 3.543,
+ "y": 4.217
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 3.5036773428232504,
+ "y": 6.55803084223013
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 0.1575682087781738,
+ "y": 4.9871886120996445
+ },
+ "prevControl": {
+ "x": 0.1575682087781738,
+ "y": 4.216722182324977
+ },
+ "nextControl": {
+ "x": 0.1575682087781738,
+ "y": 5.41755634638197
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 0.38351126927639445,
+ "y": 7.257378410438909
+ },
+ "prevControl": {
+ "x": 0.06367204449278147,
+ "y": 7.034460162862452
+ },
+ "nextControl": {
+ "x": 0.7385646500593126,
+ "y": 7.504839857651245
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 1.5354,
+ "y": 7.408
+ },
+ "prevControl": {
+ "x": 0.932230130486359,
+ "y": 7.41876631079478
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 0.89,
+ "rotationDegrees": 90.0
+ },
+ {
+ "waypointRelativePos": 2.0454545454545388,
+ "rotationDegrees": 90.0
+ },
+ {
+ "waypointRelativePos": 3.0,
+ "rotationDegrees": 0.0
+ }
+ ],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0.5061867266591578,
+ "maxWaypointRelativePos": 2.3284589426321785,
+ "constraints": {
+ "maxVelocity": 1.0,
+ "maxAcceleration": 1.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Spin Intake Rollers",
+ "waypointRelativePos": 0.9988974641675645,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ }
+ },
+ {
+ "name": "Stop Spindexer",
+ "waypointRelativePos": 1.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ }
+ },
+ {
+ "name": "Extend Intake",
+ "waypointRelativePos": 1.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ }
+ },
+ {
+ "name": "Start Spindexer",
+ "waypointRelativePos": 3.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ },
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 3.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": null,
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 180.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.5,
+ "y": 7.623190984573506
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 7.05559305204347,
+ "y": 7.1393105448289385
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.678244365361804,
+ "y": 6.364365361803084
+ },
+ "prevControl": {
+ "x": 7.555116359470672,
+ "y": 6.629895424661244
+ },
+ "nextControl": {
+ "x": 7.804590465430033,
+ "y": 6.091895360764787
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.699157769869515,
+ "y": 6.536512455516014
+ },
+ "prevControl": {
+ "x": 6.8860851430626155,
+ "y": 6.1598703844515175
+ },
+ "nextControl": {
+ "x": 6.2963922199408735,
+ "y": 7.34804930155004
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.6124792408066435,
+ "y": 7.408007117437722
+ },
+ "prevControl": {
+ "x": 5.93853054547692,
+ "y": 7.303899970283091
+ },
+ "nextControl": {
+ "x": 5.324138304321591,
+ "y": 7.500073463670289
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4.0,
+ "y": 7.5
+ },
+ "prevControl": {
+ "x": 4.5311698343686535,
+ "y": 7.430992876119471
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 1.1,
+ "rotationDegrees": -56.57915082012972
+ },
+ {
+ "waypointRelativePos": 2.0994671403197227,
+ "rotationDegrees": -74.10500133422103
+ },
+ {
+ "waypointRelativePos": 2.86,
+ "rotationDegrees": 0.0
+ }
+ ],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0.7761529808773961,
+ "maxWaypointRelativePos": 1.3,
+ "constraints": {
+ "maxVelocity": 1.0,
+ "maxAcceleration": 1.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 150.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 3.9685039370078883,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Start Spindexer",
+ "waypointRelativePos": 4.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 2.0,
+ "maxAcceleration": 3.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0.0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 5 (new stuff)",
+ "idealStartingState": {
+ "velocity": 0.5,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 3.6,
+ "y": 6.05
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 2.497585358790806,
+ "y": 7.371426793985635
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 1.3498603153880282,
+ "y": 7.307390914356006
+ },
+ "prevControl": {
+ "x": 2.622835865156547,
+ "y": 7.374641710073599
+ },
+ "nextControl": {
+ "x": 0.9474568787738418,
+ "y": 7.28613209967043
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 0.4457471788194449,
+ "y": 6.314113064236111
+ },
+ "prevControl": {
+ "x": 0.5182848753058401,
+ "y": 7.712424199882634
+ },
+ "nextControl": {
+ "x": 0.4069057436342598,
+ "y": 5.565365668402778
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 1.3498603153880282,
+ "y": 5.474232638888889
+ },
+ "prevControl": {
+ "x": 0.6358208912037042,
+ "y": 5.63668431712963
+ },
+ "nextControl": {
+ "x": 1.9579748272262023,
+ "y": 5.335880021841852
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 2.3516675347222225,
+ "y": 4.181836371527778
+ },
+ "prevControl": {
+ "x": 2.022761935763889,
+ "y": 5.547716435185186
+ },
+ "nextControl": {
+ "x": 2.5079680193043496,
+ "y": 3.5327512951202924
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 2.10549681712963,
+ "y": 2.418553313078704
+ },
+ "prevControl": {
+ "x": 2.526913266782408,
+ "y": 3.2032552806712964
+ },
+ "nextControl": {
+ "x": 1.6009065790249482,
+ "y": 1.4789769083251625
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 1.5986554542824076,
+ "y": 0.6042908709490746
+ },
+ "prevControl": {
+ "x": 1.6121712239583337,
+ "y": 1.1026028645833332
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 1.3,
+ "rotationDegrees": -90.0
+ },
+ {
+ "waypointRelativePos": 3.8,
+ "rotationDegrees": -90.0
+ }
+ ],
+ "constraintZones": [
+ {
+ "name": "Depot",
+ "minWaypointRelativePos": 6.0,
+ "maxWaypointRelativePos": 6.5,
+ "constraints": {
+ "maxVelocity": 0.5,
+ "maxAcceleration": 2.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ },
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0.4,
+ "maxWaypointRelativePos": 0.8,
+ "constraints": {
+ "maxVelocity": 0.3,
+ "maxAcceleration": 2.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ },
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 1.0,
+ "maxWaypointRelativePos": 5.0,
+ "constraints": {
+ "maxVelocity": 2.0,
+ "maxAcceleration": 1.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Hood Down",
+ "waypointRelativePos": 0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Extend Intake",
+ "waypointRelativePos": 0.2,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ }
+ },
+ {
+ "name": "Spin Intake Rollers",
+ "waypointRelativePos": 0.2,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ }
+ },
+ {
+ "name": "Start Spindexer",
+ "waypointRelativePos": 0.2,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ },
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 0.2,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Stop Spindexer",
+ "waypointRelativePos": 1.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ }
+ },
+ {
+ "name": "Hood Down",
+ "waypointRelativePos": 1.4,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 5.4,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Start Spindexer",
+ "waypointRelativePos": 5.6,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 0.75,
+ "maxAcceleration": 0.75,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 150.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0.0,
+ "rotation": 180.0
+ },
+ "reversed": false,
+ "folder": "week 2 autos",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 3.696241721854304,
+ "y": 7.623190984573506
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 6.785884183141178,
+ "y": 7.490581532356668
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.667485172004747,
+ "y": 6.428920521945433
+ },
+ "prevControl": {
+ "x": 7.642160343339249,
+ "y": 6.6776345204518766
+ },
+ "nextControl": {
+ "x": 7.9046446321955335,
+ "y": 4.099788174920342
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.667485172004747,
+ "y": 4.6644128113879
+ },
+ "prevControl": {
+ "x": 7.6169594963016465,
+ "y": 5.113190381183536
+ },
+ "nextControl": {
+ "x": 7.696238889728087,
+ "y": 4.409017442936094
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.236512455516015,
+ "y": 5.084021352313167
+ },
+ "prevControl": {
+ "x": 6.581262461023123,
+ "y": 4.633009890316522
+ },
+ "nextControl": {
+ "x": 5.730020148249949,
+ "y": 5.746628552722371
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.236512455516015,
+ "y": 6.808294701986754
+ },
+ "prevControl": {
+ "x": 6.825847617882709,
+ "y": 6.047265328866834
+ },
+ "nextControl": {
+ "x": 5.336986754966887,
+ "y": 7.969884105960265
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 0.6955278766310802,
+ "y": 7.623190984573506
+ },
+ "prevControl": {
+ "x": 0.05365961489070481,
+ "y": 7.556400025312338
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 1.087033747779748,
+ "rotationDegrees": -90.0
+ },
+ {
+ "waypointRelativePos": 2.5364120781527437,
+ "rotationDegrees": 155.02674741986806
+ },
+ {
+ "waypointRelativePos": 3.85,
+ "rotationDegrees": 138.60721817200425
+ },
+ {
+ "waypointRelativePos": 4.4,
+ "rotationDegrees": 180.0
+ },
+ {
+ "waypointRelativePos": 4.660237388724038,
+ "rotationDegrees": 180.0
+ },
+ {
+ "waypointRelativePos": 5,
+ "rotationDegrees": 180.0
+ }
+ ],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0.9718785151856066,
+ "maxWaypointRelativePos": 2.4701912260967447,
+ "constraints": {
+ "maxVelocity": 0.3,
+ "maxAcceleration": 2.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ },
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 2.767154105736777,
+ "maxWaypointRelativePos": 4.467941507311604,
+ "constraints": {
+ "maxVelocity": 3.5,
+ "maxAcceleration": 1.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ },
+ {
+ "name": "Depot",
+ "minWaypointRelativePos": 4.727915194346288,
+ "maxWaypointRelativePos": 5.5,
+ "constraints": {
+ "maxVelocity": 2.0,
+ "maxAcceleration": 2.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Spin Intake Rollers",
+ "waypointRelativePos": 0.17547806524185328,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ }
+ },
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 0.39145106861638246,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Stop Spindexer",
+ "waypointRelativePos": 2.9156355455568033,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ }
+ },
+ {
+ "name": "Hood Down",
+ "waypointRelativePos": 3.3745781777277815,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 4.52,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Start Spindexer",
+ "waypointRelativePos": 4.54,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 1.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 150.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0.0,
+ "rotation": 180.0
+ },
+ "reversed": false,
+ "folder": "week 2 autos",
+ "idealStartingState": {
+ "velocity": 0.5,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 3.7,
+ "y": 0.4481350154264931
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 6.893921116983211,
+ "y": 0.4481350154264931
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.667485172004747,
+ "y": 1.642405478054566
+ },
+ "prevControl": {
+ "x": 7.665365003292386,
+ "y": 1.3845651641599017
+ },
+ "nextControl": {
+ "x": 7.684288184321054,
+ "y": 3.6858720874587565
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.667485172004747,
+ "y": 3.406913188612099
+ },
+ "prevControl": {
+ "x": 7.722519349903317,
+ "y": 3.1630459311480665
+ },
+ "nextControl": {
+ "x": 7.611361211994106,
+ "y": 3.655609484776302
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.236512455516015,
+ "y": 2.987304647686832
+ },
+ "prevControl": {
+ "x": 7.118832485314941,
+ "y": 3.517259257915697
+ },
+ "nextControl": {
+ "x": 5.563661514586408,
+ "y": 2.583165054229189
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.236512455516015,
+ "y": 1.0398906500593113
+ },
+ "prevControl": {
+ "x": 6.607692290036552,
+ "y": 1.9733939152629345
+ },
+ "nextControl": {
+ "x": 5.905339551755663,
+ "y": 0.2070033047470511
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 0.6955278766310802,
+ "y": 0.4481350154264931
+ },
+ "prevControl": {
+ "x": -0.09030729073703303,
+ "y": 0.4481350154264932
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 1.087033747779748,
+ "rotationDegrees": 90.0
+ },
+ {
+ "waypointRelativePos": 2.5364120781527437,
+ "rotationDegrees": -155.02674741986806
+ },
+ {
+ "waypointRelativePos": 3.85,
+ "rotationDegrees": -138.60721817200425
+ },
+ {
+ "waypointRelativePos": 4.4,
+ "rotationDegrees": -180.0
+ },
+ {
+ "waypointRelativePos": 4.660237388724038,
+ "rotationDegrees": -180.0
+ },
+ {
+ "waypointRelativePos": 5,
+ "rotationDegrees": -180.0
+ }
+ ],
+ "constraintZones": [
+ {
+ "name": "Depot",
+ "minWaypointRelativePos": 4.953880764904385,
+ "maxWaypointRelativePos": 5.5,
+ "constraints": {
+ "maxVelocity": 0.5,
+ "maxAcceleration": 2,
+ "maxAngularVelocity": 200,
+ "maxAngularAcceleration": 300,
+ "nominalVoltage": 12,
+ "unlimited": false
+ }
+ },
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0.9718785151856066,
+ "maxWaypointRelativePos": 2.4701912260967447,
+ "constraints": {
+ "maxVelocity": 0.3,
+ "maxAcceleration": 2,
+ "maxAngularVelocity": 200,
+ "maxAngularAcceleration": 300,
+ "nominalVoltage": 12,
+ "unlimited": false
+ }
+ },
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 2.767154105736777,
+ "maxWaypointRelativePos": 4.467941507311604,
+ "constraints": {
+ "maxVelocity": 2,
+ "maxAcceleration": 1,
+ "maxAngularVelocity": 200,
+ "maxAngularAcceleration": 300,
+ "nominalVoltage": 12,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Hood Down",
+ "waypointRelativePos": 0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Extend Intake",
+ "waypointRelativePos": 0.0944881889763795,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ }
+ },
+ {
+ "name": "Spin Intake Rollers",
+ "waypointRelativePos": 0.17547806524185328,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ }
+ },
+ {
+ "name": "Start Spindexer",
+ "waypointRelativePos": 0.3104611923509808,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ },
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 0.39145106861638246,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Stop Spindexer",
+ "waypointRelativePos": 2.9156355455568033,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ }
+ },
+ {
+ "name": "Hood Down",
+ "waypointRelativePos": 3.3745781777277815,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 4.52,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Start Spindexer",
+ "waypointRelativePos": 4.544431946006741,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 1.5,
+ "maxAcceleration": 1.5,
+ "maxAngularVelocity": 200,
+ "maxAngularAcceleration": 150,
+ "nominalVoltage": 12,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": -180.0
+ },
+ "reversed": false,
+ "folder": "week 2 autos",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 3.381592574499267,
+ "y": 7.595268685881777
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 4.216148283887304,
+ "y": 7.595268685881777
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.123556424035175,
+ "y": 6.764110893991207
+ },
+ "prevControl": {
+ "x": 6.055007327796776,
+ "y": 7.732366878358572
+ },
+ "nextControl": {
+ "x": 6.194176127540517,
+ "y": 5.766607581978255
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.123556424035175,
+ "y": 1.5543795798729834
+ },
+ "prevControl": {
+ "x": 6.123556424035175,
+ "y": 2.5954249007095656
+ },
+ "nextControl": {
+ "x": 6.123556424035175,
+ "y": 0.5857824267034357
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4.5469272105520275,
+ "y": 0.5775549584758164
+ },
+ "prevControl": {
+ "x": 5.560347414214745,
+ "y": 0.5775549584758162
+ },
+ "nextControl": {
+ "x": 3.5335070068893097,
+ "y": 0.5775549584758165
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 2.6532584269662918,
+ "y": 0.5775549584758164
+ },
+ "prevControl": {
+ "x": 2.9032584269662918,
+ "y": 0.5775549584758164
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Hood Down",
+ "waypointRelativePos": 0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 4.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": -0.4151795411448859
+ },
+ "reversed": false,
+ "folder": "Field Testing Autos",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.436754966887417,
+ "y": 7.6431870860927145
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 5.436754966887418,
+ "y": 7.6431870860927145
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.921523178807947,
+ "y": 6.750215231788079
+ },
+ "prevControl": {
+ "x": 7.9078401005687935,
+ "y": 7.287276052674142
+ },
+ "nextControl": {
+ "x": 7.9796026490066865,
+ "y": 4.4705960264905045
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.534875827814634,
+ "y": 4.819072847682557
+ },
+ "prevControl": {
+ "x": 7.224569536423906,
+ "y": 4.753733443709048
+ },
+ "nextControl": {
+ "x": 5.615154835282087,
+ "y": 4.906204310133009
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.213567880794766,
+ "y": 5.42890728476865
+ },
+ "prevControl": {
+ "x": 5.888741721854368,
+ "y": 5.153029801324941
+ },
+ "nextControl": {
+ "x": 4.393659545209185,
+ "y": 5.7639235939326525
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 3.3,
+ "y": 5.75
+ },
+ "prevControl": {
+ "x": 4.100683433038495,
+ "y": 5.703196560889764
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 1.002386634844876,
+ "rotationDegrees": -90.0
+ },
+ {
+ "waypointRelativePos": 1.6658711217183793,
+ "rotationDegrees": -171.29413327946216
+ },
+ {
+ "waypointRelativePos": 2.6348448687350827,
+ "rotationDegrees": 90.0
+ }
+ ],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 2.446143154968738,
+ "maxWaypointRelativePos": 3.6302988186240435,
+ "constraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Hood Down",
+ "waypointRelativePos": 0,
+ "endWaypointRelativePos": null,
+ "command": null
+ },
+ {
+ "name": "Hood Down",
+ "waypointRelativePos": 0.005559416261295088,
+ "endWaypointRelativePos": null,
+ "command": null
+ },
+ {
+ "name": "Hood Down",
+ "waypointRelativePos": 0.01667824878387202,
+ "endWaypointRelativePos": null,
+ "command": null
+ },
+ {
+ "name": "Extend Intake",
+ "waypointRelativePos": 0.41695621959694984,
+ "endWaypointRelativePos": null,
+ "command": null
+ },
+ {
+ "name": "Spin Intake Rollers",
+ "waypointRelativePos": 0.4447533009034007,
+ "endWaypointRelativePos": null,
+ "command": null
+ },
+ {
+ "name": "Start Spindexer",
+ "waypointRelativePos": 0.9506601806810285,
+ "endWaypointRelativePos": null,
+ "command": null
+ },
+ {
+ "name": "Stop Spindexer",
+ "waypointRelativePos": 1.2119527449617822,
+ "endWaypointRelativePos": null,
+ "command": null
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 2.0,
+ "maxAcceleration": 2.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 90.0
+ },
+ "reversed": false,
+ "folder": "week 2 autos",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 3.3,
+ "y": 5.75
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 2.9339486754966884,
+ "y": 5.748344370860926
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4.088278145695364,
+ "y": 7.599627483443708
+ },
+ "prevControl": {
+ "x": 3.0882889621775207,
+ "y": 7.59497637096223
+ },
+ "nextControl": {
+ "x": 5.649163907284769,
+ "y": 7.606887417218543
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.925041390728477,
+ "y": 6.677615894039734
+ },
+ "prevControl": {
+ "x": 5.927124705977556,
+ "y": 7.17761155381847
+ },
+ "nextControl": {
+ "x": 5.922958075479399,
+ "y": 6.177620234260998
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.801622516552661,
+ "y": 4.390736754963256
+ },
+ "prevControl": {
+ "x": 5.824062934803918,
+ "y": 5.123790417837672
+ },
+ "nextControl": {
+ "x": 5.779842715228158,
+ "y": 3.6792632450294818
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.868832781456953,
+ "y": 4.811812913907284
+ },
+ "prevControl": {
+ "x": 6.9559519867549655,
+ "y": 3.7155629139072834
+ },
+ "nextControl": {
+ "x": 6.78171357615894,
+ "y": 5.908062913907284
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.489445364238411,
+ "y": 5.552326158940397
+ },
+ "prevControl": {
+ "x": 6.081129966887417,
+ "y": 5.6212955298013245
+ },
+ "nextControl": {
+ "x": 4.897760761589405,
+ "y": 5.483356788079469
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 3.3,
+ "y": 5.552326158940397
+ },
+ "prevControl": {
+ "x": 3.83542011589404,
+ "y": 5.492431705298013
+ },
+ "nextControl": {
+ "x": 2.7645798841059595,
+ "y": 5.612220612582781
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 3.384064569536424,
+ "y": 7.505248344370861
+ },
+ "prevControl": {
+ "x": 2.9266887417218546,
+ "y": 7.338269867549668
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 1.1026252983293603,
+ "rotationDegrees": 0.0
+ },
+ {
+ "waypointRelativePos": 2.0095465393794885,
+ "rotationDegrees": -90.0
+ },
+ {
+ "waypointRelativePos": 2.983293556085921,
+ "rotationDegrees": -75.19021917064742
+ },
+ {
+ "waypointRelativePos": 3.326968973747023,
+ "rotationDegrees": 57.25248324201524
+ },
+ {
+ "waypointRelativePos": 3.646778042959423,
+ "rotationDegrees": 64.7436522905489
+ },
+ {
+ "waypointRelativePos": 3.9260143198090636,
+ "rotationDegrees": 90.0
+ },
+ {
+ "waypointRelativePos": 4.853221957040566,
+ "rotationDegrees": 45.0
+ },
+ {
+ "waypointRelativePos": 5.939140811455842,
+ "rotationDegrees": 45.0
+ }
+ ],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0.0,
+ "maxWaypointRelativePos": 1.0368311327310604,
+ "constraints": {
+ "maxVelocity": 1.0,
+ "maxAcceleration": 2.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ },
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 5.662265462126515,
+ "maxWaypointRelativePos": 7.0,
+ "constraints": {
+ "maxVelocity": 1.0,
+ "maxAcceleration": 2.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Stop Spindexer",
+ "waypointRelativePos": 0.9631688672689134,
+ "endWaypointRelativePos": null,
+ "command": null
+ },
+ {
+ "name": "Hood Down",
+ "waypointRelativePos": 1.002084781098009,
+ "endWaypointRelativePos": null,
+ "command": null
+ },
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 5.86657400972897,
+ "endWaypointRelativePos": null,
+ "command": null
+ },
+ {
+ "name": "Start Spindexer",
+ "waypointRelativePos": 5.944405837387076,
+ "endWaypointRelativePos": null,
+ "command": null
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 2.0,
+ "maxAcceleration": 2.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 1.8279682443049239
+ },
+ "reversed": false,
+ "folder": "week 2 autos",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 90.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 3.568232502965599,
+ "y": 6.041589561086512
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 2.1695373665480435,
+ "y": 6.353606168446026
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 0.12529062871188268,
+ "y": 4.847319098461843
+ },
+ "prevControl": {
+ "x": 0.33332537129000794,
+ "y": 3.6429074309042764
+ },
+ "nextControl": {
+ "x": -0.07913404507222188,
+ "y": 6.030830367738237
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 1.534744958481614,
+ "y": 7.408007117443102
+ },
+ "prevControl": {
+ "x": 0.157568208783057,
+ "y": 7.827615658366945
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 0.5,
+ "rotationDegrees": 89.56096047431873
+ },
+ {
+ "waypointRelativePos": 1.35,
+ "rotationDegrees": 90.0
+ },
+ {
+ "waypointRelativePos": 1.5417406749555937,
+ "rotationDegrees": 58.875208713955935
+ }
+ ],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0.0,
+ "maxWaypointRelativePos": 1.8,
+ "constraints": {
+ "maxVelocity": 1.0,
+ "maxAcceleration": 1.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Spin Intake Rollers",
+ "waypointRelativePos": 0.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ }
+ },
+ {
+ "name": "Extend Intake",
+ "waypointRelativePos": 0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ }
+ },
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Stop Spindexer",
+ "waypointRelativePos": 0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ }
+ },
+ {
+ "name": "Start Spindexer",
+ "waypointRelativePos": 2.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": null,
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 180.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 3.6,
+ "y": 6.05
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 3.8500000000000227,
+ "y": 6.05
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 3.495,
+ "y": 6.05
+ },
+ "prevControl": {
+ "x": 3.245,
+ "y": 6.05
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 2 autos",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.5,
+ "y": 7.623
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 3.001456767975733,
+ "y": 7.623
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 3.2,
+ "y": 7.623
+ },
+ "prevControl": {
+ "x": 3.5070000000000006,
+ "y": 7.623
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": null,
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.5,
+ "y": 7.623190984573506
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 6.348201219731569,
+ "y": 7.398522949592349
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 8.182880794701987,
+ "y": 6.924453642384106
+ },
+ "prevControl": {
+ "x": 8.093113720311207,
+ "y": 7.15778146403279
+ },
+ "nextControl": {
+ "x": 8.386736239095766,
+ "y": 6.394580667304612
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.983041059705947,
+ "y": 5.528804199218751
+ },
+ "prevControl": {
+ "x": 8.55671378893142,
+ "y": 5.911793106342707
+ },
+ "nextControl": {
+ "x": 7.775118932021209,
+ "y": 5.389993577078183
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.168639049388745,
+ "y": 5.528804199218751
+ },
+ "prevControl": {
+ "x": 7.41829654935109,
+ "y": 5.515722428981051
+ },
+ "nextControl": {
+ "x": 5.921706780327692,
+ "y": 5.5941420378508395
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 2.1743586589671953,
+ "y": 6.332585504602503
+ },
+ "prevControl": {
+ "x": 2.4952956574714373,
+ "y": 4.763190617315148
+ },
+ "nextControl": {
+ "x": 1.9816571767447497,
+ "y": 7.274903537057722
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4.5,
+ "y": 7.623190984573506
+ },
+ "prevControl": {
+ "x": 3.405986634610018,
+ "y": 7.462622847315379
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 0.5,
+ "rotationDegrees": 0.0
+ },
+ {
+ "waypointRelativePos": 1.15,
+ "rotationDegrees": -101.33234295737161
+ },
+ {
+ "waypointRelativePos": 3.3498920641447336,
+ "rotationDegrees": 179.39787584122604
+ },
+ {
+ "waypointRelativePos": 3.5838863975123343,
+ "rotationDegrees": 178.04249394992306
+ },
+ {
+ "waypointRelativePos": 4.176755447941895,
+ "rotationDegrees": 49.5862098925657
+ }
+ ],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0.7761529808773961,
+ "maxWaypointRelativePos": 1.642294713160863,
+ "constraints": {
+ "maxVelocity": 1.0,
+ "maxAcceleration": 2.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 150.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 5.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Start Spindexer",
+ "waypointRelativePos": 5.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0.0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 5 (new stuff)",
+ "idealStartingState": {
+ "velocity": 0.5,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.5,
+ "y": 7.623
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 6.341153730575683,
+ "y": 7.3388762426119305
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 8.182880794701987,
+ "y": 6.924453642384106
+ },
+ "prevControl": {
+ "x": 8.031951473995347,
+ "y": 7.123753266445548
+ },
+ "nextControl": {
+ "x": 8.333810115408626,
+ "y": 6.725154018322663
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 8.023162251655629,
+ "y": 5.668485099337747
+ },
+ "prevControl": {
+ "x": 8.618618682530846,
+ "y": 5.7310165129163355
+ },
+ "nextControl": {
+ "x": 7.627292843251787,
+ "y": 5.626913168266943
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.311688741721855,
+ "y": 6.27831953642384
+ },
+ "prevControl": {
+ "x": 7.433678730378213,
+ "y": 5.997765220428201
+ },
+ "nextControl": {
+ "x": 7.0916784354147495,
+ "y": 6.784302377129385
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.6124792408066435,
+ "y": 7.408007117437722
+ },
+ "prevControl": {
+ "x": 5.877242530931041,
+ "y": 7.381065257813595
+ },
+ "nextControl": {
+ "x": 4.284691670946883,
+ "y": 7.543120509860877
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4.0,
+ "y": 7.5
+ },
+ "prevControl": {
+ "x": 4.397534345753931,
+ "y": 7.5120104256577624
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 1.15,
+ "rotationDegrees": -101.33234295737161
+ },
+ {
+ "waypointRelativePos": 3.0994671403197227,
+ "rotationDegrees": -74.10500133422103
+ },
+ {
+ "waypointRelativePos": 4.176755447941895,
+ "rotationDegrees": 0.0
+ }
+ ],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0.7761529808773961,
+ "maxWaypointRelativePos": 1.642294713160863,
+ "constraints": {
+ "maxVelocity": 1.0,
+ "maxAcceleration": 2.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 150.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0.0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 5 (new stuff)",
+ "idealStartingState": {
+ "velocity": 0.5,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.0,
+ "y": 7.5
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 5.75,
+ "y": 7.5
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.258030842230131,
+ "y": 6.330122034143519
+ },
+ "prevControl": {
+ "x": 6.372678524731828,
+ "y": 7.323528249594676
+ },
+ "nextControl": {
+ "x": 6.1659787419563115,
+ "y": 5.532503394482046
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.258030842230131,
+ "y": 4.449228944246738
+ },
+ "prevControl": {
+ "x": 5.653713425517404,
+ "y": 4.619355154982582
+ },
+ "nextControl": {
+ "x": 6.687014708632405,
+ "y": 4.3284622781876605
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.892823250296561,
+ "y": 5.546666666666668
+ },
+ "prevControl": {
+ "x": 6.892823250296561,
+ "y": 4.907866997335997
+ },
+ "nextControl": {
+ "x": 6.892823250296561,
+ "y": 6.2753696930420535
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.376611255787037,
+ "y": 7.154124710648148
+ },
+ "prevControl": {
+ "x": 6.541849794830491,
+ "y": 6.966518140361323
+ },
+ "nextControl": {
+ "x": 6.2113727167435835,
+ "y": 7.3417312809349715
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4.0,
+ "y": 7.5
+ },
+ "prevControl": {
+ "x": 5.5,
+ "y": 7.5
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 1.2433392539964518,
+ "rotationDegrees": -90.0
+ },
+ {
+ "waypointRelativePos": 3.0373001776198882,
+ "rotationDegrees": 90.0
+ },
+ {
+ "waypointRelativePos": 4.129662522202486,
+ "rotationDegrees": 0.0
+ }
+ ],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 5 (new stuff)",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.5,
+ "y": 7.623
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 6.042846975088969,
+ "y": 7.558635824436536
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.064365361803085,
+ "y": 5.9124792408066424
+ },
+ "prevControl": {
+ "x": 6.179013044304782,
+ "y": 6.905885456257799
+ },
+ "nextControl": {
+ "x": 5.972313261529266,
+ "y": 5.114860601145169
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.376381969157771,
+ "y": 4.363155397390273
+ },
+ "prevControl": {
+ "x": 5.772064552445045,
+ "y": 4.533281608126116
+ },
+ "nextControl": {
+ "x": 6.805365835560045,
+ "y": 4.242388731331195
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.021933570581259,
+ "y": 5.3422419928825615
+ },
+ "prevControl": {
+ "x": 7.021933570581259,
+ "y": 4.703442323551891
+ },
+ "nextControl": {
+ "x": 7.021933570581259,
+ "y": 6.070945019257947
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.5915658362989324,
+ "y": 7.19282325029656
+ },
+ "prevControl": {
+ "x": 6.756804375342386,
+ "y": 7.0052166800097355
+ },
+ "nextControl": {
+ "x": 6.426327297255479,
+ "y": 7.380429820583384
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4.0,
+ "y": 7.5
+ },
+ "prevControl": {
+ "x": 5.5,
+ "y": 7.5
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 1.2433392539964518,
+ "rotationDegrees": -90.0
+ },
+ {
+ "waypointRelativePos": 3.0373001776198882,
+ "rotationDegrees": 90.0
+ },
+ {
+ "waypointRelativePos": 4.129662522202486,
+ "rotationDegrees": 0.0
+ }
+ ],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 5.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Start Spindexer",
+ "waypointRelativePos": 5.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 5 (new stuff)",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.5,
+ "y": 7.623
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 2.836607354685647,
+ "y": 6.8485290628707
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 0.10377224199288304,
+ "y": 6.622586002372479
+ },
+ "prevControl": {
+ "x": 0.10377224199288315,
+ "y": 7.93520759193357
+ },
+ "nextControl": {
+ "x": 0.10377224199288304,
+ "y": 5.953350337515722
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 0.4803440094899176,
+ "y": 4.61061684460261
+ },
+ "prevControl": {
+ "x": 0.2616461704685724,
+ "y": 4.489491887606174
+ },
+ "nextControl": {
+ "x": 1.1796915776986958,
+ "y": 4.997947805456701
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 1.3518386714116257,
+ "y": 5.923238434163701
+ },
+ "prevControl": {
+ "x": 0.9099310791445863,
+ "y": 5.2840857560171
+ },
+ "nextControl": {
+ "x": 1.5452491103202861,
+ "y": 6.202977461447212
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 1.535,
+ "y": 7.408
+ },
+ "prevControl": {
+ "x": 1.7391696322657184,
+ "y": 6.536512455516014
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 0.3481349911189977,
+ "rotationDegrees": 0.0
+ },
+ {
+ "waypointRelativePos": 0.5186500888099518,
+ "rotationDegrees": -90.0
+ },
+ {
+ "waypointRelativePos": 1.8741258741258744,
+ "rotationDegrees": -90.0
+ },
+ {
+ "waypointRelativePos": 2.6287744227353658,
+ "rotationDegrees": -90.0
+ }
+ ],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0.4769403824521941,
+ "maxWaypointRelativePos": 3.1136107986502153,
+ "constraints": {
+ "maxVelocity": 1.0,
+ "maxAcceleration": 1.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Hood Down",
+ "waypointRelativePos": 0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Stop Spindexer",
+ "waypointRelativePos": 0.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ }
+ },
+ {
+ "name": "Spin Intake Rollers",
+ "waypointRelativePos": 0.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ }
+ },
+ {
+ "name": "Extend Intake",
+ "waypointRelativePos": 0.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ }
+ },
+ {
+ "name": "Start Spindexer",
+ "waypointRelativePos": 4.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ },
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 4.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": null,
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 3.6,
+ "y": 7.623
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 3.8500000000000223,
+ "y": 7.623
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 3.7,
+ "y": 7.623
+ },
+ "prevControl": {
+ "x": 3.449999999999994,
+ "y": 7.623
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0,
+ "maxWaypointRelativePos": 1.0,
+ "constraints": {
+ "maxVelocity": 0.5,
+ "maxAcceleration": 0.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0.5,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 2 autos",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.5,
+ "y": 7.623
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 6.819805487632309,
+ "y": 6.77562980981629
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.159230132450332,
+ "y": 6.641316225165562
+ },
+ "prevControl": {
+ "x": 6.869067715509156,
+ "y": 6.9193627325227665
+ },
+ "nextControl": {
+ "x": 7.339735203900921,
+ "y": 6.46834825361991
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.398505338078292,
+ "y": 5.566846026490066
+ },
+ "prevControl": {
+ "x": 7.64577301033512,
+ "y": 5.705763638397914
+ },
+ "nextControl": {
+ "x": 7.031343434260263,
+ "y": 5.360570558304471
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.46953642384106,
+ "y": 6.401738410596026
+ },
+ "prevControl": {
+ "x": 6.69147920841791,
+ "y": 6.100131466298098
+ },
+ "nextControl": {
+ "x": 6.078640973388454,
+ "y": 6.9329419033148865
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.6124792408066435,
+ "y": 7.408007117437722
+ },
+ "prevControl": {
+ "x": 5.8389868583347635,
+ "y": 7.302204002163478
+ },
+ "nextControl": {
+ "x": 5.3859716232785235,
+ "y": 7.5138102327119665
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4.0,
+ "y": 7.5
+ },
+ "prevControl": {
+ "x": 4.396406066666464,
+ "y": 7.41042919989128
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 0.9685230024212932,
+ "rotationDegrees": -64.90375286541574
+ },
+ {
+ "waypointRelativePos": 3.86,
+ "rotationDegrees": 0.0
+ }
+ ],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0.7761529808773961,
+ "maxWaypointRelativePos": 1.8335208098987592,
+ "constraints": {
+ "maxVelocity": 1.0,
+ "maxAcceleration": 1.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 150.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 2.0,
+ "maxAcceleration": 2.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0.0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 5 (new stuff)",
+ "idealStartingState": {
+ "velocity": 0.5,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.4,
+ "y": 7.623
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 4.649689734980109,
+ "y": 7.635451355169741
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.852442052980133,
+ "y": 7.468948675496688
+ },
+ "prevControl": {
+ "x": 5.620124172185431,
+ "y": 7.599627483443708
+ },
+ "nextControl": {
+ "x": 6.07033593726127,
+ "y": 7.346383365588548
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.993052083333334,
+ "y": 6.644330873842592
+ },
+ "prevControl": {
+ "x": 6.002728406424771,
+ "y": 6.894143541198413
+ },
+ "nextControl": {
+ "x": 5.96856842368264,
+ "y": 6.012238676612816
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.993052083333334,
+ "y": 4.640374290829613
+ },
+ "prevControl": {
+ "x": 5.77132848576112,
+ "y": 4.980825878341205
+ },
+ "nextControl": {
+ "x": 6.258514999588687,
+ "y": 4.2327619651494075
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.9488311476934514,
+ "y": 4.5376008533296135
+ },
+ "prevControl": {
+ "x": 6.714643852212108,
+ "y": 4.306406270895929
+ },
+ "nextControl": {
+ "x": 7.126740955423579,
+ "y": 4.71323712610215
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.069917751736111,
+ "y": 5.2388220486111114
+ },
+ "prevControl": {
+ "x": 7.180039999523313,
+ "y": 5.014382545532232
+ },
+ "nextControl": {
+ "x": 6.935152848707392,
+ "y": 5.513485601853203
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.309817880794702,
+ "y": 5.501506622516556
+ },
+ "prevControl": {
+ "x": 6.672312380421229,
+ "y": 5.595486677975285
+ },
+ "nextControl": {
+ "x": 6.067818656253252,
+ "y": 5.438766082820624
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 3.3913245033112585,
+ "y": 5.385347682119204
+ },
+ "prevControl": {
+ "x": 3.6817218543046355,
+ "y": 5.37808774834437
+ },
+ "nextControl": {
+ "x": 3.002075275595232,
+ "y": 5.395078912812104
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 1.7994537062872022,
+ "y": 5.001661667596727
+ },
+ "prevControl": {
+ "x": 2.2296859422923645,
+ "y": 5.208439382934831
+ },
+ "nextControl": {
+ "x": 1.3440231788079464,
+ "y": 4.782773178807946
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 0.5454304635761584,
+ "y": 4.760993377483443
+ },
+ "prevControl": {
+ "x": 0.7366582631054183,
+ "y": 4.599959441037751
+ },
+ "nextControl": {
+ "x": 0.35420266404689527,
+ "y": 4.922027313929137
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 0.4437913907284766,
+ "y": 6.968013245033112
+ },
+ "prevControl": {
+ "x": 0.4437913907284766,
+ "y": 6.718013245033112
+ },
+ "nextControl": {
+ "x": 0.4437913907284766,
+ "y": 7.218013245033112
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4.5,
+ "y": 7.623
+ },
+ "prevControl": {
+ "x": 4.081018211920529,
+ "y": 7.4834685430463574
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 0.5,
+ "rotationDegrees": 0.0
+ },
+ {
+ "waypointRelativePos": 1.658595641646543,
+ "rotationDegrees": -89.54489461997562
+ },
+ {
+ "waypointRelativePos": 2.6465364120781536,
+ "rotationDegrees": -89.07124990057926
+ },
+ {
+ "waypointRelativePos": 4.261501210653795,
+ "rotationDegrees": 67.25186374927067
+ },
+ {
+ "waypointRelativePos": 6.219128329297827,
+ "rotationDegrees": 89.24946813432852
+ },
+ {
+ "waypointRelativePos": 6.50258863926749,
+ "rotationDegrees": 89.03627314363375
+ },
+ {
+ "waypointRelativePos": 9.77481840193705,
+ "rotationDegrees": 90.0
+ }
+ ],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Hood Down",
+ "waypointRelativePos": 0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Extend Intake",
+ "waypointRelativePos": 0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ }
+ },
+ {
+ "name": "Spin Intake Rollers",
+ "waypointRelativePos": 1.3498312710910954,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ }
+ },
+ {
+ "name": "Stop Intake Rollers",
+ "waypointRelativePos": 6.209223847019097,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Intake Rollers"
+ }
+ }
+ },
+ {
+ "name": "Retract Intake",
+ "waypointRelativePos": 6.25,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Retract Intake"
+ }
+ }
+ },
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 6.8841394825646685,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Start Spindexer",
+ "waypointRelativePos": 6.884139482564717,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ },
+ {
+ "name": "Extend Intake",
+ "waypointRelativePos": 8.691166077738544,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ }
+ },
+ {
+ "name": "Spin Intake Rollers",
+ "waypointRelativePos": 8.73780918727915,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ }
+ },
+ {
+ "name": "Stop Spindexer",
+ "waypointRelativePos": 9.126501766784463,
+ "endWaypointRelativePos": null,
+ "command": null
+ },
+ {
+ "name": "Hood Down",
+ "waypointRelativePos": 9.219787985865741,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Start Spindexer",
+ "waypointRelativePos": 9.82614840989399,
+ "endWaypointRelativePos": null,
+ "command": null
+ },
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 9.83,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": -0.19252622320957904
+ },
+ "reversed": false,
+ "folder": null,
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.20559776060051335
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.4,
+ "y": 7.623
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 5.4156465005931205,
+ "y": 7.580154211150653
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.827663107947806,
+ "y": 7.504839857651245
+ },
+ "prevControl": {
+ "x": 5.5797516965844745,
+ "y": 7.537087836128061
+ },
+ "nextControl": {
+ "x": 6.089809247306431,
+ "y": 7.4707402451593135
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.087238932291667,
+ "y": 7.2053665364583335
+ },
+ "prevControl": {
+ "x": 6.795990510083039,
+ "y": 7.429525504151838
+ },
+ "nextControl": {
+ "x": 7.827939446866852,
+ "y": 6.6352873529965715
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.830212601273148,
+ "y": 6.273040870949074
+ },
+ "prevControl": {
+ "x": 7.696276229545007,
+ "y": 6.484135750868747
+ },
+ "nextControl": {
+ "x": 8.052156231843401,
+ "y": 5.923239219316608
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.830212601273148,
+ "y": 5.568185053380782
+ },
+ "prevControl": {
+ "x": 8.028429824610942,
+ "y": 5.720533116618405
+ },
+ "nextControl": {
+ "x": 7.631995377935355,
+ "y": 5.41583699014316
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.742194543297748,
+ "y": 5.568185053380782
+ },
+ "prevControl": {
+ "x": 6.991936212700865,
+ "y": 5.579547208270771
+ },
+ "nextControl": {
+ "x": 6.37612481071697,
+ "y": 5.551530479828598
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.526405693950179,
+ "y": 5.568185053380782
+ },
+ "prevControl": {
+ "x": 5.9940016886817045,
+ "y": 5.551617678371244
+ },
+ "nextControl": {
+ "x": 5.219470974518611,
+ "y": 5.579060045163357
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 3.439122182680902,
+ "y": 5.568185053380782
+ },
+ "prevControl": {
+ "x": 4.762502965599051,
+ "y": 5.546666666666668
+ },
+ "nextControl": {
+ "x": 2.5231053186257717,
+ "y": 5.583079636536149
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 2.7081951678240745,
+ "y": 6.473218605324075
+ },
+ "prevControl": {
+ "x": 2.6571916654866383,
+ "y": 6.228476613117353
+ },
+ "nextControl": {
+ "x": 2.7591986701615108,
+ "y": 6.717960597530796
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 2.9613862123842596,
+ "y": 7.2053665364583335
+ },
+ "prevControl": {
+ "x": 2.793264485781677,
+ "y": 7.020339281589851
+ },
+ "nextControl": {
+ "x": 3.1733449921664065,
+ "y": 7.438638891917008
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 3.664271846064816,
+ "y": 7.612545500578703
+ },
+ "prevControl": {
+ "x": 3.3162524646476355,
+ "y": 7.610200053100003
+ },
+ "nextControl": {
+ "x": 4.114582849281393,
+ "y": 7.615580333876848
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4.4,
+ "y": 7.623
+ },
+ "prevControl": {
+ "x": 4.150105477767021,
+ "y": 7.630261388155952
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 1.48390501725838,
+ "rotationDegrees": -20.130704916602326
+ },
+ {
+ "waypointRelativePos": 1.975121733234714,
+ "rotationDegrees": -49.65081094572614
+ },
+ {
+ "waypointRelativePos": 2.53049494575937,
+ "rotationDegrees": -89.0834827186487
+ },
+ {
+ "waypointRelativePos": 5.3924509985207205,
+ "rotationDegrees": -90.0
+ },
+ {
+ "waypointRelativePos": 6.970938116370813,
+ "rotationDegrees": -90.0
+ },
+ {
+ "waypointRelativePos": 7.622418947238659,
+ "rotationDegrees": 35.39060987236753
+ },
+ {
+ "waypointRelativePos": 9.512704943293905,
+ "rotationDegrees": 0.0
+ }
+ ],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Hood Down",
+ "waypointRelativePos": 0,
+ "endWaypointRelativePos": null,
+ "command": null
+ },
+ {
+ "name": "Extend Intake",
+ "waypointRelativePos": 0,
+ "endWaypointRelativePos": null,
+ "command": null
+ },
+ {
+ "name": "Spin Intake Rollers",
+ "waypointRelativePos": 0.6929133858268031,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ }
+ },
+ {
+ "name": "Retract Intake",
+ "waypointRelativePos": 5.54,
+ "endWaypointRelativePos": null,
+ "command": null
+ },
+ {
+ "name": "Stop Intake Rollers",
+ "waypointRelativePos": 5.543307086614183,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Intake Rollers"
+ }
+ }
+ },
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 6.934275618374536,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Start Spindexer",
+ "waypointRelativePos": 6.94982332155476,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ },
+ {
+ "name": "Hood Down",
+ "waypointRelativePos": 9.676040494938205,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Stop Spindexer",
+ "waypointRelativePos": 9.68,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 1.0194290739247367
+ },
+ "reversed": false,
+ "folder": null,
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 0.4583112582781458,
+ "y": 6.975273178807948
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 0.42180298013245043,
+ "y": 7.746221854304635
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4.5,
+ "y": 7.623
+ },
+ "prevControl": {
+ "x": 4.088447458765173,
+ "y": 7.570432330258617
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "",
+ "waypointRelativePos": 0,
+ "endWaypointRelativePos": null,
+ "command": null
+ },
+ {
+ "name": "",
+ "waypointRelativePos": 0,
+ "endWaypointRelativePos": null,
+ "command": null
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": null,
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 90.87947248129271
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 3.6,
+ "y": 7.623
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 4.6,
+ "y": 7.623
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4.0,
+ "y": 7.623
+ },
+ "prevControl": {
+ "x": 3.0,
+ "y": 7.623
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "Field Testing Autos",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.5,
+ "y": 0.448
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 3.001456767975733,
+ "y": 0.4480000000000002
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 3.2,
+ "y": 0.448
+ },
+ "prevControl": {
+ "x": 3.506999999999998,
+ "y": 0.44799999999999995
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": null,
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.5,
+ "y": 0.448
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 6.341153730575683,
+ "y": 0.7321237573880697
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 8.306299668874171,
+ "y": 1.037973350993377
+ },
+ "prevControl": {
+ "x": 8.155370348167532,
+ "y": 0.8386737269319344
+ },
+ "nextControl": {
+ "x": 8.45722898958081,
+ "y": 1.2372729750548204
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 8.023162251655629,
+ "y": 2.402840900662252
+ },
+ "prevControl": {
+ "x": 8.618618682530846,
+ "y": 2.3403094870836636
+ },
+ "nextControl": {
+ "x": 7.627292843251787,
+ "y": 2.444412831733057
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.311688741721855,
+ "y": 1.7930064635761587
+ },
+ "prevControl": {
+ "x": 7.433678730378213,
+ "y": 2.073560779571798
+ },
+ "nextControl": {
+ "x": 7.0916784354147495,
+ "y": 1.2870236228706147
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.6124792408066435,
+ "y": 0.6633188825622768
+ },
+ "prevControl": {
+ "x": 5.877242530931041,
+ "y": 0.6902607421864038
+ },
+ "nextControl": {
+ "x": 4.284691670946883,
+ "y": 0.5282054901391218
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4,
+ "y": 0.571325999999999
+ },
+ "prevControl": {
+ "x": 4.397534345753931,
+ "y": 0.5593155743422364
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 1.15,
+ "rotationDegrees": 101.33234295737161
+ },
+ {
+ "waypointRelativePos": 3.0994671403197227,
+ "rotationDegrees": 74.10500133422103
+ },
+ {
+ "waypointRelativePos": 4.176755447941895,
+ "rotationDegrees": 0.0
+ }
+ ],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0.7761529808773961,
+ "maxWaypointRelativePos": 1.642294713160863,
+ "constraints": {
+ "maxVelocity": 1,
+ "maxAcceleration": 2,
+ "maxAngularVelocity": 200,
+ "maxAngularAcceleration": 150,
+ "nominalVoltage": 12,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200,
+ "maxAngularAcceleration": 300,
+ "nominalVoltage": 12,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 5 (new stuff)",
+ "idealStartingState": {
+ "velocity": 0.5,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4,
+ "y": 0.571325999999999
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 5.75,
+ "y": 0.571325999999999
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.258030842230131,
+ "y": 1.513718460648148
+ },
+ "prevControl": {
+ "x": 6.372678524731828,
+ "y": 0.520312245196991
+ },
+ "nextControl": {
+ "x": 6.1659787419563115,
+ "y": 2.31133710030962
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.258030842230131,
+ "y": 3.6220970557532612
+ },
+ "prevControl": {
+ "x": 5.653713425517404,
+ "y": 3.451970845017417
+ },
+ "nextControl": {
+ "x": 6.687014708632405,
+ "y": 3.7428637218123386
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.892823250296561,
+ "y": 2.524659333333331
+ },
+ "prevControl": {
+ "x": 6.892823250296561,
+ "y": 3.163459002664002
+ },
+ "nextControl": {
+ "x": 6.892823250296561,
+ "y": 1.7959563069579454
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.510128689236111,
+ "y": 0.8553167679398155
+ },
+ "prevControl": {
+ "x": 6.675367228279565,
+ "y": 1.0429233382266405
+ },
+ "nextControl": {
+ "x": 6.344890150192657,
+ "y": 0.6677101976529917
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4,
+ "y": 0.571325999999999
+ },
+ "prevControl": {
+ "x": 5.5,
+ "y": 0.571325999999999
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 1.2433392539964518,
+ "rotationDegrees": 90.0
+ },
+ {
+ "waypointRelativePos": 3.0373001776198882,
+ "rotationDegrees": -90.0
+ },
+ {
+ "waypointRelativePos": 4.129662522202486,
+ "rotationDegrees": 0.0
+ }
+ ],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200,
+ "maxAngularAcceleration": 300,
+ "nominalVoltage": 12,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 5 (new stuff)",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.5,
+ "y": 0.448
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 6.25,
+ "y": 0.448
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.15043890865955,
+ "y": 2.6309252668985335
+ },
+ "prevControl": {
+ "x": 6.440836370311678,
+ "y": 1.372536266389839
+ },
+ "nextControl": {
+ "x": 5.956773428232504,
+ "y": 3.4701423487598277
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.376381969157771,
+ "y": 3.9758244365415596
+ },
+ "prevControl": {
+ "x": 5.999810201660737,
+ "y": 3.8467141162568614
+ },
+ "nextControl": {
+ "x": 6.8334690871605615,
+ "y": 4.132540019856803
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.021933570581259,
+ "y": 2.921423487549863
+ },
+ "prevControl": {
+ "x": 7.0105248157678215,
+ "y": 3.7542625889308727
+ },
+ "nextControl": {
+ "x": 7.043451957295374,
+ "y": 1.350581257419376
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.15043890865955,
+ "y": 0.7803440094845382
+ },
+ "prevControl": {
+ "x": 6.612019589411609,
+ "y": 0.934204236395481
+ },
+ "nextControl": {
+ "x": 5.375776986951365,
+ "y": 0.5221233689259019
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4,
+ "y": 0.571325999999999
+ },
+ "prevControl": {
+ "x": 5.5,
+ "y": 0.571325999999999
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 1.2433392539964518,
+ "rotationDegrees": 90.0
+ },
+ {
+ "waypointRelativePos": 3.0373001776198882,
+ "rotationDegrees": -90.0
+ },
+ {
+ "waypointRelativePos": 4.129662522202486,
+ "rotationDegrees": 0.0
+ }
+ ],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200,
+ "maxAngularAcceleration": 300,
+ "nominalVoltage": 12,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 5 (new stuff)",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 3.6,
+ "y": 0.4481350154264931
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 3.8500000000000223,
+ "y": 0.4481350154264927
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 3.7,
+ "y": 0.4481350154264931
+ },
+ "prevControl": {
+ "x": 3.449999999999994,
+ "y": 0.4481350154264927
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0,
+ "maxWaypointRelativePos": 1.0,
+ "constraints": {
+ "maxVelocity": 0.5,
+ "maxAcceleration": 0.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 2 autos",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.5,
+ "y": 0.448
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 6.819805487632309,
+ "y": 1.2953701901837102
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.159230132450332,
+ "y": 1.430009774834437
+ },
+ "prevControl": {
+ "x": 6.869067715509156,
+ "y": 1.151963267477233
+ },
+ "nextControl": {
+ "x": 7.339735203900921,
+ "y": 1.6029777463800896
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.398505338078292,
+ "y": 2.5044799735099326
+ },
+ "prevControl": {
+ "x": 7.64577301033512,
+ "y": 2.3655623616020853
+ },
+ "nextControl": {
+ "x": 7.031343434260263,
+ "y": 2.710755441695528
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.46953642384106,
+ "y": 1.6695875894039724
+ },
+ "prevControl": {
+ "x": 6.69147920841791,
+ "y": 1.9711945337019008
+ },
+ "nextControl": {
+ "x": 6.078640973388454,
+ "y": 1.1383840966851129
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.6124792408066435,
+ "y": 0.6633188825622768
+ },
+ "prevControl": {
+ "x": 5.8389868583347635,
+ "y": 0.7691219978365208
+ },
+ "nextControl": {
+ "x": 5.3859716232785235,
+ "y": 0.5575157672880328
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4,
+ "y": 0.571325999999999
+ },
+ "prevControl": {
+ "x": 4.396406066666464,
+ "y": 0.660896800108719
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 0.9685230024212932,
+ "rotationDegrees": 64.90375286541574
+ },
+ {
+ "waypointRelativePos": 3.86,
+ "rotationDegrees": 0.0
+ }
+ ],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0.7761529808773961,
+ "maxWaypointRelativePos": 1.8335208098987592,
+ "constraints": {
+ "maxVelocity": 1,
+ "maxAcceleration": 1.5,
+ "maxAngularVelocity": 200,
+ "maxAngularAcceleration": 150,
+ "nominalVoltage": 12,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 2,
+ "maxAcceleration": 2,
+ "maxAngularVelocity": 200,
+ "maxAngularAcceleration": 300,
+ "nominalVoltage": 12,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 5 (new stuff)",
+ "idealStartingState": {
+ "velocity": 0.5,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 3.5,
+ "y": 7.5
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 3.75,
+ "y": 7.5
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 3.8,
+ "y": 7.5
+ },
+ "prevControl": {
+ "x": 3.55,
+ "y": 7.5
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 2 autos",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 3.5036773428232504,
+ "y": 7.612
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 3.514743377483444,
+ "y": 5.718872844539763
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 3.5036773428232504,
+ "y": 1.516
+ },
+ "prevControl": {
+ "x": 3.5251957295373675,
+ "y": 2.4197722419928844
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 1.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0.0,
+ "rotation": -90.0
+ },
+ "reversed": false,
+ "folder": "Field Testing Autos",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": -90.0
+ },
+ "useDefaultConstraints": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.5,
+ "y": 7.623190984573506
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 6.735631586835829,
+ "y": 7.2764780852994
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.710521945432978,
+ "y": 6.697900355871886
+ },
+ "prevControl": {
+ "x": 7.529042978857038,
+ "y": 6.86984623308483
+ },
+ "nextControl": {
+ "x": 7.928919995080244,
+ "y": 6.490974754868527
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.710521945432978,
+ "y": 5.7188137603795965
+ },
+ "prevControl": {
+ "x": 7.925365275955983,
+ "y": 5.863911730255719
+ },
+ "nextControl": {
+ "x": 7.408579979016614,
+ "y": 5.514892297047377
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.860545670225386,
+ "y": 6.364365361803084
+ },
+ "prevControl": {
+ "x": 6.917316499510639,
+ "y": 6.120896528187229
+ },
+ "nextControl": {
+ "x": 6.574474152212791,
+ "y": 7.591219006502909
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.6124792408066435,
+ "y": 7.408007117437722
+ },
+ "prevControl": {
+ "x": 5.872250653983406,
+ "y": 7.333391289226129
+ },
+ "nextControl": {
+ "x": 5.300042806450162,
+ "y": 7.497750261334479
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4.0,
+ "y": 7.5
+ },
+ "prevControl": {
+ "x": 4.245580247742288,
+ "y": 7.453198911136212
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0.7761529808773961,
+ "maxWaypointRelativePos": 1.642294713160863,
+ "constraints": {
+ "maxVelocity": 1.0,
+ "maxAcceleration": 1.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 150.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 4.968503937007888,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Start Spindexer",
+ "waypointRelativePos": 5.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 2.0,
+ "maxAcceleration": 2.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0.0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 5 (new stuff)",
+ "idealStartingState": {
+ "velocity": 0.5,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.0,
+ "y": 7.5
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 5.75,
+ "y": 7.5
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.0213285883748515,
+ "y": 6.289051008303677
+ },
+ "prevControl": {
+ "x": 6.135976270876549,
+ "y": 7.282457223754833
+ },
+ "nextControl": {
+ "x": 5.929276488101032,
+ "y": 5.4914323686422035
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.258030842230131,
+ "y": 4.449228944246738
+ },
+ "prevControl": {
+ "x": 5.653713425517404,
+ "y": 4.619355154982582
+ },
+ "nextControl": {
+ "x": 6.687014708632405,
+ "y": 4.3284622781876605
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.892823250296561,
+ "y": 5.546666666666668
+ },
+ "prevControl": {
+ "x": 6.892823250296561,
+ "y": 4.907866997335997
+ },
+ "nextControl": {
+ "x": 6.892823250296561,
+ "y": 6.2753696930420535
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.258030842230131,
+ "y": 6.977639383155397
+ },
+ "prevControl": {
+ "x": 6.423269381273585,
+ "y": 6.7900328128685725
+ },
+ "nextControl": {
+ "x": 6.092792303186677,
+ "y": 7.165245953442221
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4.0,
+ "y": 7.5
+ },
+ "prevControl": {
+ "x": 5.5,
+ "y": 7.5
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 2.0,
+ "maxAcceleration": 2.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 5 (new stuff)",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.42896797153025,
+ "y": 7.655468564650059
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 6.801595217095044,
+ "y": 6.510923411540405
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 8.194685646500595,
+ "y": 5.772609727164888
+ },
+ "prevControl": {
+ "x": 9.176240918430604,
+ "y": 6.710214763038332
+ },
+ "nextControl": {
+ "x": 7.473819691577701,
+ "y": 5.084021352313167
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 1.965112692763939,
+ "y": 5.353001186239621
+ },
+ "prevControl": {
+ "x": 4.500675461892897,
+ "y": 5.5286443358904505
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": -46.123302714075294
+ },
+ "reversed": false,
+ "folder": "week 5 (new stuff)",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 0.4682559843673668,
+ "y": 7.603837322911579
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 2.6361211529066964,
+ "y": 7.775210063507574
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 2.764650708353688,
+ "y": 4.064990229604298
+ },
+ "prevControl": {
+ "x": 2.953160723009282,
+ "y": 4.973265754763068
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0.1427297056199817,
+ "maxWaypointRelativePos": 1.0,
+ "constraints": {
+ "maxVelocity": 5.2,
+ "maxAcceleration": 3.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "Field Testing Autos",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.4,
+ "y": 7.623
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 4.650000000000023,
+ "y": 7.623
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4.5,
+ "y": 7.623
+ },
+ "prevControl": {
+ "x": 4.249999999999994,
+ "y": 7.623
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0.5,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 5 (new stuff)",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.4,
+ "y": 0.4483259999999985
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 4.650000000000023,
+ "y": 0.4483259999999985
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4.5,
+ "y": 0.4483259999999985
+ },
+ "prevControl": {
+ "x": 4.249999999999994,
+ "y": 0.44832599999999856
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200,
+ "maxAngularAcceleration": 300,
+ "nominalVoltage": 12,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0.5,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 5 (new stuff)",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 3.5,
+ "y": 7.623
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 3.75,
+ "y": 7.623
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 3.8,
+ "y": 7.623
+ },
+ "prevControl": {
+ "x": 3.5421509693935853,
+ "y": 7.623
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Extend Intake",
+ "waypointRelativePos": 0.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ }
+ },
+ {
+ "name": "Spin Intake Rollers",
+ "waypointRelativePos": 0.1,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ }
+ },
+ {
+ "name": "Start Spindexer",
+ "waypointRelativePos": 0.1,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ },
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 0.1,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Stop Spindexer",
+ "waypointRelativePos": 1.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ }
+ },
+ {
+ "name": "Hood Down",
+ "waypointRelativePos": 1.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 1.0,
+ "maxAcceleration": 0.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 2 autos",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": -0.0
+ },
+ "useDefaultConstraints": false
+}
\ No newline at end of file
--- /dev/null
+{
+ "robotWidth": 0.832,
+ "robotLength": 0.832,
+ "holonomicMode": true,
+ "pathFolders": [
+ "Center Autos",
+ "Field Testing Autos",
+ "Left Autos",
+ "Miscellaneous",
+ "week 5 (new stuff)",
+ "Right Autos",
+ "week 2 autos"
+ ],
+ "autoFolders": [
+ "Week 5 autos",
+ "Testing",
+ "Week 2 autos"
+ ],
+ "defaultMaxVel": 3.0,
+ "defaultMaxAccel": 2.5,
+ "defaultMaxAngVel": 200.0,
+ "defaultMaxAngAccel": 300.0,
+ "defaultNominalVoltage": 12.0,
+ "robotMass": 63.37,
+ "robotMOI": 6.6,
+ "robotTrackwidth": 0.546,
+ "driveWheelRadius": 0.05,
+ "driveGearing": 7.03,
+ "maxDriveSpeed": 5.41,
+ "driveMotorType": "krakenX60",
+ "driveCurrentLimit": 52.0,
+ "wheelCOF": 0.9,
+ "flModuleX": 0.2635,
+ "flModuleY": 0.2635,
+ "frModuleX": 0.2635,
+ "frModuleY": -0.2635,
+ "blModuleX": -0.2635,
+ "blModuleY": 0.2635,
+ "brModuleX": -0.2635,
+ "brModuleY": -0.2635,
+ "bumperOffsetX": 0.0,
+ "bumperOffsetY": 0.0,
+ "robotFeatures": [
+ "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.55,\"y\":0.0},\"size\":{\"width\":0.5967,\"length\":0.254},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}"
+ ]
+}
\ No newline at end of file
--- /dev/null
+// 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.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.
+ *
+ * <p>If you change your main robot class, change the parameter type.
+ */
+ public static void main(String... args) {
+ RobotBase.startRobot(Robot::new);
+ }
+}
--- /dev/null
+// 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 java.util.Optional;
+
+import org.littletonrobotics.junction.LogFileUtil;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGReader;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+
+import au.grapplerobotics.CanBridge;
+import edu.wpi.first.net.PortForwarder;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.constants.Constants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.util.BuildData;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends LoggedRobot {
+ private Command autoCommand;
+ private RobotContainer robotContainer;
+
+ public Robot(){
+ CanBridge.runTCP();
+ PortForwarder.add(5800, Constants.VISION_CAMERA_HOST, 5800);
+ PortForwarder.add(1182, Constants.VISION_CAMERA_HOST, 1182);
+
+ // Set up data receivers & replay source
+ switch (Constants.CURRENT_MODE) {
+ case REAL:
+ // Running on a real robot, log to a USB stick ("/U/logs")
+ Logger.addDataReceiver(new WPILOGWriter());
+ Logger.addDataReceiver(new NT4Publisher());
+ break;
+
+ case SIM:
+ // Running a physics simulator, log to NT
+ Logger.addDataReceiver(new NT4Publisher());
+ break;
+
+ case REPLAY:
+ // Replaying a log, set up replay source
+ setUseTiming(false); // Run as fast as possible
+ String logPath = LogFileUtil.findReplayLog();
+ Logger.setReplaySource(new WPILOGReader(logPath));
+ Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
+ break;
+ }
+ Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may be added.
+ }
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // To Set the Robot Identity
+ // SimGUI: Persistent Values, Preferences, RobotId, then restart Simulation
+ // changes networktables.json, networktables.json.bck (both Untracked)
+ // Uncomment the next line, set the desired RobotId, deploy, and then comment the line out
+ // RobotId.setRobotId(RobotId.SwerveCompetition);
+
+ RobotController.setBrownoutVoltage(6.0);
+ // obtain this robot's identity
+ RobotId robotId = RobotId.getRobotId();
+
+ DriveConstants.update(robotId);
+
+ // Record metadata
+ Logger.recordMetadata("ProjectName", BuildData.MAVEN_NAME);
+ Logger.recordMetadata("BuildDate", BuildData.BUILD_DATE);
+ Logger.recordMetadata("GitSHA", BuildData.GIT_SHA);
+ Logger.recordMetadata("GitDate", BuildData.GIT_DATE);
+ Logger.recordMetadata("GitBranch", BuildData.GIT_BRANCH);
+ switch (BuildData.DIRTY) {
+ case 0:
+ Logger.recordMetadata("GitDirty", "All changes committed");
+ break;
+ case 1:
+ Logger.recordMetadata("GitDirty", "Uncomitted changes");
+ break;
+ default:
+ Logger.recordMetadata("GitDirty", "Unknown");
+ break;
+ }
+
+ robotContainer = new RobotContainer(robotId);
+
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode-specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+
+ CommandScheduler.getInstance().run();
+
+ robotContainer.logComponents();
+ robotContainer.periodic();
+ }
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically when the robot is disabled
+ */
+ @Override
+ public void disabledPeriodic() {
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ // Disable vision if the constant is false.
+ robotContainer.setVisionEnabled(VisionConstants.ENABLED_AUTO);
+
+ // Get the autonomous command.
+ // This access is fast (about 14 microseconds) because the value is already resident in the Network Tables.
+ // There was a problem last year because the operation also installed about over a dozen items (taking more than 20 ms).
+ autoCommand = robotContainer.getAutoCommand();
+
+ // If there is an autonomous command, then schedule it
+ if (autoCommand != null) {
+ CommandScheduler.getInstance().schedule(autoCommand);
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ }
+
+
+ /**
+ * This function is called once each time the robot enters Teleop mode.
+ */
+ @Override
+ public void teleopInit() {
+ robotContainer.setVisionEnabled(true);
+
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (autoCommand != null) {
+ autoCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+ }
+
+ /**
+ * This function is called once each time the robot enters Test mode.
+ */
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+
+ @Override
+ public void simulationPeriodic() {
+ }
+
+ /**
+ * Gets the set Alliance; defaults to red if not set.
+ * This method replaces {@link edu.first.wpilibj.DriverStation.getAlliance}.
+ * The .get() is not necessary, so DriverStation.getAlliance().get() becomes Robot.getAlliance()
+ */
+ public static Alliance getAlliance() {
+ Optional<Alliance> dsAlliance = DriverStation.getAlliance();
+ if (dsAlliance.isPresent())
+ return dsAlliance.get();
+ else
+ return Alliance.Red; // default to Red alliance
+ }
+}
--- /dev/null
+package frc.robot;
+
+import java.util.function.BooleanSupplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.AutoBuilderException;
+import com.pathplanner.lib.auto.NamedCommands;
+import com.pathplanner.lib.commands.PathPlannerAuto;
+
+import choreo.auto.AutoChooser;
+import choreo.auto.AutoFactory;
+import choreo.auto.AutoRoutine;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import frc.robot.commands.DoNothing;
+import frc.robot.commands.LogCommand;
+import frc.robot.commands.auto_comm.ChoreoPathCommandBuilder;
+import frc.robot.commands.auto_comm.DynamicAutoBuilder;
+import frc.robot.commands.drive_comm.DefaultDriveCommand;
+import frc.robot.commands.drive_comm.SysIDDriveCommand;
+import frc.robot.commands.gpm.IntakeMovementCommand;
+import frc.robot.commands.gpm.LockedShoot;
+import frc.robot.commands.gpm.RunSpindexer;
+import frc.robot.commands.gpm.Superstructure;
+import frc.robot.commands.vision.ShutdownAllPis;
+import frc.robot.constants.AutoConstants;
+import frc.robot.constants.Constants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.controls.BaseDriverConfig;
+import frc.robot.controls.Operator;
+import frc.robot.controls.PS5ControllerDriverConfig;
+import frc.robot.subsystems.Intake.Intake;
+import frc.robot.subsystems.PowerControl.EMABreaker;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.drivetrain.GyroIOPigeon2;
+import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.spindexer.Spindexer;
+import frc.robot.subsystems.turret.Turret;
+import frc.robot.util.PathGroupLoader;
+import frc.robot.util.Vision.DetectedObject;
+import frc.robot.util.Vision.Vision;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in
+ * the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of
+ * the robot (including
+ * subsystems, commands, and trigger mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems are defined here...
+ private Drivetrain drive = null;
+ private Vision vision = null;
+ private Turret turret = null;
+ private Shooter shooter = null;
+ private Hood hood = null;
+ private Spindexer spindexer = null;
+ private Intake intake = null;
+ // private LED led = null;
+
+ // Controllers are defined here
+ private BaseDriverConfig driver = null;
+ private Operator operator = null;
+
+ private EMABreaker breaker = null;
+
+ // auto Command selection
+ private final SendableChooser<Command> autoChooser = new SendableChooser<>();
+ private final AutoChooser choreoAutoChooser = new AutoChooser();
+
+ // choreo auto factory
+ AutoFactory autoFactory ;
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ * <p>
+ * Different robots may have different subsystems.
+ */
+ public RobotContainer(RobotId robotId) {
+ // display the current robot id on smartdashboard
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putString("RobotID", robotId.toString());
+
+ SmartDashboard.putNumber("Match Time", 0.0);
+ }
+
+
+ // Filling the SendableChooser on SmartDashboard
+
+ // dispatch on the robot
+ switch (robotId) {
+ case TestBed1:
+ break;
+
+ case TestBed2:
+ break;
+
+ default:
+
+ case TwinBot:
+
+
+ case PrimeJr: // AKA Valence
+ spindexer = new Spindexer();
+ intake = new Intake();
+ // led = new LED();
+ breaker = new EMABreaker();
+
+ case WaffleHouse: // AKA Betabot
+ turret = new Turret();
+ shooter = new Shooter();
+ hood = new Hood();
+
+ case SwerveCompetition: // AKA "Vantage"
+
+ case BetaBot: // AKA "Pancake"
+ vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS);
+ // fall-through
+
+ case Vivace:
+
+ case Phil: // AKA "IHOP"
+
+ case Vertigo: // AKA "French Toast"
+ drive = new Drivetrain(vision, new GyroIOPigeon2());
+ driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood, intake, spindexer);
+ operator = new Operator(drive);
+
+ initChoreo();
+
+ // Detected objects need access to the drivetrain
+ DetectedObject.setDrive(drive);
+
+ // SignalLogger.start();
+ driver.configureControls();
+ operator.configureControls();
+
+ registerCommands();
+ PathGroupLoader.loadPathGroups();
+
+ initializeAutoBuilder();
+ autoChooserInit();
+
+ if (turret != null && drive != null && hood != null && shooter != null) {
+ SmartDashboard.putData("Lock Shooting", new LockedShoot(turret, drive, hood, shooter));
+ }
+
+ if (turret != null) {
+ turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer));
+ }
+
+ if (drive != null && driver != null) {
+ // drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
+ SmartDashboard.putData("SysId Characterization", new SysIDDriveCommand(drive));
+ }
+ break;
+ }
+
+
+ if (intake != null && hood != null && turret != null)
+ // CommandScheduler.getInstance().schedule(new HardstopWarning(hood, intake, turret)); (no more crt for this)
+ // This is really annoying so it's disabled
+ DriverStation.silenceJoystickConnectionWarning(true);
+
+ CommandScheduler.getInstance().schedule(new LogCommand());
+
+ // TODO: verify this claim.
+ // LiveWindow is causing periodic loop overruns
+ LiveWindow.disableAllTelemetry();
+ LiveWindow.setEnabled(false);
+
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putData("Shutdown Orange Pis", new ShutdownAllPis());
+ }
+ }
+
+ private void initChoreo() {
+ // choreo auto factory init
+ autoFactory = new AutoFactory(
+ drive::getPose,
+ drive::resetOdometry,
+ sample -> drive.setChassisSpeeds(sample.getChassisSpeeds(), false),
+ true,
+ drive,
+ (trajectory, startOrFinish) -> {
+ Logger.recordOutput(
+ "Autos/Trajectory", trajectory.getPoses());
+ Logger.recordOutput("Autos/StartingOrFinishing", startOrFinish);
+ });
+
+ autoFactory.bind("hoodUp", new InstantCommand(() -> hood.forceHoodDown(false)));
+ autoFactory.bind("hoodDown", new InstantCommand(() -> hood.forceHoodDown(true)));
+
+ // warmup command for choreo, prevents lag on auto startup
+ CommandScheduler.getInstance().schedule(autoFactory.warmupCmd().ignoringDisable(true));
+ }
+
+ /**
+ * Sets whether the drivetrain uses vision toupdate odometry
+ */
+ public void setVisionEnabled(boolean enabled) {
+ if (drive != null)
+ drive.setVisionEnabled(enabled);
+ }
+
+ public void initializeAutoBuilder() {
+ AutoBuilder.configure(
+ () -> drive.getPose(),
+ (pose) -> {
+ drive.resetOdometry(pose);
+ },
+ () -> drive.getChassisSpeeds(),
+ (chassisSpeeds) -> {
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Auto/ChassisSpeeds", chassisSpeeds);
+ }
+ drive.setChassisSpeeds(chassisSpeeds, false); // problem??
+ },
+ AutoConstants.AUTO_CONTROLLER,
+ AutoConstants.CONFIG,
+ getAllianceColorBooleanSupplier(),
+ drive);
+ }
+
+ private boolean seizing;
+
+ public void registerCommands() {
+
+ if (intake != null) {
+
+ NamedCommands.registerCommand("Extend Intake", new InstantCommand(() -> {
+ intake.extend();
+ }));
+ NamedCommands.registerCommand("Retract Intake", new InstantCommand(() -> intake.retract()));
+ NamedCommands.registerCommand("Intermediate Extend", new InstantCommand(() -> intake.intermediateExtend()));
+ NamedCommands.registerCommand("Spin Intake Rollers", new InstantCommand(() -> intake.spinStart()));
+ NamedCommands.registerCommand("Stop Intake Rollers", new InstantCommand(() -> intake.spinStop()));
+
+ NamedCommands.registerCommand("Start Intake Seizure", new InstantCommand(() -> {
+ seizing = true;
+ CommandScheduler.getInstance().schedule(new IntakeMovementCommand(intake).until(() -> !seizing));
+ }));
+ NamedCommands.registerCommand("Stop Intake Seizure", new InstantCommand(() -> {
+ seizing = false;
+ }));
+ }
+
+ if (turret != null && drive != null && hood != null && shooter != null && spindexer != null && intake != null) {
+ Command runSpindexer = new RunSpindexer(spindexer, turret, hood, intake);
+ NamedCommands.registerCommand("Start Spindexer",
+ new InstantCommand(() -> CommandScheduler.getInstance().schedule(runSpindexer)));
+ NamedCommands.registerCommand("Stop Spindexer", new InstantCommand(() -> runSpindexer.cancel()));
+ }
+
+ if (hood != null) {
+
+ NamedCommands.registerCommand("Hood Down", new InstantCommand(() -> {
+ hood.forceHoodDown(true);
+ }));
+ NamedCommands.registerCommand("Stop Hood Down", new InstantCommand(() -> {
+ hood.forceHoodDown(false);
+ }));
+ }
+
+ NamedCommands.registerCommand("After Depot", new InstantCommand());
+ NamedCommands.registerCommand("Constraints Zone", new InstantCommand());
+ NamedCommands.registerCommand("Depot", new InstantCommand());
+ NamedCommands.registerCommand("Reset Spindexer", new InstantCommand());
+ }
+
+ public void addAuto(String name) {
+ try {
+ Command auto = new PathPlannerAuto(name);
+ autoChooser.addOption(name, auto);
+ }
+ // is this the right one??
+ catch (AutoBuilderException e) {
+ e.printStackTrace();
+ System.out.println("HELLOOOO AUTO \"" + name + "\" NOT FOUND");
+ }
+ }
+
+ public void addAuto(String name, Command auto) {
+ try {
+ autoChooser.addOption(name, auto);
+ } catch (AutoBuilderException e){
+ e.printStackTrace();
+ System.out.println("HELLOOOO AUTO \"" + name + "\" NOT FOUND");
+ }
+ }
+
+ public void addChoreoAuto(String name, AutoRoutine auto) {
+ choreoAutoChooser.addCmd(name, auto::cmd);
+ }
+
+ /**
+ * Initialize the SendableChooser on the SmartDashboard.
+ * Fill the SendableChooser with available Commands.
+ */
+ public void autoChooserInit() {
+ // add the options to the Chooser
+ String leftSideAuto = "Left Week V1";
+ String rightSideAuto = "Right Week V1";
+ String shootOnlyAuto = "Shoot Only Left Week V1";
+ String leftLiberalSwipe = "LeftLiberalDoubleSwipe";
+ String rightLiberalSwipe = "RightLiberalDoubleSwipe";
+ String leftLiberalSwipeTranslation = "LeftLiberalDoubleSwipeTranslation";
+ String leftConservativeSwipe = "LeftConservativeDoubleSwipe";
+ String leftDoNothing = "Left Do Nothing";
+ String rightDoNothing = "Right Do Nothing";
+ String centerDoNothing = "Center Do Nothing";
+ String leftShallowDoubleSwipe = "LeftShallowDoubleSwipe";
+ String rightShallowDoubleSwipe = "RightShallowDoubleSwipe";
+ String leftBumpDepotCenter = "LeftBumpDepotCenter";
+ String leftTrenchDepotCenter = "LeftTrenchDepotCenter";
+ String depotCenterPath = "DepotCenterPath";
+
+ autoChooser.setDefaultOption("Default", getDefaultAuto());
+ addAuto(leftSideAuto);
+ addAuto(rightSideAuto);
+ addAuto(shootOnlyAuto);
+ addAuto(leftConservativeSwipe);
+ addAuto(leftLiberalSwipe);
+ addAuto(rightLiberalSwipe);
+ addAuto(leftLiberalSwipeTranslation);
+ addAuto(leftDoNothing);
+ addAuto(rightDoNothing);
+ addAuto(centerDoNothing);
+ addAuto(leftShallowDoubleSwipe);
+ addAuto(rightShallowDoubleSwipe);
+ addAuto(leftBumpDepotCenter);
+ addAuto(leftTrenchDepotCenter);
+ addAuto(depotCenterPath);
+
+
+ DynamicAutoBuilder dynamicAutoBuilder = new DynamicAutoBuilder(spindexer, turret, hood, intake);
+
+ // names
+ String leftDynamicLiberalDoubleSwipe = "LeftDynamicDoubleLiberalSwipe";
+ String rightDynamicLiberalDoubleSwipe = "RightDynamicDoubleLiberalSwipe";
+ String leftDynamicConservativeDoubleSwipe = "LeftDynamicDoubleConservativeSwipe";
+ String rightDynamicConservativeDoubleSwipe = "RightDynamicDoubleConservativeSwipe";
+ // String leftDynamicShallowDoubleSwipe = "LeftDynamicShallowDoubleSwipe";
+ // String rightDynamicShallowDoubleSwipe = "RightDynamicShallowDoubleSwipe";
+
+ // add commands
+ addAuto(leftDynamicLiberalDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleLiberalSwipe(true));
+ addAuto(rightDynamicLiberalDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleLiberalSwipe(false));
+ addAuto(leftDynamicConservativeDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleConservativeSwipe(true));
+ addAuto(rightDynamicConservativeDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleConservativeSwipe(false));
+
+ ChoreoPathCommandBuilder choreo = new ChoreoPathCommandBuilder(intake, spindexer, turret, hood);
+
+ addAuto("testChoreo", ChoreoPathCommandBuilder.basicTrajectoryAuto("test.traj", true, autoFactory));
+ addChoreoAuto("choreoLiberalLeft", choreo.leftLiberal(autoFactory));
+ addChoreoAuto("choreoLiberalRight", choreo.rightLiberal(autoFactory));
+ addChoreoAuto("choreoConservativeLeft", choreo.leftConservative(autoFactory));
+ addChoreoAuto("choreoConservativeRight", choreo.rightConservative(autoFactory));
+ addChoreoAuto("choreoShallowLeft", choreo.leftShallow(autoFactory));
+ addChoreoAuto("choreoShallowRight", choreo.rightShallow(autoFactory));
+
+ // put the Chooser on the SmartDashboard
+ SmartDashboard.putData("Auto chooser", autoChooser);
+ SmartDashboard.putData("Choreo auto chooser", choreoAutoChooser);
+ }
+
+ public static BooleanSupplier getAllianceColorBooleanSupplier() {
+ return () -> {
+ // Boolean supplier that controls when the path will be mirrored for the red
+ // alliance
+ // This will flip the path being followed to the red side of the field.
+ // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+
+ var alliance = DriverStation.getAlliance();
+ if (alliance.isPresent()) {
+ return alliance.get() == DriverStation.Alliance.Red;
+ }
+ return false;
+ };
+ }
+
+ public boolean brownout() {
+ if (RobotController.getBatteryVoltage() < 6.0) {
+ return true;
+ } else {
+ return false;
+ }
+ }
+
+ public Command getDefaultAuto() {
+ if (spindexer == null || turret == null || hood == null || intake == null) {
+ return new DoNothing();
+ }
+ ParallelCommandGroup defaultShoot = new ParallelCommandGroup(
+ new RunSpindexer(spindexer, turret, hood, intake)
+ );
+ return defaultShoot;
+ }
+
+ public Command getAutoCommand() {
+ // return autoChooser.getSelected();
+ return choreoAutoChooser.selectedCommand();
+ }
+
+ public void logComponents() {
+ if (!Constants.LOG_MECHANISMS)
+ return;
+
+ Logger.recordOutput(
+ "ComponentPoses",
+ new Pose3d[] {
+ // Subsystem Pose3ds
+ });
+ }
+
+ public void periodic() {
+
+ }
+}
--- /dev/null
+package frc.robot;
+
+import edu.wpi.first.wpilibj.Preferences;
+
+/**
+ * Set of known Robot Names.
+ * <p>The name of a robot in the RoboRIO's persistent memory.
+ * At deploy time, that name is used to set the corresponding RobotId.
+ * <p>Note that the RobotId is determined at Deploy time.
+ */
+public enum RobotId {
+ Default,
+ PrimeJr, WaffleHouse, TwinBot, SwerveCompetition, Vertigo, Vivace, Phil, BetaBot,
+ ClassBot1, ClassBot2, ClassBot3, ClassBot4,
+ TestBed1, TestBed2;
+
+ /** The key used to access the RobotId name in the RoboRIO's persistent memory. */
+ public static final String ROBOT_ID_KEY = "RobotId";
+
+ /**
+ * Is this robot a classbot?
+ * @return true if a classbot
+ * @deprecated this method is not needed....
+ */
+ @Deprecated
+ public boolean isClassBot() {
+ return this == WaffleHouse || this == ClassBot1 || this == ClassBot2 || this == ClassBot3 || this == ClassBot4;
+ }
+
+ /**
+ * Whether this robot is a swerve bot
+ * @return true if a swerve bot
+ * @deprecated this method is not needed....
+ */
+ @Deprecated
+ public boolean isSwerveBot() {
+ return this == SwerveCompetition || this == Phil || this == Vertigo || this == Vivace || this == BetaBot;
+ }
+
+ /**
+ * Determine the Robot Identity from the RoboRIO's onboard Preferences (flash memory).
+ * @returns the RobotId
+ */
+ public static RobotId getRobotId() {
+ // assume a default identity
+ RobotId robotId = RobotId.Default;
+
+ // check whether Preferences has an entry for the RobotId
+ if (!Preferences.containsKey(ROBOT_ID_KEY)) {
+ // There is no such key. Set it to the default identity.
+ // This step guarantees persistent memory will have a key.
+ setRobotId(RobotId.Default);
+ }
+
+ // Remove the "Default" key if present.
+ // This key was the result of a programming error in 2023.
+ if (Preferences.containsKey("Default")) {
+ Preferences.remove("Default");
+ }
+
+ // get the RobotId string from the RoboRIO's Preferences
+ String strId = Preferences.getString(ROBOT_ID_KEY, RobotId.Default.name());
+
+ // match that string to a RobotId by looking at all possible RobotId enums
+ for (RobotId rid : RobotId.values()) {
+ // does the preference string match the RobotId enum?
+ if (strId.equals(rid.name())) {
+ // yes, this instance is the desired RobotId
+ robotId = rid;
+ break;
+ }
+ }
+
+ if (robotId == RobotId.Default) {
+ if (Robot.isSimulation()) {
+ robotId = RobotId.SwerveCompetition; // Default to competition robot for simulation
+ } else {
+ throw new RuntimeException("RobotId is set to Default (or was unset)! Please set it to something.");
+ }
+ }
+
+ // return the robot identity
+ return robotId;
+ }
+
+ /**
+ * Set the RobotId in the RoboRIO's preferences (flash memory).
+ * <p>
+ * Calling it after the robot has been constructed (robotInit()) does not affect the robot.
+ */
+ static void setRobotId(RobotId robotId) {
+ // Set the robot identity in the RoboRIO Preferences
+ Preferences.setString(ROBOT_ID_KEY, robotId.name());
+ }
+}
--- /dev/null
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+/**
+ * Does nothing. Can be used to more clearly mark commands intended not to do anything.
+ */
+public class DoNothing extends InstantCommand {
+}
--- /dev/null
+package frc.robot.commands;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.Constants;
+import frc.robot.util.Elastic;
+import frc.robot.util.HubActive;
+import frc.robot.util.Elastic.Notification;
+import frc.robot.util.Elastic.NotificationLevel;
+
+public class LogCommand extends Command {
+
+ private boolean hubActive = false;
+
+ public LogCommand() {
+ }
+
+ @Override
+ public void execute() {
+ if (Constants.DISABLE_LOGGING) {
+ return;
+ }
+
+ boolean current = HubActive.isHubActive();
+ Logger.recordOutput("HubActive", current);
+
+ if (current && !hubActive) {
+ Elastic.sendNotification(new Notification(NotificationLevel.INFO, "HUB ACTIVE", ""));
+ } else if (!current && hubActive) {
+ Elastic.sendNotification(new Notification(NotificationLevel.INFO, "HUB DEACTIVATED", ""));
+ }
+
+ hubActive = current;
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+}
--- /dev/null
+package frc.robot.commands;
+
+import com.ctre.phoenix6.Orchestra;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class Music extends Command {
+ private Orchestra orchestra;
+
+ public Music(TalonFX[] motors) {
+ orchestra = new Orchestra(Filesystem.getDeployDirectory() + "/chirp/file.chrp");
+ for (TalonFX motor : motors) {
+ System.out.println(motor.getDescription());
+ orchestra.addInstrument(motor);
+ }
+ }
+
+ @Override
+ public void initialize() {
+ orchestra.play();
+ }
+
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ orchestra.stop();
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+}
--- /dev/null
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+
+import java.util.function.Supplier;
+
+/**
+ * Runs the given command when this command is initialized, and ends when it ends.
+ * Useful for commands that are not created yet because the constructor parameters are not available until initialization.
+ * This is very similar to WPILib's DeferredCommand
+ */
+public class SupplierCommand extends Command {
+
+ private final Supplier<Command> commandSupplier;
+ private Command command;
+
+ /**
+ * Runs the given command when this command is initialized, and ends when it ends.
+ * Useful for commands that are not created yet because the constructor parameters are not available until initialization.
+ *
+ * @param commandSupplier A Supplier to the command to run
+ * @param Subsystem all subsystems that may be required to run the command supplied
+ */
+ public SupplierCommand(Supplier<Command> commandSupplier, Subsystem... Subsystem) {
+ addRequirements(Subsystem);
+ this.commandSupplier = commandSupplier;
+ }
+
+ @Override
+ public final void initialize() {
+ command = commandSupplier.get();
+ command.initialize();
+ }
+
+ @Override
+ public final void execute() {
+ command.execute();
+ }
+
+ @Override
+ public final void end(boolean interrupted) {
+ command.end(interrupted);
+ }
+
+ @Override
+ public final boolean isFinished() {
+ return command.isFinished();
+ }
+
+}
--- /dev/null
+package frc.robot.commands.auto_comm;
+
+import choreo.auto.AutoFactory;
+import choreo.auto.AutoRoutine;
+import choreo.auto.AutoTrajectory;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.commands.DoNothing;
+import frc.robot.commands.gpm.IntakeMovementCommand;
+import frc.robot.commands.gpm.RunSpindexer;
+import frc.robot.commands.gpm.RunSpindexerWithStop;
+import frc.robot.subsystems.Intake.Intake;
+import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.spindexer.Spindexer;
+import frc.robot.subsystems.turret.Turret;
+
+public class ChoreoPathCommandBuilder {
+
+ private Intake intake;
+ private Spindexer spindexer;
+ private Turret turret;
+ private Hood hood;
+
+ public ChoreoPathCommandBuilder(Intake intake, Spindexer spindexer, Turret turret, Hood hood) {
+ this.intake = intake;
+ this.spindexer = spindexer;
+ this.turret = turret;
+
+ }
+
+ public static Command basicTrajectoryAuto(String pathName, boolean resetOdemetry, AutoFactory factory) {
+ Command command = factory.trajectoryCmd(pathName);
+
+ return Commands.sequence(
+ resetOdemetry ? new InstantCommand(() -> factory.resetOdometry(pathName)) : new DoNothing(),
+ command);
+ }
+
+ public AutoRoutine leftLiberal(AutoFactory factory) {
+ AutoRoutine routine = factory.newRoutine("leftLiberal");
+
+ AutoTrajectory liberalSwipe = routine.trajectory("liberal", 0);
+ AutoTrajectory shallowSwipe = routine.trajectory("liberal", 1);
+
+ routine.active().onTrue(
+ Commands.sequence(
+ liberalSwipe.resetOdometry(),
+ new InstantCommand(() -> {
+ intake.extend();
+ intake.spinStart();
+ hood.forceHoodDown(true);
+ }),
+ liberalSwipe.cmd()));
+
+ liberalSwipe.done()
+ .onTrue(Commands.sequence(
+ new InstantCommand(() -> {
+ hood.forceHoodDown(false);
+ }),
+ new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+ new InstantCommand(() -> {
+ intake.extend();
+ intake.spinStart();
+ hood.forceHoodDown(true);
+ }),
+ shallowSwipe.cmd()));
+
+ shallowSwipe.done()
+ .onTrue(Commands.sequence(
+ new InstantCommand(() -> {
+ hood.forceHoodDown(false);
+ }),
+ new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+ new InstantCommand(() -> {
+ intake.extend();
+ intake.spinStart();
+ hood.forceHoodDown(true);
+ }),
+ shallowSwipe.cmd()));
+
+ return routine;
+
+ }
+
+ public AutoRoutine rightLiberal(AutoFactory factory) {
+ AutoRoutine routine = factory.newRoutine("rightLiberal");
+
+ AutoTrajectory liberalSwipe = routine.trajectory("liberal", 0).mirrorY();
+ AutoTrajectory shallowSwipe = routine.trajectory("liberal", 1).mirrorY();
+
+ routine.active().onTrue(
+ Commands.sequence(
+ liberalSwipe.resetOdometry(),
+ new InstantCommand(() -> {
+ intake.extend();
+ intake.spinStart();
+ }),
+ liberalSwipe.cmd()));
+
+ liberalSwipe.done()
+ .onTrue(Commands.sequence(
+ new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+ shallowSwipe.cmd()));
+
+ shallowSwipe.done()
+ .onTrue(Commands.sequence(
+ new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+ shallowSwipe.cmd()));
+
+ return routine;
+
+ }
+
+ public AutoRoutine leftConservative(AutoFactory factory) {
+ AutoRoutine routine = factory.newRoutine("leftConservative");
+
+ AutoTrajectory liberalSwipe = routine.trajectory("conservative", 0);
+ AutoTrajectory shallowSwipe = routine.trajectory("conservative", 1);
+
+ routine.active().onTrue(
+ Commands.sequence(
+ liberalSwipe.resetOdometry(),
+ new InstantCommand(() -> {
+ intake.extend();
+ intake.spinStart();
+ hood.forceHoodDown(true);
+ }),
+ liberalSwipe.cmd()));
+
+ liberalSwipe.done()
+ .onTrue(Commands.sequence(
+ new InstantCommand(() -> {
+ hood.forceHoodDown(false);
+ }),
+ new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+ new InstantCommand(() -> {
+ intake.extend();
+ intake.spinStart();
+ hood.forceHoodDown(true);
+ }),
+ shallowSwipe.cmd()));
+
+ shallowSwipe.done()
+ .onTrue(Commands.sequence(
+ new InstantCommand(() -> {
+ hood.forceHoodDown(false);
+ }),
+ new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+ new InstantCommand(() -> {
+ intake.extend();
+ intake.spinStart();
+ hood.forceHoodDown(true);
+ }),
+ shallowSwipe.cmd()));
+
+ return routine;
+
+ }
+
+ public AutoRoutine rightConservative(AutoFactory factory) {
+ AutoRoutine routine = factory.newRoutine("rightConservative");
+
+ AutoTrajectory liberalSwipe = routine.trajectory("conservative", 0).mirrorY();
+ AutoTrajectory shallowSwipe = routine.trajectory("conservative", 1).mirrorY();
+
+ routine.active().onTrue(
+ Commands.sequence(
+ liberalSwipe.resetOdometry(),
+ new InstantCommand(() -> {
+ intake.extend();
+ intake.spinStart();
+ hood.forceHoodDown(true);
+ }),
+ liberalSwipe.cmd()));
+
+ liberalSwipe.done()
+ .onTrue(Commands.sequence(
+ new InstantCommand(() -> {
+ hood.forceHoodDown(false);
+ }),
+ new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+ new InstantCommand(() -> {
+ intake.extend();
+ intake.spinStart();
+ hood.forceHoodDown(true);
+ }),
+ shallowSwipe.cmd()));
+
+ shallowSwipe.done()
+ .onTrue(Commands.sequence(
+ new InstantCommand(() -> {
+ hood.forceHoodDown(false);
+ }),
+ new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+ new InstantCommand(() -> {
+ intake.extend();
+ intake.spinStart();
+ hood.forceHoodDown(true);
+ }),
+ shallowSwipe.cmd()));
+
+ return routine;
+
+ }
+
+ public AutoRoutine leftShallow(AutoFactory factory) {
+ AutoRoutine routine = factory.newRoutine("leftShallow");
+
+ AutoTrajectory shallowSwipe = routine.trajectory("shallowSwipe");
+
+ routine.active().onTrue(
+ Commands.sequence(
+ shallowSwipe.resetOdometry(),
+ new InstantCommand(() -> {
+ intake.extend();
+ intake.spinStart();
+ hood.forceHoodDown(true);
+ }),
+ shallowSwipe.cmd()));
+
+ shallowSwipe.done()
+ .onTrue(Commands.sequence(
+ new InstantCommand(() -> {
+ hood.forceHoodDown(false);
+ }),
+ new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+ new InstantCommand(() -> {
+ intake.extend();
+ intake.spinStart();
+ hood.forceHoodDown(true);
+ }),
+ shallowSwipe.cmd()));
+
+ return routine;
+
+ }
+
+ public AutoRoutine rightShallow(AutoFactory factory) {
+ AutoRoutine routine = factory.newRoutine("rightShallow");
+
+ AutoTrajectory shallowSwipe = routine.trajectory("shallowSwipe").mirrorY();
+
+ routine.active().onTrue(
+ Commands.sequence(
+ shallowSwipe.resetOdometry(),
+ new InstantCommand(() -> {
+ intake.extend();
+ intake.spinStart();
+ hood.forceHoodDown(true);
+ }),
+ shallowSwipe.cmd()));
+
+ shallowSwipe.done()
+ .onTrue(Commands.sequence(
+ new InstantCommand(() -> {
+ hood.forceHoodDown(false);
+ }),
+ new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+ new InstantCommand(() -> {
+ intake.extend();
+ intake.spinStart();
+ hood.forceHoodDown(true);
+ }),
+ shallowSwipe.cmd()));
+
+ return routine;
+
+ }
+
+ public AutoRoutine leftSuperShuttling(AutoFactory factory) {
+ AutoRoutine routine = factory.newRoutine("leftSuperShuttling");
+
+ AutoTrajectory shuttlingTrajectory = routine.trajectory("superShuttling");
+
+ routine.active().onTrue(Commands.sequence(
+ shuttlingTrajectory.resetOdometry(),
+ new InstantCommand(() -> {
+ intake.extend();
+ intake.spinStart();
+ }),
+ shuttlingTrajectory.cmd()));
+
+ routine.active().whileTrue(new RunSpindexer(spindexer, turret, hood, intake));
+
+ shuttlingTrajectory.done().onTrue(Commands.sequence(
+ new InstantCommand(() -> {
+ hood.forceHoodDown(false);
+ }),
+ new WaitCommand(2.0).raceWith(new IntakeMovementCommand(intake)),
+ new InstantCommand(() -> {
+ intake.extend();
+ intake.spinStart();
+ hood.forceHoodDown(true);
+ }),
+ shuttlingTrajectory.cmd()));
+
+ return routine;
+
+ }
+
+ public AutoRoutine rightSuperShuttling(AutoFactory factory) {
+ AutoRoutine routine = factory.newRoutine("rightSuperShuttling");
+
+ AutoTrajectory shuttlingTrajectory = routine.trajectory("superShuttling");
+
+ routine.active().onTrue(Commands.sequence(
+ shuttlingTrajectory.resetOdometry(),
+ new InstantCommand(() -> {
+ intake.extend();
+ intake.spinStart();
+ }),
+ shuttlingTrajectory.cmd()));
+
+ routine.active().whileTrue(new RunSpindexer(spindexer, turret, hood, intake));
+
+ shuttlingTrajectory.done().onTrue(Commands.sequence(
+ new InstantCommand(() -> {
+ hood.forceHoodDown(false);
+ }),
+ new WaitCommand(2.0).raceWith(new IntakeMovementCommand(intake)),
+ new InstantCommand(() -> {
+ intake.extend();
+ intake.spinStart();
+ hood.forceHoodDown(true);
+ }),
+ shuttlingTrajectory.cmd()));
+
+ return routine;
+
+ }
+}
--- /dev/null
+package frc.robot.commands.auto_comm;
+
+import edu.wpi.first.wpilibj2.command.*;
+import frc.robot.commands.gpm.RunSpindexerWithStop;
+import frc.robot.subsystems.Intake.Intake;
+import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.spindexer.Spindexer;
+import frc.robot.subsystems.turret.Turret;
+
+import com.pathplanner.lib.commands.PathPlannerAuto;
+
+public class DynamicAutoBuilder {
+
+ private final Spindexer spindexer;
+ private final Turret turret;
+ private final Hood hood;
+ private final Intake intake;
+
+ public DynamicAutoBuilder(Spindexer spindexer, Turret turret, Hood hood, Intake intake) {
+ this.spindexer = spindexer;
+ this.turret = turret;
+ this.hood = hood;
+ this.intake = intake;
+ }
+
+ /*
+ * Autos have no named commands within them. They must be added here
+ * Still need to make one method to call that four command block in each
+ * sequential
+ */
+
+ public Command getDynamicDoubleLiberalSwipe(boolean left) {
+ return new SequentialCommandGroup(
+ departCommand(),
+ new PathPlannerAuto(left ? "LeftSwipeOne" : "RightSwipeOne"),
+ startShootingCommand(),
+ runSpindexerWithAbort(),
+
+ departCommand(),
+ new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"),
+ startShootingCommand(),
+ runSpindexerWithAbort(),
+
+ departCommand(),
+ new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"),
+ startShootingCommand(),
+ runSpindexerWithAbort(),
+
+ departCommand(),
+ new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"),
+ startShootingCommand(),
+ runSpindexerWithAbort());
+ }
+
+ public Command getDynamicDoubleConservativeSwipe(boolean left) {
+ return new SequentialCommandGroup(
+ departCommand(),
+ new PathPlannerAuto(left ? "LeftSwipeConservative" : "RightSwipeConservative"),
+ startShootingCommand(),
+ runSpindexerWithAbort(),
+
+ departCommand(),
+ new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"),
+ startShootingCommand(),
+ runSpindexerWithAbort(),
+
+ departCommand(),
+ new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"),
+ startShootingCommand(),
+ runSpindexerWithAbort(),
+
+ departCommand(),
+ new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"),
+ startShootingCommand(),
+ runSpindexerWithAbort());
+ }
+
+ public Command getDynamicDoubleShallowSwipe(boolean left) {
+ return new SequentialCommandGroup(
+ departCommand(),
+ new PathPlannerAuto(left ? "LeftSwipeThree" : "RightSwipeThree"),
+ startShootingCommand(),
+ runSpindexerWithAbort(),
+
+ departCommand(),
+ new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"),
+ startShootingCommand(),
+ runSpindexerWithAbort(),
+
+ departCommand(),
+ new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"),
+ startShootingCommand(),
+ runSpindexerWithAbort(),
+
+ departCommand(),
+ new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"),
+ startShootingCommand(),
+ runSpindexerWithAbort());
+ }
+
+ private Command departCommand() {
+ return new InstantCommand(() -> {
+ hood.forceHoodDown(true);
+ intake.extend();
+ intake.spinStart();
+ spindexer.stopSpindexer();
+ });
+ }
+
+ private Command runSpindexerWithAbort() {
+ // has an isFinnished so works
+ return new RunSpindexerWithStop(spindexer, turret, hood, intake);
+ }
+
+ private Command startShootingCommand() {
+ return new InstantCommand(() -> hood.forceHoodDown(false));
+ }
+}
--- /dev/null
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.auto_comm;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathPlannerPath;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.RobotContainer;
+import frc.robot.commands.SupplierCommand;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.PathGroupLoader;
+
+/** Add your docs here. */
+public class FollowPathCommand extends SequentialCommandGroup {
+ Drivetrain drive;
+ PathPlannerPath path;
+
+
+ public FollowPathCommand(String name, Drivetrain drive){
+ this(name, false, drive);
+ }
+
+ public FollowPathCommand(String pathName, boolean resetOdemetry, Drivetrain drive){
+ this.drive = drive;
+ this.path = PathGroupLoader.getPathGroup(pathName);
+ addCommands(
+ new InstantCommand(()->resetOdemetry(resetOdemetry)),
+ new SupplierCommand(()->AutoBuilder.followPath(path), drive) // "problem" (254)
+ // or pp's interaction with the drivetrain
+ // or pp config
+ );
+ }
+
+ public void resetOdemetry(boolean resetOdemetry){
+ if (resetOdemetry){
+ if(RobotContainer.getAllianceColorBooleanSupplier().getAsBoolean()){
+ drive.resetOdometry(new Pose2d(path.getAllPathPoints().get(0).flip().position, path.getIdealStartingState().flip().rotation()));
+ }else{
+ drive.resetOdometry(new Pose2d(path.getAllPathPoints().get(0).position, path.getIdealStartingState().rotation()));
+ }
+ }
+ }
+}
--- /dev/null
+package frc.robot.commands.drive_comm;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Robot;
+import frc.robot.constants.Constants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.controls.BaseDriverConfig;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.TrenchAssist.TrenchAssist;
+import frc.robot.util.TrenchAssist.TrenchAssistConstants;
+import frc.robot.util.Vision.DriverAssist;
+
+/**
+ * Default drive command. Drives robot using driver controls.
+ */
+public class DefaultDriveCommand extends Command {
+ protected final Drivetrain swerve;
+ private final BaseDriverConfig driver;
+ private PIDController trenchAssistPid = new PIDController(9, 0.0, 3);
+
+ public DefaultDriveCommand(
+ Drivetrain swerve,
+ BaseDriverConfig driver) {
+ this.swerve = swerve;
+ this.driver = driver;
+
+ addRequirements(swerve);
+ }
+
+ @Override
+ public void initialize() {
+ swerve.setStateDeadband(true);
+
+ trenchAssistPid.setIZone(2);
+ trenchAssistPid.setIntegratorRange(-1, 1);
+
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putNumber("0 degrees snap location", 0);
+ }
+ }
+
+ @Override
+ public void execute() {
+ double forwardTranslation = driver.getForwardTranslation();
+ double sideTranslation = driver.getSideTranslation();
+ double rotation = -driver.getRotation();
+
+ double slowFactor = driver.getIsSlowMode() ? DriveConstants.SLOW_DRIVE_FACTOR : 1;
+
+ forwardTranslation *= slowFactor;
+ sideTranslation *= slowFactor;
+ rotation *= driver.getIsSlowMode() ? DriveConstants.SLOW_ROT_FACTOR : 1;
+
+ int allianceReversal = Robot.getAlliance() == Alliance.Red ? 1 : -1;
+ forwardTranslation *= allianceReversal;
+ sideTranslation *= allianceReversal;
+
+ ChassisSpeeds driverInput = new ChassisSpeeds(forwardTranslation, sideTranslation, rotation);
+ ChassisSpeeds corrected = DriverAssist.calculate(swerve, driverInput, swerve.getDesiredPose(), true);
+
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("TrenchAlign", swerve.getTrenchAlign());
+ Logger.recordOutput("AlignZones", TrenchAssistConstants.ALIGN_ZONES);
+ }
+ // if (swerve.getTrenchAlign()) {
+ // boolean inZone = false;
+ // for (Rectangle2d rectangle : TrenchAssistConstants.ALIGN_ZONES) {
+ // if (rectangle.contains(swerve.getPose().getTranslation())) {
+ // inZone = true;
+ // }
+ // }
+
+ // if (inZone) {
+
+ // double yawDegrees = swerve.getYaw().getDegrees();
+ // // double snappedDeg = Math.round(yawDegrees / 90.0) * 90.0;
+ // if (Math.abs(yawDegrees) <= 90) {
+ // swerve.setAlignAngle(Units.degreesToRadians(0.0));
+ // } else {
+ // swerve.setAlignAngle(Units.degreesToRadians(180.0));
+ // }
+ // // swerve.setAlignAngle(snappedDeg);
+ // // Logger.recordOutput("snappy", snappedDeg);
+ // } else {
+ // swerve.setIsAlign(false);
+ // }
+ // } else {
+ // swerve.setIsAlign(false);
+ // }
+ // if (!Constants.DISABLE_LOGGING) {
+ // Logger.recordOutput("TrenchAssist", swerve.getTrenchAssist());
+ // }
+
+ if (swerve.getTrenchAssist()) {
+ drive(TrenchAssist.calculate(swerve, corrected, trenchAssistPid));
+ } else {
+ trenchAssistPid.reset();
+ drive(corrected);
+ }
+ }
+
+ /**
+ * Drives the robot
+ *
+ * @param speeds The ChassisSpeeds to drive at
+ */
+ protected void drive(ChassisSpeeds speeds) {
+ // If the driver is pressing the align button or a command set the drivetrain to
+ // align, then align to speaker
+ if (driver.getIsAlign() || swerve.getIsAlign()) {
+ swerve.driveHeading(
+ speeds.vxMetersPerSecond,
+ speeds.vyMetersPerSecond,
+ swerve.getAlignAngle(),
+ true);
+ } else {
+ swerve.drive(
+ speeds.vxMetersPerSecond,
+ speeds.vyMetersPerSecond,
+ speeds.omegaRadiansPerSecond,
+ true,
+ false);
+ }
+ }
+}
--- /dev/null
+// Copyright (c) 2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.commands.drive_comm;
+
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.Constants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.GeomUtil;
+
+public class DriveToPose extends Command {
+ protected static boolean updateTarget = false;
+ private static final double drivekP = 5.0;
+ private static final double drivekD = 0.0;
+ private static final double thetakP = 7.0;
+ private static final double thetakD = 0.0;
+ private static final double driveMaxVelocity = DriveConstants.MAX_SPEED;
+ private static final double driveMaxAcceleration = 2.6;
+ private static final double thetaMaxVelocity = 5.0;
+ private static final double thetaMaxAcceleration = 5.0;
+ private static final double driveTolerance = 0.015;
+ private static final double thetaTolerance = Units.degreesToRadians(1.0);
+ private static final double ffMinRadius = 0.05;
+ private static final double ffMaxRadius = 0.1;
+
+ private final Drivetrain drive;
+ private final Supplier<Pose2d> target;
+ private Pose2d targetPose;
+
+ private final ProfiledPIDController driveController =
+ new ProfiledPIDController(
+ drivekP, 0.0, drivekD, new TrapezoidProfile.Constraints(driveMaxVelocity, driveMaxAcceleration), Constants.LOOP_TIME);
+ private final ProfiledPIDController thetaController =
+ new ProfiledPIDController(
+ thetakP, 0.0, thetakD, new TrapezoidProfile.Constraints(thetaMaxVelocity, thetaMaxAcceleration), Constants.LOOP_TIME);
+
+ private Translation2d lastSetpointTranslation = new Translation2d();
+ private double driveErrorAbs = 0.0;
+ private double thetaErrorAbs = 0.0;
+ private boolean running = false;
+ private Supplier<Pose2d> robot;
+
+ private Supplier<Translation2d> linearFF = () -> Translation2d.kZero;
+ private DoubleSupplier omegaFF = () -> 0.0;
+
+ private Debouncer debouncer = new Debouncer(0.2);
+
+ public DriveToPose(Drivetrain drive, Supplier<Pose2d> target) {
+ this.drive = drive;
+ this.target = target;
+ robot = drive::getPose;
+
+ // Set tolerance
+ driveController.setTolerance(driveTolerance);
+ thetaController.setTolerance(thetaTolerance);
+
+ // Enable continuous input for theta controller
+ thetaController.enableContinuousInput(-Math.PI, Math.PI);
+
+ addRequirements(drive);
+ }
+
+ public DriveToPose(
+ Drivetrain drive,
+ Supplier<Pose2d> target,
+ Supplier<Translation2d> linearFF,
+ DoubleSupplier omegaFF) {
+ this(drive, target);
+ this.linearFF = linearFF;
+ this.omegaFF = omegaFF;
+ }
+
+ @Override
+ public void initialize() {
+ drive.setVisionEnabled(VisionConstants.ENABLED_GO_TO_POSE);
+
+ targetPose = target.get();
+ Pose2d currentPose = robot.get();
+ ChassisSpeeds fieldVelocity = ChassisSpeeds.fromRobotRelativeSpeeds(drive.getChassisSpeeds(), currentPose.getRotation());
+ Translation2d linearFieldVelocity =
+ new Translation2d(fieldVelocity.vxMetersPerSecond, fieldVelocity.vyMetersPerSecond);
+
+ thetaController.reset(
+ currentPose.getRotation().getRadians(), fieldVelocity.omegaRadiansPerSecond);
+ lastSetpointTranslation = currentPose.getTranslation();
+
+ if(targetPose != null){
+ driveController.reset(
+ currentPose.getTranslation().getDistance(target.get().getTranslation()),
+ -linearFieldVelocity
+ .rotateBy(
+ targetPose
+ .getTranslation()
+ .minus(currentPose.getTranslation())
+ .getAngle()
+ .unaryMinus())
+ .getX());
+ }
+ }
+
+ @Override
+ public void execute() {
+ running = true;
+
+ // Get current pose and target pose
+ Pose2d currentPose = robot.get();
+ if(updateTarget){
+ targetPose = target.get();
+ }
+ if(targetPose == null){
+ return;
+ }
+
+ // Calculate drive speed
+ double currentDistance = currentPose.getTranslation().getDistance(targetPose.getTranslation());
+ double ffScaler =
+ MathUtil.clamp(
+ (currentDistance - ffMinRadius) / (ffMaxRadius - ffMinRadius),
+ 0.0,
+ 1.0);
+ driveErrorAbs = currentDistance;
+ driveController.reset(
+ lastSetpointTranslation.getDistance(targetPose.getTranslation()),
+ driveController.getSetpoint().velocity);
+ double driveVelocityScalar =
+ driveController.getSetpoint().velocity * ffScaler
+ + driveController.calculate(driveErrorAbs, 0.0);
+ if (currentDistance < driveController.getPositionTolerance()) driveVelocityScalar = 0.0;
+ lastSetpointTranslation =
+ new Pose2d(
+ targetPose.getTranslation(),
+ currentPose.getTranslation().minus(targetPose.getTranslation()).getAngle())
+ .transformBy(GeomUtil.toTransform2d(driveController.getSetpoint().position, 0.0))
+ .getTranslation();
+
+ // Calculate theta speed
+ double thetaVelocity =
+ thetaController.getSetpoint().velocity * ffScaler
+ + thetaController.calculate(
+ currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians());
+ thetaErrorAbs =
+ Math.abs(currentPose.getRotation().minus(targetPose.getRotation()).getRadians());
+ if (thetaErrorAbs < thetaController.getPositionTolerance()) thetaVelocity = 0.0;
+
+ Translation2d driveVelocity =
+ new Pose2d(
+ new Translation2d(),
+ currentPose.getTranslation().minus(targetPose.getTranslation()).getAngle())
+ .transformBy(GeomUtil.toTransform2d(driveVelocityScalar, 0.0))
+ .getTranslation();
+
+ // Scale feedback velocities by input ff
+ final double linearS = linearFF.get().getNorm() * 3.0;
+ final double thetaS = Math.abs(omegaFF.getAsDouble()) * 3.0;
+ driveVelocity =
+ driveVelocity.interpolate(linearFF.get().times(DriveConstants.MAX_SPEED), linearS);
+ thetaVelocity =
+ MathUtil.interpolate(
+ thetaVelocity, omegaFF.getAsDouble() * DriveConstants.MAX_ANGULAR_SPEED, thetaS);
+
+ // Command speeds
+ drive.drive(driveVelocity.getX(), driveVelocity.getY(), thetaVelocity, true, false);
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ drive.stop();
+ drive.setVisionEnabled(true);
+ running = false;
+ }
+
+ /** Checks if the robot is stopped at the final pose. */
+ public boolean atGoal() {
+ return running && (driveController.atGoal() && thetaController.atGoal() || targetPose == null);
+ }
+
+ /** Checks if the robot pose is within the allowed drive and theta tolerances. */
+ public boolean withinTolerance(double driveTolerance, Rotation2d thetaTolerance) {
+ return running
+ && (Math.abs(driveErrorAbs) < driveTolerance
+ && Math.abs(thetaErrorAbs) < thetaTolerance.getRadians()
+ || targetPose == null);
+ }
+
+ @Override
+ public boolean isFinished(){
+ return debouncer.calculate(withinTolerance(driveTolerance, new Rotation2d(thetaTolerance)));
+ }
+}
--- /dev/null
+package frc.robot.commands.drive_comm;
+
+import java.util.function.Supplier;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathConstraints;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.DoNothing;
+import frc.robot.commands.SupplierCommand;
+import frc.robot.constants.AutoConstants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/**
+* Moves the robot to a pose using PathPlanner
+*/
+public class GoToPose extends SequentialCommandGroup {
+
+ private Drivetrain drive;
+ private Supplier<Pose2d> poseSupplier;
+ private double maxSpeed;
+ private double maxAccel;
+
+ /**
+ * Uses PathPlanner to go to a pose
+ * @param poseSupplier The supplier for the pose to use
+ * @param drive The drivetrain
+ */
+ public GoToPose(Supplier<Pose2d> poseSupplier, Drivetrain drive) {
+ this(poseSupplier, AutoConstants.MAX_AUTO_SPEED, AutoConstants.MAX_AUTO_ACCEL, drive);
+ }
+ public GoToPose(Pose2d pose, Drivetrain drive){
+ this(()->pose, drive);
+ }
+
+ /**
+ * Uses PathPlanner to go to a pose
+ * @param poseSupplier The supplier for the pose to use
+ * @param maxSpeed The maximum speed to use
+ * @param maxAccel The maximum acceleration to use
+ * @param drive The drivetrain
+ */
+ public GoToPose(Supplier<Pose2d> poseSupplier, double maxSpeed, double maxAccel, Drivetrain drive) {
+ this.poseSupplier = poseSupplier;
+ this.maxSpeed = maxSpeed;
+ this.maxAccel = maxAccel;
+ this.drive = drive;
+ addCommands(
+ new InstantCommand(()->drive.setVisionEnabled(VisionConstants.ENABLED_GO_TO_POSE)),
+ new SupplierCommand(() -> createCommand(), drive).handleInterrupt(()->drive.setVisionEnabled(true)),
+ new InstantCommand(()->drive.setVisionEnabled(true))
+ );
+ }
+
+ /**
+ * Creates the PathPlanner command and schedules it
+ */
+ public Command createCommand() {
+ Pose2d pose = poseSupplier.get();
+ if(pose==null){
+ return new DoNothing();
+ }
+ Command command = AutoBuilder.pathfindToPose(
+ pose,
+ new PathConstraints(maxSpeed, maxAccel, DriveConstants.MAX_ANGULAR_SPEED, DriveConstants.MAX_ANGULAR_ACCEL),
+ 0
+ );
+
+ // get the distance to the pose.
+ double dist = drive.getPose().minus(pose).getTranslation().getNorm();
+
+ // if greater than 3m or less than 2 cm, don't run it. If the path is too small pathplanner makes weird paths.
+ if (dist > 3) {
+ command = new DoNothing();
+ DriverStation.reportWarning("Alignment Path too long, doing nothing, GoToPose.java", false);
+ } else if (dist < 0.02) {
+ command = new DoNothing();
+ DriverStation.reportWarning("Alignment Path too short, doing nothing, GoToPose.java", false);
+ }
+
+ return command;
+ }
+}
--- /dev/null
+package frc.robot.commands.drive_comm;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.VisionConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/**
+ * Runs the chassis PIDs to move the robot to a specific pose.
+ */
+public class GoToPosePID extends Command {
+
+ private Drivetrain drive;
+
+ private Supplier<Pose2d> poseSupplier;
+ private Pose2d pose;
+
+ /**
+ * Runs the chassis PIDs to move the robot to a specific pose.
+ * @param pose The pose supplier to go to
+ * @param drive The drivetrain
+ */
+ public GoToPosePID(Supplier<Pose2d> pose, Drivetrain drive) {
+ this.drive = drive;
+ this.poseSupplier = pose;
+
+ addRequirements(drive);
+ }
+
+ public GoToPosePID(Pose2d pose, Drivetrain drive){
+ this(()->pose, drive);
+ }
+
+ @Override
+ public void initialize() {
+ pose = poseSupplier.get();
+ drive.setVisionEnabled(VisionConstants.ENABLED_GO_TO_POSE);
+ }
+
+ @Override
+ public void execute() {
+ if(pose == null) {
+ return;
+ }
+
+ drive.driveWithPID(pose.getX(), pose.getY(), pose.getRotation().getRadians());
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ drive.stop();
+ drive.setVisionEnabled(true);
+ }
+
+ @Override
+ public boolean isFinished() {
+ return pose == null || drive.getXController().atSetpoint() && drive.getYController().atSetpoint() && drive.getRotationController().atSetpoint();
+ }
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.commands.drive_comm;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/**
+ * Sets the robot's wheels to an X formation to prevent being pushed around by other bots.
+ */
+public class SetFormationX extends SequentialCommandGroup {
+ public SetFormationX(Drivetrain drive) {
+ addRequirements(drive);
+ addCommands(
+ new InstantCommand(() -> drive.setStateDeadband(false), drive),
+ new RunCommand(() -> drive.setModuleStates(new SwerveModuleState[]{
+ new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(45))),
+ new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(-45))),
+ new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(-45))),
+ new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(45)))
+ }, false), drive)
+ );
+ }
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.commands.drive_comm;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/**
+ * Attempts to set all four modules to a constant angle. Determines if the modules are able to reach the angle requested in a certain time.
+ */
+public class SimplePresetSteerAngles extends InstantCommand {
+
+ /**
+ * sets the angle of module steer to 0 to remove initial turn time and drift
+ *
+ * @param drive drivetrain to be used
+ */
+ public SimplePresetSteerAngles(Drivetrain drive) {
+ this(drive, new Rotation2d());
+ }
+
+ /**
+ * sets the angle of module steer to a angle to remove initial turn time and drift
+ *
+ * @param angle angle to set module steer to in radians
+ * @param drive drivetrain to be used
+ */
+ public SimplePresetSteerAngles(Drivetrain drive, double angle) {
+ this(drive, new Rotation2d(angle));
+ }
+
+ /**
+ * sets the angle of module steer to a angle to remove initial turn time and drift
+ *
+ * @param rotation rotation to set module steer to
+ * @param drive drivetrain to be used
+ */
+ public SimplePresetSteerAngles(Drivetrain drive, Rotation2d rotation) {
+ super(() -> {
+ drive.setStateDeadband(false);
+ drive.setModuleStates(new SwerveModuleState[]{
+ new SwerveModuleState(0, rotation),
+ new SwerveModuleState(0, rotation),
+ new SwerveModuleState(0, rotation),
+ new SwerveModuleState(0, rotation)
+ }, true);
+ drive.setStateDeadband(true);
+ }, drive);
+ drive.setStateDeadband(true);
+ }
+}
--- /dev/null
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.drive_comm;
+
+
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.SignalLogger;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.SysId;
+
+/**
+ * A command to run all 4 SysId routines on the drivetrain
+*/
+public class SysIDDriveCommand extends SequentialCommandGroup {
+
+ private Config config = new Config();
+ private SysId sysId;
+ public SysIDDriveCommand(Drivetrain drive) {
+ config = new Config(
+ Units.Volts.of(0.5).per(Units.Seconds),
+ Units.Volts.of(3),
+ Units.Seconds.of(5),
+ (state) -> Logger.recordOutput("SysIdTestState", state.toString())
+ );
+ Rotation2d[] angles = {
+ Rotation2d.fromDegrees(0),//-45-180
+ Rotation2d.fromDegrees(0),//45
+ Rotation2d.fromDegrees(0),//45+180
+ Rotation2d.fromDegrees(0),//-45
+ };
+ sysId = new SysId(
+ "Drivetrain",
+ x ->{
+ drive.setAngleMotors(angles);
+ drive.setDriveVoltages(x);
+ },
+ drive,
+ config
+ );
+ addCommands(
+ sysId.runQuasisStatic(Direction.kForward),
+ new WaitCommand(0.5),
+ sysId.runQuasisStatic(Direction.kReverse),
+ new WaitCommand(0.5),
+ sysId.runDynamic(Direction.kForward),
+ new WaitCommand(0.5),
+ sysId.runDynamic(Direction.kReverse)
+ );
+ }
+
+
+
+}
--- /dev/null
+package frc.robot.commands.drive_comm;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.Trajectory.State;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/**
+ * Sets all module angles to a given trajectory's initial angle.
+ */
+public class TrajectoryPresetSteerAngles extends InstantCommand {
+ /*
+ * make sure to add wait command after called to give time to correct
+ */
+ public TrajectoryPresetSteerAngles(Drivetrain drive, Trajectory trajectory) {
+ super(
+ () -> {
+
+ // 0.01 is the time between trajectory samples, in seconds
+ // Can be replaced for any small number, but it should be the same as the time between all uses
+ double time = 0.01;
+
+ drive.setStateDeadband(false);
+
+ Pose2d initialPose = trajectory.getInitialPose();
+ State sample = trajectory.sample(time);
+ Pose2d nextPose = sample.poseMeters;
+
+ double xVelocity = sample.velocityMetersPerSecond * nextPose.getRotation().getCos();
+ double yVelocity = sample.velocityMetersPerSecond * nextPose.getRotation().getSin();
+ double angularVelo = (nextPose.getRotation().getRadians() - initialPose.getRotation().getRadians()) / time;
+
+ ChassisSpeeds chassisSpeeds = new ChassisSpeeds(xVelocity, yVelocity, angularVelo);
+ chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds, initialPose.getRotation());
+
+ SwerveModuleState[] swerveModuleStates = DriveConstants.KINEMATICS.toSwerveModuleStates(chassisSpeeds);
+ for (SwerveModuleState swerveModuleState : swerveModuleStates) {
+ swerveModuleState.speedMetersPerSecond = 0;
+ }
+ drive.setModuleStates(swerveModuleStates, true);
+ drive.setStateDeadband(true);
+ },
+ drive
+ );
+
+ }
+}
--- /dev/null
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.Intake.Intake;
+import frc.robot.subsystems.Intake.IntakeConstants;
+import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.hood.HoodConstants;
+
+public class HardstopWarning extends Command {
+ private Hood hood;
+ private Intake intake;
+
+ public HardstopWarning(Hood hood, Intake intake) {
+ this.hood = hood;
+ this.intake = intake;
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+
+ @Override
+ public void execute() {
+ double epsilon = 0.05;
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putBoolean("Hood OK", hood.getPositionDeg() >= HoodConstants.MIN_ANGLE - epsilon);
+ SmartDashboard.putBoolean("Intake OK", intake.getPosition() >= IntakeConstants.STARTING_POINT - epsilon);
+ }
+ }
+
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+}
--- /dev/null
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Intake.Intake;
+
+public class IntakeCommand extends Command{
+ private Intake intake;
+
+ public IntakeCommand(Intake intake){
+ this.intake = intake;
+ addRequirements(intake);
+ }
+
+ @Override
+ public void initialize() {
+ intake.extend();
+ intake.spinStart();
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ intake.intermediateExtend();
+ intake.spinStop();
+ }
+
+}
--- /dev/null
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Intake.Intake;
+
+public class IntakeMovementCommand extends Command {
+ private final Intake intake;
+ private final double interval = 0.6; // Change this to make it faster/slower (seconds)
+
+ public IntakeMovementCommand(Intake intake) {
+ this.intake = intake;
+ addRequirements(intake);
+ }
+
+ @Override
+ public void execute() {
+ intake.spinStart();
+ if ((int) (Timer.getFPGATimestamp() / interval) % 2 == 0) {
+ intake.extend();
+ } else {
+ intake.intermediateExtend();
+ }
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ intake.extend();
+ }
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.Constants;
+import frc.robot.constants.FieldConstants;
+import frc.robot.constants.ShotInterpolation;
+import frc.robot.constants.ShuttleInterpolation;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.hood.HoodConstants;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.turret.Turret;
+import frc.robot.subsystems.turret.TurretConstants;
+import frc.robot.util.PhaseManager;
+import frc.robot.util.ShooterPhysics;
+import frc.robot.util.ShooterPhysics.TurretState;
+
+public class LockedShoot extends Command {
+ private Turret turret;
+ private Drivetrain drive;
+ private Hood hood;
+ private Shooter shooter;
+
+ private Pose2d drivepose;
+
+ private final LinearFilter hoodAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
+
+ private Rotation2d targetAngle;
+
+ private double lastHoodAngle;
+ private double hoodAngle;
+ private double hoodVelocity;
+
+ private TurretState goalState;
+
+ private double phaseDelay = 0.03; // Extrapolation delay due to latency
+
+ private Translation2d target = FieldConstants.HUB_BLUE.toTranslation2d();
+
+ private PhaseManager phaseManager = new PhaseManager();
+
+ private double distanceFromTarget = 0.0;
+
+ private double TOFAdjustment = 0.85;
+
+ public LockedShoot(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter) {
+ this.turret = turret;
+ this.drive = drivetrain;
+ this.hood = hood;
+ this.shooter = shooter;
+ drivepose = drivetrain.getPose();
+
+ goalState = ShooterPhysics.getShotParams(
+ Translation2d.kZero,
+ FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())),
+ 8.0); // Random initial goalState to prevent it being null
+
+ addRequirements(turret, shooter);
+ }
+
+ public void updateSetpoints(Pose2d drivepose) {
+ Pose2d turretPosition = drivepose.transformBy(
+ new Transform2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.toTranslation2d(), Rotation2d.kZero));
+
+ // If the robot is moving, adjust the target position based on velocity
+ ChassisSpeeds robotRelVel = drive.getChassisSpeeds();
+ ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drive.getYaw());
+
+ // Rotational adjustment is not being used, since turret is in center of robot
+ double turretVelocityX =
+ fieldRelVel.vxMetersPerSecond;
+ double turretVelocityY =
+ fieldRelVel.vyMetersPerSecond;
+
+ double timeOfFlight = 0;
+ Pose2d lookaheadPose = turretPosition;
+
+ /*
+ * Loop (max 20) until lookaheadPose converges BECAUSE -->
+ * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate for 6m (t = 0.8s)
+ * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was derived using t = 1.0s
+ * So we make a bunch of guesses until it converges
+ * Early exit when change < 1mm to avoid unnecessary iterations
+ */
+ for (int i = 0; i < 20; i++) {
+ Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ());
+
+ Translation3d target3d = new Translation3d(target.getX(), target.getY(),
+ target.equals(FieldConstants.getHubTranslation().toTranslation2d()) ?
+ FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not the hub
+
+ goalState = ShooterPhysics.getShotParams(
+ Translation2d.kZero,
+ target3d.minus(lookahead3d),
+ 2.0);
+
+ timeOfFlight = goalState.timeOfFlight() * TOFAdjustment;
+ double offsetX = turretVelocityX * timeOfFlight;
+ double offsetY = turretVelocityY * timeOfFlight;
+ Pose2d newLookaheadPose =
+ new Pose2d(
+ turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)),
+ turretPosition.getRotation());
+
+ // early exit if converged (change < 1mm)
+ if (i > 0 && lookaheadPose.getTranslation().getDistance(newLookaheadPose.getTranslation()) < 0.001) {
+ lookaheadPose = newLookaheadPose;
+ break;
+ }
+ lookaheadPose = newLookaheadPose;
+ }
+
+ // Get the field angle relative to the target pose
+ targetAngle = target.minus(lookaheadPose.getTranslation()).getAngle();
+
+ targetAngle = target.minus(lookaheadPose.getTranslation()).getAngle().minus(new Rotation2d(turret.getPositionRad()));
+
+
+ // Pitch is in radians
+ hoodAngle = goalState.pitch();
+ hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME);
+ lastHoodAngle = hoodAngle;
+
+ distanceFromTarget = target.getDistance(lookaheadPose.getTranslation());
+ }
+
+ public void updateDrivePose(){
+ Pose2d currentPose = drive.getPose();
+
+ drivepose = new Pose2d(
+ currentPose.getTranslation(),
+ // Uncomment this if robot is backwards
+ currentPose.getRotation()//.plus(new Rotation2d(Math.PI))
+ );
+ ChassisSpeeds robotRelVel = drive.getChassisSpeeds();
+
+ // Add a phase delay extrapolation component for latency delay
+ drivepose = drivepose.exp(
+ new Twist2d(
+ robotRelVel.vxMetersPerSecond * phaseDelay,
+ robotRelVel.vyMetersPerSecond * phaseDelay,
+ robotRelVel.omegaRadiansPerSecond * phaseDelay));
+ }
+
+ /**
+ * Stops and stows all subsystems involved in the command
+ */
+ public void stowEverything(){
+ hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0);
+ shooter.setShooter(0.0);
+ //spindexer.stopSpindexer();
+ }
+
+ @Override
+ public void execute() {
+ updateDrivePose();
+
+ // Phase manager stuff
+ phaseManager.update(drivepose, shooter, turret);
+ target = phaseManager.getTarget(drivepose);
+
+ updateSetpoints(drivepose);
+
+ if (phaseManager.isIdle()) {
+ stowEverything();
+ } else {
+ drive.setAlignAngle(targetAngle.getRadians());
+
+ boolean shuttling = !target.equals(FieldConstants.getHubTranslation().toTranslation2d()); // if we're aiming at the hub, we're not shuttling
+
+ // shuttling will move the hood so its angles very close to max (less arch)
+ if (shuttling) {
+ hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShuttleInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity);
+ } else {
+ hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity);
+ }
+
+ // if (FieldConstants.underTrench(x, y)) {
+ // System.out.println("Hood forced down");
+ // } else {
+ // hood.forceHoodDown(false);
+ // }
+
+ // different maps for shuttling vs shooting. Less powerful when shuttling.
+ if (shuttling) {
+ shooter.setShooter(-ShuttleInterpolation.shooterVelocityMap.get(distanceFromTarget));
+ } else {
+ shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget));
+ }
+
+ }
+
+
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ stowEverything();
+
+ drive.setIsAlign(false);
+
+ turret.locked = false;
+
+ }
+
+ @Override
+ public void initialize() {
+ drive.setIsAlign(true);
+ turret.locked = true;
+ }
+
+}
--- /dev/null
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.Constants;
+import frc.robot.constants.FieldConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.hood.HoodConstants;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.ShooterConstants;
+import frc.robot.subsystems.turret.Turret;
+import frc.robot.util.ShooterPhysics;
+
+public class PhysicsAutoShoot extends Command {
+ private Turret turret;
+ private Hood hood;
+ private Shooter shooter;
+ private Drivetrain drivetrain;
+ private ShooterPhysics.Constraints constraints;
+
+ public PhysicsAutoShoot(Turret turret, Hood hood, Shooter shooter, Drivetrain drivetrain) {
+ this.turret = turret;
+ this.hood = hood;
+ this.shooter = shooter;
+ this.drivetrain = drivetrain;
+
+ this.constraints = new ShooterPhysics.Constraints(2.2, ShooterConstants.SHOOTER_VELOCITY,
+ Units.degreesToRadians(HoodConstants.MIN_ANGLE), Units.degreesToRadians(HoodConstants.MAX_ANGLE));
+
+ addRequirements(turret, hood, shooter);
+ }
+
+ @Override
+ public void execute() {
+ ChassisSpeeds chassisSpeeds = drivetrain.getChassisSpeeds();
+ Translation2d robotVelocity = new Translation2d(chassisSpeeds.vxMetersPerSecond,
+ chassisSpeeds.vyMetersPerSecond);
+ Translation3d robotToTarget = FieldConstants.getHubTranslation()
+ .minus(new Translation3d(drivetrain.getPose().getTranslation()));
+
+ var stateOpt = ShooterPhysics.getConstrainedParams(
+ robotVelocity,
+ robotToTarget,
+ this.constraints);
+
+ // in one periodic
+ var futureStateOpt = ShooterPhysics.getConstrainedParams(
+ robotVelocity,
+ robotToTarget.plus(new Translation3d(robotVelocity.times(Constants.LOOP_TIME))),
+ this.constraints);
+
+ if (stateOpt.isPresent() && futureStateOpt.isPresent()) {
+ ShooterPhysics.TurretState state = stateOpt.get();
+ ShooterPhysics.TurretState futureState = futureStateOpt.get();
+
+ double yawSlope = (futureState.yaw().getRadians() - state.yaw().getRadians()) / Constants.LOOP_TIME;
+ double hoodSlope = (futureState.pitch() - state.pitch()) / Constants.LOOP_TIME;
+
+ turret.setFieldRelativeTarget(state.yaw(), yawSlope);
+ hood.setFieldRelativeTarget(new Rotation2d(state.pitch()), hoodSlope);
+ shooter.setShooter(state.exitVel());
+
+ } else if (stateOpt.isPresent() && futureStateOpt.isEmpty()) {
+ ShooterPhysics.TurretState state = stateOpt.get();
+
+ turret.setFieldRelativeTarget(state.yaw(), 0.0);
+ hood.setFieldRelativeTarget(new Rotation2d(state.pitch()), 0.0);
+ shooter.setShooter(state.exitVel());
+ }
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ // stop the turret where it is
+ turret.setFieldRelativeTarget(new Rotation2d(turret.getPositionRad()), 0.0);
+ hood.setFieldRelativeTarget(new Rotation2d(0), 0.0);
+ shooter.setShooter(0);
+ }
+}
--- /dev/null
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.PowerControl.Battery;
+import frc.robot.subsystems.PowerControl.EMABreaker;
+
+
+public class PowerControl extends Command {
+ // my beautiful power control subsystems
+ private EMABreaker breaker;
+ private Battery battery;
+ // TODO: add subsystems back when implementing logic:
+ // the real subsystems
+ // private Drivetrain drivetrain;
+ // private Shooter shooter;
+ // private Turret turret;
+ // private Hood hood;
+ // private Intake intake;
+ // private Spindexer spindexer;
+
+ public SeverityLevel severityLevel;
+
+ public enum SeverityLevel {
+ SEVERITY_LVL_ZERO,
+ SEVERITY_LVL_ONE,
+ SEVERITY_LVL_TWO,
+ SEVERITY_LVL_THREE,
+ SEVERITY_LVL_FOUR,
+ SEVERITY_LVL_FIVE,
+ }
+
+ public PowerControl(
+ EMABreaker breaker, // pc
+ Battery battery // pc
+ // Drivetrain drivetrain, // main draw
+ // Shooter shooter, // aiming (vital)
+ // Turret turret, // aiming
+ // Hood hood, // aiming
+ // Intake intake, // bps
+ // Spindexer spindexer // bps
+ ) {
+ this.breaker = breaker;
+ this.battery = battery;
+ // this.drivetrain = drivetrain;
+ // this.shooter = shooter;
+ // this.turret = turret;
+ // this.hood = hood;
+ // this.intake = intake;
+ // this.spindexer = spindexer;
+
+ addRequirements(breaker, battery); // not sure if I'll need requirement access for setting new current limits
+ }
+
+ @Override
+ public void initialize() {
+ severityLevel = SeverityLevel.SEVERITY_LVL_ZERO;
+ }
+
+ @Override
+ public void execute() {
+ double[] worstFilter = breaker.percentageMaxUsage();
+ double percentage = worstFilter[0]; // percentage of current average until we trip breaker
+ double tau = worstFilter[1]; // how quickly this issue is happenning and if we need to respond quickly
+
+ // Some logic here
+ }
+
+ @Override
+ public void end(boolean interupted) {
+ severityLevel = SeverityLevel.SEVERITY_LVL_ZERO; // in the case of disabling this command we shoud reset its effects
+ }
+}
+
--- /dev/null
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Intake.Intake;
+
+public class ReverseMotors extends Command {
+ private Intake intake;
+
+
+ public ReverseMotors(Intake intake){
+ this.intake = intake;
+
+ addRequirements(intake);
+ }
+
+ @Override
+ public void initialize(){
+
+ }
+
+ @Override
+ public void execute(){
+ intake.extend();
+ intake.spinReverse();
+ //spindexer.reverseSpindexer();
+ }
+
+ @Override
+ public void end(boolean interrupted){
+ intake.extend();
+ intake.spinStart();
+ //spindexer.maxSpindexer();
+ }
+
+}
--- /dev/null
+package frc.robot.commands.gpm;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.Intake.Intake;
+import frc.robot.subsystems.Intake.IntakeConstants;
+import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.spindexer.Spindexer;
+import frc.robot.subsystems.spindexer.SpindexerConstants;
+import frc.robot.subsystems.turret.Turret;
+
+public class RunSpindexer extends Command {
+ private Spindexer spindexer;
+ private Turret turret;
+ private Hood hood;
+ private Intake intake;
+
+ private Debouncer jam_debouncer = new Debouncer(SpindexerConstants.JAM_DEBOUNCE_TIME, DebounceType.kRising); // if there is jam I would think this is 0 -> 1
+
+ private boolean reversing = false;
+ private boolean wasHoodForcedDown = false;
+
+ private Timer reverseTimer = new Timer();
+
+ private double storedIntakeSpeed = 0.0;
+
+ public RunSpindexer(Spindexer spindexer, Turret turret, Hood hood, Intake intake) {
+ this.spindexer = spindexer;
+ this.turret = turret;
+ this.hood = hood;
+ this.intake = intake;
+
+ addRequirements(spindexer);
+ }
+
+ // public RunSpindexer(Spindexer spindexer) {
+ // this.spindexer = spindexer;
+ // addRequirements(spindexer);
+ // }
+
+ @Override
+ public void initialize() {
+ wasHoodForcedDown = hood.getHoodForcedDown();
+ }
+
+ @Override
+ public void execute() {
+ boolean hoodForcedDown = hood.getHoodForcedDown();
+
+ if (wasHoodForcedDown && !hoodForcedDown) {
+ spindexer.maxSpindexer();
+ }
+ wasHoodForcedDown = hoodForcedDown;
+
+ if (!turret.atSetpoint() || hoodForcedDown || spindexer.noIndexing) {
+ spindexer.stopSpindexer();
+ reversing = false;
+ return; // this is so the balls don't fly out when unaligned
+ }
+ boolean jammed = spindexer.getMotorOneVelocity() < SpindexerConstants.JAM_CURRENT_THRESHOLD && spindexer.getMotorOneStatorCurrent() > SpindexerConstants.JAM_CURRENT_THRESHOLD;
+ Logger.recordOutput("SpindexerJammed", jammed);
+ if (jam_debouncer.calculate(jammed)) {
+ Logger.recordOutput("SpindexerJammedDebounced", jammed);
+
+ reversing = true;
+ reverseTimer.reset();
+ reverseTimer.start();
+ storedIntakeSpeed = intake.getSpeed();
+ }
+ if (!reversing) {
+ spindexer.maxSpindexer();
+ } else {
+ spindexer.reverseSpindexer();
+
+ if (intake.getPosition() > IntakeConstants.INTERMEDIATE_EXTENSION + 1.0) {
+ intake.spinReverse();
+ } else {
+ intake.extend();
+ }
+
+ if (reverseTimer.hasElapsed(SpindexerConstants.REVERSE_DEBOUNCE_TIME)) {
+ reversing = false;
+ intake.spin(storedIntakeSpeed);
+ }
+ }
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putBoolean("Spindexer Jamming", reversing);
+ }
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ spindexer.stopSpindexer();
+ reversing = false;
+ }
+
+ // @Override
+ // public boolean isFinished() {
+ // return false; // never ends on its own
+ // }
+}
--- /dev/null
+package frc.robot.commands.gpm;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.Intake.Intake;
+import frc.robot.subsystems.Intake.IntakeConstants;
+import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.spindexer.Spindexer;
+import frc.robot.subsystems.spindexer.SpindexerConstants;
+import frc.robot.subsystems.turret.Turret;
+
+public class RunSpindexerWithStop extends Command {
+ private Spindexer spindexer;
+ private Turret turret;
+ private Hood hood;
+ private Intake intake;
+
+ // if there is jam I would think this is 0 -> 1
+ private Debouncer jam_debouncer = new Debouncer(SpindexerConstants.JAM_DEBOUNCE_TIME, DebounceType.kRising);
+
+ private boolean reversing = false;
+ private boolean wasHoodForcedDown = false;
+
+ private Timer reverseTimer = new Timer();
+
+ private double storedIntakeSpeed = 0.0;
+
+ private Timer runTimer = new Timer();
+
+ private Debouncer debouncer = new Debouncer(0.3, DebounceType.kRising);
+
+ private final double interval = 0.6; // Change this to make it faster/slower (seconds)
+
+ public RunSpindexerWithStop(Spindexer spindexer, Turret turret, Hood hood, Intake intake) {
+ this.spindexer = spindexer;
+ this.turret = turret;
+ this.hood = hood;
+ this.intake = intake;
+
+ addRequirements(spindexer);
+ }
+
+ @Override
+ public void initialize() {
+ wasHoodForcedDown = hood.getHoodForcedDown();
+ runTimer.reset();
+ runTimer.start();
+ }
+
+ @Override
+ public void execute() {
+ boolean hoodForcedDown = hood.getHoodForcedDown();
+
+ if (wasHoodForcedDown && !hoodForcedDown) {
+ spindexer.maxSpindexer();
+ }
+ wasHoodForcedDown = hoodForcedDown;
+
+ if (!turret.atSetpoint() || hoodForcedDown || spindexer.noIndexing) {
+ spindexer.stopSpindexer();
+ reversing = false;
+ return; // this is so the balls don't fly out when unaligned
+ }
+ boolean jammed = spindexer.getSubsystemStatorCurrent() / 2 > SpindexerConstants.JAM_CURRENT_THRESHOLD;
+ Logger.recordOutput("SpindexerJammed", jammed);
+ if (jam_debouncer.calculate(jammed)) {
+ Logger.recordOutput("SpindexerJammedDebounced", jammed);
+
+ reversing = true;
+ reverseTimer.reset();
+ reverseTimer.start();
+ storedIntakeSpeed = intake.getSpeed();
+ }
+ if (!reversing) {
+ spindexer.maxSpindexer();
+ } else {
+ spindexer.reverseSpindexer();
+
+ if (intake.getPosition() > IntakeConstants.INTERMEDIATE_EXTENSION + 1.0) {
+ intake.spinReverse();
+ } else {
+ intake.extend();
+ }
+
+ if (reverseTimer.hasElapsed(SpindexerConstants.REVERSE_DEBOUNCE_TIME)) {
+ reversing = false;
+ intake.spin(storedIntakeSpeed);
+ }
+ }
+
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putBoolean("Spindexer Jamming", reversing);
+ }
+
+ if ((int) (Timer.getFPGATimestamp() / interval) % 2 == 0) {
+ intake.extend();
+ } else {
+ intake.intermediateExtend();
+ }
+
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ spindexer.stopSpindexer();
+ reversing = false;
+
+ intake.extend();
+ }
+
+ @Override
+ public boolean isFinished() {
+ return (runTimer.hasElapsed(1.0) && debouncer.calculate(spindexer.spinningAir())) || runTimer.hasElapsed(4.0);
+ }
+}
--- /dev/null
+package frc.robot.commands.gpm;
+
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.FieldConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.ShooterConstants;
+import frc.robot.subsystems.turret.Turret;
+
+public class SimpleAutoShoot extends Command {
+ private Turret turret;
+ private Drivetrain drivetrain;
+ private Shooter shooter;
+
+ private double fieldAngleRad;
+ private double turretSetpoint;
+
+ private boolean SOTM = true;
+ private Translation2d drivepose;
+
+ public SimpleAutoShoot(Turret turret, Drivetrain drivetrain, Shooter shooter) {
+ this.turret = turret;
+ this.drivetrain = drivetrain;
+ drivepose = drivetrain.getPose().getTranslation();
+
+ addRequirements(turret);
+ }
+
+ public void updateTurretSetpoint(Translation2d drivepose) {
+
+ //FieldZone currentZone = getZone(drivepose);
+ Translation2d target = FieldConstants.getHubTranslation().toTranslation2d();
+
+ double D_y;
+ double D_x;
+ double timeToGoal = 0.0;
+
+ // If the robot is moving, adjust the target position based on velocity
+ if (SOTM) {
+ ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
+ ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw());
+ double xVel = fieldRelVel.vxMetersPerSecond;
+ double yVel = fieldRelVel.vyMetersPerSecond;
+
+ D_y = target.getY() - drivepose.getY() - timeToGoal * yVel;
+ D_x = target.getX() - drivepose.getX() - timeToGoal * xVel;
+ } else {
+ D_y = target.getY() - drivepose.getY();
+ D_x = target.getX() - drivepose.getX();
+ }
+
+ // Calculate the field-relative angle
+ fieldAngleRad = Math.atan2(D_y, D_x);
+
+ // Calculate robot heading and adjust for reverse drive
+ double robotHeading = MathUtil.angleModulus((drivetrain.getYaw().getRadians() + Math.PI)); // Reverse drive adjustment
+
+ // Calculate turret setpoint (angle relative to robot heading)
+ turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(fieldAngleRad - robotHeading), -180.0, 180.0);
+
+ }
+
+ @Override
+ public void initialize() {
+ // Initialize setpoint calculation and set the initial goal for the turret
+ updateTurretSetpoint(drivepose);
+ turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), 0);
+ }
+
+ @Override
+ public void execute() {
+ // Continuously update setpoints and adjust based on vision if available
+ drivepose = drivetrain.getPose().getTranslation();
+ updateTurretSetpoint(drivepose);
+
+ turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2));
+ shooter.setShooter(ShooterConstants.SHOOTER_VELOCITY);
+
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ // Set the turret to a safe position when the command ends
+ turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0);
+ shooter.setShooter(0.0);
+ }
+
+}
+
--- /dev/null
+package frc.robot.commands.gpm;
+
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.Constants;
+import frc.robot.constants.FieldConstants;
+import frc.robot.constants.ShotInterpolation;
+import frc.robot.constants.ShuttleInterpolation;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.hood.HoodConstants;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.spindexer.Spindexer;
+import frc.robot.subsystems.turret.Turret;
+import frc.robot.subsystems.turret.TurretConstants;
+import frc.robot.util.PhaseManager;
+import frc.robot.util.ShooterPhysics;
+import frc.robot.util.ShooterPhysics.TurretState;
+
+public class Superstructure extends Command {
+ private Turret turret;
+ private Drivetrain drivetrain;
+ private Hood hood;
+ private Shooter shooter;
+ private Spindexer spindexer;
+
+ private double turretSetpoint;
+ private double hoodSetpoint;
+
+ private Pose2d drivepose;
+
+ private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
+ private final LinearFilter hoodAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
+
+ private Rotation2d lastTurretAngle;
+ private Rotation2d turretAngle;
+ private double turretVelocity;
+
+ private double lastHoodAngle;
+ private double hoodAngle;
+ private double hoodVelocity;
+
+ private TurretState goalState;
+
+ private LoggedNetworkNumber phaseDelay = new LoggedNetworkNumber("/Tuning/OPERATOR/Phase Delay", 0.03); //Extrapolation delay due to latency
+
+ private Translation2d target = FieldConstants.HUB_BLUE.toTranslation2d();
+
+ private PhaseManager phaseManager = new PhaseManager();
+
+ private LoggedNetworkNumber hoodOffset = new LoggedNetworkNumber("/Tuning/OPERATOR/Hood Offset", 0.0);
+
+ private LoggedNetworkNumber turretOffset = new LoggedNetworkNumber("/Tuning/OPERATOR/Turret Offet",0.0);
+
+ private double distanceFromTarget = 0.0;
+ private LoggedNetworkNumber TOFAdjustment = new LoggedNetworkNumber("/Tuning/OPERATOR/TOF Adjustment", 1.1);
+
+ public Superstructure(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter, Spindexer spindexer) {
+ this.turret = turret;
+ this.drivetrain = drivetrain;
+ this.hood = hood;
+ this.shooter = shooter;
+ this.spindexer = spindexer;
+ drivepose = drivetrain.getPose();
+
+ goalState = ShooterPhysics.getShotParams(
+ Translation2d.kZero,
+ FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())),
+ 8.0); // Random initial goalState to prevent it being null
+
+ addRequirements(turret, shooter);
+ }
+
+ public void updateSetpoints(Pose2d drivepose) {
+ Pose2d turretPosition = drivepose.transformBy(
+ new Transform2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.toTranslation2d(), Rotation2d.kZero));
+
+ // If the robot is moving, adjust the target position based on velocity
+ ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
+ ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw());
+
+ // Rotational adjustment is not being used, since turret is in center of robot
+ double turretVelocityX =
+ fieldRelVel.vxMetersPerSecond;
+ double turretVelocityY =
+ fieldRelVel.vyMetersPerSecond;
+
+ double timeOfFlight = 0;
+ Pose2d lookaheadPose = turretPosition;
+
+ /*
+ * Loop (max 20) until lookaheadPose converges BECAUSE -->
+ * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate for 6m (t = 0.8s)
+ * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was derived using t = 1.0s
+ * So we make a bunch of guesses until it converges
+ * Early exit when change < 1mm to avoid unnecessary iterations
+ */
+ for (int i = 0; i < 20; i++) {
+ Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ());
+
+ Translation3d target3d = new Translation3d(target.getX(), target.getY(),
+ target.equals(FieldConstants.getHubTranslation().toTranslation2d()) ?
+ FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not the hub
+
+ goalState = ShooterPhysics.getShotParams(
+ Translation2d.kZero,
+ target3d.minus(lookahead3d),
+ 2.0);
+
+ timeOfFlight = goalState.timeOfFlight() * TOFAdjustment.get();
+ double offsetX = turretVelocityX * timeOfFlight;
+ double offsetY = turretVelocityY * timeOfFlight;
+ Pose2d newLookaheadPose =
+ new Pose2d(
+ turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)),
+ turretPosition.getRotation());
+
+ // early exit if converged (change < 1mm)
+ if (i > 0 && lookaheadPose.getTranslation().getDistance(newLookaheadPose.getTranslation()) < 0.001) {
+ lookaheadPose = newLookaheadPose;
+ break;
+ }
+ lookaheadPose = newLookaheadPose;
+ }
+
+ // Get the field angle relative to the target pose
+ turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle();
+ if (lastTurretAngle == null) {
+ lastTurretAngle = turretAngle;
+ }
+
+ // Take the filtered average as the turret's velocity when robot is moving translationally
+ turretVelocity =
+ turretAngleFilter.calculate(turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME);
+
+ lastTurretAngle = turretAngle;
+
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Turret/Target Pose", target);
+ Logger.recordOutput("Lookahead Pose", lookaheadPose);
+ }
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putNumber("Time of flight", timeOfFlight);
+ SmartDashboard.putNumber("Turret X-Velocity", turretVelocityX);
+ SmartDashboard.putNumber("Turret Y-Velocity", turretVelocityY);
+ }
+
+ // Subtract the rotational angle of the robot from the setpoint
+ double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians());
+
+ // Shortest path
+ double error = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180);
+ double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error + turretOffset.get();
+
+ // Stay within physical limits -- if shortest path is past max angle, we go long way around
+ if (potentialSetpoint > TurretConstants.MAX_ANGLE) {
+ potentialSetpoint -= 360;
+ } else if (potentialSetpoint < TurretConstants.MIN_ANGLE) {
+ potentialSetpoint += 360;
+ }
+
+ turretSetpoint = potentialSetpoint;
+
+ // Pitch is in radians
+ hoodAngle = goalState.pitch();
+ hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
+ hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME);
+ lastHoodAngle = hoodAngle;
+
+ distanceFromTarget = target.getDistance(lookaheadPose.getTranslation());
+ Logger.recordOutput("Shooting/distanceToTarget", distanceFromTarget);
+ }
+
+ public void updateDrivePose(){
+ Pose2d currentPose = drivetrain.getPose();
+
+ drivepose = new Pose2d(
+ currentPose.getTranslation(),
+ // Uncomment this if robot is backwards
+ currentPose.getRotation()//.plus(new Rotation2d(Math.PI))
+ );
+ ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
+
+ // Add a phase delay extrapolation component for latency delay
+ // drivepose = drivepose.exp(
+ // new Twist2d(
+ // robotRelVel.vxMetersPerSecond * phaseDelay.get(),
+ // robotRelVel.vyMetersPerSecond * phaseDelay.get(),
+ // robotRelVel.omegaRadiansPerSecond * phaseDelay.get()));
+ }
+
+ /**
+ * Stops and stows all subsystems involved in the command
+ */
+ public void stowEverything(){
+ turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0);
+ hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0);
+ shooter.setShooter(0.0);
+ spindexer.noIndexing = true;
+ }
+
+ public void underLadder(){
+ spindexer.noIndexing = true;
+ }
+
+ // shoot higher
+ public void bumpUpHoodOffset() {
+ hoodOffset.set(hoodOffset.get() + 1.0); //1 deg
+ }
+
+ // shoot lower
+ public void bumpDownHoodOffset() {
+ hoodOffset.set(hoodOffset.get() - 1.0); //1 deg
+ }
+
+ // aim more left
+ public void bumpUpTurretOffset() {
+ turretOffset.set(turretOffset.get() + 2.5); //2.5 deg
+ }
+
+ // aim more right
+ public void bumpDownTurretOffset() {
+ turretOffset.set(turretOffset.get() - 2.5); //2.5 deg
+ }
+
+ @Override
+ public void execute() {
+ // Phase manager stuff
+ phaseManager.update(drivepose, shooter, turret);
+ target = phaseManager.getTarget(drivepose);
+
+ updateDrivePose();
+ updateSetpoints(drivepose);
+
+ if (phaseManager.isIdle()) {
+ underLadder();
+ } else {
+ if (spindexer.noIndexing) {
+ spindexer.noIndexing = false;
+ }
+ turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), turretVelocity - drivetrain.getAngularRate(2));
+
+ boolean shuttling = !target.equals(FieldConstants.getHubTranslation().toTranslation2d()); // if we're aiming at the hub, we're not shuttling
+
+ // shuttling will move the hood so its angles very close to max (less arch)
+ if (shuttling) {
+ hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShuttleInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity);
+ } else {
+ hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity);
+ }
+
+ // if (FieldConstants.underTrench(x, y)) {
+ // System.out.println("Hood forced down");
+ // } else {
+ // hood.forceHoodDown(false);
+ // }
+
+ // different maps for shuttling vs shooting. Less powerful when shuttling.
+ if (shuttling) {
+ shooter.setShooter(-ShuttleInterpolation.shooterVelocityMap.get(distanceFromTarget));
+ } else {
+ shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget));
+ }
+
+ if (!Constants.DISABLE_LOGGING) {
+ // record when shuttling
+ Logger.recordOutput("Shuttling", shuttling);
+ // record distance for tuning if needed
+ Logger.recordOutput("Distance From Target", distanceFromTarget);
+ }
+ }
+
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Turret Calculated Setpoint", turretSetpoint);
+ Logger.recordOutput("Hood Calculate Setpoint", hoodSetpoint);
+ Logger.recordOutput("Shooter Calculate Velocity", goalState.exitVel());
+
+ Logger.recordOutput("DistanceToTarget", target.getDistance(drivepose.getTranslation()));
+ }
+
+ // for operator
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putString("Phase Manager State", phaseManager.getCurrentState().toString());
+
+ } else {
+ phaseDelay.set(0.03);
+ }
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ stowEverything();
+ }
+
+}
--- /dev/null
+package frc.robot.commands.vision;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.DoNothing;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.Vision.DetectedObject;
+
+public class AcquireGamePiece extends SequentialCommandGroup {
+ /**
+ * Intakes a game piece
+ *
+ * @param gamePiece The supplier for the game piece to intake
+ * @param drive The drivetrain
+ */
+ public AcquireGamePiece(Supplier<DetectedObject> gamePiece, Drivetrain drive){
+ // TODO: Replace DoNothing with next year's intake command
+ addCommands(new DoNothing().deadlineFor(new DriveToGamePiece(gamePiece, drive)));
+ }
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.commands.vision;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import frc.robot.commands.drive_comm.DefaultDriveCommand;
+import frc.robot.constants.VisionConstants;
+import frc.robot.controls.BaseDriverConfig;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.Vision.DetectedObject;
+
+public class AimAtGamePiece extends DefaultDriveCommand {
+ private Supplier<DetectedObject> objectSupplier;
+ private static int ticksSinceLastObject;
+ private static DetectedObject cachedObject;
+
+
+ public AimAtGamePiece(Drivetrain drive, BaseDriverConfig driver, Supplier<DetectedObject> objectSupplier){
+ super(drive, driver);
+ this.objectSupplier = objectSupplier;
+ }
+
+ @Override
+ public void initialize() {
+ cachedObject = null;
+ ticksSinceLastObject = 0;
+ super.initialize();
+ }
+
+ @Override
+ protected void drive(ChassisSpeeds speeds){
+ if(!VisionConstants.OBJECT_DETECTION_ENABLED){
+ super.drive(speeds);
+ return;
+ }
+ DetectedObject object = objectSupplier.get();
+
+ if(object == null || !object.isGamePiece()) {
+ if (ticksSinceLastObject <= VisionConstants.MAX_EMPTY_TICKS && cachedObject != null) {
+ object = cachedObject;
+ } else {
+ super.drive(speeds);
+ return;
+ }
+ ticksSinceLastObject++;
+ } else {
+ ticksSinceLastObject = 0;
+ cachedObject = object;
+ }
+
+ // System.out.println("objangle " + object.getAngle());
+ swerve.driveHeading(
+ speeds.vxMetersPerSecond,
+ speeds.vyMetersPerSecond,
+ MathUtil.angleModulus(object.getAngle()),
+ true);
+ }
+}
--- /dev/null
+package frc.robot.commands.vision;
+
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.FieldConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/**
+ * Aims the robot at the closest April tag
+ */
+public class AimAtTag extends Command {
+ private Drivetrain drive;
+ private PIDController pid;
+
+ /**
+ * Aims the robot at the closest April tag
+ * @param drive The drivetrain
+ */
+ public AimAtTag(Drivetrain drive){
+ this.drive = drive;
+ // Copy drive PID and changetolerance
+ pid = new PIDController(
+ drive.getRotationController().getP(),
+ drive.getRotationController().getI(),
+ drive.getRotationController().getD()
+ );
+ pid.setTolerance(Units.degreesToRadians(1));
+ addRequirements(drive);
+ }
+
+ /**
+ * Gets the closest tag and sets the setpoint to aim at it
+ */
+ @Override
+ public void initialize(){
+ double dist = Double.POSITIVE_INFINITY;
+ Translation2d closest = new Translation2d();
+ Translation2d driveTranslation = drive.getPose().getTranslation();
+ for(AprilTag tag : FieldConstants.field.getTags()){
+ Translation2d translation = tag.pose.toPose2d().getTranslation();
+ double dist2 = driveTranslation.getDistance(translation);
+ if(dist2 < dist){
+ dist = dist2;
+ closest = translation;
+ }
+ }
+ pid.reset();
+ pid.setSetpoint(Math.atan2(closest.getY() - driveTranslation.getY(), closest.getX() - driveTranslation.getX()));
+ }
+
+ /**
+ * Runs the PID
+ */
+ @Override
+ public void execute() {
+ double angle = drive.getPose().getRotation().getRadians();
+ // If the distance between the angles is more than 180 degrees, use an identical angle ±360 degrees
+ if(angle - pid.getSetpoint() > Math.PI){
+ angle -= 2*Math.PI;
+ }else if(angle - pid.getSetpoint() < -Math.PI){
+ angle += 2*Math.PI;
+ }
+ double speed = pid.calculate(angle);
+ drive.drive(0, 0, speed, true, false);
+ }
+
+ /**
+ * Stops the drivetrain
+ * @param interrupted If the command is interrupted
+ */
+ @Override
+ public void end(boolean interrupted) {
+ drive.stop();
+ }
+
+ /**
+ * Returns if the command is finished
+ * @return If the PID is at the setpoint
+ */
+ @Override
+ public boolean isFinished() {
+ return pid.atSetpoint();
+ }
+}
+
--- /dev/null
+package frc.robot.commands.vision;
+
+import java.util.ArrayList;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.MathUtils;
+import frc.robot.util.Vision.Vision;
+
+/**
+ * Calculates standard deviations for vision
+ */
+public class CalculateStdDevs extends Command {
+ private final Vision vision;
+ private ArrayList<Pose2d> poses;
+ private int arrayLength;
+ private Timer endTimer;
+ private Drivetrain drive;
+
+ /**
+ * Constructor for CalculateStdDevs
+ * @param posesToUse the amount of poses to take the standard deviation of. More poses will take more time.
+ * @param vision The vision
+ */
+ public CalculateStdDevs(int posesToUse, Vision vision, Drivetrain drive) {
+ this.vision = vision;
+ this.drive = drive;
+ arrayLength = posesToUse;
+ endTimer = new Timer();
+ }
+
+ /**
+ * Resets the pose array
+ */
+ @Override
+ public void initialize() {
+ // create the ArrayList of poses to store
+ // an ArrayList prevents issues if the command ends early, and makes checking if the command has finished easy
+ poses = new ArrayList<Pose2d>();
+
+ drive.setVisionEnabled(false);
+ }
+
+ /**
+ * Adds a pose to the array
+ */
+ @Override
+ public void execute() {
+ Pose2d pose = vision.getPose2d(drive.getPose());
+ // If the pose exists, add it to the first open spot in the array
+ if (pose != null) {
+ // if we see a pose, reset the timer (it will be started the next time it doesn't get a pose)
+ endTimer.stop();
+ endTimer.reset();
+ // add the pose to our data
+ poses.add(pose);
+ if(poses.size()%10==0){
+ System.out.printf("%.0f%% done\n", ((double)poses.size())/arrayLength * 100);
+ }
+ } else {
+ endTimer.start();
+ // If kStdDevCommandEndTime seconds have passed since it saw an April tag, stop the command
+ // Prevents it from running forever
+ if (endTimer.hasElapsed(10)) {
+ cancel();
+ }
+ }
+ }
+
+ /**
+ * Calculates the standard deviation
+ */
+ @Override
+ public void end(boolean interrupted) {
+ drive.setVisionEnabled(true);
+
+ // If the array is empty, don't try to calculate std devs
+ if (poses.size() == 0) {
+ System.out.println("There are no poses in the array\nTry again where the robot can see an April tag.");
+ return;
+ }
+
+ // create arrays of the poses by X, Y, and Rotation for calculations
+ double[] xArray = new double[poses.size()];
+ double[] yArray = new double[poses.size()];
+ double[] rotArray = new double[poses.size()];
+
+ // copy the values into the arrays
+ for (int i = 0; i < poses.size(); i++) {
+ xArray[i] = poses.get(i).getX();
+ yArray[i] = poses.get(i).getY();
+ rotArray[i] = poses.get(i).getRotation().getRadians();
+ }
+
+ // Calculate the standard deviations
+ double stdDevX = MathUtils.stdDev(xArray);
+ double stdDevY = MathUtils.stdDev(yArray);
+ double stdDevRot = MathUtils.stdDev(rotArray);
+
+ // Find distance to tag
+ double distance;
+ try{
+ distance = vision.getEstimatedPoses(drive.getPose()).get(0).targetsUsed.get(0).getBestCameraToTarget().getTranslation().getNorm();
+ }catch(Exception e){
+ System.out.println("Could not see a target");
+ distance = -1;
+ }
+
+ // Print and log values
+ System.out.printf("Standard deviation values:\nX: %.5f\nY: %.5f\nRotation: %.5f\nDistance: %.5f\n",
+ stdDevX, stdDevY, stdDevRot, distance);
+ }
+
+ /**
+ * Returns if the command is finished
+ * @return If the array is full
+ */
+ @Override
+ public boolean isFinished() {
+ return poses.size() >= arrayLength;
+ }
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.commands.vision;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import frc.robot.commands.drive_comm.DriveToPose;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.Vision.DetectedObject;
+
+/**
+ * Moves toward the detected object
+ */
+public class DriveToGamePiece extends DriveToPose {
+ private static boolean constantUpdate = true;
+ private static int ticksSinceLastObject;
+ private static DetectedObject cachedObject;
+
+ /**
+ * Moves toward the detected object
+ * @param detectedObject The supplier for the detected object to use
+ * @param drive The drivetrain
+ */
+ public DriveToGamePiece(Supplier<DetectedObject> detectedObject, Drivetrain drive) {
+ super(drive,
+ () -> getPose(detectedObject, drive)
+ );
+ updateTarget = constantUpdate;
+ }
+
+ @Override
+ public void initialize() {
+ cachedObject = null;
+ ticksSinceLastObject = 0;
+ super.initialize();
+ }
+
+ public static Pose2d getPose(Supplier<DetectedObject> supplier, Drivetrain drive){
+ DetectedObject object = supplier.get();
+ if(object == null || !object.isGamePiece()) {
+ if (ticksSinceLastObject <= VisionConstants.MAX_EMPTY_TICKS && cachedObject != null) {
+ object = cachedObject;
+ } else {
+ return null;
+ }
+ ticksSinceLastObject++;
+ } else {
+ ticksSinceLastObject = 0;
+ cachedObject = object;
+ }
+ Rotation2d rotation = new Rotation2d(MathUtil.angleModulus(object.getAngle()));
+ Translation2d objectTranslation = object.pose.toPose2d().getTranslation();
+ Translation2d diff = objectTranslation.minus(drive.getPose().getTranslation());
+ Translation2d translation = objectTranslation.minus(diff.times(DriveConstants.ROBOT_WIDTH_WITH_BUMPERS/2/diff.getNorm()));
+ return new Pose2d(translation, rotation);
+ }
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.commands.vision;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+public class GoToPose2 extends Command {
+ private static final double MIN_ACCEL = 2;
+ private final Supplier<Pose2d> poseSupplier;
+ private final Drivetrain drive;
+ private Pose2d pose;
+ private double vx;
+ private double vy;
+ private Pose2d error;
+
+ public GoToPose2(Supplier<Pose2d> poseSupplier, Drivetrain drive){
+ this.poseSupplier = poseSupplier;
+ this.drive = drive;
+ addRequirements(drive);
+ }
+
+ @Override
+ public void initialize(){
+ pose = poseSupplier.get();
+ ChassisSpeeds v = drive.getChassisSpeeds();
+ vx = v.vxMetersPerSecond;
+ vy = v.vyMetersPerSecond;
+ error = null;
+ }
+
+ @Override
+ public void execute(){
+ if(pose == null){
+ return;
+ }
+ Pose2d drivePose = drive.getPose();
+ error = drivePose.relativeTo(pose);
+ double ax = calcAccel(vx, error.getX());
+ double ay = calcAccel(vy, error.getY());
+ if(Math.abs(ax) < MIN_ACCEL && Math.abs(error.getX()) > 0.01){
+ ax = -Math.signum(error.getX())*MIN_ACCEL;
+ }
+ if(Math.abs(ay) < MIN_ACCEL && Math.abs(error.getY()) > 0.01){
+ ay = -Math.signum(error.getY())*MIN_ACCEL;
+ }
+ vx += ax*Constants.LOOP_TIME;
+ vy += ay*Constants.LOOP_TIME;
+ Translation2d v = new Translation2d(vx, vy).rotateBy(pose.getRotation());
+ drive.driveHeading(v.getX(), v.getY(), pose.getRotation().getRadians(), true);
+ }
+
+ @Override
+ public void end(boolean interrupted){
+ drive.stop();
+ }
+
+ @Override
+ public boolean isFinished(){
+ return pose == null || error != null && error.getTranslation().getNorm() < 0.01;
+ }
+
+ private double calcAccel(double v, double x){
+ if(Math.abs(x) < 0.001 || Math.abs(Math.signum(v) - Math.signum(x)) < 0.5){
+ return 0;
+ }
+ double a = v*v/2/x;
+ double a2 = -v/Constants.LOOP_TIME;
+ if(Math.abs(a2) < Math.abs(a)){
+ return a2;
+ }
+ return a;
+ }
+}
--- /dev/null
+package frc.robot.commands.vision;
+
+import java.util.function.Supplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.Constants;
+import frc.robot.util.Vision.DetectedObject;
+
+public class LogVision extends Command {
+ private Supplier<DetectedObject> objectSupplier;
+ public LogVision(Supplier<DetectedObject> objectSupplier){
+ this.objectSupplier = objectSupplier;
+ }
+
+ @Override
+ public void execute() {
+ DetectedObject object = this.objectSupplier.get();
+ if (object != null) {
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Vision/object_angle", object.getAngle());
+ Logger.recordOutput("Vision/object_distance", object.getDistance());
+ }
+ }
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+
+}
--- /dev/null
+package frc.robot.commands.vision;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.VisionConstants;
+import frc.robot.util.Vision.DetectedObject;
+import frc.robot.util.Vision.Vision;
+
+/**
+ * Adds data from object detection vision to SmartDashboard
+ */
+public class ReturnData extends Command{
+ private final Vision vision;
+ private final Timer timer = new Timer();
+
+ /**
+ * Adds data from object detection vision to Smartdashboard
+ * @param vision The vision
+ */
+ public ReturnData(Vision vision){
+ this.vision = vision;
+ }
+
+ @Override
+ public void initialize(){
+ timer.reset();
+ timer.start();
+ }
+
+ /**
+ * Adds the data to SmartDashboard
+ */
+ @Override
+ public void execute() {
+ if(timer.advanceIfElapsed(2)){
+ double[] xOffset = vision.getHorizontalOffset();
+ double[] yOffset = vision.getVerticalOffset();
+ // long[] objectClass = vision.getDetectedObjectClass();
+
+ // //put the offsets and area on SmartDashboard for testing
+ // SmartDashboard.putNumberArray("Object X offsets degrees", xOffset);
+ // SmartDashboard.putNumberArray("Object Y offsets degrees", yOffset);
+ // SmartDashboard.putNumberArray("Object Distances", vision.getDistance());
+
+ DetectedObject bestGamePiece = vision.getBestGamePiece(Math.PI, false);
+ if(bestGamePiece!=null){
+ // SmartDashboard.putString("Vision best game piece", bestGamePiece.toString());
+ System.out.println("\n\nBest game piece: "+bestGamePiece);
+ }
+
+ if ((xOffset.length != 0) == (yOffset.length != 0)) {
+ for (int i = 0; i < xOffset.length; i++) {
+ System.out.printf("\nx: %.2f, y: %.2f, type: %s\n", xOffset[i], yOffset[i], DetectedObject.getType(0));
+ DetectedObject object = new DetectedObject(Units.degreesToRadians(xOffset[i]), Units.degreesToRadians(yOffset[i]), 0, VisionConstants.OBJECT_DETECTION_CAMERAS.get(0));
+ System.out.printf("Object: %s\nDistance: %.2f, Angle: %.2f\n", object, object.getDistance(), Units.radiansToDegrees(object.getAngle()));
+ }
+ }else {
+ System.out.println("One of the arrays is empty!");
+ }
+ }
+ }
+
+ /**
+ * Does nothing
+ * @param interrupted If the command is interrupted
+ */
+ @Override
+ public void end(boolean interrupted) {
+
+ }
+
+ /**
+ * Returns if the command is finished
+ * @retrun Always false (command never finishes)
+ */
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+
+
+}
+
--- /dev/null
+package frc.robot.commands.vision;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import frc.robot.constants.VisionConstants;
+
+/**
+ * Shutdown all Orange Pis listed by hostname in
+ * {@link frc.robot.constants.VisionConstants}
+ */
+public class ShutdownAllPis extends ParallelCommandGroup {
+ public ShutdownAllPis() {
+ ShutdownOrangePi[] commands =
+ new ShutdownOrangePi[VisionConstants.ORANGEPI_HOSTNAMES.length];
+ for (int i = 0; i < commands.length; i++) {
+ commands[i] = new ShutdownOrangePi(VisionConstants.ORANGEPI_HOSTNAMES[i]);
+ }
+
+ addCommands(commands);
+ }
+}
--- /dev/null
+package frc.robot.commands.vision;
+
+import java.io.IOException;
+import java.io.InputStream;
+import java.nio.charset.StandardCharsets;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.nio.file.StandardCopyOption;
+import java.nio.file.attribute.PosixFilePermissions;
+
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Robot;
+import frc.robot.constants.VisionConstants;
+
+/**
+ * Run the ssh command to shutdown a single Orange Pi.
+ * Uses the username and password set in {@link frc.robot.constants.VisionConstants}.
+ */
+public class ShutdownOrangePi extends Command {
+ private String hostname;
+ private Process process;
+
+ /**
+ * @param hostname The hostname or IP of the orangepi to shut down.
+ */
+ public ShutdownOrangePi(String hostname) {
+ assert hostname != null;
+ this.hostname = hostname;
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+
+ @Override
+ public void initialize() {
+ if (Robot.isSimulation()) {
+ // needs to run on an actual roborio because of architecture-specific binaries
+ System.out.println("Would shut down OrangePi at " + hostname + " if this was real.");
+ return;
+ }
+
+ try {
+ String initialPath = Filesystem.getDeployDirectory() + "/sshpass";
+ Path initalPathPath = Path.of(initialPath);
+ String binPath = "/home/lvuser/sshpass2";
+ Path binPathPath = Path.of(binPath);
+ // copies to be able to get executable permissions on the new binary
+ Files.copy(initalPathPath, binPathPath, StandardCopyOption.REPLACE_EXISTING);
+ Files.setPosixFilePermissions(binPathPath, PosixFilePermissions.fromString("rwxr-xr-x"));
+
+ String[] commandString = new String[] {
+ binPath,
+ "-p", "raspberry",
+ "ssh",
+ "-o", "StrictHostKeyChecking=no",
+ VisionConstants.ORANGEPI_USERNAME + "@" + hostname,
+ "sudo", "shutdown", "now"
+ };
+
+ this.process = Runtime.getRuntime().exec(commandString);
+ } catch (Exception e) {
+ String message = e.getMessage() == null ? "unknown" : e.getMessage();
+ System.out.println("Failed to shutdown OrangePi. Reason: " + e.getClass() + " -- " + message);
+ }
+ }
+
+ @Override
+ public void execute() {
+ if (this.process == null) return;
+
+ try {
+ InputStream stdout = this.process.getInputStream();
+ InputStream stderr = this.process.getErrorStream();
+
+ int remainingStdoutBytes = stdout.available();
+ int remainingStderrBytes = stderr.available();
+
+ if (remainingStdoutBytes > 0) {
+ byte[] stdoutBytes = stdout.readNBytes(remainingStdoutBytes);
+ System.out.println("OPI: " + new String(stdoutBytes, StandardCharsets.UTF_8));
+ }
+
+ if (remainingStderrBytes > 0) {
+ byte[] stderrBytes = stderr.readNBytes(remainingStderrBytes);
+ System.err.println("OPI: " + new String(stderrBytes, StandardCharsets.UTF_8));
+ }
+ } catch (IOException e) {}
+ }
+
+ @Override
+ public boolean isFinished() {
+ return this.process == null || !this.process.isAlive();
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ if (this.process == null) return;
+
+ if (this.process.isAlive()) {
+ this.process.destroy(); // end the process if we've been interrupted
+ } else {
+ // only grab exit value if the process has had time to exit
+ int exitValue = this.process.exitValue();
+ if (exitValue != 0) // abnormal termination
+ System.out.println("OrangePi shutdown of " + hostname + " failed with exit code " + exitValue + ".");
+ else
+ System.out.println("OrangePi shutdown of " + hostname + " succesful.");
+ }
+ }
+}
--- /dev/null
+package frc.robot.commands.vision;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.Vision.Vision;
+
+/**
+ * Gathers data on the distance limits of the camera used for vision.
+ */
+public class TestVisionDistance extends Command {
+ private final Drivetrain drive;
+ private final Vision vision;
+ private Translation2d visionStartTranslation, driveStartTranslation;
+ private Pose2d currentPose = null;
+ private double driveDistance;
+ private double visionDistance;
+
+ private double speed;
+
+ private final Timer endTimer = new Timer();
+ private final Timer printTimer = new Timer();
+
+ // How many seconds of not seeing april tag before ending the command
+ private static final double END_DELAY = 0.25;
+
+ // How many seconds between each data print
+ private static final double PRINT_DELAY = 1;
+
+ /**
+ * Constructor for TestVisionDistance
+ * @param speed What speed to move at, negative if backward
+ * @param drive The drivetrain
+ * @param vision The vision
+ */
+ public TestVisionDistance(double speed, Drivetrain drive, Vision vision){
+ addRequirements(drive);
+ this.drive = drive;
+ this.speed = speed;
+ this.vision = vision;
+ }
+
+ /**
+ * Starts the timers and disables vision for odometry
+ */
+ @Override
+ public void initialize() {
+
+ endTimer.reset();
+ printTimer.restart();
+
+ drive.setVisionEnabled(false);
+
+ currentPose = vision.getPose2d(drive.getPose());
+ visionStartTranslation = currentPose.getTranslation();
+ driveStartTranslation = drive.getPose().getTranslation();
+ driveDistance = 0;
+ visionDistance = 0;
+ }
+
+ /**
+ * Drives the robot, finds the pose from the drivetrain and vision, and someimes prints the distances
+ */
+ @Override
+ public void execute() {
+ drive.drive(speed, 0, 0, false, false);
+ Pose2d newestPose = vision.getPose2d(currentPose, drive.getPose());
+
+ // If the camera can see the apriltag
+ if (newestPose != null) {
+ //update current pose
+ currentPose = newestPose;
+ // reset the timer
+ endTimer.reset();
+ driveDistance = drive.getPose().getTranslation().getDistance(driveStartTranslation);
+ visionDistance = currentPose.getTranslation().getDistance(visionStartTranslation);
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putNumber("Vision test drive distance", driveDistance);
+ SmartDashboard.putNumber("Vision test vision distnace", visionDistance);
+ SmartDashboard.putNumber("Vision test error", visionDistance - driveDistance);
+ SmartDashboard.putNumber("Vision test % error", (visionDistance-driveDistance) / driveDistance * 100);
+ }
+
+ // If kPrintDelay seconds have passed, print the data
+ if (printTimer.advanceIfElapsed(PRINT_DELAY)) {
+ System.out.printf("\nDrive dist: %.2f\nVision dist: %.2f\nError: %.2f\n %% error: %.2f\n",
+ driveDistance, visionDistance,
+ visionDistance-driveDistance, (visionDistance-driveDistance) / driveDistance * 100
+ );
+ }
+ } else {
+ endTimer.start();
+ }
+ }
+
+ /**
+ * Re-enables vision and stops the robot
+ */
+ @Override
+ public void end(boolean interrupted) {
+ drive.setVisionEnabled(true);
+ drive.stop();
+ }
+
+ /**
+ * Returns if the command is finished
+ * @return If the end delay has elapsed
+ */
+ @Override
+ public boolean isFinished() {
+ return endTimer.hasElapsed(END_DELAY);
+ }
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.constants;
+
+import com.pathplanner.lib.config.ModuleConfig;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import frc.robot.constants.swerve.DriveConstants;
+
+/**
+ * Container class for auto constants.
+ */
+public class AutoConstants {
+
+ // Pathplanner output folder should be src/main/deploy/pathplanner
+ public static final String TRAJECTORY_DIRECTORY = "pathplanner/paths/";
+
+ public static final double MAX_AUTO_SPEED = 5.2; // m/s
+ public static final double MAX_AUTO_ACCEL = 4.8; // m/s^2
+
+ public static RobotConfig CONFIG;
+ public static final PPHolonomicDriveController AUTO_CONTROLLER = new PPHolonomicDriveController(
+ new PIDConstants(3.5, 0.0, 1.0), // Translation PID constants
+ new PIDConstants(4.0, 0.0, 1.0) // Rotation PID constants
+ );
+
+
+
+ static {
+ try{
+ CONFIG = RobotConfig.fromGUISettings();
+ }catch(Exception e){
+ e.printStackTrace();
+ // Although these values are probably wrong and auto might not work correctly, at least it won't cause NullPointerExceptions
+ CONFIG = new RobotConfig(50, 0.5, new ModuleConfig(DriveConstants.WHEEL_RADIUS, MAX_AUTO_SPEED, DriveConstants.COSF, DCMotor.getKrakenX60(1).withReduction(DriveConstants.DRIVE_GEAR_RATIO), DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT, 1), DriveConstants.MODULE_LOCATIONS);
+ }
+ }
+}
--- /dev/null
+package frc.robot.constants;
+
+import com.ctre.phoenix6.CANBus;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+public class Constants {
+
+ // constants:
+
+ public static final double GRAVITY_ACCELERATION = 9.8;
+ public static final double ROBOT_VOLTAGE = 12.0;
+ public static final double LOOP_TIME = 0.02;
+
+ // CAN bus names
+ public static final CANBus CANIVORE_CAN = new CANBus("CANivore");
+ public static final CANBus CANIVORE_SUB = new CANBus("CANivoreSub");
+ public static final CANBus RIO_CAN = new CANBus("rio");
+
+ // Logging
+ public static final boolean USE_TELEMETRY = true;
+
+ // this would disable all logger calls
+ public static final boolean DISABLE_LOGGING = true;
+ public static final boolean DISABLE_SMART_DASHBOARD = true; // wont disable auto picker
+
+ public static enum Mode {
+ /** Running on a real robot. */
+ REAL,
+
+ /** Running a physics simulator. */
+ SIM,
+
+ /** Replaying from a log file. */
+ REPLAY
+ }
+
+ // Kraken Speed
+ public static double MAX_RPM = 5800.0; // Rotations per minute
+
+ /*
+ * Talon Stator / Supply Limits explanation
+ * Supply current is current that's being drawn at the input bus voltage. Stator
+ * current is current that's being drawn by the motor.
+ * Supply limiting (supported by Talon FX and SRX) is useful for preventing
+ * breakers from tripping in the PDP.
+ * Stator limiting (supported by Talon FX) is useful for limiting
+ * acceleration/heat.
+ */
+
+ // These are the default values
+
+ // Stator
+ public static final boolean TALONFX_STATOR_LIMIT_ENABLE = false; // enabled?
+ public static final double TALONFX_STATOR_CURRENT_LIMIT = 100; // Limit(amp)
+ public static final double TALONFX_STATOR_TRIGGER_THRESHOLD = 100; // Trigger Threshold(amp)
+ public static final double TALONFX_STATOR_TRIGGER_DURATION = 0; // Trigger Threshold Time(s)
+
+ // Supply
+ public static final boolean TALONFX_SUPPLY_LIMIT_ENABLE = false; // enabled?
+ public static final double TALONFX_SUPPLY_CURRENT_LIMIT = 40; // Limit(amp), current to hold after trigger hit
+ public static final double TALONFX_SUPPLY_TRIGGER_THRESHOLD = 55; // (amp), amps to activate trigger
+ public static final double TALONFX_SUPPLY_TRIGGER_DURATION = 3; // (s), how long after trigger before reducing
+
+ // OIConstants:
+
+ public static final int DRIVER_JOY = 0;
+ public static final int OPERATOR_JOY = 1;
+ public static final int TEST_JOY = 2;
+ public static final int MANUAL_JOY = 3;
+ public static final double DEFAULT_DEADBAND = 0.00005;
+
+ public static final double TRANSLATIONAL_DEADBAND = 0.01;
+
+ public static final double ROTATION_DEADBAND = 0.01;
+
+ public static final double HEADING_DEADBAND = 0.05;
+ public static final double HEADING_SLEWRATE = 10;
+
+ //Modes
+ public static final Mode SIM_MODE = Mode.SIM;
+ public static final Mode CURRENT_MODE = RobotBase.isReal() ? Mode.REAL : SIM_MODE;
+
+ // Enables 3D logs of mechanisms
+ public static final boolean LOG_MECHANISMS = true;
+
+ // Network setting for vision
+ public static final String VISION_CAMERA_HOST = "10.9.72.12";
+}
--- /dev/null
+package frc.robot.constants;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import frc.robot.util.Zone;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc.robot.Robot;
+import frc.robot.constants.swerve.DriveConstants;
+
+public class FieldConstants {
+
+ /** Apriltag layout for 2026 REBUILT */
+ public static final AprilTagFieldLayout field = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
+
+ /** Width of the field [meters] */
+ public static final double FIELD_LENGTH = field.getFieldLength();
+ /** Height of the field [meters] */
+ public static final double FIELD_WIDTH = field.getFieldWidth();
+
+ public static final double RED_BORDER = FIELD_LENGTH / 2 + Units.inchesToMeters(155);
+ public static final double BLUE_BORDER = FIELD_LENGTH / 2 - Units.inchesToMeters(155);
+ public static final double LEFT_SIDE_TARGET = FIELD_WIDTH * 0.225;
+ public static final double RIGHT_SIDE_TARGET = FIELD_WIDTH * 0.775;
+
+ /** Location of hub target */
+ public static final Translation3d HUB_BLUE = new Translation3d(Units.inchesToMeters(182.11), FIELD_WIDTH / 2,
+ Units.inchesToMeters(72));
+
+ public static final Translation3d HUB_RED = new Translation3d(FIELD_LENGTH - Units.inchesToMeters(182.11),
+ FIELD_WIDTH / 2, Units.inchesToMeters(72));
+
+ // shuttle safety constants
+ public static final double LADDER_ZONE_WIDTH = 1.251; // meters
+ public static final double LADDER_ZONE_DEPTH = 1.143; // meters
+
+ // tower positions (center of the zone)
+ public static final double LADDER_Y_OFFSET = 1.7526 * 2; // two driver stations
+ public static final Pose2d BLUE_CLIMB_LOCATION = new Pose2d(LADDER_ZONE_DEPTH / 2,
+ FIELD_WIDTH - LADDER_Y_OFFSET - LADDER_ZONE_WIDTH / 2, new Rotation2d());
+ public static final Pose2d RED_CLIMB_LOCATION = new Pose2d(FIELD_LENGTH - LADDER_ZONE_DEPTH / 2,
+ LADDER_Y_OFFSET + LADDER_ZONE_WIDTH / 2, new Rotation2d());
+
+ public static final Pose2d getClimbLocation() {
+ if (Robot.getAlliance() == Alliance.Blue) {
+ return BLUE_CLIMB_LOCATION;
+ } else {
+ return RED_CLIMB_LOCATION;
+ }
+ }
+
+ public static final Translation3d NEUTRAL_LEFT = new Translation3d(FIELD_LENGTH / 2, LEFT_SIDE_TARGET, 0);
+
+ public static final Translation3d NEUTRAL_RIGHT = new Translation3d(FIELD_LENGTH / 2, RIGHT_SIDE_TARGET, 0);
+
+ // previous hub + a few feet further back
+ public static final Translation3d ALLIANCE_LEFT_BLUE = new Translation3d(BLUE_BORDER - 3.2, LEFT_SIDE_TARGET, 0);
+
+ public static final Translation3d ALLIANCE_RIGHT_BLUE = new Translation3d(BLUE_BORDER - 2.2, RIGHT_SIDE_TARGET, 0);
+ // previous hub + a few feet further back
+
+ public static final Translation3d ALLIANCE_LEFT_RED = new Translation3d(RED_BORDER + 2.2, LEFT_SIDE_TARGET, 0);
+
+ public static final Translation3d ALLIANCE_RIGHT_RED = new Translation3d(RED_BORDER + 3.2, RIGHT_SIDE_TARGET, 0);
+
+ public static final Translation3d ALLIANCE_CENTER_BLUE = new Translation3d(BLUE_BORDER - 2, FIELD_WIDTH / 2, 0);
+
+ public static final Translation3d ALLIANCE_CENTER_RED = new Translation3d(RED_BORDER + 2, FIELD_WIDTH / 2, 0);
+
+ public static final double BLUE_ALLIANCE_LINE = BLUE_BORDER; // That's the distance from one side to the blue bump
+ public static final double RED_ALLIANCE_LINE = RED_BORDER; // That's the distance from one side to the red bump
+
+ // my zones
+ public static final double leftNeutralLine = FIELD_LENGTH * 0.25;
+ public static final double rightNeutralLine = FIELD_LENGTH * 0.75;
+ public static final double centerLengthLine = FIELD_LENGTH * 0.5;
+ public static final double centerWidthLine = FIELD_WIDTH * 0.5;
+ public static final double redLine = 179.111250;
+ public static final double blueLine = FIELD_LENGTH - 179.111250;
+
+ public static final double hubWidthLeft = FIELD_WIDTH / 2 - (47.0 / 2);
+ public static final double hubWidthRight = FIELD_WIDTH / 2 + (47.0 / 2);
+ public static final double hubBackRed = FIELD_LENGTH + 120.0;
+ public static final double hubBackBlue = FIELD_LENGTH - 120.0;
+
+ public static final double ladderRedLeft = FIELD_WIDTH + 13.0;
+ public static final double ladderBlueLeft = FIELD_WIDTH - 12.375;
+ public static final double ladderRedRight = FIELD_WIDTH - 35.75;
+ public static final double ladderBlueRight = FIELD_WIDTH + 35.75;
+
+ public static final double TRENCH_CENTER_CHANNEL_WIDTH_INCHES = 50.0;
+ public static final double TRENCH_X_MIN_INCHES = 152.5;
+ public static final double TRENCH_X_MAX_INCHES = 187.5;
+
+ public static final Zone neutralStrip = new Zone(centerLengthLine, centerWidthLine,
+ rightNeutralLine - leftNeutralLine, redLine - blueLine);
+ public static final Zone neutralLeft = new Zone(centerLengthLine, centerWidthLine, rightNeutralLine - leftNeutralLine,
+ redLine - blueLine);
+ public static final Zone neutralRight = new Zone(centerLengthLine, centerWidthLine,
+ rightNeutralLine - leftNeutralLine, redLine - blueLine);
+ public static final Zone blueHubOut = new Zone(centerLengthLine, centerWidthLine, rightNeutralLine - leftNeutralLine,
+ redLine - blueLine);
+ public static final Zone redHubOut = new Zone(centerLengthLine, centerWidthLine, rightNeutralLine - leftNeutralLine,
+ redLine - blueLine);
+
+ public enum ShootingTarget {
+ HUB,
+ NEUTRAL,
+ ALLIANCE,
+ OPPOSITION, // not sure why you'd ever do this :)
+ }
+
+ public enum FieldZone {
+ ALLIANCE,
+ NEUTRAL,
+ OPPOSITION,
+ TRENCH_BUMP,
+ UNDER_LADDER,
+ UNDER_MY_HUB
+ }
+
+ /** checks if robot is under climb structure */
+ public static boolean underLadder(Translation2d drivepose) {
+ Pose2d ladderPos = getClimbLocation();
+ double dx = Math.abs(drivepose.getX() - ladderPos.getX());
+ double dy = Math.abs(drivepose.getY() - ladderPos.getY());
+ return (dx < LADDER_ZONE_DEPTH / 2.0) && (dy < LADDER_ZONE_WIDTH / 2.0);
+ }
+
+ public static Translation3d getHubTranslation() {
+ if (Robot.getAlliance() == Alliance.Blue) {
+ return HUB_BLUE;
+ } else {
+ return HUB_RED;
+ }
+ }
+
+ public static Translation3d getNeutralTranslation(boolean sideLeft) {
+ if (sideLeft) {
+ return NEUTRAL_LEFT;
+ } else {
+ return NEUTRAL_RIGHT;
+ }
+ }
+
+ public static Translation3d getAllianceSideTranslation(boolean sideLeft) {
+ if (sideLeft) {
+ if (Robot.getAlliance() == Alliance.Blue) {
+ return ALLIANCE_LEFT_BLUE;
+ } else {
+ return ALLIANCE_LEFT_RED;
+ }
+ } else {
+ if (Robot.getAlliance() == Alliance.Blue) {
+ return ALLIANCE_RIGHT_BLUE;
+ } else {
+ return ALLIANCE_RIGHT_RED;
+ }
+ }
+ }
+
+ public static Translation3d getAllianceCenterTranslation() {
+ if (Robot.getAlliance() == Alliance.Blue) {
+ return ALLIANCE_CENTER_BLUE;
+ } else {
+ return ALLIANCE_CENTER_RED;
+ }
+ }
+
+ public static FieldZone getZone(Translation2d drivepose) {
+ double x = drivepose.getX();
+
+ if ((x < FIELD_LENGTH / 2 - Units.inchesToMeters(120.0)
+ && x > (BLUE_ALLIANCE_LINE + (DriveConstants.ROBOT_WIDTH_WITH_BUMPERS) / 2)) // blue alliance line
+ || x > FIELD_LENGTH / 2 + Units.inchesToMeters(120.0)
+ && x < (RED_ALLIANCE_LINE - (DriveConstants.ROBOT_WIDTH_WITH_BUMPERS) / 2)) {
+ return FieldZone.TRENCH_BUMP;
+ }
+
+ // if (underLadder(drivepose)) {
+ // return FieldZone.UNDER_LADDER;
+ // }
+
+ if (x > FieldConstants.RED_ALLIANCE_LINE - (DriveConstants.ROBOT_WIDTH_WITH_BUMPERS) / 2) {
+ return (Robot.getAlliance() == Alliance.Red) ? FieldZone.ALLIANCE : FieldZone.OPPOSITION;
+ } else if (x < FieldConstants.BLUE_ALLIANCE_LINE + (DriveConstants.ROBOT_WIDTH_WITH_BUMPERS) / 2) {
+ return (Robot.getAlliance() == Alliance.Blue) ? FieldZone.ALLIANCE : FieldZone.OPPOSITION;
+ } else {
+ return FieldZone.NEUTRAL;
+ }
+ }
+
+ /**
+ * returns true if robot is currently underneath or touching the climbing ladder
+ */
+ public static boolean isUnderLadder(Translation2d drivepose) {
+ return getZone(drivepose) == FieldZone.UNDER_LADDER;
+ }
+
+ public static boolean underTrench(double x, double y) {
+ // ensures we aren't in center channel
+ if (y > Units.inchesToMeters(TRENCH_CENTER_CHANNEL_WIDTH_INCHES)
+ && y < FIELD_WIDTH - Units.inchesToMeters(TRENCH_CENTER_CHANNEL_WIDTH_INCHES)) {
+ return false;
+ }
+ // if our location is to far away from right underneath trench in terms of x
+ // in between blue alliance trench
+ if (!(x > Units.inchesToMeters(TRENCH_X_MIN_INCHES) && x < Units.inchesToMeters(TRENCH_X_MAX_INCHES))
+ && !(x < FIELD_LENGTH - Units.inchesToMeters(TRENCH_X_MIN_INCHES)
+ && x > FIELD_LENGTH - Units.inchesToMeters(TRENCH_X_MAX_INCHES))) {
+ return false;
+ }
+ return true;
+ }
+
+ /**
+ *
+ * @return Whether Y coordinate is in the upper half (left side on blue
+ * alliance)
+ */
+ public static boolean isOnLeftSideOfField(Translation2d drivepose) {
+ return drivepose.getY() > FIELD_WIDTH / 2;
+ }
+
+ public static Translation3d getOppositionTranslation(boolean sideLeft) {
+ if (sideLeft) {
+ if (Robot.getAlliance() == Alliance.Blue) {
+ return ALLIANCE_LEFT_RED;
+ } else {
+ // Reversed it so we shoot same side, but probably need to change this
+ return ALLIANCE_LEFT_BLUE;
+ }
+ } else {
+ if (Robot.getAlliance() == Alliance.Blue) {
+ return ALLIANCE_RIGHT_RED;
+ } else {
+ return ALLIANCE_RIGHT_BLUE;
+ }
+ }
+ }
+
+}
--- /dev/null
+package frc.robot.constants;
+
+/**
+ * constants for gyro bias estimation and correction via vision.
+ */
+public class GyroBiasConstants {
+ /** minimum samples before applying correction */
+ public static final int MIN_SAMPLES = 10;
+
+ /** maximum angle difference to accept in radians */
+ public static final double MAX_ANGLE_DIFF_RAD = Math.toRadians(45);
+
+ /** minimum correction to apply in radians */
+ public static final double MIN_CORRECTION_RAD = Math.toRadians(0.1);
+
+ /** fraction of the correction to apply (0.0 to 1.0) */
+ public static final double CORRECTION_FRACTION = 0.2;
+
+ /** maximum correction per cycle in radians */
+ public static final double MAX_CORRECTION_PER_CYCLE_RAD = Math.toRadians(5);
+
+ /** alpha for exponential moving average 0.0 to 1.0, higher is more responsive */
+ public static final double EMA_ALPHA = 0.3;
+
+ /** min total weight required for weighted average */
+ public static final double MIN_TOTAL_WEIGHT = 3.0;
+}
--- /dev/null
+package frc.robot.constants;
+
+public class IdConstants {
+ // Drivetrain
+ public static final int DRIVE_FRONT_LEFT_ID = 1;
+ public static final int STEER_FRONT_LEFT_ID = 2;
+ public static final int ENCODER_FRONT_LEFT_ID = 3;
+ public static final int DRIVE_FRONT_RIGHT_ID = 10;
+ public static final int STEER_FRONT_RIGHT_ID = 11;
+ public static final int ENCODER_FRONT_RIGHT_ID = 12;
+ public static final int DRIVE_BACK_LEFT_ID = 7;
+ public static final int STEER_BACK_LEFT_ID = 8;
+ public static final int ENCODER_BACK_LEFT_ID = 9;
+ public static final int DRIVE_BACK_RIGHT_ID = 4;
+ public static final int STEER_BACK_RIGHT_ID = 5;
+ public static final int ENCODER_BACK_RIGHT_ID = 6;
+ public static final int PIGEON = 13;
+
+ // LEDs
+ public static final int CANDLE_ID = 1;
+
+ // Turret
+ public static final int TURRET_MOTOR_ID = 5;
+ public static final int TURRET_ENCODER_LEFT_ID = 6;
+ public static final int TURRET_ENCODER_RIGHT_ID = 7;
+
+ // Shooter
+ public static final int SHOOTER_LEFT_ID = 9;
+ public static final int SHOOTER_RIGHT_ID = 10;
+
+ // Hood
+ public static final int HOOD_ID = 11;
+
+ // Spindexer
+ public static final int SPINDEXER_ONE_ID = 4;
+ public static final int SPINDEXER_TWO_ID = 8;
+
+ // Intake
+ public static final int RIGHT_MOTOR_ID = 1;
+ public static final int LEFT_MOTOR_ID = 2;
+ public static final int ROLLER_MOTOR_ID = 3;
+}
--- /dev/null
+package frc.robot.constants;
+
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+
+public class ShotInterpolation {
+ public static final InterpolatingDoubleTreeMap timeOfFlightMap = new InterpolatingDoubleTreeMap();
+ public static final InterpolatingDoubleTreeMap shooterPowerMap = new InterpolatingDoubleTreeMap();
+ public static final InterpolatingDoubleTreeMap hoodAngleMap = new InterpolatingDoubleTreeMap();
+
+ public static final InterpolatingDoubleTreeMap exitVelocityMap = new InterpolatingDoubleTreeMap();
+
+ public static final InterpolatingDoubleTreeMap shooterVelocityMap = new InterpolatingDoubleTreeMap();
+
+ public static final InterpolatingDoubleTreeMap newHoodMap = new InterpolatingDoubleTreeMap();
+
+ static{
+ timeOfFlightMap.put(0.0, 0.67);
+ timeOfFlightMap.put(1.0, 0.67);
+
+ shooterPowerMap.put(0.0, 1.0);
+ shooterPowerMap.put(1.0, 1.0);
+
+ //hoodAngleMap.put(HoodConstants.MAX_ANGLE, HoodConstants.MAX_ANGLE);
+ hoodAngleMap.put(81.3, 70.25);
+ hoodAngleMap.put(79.0, 65.9);
+ hoodAngleMap.put(58.5, 48.5);
+ //hoodAngleMap.put(1.0, Units.degreesToRadians(90));
+
+ exitVelocityMap.put(0.0, 0.0);
+ exitVelocityMap.put(1.0, 2.2);
+ exitVelocityMap.put(2.0, 4.4);
+ exitVelocityMap.put(7.0, 12.0);
+ exitVelocityMap.put(7.78, 16.8);
+ exitVelocityMap.put(7.8, 15.2);
+ exitVelocityMap.put(7.9, 17.1);
+ exitVelocityMap.put(8.0, 17.9);
+ exitVelocityMap.put(8.08, 19.0);
+ exitVelocityMap.put( 21.0, 19.0);
+
+ exitVelocityMap.put(9.90, 14.0);
+ exitVelocityMap.put(9.95, 16.5);
+ exitVelocityMap.put(10.0, 19.2);
+ exitVelocityMap.put(11.0, 26.0);
+ exitVelocityMap.put(25.0, 25.0* 3.2);
+
+ // currently regresses to y = 1.34959x + 9.79618
+ shooterVelocityMap.put(0.0, 9.55 * 1.05);
+ shooterVelocityMap.put(1.00, 11.5 * 1.05);
+ shooterVelocityMap.put(2.00, 12.3 * 1.1);
+ shooterVelocityMap.put(3.00, 14.0 * 1.075);
+ shooterVelocityMap.put(4.00, 15.5 * 1.075);
+ shooterVelocityMap.put(5.00, 17.0 * 1.05);
+ shooterVelocityMap.put(5.60, 18.0 * 1.05);
+ shooterVelocityMap.put(25.0, 43.44 * 1.05);
+
+
+ newHoodMap.put(0.0, 75.9);
+ newHoodMap.put(1.00, 78.0);
+ newHoodMap.put(1.49, 72.0 - 2.0 - 1.0);
+ newHoodMap.put(2.09, 70.0 - 4.0 - 2.0);
+ newHoodMap.put(2.95, 68.0 - 4.0 - 2.0);
+ newHoodMap.put(4.07, 65.0 - 3.0 - 2.0);
+ newHoodMap.put(5.05, 60.0 - 1.0 - 1.0);
+ newHoodMap.put(5.79, 59.0 - 2.0);
+ newHoodMap.put(27.99, 0.0);
+ }
+}
--- /dev/null
+package frc.robot.constants;
+
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+
+public class ShuttleInterpolation {
+ public static final InterpolatingDoubleTreeMap newHoodMap = new InterpolatingDoubleTreeMap();
+ public static final InterpolatingDoubleTreeMap shooterVelocityMap = new InterpolatingDoubleTreeMap();
+ /*
+ * guide to tuning:
+ * modify the left values in the parenthesis to change the distance of the shot.
+ * setup the robot at that distance and shoot. Then modifty the right value to
+ * be more or less until it hits target perfectly.
+ * Repeat
+ * OR
+ * Run robot, estimate distance, and tweak values when nobodies looking until it
+ * works.
+ */
+ static {
+ // we can be less aggressive: y = 0.65 * (1.34959x + 9.79618)
+ // will likely be this that requires tuning.
+ shooterVelocityMap.put(0.0, 9.0);
+ shooterVelocityMap.put(4.0, 12.0 * 1.3); // tuned by wesley
+ shooterVelocityMap.put(8.0, 22.0 * 1.075); // tuned by wesley
+ shooterVelocityMap.put(16.0, 100.0); // tuned by taren
+
+ // always shoot at low angle to ground.
+ newHoodMap.put(0.0, 55.0); // min angle (w/ 0.5 deg buffer)
+ newHoodMap.put(27.99, 55.0); // min angle (w/ 0.5 deg buffer)
+ }
+}
--- /dev/null
+package frc.robot.constants;
+
+/**
+ * Container class for test constants.
+ */
+public class TestConstants {
+ public static final double POSE_TRANSFORM_TRANSLATION_ERROR = 0.6;
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.constants;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.util.Units;
+
+/**
+ * Container class for vision constants.
+ */
+public class VisionConstants {
+ /**
+ * If April tag vision is enabled on the robot
+ */
+ public static final boolean ENABLED = true;
+
+ /**
+ * If object detection should be enabled
+ */
+ public static final boolean OBJECT_DETECTION_ENABLED = false;
+
+ /** If odometry should be updated using vision during auto */
+ public static final boolean ENABLED_AUTO = true;
+
+ /**
+ * If odometry should be updated using vision while running the GoToPose,
+ * GoToPosePID, and DriveToPose commands in teleop
+ */
+ public static final boolean ENABLED_GO_TO_POSE = true;
+
+ /** If vision should be simulated */
+ public static final boolean ENABLED_SIM = false;
+
+ /** If vision should only return values if it can see 2 good targets */
+ public static final boolean ONLY_USE_2_TAGS = false;
+
+ /** PoseStrategy to use in pose estimation */
+ public static final PoseStrategy POSE_STRATEGY = PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR;
+
+ /** Fallback PoseStrategy if MultiTag doesn't work */
+ public static final PoseStrategy MULTITAG_FALLBACK_STRATEGY = PoseStrategy.LOWEST_AMBIGUITY;
+
+ /**
+ * Any April tags we always want to ignore. To ignore a tag, put its id in this
+ * array.
+ */
+ public static final int[] TAGS_TO_IGNORE = {};
+
+ /**
+ * If multiple cameras return different poses, they will be ignored if the
+ * difference between them is greater than this
+ */
+ public static final double MAX_POSE_DIFFERENCE = 0.2;
+
+ /**
+ * The maximum distance to the tag to use
+ */
+ public static final double MAX_DISTANCE = 6;
+
+ /** If vision should use manual calculations (yawFunction-based vs referencePose-based). Changed to false to support gyro bias correction. */
+ public static final boolean USE_MANUAL_CALCULATIONS = false;
+
+ // <ol start="0"> did not work
+ /**
+ * Which version of driver assist to use. This would be an enum, except there is
+ * no short and descriptive name for all of these.
+ * <p>
+ * The options are:
+ * <p>
+ * 0: Disable driver assist
+ * <p>
+ * 1: Completely remove the component of the driver's input that is not toward
+ * the object
+ * <p>
+ * 2: Interpolate between the next achievable driver speed and a speed
+ * calculated using trapezoid profiles
+ * <p>
+ * 3-5: Add a speed perpendicular to the driver input; there are 3 similar but
+ * different calculations for this
+ */
+ public static final int DRIVER_ASSIST_MODE = 5;
+
+ /**
+ * The number to multiply the distance to the April tag by.
+ * <p>
+ * Only affects manual calculations.
+ * <p>
+ * To find this, set it to 1 and measure the actual distance and the calculated
+ * distance.
+ * <p>
+ * This should not be needed, and it is only here because it improved the
+ * accuracy of vision in the 2023 fall semester
+ */
+ public static final double DISTANCE_SCALE = 1;
+
+ /**
+ * The standard deviations to use for vision
+ */
+ public static final Matrix<N3, N1> VISION_STD_DEVS = VecBuilder.fill(
+ 0.3, // x in meters (default=0.9)
+ 0.3, // y in meters (default=0.9)
+ 0.9 // heading in radians. The gyroscope is very accurate, so as long as it is reset
+ // correctly it is unnecessary to correct it with vision
+ );
+
+ /**
+ * The standard deviations to use for vision when the wheels slip
+ */
+ public static final Matrix<N3, N1> VISION_STD_DEVS_2 = VecBuilder.fill(
+ 0.01, // x in meters (default=0.9)
+ 0.01, // y in meters (default=0.9)
+ 0.9 // heading in radians. The gyroscope is very accurate, so as long as it is reset
+ // correctly it is unnecessary to correct it with vision
+ );
+
+ /**
+ * The highest ambiguity to use. Ambiguities higher than this will be ignored.
+ * <p>
+ * Only affects calculations using PhotonVision, not manual calculations.
+ */
+ public static final double HIGHEST_AMBIGUITY = 0.05;
+
+ public static final int MAX_EMPTY_TICKS = 10;
+
+ /**
+ * The camera poses
+ * <p>
+ * Everything is in meters and radians
+ * <p>
+ * 0 for all numbers is center of the robot, on the ground, looking straight
+ * toward the front
+ * <p>
+ * + X: Front of Robot
+ * <p>
+ * + Y: Left of Robot
+ * <p>
+ * + Z: Top of Robot
+ * <p>
+ * + Pitch: Down
+ * <p>
+ * + Yaw: Counterclockwise
+ */
+ public static final ArrayList<Pair<String, Transform3d>> APRIL_TAG_CAMERAS = new ArrayList<Pair<String, Transform3d>>(
+ List.of(
+ new Pair<String, Transform3d>(
+ "CameraFrontLeft",
+ new Transform3d(
+ new Translation3d(Units.inchesToMeters(-8.47),
+ Units.inchesToMeters(11.54),
+ Units.inchesToMeters(17.7)),
+ new Rotation3d(0, Units.degreesToRadians(-22.0),
+ Units.degreesToRadians(55.0)))),
+ new Pair<String, Transform3d>(
+ "CameraFrontRight",
+ new Transform3d(
+ new Translation3d(Units.inchesToMeters(-8.47),
+ Units.inchesToMeters(-11.54),
+ Units.inchesToMeters(17.7)),
+ new Rotation3d(0, Units.degreesToRadians(-22.0),
+ Units.degreesToRadians(-55.0)))),
+ new Pair<String, Transform3d>(
+ "CameraBackLeft",
+ new Transform3d(
+ new Translation3d(Units.inchesToMeters(-10.91),
+ Units.inchesToMeters(12),
+ Units.inchesToMeters(17.66)),
+ new Rotation3d(0, Units.degreesToRadians(-22.0),
+ Units.degreesToRadians(145.0)))),
+ new Pair<String, Transform3d>(
+ "CameraBackRight",
+ new Transform3d(
+ new Translation3d(Units.inchesToMeters(-10.91),
+ Units.inchesToMeters(-12),
+ Units.inchesToMeters(17.66)),
+ new Rotation3d(0, Units.degreesToRadians(-22.0),
+ Units.degreesToRadians(-145.0))))
+ ));
+
+ /**
+ * The transformations from the robot to object detection cameras
+ */
+ public static final ArrayList<Transform3d> OBJECT_DETECTION_CAMERAS = new ArrayList<>(List.of(
+ new Transform3d(
+ new Translation3d(Units.inchesToMeters(10), 0, Units.inchesToMeters(24)),
+ new Rotation3d(0, Units.degreesToRadians(20), 0))));
+
+ // used to cleanly shutdown the OrangePi
+ public static final String[] ORANGEPI_HOSTNAMES = { "photonfront.local", "photonback.local" };
+ public static final String ORANGEPI_USERNAME = "pi";
+ public static final String ORANGEPI_PASSWORD = "raspberry";
+}
--- /dev/null
+package frc.robot.constants.swerve;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.util.Units;
+import frc.robot.RobotId;
+import frc.robot.constants.Constants;
+import frc.robot.util.SwerveStuff.ModuleLimits;
+import lib.COTSFalconSwerveConstants;
+
+/**
+ * Global constants are, by default, for the competition robot.
+ * Global constants get changed in the update method if the RobotId detected is not the competition robot.
+ */
+public class DriveConstants {
+ /**
+ * The robot's width with its bumpers on.
+ * <p>
+ * The frame width is 26.5 inches, and each bumper is 3.25 inches.
+ */
+ public static final double ROBOT_WIDTH_WITH_BUMPERS = 0.83185; // 32.75 inches in meters
+
+ public static double ROBOT_MASS = Units.lbsToKilograms(111.6 + 13 + 13.4 + 5.0);
+
+ /** Radius of the drive wheels [meters]. */
+ public static final double WHEEL_RADIUS = Units.inchesToMeters(1.95);
+
+ public static double WHEEL_MOI = 0.000326 * ROBOT_MASS;
+
+
+ /** Distance between the left and right wheels [meters]. */
+ // from center of wheels btw
+ public static double TRACK_WIDTH = Units.inchesToMeters(20.75);//22.75 swerve bot, 20.75 comp bot
+
+ // Mk4i gear ratios
+ // https://www.swervedrivespecialties.com/products/mk4i-swerve-module
+ // standard gear ratios
+ // https://www.swervedrivespecialties.com/products/kit-adapter-16t-drive-pinion-gear-mk4i
+ // changes 14-tooth pinion to 16-tooth pinion -- (50.0 / 14.0) becomes (50.0 / 16.0).
+ /** Drive gear ratio for an Mk4i with L2-Plus gearing */
+ public static double DRIVE_GEAR_RATIO = (50.0 / 16.0) * (17.0 / 27.0) * (45.0 / 15.0);
+ // all MK4i modules have the same steering gear ratio
+ public static double STEER_GEAR_RATIO = 150.0 / 7.0;
+
+ /** Theoretical maximum speed of the robot based on maximum motor RPM, gear ratio, and wheel radius */
+ public static final double MAX_SPEED = 4.5;
+
+ // Need to convert tangential velocity (the m/s of the edge of the robot) to angular velocity (the radians/s of the robot)
+ // To do so, divide by the radius. The radius is the diagonal of the square chassis, diagonal = sqrt(2) * side_length.
+ public static final double MAX_ANGULAR_SPEED = MAX_SPEED / ((TRACK_WIDTH / 2) * Math.sqrt(2));
+
+ public static final double COSF = 1.5;
+
+ // The maximum acceleration of the robot, limited by friction
+ public static final double MAX_LINEAR_ACCEL = COSF * Constants.GRAVITY_ACCELERATION;
+ // The maximum amount a drive motor can accelerate, independant of friction
+ // This does nothing if greater than LINEAR_ACCEL
+ public static final double MAX_DRIVE_ACCEL = MAX_LINEAR_ACCEL;
+ // The maximum angular acceleration of the robot
+ public static final double MAX_ANGULAR_ACCEL = MAX_LINEAR_ACCEL / TRACK_WIDTH * Math.sqrt(2);
+
+ /**
+ * If this is false, Drivetrain will use the previous setpoint to calculate the new setpoint.
+ * <p> If this is true, Drivetrain will use the actual current setpoint instead.
+ */
+ public static final boolean USE_ACTUAL_SPEED = false;
+
+ /**
+ * Disables the deadband and optimization for the modules.
+ * SwerveSetpointGenerator adds its own optimization and deadband, and the controllers also have a deadband.
+ * Setting this to true fixes bugs caused by using hte actual current state.
+ */
+ public static final boolean DISABLE_DEADBAND_AND_OPTIMIZATION = false;
+
+ public static final Rotation2d STARTING_HEADING = new Rotation2d();
+
+ public static final Translation2d[] MODULE_LOCATIONS = {
+ new Translation2d(DriveConstants.TRACK_WIDTH / 2, DriveConstants.TRACK_WIDTH / 2),
+ new Translation2d(DriveConstants.TRACK_WIDTH / 2, -DriveConstants.TRACK_WIDTH / 2),
+ new Translation2d(-DriveConstants.TRACK_WIDTH / 2, DriveConstants.TRACK_WIDTH / 2),
+ new Translation2d(-DriveConstants.TRACK_WIDTH / 2, -DriveConstants.TRACK_WIDTH / 2)
+ };
+
+ public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics(MODULE_LOCATIONS);
+
+ /**
+ * Default values for SwerveCompetition drivetrain
+ * Sets to correct value later if robotID is different
+ */
+ public static double STEER_OFFSET_FRONT_LEFT = 302.646;
+ public static double STEER_OFFSET_FRONT_RIGHT = 103.039+180;
+ public static double STEER_OFFSET_BACK_LEFT = 165.49+90;
+ public static double STEER_OFFSET_BACK_RIGHT = 73.563;
+
+ // Heading PID.
+ public static final double HEADING_P = 5.5;
+ public static final double HEADING_D = 0;
+
+ public static final double HEADING_TOLERANCE = Units.degreesToRadians(1.5);
+
+ // Translational PID
+ // TODO: Tune this better (low priority since we aren't using it in 2025)
+ public static final double TRANSLATIONAL_P = 1;
+ public static final double TRANSLATIONAL_D = 0.001;
+
+ //The PIDs for PathPlanner Command
+ public static final double PATH_PLANNER_HEADING_P = 3.5/2;
+ public static final double PATH_PLANNER_HEADING_D = 0;
+
+ public static final double PATH_PLANNER_TRANSLATIONAL_P = 6/2;
+ public static final double PATH_PLANNER_TRANSLATIONAL_D = 0;
+
+ // CAN
+ public static CANBus DRIVE_MOTOR_CAN = Constants.CANIVORE_CAN;
+ public static CANBus STEER_MOTOR_CAN = Constants.CANIVORE_CAN;
+ public static CANBus STEER_ENCODER_CAN = Constants.CANIVORE_CAN;
+ public static CANBus PIGEON_CAN = Constants.CANIVORE_CAN;
+
+
+ public static COTSFalconSwerveConstants MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+ /* Swerve Current Limiting */
+ public static final int STEER_CONTINUOUS_CURRENT_LIMIT = 15;
+ public static final int STEER_PEAK_CURRENT_LIMIT = 15;
+ public static final double STEER_PEAK_CURRENT_DURATION = 0.01;
+ public static final boolean STEER_ENABLE_CURRENT_LIMIT = true;
+
+ public static final int DRIVE_CONTINUOUS_CURRENT_LIMIT = 40;
+ public static final int DRIVE_PEAK_CURRENT_LIMIT = 40;
+ public static final double DRIVE_PEAK_CURRENT_DURATION = 0.01;
+ public static final boolean DRIVE_ENABLE_CURRENT_LIMIT = true;
+
+ /* Motor inversions */
+ public static final InvertedValue INVERT_DRIVE_MOTOR = InvertedValue.CounterClockwise_Positive;
+ public static InvertedValue INVERT_STEER_MOTOR = InvertedValue.Clockwise_Positive;
+
+ /* Neutral Modes */
+ public static final NeutralModeValue DRIVE_NEUTRAL_MODE = NeutralModeValue.Brake;
+ public static final NeutralModeValue STEER_NEUTRAL_MODE = NeutralModeValue.Brake;
+
+ /* Gyro mount pose roll in deg (180.0 if placed under the robot) */
+ public static double GYRO_MOUNT_POSE_ROLL = 0.0;
+
+ /* Drive Motor PID Values */
+ public static final double[] P_VALUES = {
+ 0.3,
+ 0.3,
+ 0.3,
+ 0.3
+ };
+ public static final double[] I_VALUES = {
+ 0,
+ 0,
+ 0,
+ 0
+ };
+ public static final double[] D_VALUES = {
+ 0,
+ 0,
+ 0,
+ 0
+ };
+ /* Drive Motor Characterization Values
+ * Divide SYSID values by 12 to convert from volts to percent output for CTRE */
+ public static final double[] S_VALUES = {
+ 0.11,
+ 0.11,
+ 0.11,
+ 0.11
+ };
+ public static final double[] V_VALUES = {
+ 0.11079,
+ 0.10718,
+ 0.11009,
+ 0.1164
+ };
+ public static final double[] A_VALUES = {
+ 0.005482,
+ 0.0049593,
+ 0.010156,
+ 0.0065708
+ };
+ /* Ramp values for drive motors in open loop driving. */
+ // Open loop prevents throttle from changing too quickly.
+ // It will limit it to time given (in seconds) to go from zero to full throttle.
+ // A small open loop ramp (0.25) helps with tread wear, tipping, etc
+ public static final double OPEN_LOOP_RAMP = 0.1;
+
+ // limits maximum rate of change for motor
+ public static final double CLOSE_LOOP_RAMP = 0.0;
+
+ public static final double WHEEL_CIRCUMFERENCE = 2*Math.PI*WHEEL_RADIUS;
+
+ public static final boolean INVERT_GYRO = false; // Make sure gyro is CCW+ CW-
+
+ public static final double SLOW_DRIVE_FACTOR = 0.2;
+ public static final double SLOW_ROT_FACTOR = 0.1;
+
+ public static final ModuleLimits MODULE_LIMITS = new ModuleLimits(MAX_SPEED, MAX_DRIVE_ACCEL, COSF, Units.rotationsPerMinuteToRadiansPerSecond(Constants.MAX_RPM / STEER_GEAR_RATIO));
+
+ /**
+ * Updates the constants if the RobotId is not the default SwerveCompetition robot.
+ */
+ public static void update(RobotId robotId) {
+ if (robotId == RobotId.PrimeJr) {
+ STEER_OFFSET_FRONT_LEFT = 187.64+180; // module zero
+ STEER_OFFSET_FRONT_RIGHT = 162+180+180; // module one
+ STEER_OFFSET_BACK_LEFT = 196.3+180; // module two
+ STEER_OFFSET_BACK_RIGHT = 357+180+180; // module three
+
+ // MK5n
+ INVERT_STEER_MOTOR = InvertedValue.CounterClockwise_Positive;
+
+ // Gear ratios
+ //DRIVE_GEAR_RATIO = (54.0 / 14.0) * (25.0 / 32.0) * (30.0 / 15.0); //R2 Ratio
+ DRIVE_GEAR_RATIO = (54.0 / 12.0) * (25.0 / 32.0) * (30.0 / 15.0); //R1 Ratio
+ STEER_GEAR_RATIO = 287.0 / 11.0;
+
+ // Gyro is mounted under the robot
+ GYRO_MOUNT_POSE_ROLL = 180.0;
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK5n(DRIVE_GEAR_RATIO);
+
+ } else if(robotId == RobotId.TwinBot){
+ STEER_OFFSET_FRONT_LEFT = 131.201172;
+ STEER_OFFSET_FRONT_RIGHT = 247.324219;
+ STEER_OFFSET_BACK_LEFT = 39.814463;
+ STEER_OFFSET_BACK_RIGHT = 294.873047;
+
+ // MK5n gear ratio
+ INVERT_STEER_MOTOR = InvertedValue.CounterClockwise_Positive;
+
+ DRIVE_GEAR_RATIO = (54.0 / 14.0) * (25.0 / 32.0) * (30.0 / 15.0);
+ STEER_GEAR_RATIO = 287.0 / 11.0;
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK5n(DRIVE_GEAR_RATIO);
+
+ } else if(robotId == RobotId.SwerveCompetition){
+ STEER_OFFSET_FRONT_LEFT = 302.646;
+ STEER_OFFSET_FRONT_RIGHT = 103.039+180;
+ STEER_OFFSET_BACK_LEFT = 165.49+90;
+ STEER_OFFSET_BACK_RIGHT = 73.563;
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+ } else if(robotId == RobotId.BetaBot) {
+ STEER_OFFSET_FRONT_LEFT = 193.884-180;
+ STEER_OFFSET_FRONT_RIGHT = 110.914;
+ STEER_OFFSET_BACK_LEFT = 128.054+180;
+ STEER_OFFSET_BACK_RIGHT = 107.43;
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+ } else if (robotId == RobotId.Vivace) {
+ STEER_OFFSET_FRONT_LEFT = 100.184+180;
+ STEER_OFFSET_FRONT_RIGHT = 224.293;
+ STEER_OFFSET_BACK_LEFT = 304.795-180;
+ STEER_OFFSET_BACK_RIGHT = 201.177-180;
+
+ ROBOT_MASS = 50;
+ WHEEL_MOI = 0.000326 * ROBOT_MASS;
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+ } else if (robotId == RobotId.Vertigo) {
+ STEER_OFFSET_FRONT_LEFT = Units.radiansToDegrees(3.43);
+ STEER_OFFSET_FRONT_RIGHT = Units.radiansToDegrees(1.91) + 180;
+ STEER_OFFSET_BACK_LEFT = Units.radiansToDegrees(2.28);
+ STEER_OFFSET_BACK_RIGHT = Units.radiansToDegrees(5.03);
+
+ DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
+
+ ROBOT_MASS = 20;
+
+ WHEEL_MOI = 0.000326 * ROBOT_MASS;
+
+ // Falcon Speed
+ Constants.MAX_RPM = 6080.0;
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+ } else if (robotId == RobotId.Phil) {
+ ROBOT_MASS = 30;
+ WHEEL_MOI = 0.000326 * ROBOT_MASS;
+
+ STEER_OFFSET_FRONT_LEFT = 121.463+180;
+ STEER_OFFSET_FRONT_RIGHT = 284.242;
+ STEER_OFFSET_BACK_LEFT = 157.676;
+ STEER_OFFSET_BACK_RIGHT = 77.199;
+
+ DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+ } else{
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+ }
+ }
+}
--- /dev/null
+package frc.robot.constants.swerve;
+
+import frc.robot.constants.IdConstants;
+
+/**
+ * Container class for module constants, defined using constants from {@link DriveConstants}
+ * .
+ *
+ * @see DriveConstants
+ */
+public enum ModuleConstants {
+
+ FRONT_LEFT(
+ IdConstants.DRIVE_FRONT_LEFT_ID,
+ IdConstants.STEER_FRONT_LEFT_ID,
+ IdConstants.ENCODER_FRONT_LEFT_ID,
+ DriveConstants.STEER_OFFSET_FRONT_LEFT,
+ ModuleType.FRONT_LEFT,
+ DriveConstants.S_VALUES[0],
+ DriveConstants.V_VALUES[0],
+ DriveConstants.A_VALUES[0],
+ DriveConstants.P_VALUES[0],
+ DriveConstants.I_VALUES[0],
+ DriveConstants.D_VALUES[0]
+ ),
+ FRONT_RIGHT(
+ IdConstants.DRIVE_FRONT_RIGHT_ID,
+ IdConstants.STEER_FRONT_RIGHT_ID,
+ IdConstants.ENCODER_FRONT_RIGHT_ID,
+ DriveConstants.STEER_OFFSET_FRONT_RIGHT,
+ ModuleType.FRONT_RIGHT,
+ DriveConstants.S_VALUES[1],
+ DriveConstants.V_VALUES[1],
+ DriveConstants.A_VALUES[1],
+ DriveConstants.P_VALUES[1],
+ DriveConstants.I_VALUES[1],
+ DriveConstants.D_VALUES[1]
+ ),
+ BACK_LEFT(
+ IdConstants.DRIVE_BACK_LEFT_ID,
+ IdConstants.STEER_BACK_LEFT_ID,
+ IdConstants.ENCODER_BACK_LEFT_ID,
+ DriveConstants.STEER_OFFSET_BACK_LEFT,
+ ModuleType.BACK_LEFT,
+ DriveConstants.S_VALUES[2],
+ DriveConstants.V_VALUES[2],
+ DriveConstants.A_VALUES[2],
+ DriveConstants.P_VALUES[2],
+ DriveConstants.I_VALUES[2],
+ DriveConstants.D_VALUES[2]
+ ),
+ BACK_RIGHT(
+ IdConstants.DRIVE_BACK_RIGHT_ID,
+ IdConstants.STEER_BACK_RIGHT_ID,
+ IdConstants.ENCODER_BACK_RIGHT_ID,
+ DriveConstants.STEER_OFFSET_BACK_RIGHT,
+ ModuleType.BACK_RIGHT,
+ DriveConstants.S_VALUES[3],
+ DriveConstants.V_VALUES[3],
+ DriveConstants.A_VALUES[3],
+ DriveConstants.P_VALUES[3],
+ DriveConstants.I_VALUES[3],
+ DriveConstants.D_VALUES[3]
+ ),
+
+ NONE(0, 0, 0, 0.0, ModuleType.NONE,0,0,0,0,0,0);
+
+ private final int drivePort;
+ private final int steerPort;
+ private final int encoderPort;
+ private final double steerOffset;
+ private final double ks;
+ private final double kv;
+ private final double ka;
+ private final double driveP;
+ private final double driveI;
+ private final double driveD;
+ private final ModuleType type;
+
+ ModuleConstants(
+ int drivePort,
+ int steerPort,
+ int encoderPort,
+ double steerOffset,
+ ModuleType type,
+ double ks,
+ double kv,
+ double ka,
+ double driveP,
+ double driveI,
+ double driveD
+
+ ) {
+ this.drivePort = drivePort;
+ this.steerPort = steerPort;
+ this.encoderPort = encoderPort;
+ this.steerOffset = steerOffset;
+ this.type = type;
+ this.ks =ks;
+ this.kv= kv;
+ this.ka = ka;
+ this.driveP =driveP;
+ this.driveI = driveI;
+ this.driveD = driveD;
+ }
+
+ public int getDrivePort() {
+ return drivePort;
+ }
+
+ public int getSteerPort() {
+ return steerPort;
+ }
+
+ public int getEncoderPort() {
+ return encoderPort;
+ }
+
+ public double getSteerOffset() {
+ return steerOffset;
+ }
+
+ public ModuleType getType() {
+ return type;
+ }
+ public double getDriveS(){
+ return ks;
+ }
+ public double getDriveV(){
+ return kv;
+ }
+ public double getDriveA(){
+ return ka;
+ }
+ public double getDriveP(){
+ return driveP;
+ }
+ public double getDriveI(){
+ return driveI;
+ }
+ public double getDriveD(){
+ return driveD;
+ }
+
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.constants.swerve;
+
+/**
+ * Represents the type for a module on the robot.
+ * <p/>
+ * IDs:
+ * 0 - FRONT_LEFT
+ * 1 - FRONT_RIGHT
+ * 2 - BACK_LEFT
+ * 3 - BACK_RIGHT
+ */
+public enum ModuleType {
+ FRONT_LEFT,
+ FRONT_RIGHT,
+ BACK_LEFT,
+ BACK_RIGHT,
+ NONE;
+
+ public final byte id;
+
+ ModuleType() {
+ this.id = id();
+ }
+
+ private byte id() {
+ if (this == NONE)
+ return -1;
+ // This is a trick that relies on the order the enums are defined.
+ return (byte) this.ordinal();
+ }
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.controls;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.RobotController;
+import frc.robot.constants.Constants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.DynamicSlewRateLimiter;
+import frc.robot.util.MathUtils;
+
+/**
+ * Abstract class for different controller types.
+ */
+public abstract class BaseDriverConfig {
+
+ protected final Drivetrain drive;
+
+ private double previousHeading = 0;
+
+ private final DynamicSlewRateLimiter headingLimiter = new DynamicSlewRateLimiter(Constants.HEADING_SLEWRATE);
+
+ /**
+ * @param drive the drivetrain instance
+ * @param controllerTab the shuffleboard controller tab
+ * @param shuffleboardUpdates whether to update the shuffleboard
+ */
+ public BaseDriverConfig(Drivetrain drive) {
+ headingLimiter.setContinuousLimits(-Math.PI, Math.PI);
+ headingLimiter.enableContinuous(true);
+ this.drive = drive;
+ }
+
+ public double getForwardTranslation() {
+ double forward = getRawForwardTranslation();
+ return forward * DriveConstants.MAX_SPEED * Math.min(1,RobotController.getBatteryVoltage()/12) * MathUtil.applyDeadband(Math.sqrt(forward*forward + Math.pow(getRawSideTranslation(), 2)), Constants.TRANSLATIONAL_DEADBAND);
+ }
+
+ public double getSideTranslation() {
+ double side = getRawSideTranslation();
+ return side * DriveConstants.MAX_SPEED * Math.min(1,RobotController.getBatteryVoltage()/12) * MathUtil.applyDeadband(Math.sqrt(side*side + Math.pow(getRawForwardTranslation(), 2)), Constants.TRANSLATIONAL_DEADBAND);
+ }
+
+ public double getRotation() {
+ return MathUtils.expoMS(MathUtil.applyDeadband(getRawRotation(), Constants.ROTATION_DEADBAND), 2)
+ * DriveConstants.MAX_ANGULAR_SPEED * Math.min(1, RobotController.getBatteryVoltage()/12);
+ }
+
+ public double getHeading() {
+ if (getRawHeadingMagnitude() <= Constants.HEADING_DEADBAND)
+ return headingLimiter.calculate(previousHeading, 1e-6);
+ previousHeading = headingLimiter.calculate(getRawHeadingAngle(),
+ MathUtils.expoMS(getRawHeadingMagnitude(), 2));
+ return previousHeading;
+ }
+
+ protected Drivetrain getDrivetrain() {
+ return drive;
+ }
+
+ /**
+ * Configures the controls for the controller.
+ */
+ public abstract void configureControls();
+
+ public abstract double getRawSideTranslation();
+
+ public abstract double getRawForwardTranslation();
+
+ public abstract double getRawRotation();
+
+ public abstract double getRawHeadingAngle();
+
+ public abstract double getRawHeadingMagnitude();
+
+ public abstract boolean getIsSlowMode();
+
+ public abstract boolean getIsAlign();
+
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.controls;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.commands.drive_comm.SetFormationX;
+import frc.robot.constants.Constants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import lib.controllers.Ex3DProController;
+import lib.controllers.Ex3DProController.Ex3DProAxis;
+import lib.controllers.Ex3DProController.Ex3DProButton;
+
+/**
+ * Driver controls for the Ex3D Pro controller.
+ */
+public class Ex3DProDriverConfig extends BaseDriverConfig {
+
+ private final Ex3DProController kDriver = new Ex3DProController(Constants.DRIVER_JOY);
+ private final BooleanSupplier slowModeSupplier = kDriver.get(Ex3DProButton.B11);
+
+ public Ex3DProDriverConfig(Drivetrain drive) {
+ super(drive);
+ }
+
+ @Override
+ public void configureControls() {
+ kDriver.get(Ex3DProButton.B1).whileTrue(new SetFormationX(super.getDrivetrain()));
+ kDriver.get(Ex3DProButton.B2).onTrue(new InstantCommand(() -> super.getDrivetrain().setYaw(DriveConstants.STARTING_HEADING)));
+ }
+
+ @Override
+ public double getRawSideTranslation() {
+ return -kDriver.get(Ex3DProAxis.X);
+ }
+
+ @Override
+ public double getRawForwardTranslation() {
+ return -kDriver.get(Ex3DProAxis.Y);
+ }
+
+ @Override
+ public double getRawRotation() {
+ return kDriver.get(Ex3DProAxis.Z);
+ }
+
+ @Override
+ public double getRawHeadingAngle() {
+ return kDriver.get(Ex3DProAxis.Z) * Math.PI;
+ }
+
+ @Override
+ public double getRawHeadingMagnitude() {
+ return kDriver.get(Ex3DProAxis.SLIDER);
+ }
+
+ @Override
+ public boolean getIsSlowMode() {
+ return slowModeSupplier.getAsBoolean();
+ }
+
+ @Override
+ public boolean getIsAlign() {
+ return false;
+ }
+}
--- /dev/null
+package frc.robot.controls;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.Robot;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import lib.controllers.GameController;
+import lib.controllers.GameController.Axis;
+import lib.controllers.GameController.Button;
+
+/**
+ * Driver controls for the generic game controller.
+ */
+public class GameControllerDriverConfig extends BaseDriverConfig {
+ private final GameController driver = new GameController(Constants.DRIVER_JOY);
+ private final BooleanSupplier slowModeSupplier = driver.get(Button.RIGHT_JOY);
+
+ public GameControllerDriverConfig(Drivetrain drive) {
+ super(drive);
+ }
+
+ @Override
+ public void configureControls() {
+ // Reset yaw to be away from driver
+ driver.get(Button.START).onTrue(new InstantCommand(() -> super.getDrivetrain().setYaw(
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
+
+ // Cancel commands
+ driver.get(driver.RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> {
+ getDrivetrain().setIsAlign(false);
+ getDrivetrain().setDesiredPose(() -> null);
+ CommandScheduler.getInstance().cancelAll();
+ }));
+ }
+
+ @Override
+ public double getRawForwardTranslation() {
+ return driver.get(Axis.LEFT_Y);
+ }
+
+ @Override
+ public double getRawSideTranslation() {
+ return driver.get(Axis.LEFT_X);
+ }
+
+ @Override
+ public double getRawRotation() {
+ return driver.get(Axis.RIGHT_X);
+ }
+
+ @Override
+ public double getRawHeadingAngle() {
+ return Math.atan2(driver.get(Axis.RIGHT_X), -driver.get(Axis.RIGHT_Y)) - Math.PI / 2;
+ }
+
+ @Override
+ public double getRawHeadingMagnitude() {
+ return Math.hypot(driver.get(Axis.RIGHT_X), driver.get(Axis.RIGHT_Y));
+ }
+
+ @Override
+ public boolean getIsSlowMode() {
+ return slowModeSupplier.getAsBoolean();
+ }
+
+ @Override
+ public boolean getIsAlign() {
+ return false;
+ // return kDriver.LEFT_TRIGGER_BUTTON.getAsBoolean();
+ }
+
+ public GameController getGameController() {
+ return driver;
+ }
+}
--- /dev/null
+package frc.robot.controls;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.commands.drive_comm.SetFormationX;
+import frc.robot.constants.Constants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import lib.controllers.MadCatzController;
+import lib.controllers.MadCatzController.MadCatzAxis;
+import lib.controllers.MadCatzController.MadCatzButton;
+
+/**
+ * Driver controls for the MadCatz controller.
+ */
+public class MadCatzDriverConfig extends BaseDriverConfig {
+
+ private final MadCatzController kDriver = new MadCatzController(Constants.DRIVER_JOY);
+ private final BooleanSupplier slowModeSupplier = kDriver.get(MadCatzButton.B6);
+
+ public MadCatzDriverConfig(Drivetrain drive) {
+ super(drive);
+ }
+
+ @Override
+ public void configureControls() {
+ kDriver.get(MadCatzButton.B1).whileTrue(new SetFormationX(super.getDrivetrain()));
+ kDriver.get(MadCatzButton.B2).onTrue(new InstantCommand(() -> super.getDrivetrain().setYaw(DriveConstants.STARTING_HEADING)));
+ }
+
+ @Override
+ public double getRawSideTranslation() {
+ return kDriver.get(MadCatzAxis.X);
+ }
+
+ @Override
+ public double getRawForwardTranslation() {
+ return -kDriver.get(MadCatzAxis.Y);
+ }
+
+ @Override
+ public double getRawRotation() {
+ return kDriver.get(MadCatzAxis.ZROTATE);
+ }
+
+ @Override
+ public double getRawHeadingAngle() {
+ return kDriver.get(MadCatzAxis.ZROTATE) * Math.PI;
+ }
+
+ @Override
+ public double getRawHeadingMagnitude() {
+ return kDriver.get(MadCatzAxis.SLIDER);
+ }
+
+ @Override
+ public boolean getIsSlowMode() {
+ return slowModeSupplier.getAsBoolean();
+ }
+
+ @Override
+ public boolean getIsAlign() {
+ return false;
+ }
+
+}
\ No newline at end of file
--- /dev/null
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.controls;
+
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import lib.controllers.GameController;
+
+/**
+ * Controls for the operator, which are almost a duplicate of most of the driver's controls
+ */
+public class Operator {
+
+ private final GameController driver = new GameController(Constants.OPERATOR_JOY);
+
+ private final Drivetrain drive;
+
+ public Operator(Drivetrain drive) {
+ this.drive = drive;
+ }
+
+ public void configureControls() {
+ // Cancel commands, could be removed if the operator doesn't need this button
+ driver.get(driver.RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> {
+ drive.setIsAlign(false);
+ drive.setDesiredPose(() -> null);
+ CommandScheduler.getInstance().cancelAll();
+ }));
+ }
+
+
+ public Trigger getRightTrigger(){
+ return new Trigger(driver.RIGHT_TRIGGER_BUTTON);
+ }
+ public Trigger getLeftTrigger(){
+ return new Trigger(driver.LEFT_TRIGGER_BUTTON);
+ }
+ public GameController getGameController(){
+ return driver;
+ }
+}
--- /dev/null
+package frc.robot.controls;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.Robot;
+import frc.robot.commands.drive_comm.SysIDDriveCommand;
+import frc.robot.commands.gpm.IntakeMovementCommand;
+import frc.robot.commands.gpm.ReverseMotors;
+import frc.robot.commands.gpm.RunSpindexer;
+import frc.robot.commands.gpm.Superstructure;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.Intake.Intake;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.spindexer.Spindexer;
+import frc.robot.subsystems.turret.Turret;
+import lib.controllers.PS5Controller;
+import lib.controllers.PS5Controller.DPad;
+import lib.controllers.PS5Controller.PS5Axis;
+import lib.controllers.PS5Controller.PS5Button;
+
+/**
+ * Driver controls for the PS5 controller
+ */
+public class PS5ControllerDriverConfig extends BaseDriverConfig {
+ private final PS5Controller controller = new PS5Controller(Constants.DRIVER_JOY);
+ private final BooleanSupplier slowModeSupplier = () -> false;
+ private boolean intakeBoolean = true;
+ private Command autoShoot = null;
+ private Shooter shooter;
+ private Turret turret;
+ private Hood hood;
+ private Intake intake;
+ private Spindexer spindexer;
+
+ public PS5ControllerDriverConfig(
+ Drivetrain drive,
+ Shooter shooter,
+ Turret turret,
+ Hood hood,
+ Intake intake,
+ Spindexer spindexer) {
+ super(drive);
+ this.shooter = shooter;
+ this.turret = turret;
+ this.hood = hood;
+ this.intake = intake;
+ this.spindexer = spindexer;
+ }
+
+ public void configureControls() {
+ // Reset the yaw. Mainly useful for testing/driver practice
+ controller.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
+
+ // Cancel commands
+ controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> {
+ getDrivetrain().setIsAlign(false);
+ getDrivetrain().setDesiredPose(() -> null);
+ CommandScheduler.getInstance().cancelAll();
+ }));
+
+ // Reverse motors
+ if (intake != null && spindexer != null) {
+ controller.get(PS5Button.LB).whileTrue(new ReverseMotors(intake));
+ }
+
+ // Intake
+ if (intake != null) {
+ // Toggle intake
+ controller.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
+ if (intakeBoolean) {
+ intake.extend();
+ intake.spinStart();
+ intakeBoolean = false;
+ } else {
+ intake.intermediateExtend();
+ intake.spinStop();
+ intakeBoolean = true;
+ }
+ }, intake));
+
+ // Retract if hold for 2 seconds
+ controller.get(PS5Button.RIGHT_TRIGGER).debounce(2.0).onTrue(new InstantCommand(() -> {
+ intake.retract();
+ intakeBoolean = true;
+ intake.spinStop();
+ }, intake));
+
+ // Make the intake go in and out while shooting
+ controller.get(DPad.UP).whileTrue(new IntakeMovementCommand(intake)
+ .alongWith(new InstantCommand(()-> intakeBoolean = true)));
+
+ // Calibration: you can now calibrate easily using this button
+ if (hood != null && intake != null) {
+ controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
+ intake.calibrate();
+ hood.calibrate();
+ }, intake, hood)).onFalse(new InstantCommand(() -> {
+ intake.stopCalibrating();
+ hood.stopCalibrating();
+ }, intake, hood));
+ }
+
+ // Stop intake roller
+ controller.get(DPad.DOWN).onTrue(new InstantCommand(()->{
+ if(intakeBoolean){
+ intake.spinStart();
+ intakeBoolean = false;
+ } else{
+ intake.spinStop();
+ intakeBoolean = true;
+ }
+ }));
+ }
+
+ // Spindexer
+ if (spindexer != null && turret != null && hood != null && intake != null) {
+
+ // Toggle spindexer
+ controller.get(PS5Button.LEFT_TRIGGER).toggleOnTrue(
+ new RunSpindexer(spindexer, turret, hood, intake)
+ );
+ }
+
+ // Auto shoot
+ if (turret != null && hood != null && shooter != null && spindexer != null) {
+ autoShoot = new Superstructure(turret, getDrivetrain(), hood, shooter, spindexer);
+ controller.get(PS5Button.SQUARE).toggleOnTrue(autoShoot);
+ }
+
+
+ // Hood
+ if (hood != null) {
+ // Set the hood down -- for safety measures under trench
+ controller.get(DPad.LEFT).onTrue(new InstantCommand(()->{
+ hood.forceHoodDown(true);
+ }, hood)).onFalse(new InstantCommand(()->{
+ hood.forceHoodDown(false);
+ }));
+ }
+ }
+
+ @Override
+ public double getRawSideTranslation() {
+ return controller.get(PS5Axis.LEFT_X);
+ }
+
+ @Override
+ public double getRawForwardTranslation() {
+ return controller.get(PS5Axis.LEFT_Y);
+ }
+
+ @Override
+ public double getRawRotation() {
+ return controller.get(PS5Axis.RIGHT_X);
+ }
+
+ @Override
+ public double getRawHeadingAngle() {
+ return Math.atan2(controller.get(PS5Axis.RIGHT_X), -controller.get(PS5Axis.RIGHT_Y)) - Math.PI / 2;
+ }
+
+ @Override
+ public double getRawHeadingMagnitude() {
+ return Math.hypot(controller.get(PS5Axis.RIGHT_X), controller.get(PS5Axis.RIGHT_Y));
+ }
+
+ @Override
+ public boolean getIsSlowMode() {
+ return slowModeSupplier.getAsBoolean();
+ }
+
+ @Override
+ public boolean getIsAlign() {
+ return false;
+ }
+}
--- /dev/null
+package frc.robot.controls;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.Robot;
+import frc.robot.commands.gpm.ReverseMotors;
+import frc.robot.commands.gpm.Superstructure;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.Intake.Intake;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.spindexer.Spindexer;
+import frc.robot.subsystems.turret.Turret;
+import lib.controllers.GameController;
+import lib.controllers.GameController.Axis;
+import lib.controllers.GameController.Button;
+import lib.controllers.GameController.DPad;
+
+/**
+ * Driver config for PS5 controllers using Xbox 360 emulation mode.
+ * This lets SCUF and other PS5 controllers work with WPILib rumble.
+ *
+ * Setup:
+ * - download DSX (https://dualsensex.com/download/)
+ * - install ViGEmBus driver (if app doesn't auto prompt)
+ * - in dsx, set "controller emulation" to Xbox 360
+ * - ensure rumble is enabled in dsx settings
+ * - once code is depoloyed, change controller to "Xbox 360" in driverstation
+ */
+
+public class PS5XboxModeDriverConfig extends BaseDriverConfig {
+ private final GameController controller = new GameController(Constants.DRIVER_JOY);
+ private final BooleanSupplier slowModeSupplier = () -> false;
+ private boolean intakeBoolean = true;
+ private Command autoShoot = null;
+ private Command reverseMotors = null;
+ private Shooter shooter;
+ private Turret turret;
+ private Hood hood;
+ private Intake intake;
+ private Spindexer spindexer;
+
+ // PS5 button aliases
+ // private final Button CROSS = Button.A;
+ private final Button CIRCLE = Button.B;
+ private final Button SQUARE = Button.X;
+ // private final Button TRIANGLE = Button.Y;
+ // private final Button LB = Button.LB;
+ private final Button RB = Button.RB;
+ private final Button CREATE = Button.BACK;
+ // private final Button OPTIONS = Button.START;
+ private final Button LEFT_JOY = Button.LEFT_JOY;
+ private final Button RIGHT_JOY = Button.RIGHT_JOY;
+
+ // PS5 trigger buttons
+ private final BooleanSupplier LEFT_TRIGGER_BUTTON = controller.LEFT_TRIGGER_BUTTON;
+ private final BooleanSupplier RIGHT_TRIGGER_BUTTON = controller.RIGHT_TRIGGER_BUTTON;
+
+ // PS5 axis aliases
+ private final Axis LEFT_X = Axis.LEFT_X;
+ private final Axis LEFT_Y = Axis.LEFT_Y;
+ private final Axis RIGHT_X = Axis.RIGHT_X;
+ private final Axis RIGHT_Y = Axis.RIGHT_Y;
+ // private final Axis LEFT_TRIGGER = Axis.LEFT_TRIGGER;
+ // private final Axis RIGHT_TRIGGER = Axis.RIGHT_TRIGGER;
+
+ public PS5XboxModeDriverConfig(
+ Drivetrain drive,
+ Shooter shooter,
+ Turret turret,
+ Hood hood,
+ Intake intake,
+ Spindexer spindexer) {
+ super(drive);
+ this.shooter = shooter;
+ this.turret = turret;
+ this.hood = hood;
+ this.intake = intake;
+ this.spindexer = spindexer;
+ }
+
+ public void configureControls() {
+ // Reset the yaw. Mainly useful for testing/driver practice
+ controller.get(CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
+
+ // Cancel commands
+ controller.get(RB).onTrue(new InstantCommand(() -> {
+ getDrivetrain().setIsAlign(false);
+ getDrivetrain().setDesiredPose(() -> null);
+ CommandScheduler.getInstance().cancelAll();
+ }));
+
+ // Align wheels
+ controller.get(DPad.RIGHT).onTrue(new FunctionalCommand(
+ () -> getDrivetrain().setStateDeadband(false),
+ getDrivetrain()::alignWheels,
+ interrupted -> getDrivetrain().setStateDeadband(true),
+ () -> false, getDrivetrain()).withTimeout(2));
+
+ // Trench align
+ controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> {
+ getDrivetrain().setTrenchAssist(true);
+ getDrivetrain().setTrenchAlign(true);
+ }))
+ .onFalse(new InstantCommand(() -> {
+ getDrivetrain().setTrenchAssist(false);
+ getDrivetrain().setTrenchAlign(false);
+ }));
+
+ // Reverse motors
+ if (intake != null && spindexer != null && shooter != null) {
+ controller.get(CIRCLE).onTrue(new InstantCommand(() -> {
+ reverseMotors = new ReverseMotors(intake);
+ CommandScheduler.getInstance().schedule(reverseMotors);
+ })).onFalse(new InstantCommand(() -> {
+ if (reverseMotors != null) {
+ reverseMotors.cancel();
+ }
+ }));
+ }
+
+ // Intake
+ if (intake != null) {
+ // Toggle intake
+ controller.get(RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> {
+ if (intakeBoolean) {
+ intake.extend();
+ intake.spinStart();
+ intakeBoolean = false;
+ } else {
+ intake.intermediateExtend();
+ intake.spinStop();
+ intakeBoolean = true;
+ }
+ }));
+
+ // Retract if hold for 3 seconds
+ controller.get(RIGHT_TRIGGER_BUTTON).debounce(3.0).onTrue(new InstantCommand(() -> {
+ intake.retract();
+ intakeBoolean = true;
+ }));
+
+ // Calibration
+ controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> {
+ intake.calibrate();
+ })).onFalse(new InstantCommand(() -> {
+ intake.stopCalibrating();
+ }));
+ }
+
+ // Spindexer
+ if (spindexer != null) {
+ // Will only run if we are not calling default shoot command
+ controller.get(LEFT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> spindexer.maxSpindexer()))
+ .onFalse(new InstantCommand(() -> spindexer.stopSpindexer()));
+ }
+
+ // Auto shoot
+ if (turret != null && hood != null && shooter != null && spindexer != null) {
+ autoShoot = new Superstructure(turret, getDrivetrain(), hood, shooter, spindexer);
+ controller.get(SQUARE).toggleOnTrue(autoShoot);
+ }
+
+ // Hood
+ if (hood != null) {
+ controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> {
+ hood.calibrate();
+ })).onFalse(new InstantCommand(() -> {
+ hood.stopCalibrating();
+ }));
+ }
+
+ // Rumble test
+ controller.get(RIGHT_JOY).onTrue(new SequentialCommandGroup(
+ new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_ON)),
+ new WaitCommand(0.5),
+ new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF))));
+ }
+
+ @Override
+ public double getRawSideTranslation() {
+ return controller.get(LEFT_X);
+ }
+
+ @Override
+ public double getRawForwardTranslation() {
+ return controller.get(LEFT_Y);
+ }
+
+ @Override
+ public double getRawRotation() {
+ return controller.get(RIGHT_X);
+ }
+
+ @Override
+ public double getRawHeadingAngle() {
+ return Math.atan2(controller.get(RIGHT_X), -controller.get(RIGHT_Y)) - Math.PI / 2;
+ }
+
+ @Override
+ public double getRawHeadingMagnitude() {
+ return Math.hypot(controller.get(RIGHT_X), controller.get(RIGHT_Y));
+ }
+
+ @Override
+ public boolean getIsSlowMode() {
+ return slowModeSupplier.getAsBoolean();
+ }
+
+ @Override
+ public boolean getIsAlign() {
+ return false;
+ }
+
+ public void startRumble() {
+ controller.setRumble(GameController.RumbleStatus.RUMBLE_ON);
+ }
+
+ public void endRumble() {
+ controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF);
+ }
+}
--- /dev/null
+package frc.robot.subsystems.Intake;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+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.Color8Bit;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.Constants;
+import frc.robot.constants.IdConstants;
+
+public class Intake extends SubsystemBase implements IntakeIO{
+ // Mechanism Display...
+ private final Mechanism2d mechanism;
+ private final MechanismLigament2d robotExtension;
+ @SuppressWarnings("unused")
+ private final MechanismLigament2d robotFrame;
+ private final MechanismLigament2d robotHeight;
+ private final MechanismLigament2d robotPos;
+
+ // create the motors
+ /** Motor to move the roller */
+ private TalonFX rollerMotor = new TalonFX(IdConstants.ROLLER_MOTOR_ID, Constants.CANIVORE_SUB);
+ /** Right motor (master) */
+ private TalonFX rightMotor = new TalonFX(IdConstants.RIGHT_MOTOR_ID, Constants.CANIVORE_SUB);
+ /** Left motor (slave) */
+ private TalonFX leftMotor = new TalonFX(IdConstants.LEFT_MOTOR_ID, Constants.CANIVORE_SUB);
+
+ /** Motor characteristics for the roller motor, a single Kraken X44 (aka gearbox) */
+ private final DCMotor dcMotorRoller = DCMotor.getKrakenX44(1);
+ /** Motor characteristics for the extending pair of Kraken X44 motors (aka gearbox) */
+ private final DCMotor dcMotorExtend = DCMotor.getKrakenX44(2);
+
+ private double maxVelocity;
+ private double maxAcceleration;
+
+ // Use FlywheelSim for the roller
+ private FlywheelSim rollerSim;
+
+ // Use ElevatorSim for the extender
+ private ElevatorSim intakeSim;
+
+ private final MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0);
+
+ private double setpointInches = 0.0;
+
+ private boolean calibrating = false;
+
+ private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged();
+
+ public Intake() {
+
+ // get the maximum free speed
+ double maxFreeSpeed = Units.radiansToRotations(dcMotorExtend.freeSpeedRadPerSec)/ IntakeConstants.GEAR_RATIO;
+
+ // max free speed (rot/s) = motor free speed (rad/s to rot/s)/ gear ratio
+ // safety margin, limits velocity to .75 free speed
+ maxVelocity = 0.75 * maxFreeSpeed;
+ maxAcceleration = maxVelocity / 0.25;
+
+ // ----Rollers
+ // Configure the motors
+ // Build the configuration for the roller
+ TalonFXConfiguration rollerConfig = new TalonFXConfiguration();
+
+ // config Slot 0 PID params
+ var slot0Configs = rollerConfig.Slot0;
+ slot0Configs.kP = 5.0;
+ slot0Configs.kI = 0.0;
+ slot0Configs.kD = 0.0;
+ slot0Configs.kV = 0.0;
+ slot0Configs.kA = 0.0;
+
+ // set the brake mode
+ rollerConfig.MotorOutput.withNeutralMode(NeutralModeValue.Brake);
+
+ // apply the configuration to the right motor (master)
+ rollerMotor.getConfigurator().apply(rollerConfig);
+
+ // --- Extenders
+
+ // Build the configuration for the left and right Motor
+ TalonFXConfiguration config = new TalonFXConfiguration();
+
+ // config Slot 0 PID params
+ var extenderSlot0Configs = config.Slot0;
+ extenderSlot0Configs.kP = 0.5;
+ extenderSlot0Configs.kI = 0.0;
+ extenderSlot0Configs.kD = 0.0;
+ extenderSlot0Configs.kV = 0.0;
+ extenderSlot0Configs.kA = 0.0;
+
+ // configure MotionMagic
+ MotionMagicConfigs motionMagicConfigs = config.MotionMagic;
+
+ motionMagicConfigs.MotionMagicCruiseVelocity = IntakeConstants.GEAR_RATIO * maxVelocity/IntakeConstants.RADIUS_RACK_PINION/Math.PI/2;
+ motionMagicConfigs.MotionMagicAcceleration = IntakeConstants.GEAR_RATIO * maxAcceleration/IntakeConstants.RADIUS_RACK_PINION/Math.PI/2;
+
+ rightMotor.getConfigurator().apply(config);
+ leftMotor.getConfigurator().apply(config);
+
+ leftMotor.getConfigurator().apply(
+ new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)
+ .withNeutralMode(NeutralModeValue.Coast)
+ );
+
+ rightMotor.getConfigurator().apply(
+ new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast)
+ );
+
+
+ leftMotor.setPosition(0.0);
+ rightMotor.setPosition(0.0);
+
+ setNewCurrentLimit(IntakeConstants.STATOR_CURRENT_EXTENDER_LIMIT, IntakeConstants.SUPPLY_CURRENT_EXTENDER_LIMIT, IntakeConstants.STATOR_ROLLER_CURRENT_LIMIT, IntakeConstants.SUPPLY_ROLLER_CURRENT_LIMIT);
+
+ // Build the mechanism for display
+ mechanism = new Mechanism2d(80, 80);
+ MechanismRoot2d root = mechanism.getRoot("Root", 0, 1);
+ robotPos = root.append(new MechanismLigament2d("robotPos", 40, 0.0, 1, new Color8Bit(0, 0, 0)));
+ robotFrame = robotPos.append(new MechanismLigament2d("Robot Frame",28,0.0, 2, new Color8Bit(0, 255, 255)));
+ robotHeight = robotPos.append(new MechanismLigament2d("Robot Height", 22.5, 90, 1, new Color8Bit(0,255,255)));
+ // extensiion is initially retracted.
+ robotExtension = robotHeight.append(new MechanismLigament2d("Robot Extension", 0, 90, 2, new Color8Bit(255, 0, 0) ));
+
+ // add some test commands.
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putData("Extension Mechanism", mechanism);
+ SmartDashboard.putData("Intake Calibrate", new InstantCommand(() -> calibrate()));
+ SmartDashboard.putData("Intake Stop Calibrating", new InstantCommand(() -> stopCalibrating()));
+ SmartDashboard.putData("Extend Intake", new InstantCommand(() -> extend()));
+ SmartDashboard.putData("Retract Intake", new InstantCommand(() -> retract()));
+ }
+
+ if (RobotBase.isSimulation()) {
+ // Extender simulation
+ // the supply voltage should change with load....
+ rightMotor.getSimState().setSupplyVoltage(12.0);
+
+ // rack pinion is 10 teeth and 10 DP for a radius of 1 inches
+ double drumRadiusMeters = Units.inchesToMeters(1.0);
+ double minHeightMeters = Units.inchesToMeters(0.0);
+ double maxHeightMeters = Units.inchesToMeters(IntakeConstants.MAX_EXTENSION);
+ // start retracted
+ double startingHeightMeters = Units.inchesToMeters(0.0);
+ intakeSim = new ElevatorSim(
+ dcMotorExtend,
+ IntakeConstants.GEAR_RATIO,
+ IntakeConstants.CARRIAGE_MASS_KG,
+ drumRadiusMeters,
+ minHeightMeters,
+ maxHeightMeters,
+ false,
+ startingHeightMeters);
+
+ // Roller simulation
+ rollerSim = new FlywheelSim(
+ LinearSystemId.createFlywheelSystem(dcMotorRoller, IntakeConstants.ROLLER_MOI_KG_M_SQ, IntakeConstants.ROLLER_GEARING),
+ dcMotorRoller);
+ }
+ }
+
+ public void periodic() {
+ double inchExtension = getPosition();
+
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Intake/Setpoint", setpointInches);
+ }
+ robotExtension.setLength(inchExtension);
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putNumber("Intake Extension (in)", inchExtension);
+ SmartDashboard.putBoolean("Intake Extended", inchExtension > 1.0);
+ }
+
+ if(calibrating){
+ leftMotor.set(-0.1);
+ rightMotor.set(-0.1);
+ }
+
+ updateInputs();
+ Logger.processInputs("Intake", inputs);
+
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putBoolean("Intake Calibrated", !calibrating);
+ SmartDashboard.putBoolean("Intake At Setpoint", Math.abs(inchExtension - setpointInches) < 0.5);
+ }
+ }
+
+ public void simulationPeriodic(){
+ // get the applied motor voltage
+ double voltage = rightMotor.getMotorVoltage().getValueAsDouble();
+
+ // tell the simulator that voltage
+ intakeSim.setInputVoltage(voltage);
+ // run the siimulation
+ intakeSim.update(0.02);
+
+ // get the simulation result
+ double metersExtend = intakeSim.getPositionMeters();
+ double inchesExtend = Units.metersToInches(metersExtend);
+ double motorRotations = inchesToRotations(inchesExtend);
+
+ // set the motor to that position
+ rightMotor.getSimState().setRawRotorPosition(motorRotations);
+
+ // update the display
+ robotExtension.setLength(inchesExtend);
+
+ // simulate roller
+ voltage = rollerMotor.getMotorVoltage().getValueAsDouble();
+ rollerSim.setInputVoltage(voltage);
+ rollerSim.update(0.020);
+
+ double velocity = Units.radiansToRotations(rollerSim.getAngularVelocityRadPerSec()) * IntakeConstants.ROLLER_GEARING;
+
+ rollerMotor.getSimState().setRotorVelocity(velocity);
+ }
+
+ /**
+ * Set the intake extender position
+ * @param setpoint in inches
+ */
+ public void setPosition(double setpoint) {
+ double motorRotations = -inchesToRotations(setpoint);
+ rightMotor.setControl(voltageRequest.withPosition(motorRotations).withEnableFOC(true));
+ leftMotor.setControl(voltageRequest.withPosition(motorRotations).withEnableFOC(true));
+
+ setpointInches = setpoint;
+ }
+
+ /**
+ * Get the intake extender position
+ * @return inches
+ */
+ public double getPosition(){
+ return inputs.leftPosition;
+ }
+
+ /**
+ * convert rotations to inches
+ * @param rotations of the motor
+ * @return inches of rack travel
+ */
+ public double rotationsToInches(double motorRotations) {
+ // circumference of the rack pinion
+ double circ = 2 * Math.PI * IntakeConstants.RADIUS_RACK_PINION;
+ double pinionRotations = motorRotations / IntakeConstants.GEAR_RATIO;
+ double inches = pinionRotations * circ;
+ return inches;
+ }
+
+ /**
+ * convert inches to rotations
+ * @param inches of rack travel
+ * @return motor rotations
+ */
+ public double inchesToRotations(double inches){
+ double circ = 2 * Math.PI * IntakeConstants.RADIUS_RACK_PINION;
+ double pinionRotations = inches / circ;
+ double motorRotations = pinionRotations * IntakeConstants.GEAR_RATIO;
+ return motorRotations;
+ }
+
+ /**
+ * Set the roller speed.
+ * @param speed duty cycle in the range [-1, 1]
+ */
+ public void spin(double speed) {
+ rollerMotor.set(speed);
+ }
+
+ public double getSpeed() {
+ return rollerMotor.get();
+ }
+
+ /**
+ * Start the intake roller spinning.
+ */
+ public void spinStart() {
+ spin(IntakeConstants.SPEED);
+ }
+
+ /**
+ * Stop the intake roller.
+ */
+ public void spinStop() {
+ spin(0.0);
+ }
+
+ /**
+ * Reverses the intake roller
+ */
+ public void spinReverse() {
+ spin(-IntakeConstants.SPEED);
+ }
+
+ /** Extend the intake the maximum distance. */
+ public void extend() {
+ setPosition(IntakeConstants.MAX_EXTENSION);
+ }
+
+ /** Extend to a position that doesn't hit the spindexer */
+ public void intermediateExtend(){
+ setPosition(IntakeConstants.INTERMEDIATE_EXTENSION);
+ }
+
+ /** Retract the intake to a safe starting position. */
+ public void retract(){
+ setPosition(IntakeConstants.STOW_EXTENSION);
+ }
+
+ /** Goes to the zero position */
+ public void zeroPosition(){
+ setPosition(0.0);
+ }
+
+ public void zeroMotors() {
+ rightMotor.setPosition(0.0);
+ leftMotor.setPosition(0.0);
+ }
+
+ /**
+ * Reclaim all the resources (e.g., motors and other devices).
+ * This step is necessary for multiple unit tests to work.
+ */
+ public void close() {
+ leftMotor.close();
+ rightMotor.close();
+ rollerMotor.close();
+ }
+
+ /**
+ * Starts calibrating by running it backwards
+ */
+ public void calibrate(){
+ setNewCurrentLimit(IntakeConstants.CALIBRATING_CURRENT_LIMITS, IntakeConstants.CALIBRATING_CURRENT_LIMITS, IntakeConstants.CALIBRATING_CURRENT_LIMITS, IntakeConstants.CALIBRATING_CURRENT_LIMITS);
+ calibrating = true;
+ }
+
+ /**
+ * Stops, zeros, and moves it to retract position
+ */
+ public void stopCalibrating(){
+ zeroMotors();
+ setNewCurrentLimit(IntakeConstants.STATOR_CURRENT_EXTENDER_LIMIT, IntakeConstants.SUPPLY_CURRENT_EXTENDER_LIMIT, IntakeConstants.STATOR_ROLLER_CURRENT_LIMIT, IntakeConstants.SUPPLY_ROLLER_CURRENT_LIMIT);
+ calibrating = false;
+ retract();
+ }
+
+ public void setNewCurrentLimit(double statorExtenders, double supplyExtenders, double statorRoller, double supplyRollers) {
+ CurrentLimitsConfigs limitConfigExtender = new CurrentLimitsConfigs();
+ limitConfigExtender.StatorCurrentLimit = statorExtenders;
+ limitConfigExtender.StatorCurrentLimitEnable = true;
+ limitConfigExtender.SupplyCurrentLimit = statorExtenders;
+ limitConfigExtender.SupplyCurrentLimitEnable = true;
+ leftMotor.getConfigurator().apply(limitConfigExtender);
+ rightMotor.getConfigurator().apply(limitConfigExtender);
+
+ // roller
+ CurrentLimitsConfigs limitConfigRoller = new CurrentLimitsConfigs();
+ limitConfigRoller.StatorCurrentLimit = statorRoller;
+ limitConfigRoller.StatorCurrentLimitEnable = true;
+ limitConfigRoller.SupplyCurrentLimit = supplyRollers;
+ limitConfigRoller.SupplyCurrentLimitEnable = true;
+ rollerMotor.getConfigurator().apply(limitConfigRoller);
+ }
+
+ public double getSubsystemStatorCurrent() {
+ return inputs.leftStatorCurrent + inputs.rightStatorCurrent + inputs.rollerStatorCurrent;
+ }
+
+ public double getSubsystemSupplyCurrent() {
+ return inputs.leftSupplyCurrent + inputs.rightSupplyCurrent + inputs.rollerSupplyCurrent;
+ }
+
+ @Override
+ public void updateInputs() {
+ inputs.leftPosition = rotationsToInches(leftMotor.getPosition().getValueAsDouble());
+ inputs.rightPosition = rotationsToInches(rightMotor.getPosition().getValueAsDouble());
+ inputs.leftStatorCurrent = leftMotor.getStatorCurrent().getValueAsDouble();
+ inputs.rightStatorCurrent = rightMotor.getStatorCurrent().getValueAsDouble();
+ inputs.leftSupplyCurrent = leftMotor.getSupplyCurrent().getValueAsDouble();
+ inputs.rightSupplyCurrent = rightMotor.getSupplyCurrent().getValueAsDouble();
+ inputs.rollerVelocity = rollerMotor.getVelocity().getValueAsDouble();
+ inputs.rollerStatorCurrent = rollerMotor.getStatorCurrent().getValueAsDouble();
+ inputs.rollerSupplyCurrent = rollerMotor.getSupplyCurrent().getValueAsDouble();
+ }
+
+}
--- /dev/null
+package frc.robot.subsystems.Intake;
+
+import edu.wpi.first.math.util.Units;
+
+public class IntakeConstants {
+ /** Intake roller motor speed in range [-1, 1] */
+ public static final double SPEED = 1.0;
+ /** 12 tooth pinion driving 36 tooth driven gear */
+ public static final double GEAR_RATIO = 48.0/10.0;
+ /** radius (inches) of the rack gear which is a 10 tooth pinion at 10 DP */
+ public static final double RADIUS_RACK_PINION = 0.5;
+
+ /**Current limits when calibrating */
+ public static final double CALIBRATING_CURRENT_LIMITS = 10.0;
+ public static final double CALIBRATING_CURRENT_THRESHOLD = 9.0;
+
+ /** Current limit for normal operation */
+ public static final double STATOR_CURRENT_EXTENDER_LIMIT = 100.0;
+ public static final double SUPPLY_CURRENT_EXTENDER_LIMIT = 100.0;
+ public static final double STATOR_ROLLER_CURRENT_LIMIT = 40.0;
+ public static final double SUPPLY_ROLLER_CURRENT_LIMIT = 40.0;
+
+
+ public static final double ROLLER_MOI_KG_M_SQ = 0.5 * 0.020 * 0.020; // 0.5kg roller, 20mm radius for now
+ public static final double ROLLER_GEARING = 2.0;
+ public static final double CARRIAGE_MASS_KG = 3.0;
+ public static final double DRUM_RADIUS_METERS = Units.inchesToMeters(1.0);
+
+
+ /** max extension in inches */
+ public static final double MAX_EXTENSION = 10.5; // inches
+
+ public static final double INTERMEDIATE_EXTENSION = 5.0; //inches
+
+ public static final double STOW_EXTENSION = 0.2; // inches
+
+ /** starting point in inches */
+ public static final double STARTING_POINT = 0;
+ /** rack pitch in teeth per inch of diameter (Diametral Pitch) DP = N teeth / Diameter in inches */
+ public static final double RACK_PITCH = 10;
+
+ // Simulation
+
+}
--- /dev/null
+package frc.robot.subsystems.Intake;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IntakeIO {
+ @AutoLog
+ public static class IntakeIOInputs {
+ public double leftPosition = 0.0;
+ public double rightPosition = 0.0;
+ public double leftSupplyCurrent = 0.0;
+ public double rightSupplyCurrent = 0.0;
+ public double leftStatorCurrent = 0.0;
+ public double rightStatorCurrent = 0.0;
+ public double rollerVelocity = 0.0;
+ public double rollerSupplyCurrent = 0.0;
+ public double rollerStatorCurrent = 0.0;
+ }
+
+ public void updateInputs();
+}
--- /dev/null
+package frc.robot.subsystems.LED;
+
+import java.util.Optional;
+
+import com.ctre.phoenix6.configs.CANdleConfigurator;
+import com.ctre.phoenix6.configs.CANdleFeaturesConfigs;
+import com.ctre.phoenix6.configs.LEDConfigs;
+import com.ctre.phoenix6.controls.ColorFlowAnimation;
+import com.ctre.phoenix6.controls.FireAnimation;
+import com.ctre.phoenix6.controls.RainbowAnimation;
+import com.ctre.phoenix6.controls.RgbFadeAnimation;
+import com.ctre.phoenix6.controls.SolidColor;
+import com.ctre.phoenix6.controls.StrobeAnimation;
+import com.ctre.phoenix6.controls.TwinkleAnimation;
+import com.ctre.phoenix6.hardware.CANdle;
+import com.ctre.phoenix6.signals.Enable5VRailValue;
+import com.ctre.phoenix6.signals.LossOfSignalBehaviorValue;
+import com.ctre.phoenix6.signals.RGBWColor;
+import com.ctre.phoenix6.signals.StatusLedWhenActiveValue;
+import com.ctre.phoenix6.signals.StripTypeValue;
+import com.ctre.phoenix6.signals.VBatOutputModeValue;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.Constants;
+import frc.robot.constants.IdConstants;
+import frc.robot.util.HubActive;
+
+public class LED extends SubsystemBase {
+
+ private CANdle candle;
+ public static final int stripLength = 67;
+
+ /// Hz
+ public static final int FLASH_RATE = 4;
+
+ private Color color;
+
+ public LED() {
+ candle = new CANdle(IdConstants.CANDLE_ID, Constants.RIO_CAN);
+ CANdleConfigurator configurator = candle.getConfigurator();
+
+ LEDConfigs ledConf = new LEDConfigs()
+ .withStripType(StripTypeValue.GRB)
+ .withLossOfSignalBehavior(LossOfSignalBehaviorValue.KeepRunning)
+ .withBrightnessScalar(1);
+
+ CANdleFeaturesConfigs featureConf = new CANdleFeaturesConfigs()
+ .withEnable5VRail(Enable5VRailValue.Enabled) // Turns off LEDs
+ .withStatusLedWhenActive(StatusLedWhenActiveValue.Disabled)
+ .withVBatOutputMode(VBatOutputModeValue.On);
+
+ configurator.apply(featureConf);
+ configurator.apply(ledConf);
+
+ setColor();
+
+ candle.clearAllAnimations();
+ lightsOff();
+
+ // System.out.println("CANdle features: " + featureConf + ", LED config: " + ledConf);
+
+ SmartDashboard.putData("LED Disable", new InstantCommand(() -> {forceOff = true; lightsOff();}).ignoringDisable(true));
+ SmartDashboard.putData("LED Enable", new InstantCommand(() -> forceOff = false).ignoringDisable(true));
+ SmartDashboard.putData("LED Strobe", new InstantCommand(() -> setStrobe()).ignoringDisable(true));
+ SmartDashboard.putData("LED Static", new InstantCommand(() -> setStatic()).ignoringDisable(true));
+ SmartDashboard.putData("LED Fire", new InstantCommand(() -> setFire()).ignoringDisable(true));
+ SmartDashboard.putData("LED Rainbow", new InstantCommand(() -> setRainbow()).ignoringDisable(true));
+ SmartDashboard.putData("LED Fade", new InstantCommand(() -> setRgbFadeAnimation()).ignoringDisable(true));
+ SmartDashboard.putData("LED Twinkle", new InstantCommand(() -> setTwinkle()).ignoringDisable(true));
+ SmartDashboard.putData("LED Color Flow", new InstantCommand(() -> setColorFlow()).ignoringDisable(true));
+ SmartDashboard.putData("LED Color Team Reset", new InstantCommand(() -> setColor()).ignoringDisable(true));
+ }
+
+ public void setColor() {
+ var alliance = DriverStation.getAlliance();
+ if (alliance.isEmpty()) {
+ color = Color.kOrangeRed;
+ } else if (alliance.get() == Alliance.Red) {
+ color = Color.kRed;
+ } else if (alliance.get() == Alliance.Blue) {
+ color = Color.kBlue;
+ } else {
+ color = Color.kOrangeRed;
+ }
+ }
+
+ private enum State { OFF, ON, AUTO, SLOW, FAST, ENDGAME };
+
+ private State lastState = State.OFF;
+ private boolean forceOff = false;
+
+ @Override
+ public void periodic() {
+ State targetState = State.ON;
+ if (underSecsToFlip(5)) targetState = State.SLOW;
+ if (underSecsToFlip(1)) targetState = State.FAST;
+ if (DriverStation.isAutonomous()) targetState = State.AUTO;
+ if (DriverStation.getMatchTime() < 30) targetState = State.ENDGAME;
+ if (forceOff) targetState = State.OFF;
+
+ if (targetState != lastState) {
+ switch (targetState) {
+ case OFF: lightsOff(); break;
+ case ON: setStatic(); break;
+ case AUTO: setTwinkle(); break;
+ case SLOW: setStrobe(); break;
+ case FAST: setFastStrobe(); break;
+ case ENDGAME: setRainbow(); break;
+ }
+ lastState = targetState;
+ }
+ }
+
+ public void setFire() {
+ candle.clearAllAnimations();
+ candle.setControl(new FireAnimation(8, 8 + stripLength).withSparking(0.5));
+ }
+
+ public void setRainbow() {
+ candle.clearAllAnimations();
+ candle.setControl(new RainbowAnimation(8, 8 + stripLength));
+ }
+
+ public void setRgbFadeAnimation() {
+ candle.clearAllAnimations();
+ candle.setControl(new RgbFadeAnimation(8, 8 + stripLength));
+ }
+
+ public void setTwinkle() {
+ candle.clearAllAnimations();
+ candle.setControl(new TwinkleAnimation(8, 8 + stripLength).withColor(new RGBWColor(Color.kViolet)));
+ }
+
+ public void setColorFlow() {
+ candle.clearAllAnimations();
+ candle.setControl(new ColorFlowAnimation(8, 8 + stripLength).withColor(new RGBWColor(Color.kAzure)));
+ }
+
+ public void setStrobe() {
+ candle.clearAllAnimations();
+ candle.setControl(new StrobeAnimation(8, 8 + stripLength).withFrameRate(FLASH_RATE).withColor(new RGBWColor(color)));
+ }
+
+ public void setFastStrobe() {
+ candle.clearAllAnimations();
+ candle.setControl(new StrobeAnimation(8, 8 + stripLength).withFrameRate(FLASH_RATE * 4).withColor(new RGBWColor(color)));
+ }
+
+ public void setStatic() {
+ candle.clearAllAnimations();
+ candle.setControl(new SolidColor(8, 8 + stripLength).withColor(new RGBWColor(color)));
+ }
+
+ public void lightsOff() {
+ candle.clearAllAnimations();
+ candle.setControl(new SolidColor(8 , 8 + stripLength).withColor(new RGBWColor(0, 0, 0, 0)));
+ }
+
+
+ private boolean underSecsToFlip(double secs) {
+ Optional<Double> timeToActive = HubActive.timeToActive();
+ Optional<Double> timeToInactive = HubActive.timeToInactive();
+
+ if (timeToActive.isEmpty() && timeToInactive.isEmpty()) {
+ return false;
+ } else if (timeToActive.isPresent() && timeToActive.get() != 0) {
+ return (timeToActive.get() <= secs);
+
+ } else if (timeToInactive.isPresent() && timeToInactive.get() != 0) {
+ return (timeToInactive.get() <= secs);
+ } else {
+ return false;
+ }
+ }
+
+}
--- /dev/null
+package frc.robot.subsystems.PowerControl;
+
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Battery extends SubsystemBase {
+ private double voltage;
+
+ public Battery() {
+ updateVoltageFromBattery();
+ }
+
+ private void updateVoltageFromBattery() {
+ voltage = RobotController.getBatteryVoltage();
+ }
+
+ public double getVoltage() {
+ return voltage;
+ }
+
+ public double voltsTillBrownOut() {
+ return voltage - RobotController.getBrownoutVoltage();
+ }
+
+ public double toBrownOut() {
+ // percent of volts we've got left over what we had to start with
+ return voltsTillBrownOut() / (BatteryConstants.MAX_STARTING_VOLTS - RobotController.getBrownoutVoltage());
+ }
+
+ @Override
+ public void periodic() {
+ updateVoltageFromBattery();
+ }
+}
--- /dev/null
+package frc.robot.subsystems.PowerControl;
+
+public class BatteryConstants {
+ public static final double MAX_STARTING_VOLTS = 12.5; // V
+}
--- /dev/null
+package frc.robot.subsystems.PowerControl;
+
+import java.util.LinkedHashMap;
+import java.util.Map;
+
+public class BreakerConstants {
+ public static final Map<Double, Double> THRESHOLDS = new LinkedHashMap<>();
+ static {
+ THRESHOLDS.put(1.0, 6.0 * 120); // breaker default at 120
+ THRESHOLDS.put(4.0, 3.4 * 120);
+ THRESHOLDS.put(10.0, 2.0 * 120);
+ THRESHOLDS.put(20.0, 1.6 * 120);
+ THRESHOLDS.put(30.0, 1.5 * 120);
+ }
+
+ public static final double WARNING_PERCENTAGE = 0.6; // percent that the system reacts to approaching thresholds
+
+ // ports
+ public static int[] DRIVETRAIN_PORTS = { 8, 9, 10, 11, 18, 19, 0, 1 }; // bls, bld, fld, fls, frs, frd, brd, brs
+ public static int[] TURRET_PORTS = { 2 };
+ public static int[] INTAKE_PORTS = { 15, 14, 13 }; // right, left, roller
+ public static int[] SHOOTER_PORTS = { 3, 4 }; // left, right
+ public static int[] HOOD_PORTS = { 5 }; // shooter
+ public static int[] SPINDEXER_PORTS = { 12 }; // spindexer (unupdated on sheets)
+}
--- /dev/null
+package frc.robot.subsystems.PowerControl;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Map;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.Constants;
+
+public class EMABreaker extends SubsystemBase {
+
+ private static class Current {
+ double tau;
+ double alpha; // how much of the error we correct per loop
+ double average = 0;
+ double threshold;
+ }
+
+ PowerDistribution pDis = new PowerDistribution();
+
+ double[] subsystemCurrents;
+
+ private List<Current> filters = new ArrayList<>(); // contains currents with their alphas and thresholds
+ private List<Current> subsystems = new ArrayList<>();
+
+ public EMABreaker() {
+ for (Map.Entry<Double, Double> entry : BreakerConstants.THRESHOLDS.entrySet()) {
+ double tau = entry.getKey(); // sec
+ double threshold = entry.getValue(); // A
+
+ Current w = new Current(); // create a filter for the threshold
+ w.tau = tau;
+ w.threshold = threshold;
+ w.alpha = 1 - Math.exp(-Constants.LOOP_TIME / tau); // 1 - e^(-0.02/1) = 0.0198, 1 - e^(-0.02/2) = 0.00995
+
+ filters.add(w);
+ }
+
+ // subsystems
+ for (int i = 0; i < pDis.getNumChannels(); i++) {
+ double tau = 1.0;
+ double threshold = i;
+ Current w = new Current();
+ w.tau = tau;
+ w.threshold = threshold;
+ w.alpha = 1 - Math.exp(-Constants.LOOP_TIME / tau); // 1 - e^(-0.02/1) = 0.
+
+ subsystems.add(w);
+ }
+ }
+
+ @Override
+ public void periodic() {
+ double current = getCurrentFromPowerDistribution();
+ // this is total current averages
+ for (Current f : filters) {
+ // new avg = old avg + fractionAlpha * difference
+ f.average += f.alpha * (current - f.average);
+ Logger.recordOutput("Breaker/IntervalAverage/" + f.tau, f.average);
+ }
+
+ // this is getting currents coming out of all the ports from PDH (big thing
+ // under robot all the wires come out of)
+ subsystemCurrents = getAllCurrentFromPowerDistribution();
+
+ // this should average out all ports
+ for (Current s : subsystems) {
+ s.average += s.alpha * (subsystemCurrents[(int) s.threshold] - s.average);
+ }
+
+ // this should use updated port averages and sum them to get drivetrain average
+ // draw for 1 tau (can add more later)
+ Logger.recordOutput("Breaker/DrivetrainAverageDraw", getAverageCurrentDraw(BreakerConstants.DRIVETRAIN_PORTS));
+ Logger.recordOutput("Breaker/SpindexerDraw", getAverageCurrentDraw(BreakerConstants.SPINDEXER_PORTS));
+ Logger.recordOutput("Breaker/ShooterDraw", getAverageCurrentDraw(BreakerConstants.SHOOTER_PORTS));
+ Logger.recordOutput("Breaker/IntakeDraw", getAverageCurrentDraw(BreakerConstants.INTAKE_PORTS));
+ Logger.recordOutput("Breaker/TurretDraw", getAverageCurrentDraw(BreakerConstants.TURRET_PORTS));
+ Logger.recordOutput("Breaker/HoodDraw", getAverageCurrentDraw(BreakerConstants.HOOD_PORTS));
+
+ // total stuff
+ Logger.recordOutput("Breaker/TotalCurrent", current);
+ Logger.recordOutput("Breaker/CurrentWarning", isInWarning());
+ }
+
+ public double getAverageCurrentDraw(int[] ports) {
+ double sum = 0;
+ for (int number : ports) {
+ sum += subsystems.get(number).average;
+ }
+ return sum;
+ }
+
+ public double getCurrentFromPowerDistribution() {
+ return pDis.getTotalCurrent(); // not using .getCurrent() and then an arguement for the port you can get just
+ // one port
+ }
+
+ public double[] getAllCurrentFromPowerDistribution() {
+ return pDis.getAllCurrents();
+ }
+
+ public boolean isInWarning() {
+ for (Current f : filters) {
+ if (f.average > f.threshold * BreakerConstants.WARNING_PERCENTAGE) {
+ return true; // uh oh
+ }
+ }
+ return false;
+ }
+
+ // returns an average of the filters
+ public double percentageAverageUsage() {
+ double sumAvg = 0;
+ for (Current f : filters) {
+ sumAvg += f.average / f.threshold; // gets percentage of us
+ }
+ return sumAvg / filters.size(); // average across filters
+ }
+
+ // gives the worst case filter
+ public double[] percentageMaxUsage() {
+ Current worst = filters.get(0); // returns worst (default to tau filter)
+ for (Current f : filters) {
+ if (f.average / f.threshold > worst.average / worst.threshold) {
+ worst = f;
+ }
+ }
+ double[] returnValue = { worst.average / worst.threshold, worst.tau };
+ return returnValue;
+ }
+}
--- /dev/null
+package frc.robot.subsystems.drivetrain;
+
+import java.util.Arrays;
+import java.util.ArrayList;
+import java.util.Optional;
+import java.util.concurrent.locks.Lock;
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.function.Supplier;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.pathplanner.lib.util.PathPlannerLogging;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.commands.Music;
+import frc.robot.constants.Constants;
+import frc.robot.constants.FieldConstants;
+import frc.robot.constants.GyroBiasConstants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.constants.swerve.ModuleConstants;
+import frc.robot.util.EqualsUtil;
+import frc.robot.util.PhoenixOdometryThread;
+import frc.robot.util.SwerveModulePose;
+import frc.robot.util.SwerveStuff.SwerveSetpoint;
+import frc.robot.util.SwerveStuff.SwerveSetpointGenerator;
+import frc.robot.util.Vision.Vision;
+import frc.robot.util.Vision.GyroBiasEstimator;
+import org.photonvision.EstimatedRobotPose;
+
+/**
+ * Represents a swerve drive style drivetrain.
+ * <p>
+ * Module IDs are:
+ * 1: Front left
+ * 2: Front right
+ * 3: Back left
+ * 4: Back right
+ */
+public class Drivetrain extends SubsystemBase {
+
+ protected final Module[] modules;
+
+ private final GyroIO gyroIO;
+ private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
+
+ public static Lock odometryLock = new ReentrantLock();
+
+ private SwerveSetpoint currentSetpoint = new SwerveSetpoint(
+ new ChassisSpeeds(),
+ new SwerveModuleState[] {
+ new SwerveModuleState(),
+ new SwerveModuleState(),
+ new SwerveModuleState(),
+ new SwerveModuleState()
+ });
+ // Odometry
+ private final SwerveDrivePoseEstimator poseEstimator;
+
+ // Vision
+ private final Vision vision;
+
+ // PID Controllers for chassis movement
+ private final PIDController xController;
+ private final PIDController yController;
+ private final PIDController rotationController;
+
+ // If vision is enabled for drivetrain odometry updating
+ // DO NOT CHANGE THIS HERE TO DISABLE VISION, change VisionConstants.ENABLED
+ // instead
+ private boolean visionEnabled = true;
+
+ // Disables vision for the first few seconds after deploying
+ private Timer visionEnableTimer = new Timer();
+
+ // If the robot should algin to the angle
+ private boolean isAlign = false;
+ // Angle to align to, can be null
+ private Double alignAngle = null;
+ // used for drift control
+ private double currentHeading = 0;
+ // used for drift control
+ private boolean drive_turning = false;
+
+ private SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator();
+
+ // The pose supplier to drive to
+ private Supplier<Pose2d> desiredPoSupplier = () -> null;
+
+ private SwerveModulePose modulePoses;
+
+ // The previous pose to reset to if the current pose gets too far off the field
+ private Pose2d prevPose = new Pose2d();
+
+ private SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];;
+
+ private boolean slipped = false;
+
+ private double previousAngularVelocity = 0;
+
+ private double centerOfMassHeight = 0;
+
+ private Rotation2d rawGyroRotation = new Rotation2d();
+
+ // for vision yaw correction
+ private GyroBiasEstimator gyroBiasEstimator = new GyroBiasEstimator();
+
+ private final Field2d field = new Field2d();
+
+ /**
+ * Creates a new Swerve Style Drivetrain.
+ */
+ public Drivetrain(Vision vision, GyroIO gyroIO) {
+ this.vision = vision;
+
+ modules = new Module[4];
+ this.gyroIO = gyroIO;
+ ModuleConstants[] constants = Arrays.copyOfRange(ModuleConstants.values(), 0, 4);
+
+ if (RobotBase.isReal()) {
+ Arrays.stream(constants).forEach(moduleConstants -> {
+ modules[moduleConstants.ordinal()] = new Module(moduleConstants);
+ });
+ } else {
+ Arrays.stream(constants).forEach(moduleConstants -> {
+ modules[moduleConstants.ordinal()] = new ModuleSim(moduleConstants);
+ });
+ }
+
+ /*
+ * By pausing init for a second before setting module offsets, we avoid a bug
+ * with inverting motors.
+ * See https://github.com/Team364/BaseFalconSwerve/issues/8 for more info.
+ */
+ Timer.delay(1.0);
+ resetModulesToAbsolute();
+ gyroIO.updateInputs(gyroInputs);
+ poseEstimator = new SwerveDrivePoseEstimator(
+ DriveConstants.KINEMATICS,
+ gyroInputs.yawPosition,
+ updateModulePositions(),
+ new Pose2d(),
+ // Defaults, except trust pigeon more
+ VecBuilder.fill(0.1, 0.1, 0),
+ VisionConstants.VISION_STD_DEVS);
+ poseEstimator.setVisionMeasurementStdDevs(VisionConstants.VISION_STD_DEVS);
+
+ // initialize PID controllers
+ xController = new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D);
+ yController = new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D);
+ rotationController = new PIDController(DriveConstants.HEADING_P, 0, DriveConstants.HEADING_D);
+ rotationController.enableContinuousInput(-Math.PI, Math.PI);
+ rotationController.setTolerance(Units.degreesToRadians(0.25), Units.degreesToRadians(0.25));
+
+ PhoenixOdometryThread.getInstance().start();
+
+ modulePoses = new SwerveModulePose(this, DriveConstants.MODULE_LOCATIONS);
+
+ PathPlannerLogging.setLogActivePathCallback(
+ (activePath) -> {
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput(
+ "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
+ }
+ });
+ PathPlannerLogging.setLogTargetPoseCallback(
+ (targetPose) -> {
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
+ }
+ });
+
+ // PPLibTelemetry.enableCompetitionMode();
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putData("Field", field);
+ }
+
+ // addMusic();
+
+ }
+
+ public void setPose(Translation2d pose) {
+ poseEstimator.resetTranslation(pose);
+ }
+
+ public void addMusic() {
+ ArrayList<TalonFX> motors = new ArrayList<>();
+ for (Module m: modules) {
+ motors.add(m.getMotors()[0]);
+ motors.add(m.getMotors()[1]);
+ }
+
+ TalonFX[] f = new TalonFX[8];
+
+ SmartDashboard.putData("Chirp", new Music(motors.toArray(f)));
+ }
+
+ public void close() {
+ // close each of the modules
+ for (int i = 0; i < modules.length; i++) {
+ modules[i].close();
+ }
+ }
+
+ @Override
+ public void periodic() {
+ odometryLock.lock(); // Prevents odometry updates while reading data
+ gyroIO.updateInputs(gyroInputs);
+ Logger.processInputs("Drive/Gyro", gyroInputs);
+ for (var module : modules) {
+ module.periodic();
+ }
+ odometryLock.unlock();
+ // Update odometry
+ double[] sampleTimestamps = gyroInputs.odometryYawTimestamps; // All signals are sampled together
+ int sampleCount = sampleTimestamps.length;
+ SwerveModulePosition[][] positions = new SwerveModulePosition[4][];
+ for (int i = 0; i < modules.length; i++) {
+ positions[i] = modules[i].getOdometryPositions();
+ sampleCount = Math.min(sampleCount, positions[i].length);
+ }
+
+ // cap samples per cycle, more gives little benefit
+ final int MAX_SAMPLES_PER_CYCLE = 10;
+ if (sampleCount > MAX_SAMPLES_PER_CYCLE) {
+ sampleCount = MAX_SAMPLES_PER_CYCLE;
+ }
+
+ for (int i = 0; i < sampleCount; i++) {
+ // Read wheel positions and deltas from each module
+ SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];
+ for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) {
+ modulePositions[moduleIndex] = positions[moduleIndex][i];
+ }
+ // Use the real gyro angle
+ rawGyroRotation = gyroInputs.odometryYawPositions[i];
+ // Apply update
+ poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions);
+ }
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Odometry/module poses", modulePoses.getModulePoses());
+ }
+ updateOdometryVision();
+
+ field.setRobotPose(getPose());
+ }
+
+ // DRIVE
+ /**
+ * Method to drive the robot using joystick info.
+ *
+ * @param xSpeed speed of the robot in the x direction (forward) in m/s
+ * @param ySpeed speed of the robot in the y direction (sideways) in m/s
+ * @param rot angular rate of the robot in rad/s
+ * @param fieldRelative whether the provided x and y speeds are relative to the
+ * field
+ * @param isOpenLoop whether to use velocity control for the drive motors
+ */
+ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean isOpenLoop) {
+ // rot = headingControl(rot, xSpeed, ySpeed);
+ ChassisSpeeds speeds = ChassisSpeeds.discretize(xSpeed, ySpeed, rot, Constants.LOOP_TIME);
+ if (fieldRelative) {
+ speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw());
+ }
+ setChassisSpeeds(speeds, isOpenLoop);
+ }
+
+ /**
+ * Drives the robot using the provided x speed, y speed, and positional heading.
+ *
+ * @param xSpeed speed of the robot in the x direction (forward)
+ * @param ySpeed speed of the robot in the y direction (sideways)
+ * @param heading target heading of the robot in radians
+ * @param fieldRelative whether the provided x and y speeds are relative to the
+ * field
+ */
+ public void driveHeading(double xSpeed, double ySpeed, double heading, boolean fieldRelative) {
+ double rot = rotationController.calculate(getYaw().getRadians(), heading);
+ ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
+ if (fieldRelative) {
+ speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw());
+ }
+ setChassisSpeeds(speeds, false);
+ }
+
+ /**
+ * Runs the PID controllers with the provided x, y, and rot values. Then, calls
+ * {@link #drive(double, double, double, boolean, boolean)} using the PID
+ * outputs.
+ * This is based on the odometry of the chassis.
+ *
+ * @param x the position to move to in the x, in meters
+ * @param y the position to move to in the y, in meters
+ * @param rot the angle to move to, in radians
+ */
+ public void driveWithPID(double x, double y, double rot) {
+ Pose2d pose = getPose();
+ double xSpeed = xController.calculate(pose.getX(), x);
+ double ySpeed = yController.calculate(pose.getY(), y);
+ double rotRadians = rotationController.calculate(pose.getRotation().getRadians(), rot);
+ drive(xSpeed, ySpeed, rotRadians, true, false);
+ }
+
+ /**
+ * Updates odometry using vision
+ */
+ public void updateOdometryVision() {
+ // Start the timer if it hasn't started yet
+ visionEnableTimer.start();
+
+ // Update the swerve module poses
+ modulePoses.update();
+
+ if (modulePoses.slipped()) {
+ slipped = true;
+ }
+
+ Pose2d pose2 = getPose();
+
+ // Even if vision is disabled, it should still update inputs
+ // This prevents it from storing a lot of unread results, and it could be useful
+ // for replays
+ if (vision != null) {
+ vision.updateInputs();
+ }
+
+ if (VisionConstants.ENABLED) {
+ if (vision != null && visionEnabled && visionEnableTimer.hasElapsed(5)) {
+ vision.updateOdometry(poseEstimator, time -> getPoseAt(time).getRotation().getRadians(), slipped);
+
+ if (vision.canSeeTag()) {
+ slipped = false;
+ modulePoses.reset();
+
+ double currentGyroYaw = gyroInputs.yawPosition.getRadians();
+
+ // to compare bias
+ ArrayList<EstimatedRobotPose> visionPoses = vision.getEstimatedPoses(getPose());
+
+ for (EstimatedRobotPose visionPose : visionPoses) {
+ if (visionPose.estimatedPose != null && visionPose.timestampSeconds > 0) {
+ double visionYaw = visionPose.estimatedPose.getRotation().getZ();
+
+ // gets at vision timestamp, not current gyro yaw
+ double gyroYawAtTimestamp = getGyroYawAtTimestamp(visionPose.timestampSeconds);
+
+ if (!Double.isNaN(gyroYawAtTimestamp)) {
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("GyroYaw", Math.toDegrees(gyroYawAtTimestamp));
+ Logger.recordOutput("VisionYaw", Math.toDegrees(visionYaw));
+ }
+ // use weighted observation
+ gyroBiasEstimator.addObservation(visionYaw, gyroYawAtTimestamp, 1.0);
+ }
+ }
+ }
+
+ // check if we have enough samples
+ if (gyroBiasEstimator.getSampleCount() >= GyroBiasConstants.MIN_SAMPLES) {
+ double fullBias = gyroBiasEstimator.getAndResetBias();
+ double bias = gyroBiasEstimator.applyPartialCorrection(fullBias);
+
+ if (Math.abs(bias) > GyroBiasConstants.MIN_CORRECTION_RAD) {
+ gyroIO.setYaw(new Rotation2d(currentGyroYaw + bias));
+ }
+ }
+ }
+ }
+ }
+
+ Pose2d pose3 = getPose();
+
+ // Reset the pose to a position on the field if it is too far off the field
+ // This uses nearField() instead of onField() so we don't reset the odometry
+ // when the wheels slip near the edge of the field
+ // This is meant for poses that are caused by errors
+ if (!Vision.nearField(prevPose)) {
+ // If the pose at the beginning of the method is off the field, reset to a
+ // position in the middle of the field
+ // Use the rotation of the pose after updating odometry so the yaw is right
+ prevPose = new Pose2d(FieldConstants.field.getFieldLength() / 2, FieldConstants.field.getFieldWidth() / 2,
+ pose2.getRotation());
+ resetOdometry(prevPose);
+ } else if (!Vision.nearField(pose2)) {
+ // if the drivetrain pose is off the field, reset our odometry to the pose
+ // before(this is the right pose)
+ // Keep the rotation from pose2 so yaw is correct for driver
+ prevPose = new Pose2d(prevPose.getTranslation(), pose2.getRotation());
+ resetOdometry(prevPose);
+ } else if (!Vision.nearField(pose3)) {
+ // if our vision+drivetrain odometry isn't near the field, reset our odometry to
+ // the pose before(this is the right pose)
+ resetOdometry(pose2);
+ prevPose = pose2;
+ } else {
+ // Set the previous pose to the current pose if we need to return to that
+ prevPose = pose3;
+ }
+
+ // if (Robot.isSimulation()) {
+ // pigeon.getSimState().addYaw(
+ // +Units.radiansToDegrees(currentSetpoint.chassisSpeeds().omegaRadiansPerSecond
+ // * Constants.LOOP_TIME));
+ // }
+ }
+
+ /**
+ * Stops all swerve modules.
+ */
+ public void stop() {
+ Arrays.stream(modules).forEach(Module::stop);
+ }
+
+ // GETTERS AND SETTERS
+
+ private boolean trenchAssist = false;
+ private boolean trenchAlign = false;
+
+ public boolean getTrenchAssist() {
+ return trenchAssist;
+ }
+
+ public boolean getTrenchAlign() {
+ return trenchAlign;
+ }
+
+ public void setTrenchAssist(boolean target) {
+ trenchAssist = target;
+ }
+
+ public void setTrenchAlign(boolean target) {
+ trenchAlign = target;
+ }
+
+ // for current limit setting (brownout protection)
+ public void applyNewModuleCurrents(
+ double steerCurrentStator, double steerCurrentSupply,
+ double driveCurrentStator, double driveCurrentSupply) {
+ for (Module module : modules) { // iterate over our modules
+ module.setNewCurrentLimit(steerCurrentStator, steerCurrentSupply, driveCurrentStator, driveCurrentSupply);
+ }
+ }
+
+ public double getSubsystemStatorCurrent() {
+ double sum = 0;
+ for (Module module : modules) {
+ sum += module.getModuleStatorCurrent();
+ }
+ return sum;
+ }
+
+ public double getSubsystemSupplyCurrent() {
+ double sum = 0;
+ for (Module module : modules) {
+ sum += module.getModuleSupplyCurrent();
+ }
+ return sum;
+ }
+
+ /**
+ * Sets the desired states for all swerve modules.
+ *
+ * @param swerveModuleStates an array of module states to set swerve modules to.
+ * Order of the array matters here!
+ */
+ public void setModuleStates(SwerveModuleState[] swerveModuleStates, boolean isOpenLoop) {
+ // makes sure speeds of modules don't exceed maximum allowed
+ SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, DriveConstants.MAX_SPEED);
+
+ for (int i = 0; i < 4; i++) {
+ modules[i].setDesiredState(swerveModuleStates[i], isOpenLoop);
+ }
+ }
+
+ /**
+ * Sets the chassis speeds of the robot.
+ *
+ * @param chassisSpeeds the target chassis speeds
+ * @param isOpenLoop if open loop control should be used for the drive
+ * velocity
+ */
+ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds, boolean isOpenLoop) {
+
+ if (DriveConstants.USE_ACTUAL_SPEED) {
+ SwerveSetpoint currentState = new SwerveSetpoint(getChassisSpeeds(), getModuleStates());
+ currentSetpoint = setpointGenerator.generateSetpoint(
+ DriveConstants.MODULE_LIMITS,
+ centerOfMassHeight,
+ currentState, chassisSpeeds,
+ Constants.LOOP_TIME);
+ } else {
+ currentSetpoint = setpointGenerator.generateSetpoint(
+ DriveConstants.MODULE_LIMITS,
+ centerOfMassHeight,
+ currentSetpoint, chassisSpeeds,
+ Constants.LOOP_TIME);
+ }
+
+ SwerveModuleState[] swerveModuleStates = currentSetpoint.moduleStates();
+ setModuleStates(swerveModuleStates, isOpenLoop);
+ }
+
+ public void setDriveVoltages(Voltage voltage) {
+ for (int i = 0; i < modules.length; i++) {
+ modules[i].setDriveVoltage(voltage);
+ }
+ }
+
+ public void setAngleMotors(Rotation2d[] angles) {
+ for (int i = 0; i < modules.length; i++) {
+ modules[i].setAngle(angles[i]);
+ }
+ }
+
+ /**
+ * Returns the angular rate from the pigeon.
+ *
+ * @param id 0 for x, 1 for y, 2 for z
+ * @return the rate in rads/s from the pigeon
+ */
+ public double getAngularRate(int id) {
+ // double speed = 0;
+ // switch(id){
+ // case 0:
+ // speed = gyroInputs..getAngularVelocityXWorld().getValueAsDouble();
+ // break;
+ // case 1:
+ // speed = pigeon.getAngularVelocityYWorld().getValueAsDouble();
+ // break;
+ // case 2:
+ // speed = pigeon.getAngularVelocityZWorld().getValueAsDouble();
+ // break;
+ // }
+ // outputs in deg/s, so convert to rad/s
+ return gyroInputs.yawVelocityRadPerSec;
+ }
+
+ /**
+ * Updates and returns the array of SwerveModulePositions, which store the
+ * distance travleled by the drive and the steer angle.
+ *
+ * @return An array of all swerve module positions
+ */
+ private SwerveModulePosition[] updateModulePositions() {
+ return modulePositions = Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new);
+ }
+
+ /**
+ * Gets an array of SwerveModulePositions, which store the distance travleled by
+ * the drive and the steer angle.
+ *
+ * @return An array of all swerve module positions
+ */
+ public SwerveModulePosition[] getModulePositions() {
+ return modulePositions;
+ }
+
+ /**
+ * Enables or disables the state deadband for all swerve modules.
+ * The state deadband determines if the robot will stop drive and steer motors
+ * when inputted drive velocity is low.
+ * It should be enabled for all regular driving, to prevent releasing the
+ * controls from setting the angles.
+ */
+ public void setStateDeadband(boolean stateDeadBand) {
+ Arrays.stream(modules).forEach(module -> module.setStateDeadband(stateDeadBand));
+ }
+
+ public void setOptimized(boolean optimized) {
+ Arrays.stream(modules).forEach(module -> module.setOptimize(optimized));
+ }
+
+ public void setVisionEnabled(boolean enabled) {
+ visionEnabled = enabled;
+ }
+
+ public void setIsAlign(boolean isAlign) {
+ this.isAlign = isAlign;
+ }
+
+ public boolean getIsAlign() {
+ return isAlign;
+ }
+
+ /**
+ * Calculates chassis speed of drivetrain using the current SwerveModuleStates
+ *
+ * @return ChassisSpeeds object
+ * This is often used as an input for other methods
+ */
+ public ChassisSpeeds getChassisSpeeds() {
+ return DriveConstants.KINEMATICS.toChassisSpeeds(getModuleStates());
+ }
+
+ /**
+ * Gets the state of each module
+ *
+ * @return An array of 4 SwerveModuleStates
+ */
+ public SwerveModuleState[] getModuleStates() {
+ return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new);
+ }
+
+ public SwerveSetpoint getCurrSetpoint() {
+ return currentSetpoint;
+ }
+
+ /**
+ * @return the yaw of the robot, aka heading, the direction it is facing
+ */
+ public Rotation2d getYaw() {
+ return getPose().getRotation();
+ }
+
+ /**
+ * @return an array of modules
+ */
+ public Module[] getModules() {
+ return modules;
+ }
+
+ /**
+ * gets gyro yaw at a specific timestamp with interpolation
+ * this is used for timestamp-synchronized gyro/vision comparison.
+ *
+ * @param timestampSeconds the timestamp to get the gyro yaw at
+ * @return the gyro yaw in radians, or Double.NaN if no valid data
+ */
+ private double
+ getGyroYawAtTimestamp(double timestampSeconds) {
+ return getPose().getRotation().getRadians();
+ }
+
+ /**
+ * Resets the yaw of the robot.
+ *
+ * @param rotation the new yaw angle as Rotation2d
+ */
+ public void setYaw(Rotation2d rotation) {
+ resetOdometry(new Pose2d(getPose().getTranslation(), rotation));
+ }
+
+ /**
+ * Resets the odometry to the given pose.
+ *
+ * @param pose the pose to reset to.
+ */
+ public void resetOdometry(Pose2d pose) {
+ // NOTE: must use pigeon yaw for odometer!
+ currentHeading = pose.getRotation().getRadians();
+ poseEstimator.resetPosition(gyroInputs.yawPosition, getModulePositions(), pose);
+ modulePoses.reset();
+ }
+
+ /**
+ * @return the pose of the robot as estimated by the odometry
+ */
+ @AutoLogOutput(key = "Odometry/Robot")
+ public Pose2d getPose() {
+ return poseEstimator.getEstimatedPosition();
+ }
+
+ /**
+ * Sets the angle to align to
+ *
+ * @param newAngle The new angle in radians, can be set to null
+ */
+ public void setAlignAngle(Double newAngle) {
+ alignAngle = newAngle;
+ }
+
+ /**
+ * Returns whether or not the robot is at the input align angle
+ *
+ * @return true if it within tolerance the align angle, false otherwise
+ */
+ public boolean atAlignAngle() {
+ if (alignAngle == null) {
+ return false;
+ }
+ double diff = Math.abs(alignAngle - getYaw().getRadians());
+ return diff < DriveConstants.HEADING_TOLERANCE || diff > 2 * Math.PI - DriveConstants.HEADING_TOLERANCE;
+ }
+
+ /**
+ * Gets the angle to align to
+ *
+ * @return The angle in radians
+ */
+ public double getAlignAngle() {
+ if (alignAngle != null) {
+ return alignAngle;
+ }
+ return 0;
+ }
+
+ /**
+ * Sets vision to only use certain April tags
+ *
+ * @param ids An array of the tags to only use
+ */
+ public void onlyUseTags(int[] ids) {
+ if (vision != null) {
+ vision.onlyUse(ids);
+ }
+ }
+
+ /**
+ * Returns if vision has seen an April tag in the last frame
+ *
+ * @return true if vision saw a tag last frame or if vision is disabled
+ */
+ public boolean canSeeTag() {
+ // if no vision system, then return true
+ if (vision == null)
+ return true;
+
+ return vision.canSeeTag() || !visionEnabled || !VisionConstants.ENABLED;
+ }
+
+ /**
+ * Gets the pose at a previous time
+ *
+ * @param timestamp The timestamp of the pose to get
+ * @return The pose, null if there are no poses yet, or the current pose if
+ * timestamp < 0
+ */
+ public Pose2d getPoseAt(double timestamp) {
+ if (timestamp < 0) {
+ return getPose();
+ }
+ Optional<Pose2d> pose = poseEstimator.sampleAt(timestamp);
+ if (pose.isPresent()) {
+ return pose.get();
+ } else {
+ return null;
+ }
+ }
+
+ /**
+ * Uses pigeon and rotational input to return a rotation that accounts for drift
+ *
+ * @return A rotation
+ */
+ public double headingControl(double rot, double xSpeed, double ySpeed) {
+ if ((!EqualsUtil.epsilonEquals(getAngularRate(0), 0, 0.0004)
+ && EqualsUtil.epsilonEquals(Math.hypot(xSpeed, ySpeed), 0, 0.1))
+ || !EqualsUtil.epsilonEquals(rot, 0, 0.0004)) {
+ drive_turning = true;
+ currentHeading = getYaw().getRadians();
+ } else {
+ drive_turning = false;
+ }
+ if (!drive_turning) {
+ rotationController.setSetpoint(currentHeading);
+ double output = rotationController.calculate(getYaw().getRadians());
+ rot = Math.abs(output) > Math.abs(rot) ? output : rot;
+ }
+ return rot;
+ }
+
+ /**
+ * Resets the swerve modules from the absolute encoders
+ */
+ public void resetModulesToAbsolute() {
+ Arrays.stream(modules).forEach(Module::resetToAbsolute);
+ }
+
+ // getters for the PID Controllers
+ public PIDController getXController() {
+ return xController;
+ }
+
+ public PIDController getYController() {
+ return yController;
+ }
+
+ public PIDController getRotationController() {
+ return rotationController;
+ }
+
+ /**
+ * Set the desired pose to drive to
+ * This will enable driver assist to go to the pose
+ *
+ * @param supplier The supplier for the desired pose, use ()->null to not use a
+ * desired pose
+ */
+ public void setDesiredPose(Supplier<Pose2d> supplier) {
+ desiredPoSupplier = supplier;
+ }
+
+ /**
+ * Set the desired pose to drive to
+ * This will enable driver assist to go to the pose
+ *
+ * @param pose The Pose2d to drive to
+ */
+ public void setDesiredPose(Pose2d pose) {
+ setDesiredPose(() -> pose);
+ }
+
+ /**
+ * Gets the current desired pose, or null if there is no desired pose
+ *
+ * @return The Pose2d if it exists, null otherwise
+ */
+ public Pose2d getDesiredPose() {
+ return desiredPoSupplier.get();
+ }
+
+ public boolean atSetpoint() {
+ Pose2d pose = getDesiredPose();
+ return pose != null && getPose().getTranslation().getDistance(pose.getTranslation()) < 0.025;
+ }
+
+ public SwerveModulePose getSwerveModulePose() {
+ return modulePoses;
+ }
+
+ public double getAcceleration() {
+ double accelX = gyroInputs.accelerationX;
+ double accelY = gyroInputs.accelerationY;
+
+ double angularVelocity = getAngularRate(3);
+ double angularAccel = (angularVelocity - previousAngularVelocity) / Constants.LOOP_TIME;
+ previousAngularVelocity = angularVelocity;
+
+ double pigeonOffsetX = 0.082677;
+ double pigeonOffsetY = 0.030603444;
+
+ double totalX = accelX + Math.pow(angularVelocity, 2) * pigeonOffsetX + angularAccel * pigeonOffsetY;
+ double totalY = accelY + Math.pow(angularVelocity, 2) * pigeonOffsetY - angularAccel * pigeonOffsetX;
+
+ return Math.hypot(totalX, totalY);
+ }
+
+ @AutoLogOutput(key = "Drivetrain/AccelerationFaults")
+ public boolean accelerationOverMax() {
+ return getAcceleration() > DriveConstants.MAX_LINEAR_ACCEL;
+ }
+
+ public void setCenterOfMass(double height) {
+ centerOfMassHeight = height;
+ }
+
+ public void alignWheels() {
+ SwerveModuleState state = new SwerveModuleState(0, new Rotation2d(0));
+ setModuleStates(new SwerveModuleState[] {
+ state, state, state, state
+ }, false);
+ }
+}
--- /dev/null
+// Copyright 2021-2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drivetrain;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+
+import com.ctre.phoenix6.StatusSignal;
+
+public interface GyroIO {
+ @AutoLog
+ public static class GyroIOInputs {
+ public boolean connected = false;
+ public Rotation2d yawPosition = new Rotation2d();
+ public double yawVelocityRadPerSec = 0.0;
+ public double accelerationX = 0.0;
+ public double accelerationY = 0.0;
+ public double[] odometryYawTimestamps = new double[] {};
+ public Rotation2d[] odometryYawPositions = new Rotation2d[] {};
+ }
+
+ public default void updateInputs(GyroIOInputs inputs) {}
+
+ /** returns the yaw status signal for time-synced odometry. */
+ public StatusSignal<?> getYawSignal();
+
+ /**
+ * set the yaw angle of the gyro.
+ *
+ * @param rotation the new yaw angle
+ */
+ public default void setYaw(Rotation2d rotation) {}
+}
--- /dev/null
+// Copyright 2021-2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drivetrain;
+
+import java.util.Queue;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.Pigeon2Configuration;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.LinearAcceleration;
+import frc.robot.constants.IdConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.util.PhoenixOdometryThread;
+
+/** IO implementation for Pigeon 2. */
+public class GyroIOPigeon2 implements GyroIO {
+ private final Pigeon2 pigeon =
+ new Pigeon2(
+ IdConstants.PIGEON,
+ DriveConstants.PIGEON_CAN);
+ private final StatusSignal<Angle> yaw = pigeon.getYaw();
+ private final StatusSignal<LinearAcceleration> accelrationx = pigeon.getAccelerationX();
+ private final StatusSignal<LinearAcceleration> accelrationy = pigeon.getAccelerationY();
+ private final Queue<Double> yawPositionQueue;
+ private final Queue<Double> yawTimestampQueue;
+ private final StatusSignal<AngularVelocity> yawVelocity = pigeon.getAngularVelocityZWorld();
+ private final Pigeon2Configuration config = new Pigeon2Configuration();
+
+ public GyroIOPigeon2() {
+ config.MountPose.MountPoseRoll = DriveConstants.GYRO_MOUNT_POSE_ROLL;
+ pigeon.getConfigurator().apply(config);
+ pigeon.getConfigurator().setYaw(0.0);
+ yaw.setUpdateFrequency(250);
+ yawVelocity.setUpdateFrequency(50.0);
+ pigeon.optimizeBusUtilization();
+ yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
+ yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(pigeon.getYaw());
+ }
+
+ @Override
+ public void updateInputs(GyroIOInputs inputs) {
+ inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity, accelrationx, accelrationy).equals(StatusCode.OK);
+ inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
+ inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());
+ inputs.accelerationX = accelrationx.getValueAsDouble();
+ inputs.accelerationY = accelrationy.getValueAsDouble();
+
+ inputs.odometryYawTimestamps =
+ yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray();
+ inputs.odometryYawPositions =
+ yawPositionQueue.stream()
+ .map((Double value) -> Rotation2d.fromDegrees(value))
+ .toArray(Rotation2d[]::new);
+ yawTimestampQueue.clear();
+ yawPositionQueue.clear();
+ }
+
+ @Override
+ public StatusSignal<Angle> getYawSignal() {
+ return yaw;
+ }
+
+ @Override
+ public void setYaw(Rotation2d rotation) {
+ pigeon.getConfigurator().setYaw(rotation.getDegrees());
+ }
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.subsystems.drivetrain;
+
+import java.util.Queue;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MagnetSensorConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.controls.PositionDutyCycle;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Angle;
+import frc.robot.constants.Constants;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.constants.swerve.ModuleConstants;
+import frc.robot.constants.swerve.ModuleType;
+import frc.robot.util.PhoenixOdometryThread;
+import lib.CTREModuleState;
+
+
+public class Module implements ModuleIO{
+ private final ModuleType type;
+
+ // Degrees
+ private final double angleOffset;
+
+ private final TalonFX angleMotor;
+ private final TalonFX driveMotor;
+ private final CANcoder CANcoder;
+ private SwerveModuleState desiredState;
+
+ protected boolean stateDeadband = true;
+
+ private boolean optimizeStates = true;
+
+ // Inputs from drive motor
+ private final StatusSignal<Angle> drivePosition;
+ private final StatusSignal<AngularVelocity> driveVelocity;
+ private final StatusSignal<Voltage> driveAppliedVolts;
+ private final StatusSignal<Current> driveCurrent;
+
+ // Inputs from turn motor
+ private final StatusSignal<Angle> turnAbsolutePosition;
+ private final StatusSignal<Angle> turnPosition;
+ private final StatusSignal<AngularVelocity> turnVelocity;
+ private final StatusSignal<Voltage> turnAppliedVolts;
+ private final StatusSignal<Current> turnCurrent;
+
+ // Timestamp inputs from Phoenix thread
+ protected final Queue<Double> timestampQueue;
+ protected final Queue<Double> drivePositionQueue;
+ protected final Queue<Double> turnPositionQueue;
+
+ private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
+
+ // Connection debouncers
+ private final Debouncer driveConnectedDebounce = new Debouncer(0.5);
+ private final Debouncer turnConnectedDebounce = new Debouncer(0.5);
+ private final Debouncer turnEncoderConnectedDebounce = new Debouncer(0.5);
+
+ private final Alert driveDisconnectedAlert;
+ private final Alert turnDisconnectedAlert;
+ private final Alert turnEncoderDisconnectedAlert;
+
+ protected final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
+
+ private ModuleConstants moduleConstants;
+ private final MotionMagicVelocityVoltage velocityRequest =
+ new MotionMagicVelocityVoltage(0.0).withUpdateFreqHz(0);
+
+
+ public Module(ModuleConstants moduleConstants) {
+ this.moduleConstants = moduleConstants;
+
+ type = moduleConstants.getType();
+ angleOffset = moduleConstants.getSteerOffset();
+
+ /* Angle Encoder Config */
+ CANcoder = new CANcoder(moduleConstants.getEncoderPort(), DriveConstants.STEER_ENCODER_CAN);
+ /* Angle Motor Config */
+ angleMotor = new TalonFX(moduleConstants.getSteerPort(), DriveConstants.STEER_ENCODER_CAN);
+ driveMotor = new TalonFX(moduleConstants.getDrivePort(), DriveConstants.DRIVE_MOTOR_CAN);
+ // Create drive status signals
+ drivePosition = driveMotor.getPosition();
+ driveVelocity = driveMotor.getVelocity();
+ driveAppliedVolts = driveMotor.getMotorVoltage();
+ driveCurrent = driveMotor.getStatorCurrent();
+
+ // Create turn status signals
+ turnAbsolutePosition = CANcoder.getAbsolutePosition();
+ turnPosition = angleMotor.getPosition();
+ turnVelocity = angleMotor.getVelocity();
+ turnAppliedVolts = angleMotor.getMotorVoltage();
+ turnCurrent = angleMotor.getStatorCurrent();
+
+ // Create timestamp queue
+ timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
+ drivePositionQueue =
+ PhoenixOdometryThread.getInstance().registerSignal(driveMotor.getPosition());
+ turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(angleMotor.getPosition());
+ updateInputs();
+
+ configCANcoder();
+ configAngleMotor();
+ configDriveMotor();
+
+ driveDisconnectedAlert =
+ new Alert(
+ "Disconnected drive motor on module " + Integer.toString(moduleConstants.ordinal()) + ".",
+ AlertType.kError);
+ turnDisconnectedAlert =
+ new Alert(
+ "Disconnected turn motor on module " + Integer.toString(moduleConstants.ordinal()) + ".", AlertType.kError);
+ turnEncoderDisconnectedAlert =
+ new Alert(
+ "Disconnected turn encoder on module " + Integer.toString(moduleConstants.ordinal()) + ".",
+ AlertType.kError);
+
+
+ // Configure periodic frames
+ BaseStatusSignal.setUpdateFrequencyForAll(
+ 250, drivePosition, turnPosition);
+ BaseStatusSignal.setUpdateFrequencyForAll(
+ 50.0,
+ driveVelocity,
+ driveAppliedVolts,
+ driveCurrent,
+ turnAbsolutePosition,
+ turnVelocity,
+ turnAppliedVolts,
+ turnCurrent);
+
+ setDesiredState(new SwerveModuleState(0, getAngle()), false);
+ }
+
+ public void close() {
+ angleMotor.close();
+ driveMotor.close();
+ CANcoder.close();
+ }
+
+ @Override
+ public void updateInputs() {
+ // Refresh all signals
+ var driveStatus =
+ BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);
+ var turnStatus =
+ BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent);
+ var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition);
+
+ // Update drive inputs
+ inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK());
+ inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()/DriveConstants.DRIVE_GEAR_RATIO);
+ inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()/DriveConstants.DRIVE_GEAR_RATIO);
+ inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
+ inputs.driveCurrentAmps = driveCurrent.getValueAsDouble();
+
+ // Update turn inputs
+ inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK());
+ inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK());
+ inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble());
+ inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()/DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()/DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble();
+ inputs.turnCurrentAmps = turnCurrent.getValueAsDouble();
+
+ // Update encoder inputs
+ inputs.encoderOffset = Units.rotationsToDegrees(CANcoder.getAbsolutePosition().getValueAsDouble());
+
+ // Update odometry inputs
+ inputs.odometryTimestamps =
+ timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
+ inputs.odometryDrivePositionsRad =
+ drivePositionQueue.stream()
+ .mapToDouble((Double value) -> Units.rotationsToRadians(value))
+ .toArray();
+ inputs.odometryTurnPositions =
+ turnPositionQueue.stream()
+ .map((Double value) -> Rotation2d.fromRotations(value))
+ .toArray(Rotation2d[]::new);
+ timestampQueue.clear();
+ drivePositionQueue.clear();
+ turnPositionQueue.clear();
+
+ inputs.driveStator = driveMotor.getStatorCurrent().getValueAsDouble();
+ inputs.driveSupply = driveMotor.getSupplyCurrent().getValueAsDouble();
+ inputs.steerStator = angleMotor.getStatorCurrent().getValueAsDouble();
+ inputs.steerSupply = angleMotor.getSupplyCurrent().getValueAsDouble();
+ }
+
+ public void periodic() {
+ updateInputs();
+ Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs);
+
+ // Calculate positions for odometry
+ int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together
+ odometryPositions = new SwerveModulePosition[sampleCount];
+ for (int i = 0; i < sampleCount; i++) {
+ double positionMeters = inputs.odometryDrivePositionsRad[i]/DriveConstants.DRIVE_GEAR_RATIO * DriveConstants.WHEEL_RADIUS;
+ Rotation2d angle = inputs.odometryTurnPositions[i].div(DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
+ }
+ // Update alerts
+ driveDisconnectedAlert.set(!inputs.driveConnected);
+ turnDisconnectedAlert.set(!inputs.turnConnected);
+ turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Angle "+ moduleConstants.ordinal(), MathUtil.inputModulus(getAngle().getDegrees(), 0, 360));
+ }
+ }
+
+ public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) {
+ // Separate if here and in setAngle() to avoid warning
+ if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){
+ /*
+ * This is a custom optimize function, since default WPILib optimize assumes
+ * continuous controller which CTRE and Rev onboard is not
+ */
+ desiredState = optimizeStates ? CTREModuleState.optimize(wantedState, getState().angle) : wantedState;
+ }else{
+ desiredState = wantedState;
+ }
+ setAngle();
+ setSpeed(isOpenLoop);
+ }
+
+ public void setSpeed(boolean isOpenLoop) {
+ if(desiredState == null){
+ return;
+ }
+ if (isOpenLoop) {
+ double percentOutput = desiredState.speedMetersPerSecond / DriveConstants.MAX_SPEED;
+ driveMotor.setControl(new DutyCycleOut(percentOutput));
+ } else {
+ double velocity = desiredState.speedMetersPerSecond/DriveConstants.WHEEL_RADIUS/2/Math.PI*DriveConstants.DRIVE_GEAR_RATIO;
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity);
+ }
+
+ double feedforward = velocity * moduleConstants.getDriveV();
+ driveMotor.setControl(
+ velocityRequest
+ .withVelocity(velocity)
+ .withFeedForward(feedforward));
+ }
+ }
+
+ private void setAngle() {
+ if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){
+ // Prevent rotating module if desired speed < 1%. Prevents jittering and unnecessary movement.
+ if (stateDeadband && (Math.abs(desiredState.speedMetersPerSecond) <= (DriveConstants.MAX_SPEED * 0.01))) {
+ stop();
+ return;
+ }
+ }
+ if(desiredState == null){
+ return;
+ }
+ angleMotor.setControl(new PositionDutyCycle(desiredState.angle.getRotations()*DriveConstants.MODULE_CONSTANTS.angleGearRatio));
+ }
+
+ public void setDriveVoltage(Voltage voltage){
+ driveMotor.setControl(new VoltageOut(voltage.baseUnitMagnitude()));
+ }
+ public void setAngle(Rotation2d angle){
+ angleMotor.setControl(new PositionDutyCycle(angle.getRotations()*DriveConstants.MODULE_CONSTANTS.angleGearRatio));
+ }
+
+ public void setOptimize(boolean enable) {
+ optimizeStates = enable;
+ }
+
+ public byte getModuleIndex() {
+ return type.id;
+ }
+
+ public Rotation2d getAngle() {
+ return inputs.turnPosition;
+ }
+
+ public Rotation2d getCANcoder() {
+ return inputs.turnAbsolutePosition;
+ }
+
+ public void resetToAbsolute() {
+ // Sensor ticks
+ double absolutePosition = getCANcoder().getRotations() - Units.degreesToRotations(angleOffset);
+ angleMotor.setPosition(absolutePosition*DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ }
+
+ private void configCANcoder() {
+ CANcoder.getConfigurator().apply(new CANcoderConfiguration());
+ CANcoder.getConfigurator().apply(new MagnetSensorConfigs().withAbsoluteSensorDiscontinuityPoint(1).
+ withSensorDirection(DriveConstants.MODULE_CONSTANTS.canCoderInvert?SensorDirectionValue.Clockwise_Positive:SensorDirectionValue.CounterClockwise_Positive));
+ }
+
+ private void configAngleMotor() {
+ angleMotor.getConfigurator().apply(new TalonFXConfiguration());
+
+ CurrentLimitsConfigs config = new CurrentLimitsConfigs();
+ config.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT;
+ config.SupplyCurrentLimit = DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT;
+ config.SupplyCurrentLowerLimit = DriveConstants.STEER_PEAK_CURRENT_LIMIT;
+ config.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
+ angleMotor.getConfigurator().apply(config);
+ angleMotor.getConfigurator().apply(new Slot0Configs()
+ .withKP(DriveConstants.MODULE_CONSTANTS.angleKP)
+ .withKI(DriveConstants.MODULE_CONSTANTS.angleKI)
+ .withKD(DriveConstants.MODULE_CONSTANTS.angleKD));
+ angleMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_STEER_MOTOR));
+ angleMotor.setNeutralMode(DriveConstants.STEER_NEUTRAL_MODE);
+ angleMotor.setPosition(0);
+
+ // optimize bus utilization for angle motor
+ angleMotor.optimizeBusUtilization();
+
+ resetToAbsolute();
+ }
+
+ /**
+ * @return Speed in RPM
+ */
+ public double getDriveVelocity() {
+ return inputs.driveVelocityRadPerSec*60/DriveConstants.MODULE_CONSTANTS.driveGearRatio/2/Math.PI;
+ }
+
+ public double getDriveVoltage(){
+ return inputs.driveAppliedVolts;
+ }
+
+ public double getDriveStatorCurrent(){
+ return inputs.driveCurrentAmps;
+ }
+
+ public double getModuleStatorCurrent() {
+ return inputs.steerStator + inputs.driveStator;
+ }
+
+ public double getModuleSupplyCurrent() {
+ return inputs.steerSupply + inputs.driveSupply;
+ }
+
+ // I took the config things straight from this file
+ public void setNewCurrentLimit(double currentSteerStator, double currentSteerSupply, double currentDriveStator, double currentDriveSupply) {
+ CurrentLimitsConfigs steerConfig = new CurrentLimitsConfigs();
+ // steer
+ steerConfig.SupplyCurrentLimitEnable = true;
+ steerConfig.StatorCurrentLimitEnable = true;
+ steerConfig.StatorCurrentLimit = currentSteerSupply;
+ steerConfig.SupplyCurrentLimit = currentSteerSupply;
+ steerConfig.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
+ angleMotor.getConfigurator().apply(steerConfig); // apply
+
+ // drive
+ CurrentLimitsConfigs driveConfig = new CurrentLimitsConfigs();
+ driveConfig.SupplyCurrentLimitEnable = true;
+ driveConfig.StatorCurrentLimitEnable = true;
+ driveConfig.SupplyCurrentLimit = currentDriveSupply;
+ driveConfig.StatorCurrentLimit = currentDriveStator;
+ driveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
+ driveMotor.getConfigurator().apply(driveConfig); // apply
+ }
+
+ private void configDriveMotor() {
+ var talonFXConfigs = new TalonFXConfiguration();
+ // set Motion Magic settings
+ var motionMagicConfigs = talonFXConfigs.MotionMagic;
+ motionMagicConfigs.MotionMagicCruiseVelocity = DriveConstants.MAX_SPEED/DriveConstants.WHEEL_CIRCUMFERENCE * DriveConstants.DRIVE_GEAR_RATIO;
+ motionMagicConfigs.MotionMagicAcceleration = DriveConstants.MAX_DRIVE_ACCEL/DriveConstants.WHEEL_CIRCUMFERENCE * DriveConstants.DRIVE_GEAR_RATIO;
+ var slot0Configs = talonFXConfigs.Slot0;
+ slot0Configs.kS = 0; // Add 0.25 V output to overcome static friction
+ slot0Configs.kV = 0.11; // A velocity target of 1 rps results in 0.12 V output
+ slot0Configs.kA = 0.006; // An acceleration of 1 rps/s requires 0.01 V output
+ slot0Configs.kP = moduleConstants.getDriveP(); // A position error of 2.5 rotations results in 12 V output
+ slot0Configs.kI = moduleConstants.getDriveI(); // no output for integrated error
+ slot0Configs.kD = moduleConstants.getDriveD(); // A velocity error of 1 rps results in 0.1 V output
+ driveMotor.getConfigurator().apply(talonFXConfigs);
+ CurrentLimitsConfigs config = new CurrentLimitsConfigs();
+ config.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+ config.SupplyCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
+ config.SupplyCurrentLowerLimit = DriveConstants.DRIVE_PEAK_CURRENT_LIMIT;
+ config.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
+ config.StatorCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
+ config.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+ driveMotor.getConfigurator().apply(config);
+ driveMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_DRIVE_MOTOR));
+ driveMotor.getConfigurator().apply(new OpenLoopRampsConfigs().withDutyCycleOpenLoopRampPeriod(DriveConstants.OPEN_LOOP_RAMP));
+ driveMotor.getConfigurator().apply(new ClosedLoopRampsConfigs().withDutyCycleClosedLoopRampPeriod(DriveConstants.CLOSE_LOOP_RAMP));
+ driveMotor.setNeutralMode(DriveConstants.DRIVE_NEUTRAL_MODE);
+
+ // optimize bus utilization for drive motor
+ driveMotor.optimizeBusUtilization();
+
+ }
+
+ public SwerveModuleState getState() {
+ return new SwerveModuleState(
+ inputs.driveVelocityRadPerSec*DriveConstants.WHEEL_RADIUS,
+ getAngle());
+ }
+
+ public SwerveModulePosition getPosition() {
+ return new SwerveModulePosition(
+ inputs.drivePositionRad*DriveConstants.WHEEL_RADIUS,
+ getAngle());
+ }
+
+ public SwerveModuleState getDesiredState() {
+ return desiredState;
+ }
+
+
+ public double getDriveVelocityError() {
+ return getDesiredState().speedMetersPerSecond - getState().speedMetersPerSecond;
+ }
+
+ public void stop() {
+ driveMotor.set(0);
+ angleMotor.set(0);
+ }
+
+ public ModuleType getModuleType(){
+ return type;
+ }
+
+ public void setStateDeadband(boolean enabled) {
+ stateDeadband = enabled;
+ }
+
+ public double getDesiredVelocity() {
+ return getDesiredState().speedMetersPerSecond;
+ }
+
+ public Rotation2d getDesiredAngle() {
+ return getDesiredState().angle;
+ }
+
+ /** Returns the module positions received this cycle. */
+ public SwerveModulePosition[] getOdometryPositions() {
+ return odometryPositions;
+ }
+
+ /** Returns the timestamps of the samples received this cycle. */
+ public double[] getOdometryTimestamps() {
+ return inputs.odometryTimestamps;
+ }
+
+ /** returns the drive position status signal for time-synced odometry. */
+ public StatusSignal<Angle> getDrivePositionSignal() {
+ return drivePosition;
+ }
+
+ /** returns the turn position status signal for time-synced odometry. */
+ public StatusSignal<Angle> getTurnPositionSignal() {
+ return turnPosition;
+ }
+
+ /** returns the turn absolute position status signal for time-synced odometry. */
+ public StatusSignal<Angle> getTurnAbsolutePositionSignal() {
+ return turnAbsolutePosition;
+ }
+
+ public TalonFX[] getMotors() {
+ return new TalonFX[]{angleMotor, driveMotor};
+ }
+}
--- /dev/null
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drivetrain;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ModuleIO {
+ @AutoLog
+ public static class ModuleIOInputs {
+ public boolean driveConnected = false;
+ public double drivePositionRad = 0.0;
+ public double driveVelocityRadPerSec = 0.0;
+ public double driveAppliedVolts = 0.0;
+ public double driveCurrentAmps = 0.0;
+
+ public boolean turnConnected = false;
+ public boolean turnEncoderConnected = false;
+ public Rotation2d turnAbsolutePosition = new Rotation2d();
+ public Rotation2d turnPosition = new Rotation2d();
+ public double turnVelocityRadPerSec = 0.0;
+ public double turnAppliedVolts = 0.0;
+ public double turnCurrentAmps = 0.0;
+
+ public double[] odometryTimestamps = new double[] {};
+ public double[] odometryDrivePositionsRad = new double[] {};
+ public Rotation2d[] odometryTurnPositions = new Rotation2d[] {};
+
+ public double encoderOffset = 0.0;
+
+ // drivetrain is scary. I'm adding my own seperate logging
+ public double driveStator = 0.0;
+ public double driveSupply = 0.0;
+ public double steerStator = 0.0;
+ public double steerSupply = 0.0;
+ }
+
+ /** Updates the set of loggable inputs. */
+ public default void updateInputs() {}
+
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.subsystems.drivetrain;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.constants.Constants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.constants.swerve.ModuleConstants;
+import lib.CTREModuleState;
+
+
+/**
+ * Swerve module for drivetrain to be used inside of simulation.
+ * TODO: improve this simulation to be more realistic
+ */
+public class ModuleSim extends Module {
+
+ private double currentSteerPositionRad = 0;
+ private double currentDrivePositionMeters = 0;
+ private double currentSpeed = 0;
+
+ private SwerveModuleState desiredState;
+
+
+ protected boolean stateDeadband = true;
+
+ public ModuleSim(ModuleConstants ignored) {
+ super(ignored);
+ }
+
+ /**
+ * Updates the simulation
+ */
+ @Override
+ public void periodic() {
+ currentDrivePositionMeters += currentSpeed * Constants.LOOP_TIME;
+ super.periodic();
+ }
+
+ @Override
+ public void updateInputs(){
+ // Update drive inputs
+ inputs.driveConnected = true;
+ inputs.drivePositionRad = currentDrivePositionMeters / DriveConstants.WHEEL_RADIUS;
+ inputs.driveVelocityRadPerSec = currentSpeed / DriveConstants.WHEEL_RADIUS;
+ inputs.driveAppliedVolts = currentSpeed / DriveConstants.MAX_SPEED * Constants.ROBOT_VOLTAGE;
+ inputs.driveCurrentAmps = 0; // This simulation currently isn't good enough to calculate this
+
+ // Update turn inputs
+ inputs.turnConnected = true;
+ inputs.turnEncoderConnected = true;
+ inputs.turnAbsolutePosition = new Rotation2d(currentSteerPositionRad);
+ inputs.turnPosition = new Rotation2d(currentSteerPositionRad);
+ inputs.turnVelocityRadPerSec = 0; // Simulated modules currently teleport
+ inputs.turnAppliedVolts = 0;
+ inputs.turnCurrentAmps = 0;
+
+ // Update odometry inputs
+ // Simulate as only getting one value per frame
+ inputs.odometryTimestamps =
+ new double[]{Timer.getFPGATimestamp()};
+ inputs.odometryDrivePositionsRad =
+ new double[]{inputs.drivePositionRad*DriveConstants.DRIVE_GEAR_RATIO};
+ inputs.odometryTurnPositions =
+ new Rotation2d[]{inputs.turnPosition};
+ timestampQueue.clear();
+ drivePositionQueue.clear();
+ turnPositionQueue.clear();
+ }
+
+ /**
+ * Sets the desired state for the module.
+ *
+ * @param desiredState Desired state with speed and angle.
+ * @param isOpenLoop whether to use closed/open loop control for drive velocity
+ */
+ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) {
+ if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){
+ // If the module isn't moving, don't rotate it
+ if (Math.abs(desiredState.speedMetersPerSecond) < 0.001) {
+ currentSpeed = 0;
+ return;
+ }
+ // Optimize the reference state to avoid spinning further than 90 degrees
+ desiredState = CTREModuleState.optimize(desiredState, new Rotation2d(currentSteerPositionRad));
+ }
+
+ currentSpeed = desiredState.speedMetersPerSecond;
+ currentSteerPositionRad = desiredState.angle.getRadians();
+ }
+
+ public void resetToAbsolute() {
+ // does nothing when robot does not have a swerve drivetrain
+ }
+
+ public SwerveModuleState getDesiredState() {
+ return desiredState;
+ }
+
+ public double getDesiredVelocity() {
+ return getDesiredState().speedMetersPerSecond;
+ }
+
+ public Rotation2d getDesiredAngle() {
+ return getDesiredState().angle;
+ }
+
+ /**
+ * Sets current speed to zero
+ */
+ public void stop() {
+ currentSpeed = 0;
+ }
+
+ public SwerveModuleState getState() {
+ return new SwerveModuleState(
+ currentSpeed,
+ getAngle()
+ );
+ }
+
+ public SwerveModulePosition getPosition() {
+ return new SwerveModulePosition(
+ currentDrivePositionMeters,
+ new Rotation2d(currentSteerPositionRad)
+ );
+ }
+
+ /**
+ * Gets the simulated angle of the module.
+ */
+ public Rotation2d getAngle() {
+ return new Rotation2d(currentSteerPositionRad);
+ }
+
+ /**
+ * Sets state deadband
+ */
+ public void setStateDeadband(boolean enabled) {
+ stateDeadband = enabled;
+ }
+
+ public TalonFX getDriveMotor(){
+ return null;
+ }
+
+ public double getDriveVoltage(){
+ return 0;
+ }
+
+ public double getDriveStatorCurrent(){
+ return 0;
+ }
+
+ public double getSteerVelocity() {
+ return 0;
+ }
+ public double getDriveVelocity() {
+ return 0;
+ }
+
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.subsystems.hood;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.Constants;
+import frc.robot.constants.IdConstants;
+
+public class Hood extends SubsystemBase implements HoodIO {
+ private TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.CANIVORE_SUB);
+
+ private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02, 0.02);
+
+ private Rotation2d goalAngle = new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE));
+ private double goalVelocityRadPerSec = 0.0;
+ private double lastFilteredRad = 0.0;
+ private double lastRawSetpoint = 0.0;
+
+ private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO);
+
+ private boolean calibrating = false;
+ private Debouncer calibrateDebouncer = new Debouncer(0.5, DebounceType.kRising);
+
+ private boolean forceHoodDown = false;
+
+ private HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged();
+
+ public Hood(){
+ motor.setNeutralMode(NeutralModeValue.Brake);
+
+ TalonFXConfiguration config = new TalonFXConfiguration();
+ config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+
+ config.Slot0.kP = 2.0;
+ config.Slot0.kS = 0.1; // Static friction compensation
+ config.Slot0.kV = 0.12; // Adjusted kV for the gear ratio
+ config.Slot0.kD = 0.02; // The "Braking" term to stop overshoot
+
+ var mm = config.MotionMagic;
+ mm.MotionMagicCruiseVelocity = Units.radiansToRotations(HoodConstants.MAX_VELOCITY) * HoodConstants.HOOD_GEAR_RATIO;
+ mm.MotionMagicAcceleration = Units.radiansToRotations(HoodConstants.MAX_ACCELERATION) * HoodConstants.HOOD_GEAR_RATIO; // Lowered for belt safety
+ mm.MotionMagicJerk = 0; // Set to > 0 for "S-Curve" smoothing if needed
+ motor.getConfigurator().apply(config);
+
+ setNewCurrentLimit(HoodConstants.STATOR_CURRENT_LIMIT, HoodConstants.SUPPLY_CURRENT_LIMIT);
+
+ motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO);
+
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putData("max", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0)));
+ SmartDashboard.putData("medium", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians((HoodConstants.MAX_ANGLE + HoodConstants.MIN_ANGLE) / 2)), 0)));
+ SmartDashboard.putData("min", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MIN_ANGLE)), 0)));
+
+ SmartDashboard.putData("force hood down", new InstantCommand(() -> forceHoodDown(true)));
+ SmartDashboard.putData("unforce hood", new InstantCommand(() -> forceHoodDown(false)));
+ }
+ }
+
+ /**
+ * @return Position of the MOTOR in radians
+ */
+ public double getMotorPositionRad(){
+ return Units.rotationsToRadians(motor.getPosition().getValueAsDouble());
+ }
+
+ /**
+ * Sets the setpoint position and velocity of the hood. Call in command execute.
+ * @param angle
+ * @param velocityRadPerSec
+ */
+ public void setFieldRelativeTarget(Rotation2d angle, double velocityRadPerSec) {
+ goalAngle = angle;
+ goalVelocityRadPerSec = velocityRadPerSec;
+ }
+
+ /**
+ * @return Position of turret in degrees
+ */
+ public double getPositionDeg(){
+ return Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / HoodConstants.HOOD_GEAR_RATIO;
+ }
+
+ public void forceHoodDown(boolean taranNathan){
+ forceHoodDown = taranNathan;
+ }
+
+ public boolean getHoodForcedDown() {
+ return this.forceHoodDown;
+ }
+
+ @Override
+ public void periodic() {
+ updateInputs();
+ Logger.processInputs("Hood", inputs);
+
+ // goalAngle = Rotation2d.fromDegrees(SmartDashboard.getNumber("Hood Setpoint", goalAngle.getDegrees()));
+ // SmartDashboard.putNumber("Hood Setpoint", goalAngle.getDegrees());
+
+ if (forceHoodDown) {
+ goalAngle = Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE);
+ goalVelocityRadPerSec = 0.0;
+ }
+
+
+ double setpointRad = goalAngle.getRadians();
+
+ // calculate shortest angular delta
+ double delta = setpointRad - lastRawSetpoint;
+ delta = MathUtil.angleModulus(delta);
+
+ // filter delta
+ double filteredDelta = setpointFilter.calculate(delta);
+
+ // apply filtered range
+ lastFilteredRad = MathUtil.angleModulus(lastFilteredRad + filteredDelta);
+ lastRawSetpoint = setpointRad;
+ setpointRad = lastFilteredRad;
+
+ // Tells the Kraken to get to this position using 1000Hz profile
+ double motorGoalRotations = Units.radiansToRotations(setpointRad) * HoodConstants.HOOD_GEAR_RATIO;
+
+ //Clamp the setpoint rotations
+ motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.radiansToRotations(Units.degreesToRadians(HoodConstants.MIN_ANGLE)) * HoodConstants.HOOD_GEAR_RATIO, Units.radiansToRotations(Units.degreesToRadians(HoodConstants.MAX_ANGLE)) * HoodConstants.HOOD_GEAR_RATIO);
+
+ // Multiply goal velocity by kV
+ double velocityCompensation = goalVelocityRadPerSec * HoodConstants.FEEDFORWARD_KV;
+
+ if (calibrating){
+ motor.set(0.1);
+ boolean atZero = Math.abs(motor.getStatorCurrent().getValueAsDouble()) >= HoodConstants.CALIBRATION_CURRENT_THRESHOLD;
+ boolean calibrated = calibrateDebouncer.calculate(atZero);
+ if (calibrated){
+ stopCalibrating();
+ }
+ } else{
+ // Set control with feedforward
+ motor.setControl(mmVoltageRequest
+ .withPosition(motorGoalRotations)
+ .withFeedForward(velocityCompensation)
+ .withEnableFOC(true));
+ }
+
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Hood/Voltage", motor.getMotorVoltage().getValue());
+ Logger.recordOutput("Hood/velocitySetpoint", goalVelocityRadPerSec / HoodConstants.HOOD_GEAR_RATIO);
+ Logger.recordOutput("Hood/SetpointDeg", Units.radiansToDegrees(goalAngle.getRadians()));
+ }
+
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putBoolean("Hood Calibrated", !calibrating);
+ SmartDashboard.putBoolean("Hood At Setpoint", Math.abs(getPositionDeg() - goalAngle.getDegrees()) < 2.0);
+ }
+ }
+
+ public void calibrate(){
+ calibrating = true;
+ setNewCurrentLimit(HoodConstants.CALIBRATING_CURRENT_LIMIT, HoodConstants.CALIBRATING_CURRENT_LIMIT);
+ }
+
+ public void stopCalibrating(){
+ motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO);
+ goalAngle = new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE));
+ setNewCurrentLimit(HoodConstants.STATOR_CURRENT_LIMIT, HoodConstants.SUPPLY_CURRENT_LIMIT);
+ calibrating = false;
+ }
+
+ public void setNewCurrentLimit(double stator, double supply) {
+ CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
+ limitConfig.StatorCurrentLimit = stator;
+ limitConfig.StatorCurrentLimitEnable = true;
+ limitConfig.SupplyCurrentLimit = supply;
+ limitConfig.SupplyCurrentLimitEnable = true;
+ motor.getConfigurator().apply(limitConfig);
+ }
+
+ public double getSubsystemStatorCurrent() {
+ return inputs.motorStatorCurrent;
+ }
+
+ public double getSubsystemSupplyCurrent() {
+ return inputs.motorSupplyCurrent;
+ }
+
+ @Override
+ public void updateInputs() {
+ inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / HoodConstants.HOOD_GEAR_RATIO;
+ inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / HoodConstants.HOOD_GEAR_RATIO;
+ inputs.motorStatorCurrent = motor.getStatorCurrent().getValueAsDouble();
+ inputs.motorSupplyCurrent = motor.getStatorCurrent().getValueAsDouble();
+ }
+}
--- /dev/null
+package frc.robot.subsystems.hood;
+
+public class HoodConstants {
+ public static final double HOOD_GEAR_RATIO = 64;
+
+ public static final double MASS = 2.46; // kilograms
+ public static final double LENGTH = 0.138 * 2; // meters
+ public static final double MOI = 0.0489969498; // kg*m^2 <-- We got this on Onshape
+
+ public static final double CENTER_OF_MASS_LENGTH = 0.138; // meters
+
+ public static final double MAX_VELOCITY = 25; // rad/s
+ public static final double MAX_ACCELERATION = 160; // rad/s^2
+
+ public static final double MAX_ANGLE = 78.0; // degrees
+ public static final double MIN_ANGLE = 54.5; // degrees
+
+ public static final double FEEDFORWARD_KV = 0.12;
+
+ public static final double STATOR_CURRENT_LIMIT = 40.0; // A
+ public static final double SUPPLY_CURRENT_LIMIT = 40.0; // A
+ public static final double CALIBRATING_CURRENT_LIMIT = 10.0; // A
+ public static final double CALIBRATION_CURRENT_THRESHOLD = 9.0; // A
+}
--- /dev/null
+package frc.robot.subsystems.hood;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface HoodIO {
+ @AutoLog
+ public static class HoodIOInputs{
+ public double positionDeg = HoodConstants.MAX_ANGLE;
+ public double velocityRadPerSec = 0.0;
+ public double motorStatorCurrent = 0.0;
+ public double motorSupplyCurrent = 0.0;
+ }
+
+ public void updateInputs();
+}
--- /dev/null
+package frc.robot.subsystems.shooter;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.Constants;
+import frc.robot.constants.IdConstants;
+import frc.robot.util.HubActive;
+
+public class Shooter extends SubsystemBase implements ShooterIO {
+
+ private TalonFX shooterMotorLeft = new TalonFX(IdConstants.SHOOTER_LEFT_ID, Constants.CANIVORE_SUB);
+ private TalonFX shooterMotorRight = new TalonFX(IdConstants.SHOOTER_RIGHT_ID, Constants.CANIVORE_SUB);
+
+ //TODO Add current limits
+
+ // Goal Velocity / Double theCircumfrence
+ private double shooterTargetSpeed = 0;
+
+ // Velocity in rotations per second
+ VelocityVoltage voltageRequest = new VelocityVoltage(0);
+
+ private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged();
+
+ private LoggedNetworkNumber powerModifier = new LoggedNetworkNumber("/Tuning/OPERATOR/Shooter Modifier", 1.0);
+
+ public Shooter() {
+ updateInputs();
+
+ TalonFXConfiguration config = new TalonFXConfiguration();
+ config.Slot0.kP = 0.5; // 0.5 stable
+ config.Slot0.kI = 0;
+ config.Slot0.kD = 0.0;
+ config.Slot0.kV = 0.125; //Maximum rps = 100 --> 12V/100rps
+
+ config.CurrentLimits
+ .withSupplyCurrentLimit(ShooterConstants.SHOOTER_CURRENT_LIMIT)
+ .withSupplyCurrentLimitEnable(true);
+
+ shooterMotorLeft.getConfigurator().apply(config);
+ shooterMotorRight.getConfigurator().apply(config);
+
+ shooterMotorLeft.getConfigurator().apply(
+ new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)
+ .withNeutralMode(NeutralModeValue.Coast)
+ );
+
+ shooterMotorRight.getConfigurator().apply(
+ new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast)
+ );
+
+ CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
+ limitConfig.StatorCurrentLimit = ShooterConstants.SHOOTER_STATOR_CURRENT_LIMIT;
+ limitConfig.StatorCurrentLimitEnable = true;
+ shooterMotorLeft.getConfigurator().apply(limitConfig);
+ shooterMotorRight.getConfigurator().apply(limitConfig);
+
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putData("Turn on shooter", new InstantCommand(()-> setShooter(12.0)));
+ }
+ }
+
+ @Override
+ public void periodic(){
+ updateInputs();
+
+ // shooterTargetSpeed = SmartDashboard.getNumber("Shooter Setpoint", shooterTargetSpeed);
+ // SmartDashboard.putNumber("Shooter Setpoint", shooterTargetSpeed);
+
+
+ // Convert to RPS
+ double targetVelocityRPS = Units.radiansToRotations(shooterTargetSpeed / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2)) * powerModifier.get();
+
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putNumber("Target Velocity RPS", targetVelocityRPS);
+ SmartDashboard.putNumber("Shooter Motor RPS", shooterMotorLeft.getVelocity().getValueAsDouble());
+ }
+
+ // Sets the motor control to target velocity
+ shooterMotorLeft.setControl(voltageRequest.withVelocity(targetVelocityRPS).withEnableFOC(true));
+ shooterMotorRight.setControl(voltageRequest.withVelocity(targetVelocityRPS).withEnableFOC(true));
+
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Shooter/realVelocity", Math.PI * ShooterConstants.SHOOTER_LAUNCH_DIAMETER * shooterMotorLeft.getVelocity().getValueAsDouble());
+ Logger.recordOutput("Shooter/targetVelocity", shooterTargetSpeed);
+ }
+
+ double actualWheelVelocity = Math.PI * ShooterConstants.SHOOTER_LAUNCH_DIAMETER * shooterMotorLeft.getVelocity().getValueAsDouble();
+
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putNumber("Shooter Speed Error (mps)", shooterTargetSpeed - actualWheelVelocity);
+ SmartDashboard.putBoolean("Shooter At Speed", atTargetSpeed());
+ SmartDashboard.putBoolean("Shooter Running", shooterTargetSpeed > 0);
+ }
+
+ // powerModifier = SmartDashboard.getNumber("OPERATOR: Shooter Power Modifier", powerModifier);
+ // SmartDashboard.putNumber("OPERATOR: Shooter Power Modifier", powerModifier);
+
+ Logger.recordOutput("WON AUTO?", (HubActive.wonAuto()) ? "WON" : "LOST");
+ }
+
+ /**
+ * Sets the target speed of the shooter
+ * @param linearVelocityMps
+ */
+ public void setShooter(double linearVelocityMps) {
+ shooterTargetSpeed = linearVelocityMps;
+ }
+
+ /**@return velocity in m/s */
+ public double getShooterVelocity(){
+ return inputs.shooterSpeedLeft;
+ }
+
+ public void setNewCurrentLimit(double stator, double supply) {
+ CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
+ limitConfig.StatorCurrentLimit = stator;
+ limitConfig.StatorCurrentLimitEnable = true;
+ limitConfig.SupplyCurrentLimit = supply;
+ limitConfig.SupplyCurrentLimitEnable = true;
+ shooterMotorLeft.getConfigurator().apply(limitConfig);
+ shooterMotorRight.getConfigurator().apply(limitConfig);
+ }
+
+ public double getSubsystemStatorCurrent() {
+ return inputs.shooterStatorCurrentLeft + inputs.shooterStatorCurrentRight;
+ }
+
+ public double getSubsystemSupplyCurrent() {
+ return inputs.shooterSupplyCurrentLeft + inputs.shooterSupplyCurrentRight;
+ }
+
+ @Override
+ public void updateInputs(){
+ inputs.shooterSpeedLeft = Units.rotationsToRadians(shooterMotorLeft.getVelocity().getValueAsDouble()) * ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2;
+ inputs.shooterSpeedRight = Units.rotationsToRadians(shooterMotorRight.getVelocity().getValueAsDouble())* ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2;
+ inputs.shooterStatorCurrentLeft = shooterMotorLeft.getStatorCurrent().getValueAsDouble();
+ inputs.shooterStatorCurrentRight = shooterMotorRight.getStatorCurrent().getValueAsDouble();
+ inputs.shooterSupplyCurrentLeft = shooterMotorLeft.getSupplyCurrent().getValueAsDouble();
+ inputs.shooterSupplyCurrentRight = shooterMotorRight.getSupplyCurrent().getValueAsDouble();
+
+ Logger.processInputs("Shooter", inputs);
+ }
+
+ public void bumpUpShooterModifier() {
+ powerModifier.set(powerModifier.get() + 0.025);
+ }
+
+ public void bumpDownShooterModifier() {
+ powerModifier.set(powerModifier.get() - 0.025);
+ }
+
+ /**
+ * @return Whether the shooter is at the target speed with tolerance of 1 m/s
+ */
+ public boolean atTargetSpeed(){
+ return Math.abs(getShooterVelocity() - shooterTargetSpeed) < 1.0;
+ }
+
+ /**
+ * @return Gets the target velocity in m/s
+ */
+ @AutoLogOutput(key="Shooter/TargetSpeed")
+ public double getTargetVelocity(){
+ return shooterTargetSpeed;
+ }
+}
--- /dev/null
+package frc.robot.subsystems.shooter;
+
+import edu.wpi.first.math.util.Units;
+
+public class ShooterConstants {
+ public static final double SHOOTER_VELOCITY = 1.0;
+ public static final double SHOOTER_LAUNCH_DIAMETER = Units.inchesToMeters(4.0);
+ public static final double SHOOTER_CURRENT_LIMIT = 40.0;
+ public static final double SHOOTER_STATOR_CURRENT_LIMIT = 120.0;
+}
--- /dev/null
+package frc.robot.subsystems.shooter;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ShooterIO {
+ @AutoLog
+ public static class ShooterIOInputs {
+ public double shooterSpeedLeft = 0.0;
+ public double shooterSpeedRight = 0.0;
+ public double shooterStatorCurrentLeft = 0.0;
+ public double shooterStatorCurrentRight = 0.0;
+ public double shooterSupplyCurrentLeft = 0.0;
+ public double shooterSupplyCurrentRight = 0.0;
+ }
+
+ public void updateInputs();
+}
--- /dev/null
+package frc.robot.subsystems.spindexer;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.Constants;
+import frc.robot.constants.IdConstants;
+
+public class Spindexer extends SubsystemBase implements SpindexerIO {
+ private TalonFX motorOne = new TalonFX(IdConstants.SPINDEXER_ONE_ID, Constants.CANIVORE_SUB);
+ private TalonFX motorTwo = new TalonFX(IdConstants.SPINDEXER_TWO_ID, Constants.CANIVORE_SUB);
+
+ private double power = 0.0;
+ public int ballCount = 0;
+ private SpindexerState state = SpindexerState.STOPPED;
+ private SpindexerIOInputsAutoLogged inputs = new SpindexerIOInputsAutoLogged();
+
+ public boolean noIndexing = false;
+
+
+ public Spindexer() {
+ updateInputs();
+
+ // configure current limit
+ CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
+ limitConfig.StatorCurrentLimit = SpindexerConstants.CURRENT_FORWARD_STATOR_LIMIT;
+ limitConfig.StatorCurrentLimitEnable = true;
+ limitConfig.SupplyCurrentLowerLimit = SpindexerConstants.SUPPLY_CURRENT_LIMIT;
+ limitConfig.SupplyCurrentLimitEnable = true;
+ motorOne.getConfigurator().apply(limitConfig);
+ motorTwo.getConfigurator().apply(limitConfig);
+ motorTwo.getConfigurator().apply(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive));
+
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putData("Spindexer Run Forward", new InstantCommand(() -> maxSpindexer()));
+ SmartDashboard.putData("Spindexer Run Reverse", new InstantCommand(() -> reverseSpindexer()));
+ SmartDashboard.putData("Spindexer Stop", new InstantCommand(() -> stopSpindexer()));
+ }
+ }
+
+ public enum SpindexerState {
+ MAX,
+ REVERSE,
+ STOPPED,
+ CUSTOM,
+ }
+
+ private SpindexerState pastState = SpindexerState.STOPPED;
+
+ @Override
+ public void periodic() {
+ updateInputs();
+ Logger.processInputs("Spindexer", inputs);
+
+ if (state == SpindexerState.MAX) {
+ setMotorVoltages(SpindexerConstants.spindexerForwardVoltage);
+ } else if (state == SpindexerState.REVERSE) {
+ setMotorVoltages(SpindexerConstants.spindexerReverseVoltage);
+ } else if (state == SpindexerState.STOPPED) {
+ setMotorVoltages(0.0);
+ } else {
+ setMotorVoltages(power);
+ }
+
+ if (state != pastState) {
+ if (state == SpindexerState.REVERSE) {
+ setNewCurrentLimit(SpindexerConstants.SUPPLY_CURRENT_LIMIT, SpindexerConstants.CURRENT_REVERSE_STATOR_LIMIT);
+ } else {
+ setNewCurrentLimit(SpindexerConstants.SUPPLY_CURRENT_LIMIT, SpindexerConstants.CURRENT_FORWARD_STATOR_LIMIT);
+ }
+ pastState = state;
+ }
+
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putBoolean("Spindexer Running", state == SpindexerState.MAX || state == SpindexerState.CUSTOM);
+ SmartDashboard.putBoolean("Spindexer Has Ball", ballCount > 0);
+ }
+
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putBoolean("Spindexer Reversing", state == SpindexerState.REVERSE);
+ }
+
+ Logger.recordOutput("HasBalls", spinningAir());
+ }
+
+ public void setMotorVoltages(double voltage) {
+ motorOne.setControl(new VoltageOut(voltage * 12).withEnableFOC(true));
+ motorTwo.setControl(new VoltageOut(voltage * 12).withEnableFOC(true));
+ }
+
+ public void maxSpindexer() {
+ state = SpindexerState.MAX;
+ }
+
+ public void reverseSpindexer(){
+ state = SpindexerState.REVERSE;
+ }
+
+ public void stopSpindexer() {
+ state = SpindexerState.STOPPED;
+ }
+
+ public void setSpindexer(double power) {
+ this.power = power;
+ state = SpindexerState.CUSTOM;
+ }
+
+ public void setNewCurrentLimit(double stator, double supply) {
+ CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
+ limitConfig.StatorCurrentLimit = stator;
+ limitConfig.StatorCurrentLimitEnable = true;
+ limitConfig.SupplyCurrentLimit = supply;
+ limitConfig.SupplyCurrentLimitEnable = true;
+ motorOne.getConfigurator().apply(limitConfig);
+ motorTwo.getConfigurator().apply(limitConfig);
+ }
+
+ public double getSubsystemStatorCurrent() {
+ return inputs.spindexerOneStatorCurrent + inputs.spindexerTwoStatorCurrent;
+ }
+
+ public double getMotorOneStatorCurrent() {
+ return inputs.spindexerOneStatorCurrent;
+ }
+
+ public double getMotorTwoStatorCurrent() {
+ return inputs.spindexerTwoStatorCurrent;
+ }
+
+ public double getSubsystemSupplyCurrent() {
+ return inputs.spindexerOneSupplyCurrent + inputs.spindexerTwoSupplyCurrent;
+ }
+
+ public boolean spinningAir() {
+ return getMotorOneStatorCurrent() < 16.0 && getMotorTwoStatorCurrent() < 28.0;
+ }
+
+ public double getMotorOneVelocity() {
+ return inputs.spindexerOneVelocity;
+ }
+
+ public double getMotorTwoVelocity() {
+ return inputs.spindexerTwoVelocity;
+ }
+
+ @Override
+ public void updateInputs() {
+ inputs.spindexerOneVelocity = motorOne.getVelocity().getValueAsDouble();
+ inputs.spindexerOneStatorCurrent = motorOne.getStatorCurrent().getValueAsDouble();
+ inputs.spindexerOneSupplyCurrent = motorOne.getSupplyCurrent().getValueAsDouble();
+ inputs.spindexerTwoVelocity = motorTwo.getVelocity().getValueAsDouble();
+ inputs.spindexerTwoStatorCurrent = motorTwo.getStatorCurrent().getValueAsDouble();
+ inputs.spindexerTwoSupplyCurrent = motorTwo.getSupplyCurrent().getValueAsDouble();
+ }
+}
--- /dev/null
+package frc.robot.subsystems.spindexer;
+
+public class SpindexerConstants {
+ public static final double spindexerVelocityWithBall = 6.0; // rps (for counting balls)
+ public static final double SUPPLY_CURRENT_LIMIT = 80; // A
+ public static final double spindexerForwardVoltage = 1.00; // Volts (set low for testing)
+ public static final double spindexerReverseVoltage = -1.00; // Volts
+ public static final double GEAR_RATIO = 27.0; // unused & both motors have same gearing
+
+ public static final double CURRENT_FORWARD_STATOR_LIMIT = 150.0;
+ public static final double CURRENT_REVERSE_STATOR_LIMIT = 20.0;
+ public static final double CURRENT_TIME_LIMIT = 1.0; //s
+ public static final double JAM_CURRENT_THRESHOLD = 10.0; // A
+ public static final double JAM_VELOCITY_THRESHOLD = 10.0; // A
+ public static final double JAM_DEBOUNCE_TIME = 0.3; // seconds
+ public static final double REVERSE_DEBOUNCE_TIME = 0.25; // seconds
+}
--- /dev/null
+package frc.robot.subsystems.spindexer;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface SpindexerIO {
+ @AutoLog
+ public static class SpindexerIOInputs {
+ public double spindexerOneVelocity = 0.0;
+ public double spindexerOneSupplyCurrent = 0.0;
+ public double spindexerOneStatorCurrent = 0.0;
+ public double spindexerTwoVelocity = 0.0;
+ public double spindexerTwoStatorCurrent = 0.0;
+ public double spindexerTwoSupplyCurrent = 0.0;
+ }
+
+ public void updateInputs();
+}
--- /dev/null
+package frc.robot.subsystems.turret;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.sim.TalonFXSimState;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import frc.robot.constants.Constants;
+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.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.IdConstants;
+
+public class Turret extends SubsystemBase implements TurretIO{
+ // Super low magnitude filter for the position to make it less jittery
+ private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02, 0.02);
+
+ private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged();
+
+ public boolean locked = false;
+
+ private boolean calibrating;
+
+ /* ---------------- Hardware ---------------- */
+
+ private final TalonFX motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.CANIVORE_SUB);
+
+ private TalonFXSimState simState;
+ private SingleJointedArmSim turretSim;
+
+ /* ---------------- Control ---------------- */
+
+ private Rotation2d goalAngle = Rotation2d.kZero;
+ private double goalVelocityRadPerSec = 0.0;
+ private double lastGoalRad = 0.0;
+ private double lastFilteredRad = 0.0;
+ private double lastRawSetpoint = 0.0;
+
+ /* ---------------- Visualization ---------------- */
+
+ private final Mechanism2d mech = new Mechanism2d(100, 100);
+ private final MechanismRoot2d root = mech.getRoot("turret", 50, 50);
+ private final MechanismLigament2d ligament = root.append(new MechanismLigament2d("barrel", 30, 0));
+
+ private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(0);
+
+ /* ---------------- Constructor ---------------- */
+
+ public Turret() {
+ motor.setNeutralMode(NeutralModeValue.Brake);
+
+ TalonFXConfiguration config = new TalonFXConfiguration();
+ config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+
+ config.Slot0.kP = 1.5;
+ config.Slot0.kS = 0.0; // Static friction compensation
+ config.Slot0.kV = 0.0; // Adjusted kV for the gear ratio
+ config.Slot0.kD = 0.0; // The "Braking" term to stop overshoot
+ config.Slot0.kA = 0.0;
+
+ var mm = config.MotionMagic;
+ mm.MotionMagicCruiseVelocity = Units.radiansToRotations(TurretConstants.MAX_VELOCITY) * TurretConstants.GEAR_RATIO;
+ mm.MotionMagicAcceleration = Units.radiansToRotations(TurretConstants.MAX_ACCELERATION) * TurretConstants.GEAR_RATIO; // Lowered for belt safety
+ mm.MotionMagicJerk = 0; // Set to > 0 for "S-Curve" smoothing if needed
+ motor.getConfigurator().apply(config);
+
+ setNewCurrentLimit(TurretConstants.STATOR_CURRENT_LIMIT, TurretConstants.SUPPLY_CURRENT_LIMIT);
+
+ lastGoalRad = 0.0;
+
+ if (RobotBase.isSimulation()) {
+ simState = motor.getSimState();
+ turretSim = new SingleJointedArmSim(
+ edu.wpi.first.math.system.plant.DCMotor.getKrakenX60(1),
+ TurretConstants.GEAR_RATIO,
+ 0.01,
+ 0.15,
+ Units.degreesToRadians(TurretConstants.MIN_ANGLE),
+ Units.degreesToRadians(TurretConstants.MAX_ANGLE),
+ false,
+ 0.0);
+ }
+
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putData("Turret Mech", mech);
+ SmartDashboard.putData("Reset Turret Position to Zero", new InstantCommand(() -> resetTurretPosition()));
+
+ SendableChooser<InstantCommand> turretTestChooser = new SendableChooser<>();
+ turretTestChooser.setDefaultOption("Turn to 0", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0)));
+ turretTestChooser.addOption("Turn to -90", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0)));
+ turretTestChooser.addOption("Turn to 90", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0)));
+ turretTestChooser.addOption("Turn to 200", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(200), 0.0)));
+ turretTestChooser.addOption("Turn to -200", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(-200), 0.0)));
+
+ SmartDashboard.putData("Turret Test Positions", turretTestChooser);
+ }
+ SmartDashboard.putData("Set Locked", new InstantCommand(() -> {locked = !locked;}));
+ //motor.setPosition(Units.degreesToRotations(238.86) * TurretConstants.GEAR_RATIO);
+
+ motor.setPosition(0.0);
+ }
+
+ /* ---------------- Public API ---------------- */
+
+ /**
+ * Sets the setpoint position and velocity of the turret. Call in command execute.
+ * @param angle
+ * @param velocityRadPerSec
+ */
+ public void setFieldRelativeTarget(Rotation2d angle, double velocityRadPerSec) {
+ goalAngle = angle;
+ goalVelocityRadPerSec = velocityRadPerSec;
+ }
+
+ public void resetTurretPosition() {
+ inputs.positionDeg = 0.0;
+ }
+
+ /**
+ * @return If the turret is at setpoint with tolerance of 10 degrees
+ */
+ public boolean atSetpoint() {
+ if (locked) return true;
+ return Math.abs(goalAngle.getRadians() - getPositionRad()) < Units.degreesToRadians(10.0);
+ }
+
+ /**
+ * @return Posiiton of the turret in radians
+ */
+ public double getPositionRad() {
+ return Units.rotationsToRadians(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO;
+ }
+
+ /**
+ * @return Posiiton of the turret in degrees
+ */
+ public double getPositionDeg() {
+ return Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO;
+ }
+
+ /* ---------------- Periodic ---------------- */
+
+ @Override
+ public void periodic() {
+ updateInputs();
+ Logger.processInputs("Turret", inputs);
+
+ // Position extrapolation
+ double lookAheadSeconds = TurretConstants.EXTRAPOLATION_TIME_CONSTANT;
+ double futureRobotAngle = goalAngle.getRadians() + (goalVelocityRadPerSec * lookAheadSeconds);
+
+ //Continuous wrap selection
+ double best = lastGoalRad;
+ boolean found = false;
+
+ for (int i = -2; i <= 2; i++) {
+ double candidate = futureRobotAngle + 2.0 * Math.PI * i;
+ if (candidate < Units.degreesToRadians(TurretConstants.MIN_ANGLE) || candidate > Units.degreesToRadians(TurretConstants.MAX_ANGLE))
+ continue;
+
+ if (!found || Math.abs(candidate - lastGoalRad) < Math.abs(best - lastGoalRad)) {
+ best = candidate;
+ found = true;
+ }
+ }
+
+ lastGoalRad = best;
+
+ // calculate shortest angular delta
+ double delta = best - lastRawSetpoint;
+
+ // filter delta
+ double filteredDelta = setpointFilter.calculate(delta);
+
+ // apply filtered range
+ lastFilteredRad += filteredDelta;
+ lastRawSetpoint = best;
+ best = lastFilteredRad;
+
+ // Tells the Kraken to get to this position using 1000Hz profile
+ double motorGoalRotations = Units.radiansToRotations(best) * TurretConstants.GEAR_RATIO;
+
+ // Clamp position setpoint to min and max angles
+ motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.degreesToRotations(TurretConstants.MIN_ANGLE) * TurretConstants.GEAR_RATIO, Units.degreesToRotations(TurretConstants.MAX_ANGLE) * TurretConstants.GEAR_RATIO);
+
+ // Multiply goal velocity by kV
+ double robotTurnCompensation = goalVelocityRadPerSec * TurretConstants.FEEDFORWARD_KV * TurretConstants.GEAR_RATIO;
+
+ // Sets motor control with feedforward
+ motor.setControl(mmVoltageRequest
+ .withPosition(motorGoalRotations)
+ .withFeedForward(robotTurnCompensation)
+ .withEnableFOC(true));
+
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue());
+ Logger.recordOutput("Turret/setpointDeg", goalAngle.getDegrees());
+ }
+
+ // --- Visualization ---
+ ligament.setAngle(Units.radiansToDegrees(getPositionRad()));
+
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putNumber("Turret position", Units.radiansToDegrees(getPositionRad()));
+ SmartDashboard.putBoolean("Turret Calibrated", !calibrating);
+ SmartDashboard.putBoolean("Turret At Setpoint", atSetpoint());
+ }
+ }
+
+ /* ---------------- Simulation ---------------- */
+
+ @Override
+ public void simulationPeriodic() {
+ turretSim.setInputVoltage(motor.getMotorVoltage().getValueAsDouble());
+ turretSim.update(Constants.LOOP_TIME);
+
+ simState.setRawRotorPosition(
+ Units.radiansToRotations(turretSim.getAngleRads()) * TurretConstants.GEAR_RATIO);
+
+ simState.setRotorVelocity(
+ Units.radiansToRotations(turretSim.getVelocityRadPerSec()) * TurretConstants.GEAR_RATIO);
+ }
+
+ @Override
+ public void updateInputs() {
+ inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO;
+ inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / TurretConstants.GEAR_RATIO;
+ inputs.motorStatorCurrent = motor.getStatorCurrent().getValueAsDouble();
+ inputs.motorSupplyCurrent = motor.getSupplyCurrent().getValueAsDouble();
+ inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble();
+ }
+
+ public void setNewCurrentLimit(double stator, double supply) {
+ CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
+ limitConfig.StatorCurrentLimit = stator;
+ limitConfig.StatorCurrentLimitEnable = true;
+ limitConfig.SupplyCurrentLimit = supply;
+ limitConfig.SupplyCurrentLimitEnable = true;
+ motor.getConfigurator().apply(limitConfig);
+ }
+
+ public double getSubsystemStatorCurrent() {
+ return inputs.motorStatorCurrent;
+ }
+
+ public double getSubsystemSupplyCurrent() {
+ return inputs.motorSupplyCurrent;
+ }
+}
--- /dev/null
+package frc.robot.subsystems.turret;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+
+public class TurretConstants {
+ public static double MAX_ANGLE = 170; // Deg
+ public static double MIN_ANGLE = -215; // Deg
+
+ public static double CALIBRATION_OFFSET = 0.0; // TODO: find this at hardstop
+
+ public static double MAX_VELOCITY = 600; // rad/s
+ // public static double MAX_ACCELERATION = 120.0; // rad/s^2
+ public static double MAX_ACCELERATION = 320.0; // rad/s^2
+
+ // Not using this, but just in case
+ public static double TURRET_WIDTH = Units.inchesToMeters(6.4);
+ public static double TURRET_RADIUS = TURRET_WIDTH / 2;
+
+ public static double GEAR_RATIO = 28;
+
+ // Turret is in center of robot, but make use of the height in shooter physics
+ public static Translation3d DISTANCE_FROM_ROBOT_CENTER = new Translation3d(0,0, Units.inchesToMeters(22.172)); //meters
+
+ public static final double EXTRAPOLATION_TIME_CONSTANT = 0.06;
+
+ public static final double FEEDFORWARD_KV = 0.02;
+
+ public static final double STATOR_CURRENT_LIMIT = 40.0; // A
+ public static final double SUPPLY_CURRENT_LIMIT = 40.0; // A
+
+}
--- /dev/null
+package frc.robot.subsystems.turret;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface TurretIO {
+ @AutoLog
+ public static class TurretIOInputs{
+ public double positionDeg = 0;
+ public double velocityRadPerSec = 0;
+ public double motorStatorCurrent = 0;
+ public double motorSupplyCurrent = 0;
+ public double encoderLeftRot = 0;
+ public double encoderRightRot = 0;
+ public double motorVoltage = 0;
+ }
+
+ public void updateInputs();
+}
--- /dev/null
+package frc.robot.util;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+
+/**
+ * Exactly the same as ElevatorSim, except it can be angled and have a constant force spring
+ */
+public class AngledElevatorSim extends ElevatorSim {
+ private double angle;
+ private boolean simulateGravity;
+ private double minHeight;
+ private double maxHeight;
+ private double springAccel;
+
+ /**
+ * Creates a simulated angled elevator mechanism.
+ *
+ * @param gearbox The type of and number of motors in the elevator gearbox.
+ * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
+ * @param carriageMassKg The mass of the elevator carriage.
+ * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
+ * @param minHeightMeters The min allowable height of the elevator.
+ * @param maxHeightMeters The max allowable height of the elevator.
+ * @param simulateGravity Whether gravity should be simulated or not.
+ * @param startingHeightMeters The starting height of the elevator.
+ * @param angleRads The angle of the elevator from vertical in radians.
+ * @param springForceNewtons The force of the constant force spring in Newtons. Up is positive.
+ * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+ * noise is desired. If present must have 1 element for position.
+ */
+ public AngledElevatorSim(
+ DCMotor gearbox,
+ double gearing,
+ double carriageMassKg,
+ double drumRadiusMeters,
+ double minHeightMeters,
+ double maxHeightMeters,
+ boolean simulateGravity,
+ double startingHeightMeters,
+ double angleRads,
+ double springForceNewtons,
+ double... measurementStdDevs) {
+ super(gearbox, gearing, carriageMassKg, drumRadiusMeters, minHeightMeters, maxHeightMeters, simulateGravity, startingHeightMeters, measurementStdDevs);
+ angle = angleRads;
+ this.simulateGravity = simulateGravity;
+ minHeight = minHeightMeters;
+ maxHeight = maxHeightMeters;
+ springAccel = springForceNewtons/carriageMassKg;
+ }
+
+ // Copied from ElevatorSim with one difference
+ /**
+ * Creates a simulated elevator mechanism.
+ *
+ * @param gearbox The type of and number of motors in the elevator gearbox.
+ * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
+ * @param carriageMassKg The mass of the elevator carriage.
+ * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
+ * @param minHeightMeters The min allowable height of the elevator.
+ * @param maxHeightMeters The max allowable height of the elevator.
+ * @param simulateGravity Whether gravity should be simulated or not.
+ * @param startingHeightMeters The starting height of the elevator.
+ * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+ * noise is desired. If present must have 1 element for position.
+ */
+ @Override
+ protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
+ // Calculate updated x-hat from Runge-Kutta.
+ var updatedXhat =
+ NumericalIntegration.rkdp(
+ (x, _u) -> {
+ Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
+ if (simulateGravity) {
+ // This is the only line that is different
+ xdot = xdot.plus(VecBuilder.fill(0, springAccel-9.8*Math.cos(angle)));
+ }
+ return xdot;
+ },
+ currentXhat,
+ u,
+ dtSeconds);
+
+ // We check for collisions after updating x-hat.
+ if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
+ return VecBuilder.fill(minHeight, 0);
+ }
+ if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
+ return VecBuilder.fill(maxHeight, 0);
+ }
+ return updatedXhat;
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+public final class ChineseRemainderTheorem {
+
+ private ChineseRemainderTheorem() {}
+
+ /**
+ * Computes x such that:
+ * x ≡ a (mod n1)
+ * x ≡ b (mod n2)
+ *
+ * n1 and n2 MUST be coprime.
+ *
+ * Returns x in range [0, n1*n2)
+ */
+
+ public static int solve(int a, int n1, int b, int n2) {
+ if (gcd(n1, n2) != 1) {
+ throw new IllegalArgumentException("Moduli must be coprime for CRT.");
+ }
+
+ int N = n1 * n2;
+
+ int invN1modN2 = modInverse(n1, n2);
+ int invN2modN1 = modInverse(n2, n1);
+
+ int result =
+ (a * n2 * invN2modN1 +
+ b * n1 * invN1modN2) % N;
+
+ return (result + N) % N;
+ }
+
+ private static int modInverse(int a, int mod) {
+ a = ((a % mod) + mod) % mod;
+
+ for (int x = 1; x < mod; x++) {
+ if ((a * x) % mod == 1) {
+ return x;
+ }
+ }
+ throw new IllegalStateException("No modular inverse exists.");
+ }
+
+ private static int gcd(int a, int b) {
+ while (b != 0) {
+ int t = b;
+ b = a % b;
+ a = t;
+ }
+ return Math.abs(a);
+ }
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.util;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+
+/**
+ * Similar to SingleJointedArmSim, except it simulates an upward normal force on the end of the arm equal to the weight of the robot
+ * Use setIsClimbing() to change whether this normal force should be simulated
+ */
+public class ClimbArmSim extends SingleJointedArmSim {
+ private boolean simulateGravity;
+ private double armLenMeters;
+ private double minAngle;
+ private double maxAngle;
+ private double mass;
+ private double momentOfInertia;
+ private boolean isClimbing;
+
+ /**
+ * Creates a simulated arm mechanism.
+ *
+ * @param plant The linear system that represents the arm. This system can be created with {@link
+ * edu.wpi.first.math.system.plant.LinearSystemId#createSingleJointedArmSystem(DCMotor,
+ * double, double)}.
+ * @param gearbox The type of and number of motors in the arm gearbox.
+ * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
+ * @param armLengthMeters The length of the arm.
+ * @param minAngleRads The minimum angle that the arm is capable of.
+ * @param maxAngleRads The maximum angle that the arm is capable of.
+ * @param simulateGravity Whether gravity should be simulated or not.
+ * @param startingAngleRads The initial position of the Arm simulation in radians.
+ * @param robotMassKilograms The mass of the robot in kilograms, including battery and bumpers
+ * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+ * noise is desired. If present must have 1 element for position.
+ */
+ public ClimbArmSim(
+ LinearSystem<N2, N1, N2> plant,
+ DCMotor gearbox,
+ double gearing,
+ double armLengthMeters,
+ double minAngleRads,
+ double maxAngleRads,
+ boolean simulateGravity,
+ double startingAngleRads,
+ double robotMasKilograms,
+ double armMassKilograms,
+ double... measurementStdDevs) {
+ super(plant, gearbox, gearing, armLengthMeters, minAngleRads, maxAngleRads, simulateGravity, startingAngleRads, measurementStdDevs);
+ armLenMeters = armLengthMeters;
+ minAngle = minAngleRads;
+ maxAngle = maxAngleRads;
+ this.simulateGravity = simulateGravity;
+ mass = robotMasKilograms;
+ momentOfInertia = 1.0/3.0 * armMassKilograms * armLengthMeters * armLengthMeters;
+ isClimbing = false;
+ }
+
+ /**
+ * Creates a simulated arm mechanism.
+ *
+ * @param gearbox The type of and number of motors in the arm gearbox.
+ * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
+ * @param jKgMetersSquared The moment of inertia of the arm; can be calculated from CAD software.
+ * @param armLengthMeters The length of the arm.
+ * @param minAngleRads The minimum angle that the arm is capable of.
+ * @param maxAngleRads The maximum angle that the arm is capable of.
+ * @param simulateGravity Whether gravity should be simulated or not.
+ * @param startingAngleRads The initial position of the Arm simulation in radians.
+ * @param robotMassKilograms The mass of the robot in kilograms, including battery and bumpers
+ * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+ * noise is desired. If present must have 1 element for position.
+ */
+ public ClimbArmSim(
+ DCMotor gearbox,
+ double gearing,
+ double jKgMetersSquared,
+ double armLengthMeters,
+ double minAngleRads,
+ double maxAngleRads,
+ boolean simulateGravity,
+ double startingAngleRads,
+ double robotMassKilograms,
+ double... measurementStdDevs) {
+ this(
+ LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing),
+ gearbox,
+ gearing,
+ armLengthMeters,
+ minAngleRads,
+ maxAngleRads,
+ simulateGravity,
+ startingAngleRads,
+ robotMassKilograms,
+ 1,
+ measurementStdDevs);
+ momentOfInertia = jKgMetersSquared;
+ }
+
+ public void setIsClimbing(boolean climbing){
+ isClimbing = climbing;
+ }
+
+ /**
+ * Updates the state of the arm.
+ *
+ * @param currentXhat The current state estimate.
+ * @param u The system inputs (voltage).
+ * @param dtSeconds The time difference between controller updates.
+ */
+ @Override
+ protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
+ // The torque on the arm is given by Ï„ = Fâ‹…r, where F is the force applied by
+ // gravity and r the distance from pivot to center of mass. Recall from
+ // dynamics that the sum of torques for a rigid body is τ = J⋅α, were τ is
+ // torque on the arm, J is the mass-moment of inertia about the pivot axis,
+ // and α is the angular acceleration in rad/s². Rearranging yields: α = F⋅r/J
+ //
+ // We substitute in F = m⋅g⋅cos(θ), where θ is the angle from horizontal:
+ //
+ // α = (m⋅g⋅cos(θ))⋅r/J
+ //
+ // Multiply RHS by cos(θ) to account for the arm angle. Further, we know the
+ // arm mass-moment of inertia J of our arm is given by J=1/3 mL², modeled as a
+ // rod rotating about it's end, where L is the overall rod length. The mass
+ // distribution is assumed to be uniform. Substitute r=L/2 to find:
+ //
+ // α = (m⋅g⋅cos(θ))⋅r/(1/3 mL²)
+ // α = (m⋅g⋅cos(θ))⋅(L/2)/(1/3 mL²)
+ // α = 3/2⋅g⋅cos(θ)/L
+ //
+ // Adding the torque from the robot weight, which is in the opposite direction as the arm's mass
+ // α = 3/2⋅g⋅cos(θ)/L - m⋅g⋅cos(θ)⋅L/J
+ //
+ // This acceleration is next added to the linear system dynamics ẋ=Ax+Bu
+ //
+ // f(x, u) = Ax + Bu + [0 α]ᵀ
+ // f(x, u) = Ax + Bu + [0 3/2⋅g⋅cos(θ)/L - m⋅g⋅cos(θ)⋅L/J]ᵀ
+
+ Matrix<N2, N1> updatedXhat =
+ NumericalIntegration.rkdp(
+ (Matrix<N2, N1> x, Matrix<N1, N1> _u) -> {
+ Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
+ if (simulateGravity) {
+ double alphaGrav = 3.0 / 2.0 * -9.8 * Math.cos(x.get(0, 0)) / armLenMeters + (isClimbing ? mass * 9.8 * Math.cos(x.get(0, 0)) * armLenMeters / momentOfInertia : 0);
+ xdot = xdot.plus(VecBuilder.fill(0, alphaGrav));
+ }
+ return xdot;
+ },
+ currentXhat,
+ u,
+ dtSeconds);
+
+ // We check for collision after updating xhat
+ if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
+ return VecBuilder.fill(minAngle, 0);
+ }
+ if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
+ return VecBuilder.fill(maxAngle, 0);
+ }
+ return updatedXhat;
+ }
+
+}
--- /dev/null
+package frc.robot.util;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc.robot.constants.FieldConstants;
+
+public class ConversionUtils {
+
+ /**
+ * @param positionCounts CANCoder Position Counts
+ * @param gearRatio Gear Ratio between CANCoder and Mechanism
+ * @return Degrees of Rotation of Mechanism
+ */
+ public static double CANcoderToDegrees(double positionCounts, double gearRatio) {
+ return positionCounts * (360.0 / (gearRatio * 4096.0));
+ }
+
+ /**
+ * @param degrees Degrees of rotation of Mechanism
+ * @param gearRatio Gear Ratio between CANCoder and Mechanism
+ * @return CANCoder Position Counts
+ */
+ public static double degreesToCANcoder(double degrees, double gearRatio) {
+ return degrees / (360.0 / (gearRatio * 4096.0));
+ }
+
+ /**
+ * @param positionCounts CANCoder Position Counts
+ * @param gearRatio Gear Ratio between CANCoder and Mechanism
+ * @return Radians of Rotation of Mechanism
+ */
+ public static double CANcoderToRadians(double positionCounts, double gearRatio) {
+ return Math.toRadians(CANcoderToDegrees(positionCounts, gearRatio));
+ }
+
+ /**
+ * @param radians Radians of rotation of Mechanism
+ * @param gearRatio Gear Ratio between CANCoder and Mechanism
+ * @return CANCoder Position Counts
+ */
+ public static double radiansToCANcoder(double radians, double gearRatio) {
+ return degreesToCANcoder(Math.toDegrees(radians), gearRatio);
+ }
+
+ /**
+ * @param positionCounts Falcon Position Counts
+ * @param gearRatio Gear Ratio between Falcon and Mechanism
+ * @return Degrees of Rotation of Mechanism
+ */
+ public static double falconToDegrees(double positionCounts, double gearRatio) {
+ return positionCounts * (360.0 / (gearRatio * 2048.0));
+ }
+
+ /**
+ * @param degrees Degrees of rotation of Mechanism
+ * @param gearRatio Gear Ratio between Falcon and Mechanism
+ * @return Falcon Position Counts
+ */
+ public static double degreesToFalcon(double degrees, double gearRatio) {
+ return degrees / (360.0 / (gearRatio * 2048.0));
+ }
+
+ /**
+ * @param velocityCounts Falcon Velocity Counts
+ * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM)
+ * @return RPM of Mechanism
+ */
+ public static double falconToRPM(double velocityCounts, double gearRatio) {
+ double motorRPM = velocityCounts * (600.0 / 2048.0);
+ return motorRPM / gearRatio;
+ }
+
+ /**
+ * @param RPM RPM of mechanism
+ * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM)
+ * @return RPM of Mechanism
+ */
+ public static double RPMToFalcon(double RPM, double gearRatio) {
+ double motorRPM = RPM * gearRatio;
+ return motorRPM * (2048.0 / 600.0);
+ }
+
+ /**
+ * @param velocitycounts Falcon Velocity Counts
+ * @param circumference Circumference of Wheel
+ * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon MPS)
+ * @return Falcon Velocity Counts
+ */
+ public static double falconToMPS(double velocitycounts, double circumference, double gearRatio) {
+ double wheelRPM = falconToRPM(velocitycounts, gearRatio);
+ return (wheelRPM * circumference) / 60;
+ }
+
+ /**
+ * @param velocity Velocity MPS
+ * @param circumference Circumference of Wheel
+ * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon MPS)
+ * @return Falcon Velocity Counts
+ */
+ public static double MPSToFalcon(double velocity, double circumference, double gearRatio) {
+ double wheelRPM = ((velocity * 60) / circumference);
+ return RPMToFalcon(wheelRPM, gearRatio);
+ }
+
+ /**
+ * @param positionCounts Falcon Position Counts
+ * @param circumference Circumference of Wheel
+ * @param gearRatio Gear Ratio between Falcon and Wheel
+ * @return Meters
+ */
+ public static double falconToMeters(double positionCounts, double circumference, double gearRatio) {
+ return positionCounts * (circumference / (gearRatio * 2048.0));
+ }
+
+ /**
+ * @param meters Meters
+ * @param circumference Circumference of Wheel
+ * @param gearRatio Gear Ratio between Falcon and Wheel
+ * @return Falcon Position Counts
+ */
+ public static double MetersToFalcon(double meters, double circumference, double gearRatio) {
+ return meters / (circumference / (gearRatio * 2048.0));
+ }
+
+
+ /**
+ * Converts between an absolute coordinate system and the pathplanner coordinate system.
+ * <p>
+ * Absolute coordinate system always has the origin right of the blue driver station from blue driver perspective,
+ * bottom left if looking down at the field. Positive X goes toward red alliance (forward from blue driver perspective)
+ * and positive Y toward red loading zone (left from blue driver perspective). The Pathplanner coordinate system has the coordinate
+ * system rotated such that the origin starts right of the current driver station.
+ * <p> The transformation is self-inverse, so there is no second function to convert back.
+ *
+ * @param pose pose to convert
+ * @param alliance alliance PathPlanner is using for their origin
+ * @return converted pose
+ */
+ public static Pose2d absolutePoseToPathPlannerPose(Pose2d pose, Alliance alliance) {
+ if (alliance == Alliance.Red) {
+ return pose.relativeTo(new Pose2d(FieldConstants.field.getFieldLength(), FieldConstants.field.getFieldWidth(), new Rotation2d(Math.PI)));
+ }
+ return new Pose2d(pose.getX(), pose.getY(), pose.getRotation());
+ }
+}
--- /dev/null
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.util;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.util.WPIUtilJNI;
+
+/**
+ * A class that limits the rate of change of an input value. Useful for implementing voltage,
+ * setpoint, and/or output ramps. A slew-rate limit is most appropriate when the quantity being
+ * controlled is a velocity or a voltage; when controlling a position, consider using a {@link
+ * edu.wpi.first.math.trajectory.TrapezoidProfile} instead.
+ * Edited by 972 to be "dynamic", that is, the slew rate can be modified on the fly.
+ * Additionally, it can be set to be continuous on a range, useful for angles.
+ */
+public class DynamicSlewRateLimiter {
+ private double positiveRateLimit;
+ private double negativeRateLimit;
+ private double prevVal;
+ private double prevTime;
+
+ private boolean continuous = false;
+ private double lowerContinuousLimit = -1;
+ private double upperContinuousLimit = 1;
+
+ /**
+ * Creates a new DynamicSlewRateLimiter with the given positive and negative rate limits and initial
+ * value.
+ *
+ * @param positiveRateLimit The rate-of-change limit in the positive direction, in units per
+ * second. This is expected to be positive.
+ * @param negativeRateLimit The rate-of-change limit in the negative direction, in units per
+ * second. This is expected to be negative.
+ * @param initialValue The initial value of the input.
+ */
+ public DynamicSlewRateLimiter(double positiveRateLimit, double negativeRateLimit, double initialValue) {
+ this.positiveRateLimit = positiveRateLimit;
+ this.negativeRateLimit = negativeRateLimit;
+ prevVal = initialValue;
+ prevTime = WPIUtilJNI.now() * 1e-6;
+ }
+
+ /**
+ * Creates a new DynamicSlewRateLimiter with the given positive rate limit and negative rate limit of
+ * -rateLimit and initial value.
+ *
+ * @param rateLimit The rate-of-change limit, in units per second.
+ * @param initialValue The initial value of the input.
+ */
+ @Deprecated(since = "2023", forRemoval = true)
+ public DynamicSlewRateLimiter(double rateLimit, double initialValue) {
+ this(rateLimit, -rateLimit, initialValue);
+ }
+
+ /**
+ * Creates a new SlewRateLimiter with the given positive rate limit and negative rate limit of
+ * -rateLimit.
+ *
+ * @param rateLimit The rate-of-change limit, in units per second.
+ */
+ public DynamicSlewRateLimiter(double rateLimit) {
+ this(rateLimit, -rateLimit, 0);
+ }
+
+ /**
+ * Filters the input to limit its slew rate.
+ *
+ * @param input The input value whose slew rate is to be limited.
+ * @return The filtered value, which will not change faster than the slew rate.
+ */
+ public double calculate(double input) {
+ double currentTime = WPIUtilJNI.now() * 1e-6;
+ double elapsedTime = currentTime - prevTime;
+ prevTime = currentTime;
+
+ double change = MathUtil.clamp(
+ input - prevVal,
+ negativeRateLimit * elapsedTime,
+ positiveRateLimit * elapsedTime);
+
+ if (continuous) {
+ change = MathUtil.clamp(
+ MathUtil.inputModulus(input - prevVal, lowerContinuousLimit, upperContinuousLimit),
+ negativeRateLimit * elapsedTime,
+ positiveRateLimit * elapsedTime);
+
+ prevVal += change;
+
+ //Extra check to make sure it is within the limits, probably unnecessary
+ prevVal = MathUtil.inputModulus(prevVal, lowerContinuousLimit, upperContinuousLimit);
+ } else {
+ prevVal += change;
+ }
+
+ return prevVal;
+ }
+
+ /**
+ * Sets a new slewrate and filters the input to limit its slew rate.
+ *
+ * @param input The input value whose slew rate is to be limited.
+ * @param rateLimit The new rate-of-change limit, in units per second.
+ * @return The filtered value, which will not change faster than the slew rate.
+ */
+ public double calculate(double input, double rateLimit) {
+ setRateLimit(rateLimit);
+ return calculate(input);
+ }
+
+ /**
+ * Sets new slew rates and filters the input to limit its slew rate.
+ *
+ * @param input The input value whose slew rate is to be limited.
+ * @param positiveRateLimit The rate-of-change limit in the positive direction, in units per
+ * second. This is expected to be positive.
+ * @param negativeRateLimit The rate-of-change limit in the negative direction, in units per
+ * second. This is expected to be negative.
+ * @return The filtered value, which will not change faster than the slew rate.
+ */
+ public double calculate(double input, double positiveRateLimit, double negativeRateLimit) {
+ setRateLimit(positiveRateLimit, negativeRateLimit);
+ return calculate(input);
+ }
+
+
+ /**
+ * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so.
+ *
+ * @param value The value to reset to.
+ */
+ public void reset(double value) {
+ prevVal = value;
+ prevTime = WPIUtilJNI.now() * 1e-6;
+ }
+
+ /**
+ * set positive rate limit
+ *
+ * @param positiveRateLimit new positive rate limit
+ */
+ public void setPositiveRateLimit(double positiveRateLimit) {
+ this.positiveRateLimit = positiveRateLimit;
+ }
+
+ /**
+ * set negative rate limit
+ *
+ * @param negativeRateLimit new negative rate limit
+ */
+ public void setNegativeRateLimit(double negativeRateLimit) {
+ this.negativeRateLimit = negativeRateLimit;
+ }
+
+ /**
+ * Sets positive and negative rate limits
+ *
+ * @param rateLimit new rate limits
+ */
+ public void setRateLimit(double rateLimit) {
+ positiveRateLimit = rateLimit;
+ negativeRateLimit = -rateLimit;
+ }
+
+ /**
+ * Sets positive and negative rate limits
+ *
+ * @param positiveRateLimit new positive rate limit
+ * @param negativeRateLimit new negative rate limit
+ */
+ public void setRateLimit(double positiveRateLimit, double negativeRateLimit) {
+ this.positiveRateLimit = positiveRateLimit;
+ this.negativeRateLimit = negativeRateLimit;
+ }
+
+ /**
+ * Sets Continuous Limits
+ *
+ * @param lowerContinuousLimit Lower Continuous Limit
+ * @param upperContinuousLimit Upper Continuous Limit
+ */
+ public void setContinuousLimits(double lowerContinuousLimit, double upperContinuousLimit) {
+ this.lowerContinuousLimit = lowerContinuousLimit;
+ this.upperContinuousLimit = upperContinuousLimit;
+ }
+
+ /**
+ * Enables or disables continuous
+ * WARNING: Continuous doesn't work properly with non-symmetrical rate limits
+ *
+ * @param continuous is continuous enabled
+ */
+ public void enableContinuous(boolean continuous) {
+ this.continuous = continuous;
+ }
+}
\ No newline at end of file
--- /dev/null
+// Copyright (c) 2023-2026 Gold87 and other Elastic contributors
+// This software can be modified and/or shared under the terms
+// defined by the Elastic license:
+// https://github.com/Gold872/elastic_dashboard/blob/main/LICENSE
+
+package frc.robot.util;
+
+import com.fasterxml.jackson.annotation.JsonProperty;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.PubSubOption;
+import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.networktables.StringTopic;
+
+public final class Elastic {
+ private static final StringTopic notificationTopic =
+ NetworkTableInstance.getDefault().getStringTopic("/Elastic/RobotNotifications");
+ private static final StringPublisher notificationPublisher =
+ notificationTopic.publish(PubSubOption.sendAll(true), PubSubOption.keepDuplicates(true));
+ private static final StringTopic selectedTabTopic =
+ NetworkTableInstance.getDefault().getStringTopic("/Elastic/SelectedTab");
+ private static final StringPublisher selectedTabPublisher =
+ selectedTabTopic.publish(PubSubOption.keepDuplicates(true));
+ private static final ObjectMapper objectMapper = new ObjectMapper();
+
+ /**
+ * Represents the possible levels of notifications for the Elastic dashboard. These levels are
+ * used to indicate the severity or type of notification.
+ */
+ public enum NotificationLevel {
+ /** Informational Message */
+ INFO,
+ /** Warning message */
+ WARNING,
+ /** Error message */
+ ERROR
+ }
+
+ /**
+ * Sends a notification to the Elastic dashboard. The notification is serialized as a JSON string
+ * before being published.
+ *
+ * @param notification the {@link Notification} object containing notification details
+ */
+ public static void sendNotification(Notification notification) {
+ try {
+ notificationPublisher.set(objectMapper.writeValueAsString(notification));
+ } catch (JsonProcessingException e) {
+ e.printStackTrace();
+ }
+ }
+
+ /**
+ * Selects the tab of the dashboard with the given name. If no tab matches the name, this will
+ * have no effect on the widgets or tabs in view.
+ *
+ * <p>If the given name is a number, Elastic will select the tab whose index equals the number
+ * provided.
+ *
+ * @param tabName the name of the tab to select
+ */
+ public static void selectTab(String tabName) {
+ selectedTabPublisher.set(tabName);
+ }
+
+ /**
+ * Selects the tab of the dashboard at the given index. If this index is greater than or equal to
+ * the number of tabs, this will have no effect.
+ *
+ * @param tabIndex the index of the tab to select.
+ */
+ public static void selectTab(int tabIndex) {
+ selectTab(Integer.toString(tabIndex));
+ }
+
+ /**
+ * Represents a notification object to be sent to the Elastic dashboard. This object holds
+ * properties such as level, title, description, display time, and dimensions to control how the
+ * notification is displayed on the dashboard.
+ */
+ public static class Notification {
+ @JsonProperty("level")
+ private NotificationLevel level;
+
+ @JsonProperty("title")
+ private String title;
+
+ @JsonProperty("description")
+ private String description;
+
+ @JsonProperty("displayTime")
+ private int displayTimeMillis;
+
+ @JsonProperty("width")
+ private double width;
+
+ @JsonProperty("height")
+ private double height;
+
+ /**
+ * Creates a new Notification with all default parameters. This constructor is intended to be
+ * used with the chainable decorator methods
+ *
+ * <p>Title and description fields are empty.
+ */
+ public Notification() {
+ this(NotificationLevel.INFO, "", "");
+ }
+
+ /**
+ * Creates a new Notification with all properties specified.
+ *
+ * @param level the level of the notification (e.g., INFO, WARNING, ERROR)
+ * @param title the title text of the notification
+ * @param description the descriptive text of the notification
+ * @param displayTimeMillis the time in milliseconds for which the notification is displayed
+ * @param width the width of the notification display area
+ * @param height the height of the notification display area, inferred if below zero
+ */
+ public Notification(
+ NotificationLevel level,
+ String title,
+ String description,
+ int displayTimeMillis,
+ double width,
+ double height) {
+ this.level = level;
+ this.title = title;
+ this.displayTimeMillis = displayTimeMillis;
+ this.description = description;
+ this.height = height;
+ this.width = width;
+ }
+
+ /**
+ * Creates a new Notification with default display time and dimensions.
+ *
+ * @param level the level of the notification
+ * @param title the title text of the notification
+ * @param description the descriptive text of the notification
+ */
+ public Notification(NotificationLevel level, String title, String description) {
+ this(level, title, description, 3000, 350, -1);
+ }
+
+ /**
+ * Creates a new Notification with a specified display time and default dimensions.
+ *
+ * @param level the level of the notification
+ * @param title the title text of the notification
+ * @param description the descriptive text of the notification
+ * @param displayTimeMillis the display time in milliseconds
+ */
+ public Notification(
+ NotificationLevel level, String title, String description, int displayTimeMillis) {
+ this(level, title, description, displayTimeMillis, 350, -1);
+ }
+
+ /**
+ * Creates a new Notification with specified dimensions and default display time. If the height
+ * is below zero, it is automatically inferred based on screen size.
+ *
+ * @param level the level of the notification
+ * @param title the title text of the notification
+ * @param description the descriptive text of the notification
+ * @param width the width of the notification display area
+ * @param height the height of the notification display area, inferred if below zero
+ */
+ public Notification(
+ NotificationLevel level, String title, String description, double width, double height) {
+ this(level, title, description, 3000, width, height);
+ }
+
+ /**
+ * Updates the level of this notification
+ *
+ * @param level the level to set the notification to
+ */
+ public void setLevel(NotificationLevel level) {
+ this.level = level;
+ }
+
+ /**
+ * @return the level of this notification
+ */
+ public NotificationLevel getLevel() {
+ return level;
+ }
+
+ /**
+ * Updates the title of this notification
+ *
+ * @param title the title to set the notification to
+ */
+ public void setTitle(String title) {
+ this.title = title;
+ }
+
+ /**
+ * Gets the title of this notification
+ *
+ * @return the title of this notification
+ */
+ public String getTitle() {
+ return title;
+ }
+
+ /**
+ * Updates the description of this notification
+ *
+ * @param description the description to set the notification to
+ */
+ public void setDescription(String description) {
+ this.description = description;
+ }
+
+ public String getDescription() {
+ return description;
+ }
+
+ /**
+ * Updates the display time of the notification
+ *
+ * @param seconds the number of seconds to display the notification for
+ */
+ public void setDisplayTimeSeconds(double seconds) {
+ setDisplayTimeMillis((int) Math.round(seconds * 1000));
+ }
+
+ /**
+ * Updates the display time of the notification in milliseconds
+ *
+ * @param displayTimeMillis the number of milliseconds to display the notification for
+ */
+ public void setDisplayTimeMillis(int displayTimeMillis) {
+ this.displayTimeMillis = displayTimeMillis;
+ }
+
+ /**
+ * Gets the display time of the notification in milliseconds
+ *
+ * @return the number of milliseconds the notification is displayed for
+ */
+ public int getDisplayTimeMillis() {
+ return displayTimeMillis;
+ }
+
+ /**
+ * Updates the width of the notification
+ *
+ * @param width the width to set the notification to
+ */
+ public void setWidth(double width) {
+ this.width = width;
+ }
+
+ /**
+ * Gets the width of the notification
+ *
+ * @return the width of the notification
+ */
+ public double getWidth() {
+ return width;
+ }
+
+ /**
+ * Updates the height of the notification
+ *
+ * <p>If the height is set to -1, the height will be determined automatically by the dashboard
+ *
+ * @param height the height to set the notification to
+ */
+ public void setHeight(double height) {
+ this.height = height;
+ }
+
+ /**
+ * Gets the height of the notification
+ *
+ * @return the height of the notification
+ */
+ public double getHeight() {
+ return height;
+ }
+
+ /**
+ * Modifies the notification's level and returns itself to allow for method chaining
+ *
+ * @param level the level to set the notification to
+ * @return the current notification
+ */
+ public Notification withLevel(NotificationLevel level) {
+ this.level = level;
+ return this;
+ }
+
+ /**
+ * Modifies the notification's title and returns itself to allow for method chaining
+ *
+ * @param title the title to set the notification to
+ * @return the current notification
+ */
+ public Notification withTitle(String title) {
+ setTitle(title);
+ return this;
+ }
+
+ /**
+ * Modifies the notification's description and returns itself to allow for method chaining
+ *
+ * @param description the description to set the notification to
+ * @return the current notification
+ */
+ public Notification withDescription(String description) {
+ setDescription(description);
+ return this;
+ }
+
+ /**
+ * Modifies the notification's display time and returns itself to allow for method chaining
+ *
+ * @param seconds the number of seconds to display the notification for
+ * @return the current notification
+ */
+ public Notification withDisplaySeconds(double seconds) {
+ return withDisplayMilliseconds((int) Math.round(seconds * 1000));
+ }
+
+ /**
+ * Modifies the notification's display time and returns itself to allow for method chaining
+ *
+ * @param displayTimeMillis the number of milliseconds to display the notification for
+ * @return the current notification
+ */
+ public Notification withDisplayMilliseconds(int displayTimeMillis) {
+ setDisplayTimeMillis(displayTimeMillis);
+ return this;
+ }
+
+ /**
+ * Modifies the notification's width and returns itself to allow for method chaining
+ *
+ * @param width the width to set the notification to
+ * @return the current notification
+ */
+ public Notification withWidth(double width) {
+ setWidth(width);
+ return this;
+ }
+
+ /**
+ * Modifies the notification's height and returns itself to allow for method chaining
+ *
+ * @param height the height to set the notification to
+ * @return the current notification
+ */
+ public Notification withHeight(double height) {
+ setHeight(height);
+ return this;
+ }
+
+ /**
+ * Modifies the notification's height and returns itself to allow for method chaining
+ *
+ * <p>This will set the height to -1 to have it automatically determined by the dashboard
+ *
+ * @return the current notification
+ */
+ public Notification withAutomaticHeight() {
+ setHeight(-1);
+ return this;
+ }
+
+ /**
+ * Modifies the notification to disable the auto dismiss behavior
+ *
+ * <p>This sets the display time to 0 milliseconds
+ *
+ * <p>The auto dismiss behavior can be re-enabled by setting the display time to a number
+ * greater than 0
+ *
+ * @return the current notification
+ */
+ public Notification withNoAutoDismiss() {
+ setDisplayTimeMillis(0);
+ return this;
+ }
+ }
+}
--- /dev/null
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.util;
+
+import edu.wpi.first.math.geometry.Twist2d;
+
+public class EqualsUtil {
+ public static boolean epsilonEquals(double a, double b, double epsilon) {
+ return (a - epsilon <= b) && (a + epsilon >= b);
+ }
+
+ public static boolean epsilonEquals(double a, double b) {
+ return epsilonEquals(a, b, 1e-9);
+ }
+
+ /** Extension methods for wpi geometry objects */
+ public static class GeomExtensions {
+ public static boolean epsilonEquals(Twist2d twist, Twist2d other) {
+ return EqualsUtil.epsilonEquals(twist.dx, other.dx)
+ && EqualsUtil.epsilonEquals(twist.dy, other.dy)
+ && EqualsUtil.epsilonEquals(twist.dtheta, other.dtheta);
+ }
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import lib.PolynomialRegression;
+
+import java.util.LinkedList;
+import java.util.List;
+
+/**
+ * A class for storing and processing feedforward characterization data. Used in automatic feedforward characterization.
+ */
+public class FeedForwardCharacterizationData {
+ private PolynomialRegression regression;
+ private final List<Double> velocityData = new LinkedList<>();
+ private final List<Double> voltageData = new LinkedList<>();
+
+ /**
+ * Adds a data point to the data set.
+ *
+ * @param velocity the velocity of the motor
+ * @param voltage the voltage applied to the motor
+ */
+ public void add(double velocity, double voltage) {
+ if (Math.abs(velocity) > 1E-4) {
+ velocityData.add(Math.abs(velocity));
+ voltageData.add(Math.abs(voltage));
+ }
+ }
+
+ /**
+ * Processes the data set using {@link PolynomialRegression}
+ *
+ * @see PolynomialRegression
+ */
+ public void process() {
+ // creates a new process polynomial regression to get calculated values
+ regression = new PolynomialRegression(
+ velocityData.stream().mapToDouble(Double::doubleValue).toArray(),
+ voltageData.stream().mapToDouble(Double::doubleValue).toArray(),
+ 1
+ );
+ }
+
+ /**
+ * Gets the static voltage of the motor.
+ *
+ * @return the static voltage of the motor
+ */
+ public double getStatic() {
+ // gets y-intercept
+ return regression.beta(0);
+ }
+
+ /**
+ * Gets the velocity of the motor.
+ *
+ * @return the velocity of the motor
+ */
+ public double getVelocity() {
+ // gets a slope of regression line
+ return regression.beta(1);
+ }
+
+ /**
+ * Gets the variance of the data set.
+ *
+ * @return the variance of the data set
+ */
+ public double getVariance() {
+ // gets variance of data set
+ return regression.R2();
+ }
+}
+
--- /dev/null
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.util;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+
+/** Geometry utilities for working with translations, rotations, transforms, and poses. */
+public class GeomUtil {
+ /**
+ * Creates a pure translating transform
+ *
+ * @param translation The translation to create the transform with
+ * @return The resulting transform
+ */
+ public static Transform2d toTransform2d(Translation2d translation) {
+ return new Transform2d(translation, new Rotation2d());
+ }
+
+ /**
+ * Creates a pure translating transform
+ *
+ * @param x The x coordinate of the translation
+ * @param y The y coordinate of the translation
+ * @return The resulting transform
+ */
+ public static Transform2d toTransform2d(double x, double y) {
+ return new Transform2d(x, y, new Rotation2d());
+ }
+
+ /**
+ * Creates a pure rotating transform
+ *
+ * @param rotation The rotation to create the transform with
+ * @return The resulting transform
+ */
+ public static Transform2d toTransform2d(Rotation2d rotation) {
+ return new Transform2d(new Translation2d(), rotation);
+ }
+
+ /**
+ * Converts a Pose2d to a Transform2d to be used in a kinematic chain
+ *
+ * @param pose The pose that will represent the transform
+ * @return The resulting transform
+ */
+ public static Transform2d toTransform2d(Pose2d pose) {
+ return new Transform2d(pose.getTranslation(), pose.getRotation());
+ }
+
+ public static Pose2d inverse(Pose2d pose) {
+ Rotation2d rotationInverse = pose.getRotation().unaryMinus();
+ return new Pose2d(
+ pose.getTranslation().unaryMinus().rotateBy(rotationInverse), rotationInverse);
+ }
+
+ /**
+ * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic
+ * chain
+ *
+ * @param transform The transform that will represent the pose
+ * @return The resulting pose
+ */
+ public static Pose2d toPose2d(Transform2d transform) {
+ return new Pose2d(transform.getTranslation(), transform.getRotation());
+ }
+
+ /**
+ * Creates a pure translated pose
+ *
+ * @param translation The translation to create the pose with
+ * @return The resulting pose
+ */
+ public static Pose2d toPose2d(Translation2d translation) {
+ return new Pose2d(translation, new Rotation2d());
+ }
+
+ /**
+ * Creates a pure rotated pose
+ *
+ * @param rotation The rotation to create the pose with
+ * @return The resulting pose
+ */
+ public static Pose2d toPose2d(Rotation2d rotation) {
+ return new Pose2d(new Translation2d(), rotation);
+ }
+
+ /**
+ * Multiplies a twist by a scaling factor
+ *
+ * @param twist The twist to multiply
+ * @param factor The scaling factor for the twist components
+ * @return The new twist
+ */
+ public static Twist2d multiply(Twist2d twist, double factor) {
+ return new Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor);
+ }
+
+ /**
+ * Converts a Pose3d to a Transform3d to be used in a kinematic chain
+ *
+ * @param pose The pose that will represent the transform
+ * @return The resulting transform
+ */
+ public static Transform3d toTransform3d(Pose3d pose) {
+ return new Transform3d(pose.getTranslation(), pose.getRotation());
+ }
+
+ /**
+ * Converts a Transform3d to a Pose3d to be used as a position or as the start of a kinematic
+ * chain
+ *
+ * @param transform The transform that will represent the pose
+ * @return The resulting pose
+ */
+ public static Pose3d toPose3d(Transform3d transform) {
+ return new Pose3d(transform.getTranslation(), transform.getRotation());
+ }
+
+ /**
+ * Converts a ChassisSpeeds to a Twist2d by extracting two dimensions (Y and Z). chain
+ *
+ * @param speeds The original translation
+ * @return The resulting translation
+ */
+ public static Twist2d toTwist2d(ChassisSpeeds speeds) {
+ return new Twist2d(
+ speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond);
+ }
+
+ /**
+ * Creates a new pose from an existing one using a different translation value.
+ *
+ * @param pose The original pose
+ * @param translation The new translation to use
+ * @return The new pose with the new translation and original rotation
+ */
+ public static Pose2d withTranslation(Pose2d pose, Translation2d translation) {
+ return new Pose2d(translation, pose.getRotation());
+ }
+
+ /**
+ * Creates a new pose from an existing one using a different rotation value.
+ *
+ * @param pose The original pose
+ * @param rotation The new rotation to use
+ * @return The new pose with the original translation and new rotation
+ */
+ public static Pose2d withRotation(Pose2d pose, Rotation2d rotation) {
+ return new Pose2d(pose.getTranslation(), rotation);
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+
+import java.util.Optional;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+
+public class HubActive {
+ static public boolean isHubActive() {
+ Optional<Alliance> alliance = DriverStation.getAlliance();
+ // If we have no alliance, we cannot be enabled, therefore no hub.
+ if (alliance.isEmpty()) {
+ return false;
+ }
+ // Hub is always enabled in autonomous.
+ if (DriverStation.isAutonomousEnabled()) {
+ return true;
+ }
+ // At this point, if we're not teleop enabled, there is no hub.
+ if (!DriverStation.isTeleopEnabled()) {
+ return false;
+ }
+
+
+ // We're teleop enabled, compute.
+ double matchTime = DriverStation.getMatchTime();
+ String gameData = DriverStation.getGameSpecificMessage();
+ // If we have no game data, we cannot compute, assume hub is active, as its
+ // likely early in teleop.
+ if (gameData.isEmpty()) {
+ return true;
+ }
+ boolean redInactiveFirst = false;
+ switch (gameData.charAt(0)) {
+ case 'R' -> redInactiveFirst = true;
+ case 'B' -> redInactiveFirst = false;
+ default -> {
+ // If we have invalid game data, assume hub is active.
+ return true;
+ }
+ }
+
+ // Shift was is active for blue if red won auto, or red if blue won auto.
+ boolean shift1Active = switch (alliance.get()) {
+ case Red -> !redInactiveFirst;
+ case Blue -> redInactiveFirst;
+ };
+ if (matchTime > 130) {
+ // Transition shift, hub is active.
+ return true;
+ } else if (matchTime > 105) {
+ // Shift 1
+ return shift1Active;
+ } else if (matchTime > 80) {
+ // Shift 2
+ return !shift1Active;
+ } else if (matchTime > 55) {
+ // Shift 3
+ return shift1Active;
+ } else if (matchTime > 30) {
+ // Shift 4
+ return !shift1Active;
+ } else {
+ // End game, hub always active.
+ return true;
+ }
+ }
+
+
+ static public Optional<Double> timeToActive() {
+
+
+ Optional<Alliance> alliance = DriverStation.getAlliance();
+ // If we have no alliance, we cannot be enabled, therefore no hub.
+ if (alliance.isEmpty()) {
+ return Optional.empty();
+ }
+ // Hub is always enabled in autonomous.
+ if (DriverStation.isAutonomousEnabled()) {
+ return Optional.of(0.0);
+ }
+ // At this point, if we're not teleop enabled, there is no hub.
+ if (!DriverStation.isTeleopEnabled()) {
+ return Optional.empty();
+ }
+
+
+ // We're teleop enabled, compute.
+ double matchTime = DriverStation.getMatchTime();
+ String gameData = DriverStation.getGameSpecificMessage();
+ // If we have no game data, we cannot compute, assume hub is active, as its
+ // likely early in teleop.
+ if (gameData.isEmpty()) {
+ return Optional.of(0.0);
+ }
+ boolean redInactiveFirst = false;
+ switch (gameData.charAt(0)) {
+ case 'R' -> redInactiveFirst = true;
+ case 'B' -> redInactiveFirst = false;
+ default -> {
+ // If we have invalid game data, assume hub is active.
+ return Optional.of(0.0);
+ }
+ }
+
+
+
+
+ // Shift was is active for blue if red won auto, or red if blue won auto.
+ boolean shift1Active = switch (alliance.get()) {
+ case Red -> !redInactiveFirst;
+ case Blue -> redInactiveFirst;
+ };
+
+
+ if (matchTime > 130) {
+ // Transition shift, hub is active.
+ return Optional.of(0.0);
+ } else if (matchTime > 105) {
+ // Shift 1
+ if (shift1Active) {
+ return Optional.of(matchTime - 105.0);
+ } else {
+ return Optional.empty();
+ }
+ } else if (matchTime > 80) {
+ // Shift 2
+
+
+ if (!shift1Active) {
+ return Optional.of(matchTime - 80.0);
+ } else {
+ return Optional.empty();
+ }
+ } else if (matchTime > 55) {
+ // Shift 3
+
+
+ if (shift1Active) {
+ return Optional.of(matchTime - 55.0);
+ } else {
+ return Optional.empty();
+ }
+ } else if (matchTime > 30) {
+ // Shift 4
+
+
+ if (!shift1Active) {
+ return Optional.of(matchTime - 30.0);
+ } else {
+ return Optional.empty();
+ }
+ } else {
+ // End game, hub always active.
+ return Optional.of(0.0);
+ }
+ }
+
+ static public Optional<Double> timeToInactive() {
+
+ Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue);
+ if (alliance == Alliance.Blue) {
+ alliance = Alliance.Red;
+ } else {
+ alliance = Alliance.Blue;
+ }
+
+ // Hub is always enabled in autonomous.
+ if (DriverStation.isAutonomousEnabled()) {
+ return Optional.of(0.0);
+ }
+ // At this point, if we're not teleop enabled, there is no hub.
+ if (!DriverStation.isTeleopEnabled()) {
+ return Optional.empty();
+ }
+
+ // We're teleop enabled, compute.
+ double matchTime = DriverStation.getMatchTime();
+ String gameData = DriverStation.getGameSpecificMessage();
+ // If we have no game data, we cannot compute, assume hub is active, as its
+ // likely early in teleop.
+ if (gameData.isEmpty()) {
+ return Optional.of(0.0);
+ }
+ boolean redInactiveFirst = false;
+ switch (gameData.charAt(0)) {
+ case 'R' -> redInactiveFirst = true;
+ case 'B' -> redInactiveFirst = false;
+ default -> {
+ // If we have invalid game data, assume hub is active.
+ return Optional.of(0.0);
+ }
+ }
+
+ // Shift was is active for blue if red won auto, or red if blue won auto.
+ boolean shift1Active = switch (alliance) {
+ case Red -> !redInactiveFirst;
+ case Blue -> redInactiveFirst;
+ };
+
+ if (matchTime > 130) {
+ // Transition shift, hub is active.
+ return Optional.of(0.0);
+ } else if (matchTime > 105) {
+ // Shift 1
+ if (shift1Active) {
+ return Optional.of(matchTime - 105.0);
+ } else {
+ return Optional.empty();
+ }
+ } else if (matchTime > 80) {
+ // Shift 2
+
+ if (!shift1Active) {
+ return Optional.of(matchTime - 80.0);
+ } else {
+ return Optional.empty();
+ }
+ } else if (matchTime > 55) {
+ // Shift 3
+
+ if (shift1Active) {
+ return Optional.of(matchTime - 55.0);
+ } else {
+ return Optional.empty();
+ }
+ } else if (matchTime > 30) {
+ // Shift 4
+
+ if (!shift1Active) {
+ return Optional.of(matchTime - 30.0);
+ } else {
+ return Optional.empty();
+ }
+ } else {
+ // End game, hub always active.
+ return Optional.of(0.0);
+ }
+ }
+
+ static public boolean wonAuto() {
+ String gameData = DriverStation.getGameSpecificMessage();
+ // If we have no game data, we cannot compute, assume hub is active, as its
+ // likely early in teleop.
+ if (gameData.isEmpty()) {
+ return false;
+ }
+ boolean redInactiveFirst = false;
+ switch (gameData.charAt(0)) {
+ case 'R' -> redInactiveFirst = true;
+ case 'B' -> redInactiveFirst = false;
+ default -> {
+ // If we have invalid game data, assume hub is active.
+ return false;
+ }
+ }
+
+
+ var alliance = DriverStation.getAlliance().get();
+ boolean x;
+ if (alliance == Alliance.Red) {
+ x = redInactiveFirst;
+ } else {
+ x = !redInactiveFirst;
+ }
+ return x;
+
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import java.util.List;
+
+import edu.wpi.first.math.MathUtil;
+import frc.robot.constants.Constants;
+
+/**
+ * Utility class for useful functions.
+ */
+public class MathUtils {
+
+ /**
+ * Deadbands an input to [-1, -deadband], [deadband, 1], rescaling inputs to be linear from
+ * (deadband, 0) to (1,1)
+ *
+ * @param input The input value to rescale
+ * @param deadband The deadband
+ * @return the input rescaled and to fit [-1, -deadband], [deadband, 1]
+ */
+ public static double deadband(double input, double deadband) {
+ if (Math.abs(input) <= deadband) {
+ return 0;
+ } else if (Math.abs(input) == 1) {
+ return input;
+ } else {
+ return (1 / (1 - deadband) * (input + Math.signum(-input) * deadband));
+ }
+ }
+
+ /**
+ * Deadbands an input to [-1, -OIConstants.DEADBAND], [OIConstants.DEADBAND, 1], rescaling inputs to be linear from
+ * (OIConstants.DEADBAND, 0) to (1,1)
+ *
+ * @param input The input value to rescale
+ * @return the input rescaled and to fit [-1, -DEADBAND], [DEADBAND, 1]
+ */
+ public static double deadband(double input) {
+ return deadband(input, Constants.DEFAULT_DEADBAND);
+ }
+
+ /**
+ * An exponential function that maintains positive or negative sign.
+ *
+ * @param exponent the power to raise the base to
+ * @param base the base which will be raised to the power
+ * @return base to the power of exponent, maintaining sign of base
+ */
+ public static double expoMS(double base, double exponent) {
+ // weird stuff will happen if you don't put a number > 0 for controller inputs
+ double finVal = Math.pow(Math.abs(base), exponent);
+ if (base < 0) {
+ finVal *= -1;
+ }
+ return finVal;
+ }
+
+ /**
+ * Calculates Midpoint of two numbers on modulus number line
+ *
+ * @param num1 first number
+ * @param num2 second number
+ * @param lowerBound lower bound of modulus number line
+ * @param upperBound upper bound of modulus number line
+ * @return midpoint of 2 numbers on modulus number line
+ */
+ public static double modulusMidpoint(double num1, double num2, double lowerBound, double upperBound) {
+ num1 = MathUtil.inputModulus(num1, lowerBound, upperBound);
+ num2 = MathUtil.inputModulus(num2, lowerBound, upperBound);
+ if (Math.abs(num1 - num2) > (upperBound - lowerBound) / 2) {
+ return MathUtil.inputModulus((num1 + num2) / 2 + (upperBound - lowerBound) / 2, lowerBound, upperBound);
+ }
+ return (num1 + num2) / 2;
+ }
+
+ /**
+ * Interpolates between two numbers on modulus number line
+ *
+ * @param num1 first number
+ * @param num2 second number
+ * @param amount the amount to interpolate, 0 = first number, 1 = second number
+ * @param lowerBound lower bound of modulus number line
+ * @param upperBound upper bound of modulus number line
+ * @return interpolated value between 2 numbers on modulus number line
+ */
+ public static double modulusInterpolate(double num1, double num2, double amount, double lowerBound, double upperBound) {
+ num1 = MathUtil.inputModulus(num1, lowerBound, upperBound);
+ num2 = MathUtil.inputModulus(num2, lowerBound, upperBound);
+ if (Math.abs(num1 - num2) > (upperBound - lowerBound) / 2) {
+ if(num1 < num2){
+ num1 += upperBound-lowerBound;
+ }else{
+ num2 += upperBound-lowerBound;
+ }
+ }
+ return MathUtil.inputModulus((1-amount)*num1 + amount*num2, lowerBound, upperBound);
+ }
+
+ /**
+ * Calls {@link #mean(double...)}.
+ *
+ * @param data the list of data to find the mean of
+ * @return the mean of the data
+ */
+ public static double mean(List<Double> data) {
+ return mean(doubleListToArray(data));
+ }
+
+ /**
+ * Finds the mean of the provided array of doubles
+ *
+ * @param data an array of doubles
+ * @return the mean of the data
+ */
+ public static double mean(double... data) {
+ double mean = 0;
+ for (double datum : data) {
+ mean += datum;
+ }
+ mean /= data.length;
+ return mean;
+ }
+
+ /**
+ * Calls {@link #stdDev(double...)}.
+ *
+ * @param data the list of data to find the standard deviation of
+ * @return the standard deviation of the data
+ */
+ public static double stdDev(List<Double> data) {
+ return stdDev(doubleListToArray(data));
+ }
+
+ /**
+ * Finds the standard deviation of the provided array of doubles
+ *
+ * @param data an array of doubles
+ * @return the standard deviation of the data
+ */
+ public static double stdDev(double... data) {
+ if (data.length == 0 || data.length == 1) return 0;
+
+ double mean = mean(data);
+
+ double total = 0;
+ for (double datum : data) {
+ total += Math.pow(datum - mean, 2);
+ }
+ return Math.sqrt(total / (data.length - 1));
+ }
+
+ private static double[] doubleListToArray(List<Double> arrayList) {
+ return arrayList.stream().mapToDouble(Double::doubleValue).toArray();
+ }
+
+}
--- /dev/null
+package frc.robot.util;
+
+public class ModifiedCRT {
+ private int gearOne;
+ private int gearTwo;
+ private int turretGear;
+
+ public ModifiedCRT(int gearOne, int gearTwo, int turretGear) {
+ this.gearOne = gearOne;
+ this.gearTwo = gearTwo;
+ this.turretGear = turretGear;
+ }
+
+ public double bruteForce(double encoderLeftRot, double encoderRightRot) {
+ double[] encoderLeft = new double[gearOne];
+ double[] encoderRight = new double[gearTwo];
+
+ // Adds all possible positons for encoder left
+ for (int n=0; n < gearOne; n++) {
+ encoderLeft[n] = (n+encoderLeftRot) * (gearOne/turretGear);
+ }
+ // Gets all possible encoder two positions
+ for (int n=0; n < gearTwo; n++) {
+ encoderRight[n] = (n+encoderRightRot) * (gearTwo/turretGear);
+ }
+
+ for (double a: encoderLeft) {
+ for (double b: encoderRight) {
+ if (a==b) {
+ return a;
+ }
+ }
+ }
+ return 0.0;
+ }
+
+ private long modInverse(long a, long m) {
+ long m0 = m, t, q;
+ long x0 = 0, x1 = 1;
+ if (m==1) return 0;
+ while (a > 1) {
+ q = a / m;
+ t = m;
+ m = a % m;
+ a = t;
+
+ t = x0;
+ x0 = x1 - q * x0;
+ x1 = t;
+ }
+
+ if (x1 < 0) {
+ x1 =+ m0;
+ }
+ return x1;
+ }
+
+ public double solve(double encoderLeftRot, double encoderRightRot) {
+ double r1 = encoderLeftRot * gearOne;
+ double r2 = encoderRightRot * gearTwo;
+
+ long m1 = gearOne;
+ long m2 = gearTwo;
+
+ long inv = modInverse(m1 % m2, m2);
+
+ double x = r1 + m1 * (((r2-r1) * inv) % m2);
+ double combined = x % (m1 * m2);
+
+ return combined / turretGear;
+ }
+
+}
--- /dev/null
+package frc.robot.util;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.VoltageConfigs;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.revrobotics.PersistMode;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+import frc.robot.constants.Constants;
+
+/**
+ * Utility class for easy creation of motor controllers.
+ */
+public class MotorFactory {
+
+ private static final int SPARK_MAX_DEFAULT_CURRENT_LIMIT = 60;
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////
+ // SPARK MAX
+ ///////////////////////////////////////////////////////////////////////////////////////////////
+
+ /**
+ * Create a SparkMax with current limiting enabled
+ *
+ * @param id the ID of the Spark MAX
+ * @param motortype the type of motor the Spark MAX is connected to
+ * @param stallLimit the current limit to set at stall
+ * @return a fully configured CANSparkMAX
+ */
+ public static SparkMax createSparkMAX(int id, MotorType motortype, int stallLimit) {
+ SparkMax sparkMAX = new SparkMax(id, motortype);
+
+ sparkMAX.configure(new SparkMaxConfig()
+ .voltageCompensation(Constants.ROBOT_VOLTAGE)
+ .smartCurrentLimit(stallLimit)
+ .idleMode(IdleMode.kBrake),
+ ResetMode.kResetSafeParameters,
+ PersistMode.kNoPersistParameters
+ );
+ return sparkMAX;
+ }
+
+ /**
+ * Create a SparkMax with default current limiting enabled
+ *
+ * @param id the ID of the Spark MAX
+ * @param motortype the type of motor the Spark MAX is connected to
+ * @return a fully configured CANSparkMAX
+ */
+ public static SparkMax createSparkMAXDefault(int id, MotorType motortype) {
+ return createSparkMAX(id, motortype, SPARK_MAX_DEFAULT_CURRENT_LIMIT);
+ }
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////
+ // TALON FX (Falcon 500 and Kraken X60)
+ ///////////////////////////////////////////////////////////////////////////////////////////////
+
+ /**
+ * Creates a TalonFX with all current limit options. If you would like to use
+ * defaults it is recommended to use the other createTalonFX.. methods.
+ *
+ * @param id the CAN ID of the TalonFX
+ * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio".
+ * @param StatorLimitEnable whether to enable stator limiting
+ * @param StatorCurrentLimit the current, in amps, to return to after the
+ * stator limit is triggered
+ * @param StatorTriggerThreshold the threshold current to trigger the stator
+ * limit
+ * @param StatorTriggerDuration the duration, in seconds, the current is above
+ * the threshold before triggering
+ * @param SupplyLimitEnable whether to enable supply limiting
+ * @param SupplyCurrentLimit the current, in amps, to return to after the
+ * supply limit is triggered
+ * @param SupplyTriggerThreshold the threshold current to trigger the supply
+ * limit
+ * @param SupplyTriggerDuration the duration, in seconds, the current is above
+ * the threshold before triggering
+ * @return A fully configured TalonFX
+ */
+ public static TalonFX createTalonFXFull(int id, CANBus CANBus, boolean StatorLimitEnable,
+ double StatorCurrentLimit,
+ double StatorTriggerThreshold, double StatorTriggerDuration, boolean SupplyLimitEnable, double SupplyCurrentLimit,
+ double SupplyTriggerThreshold, double SupplyTriggerDuration) {
+
+ if (id == -1) {
+ return null;
+ }
+
+ TalonFX talon = new TalonFX(id, CANBus);
+
+ TalonFXConfiguration config = new TalonFXConfiguration();
+
+ // See explanations for Supply and Stator limiting in FalconConstants.java
+ config.CurrentLimits = new CurrentLimitsConfigs().withStatorCurrentLimitEnable(StatorLimitEnable).withStatorCurrentLimit(StatorCurrentLimit).
+ withSupplyCurrentLimitEnable(SupplyLimitEnable).withSupplyCurrentLimit(SupplyCurrentLimit).
+ withSupplyCurrentLowerLimit(SupplyTriggerThreshold).withSupplyCurrentLowerTime(SupplyTriggerDuration);
+
+ config.Voltage = new VoltageConfigs().withPeakForwardVoltage(Constants.ROBOT_VOLTAGE);
+
+ talon.getConfigurator().apply(config);
+ talon.setNeutralMode(NeutralModeValue.Brake);
+
+
+ return talon;
+ }
+
+ /**
+ * Creates a TalonFX with all the default settings.
+ *
+ * @param id the id of the motor
+ * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio".
+ */
+ public static TalonFX createTalonFX(int id, CANBus CANBus) {
+ return createTalonFXFull(id, CANBus, Constants.TALONFX_STATOR_LIMIT_ENABLE, Constants.TALONFX_STATOR_CURRENT_LIMIT,
+ Constants.TALONFX_STATOR_TRIGGER_THRESHOLD, Constants.TALONFX_STATOR_TRIGGER_DURATION,
+ Constants.TALONFX_SUPPLY_LIMIT_ENABLE, Constants.TALONFX_SUPPLY_CURRENT_LIMIT,
+ Constants.TALONFX_SUPPLY_TRIGGER_THRESHOLD, Constants.TALONFX_SUPPLY_TRIGGER_DURATION);
+ }
+
+ /**
+ * Creates a TalonFX with supply current limit options.
+ * <p>
+ * Supply current is current that's being drawn at the input bus voltage.
+ * Supply limiting is useful for preventing breakers from tripping in the PDP.
+ *
+ * @param id the CAN ID of the TalonFX
+ * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio".
+ * @param currentLimit the current, in amps, to return to after the supply limit is triggered
+ * @param triggerThreshold the threshold current to trigger the supply limit
+ * @param triggerDuration the duration, in seconds, the current is above the threshold before triggering
+ */
+ public static TalonFX createTalonFXSupplyLimit(int id, CANBus CANBus, double currentLimit,
+ double triggerThreshold, double triggerDuration) {
+ return createTalonFXFull(id, CANBus, Constants.TALONFX_STATOR_LIMIT_ENABLE, Constants.TALONFX_STATOR_CURRENT_LIMIT,
+ Constants.TALONFX_STATOR_TRIGGER_THRESHOLD, Constants.TALONFX_STATOR_TRIGGER_DURATION, true, currentLimit,
+ triggerThreshold, triggerDuration);
+ }
+
+ /**
+ * Creates a TalonFX with stator current limit options.
+ * <p>
+ * Stator current is current that’s being drawn by the motor.
+ * Stator limiting is useful for limiting acceleration/heat.
+ *
+ * @param id the CAN ID of the TalonFX
+ * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio".
+ * @param currentLimit the current, in amps, to return to after the stator limit is triggered
+ * @param triggerThreshold the threshold current to trigger the stator limit
+ * @param triggerDuration the duration, in seconds, the current is above the threshold before triggering
+ */
+ public static TalonFX createTalonFXStatorLimit(int id, CANBus CANBus, double currentLimit,
+ double triggerThreshold, double triggerDuration) {
+ return createTalonFXFull(id, CANBus, true, currentLimit, triggerThreshold, triggerDuration,
+ Constants.TALONFX_SUPPLY_LIMIT_ENABLE, Constants.TALONFX_SUPPLY_CURRENT_LIMIT,
+ Constants.TALONFX_SUPPLY_TRIGGER_THRESHOLD, Constants.TALONFX_SUPPLY_TRIGGER_DURATION);
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Filesystem;
+import frc.robot.constants.AutoConstants;
+
+import java.io.File;
+import java.util.HashMap;
+import com.pathplanner.lib.path.PathPlannerPath;
+
+/**
+ * Utility class for loading paths using pathplanner.
+ */
+public class PathGroupLoader {
+
+ // private static final HashMap<String, List<PathPlannerTrajectory>> pathGroups = new HashMap<>();
+ private static final HashMap<String, PathPlannerPath> pathGroups = new HashMap<>();
+
+
+ /**
+ * Loads all the paths in the trajectory directory (specified in the constants).
+ * These paths are loaded and stored so that they do not take time while the robot is running
+ * and can be accessed with {@link #getPathGroup(String)}
+ */
+ public static void loadPathGroups() {
+ double totalTime = 0;
+ File[] directoryListing = Filesystem.getDeployDirectory().toPath().resolve(AutoConstants.TRAJECTORY_DIRECTORY).toFile().listFiles();
+
+ if (directoryListing != null) {
+ for (File file : directoryListing) {
+ if (file.isFile() && file.getName().contains(".")) {
+ try {
+ long startTime = System.nanoTime();
+ String name = file.getName().substring(0, file.getName().lastIndexOf("."));
+ // pathGroups.put(name, PathPlannerAuto.getPathGroupFromAutoFile(name));
+ pathGroups.put(name, PathPlannerPath.fromPathFile(name));
+ double time = (System.nanoTime() - startTime) / 1000000.0;
+ totalTime += time;
+ System.out.println("Processed file: " + file.getName() + ", took " + time + " milliseconds.");
+ } catch (Exception e) {
+ DriverStation.reportError(e.getMessage(), true);
+ }
+ }
+ }
+ } else {
+ System.out.println("Error processing file");
+ DriverStation.reportWarning(
+ "Issue with finding path files. Paths will not be loaded.",
+ true
+ );
+ }
+ System.out.println("File processing took a total of " + totalTime + " milliseconds");
+ }
+
+ /**
+ * Gets a path that has already been loaded with {@link #loadPathGroups()}. The path group is a list
+ * of trajectories that path planner can run.
+ *
+ * @param pathGroupName the name of the file, without any extensions. This should be the same exact name that is displayed in pathplanner
+ * @return a list of trajectories that path planner can run.
+ */
+ public static PathPlannerPath getPathGroup(String pathGroupName) {
+ if (pathGroups.get(pathGroupName) == null) {
+ System.out.println("Error retrieving " + pathGroupName + " path!");
+ }
+ return pathGroups.get(pathGroupName);
+ }
+
+
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.util;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import frc.robot.constants.FieldConstants;
+import frc.robot.constants.FieldConstants.FieldZone;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.turret.Turret;
+
+public class PhaseManager {
+
+ public enum WantedState {
+ IDLE,
+ SHOOTING,
+ PASSING
+ }
+
+ public enum CurrentState {
+ IDLE,
+ STARTING_UP,
+ TURNING_AROUND,
+ UNDER_TRENCH,
+ SHOOTING,
+ PASSING
+ }
+
+ private WantedState wantedState = WantedState.SHOOTING;
+ private CurrentState currentState = CurrentState.IDLE;
+
+ public void update(Pose2d drivePose, Shooter shooter, Turret turret) {
+ updateWantedState(drivePose);
+ updateCurrentState(drivePose, shooter, turret);
+ }
+
+ private void updateCurrentState(Pose2d drivePose, Shooter shooter, Turret turret) {
+ // if shooter is not trying to run -- idle
+ if (shooter.getTargetVelocity() == 0.0) {
+ currentState = CurrentState.IDLE;
+ return;
+ }
+ // if shooter velocity not ready yet -- starting up
+ if (!shooter.atTargetSpeed()) { // TODO: but then what happens when the ball goes in and the velocity dips??
+ currentState = CurrentState.STARTING_UP;
+ return;
+ }
+ // if turret is not at setpoint -- turning around
+ if (!turret.atSetpoint()) {
+ currentState = CurrentState.TURNING_AROUND;
+ return;
+ }
+
+ FieldZone zone = FieldConstants.getZone(drivePose.getTranslation());
+ if (zone == FieldConstants.FieldZone.ALLIANCE) {
+ currentState = CurrentState.SHOOTING;
+ } else if (zone == FieldConstants.FieldZone.TRENCH_BUMP){
+ currentState = CurrentState.UNDER_TRENCH;
+ } else {
+ currentState = CurrentState.PASSING;
+ }
+ }
+
+ private void updateWantedState(Pose2d drivePose) {
+ FieldZone zone = FieldConstants.getZone(drivePose.getTranslation());
+ if (zone == FieldConstants.FieldZone.UNDER_LADDER) {
+ wantedState = WantedState.IDLE;
+ } else if (zone == FieldConstants.FieldZone.ALLIANCE) {
+ wantedState = WantedState.SHOOTING;
+ } else {
+ wantedState = WantedState.PASSING;
+ }
+ if (zone == FieldConstants.FieldZone.ALLIANCE) {
+ wantedState = WantedState.SHOOTING;
+ } else {
+ wantedState = WantedState.PASSING;
+ }
+ }
+
+ public WantedState getWantedState() {
+ return wantedState;
+ }
+ public CurrentState getCurrentState() {
+ return currentState;
+ }
+
+ public boolean isIdle() {
+ return wantedState == WantedState.IDLE;
+ }
+
+ public boolean shouldFeed() {
+ // TODO: I'm gonna comment out starting up until i find a solution
+ return !(currentState == CurrentState.TURNING_AROUND) && !(currentState == CurrentState.UNDER_TRENCH); //&& !(currentState == CurrentState.STARTING_UP);
+ }
+
+ public Translation2d getTarget(Pose2d drivePose) {
+ return wantedState == WantedState.SHOOTING ? FieldConstants.getHubTranslation().toTranslation2d()
+ : (FieldConstants.isOnLeftSideOfField(drivePose.getTranslation())
+ //TODO: reversed for sm reason
+ ? FieldConstants.getAllianceSideTranslation(false).toTranslation2d()
+ : FieldConstants.getAllianceSideTranslation(true).toTranslation2d());
+ }
+}
--- /dev/null
+// Copyright 2021-2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.util;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj.RobotController;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Queue;
+import java.util.concurrent.ArrayBlockingQueue;
+import java.util.concurrent.locks.Lock;
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.function.DoubleSupplier;
+
+/**
+ * Provides an interface for asynchronously reading high-frequency measurements to a set of queues.
+ *
+ * <p>This version is intended for Phoenix 6 devices on both the RIO and CANivore buses. When using
+ * a CANivore, the thread uses the "waitForAll" blocking method to enable more consistent sampling.
+ * This also allows Phoenix Pro users to benefit from lower latency between devices using CANivore
+ * time synchronization.
+ */
+public class PhoenixOdometryThread extends Thread {
+ private final Lock signalsLock =
+ new ReentrantLock(); // Prevents conflicts when registering signals
+ private BaseStatusSignal[] phoenixSignals = new BaseStatusSignal[0];
+ private final List<DoubleSupplier> genericSignals = new ArrayList<>();
+ private final List<Queue<Double>> phoenixQueues = new ArrayList<>();
+ private final List<Queue<Double>> genericQueues = new ArrayList<>();
+ private final List<Queue<Double>> timestampQueues = new ArrayList<>();
+
+ private static PhoenixOdometryThread instance = null;
+
+ public static PhoenixOdometryThread getInstance() {
+ if (instance == null) {
+ instance = new PhoenixOdometryThread();
+ }
+ return instance;
+ }
+
+ private PhoenixOdometryThread() {
+ setName("PhoenixOdometryThread");
+ setDaemon(true);
+ }
+
+ @Override
+ public void start() {
+ if (timestampQueues.size() > 0) {
+ super.start();
+ }
+ }
+
+ /** Registers a Phoenix signal to be read from the thread. */
+ public Queue<Double> registerSignal(StatusSignal<Angle> signal) {
+ Queue<Double> queue = new ArrayBlockingQueue<>(20);
+ signalsLock.lock();
+ Drivetrain.odometryLock.lock();
+ try {
+ BaseStatusSignal[] newSignals = new BaseStatusSignal[phoenixSignals.length + 1];
+ System.arraycopy(phoenixSignals, 0, newSignals, 0, phoenixSignals.length);
+ newSignals[phoenixSignals.length] = signal;
+ phoenixSignals = newSignals;
+ phoenixQueues.add(queue);
+ } finally {
+ signalsLock.unlock();
+ Drivetrain.odometryLock.unlock();
+ }
+ return queue;
+ }
+
+ /** Registers a generic signal to be read from the thread. */
+ public Queue<Double> registerSignal(DoubleSupplier signal) {
+ Queue<Double> queue = new ArrayBlockingQueue<>(20);
+ signalsLock.lock();
+ Drivetrain.odometryLock.lock();
+ try {
+ genericSignals.add(signal);
+ genericQueues.add(queue);
+ } finally {
+ signalsLock.unlock();
+ Drivetrain.odometryLock.unlock();
+ }
+ return queue;
+ }
+
+ /** Returns a new queue that returns timestamp values for each sample. */
+ public Queue<Double> makeTimestampQueue() {
+ Queue<Double> queue = new ArrayBlockingQueue<>(20);
+ Drivetrain.odometryLock.lock();
+ try {
+ timestampQueues.add(queue);
+ } finally {
+ Drivetrain.odometryLock.unlock();
+ }
+ return queue;
+ }
+
+ @Override
+ public void run() {
+ while (true) {
+ // Wait for updates from all signals
+ signalsLock.lock();
+ try {
+ BaseStatusSignal.waitForAll(2.0 / 250, phoenixSignals);}
+ finally {
+ signalsLock.unlock();
+ }
+
+ // Save new data to queues
+ Drivetrain.odometryLock.lock();
+ try {
+ // Sample timestamp is current FPGA time minus average CAN latency
+ // Default timestamps from Phoenix are NOT compatible with
+ // FPGA timestamps, this solution is imperfect but close
+ double timestamp = RobotController.getFPGATime() / 1e6;
+ double totalLatency = 0.0;
+ for (BaseStatusSignal signal : phoenixSignals) {
+ totalLatency += signal.getTimestamp().getLatency();
+ }
+ if (phoenixSignals.length > 0) {
+ timestamp -= totalLatency / phoenixSignals.length;
+ }
+
+ // Add new samples to queues
+ for (int i = 0; i < phoenixSignals.length; i++) {
+ phoenixQueues.get(i).offer(phoenixSignals[i].getValueAsDouble());
+ }
+ for (int i = 0; i < genericSignals.size(); i++) {
+ genericQueues.get(i).offer(genericSignals.get(i).getAsDouble());
+ }
+ for (int i = 0; i < timestampQueues.size(); i++) {
+ timestampQueues.get(i).offer(timestamp);
+ }
+ } finally {
+ Drivetrain.odometryLock.unlock();
+ }
+ }
+ }
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.util;
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
+import java.util.function.Supplier;
+
+public class PhoenixUtil {
+ /** Attempts to run the command until no error is produced. */
+ public static void tryUntilOk(int maxAttempts, Supplier<StatusCode> command) {
+ for (int i = 0; i < maxAttempts; i++) {
+ var error = command.get();
+ if (error.isOK()) break;
+ }
+ }
+
+ /** Signals for synchronized refresh. */
+ private static BaseStatusSignal[] canivoreSignals = new BaseStatusSignal[0];
+
+ private static BaseStatusSignal[] rioSignals = new BaseStatusSignal[0];
+
+ /** Registers a set of signals for synchronized refresh. */
+ public static void registerSignals(boolean canivore, BaseStatusSignal... signals) {
+ if (canivore) {
+ BaseStatusSignal[] newSignals = new BaseStatusSignal[canivoreSignals.length + signals.length];
+ System.arraycopy(canivoreSignals, 0, newSignals, 0, canivoreSignals.length);
+ System.arraycopy(signals, 0, newSignals, canivoreSignals.length, signals.length);
+ canivoreSignals = newSignals;
+ } else {
+ BaseStatusSignal[] newSignals = new BaseStatusSignal[rioSignals.length + signals.length];
+ System.arraycopy(rioSignals, 0, newSignals, 0, rioSignals.length);
+ System.arraycopy(signals, 0, newSignals, rioSignals.length, signals.length);
+ rioSignals = newSignals;
+ }
+ }
+
+ /** Refresh all registered signals. */
+ public static void refreshAll() {
+ if (canivoreSignals.length > 0) {
+ BaseStatusSignal.refreshAll(canivoreSignals);
+ }
+ if (rioSignals.length > 0) {
+ BaseStatusSignal.refreshAll(rioSignals);
+ }
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import java.util.Optional;
+
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import frc.robot.constants.Constants;
+
+public class ShooterPhysics {
+ // pitch in radians, going up from the horizontal
+ // exit velocity speed in m/s
+ public record TurretState(Rotation2d yaw, double pitch, double exitVel, double height, double timeOfFlight) {
+ public boolean satisfies(Constraints constraints) {
+ if (height < constraints.height())
+ return false;
+ if (exitVel > constraints.maxVel())
+ return false;
+ if (pitch > constraints.maxPitch() || pitch < constraints.minPitch())
+ return false;
+ return true;
+ }
+ };
+
+ /**
+ * Specifies the constraints of the shot.
+ *
+ * @param height The minimum apex height of the shot.
+ * @param maxVel The maximum exit velocity of the shooter
+ * @param minPitch The minimum pitch covered by the hood
+ * @param maxPitch The maximum pitch covered by the hood
+ */
+ public record Constraints(double height, double maxVel, double minPitch, double maxPitch) {
+ };
+
+ /**
+ * Calculates shot parameters for SOTM using Physicsâ„¢.
+ *
+ * @param robotVelocity The x and y velocity of the robot, field relative.
+ * @param robotToTarget The robot to target transform. Angles are field
+ * relative, positions are with the robot as the origin.
+ * @param peakHeight The peak height the trajectory should reach.
+ * @return A TurretState that represents the shot the robot should take.
+ */
+ public static TurretState getShotParams(Translation2d robotVelocity, Translation3d robotToTarget,
+ double peakHeight) {
+ double zExitVel = Math.sqrt(2 * peakHeight * Constants.GRAVITY_ACCELERATION);
+ double t = (-zExitVel - Math.sqrt(Math.pow(zExitVel, 2) - 2 * Constants.GRAVITY_ACCELERATION * robotToTarget.getZ())) / -Constants.GRAVITY_ACCELERATION;
+ Translation3d exitVel = getRequiredExitVelocity(robotVelocity, robotToTarget, peakHeight);
+ return cvtShot(exitVel, peakHeight, t);
+ }
+
+ /**
+ * Calculates shot parameters for stationary shooting using Physicsâ„¢.
+ *
+ * @param robotToTarget The robot to target transform. Angles are field
+ * relative, positions are with the robot as the origin.
+ * @param peakHeight The peak height the trajectory should reach.
+ * @return A TurretState that represents the shot the robot should take.
+ */
+ public static TurretState getShotParams(Translation3d robotToTarget, double peakHeight) {
+ return getShotParams(new Translation2d(0, 0), robotToTarget, peakHeight);
+ }
+
+ public static Optional<TurretState> getConstrainedParams(Translation3d robotToTarget, Constraints constraints){
+ return getConstrainedParams(new Translation2d(0, 0), robotToTarget, constraints);
+ }
+
+ public static Optional<TurretState> getConstrainedParams(Translation2d robotVelocity, Translation3d robotToTarget,
+ Constraints constraints) {
+ // establish a lower bound
+ double minHeight = Math.max(robotToTarget.getZ(), constraints.height());
+ Optional<TurretState> withMinPitch = withAngle(robotVelocity, robotToTarget, constraints.minPitch());
+ if (withMinPitch.isPresent()) {
+ minHeight = Math.min(minHeight, withMinPitch.get().height());
+ }
+ Translation3d minVel = getRequiredExitVelocity(robotVelocity, robotToTarget, minHeight);
+ double minT = (-minVel.getZ() - Math.sqrt(Math.pow(minVel.getZ(), 2) - 2 * Constants.GRAVITY_ACCELERATION * robotToTarget.getZ())) / -Constants.GRAVITY_ACCELERATION;
+ TurretState withMinHeight = cvtShot(minVel, minHeight, minT);
+ if (withMinHeight.satisfies(constraints))
+ return Optional.of(withMinHeight);
+
+ // the only reason this is empty is if the highest posible trajectory is too low
+ // to intersect the goal
+ Optional<TurretState> withMaxPitchOpt = withAngle(robotVelocity, robotToTarget, constraints.minPitch());
+ if (withMaxPitchOpt.isEmpty())
+ return Optional.empty();
+ TurretState withMaxPitch = withMaxPitchOpt.get();
+
+ // the range from withMinHeight to withMaxPitch will satisfy pitch and height
+ // constraints
+ // now we need to satisfy the speed constraint
+
+ TurretState withMinSpeed = withMinimumSpeed(robotVelocity, robotToTarget);
+ // ordered such that the first element is valid and the second is not
+ Pair<TurretState, TurretState> newRange;
+
+ if (withMinSpeed.height() < withMinHeight.height()) {
+ // the minimum speed is below the lower bound, but doesn't satisfy constraints
+ return Optional.empty();
+
+ } else if (withMinSpeed.height() > withMaxPitch.height()) {
+ // the minimum speed is above the upper bound
+ if (withMaxPitch.satisfies(constraints))
+ // keep optimizing to find the lowest height
+ newRange = new Pair<TurretState, TurretState>(withMaxPitch, withMinHeight);
+ else
+ return Optional.empty();
+
+ } else {
+ // the minimum speed is within the ok range
+ assert withMinSpeed.satisfies(constraints);
+ newRange = new Pair<TurretState, TurretState>(withMinSpeed, withMinHeight);
+ }
+
+ // now we binary search the new range
+ TurretState lastValid = newRange.getFirst();
+ // use a 5cm tolerance
+ while (Math.abs(newRange.getFirst().height() - newRange.getSecond().height()) < .05) {
+ double avgHeight = (newRange.getFirst().height() + newRange.getSecond().height()) / 2;
+ Translation3d guessVel = getRequiredExitVelocity(robotVelocity, robotToTarget, avgHeight);
+ double guessT = (-guessVel.getZ() - Math.sqrt(Math.pow(guessVel.getZ(), 2) - 2 * Constants.GRAVITY_ACCELERATION * robotToTarget.getZ())) / -Constants.GRAVITY_ACCELERATION;
+ TurretState guess = cvtShot(guessVel, avgHeight, guessT);
+ if (guess.satisfies(constraints)) {
+ if (guess.height() < lastValid.height()) lastValid = guess;
+ newRange = new Pair<TurretState, TurretState>(guess, newRange.getSecond());
+ } else {
+ newRange = new Pair<TurretState, TurretState>(newRange.getFirst(), guess);
+ }
+ }
+
+ return Optional.of(lastValid);
+ }
+
+ public static TurretState cvtShot(Translation3d velocity, double height, double timeOfFlight) {
+ Translation2d onGround = velocity.toTranslation2d();
+ Rotation2d yaw = onGround.getAngle();
+ double magnitude2d = onGround.getNorm();
+
+ double pitch = new Translation2d(magnitude2d, velocity.getZ()).getAngle().getRadians();
+ pitch %= Math.PI * 2;
+ double speed = velocity.getDistance(Translation3d.kZero);
+
+ return new TurretState(yaw, pitch, speed, height, timeOfFlight);
+ }
+
+ /**
+ * Actually does the SOTM math. Assumes shots are from (0, 0, 0). This is only
+ * public so it can be unit tested, and shouldn't be called directly.
+ *
+ * @param robotVelocity The velocity of the robot, field relative.
+ * @param target The translation from the robot to the target, field
+ * relative. Aka, the position of the target if the robot
+ * was at the origin.
+ * @param peakZ The height of the highest point of the generated
+ * trajectory.
+ * @return Velocity that should be imparted on the ball, field relative.
+ */
+ public static Translation3d getRequiredExitVelocity(Translation2d robotVelocity, Translation3d target,
+ double peakZ) {
+ if (target.getZ() > peakZ)
+ throw new IllegalArgumentException(
+ "The target (" + target + ") cannot be higher than the max trajectory height (" + peakZ + ").");
+
+ // z = v_z_exit_vel * t - .5 * g * t²
+ // want vertex of this equation to equal peakZ
+ // t_vertex = -v_z_exit_vel / -g
+ // t_vertex = v_z_exit_vel / g
+ // z_vertex = v_z_exit_vel * (v_z_exit_vel / g) - .5 * g * (v_z_exit_vel / g)²
+ // peakZ = v_z_exit_vel² / g - .5 * v_z_exit_vel² / g
+ // peakZ = .5 * v_z_exit_vel² / g
+ // v_z_exit_vel² = 2 * peakZ * g
+ // v_z_exit_vel = √(2 * peakZ * g)
+ double zExitVel = Math.sqrt(2 * peakZ * Constants.GRAVITY_ACCELERATION);
+
+ // now we need time to hit target
+ // z_target = v_z_exit_vel * t - .5 * g * t²
+ // 0 = -.5 * g * t² + v_z_exit_vel * t - z_target
+ // quadratic formula
+ // t = (-v_z_exit_vel ± √(v_z_exit_vel² - 4 * (-.5 * g) * (-z_target))) / (2 *
+ // -.5 * g)
+ // t = (-v_z_exit_vel ± √(v_z_exit_vel² - 2 * g * z_target)) / -g
+ // onlz use - because we only want the part where it's coming down, and that
+ // gives the longer time
+ double t = (-zExitVel - Math.sqrt(Math.pow(zExitVel, 2) - 2 * Constants.GRAVITY_ACCELERATION * target.getZ()))
+ / -Constants.GRAVITY_ACCELERATION;
+
+ if (t < 0)
+ throw new RuntimeException("Time should never be negative (got t=" + t + ").");
+
+ // calculate x and z exit_vel
+ // x = (v_x_robot + v_x_exit_vel) * t
+ // x_target = (v_x_robot + v_x_exit_vel) * t_target
+ // v_x_robot + v_x_exit_vel = x_target / t_target
+ // v_x_exit_vel = x_target / t_target - v_x_robot
+ double xExitVel = target.getX() / t - robotVelocity.getX();
+ // same for y
+ double yExitVel = target.getY() / t - robotVelocity.getY();
+ return new Translation3d(xExitVel, yExitVel, zExitVel);
+ }
+
+ // call with default tolerance
+ public static TurretState withMinimumSpeed(Translation2d initialVelocity, Translation3d target) {
+ return withMinimumSpeed(initialVelocity, target, 0.1);
+ }
+
+ public static TurretState withMinimumSpeed(Translation2d initialVelocity, Translation3d target,
+ double tolerance) {
+
+ // calculate minimum velocity: v² = g * (R + √(R² + h²))
+ double horizontalDist = target.toTranslation2d().getNorm();
+ double verticalDist = target.getZ();
+ double g = Constants.GRAVITY_ACCELERATION;
+ double robotSpeed = initialVelocity.getNorm();
+
+ double minProjectileSpeed = Math
+ .sqrt(g * (horizontalDist + Math.sqrt(horizontalDist * horizontalDist + verticalDist * verticalDist)));
+ double minSpeed = Math.max(0, minProjectileSpeed - robotSpeed);
+
+ // guess a peak height
+ double guess = target.getZ() + 2;
+ int maxIters = 20;
+ while (maxIters >= 0) {
+ maxIters--;
+
+ // this will throw an exception, so avoid it
+ // we still might have just overshot, so keep checking
+ if (guess < target.getZ())
+ guess = target.getZ();
+
+ Translation3d guessVelocity = getRequiredExitVelocity(initialVelocity, target, guess);
+ double guessT = (-guessVelocity.getZ() - Math.sqrt(Math.pow(guessVelocity.getZ(), 2) - 2 * Constants.GRAVITY_ACCELERATION * target.getZ())) / -Constants.GRAVITY_ACCELERATION;
+ double guessSpeed = guessVelocity.getNorm();
+ double difference = minSpeed - guessSpeed;
+
+ // we've already hit minimum height and are trying to go lower
+ if (guess <= target.getZ() && difference < 0)
+ throw new RuntimeException("Incorrect minimum speed calculation in ShooterPhysics.java");
+
+ if (Math.abs(difference) <= tolerance)
+ return cvtShot(guessVelocity, guess, guessT);
+
+ guess += difference * 1.7; // experimentally determined value
+ }
+
+ throw new RuntimeException("Failed to compute a trajectory for a minimum speed.");
+ }
+
+ public static Optional<TurretState> withAngle(Translation2d initialVelocity, Translation3d target,
+ double pitch) {
+ return withAngle(initialVelocity, target, pitch, Units.degreesToRadians(1));
+ }
+
+ public static Optional<TurretState> withAngle(Translation2d initialVelocity, Translation3d target,
+ double pitch, double tolerance) {
+
+ // guess a peak height
+ double guess = target.getZ() + 2;
+ int maxIters = 20;
+ while (maxIters >= 0) {
+ maxIters--;
+
+ // this will throw an exception, so avoid it
+ // we still might have just overshot, so keep checking
+ if (guess < target.getZ())
+ guess = target.getZ();
+
+ Translation3d guessVelocity = getRequiredExitVelocity(initialVelocity, target, guess);
+ double guessT = (-guessVelocity.getZ() - Math.sqrt(Math.pow(guessVelocity.getZ(), 2) - 2 * Constants.GRAVITY_ACCELERATION * target.getZ())) / -Constants.GRAVITY_ACCELERATION;
+ TurretState polar = cvtShot(guessVelocity, guess, guessT);
+ double difference = pitch - polar.pitch();
+
+ // we've already hit minimum height and are trying to go lower
+ if (guess <= target.getZ() && difference < 0)
+ return Optional.empty();
+
+ if (Math.abs(difference) <= tolerance)
+ return Optional.of(polar);
+
+ guess += difference * 0.7; // TODO: find better value
+ }
+
+ throw new RuntimeException("Solving for angle did not converge.");
+ }
+}
--- /dev/null
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.util;
+
+import java.util.Arrays;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/**
+ * Stores and updates the position of each module
+ */
+public class SwerveModulePose {
+
+ private double[] dist = {0,0,0,0};
+ private Translation2d[] moduleTranslations;
+ private Pose2d[] modulePositions;
+ private double[] angles;
+ private Drivetrain drive;
+ private double prevRotation;
+ private Pose2d[] displayPoses;
+
+ /**
+ * Creates a new SwerveModulePose object to store and update the positions of each module
+ * @param drive The drivetrain
+ * @param modulePositions The translations of the modules relative to the center of the robot
+ */
+ public SwerveModulePose(Drivetrain drive, Translation2d... modulePositions){
+ this.drive = drive;
+ this.moduleTranslations = modulePositions;
+ this.modulePositions = new Pose2d[4];
+ angles = new double[4];
+ reset();
+ update();
+ reset();
+ }
+
+ /**
+ * Updates the module positions
+ */
+ public void update(){
+ SwerveModuleState[] states = drive.getModuleStates();
+ double currentRotation = drive.getYaw().getRadians();
+ double chassisRotation = currentRotation - prevRotation;
+
+ for(int i = 0; i<4; i++){
+ double position = drive.getModules()[i].getPosition().distanceMeters;
+ double distance = position - dist[i];
+ dist[i] = position;
+
+ Twist2d twist = new Twist2d(distance, 0, MathUtil.angleModulus(states[i].angle.getRadians()-angles[i] + chassisRotation));
+ angles[i] = states[i].angle.getRadians();
+ modulePositions[i] = modulePositions[i].exp(twist);
+
+ displayPoses[i] = new Pose2d(
+ modulePositions[i].getTranslation(),
+ EqualsUtil.epsilonEquals(states[i].speedMetersPerSecond, 0, 0.01) ? displayPoses[i].getRotation() :
+ states[i].speedMetersPerSecond < 0 ? modulePositions[i].getRotation().plus(new Rotation2d(Math.PI)) :
+ modulePositions[i].getRotation()
+ );
+ }
+ prevRotation = currentRotation;
+ }
+
+ /**
+ * Gets the positions of the modules
+ * @return The module poses as an array of Pose2ds
+ */
+ public Pose2d[] getModulePoses(){
+ return displayPoses;
+ }
+
+ /**
+ * Resets the modules to the correct positions relative to the robot
+ */
+ public void reset(){
+ Pose2d chassisPose2d = drive.getPose();
+ SwerveModuleState[] states = drive.getModuleStates();
+ for (int i = 0; i<4; i++){
+ angles[i] = states[i].angle.getRadians();
+ this.modulePositions[i] = new Pose2d(moduleTranslations[i].rotateBy(chassisPose2d.getRotation()).plus(chassisPose2d.getTranslation()), new Rotation2d(angles[i]).plus(chassisPose2d.getRotation()));
+ }
+ prevRotation = drive.getYaw().getRadians();
+ displayPoses = Arrays.copyOf(modulePositions, 4);
+ }
+
+ /**
+ * Gets whehter or not the modules have slipped
+ * A module has slipped if it has moved 0.3m (about 1ft) from its correct position relative to the other modules
+ * @return True if any of the modules have slipped, false otherwise
+ */
+ public boolean slipped(){
+ Translation2d total = new Translation2d();
+ for(Pose2d pose : modulePositions){
+ total = total.plus(pose.getTranslation());
+ }
+ Pose2d drivePose = new Pose2d(total.div(4), drive.getYaw());
+ for(int i = 0; i < 4; i++){
+ double dist = modulePositions[i].relativeTo(drivePose).getTranslation().getDistance(moduleTranslations[i]);
+ if(dist > 0.3){
+ return true;
+ }
+ }
+ return false;
+ }
+
+ /**
+ * Gets the average slip distance
+ * @return The average distance between each module and its correct position
+ */
+ public double getAverageSlip(){
+ Translation2d total = new Translation2d();
+ for(Pose2d pose : modulePositions){
+ total = total.plus(pose.getTranslation());
+ }
+ Pose2d drivePose = new Pose2d(total.div(4), drive.getYaw());
+ double slip = 0;
+ for(int i = 0; i < 4; i++){
+ slip += modulePositions[i].relativeTo(drivePose).getTranslation().getDistance(moduleTranslations[i]);
+ }
+ return slip/4;
+ }
+}
\ No newline at end of file
--- /dev/null
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.util.SwerveStuff;
+
+public record ModuleLimits(
+ double maxDriveVelocity, double maxDriveAcceleration, double staticFriction, double maxSteeringVelocity) {}
--- /dev/null
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.util.SwerveStuff;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+
+public record SwerveSetpoint(ChassisSpeeds chassisSpeeds, SwerveModuleState[] moduleStates) {}
--- /dev/null
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.util.SwerveStuff;
+
+import static frc.robot.util.EqualsUtil.*;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Optional;
+
+import frc.robot.constants.Constants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.util.EqualsUtil;
+import frc.robot.util.GeomUtil;
+
+/**
+ * "Inspired" by FRC team 254. See the license file in the root directory of this project.
+ *
+ * <p>Takes a prior setpoint (ChassisSpeeds), a desired setpoint (from a driver, or from a path
+ * follower), and outputs a new setpoint that respects all of the kinematic constraints on module
+ * rotation speed and wheel velocity/acceleration. By generating a new setpoint every iteration, the
+ * robot will converge to the desired setpoint quickly while avoiding any intermediate state that is
+ * kinematically infeasible (and can result in wheel slip or robot heading drift as a result).
+ */
+
+public class SwerveSetpointGenerator {
+ private final SwerveDriveKinematics kinematics= DriveConstants.KINEMATICS;
+ private final Translation2d[] moduleLocations = DriveConstants.MODULE_LOCATIONS;
+
+ /**
+ * Check if it would be faster to go to the opposite of the goal heading (and reverse drive
+ * direction).
+ *
+ * @param prevToGoal The rotation from the previous state to the goal state (i.e.
+ * prev.inverse().rotateBy(goal)).
+ * @return True if the shortest path to achieve this rotation involves flipping the drive
+ * direction.
+ */
+ private boolean flipHeading(Rotation2d prevToGoal) {
+ return Math.abs(prevToGoal.getRadians()) > Math.PI / 2.0;
+ }
+
+ private double unwrapAngle(double ref, double angle) {
+ double diff = angle - ref;
+ if (diff > Math.PI) {
+ return angle - 2.0 * Math.PI;
+ } else if (diff < -Math.PI) {
+ return angle + 2.0 * Math.PI;
+ } else {
+ return angle;
+ }
+ }
+
+ @FunctionalInterface
+ private interface Function2d {
+ double f(double x, double y);
+ }
+
+ /**
+ * Find the root of the generic 2D parametric function 'func' using the regula falsi technique.
+ * This is a pretty naive way to do root finding, but it's usually faster than simple bisection
+ * while being robust in ways that e.g. the Newton-Raphson method isn't.
+ *
+ * @param func The Function2d to take the root of.
+ * @param x_0 x value of the lower bracket.
+ * @param y_0 y value of the lower bracket.
+ * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during
+ * recursion)
+ * @param x_1 x value of the upper bracket.
+ * @param y_1 y value of the upper bracket.
+ * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during
+ * recursion)
+ * @param iterations_left Number of iterations of root finding left.
+ * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the
+ * (approximate) root.
+ */
+ private double findRoot(
+ Function2d func,
+ double x_0,
+ double y_0,
+ double f_0,
+ double x_1,
+ double y_1,
+ double f_1,
+ int iterations_left) {
+ if (iterations_left < 0 || epsilonEquals(f_0, f_1)) {
+ return 1.0;
+ }
+ var s_guess = Math.max(0.0, Math.min(1.0, -f_0 / (f_1 - f_0)));
+ var x_guess = (x_1 - x_0) * s_guess + x_0;
+ var y_guess = (y_1 - y_0) * s_guess + y_0;
+ var f_guess = func.f(x_guess, y_guess);
+ if (Math.signum(f_0) == Math.signum(f_guess)) {
+ // 0 and guess on same side of root, so use upper bracket.
+ return s_guess
+ + (1.0 - s_guess)
+ * findRoot(func, x_guess, y_guess, f_guess, x_1, y_1, f_1, iterations_left - 1);
+ } else {
+ // Use lower bracket.
+ return s_guess
+ * findRoot(func, x_0, y_0, f_0, x_guess, y_guess, f_guess, iterations_left - 1);
+ }
+ }
+
+ protected double findSteeringMaxS(
+ double x_0,
+ double y_0,
+ double f_0,
+ double x_1,
+ double y_1,
+ double f_1,
+ double max_deviation,
+ int max_iterations) {
+ f_1 = unwrapAngle(f_0, f_1);
+ double diff = f_1 - f_0;
+ if (Math.abs(diff) <= max_deviation) {
+ // Can go all the way to s=1.
+ return 1.0;
+ }
+ double offset = f_0 + Math.signum(diff) * max_deviation;
+ Function2d func =
+ (x, y) -> {
+ return unwrapAngle(f_0, Math.atan2(y, x)) - offset;
+ };
+ return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations);
+ }
+
+ protected double findDriveMaxS_254version(
+ double x_0,
+ double y_0,
+ double f_0,
+ double x_1,
+ double y_1,
+ double f_1,
+ double max_vel_step,
+ int max_iterations) {
+ double diff = f_1 - f_0;
+ if (Math.abs(diff) <= max_vel_step) {
+ // Can go all the way to s=1.
+ return 1.0;
+ }
+ double offset = f_0 + Math.signum(diff) * max_vel_step;
+ Function2d func =
+ (x, y) -> {
+ return Math.hypot(x, y) - offset;
+ };
+ return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations);
+ }
+
+ /**
+ * Limits the acceleration in all directions.
+ * This is different from findDriveMaxS because it includes the acceleration perpendicular to the wheel as it rotates.
+ * Given the same velocity step, this will return a lower S value than findDriveMaxS.
+ * @param x_0 The initial x velocity
+ * @param y_0 The initial y velocity
+ * @param x_1 The final x velocity
+ * @param y_1 The final y velocity
+ * @param max_vel_step The maxiumum amount the velocity can change this frame
+ * @param max_iterations The maximum number of iterations to use in findRoot
+ * @return The maximum interpolation value
+ */
+ protected double findAccelerationMaxS(
+ double x_0,
+ double y_0,
+ double x_1,
+ double y_1,
+ double max_vel_step,
+ int max_iterations) {
+ double dist = Math.hypot(x_1-x_0, y_1-y_0);
+ if(dist <= max_vel_step){
+ return 1;
+ }
+ return Math.max(0.0, Math.min(1.0, max_vel_step / dist));
+ }
+
+ protected double findDriveMaxS(
+ double x_0, double y_0, double x_1, double y_1, double max_vel_step) {
+ // Derivation:
+ // Want to find point P(s) between (x_0, y_0) and (x_1, y_1) where the
+ // length of P(s) is the target T. P(s) is linearly interpolated between the
+ // points, so P(s) = (x_0 + (x_1 - x_0) * s, y_0 + (y_1 - y_0) * s).
+ // Then,
+ // T = sqrt(P(s).x^2 + P(s).y^2)
+ // T^2 = (x_0 + (x_1 - x_0) * s)^2 + (y_0 + (y_1 - y_0) * s)^2
+ // T^2 = x_0^2 + 2x_0(x_1-x_0)s + (x_1-x_0)^2*s^2
+ // + y_0^2 + 2y_0(y_1-y_0)s + (y_1-y_0)^2*s^2
+ // T^2 = x_0^2 + 2x_0x_1s - 2x_0^2*s + x_1^2*s^2 - 2x_0x_1s^2 + x_0^2*s^2
+ // + y_0^2 + 2y_0y_1s - 2y_0^2*s + y_1^2*s^2 - 2y_0y_1s^2 + y_0^2*s^2
+ // 0 = (x_0^2 + y_0^2 + x_1^2 + y_1^2 - 2x_0x_1 - 2y_0y_1)s^2
+ // + (2x_0x_1 + 2y_0y_1 - 2x_0^2 - 2y_0^2)s
+ // + (x_0^2 + y_0^2 - T^2).
+ //
+ // To simplify, we can factor out some common parts:
+ // Let l_0 = x_0^2 + y_0^2, l_1 = x_1^2 + y_1^2, and
+ // p = x_0 * x_1 + y_0 * y_1.
+ // Then we have
+ // 0 = (l_0 + l_1 - 2p)s^2 + 2(p - l_0)s + (l_0 - T^2),
+ // with which we can solve for s using the quadratic formula.
+
+ double l_0 = x_0 * x_0 + y_0 * y_0;
+ double l_1 = x_1 * x_1 + y_1 * y_1;
+ double sqrt_l_0 = Math.sqrt(l_0);
+ double diff = Math.sqrt(l_1) - sqrt_l_0;
+ if (Math.abs(diff) <= max_vel_step) {
+ // Can go all the way to s=1.
+ return 1.0;
+ }
+
+ double target = sqrt_l_0 + Math.copySign(max_vel_step, diff);
+ double p = x_0 * x_1 + y_0 * y_1;
+
+ // Quadratic of s
+ double a = l_0 + l_1 - 2 * p;
+ double b = 2 * (p - l_0);
+ double c = l_0 - target * target;
+ double root = Math.sqrt(b * b - 4 * a * c);
+
+ // Check if either of the solutions are valid
+ // Won't divide by zero because it is only possible for a to be zero if the
+ // target velocity is exactly the same or the reverse of the current
+ // velocity, which would be caught by the difference check.
+ double s_1 = (-b + root) / (2 * a);
+ if (isValidS(s_1)) {
+ return s_1;
+ }
+ double s_2 = (-b - root) / (2 * a);
+ if (isValidS(s_2)) {
+ return s_2;
+ }
+
+ // Since we passed the initial max_vel_step check, a solution should exist,
+ // but if no solution was found anyway, just don't limit movement
+ return 1.0;
+ }
+
+ protected static boolean isValidS(double s) {
+ return Double.isFinite(s) && s >= 0 && s <= 1;
+ }
+
+ /**
+ * Generate a new setpoint.
+ *
+ * @param limits The kinematic limits to respect for this setpoint.
+ * @param centerOfMassHeight The height of the robot's center of mass, in meters, off the ground.
+ * This assumes that the center of mass is in the center of the robot in the x and y directions.
+ * If tipping is not a potential problem this year, set this to 0.
+ * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous
+ * iteration setpoint instead of the actual measured/estimated kinematic state.
+ * @param desiredState The desired state of motion, such as from the driver sticks or a path
+ * following algorithm.
+ * @param dt The loop time.
+ * @return A Setpoint object that satisfies all of the KinematicLimits while converging to
+ * desiredState quickly.
+ */
+ public SwerveSetpoint generateSetpoint(
+ final ModuleLimits limits,
+ double centerOfMassHeight,
+ final SwerveSetpoint prevSetpoint,
+ ChassisSpeeds desiredState,
+ double dt) {
+ final Translation2d[] modules = moduleLocations;
+
+ SwerveModuleState[] desiredModuleState = kinematics.toSwerveModuleStates(desiredState);
+ // Make sure desiredState respects velocity limits.
+ if (limits.maxDriveVelocity() > 0.0) {
+ SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleState, limits.maxDriveVelocity());
+ desiredState = kinematics.toChassisSpeeds(desiredModuleState);
+ }
+
+ // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so
+ // just use the previous angle.
+ boolean need_to_steer = true;
+ if (EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(desiredState),new Twist2d())) {
+ need_to_steer = false;
+ for (int i = 0; i < modules.length; ++i) {
+ desiredModuleState[i].angle = prevSetpoint.moduleStates()[i].angle;
+ desiredModuleState[i].speedMetersPerSecond = 0.0;
+ }
+ }
+
+ // For each module, compute local Vx and Vy vectors.
+ double[] prev_vx = new double[modules.length];
+ double[] prev_vy = new double[modules.length];
+ Rotation2d[] prev_heading = new Rotation2d[modules.length];
+ double[] desired_vx = new double[modules.length];
+ double[] desired_vy = new double[modules.length];
+ Rotation2d[] desired_heading = new Rotation2d[modules.length];
+ boolean all_modules_should_flip = true;
+ for (int i = 0; i < modules.length; ++i) {
+ prev_vx[i] =
+ prevSetpoint.moduleStates()[i].angle.getCos()
+ * prevSetpoint.moduleStates()[i].speedMetersPerSecond;
+ prev_vy[i] =
+ prevSetpoint.moduleStates()[i].angle.getSin()
+ * prevSetpoint.moduleStates()[i].speedMetersPerSecond;
+ prev_heading[i] = prevSetpoint.moduleStates()[i].angle;
+ if (prevSetpoint.moduleStates()[i].speedMetersPerSecond < 0.0) {
+ prev_heading[i] = prev_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI));
+ }
+ desired_vx[i] =
+ desiredModuleState[i].angle.getCos() * desiredModuleState[i].speedMetersPerSecond;
+ desired_vy[i] =
+ desiredModuleState[i].angle.getSin() * desiredModuleState[i].speedMetersPerSecond;
+ desired_heading[i] = desiredModuleState[i].angle;
+ if (desiredModuleState[i].speedMetersPerSecond < 0.0) {
+ desired_heading[i] = desired_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI));
+ }
+ if (all_modules_should_flip) {
+ double required_rotation_rad =
+ Math.abs(prev_heading[i].unaryMinus().rotateBy(desired_heading[i]).getRadians());
+ if (required_rotation_rad < Math.PI / 2.0) {
+ all_modules_should_flip = false;
+ }
+ }
+ }
+ if (all_modules_should_flip
+ && !EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(prevSetpoint.chassisSpeeds()),new Twist2d())
+ && !EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(desiredState),new Twist2d())) {
+ // It will (likely) be faster to stop the robot, rotate the modules in place to the complement
+ // of the desired
+ // angle, and accelerate again.
+ return generateSetpoint(limits, centerOfMassHeight, prevSetpoint, new ChassisSpeeds(), dt);
+ }
+
+ // Compute the deltas between start and goal. We can then interpolate from the start state to
+ // the goal state; then
+ // find the amount we can move from start towards goal in this cycle such that no kinematic
+ // limit is exceeded.
+ double dx = desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond;
+ double dy = desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond;
+ double dtheta =
+ desiredState.omegaRadiansPerSecond - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond;
+
+ // 's' interpolates between start and goal. At 0, we are at prevState and at 1, we are at
+ // desiredState.
+ double min_s = 1.0;
+
+ // In cases where an individual module is stopped, we want to remember the right steering angle
+ // to command (since
+ // inverse kinematics doesn't care about angle, we can be opportunistically lazy).
+ List<Optional<Rotation2d>> overrideSteering = new ArrayList<>(modules.length);
+ // Enforce steering velocity limits. We do this by taking the derivative of steering angle at
+ // the current angle,
+ // and then backing out the maximum interpolant between start and goal states. We remember the
+ // minimum across all modules, since
+ // that is the active constraint.
+ final double max_theta_step = dt * limits.maxSteeringVelocity();
+ for (int i = 0; i < modules.length; ++i) {
+ if (!need_to_steer) {
+ overrideSteering.add(Optional.of(prevSetpoint.moduleStates()[i].angle));
+ continue;
+ }
+ overrideSteering.add(Optional.empty());
+ if (epsilonEquals(prevSetpoint.moduleStates()[i].speedMetersPerSecond, 0.0)) {
+ // If module is stopped, we know that we will need to move straight to the final steering
+ // angle, so limit based
+ // purely on rotation in place.
+ if (epsilonEquals(desiredModuleState[i].speedMetersPerSecond, 0.0)) {
+ // Goal angle doesn't matter. Just leave module at its current angle.
+ overrideSteering.set(i, Optional.of(prevSetpoint.moduleStates()[i].angle));
+ continue;
+ }
+
+ var necessaryRotation =
+ prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(desiredModuleState[i].angle);
+ if (flipHeading(necessaryRotation)) {
+ necessaryRotation = necessaryRotation.rotateBy(Rotation2d.fromRadians(Math.PI));
+ }
+ // getRadians() bounds to +/- Pi.
+ final double numStepsNeeded = Math.abs(necessaryRotation.getRadians()) / max_theta_step;
+
+ if (numStepsNeeded <= 1.0) {
+ // Steer directly to goal angle.
+ overrideSteering.set(i, Optional.of(desiredModuleState[i].angle));
+ // Don't limit the global min_s;
+ continue;
+ } else {
+ // Adjust steering by max_theta_step.
+ overrideSteering.set(
+ i,
+ Optional.of(
+ prevSetpoint.moduleStates()[i].angle.rotateBy(
+ Rotation2d.fromRadians(
+ Math.signum(necessaryRotation.getRadians()) * max_theta_step))));
+ min_s = 0.0;
+ continue;
+ }
+ }
+ if (min_s == 0.0) {
+ // s can't get any lower. Save some CPU.
+ continue;
+ }
+
+ final int kMaxIterations = 8;
+ double s =
+ findSteeringMaxS(
+ prev_vx[i],
+ prev_vy[i],
+ prev_heading[i].getRadians(),
+ desired_vx[i],
+ desired_vy[i],
+ desired_heading[i].getRadians(),
+ max_theta_step,
+ kMaxIterations);
+ min_s = Math.min(min_s, s);
+ }
+
+ // Enforce drive wheel acceleration limits.
+ final double max_vel_step = dt * limits.maxDriveAcceleration();
+ final double max_vel_step_2 = dt * limits.staticFriction() * Constants.GRAVITY_ACCELERATION;
+ for (int i = 0; i < modules.length; ++i) {
+ if (min_s == 0.0) {
+ // No need to carry on.
+ break;
+ }
+ double vx_min_s =
+ min_s == 1.0 ? desired_vx[i] : (desired_vx[i] - prev_vx[i]) * min_s + prev_vx[i];
+ double vy_min_s =
+ min_s == 1.0 ? desired_vy[i] : (desired_vy[i] - prev_vy[i]) * min_s + prev_vy[i];
+ // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we
+ // already know we can't go faster
+ // than that.
+ final int kMaxIterations = 10;
+ double s = min_s*findDriveMaxS(prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step);
+
+ // Limit the overall acceleration of this wheel
+ double s2 = min_s*findAccelerationMaxS(prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step_2, kMaxIterations);
+
+ min_s = Math.min(Math.min(min_s, s), s2);
+ }
+
+ if(centerOfMassHeight > 0.02){
+ // Limit the acceleration in the x and y directions separately based on the center of mass.
+ // To make the torque on the robot 0, we can assume all of the mass is on the back wheel, where the front is the direction the robot is accelerating toward
+ // Torque is equal to the force times the component of the radius perpendicular to the force
+ // T = torque, m = mass, a = acceleration, g = gravity acceleration, x = distance from center to wheel
+ // T = mgx - mah = 0
+ // a = gx/h
+ double maxAccel = Constants.GRAVITY_ACCELERATION*(DriveConstants.TRACK_WIDTH/2)/centerOfMassHeight;
+ // Limit based on this calculated value
+ // x and y are limited separately because, when tipping in a diagonal direction, the distance is longer
+ double xAccel = Math.abs(desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond) / dt;
+ double yAccel = Math.abs(desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond) / dt;
+ if(!epsilonEquals(xAccel, 0)){
+ double s = maxAccel / xAccel;
+ min_s = Math.min(min_s, s);
+ }
+ if(!epsilonEquals(yAccel, 0)){
+ double s = maxAccel / yAccel;
+ min_s = Math.min(min_s, s);
+ }
+ }
+
+ ChassisSpeeds retSpeeds =
+ new ChassisSpeeds(
+ prevSetpoint.chassisSpeeds().vxMetersPerSecond + min_s * dx,
+ prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy,
+ prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta);
+ var retStates = kinematics.toSwerveModuleStates(retSpeeds);
+ for (int i = 0; i < modules.length; ++i) {
+ final var maybeOverride = overrideSteering.get(i);
+ if (maybeOverride.isPresent()) {
+ var override = maybeOverride.get();
+ if (flipHeading(retStates[i].angle.unaryMinus().rotateBy(override))) {
+ retStates[i].speedMetersPerSecond *= -1.0;
+ }
+ retStates[i].angle = override;
+ }
+ final var deltaRotation =
+ prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(retStates[i].angle);
+ if (flipHeading(deltaRotation)) {
+ retStates[i].angle = retStates[i].angle.rotateBy(Rotation2d.fromRadians(Math.PI));
+ retStates[i].speedMetersPerSecond *= -1.0;
+ }
+ }
+ return new SwerveSetpoint(retSpeeds, retStates);
+ }
+}
--- /dev/null
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.util;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism;
+
+import java.util.function.Consumer;
+
+
+/**
+ * Util class for creating SysId routines
+*/
+public class SysId {
+
+ private SysIdRoutine sysIdRoutine;
+
+ public SysId(String name, Consumer<Voltage> driveConsumer, Consumer<SysIdRoutineLog> logConsumer, Subsystem subsystem, Config config){
+ sysIdRoutine = new SysIdRoutine(
+ config,
+ new Mechanism(
+ driveConsumer,
+ logConsumer,
+ subsystem,
+ name
+ )
+ );
+ }
+ public SysId(String name, Consumer<Voltage> driveConsumer, Subsystem subsystem, Config config){
+ this(name,driveConsumer,null,subsystem,config);
+ }
+
+ public Command runQuasisStatic(Direction direction){
+ return sysIdRoutine.quasistatic(direction);
+ }
+ public Command runDynamic(Direction direction){
+ return sysIdRoutine.dynamic(direction);
+ }
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.util;
+
+import edu.wpi.first.util.WPIUtilJNI;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+
+/**
+ * Class for testing if a value is within a certain margin of error for a certain amount of time.
+ */
+public class TimeAccuracyTest {
+
+ private final BooleanSupplier accuracyTest;
+ private final double setpointUpdateTime;
+ private final double errorMargin;
+ private final double timeMargin;
+ private boolean lastUseableResult = false;
+
+ /**
+ * @param actual DoubleSupplier that returns the actual value
+ * @param setpoint DoubleSupplier that returns the setpoint
+ * @param errorMargin margin of error for the test to be accurate
+ * @param timeMargin time in seconds that the setpoint must be held for the test to be accurate
+ */
+ public TimeAccuracyTest(DoubleSupplier actual, DoubleSupplier setpoint, double errorMargin, double timeMargin) {
+ this.errorMargin = errorMargin;
+ this.timeMargin = timeMargin;
+ setpointUpdateTime = WPIUtilJNI.now() * 1e-6;
+ accuracyTest = () -> getDoubleAccuracyTest(actual, setpoint);
+ }
+
+ /**
+ * Determines if the test is successful.
+ *
+ * @return true if the test is successful, false if not
+ */
+ public boolean calculate() {
+ if (setpointUpdateTime + timeMargin <= WPIUtilJNI.now() * 1e-6)
+ lastUseableResult = accuracyTest.getAsBoolean();
+ return lastUseableResult;
+ }
+
+ /**
+ * Determines if the actual value is within the error margin of the setpoint.
+ *
+ * @return true if the actual value is within the error margin of the setpoint, false if not
+ */
+ private boolean getDoubleAccuracyTest(DoubleSupplier actual, DoubleSupplier setpoint) {
+ return Math.abs(actual.getAsDouble() - setpoint.getAsDouble()) <= errorMargin;
+ }
+}
--- /dev/null
+package frc.robot.util.TrenchAssist;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
+import frc.robot.constants.FieldConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+public class TrenchAssist {
+
+ public static ChassisSpeeds calculate(Drivetrain drive, ChassisSpeeds chassisSpeeds, PIDController pid) {
+ // ChassisSpeeds speedsFieldRelative =
+ // ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds,
+ // drive.getYaw().unaryMinus());
+
+ double distanceFromSlideLatitude;
+
+ //pick which side of field to go on
+ if (drive.getPose().getY() > (FieldConstants.FIELD_WIDTH / 2.0)) {
+ distanceFromSlideLatitude = (drive.getPose().getY() - TrenchAssistConstants.SLIDE_LATITUDES[0]);
+ } else {
+ distanceFromSlideLatitude = (drive.getPose().getY() - TrenchAssistConstants.SLIDE_LATITUDES[1]);
+ }
+
+
+ double correctionVelocity = pid.calculate(distanceFromSlideLatitude, 0);
+
+ //deadzone
+ if (Math.abs(distanceFromSlideLatitude) < Units.inchesToMeters(3)) {
+ correctionVelocity = 0.0;
+ }
+
+ // set y speed to pid calculation
+ ChassisSpeeds horizontalSpeeds = new ChassisSpeeds(chassisSpeeds.vxMetersPerSecond, correctionVelocity,
+ chassisSpeeds.omegaRadiansPerSecond);
+
+ //rotate once
+ Translation2d y = new Translation2d(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond)
+ .rotateBy(drive.getYaw());
+
+ //rotate twice
+ return ChassisSpeeds.fromFieldRelativeSpeeds(
+ new ChassisSpeeds(y.getX(), y.getY(), horizontalSpeeds.omegaRadiansPerSecond),
+ drive.getYaw());
+ }
+
+ public static ChassisSpeeds convertToChassisSpeedsRobotRelative(Translation2d translation, Drivetrain drive) {
+ Rotation2d yaw = drive.getYaw();
+ Translation2d robotRelative = translation.rotateBy(yaw);
+ return new ChassisSpeeds(robotRelative.getX(), robotRelative.getY(), 0.0);
+
+ }
+
+ public static Translation2d convertToTranslation2dFieldRelative(ChassisSpeeds speeds, Drivetrain drive) {
+ Rotation2d yaw = drive.getYaw();
+ Translation2d robotTranslation = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
+ return robotTranslation.rotateBy(yaw.times(-1));
+
+ }
+}
--- /dev/null
+package frc.robot.util.TrenchAssist;
+
+import edu.wpi.first.math.geometry.Rectangle2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import frc.robot.constants.FieldConstants;
+
+public class TrenchAssistConstants {
+ public static final Rectangle2d[] OBSTACLES = new Rectangle2d[] {
+ new Rectangle2d(new Translation2d(4.03, 1.28), new Translation2d(5.22, 1.58)),
+ new Rectangle2d(new Translation2d(4.03, 8.07 - 1.28), new Translation2d(5.22, 8.07 - 1.58)),
+ new Rectangle2d(new Translation2d(11.32, 1.28), new Translation2d(12.51, 1.58)),
+ new Rectangle2d(new Translation2d(11.32, 8.07 - 1.28), new Translation2d(12.51, 8.07 - 1.58)),
+ }; // 8.07m
+
+ public static final Rectangle2d[] ALIGN_ZONES = new Rectangle2d[] {
+ new Rectangle2d(new Translation2d(4.03 - .5, 0), new Translation2d(5.22 + .5, 3)),
+ new Rectangle2d(new Translation2d(4.03 - .5, 8.07), new Translation2d(5.22 + .5, 8.07 - 3)),
+ new Rectangle2d(new Translation2d(11.32 - .5, 0), new Translation2d(12.51 + .5, 3)),
+ new Rectangle2d(new Translation2d(11.32 - .5, 8.07), new Translation2d(12.51 + .5, 8.07 - 3)),
+ new Rectangle2d(new Translation2d(99.99, 99.9), new Translation2d(-99.0, -99.0)),
+ };
+
+ public static final double[] SLIDE_LATITUDES = new double[] {
+ FieldConstants.FIELD_WIDTH - Units.inchesToMeters(27.0),
+ Units.inchesToMeters(27.0), // should be accurate, i think our field is slightly too small
+ // 6.550,
+ // 0.668,
+
+ };
+
+}
--- /dev/null
+package frc.robot.util.Vision;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc.robot.Robot;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+/**
+ * Stores information about an object detected by vision
+ */
+public class DetectedObject {
+ private static Drivetrain drive;
+ public final Pose3d pose;
+ public final ObjectType type;
+
+ public enum ObjectType{
+ CORAL(Units.inchesToMeters(4.5/2)),
+ ALGAE(Units.inchesToMeters(16.25/2)),
+ RED_ROBOT(0),
+ BLUE_ROBOT(0),
+ NONE(0);
+
+ public final double height;
+
+ private ObjectType(double h){
+ height = h;
+ }
+ };
+
+ /**
+ * Sets the drivetrain to use for pose calculations
+ * @param drive The drivetrain
+ */
+ public static void setDrive(Drivetrain drive){
+ DetectedObject.drive = drive;
+ }
+
+ /**
+ * Creates a default DetectedObject with default attributes
+ */
+ public DetectedObject(){
+ pose = new Pose3d();
+ type = ObjectType.NONE;
+ }
+
+ /**
+ * Creates a new DetectedObject
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param distance The distance from the camera to the object in meters
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ */
+ public DetectedObject(double xOffset, double yOffset, double distance, ObjectType type, Transform3d robotToCamera){
+ this(xOffset, yOffset, distance, type, robotToCamera, -1);
+ }
+
+ /**
+ * Creates a new DetectedObject
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param distance The distance from the camera to the object in meters
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ * @param timestamp The timestamp of the picture in seconds
+ */
+ public DetectedObject(double xOffset, double yOffset, double distance, ObjectType type, Transform3d robotToCamera, double timestamp){
+ this.type = type;
+ // Get the position relative to the camera
+ Translation3d translation = new Translation3d(distance, new Rotation3d(0, -yOffset, -xOffset))
+ // Rotate and translate it to get the position relative to the robot
+ .rotateBy(robotToCamera.getRotation())
+ .plus(robotToCamera.getTranslation());
+ // If the drivetrain exists, rotate and translate it to get the field relative position
+ if(drive != null){
+ Pose2d drivePose = drive.getPoseAt(timestamp);
+ translation = translation.rotateBy(new Rotation3d(
+ 0,
+ 0,
+ drivePose.getRotation().getRadians()
+ )).plus(new Translation3d(
+ drivePose.getX(),
+ drivePose.getY(),
+ 0
+ ));
+ }
+ pose = new Pose3d(translation, new Rotation3d());
+ }
+
+ /**
+ * Creates a new DetectedObject
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param distance The distance from the camera to the object in meters
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ * @param timestamp The timestamp of the picture in seconds
+ */
+ public DetectedObject(double xOffset, double yOffset, double distance, int type, Transform3d robotToCamera, double timestamp){
+ this(xOffset, yOffset, distance, getType(type), robotToCamera, timestamp);
+ }
+
+ /**
+ * Creates a new DetectedObject
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param distance The distance from the camera to the object in meters
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ */
+ public DetectedObject(double xOffset, double yOffset, double distance, int type, Transform3d robotToCamera){
+ this(xOffset, yOffset, distance, getType(type), robotToCamera, -1);
+ }
+
+ /**
+ * Creates a new DetectedObject
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param distance The distance from the camera to the object in meters
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ * @param timestamp The timestamp of the picture in seconds
+ */
+ public DetectedObject(double xOffset, double yOffset, double distance, String type, Transform3d robotToCamera, double timestamp){
+ this(xOffset, yOffset, distance, getType(type), robotToCamera, timestamp);
+ }
+
+ /**
+ * Creates a new DetectedObject
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param distance The distance from the camera to the object in meters
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ */
+ public DetectedObject(double xOffset, double yOffset, double distance, String type, Transform3d robotToCamera){
+ this(xOffset, yOffset, distance, getType(type), robotToCamera, -1);
+ }
+
+ /**
+ * Creates a new DetectedObject, assuming the object is on the ground
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ */
+ public DetectedObject(double xOffset, double yOffset, ObjectType type, Transform3d robotToCamera){
+ this(xOffset, yOffset, type, robotToCamera, -1);
+ }
+
+ /**
+ * Creates a new DetectedObject, assuming the object is on the ground
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ * @param timestamp The timestamp of the picture in seconds
+ */
+ public DetectedObject(double xOffset, double yOffset, ObjectType type, Transform3d robotToCamera, double timestamp){
+ this.type = type;
+ // Get the position relative to the camera
+ Translation3d translation = new Translation3d(1, new Rotation3d(0, -yOffset, -xOffset))
+ // Rotate it to get the position relative to the rotated camera
+ .rotateBy(robotToCamera.getRotation());
+ // Scale it so that the object will be on the ground (- because translation's z will be negative)
+ if(!isRobot()){
+ translation = translation.times(-(robotToCamera.getZ()-type.height)/translation.getZ());
+ }else{
+ // Assume all robots are ~3m from the camera
+ translation = translation.times(3);
+ }
+ // Translate it to make it relative to the robot
+ translation = translation.plus(robotToCamera.getTranslation());
+ // If the drivetrain exists, rotate and translate it to be field relative
+ if(drive != null){
+ Pose2d drivePose = drive.getPoseAt(timestamp);
+ translation = translation.rotateBy(new Rotation3d(
+ 0,
+ 0,
+ drivePose.getRotation().getRadians()
+ )).plus(new Translation3d(
+ drivePose.getX(),
+ drivePose.getY(),
+ 0
+ ));
+ }
+ pose = new Pose3d(translation, new Rotation3d());
+ }
+
+ /**
+ * Creates a new DetectedObject, assuming the object is on the ground
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ * @param timestamp The timestamp of the picture in seconds
+ */
+ public DetectedObject(double xOffset, double yOffset, int type, Transform3d robotToCamera, double timestamp){
+ this(xOffset, yOffset, getType(type), robotToCamera, timestamp);
+ }
+
+ /**
+ * Creates a new DetectedObject, assuming the object is on the ground
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ */
+ public DetectedObject(double xOffset, double yOffset, int type, Transform3d robotToCamera){
+ this(xOffset, yOffset, getType(type), robotToCamera, -1);
+ }
+
+ /**
+ * Creates a new DetectedObject, assuming the object is on the ground
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ * @param timestamp The timestamp of the picture in seconds
+ */
+ public DetectedObject(double xOffset, double yOffset, String type, Transform3d robotToCamera, double timestamp){
+ this(xOffset, yOffset, getType(type), robotToCamera, timestamp);
+ }
+
+ /**
+ * Creates a new DetectedObject, assuming the object is on the ground
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ */
+ public DetectedObject(double xOffset, double yOffset, String type, Transform3d robotToCamera){
+ this(xOffset, yOffset, getType(type), robotToCamera, -1);
+ }
+
+ /**
+ * Converts an int to an ObjectType
+ * @param type The type as an int, between 0 and the number of object types - 1
+ * @return The type as an ObjectType
+ */
+ public static ObjectType getType(int type){
+ ObjectType[] values = ObjectType.values();
+ if(type < 0 || type >= values.length){
+ return ObjectType.NONE;
+ }
+ return values[type];
+ }
+
+ /**
+ * Converts a String to an ObjectType
+ * @param type The type as a String
+ * @return The type as an ObjectType
+ */
+ public static ObjectType getType(String type){
+ ObjectType result = ObjectType.valueOf(type.toUpperCase());
+ return result==null ? ObjectType.NONE : result;
+ }
+
+ /**
+ * Returns if the object is a game piece
+ * @return True if the object is a game piece, false otherwise
+ */
+ public boolean isGamePiece(){
+ return type==ObjectType.CORAL || type==ObjectType.ALGAE;
+ }
+ /**
+ * Returns if the object is a robot
+ * @return True if the object is a red or blue robot, false otherwise
+ */
+ public boolean isRobot(){
+ return type==ObjectType.RED_ROBOT || type==ObjectType.BLUE_ROBOT;
+ }
+ /**
+ * Returns if the object is a robot on the same alliance
+ * @return If the object is a robot on the same alliance
+ */
+ public boolean isSameAllianceRobot(){
+ return type == (Robot.getAlliance()==Alliance.Red?ObjectType.RED_ROBOT:ObjectType.BLUE_ROBOT);
+ }
+ /**
+ * Returns if the object is a robot on the other alliance
+ * @return If the object is a robot on the other alliance
+ */
+ public boolean isOtherAllianceRobot(){
+ return type == (Robot.getAlliance()==Alliance.Red?ObjectType.BLUE_ROBOT:ObjectType.RED_ROBOT);
+ }
+
+ /**
+ * Gets the distance from the center of the robot to the object
+ * @return The distance in meters
+ */
+ public double getDistance(){
+ return drive.getPose().getTranslation().getDistance(pose.getTranslation().toTranslation2d());
+ }
+
+ /**
+ * Gets the field relative angle from the robot to the object
+ * @return The angle in radians
+ */
+ public double getAngle(){
+ Pose2d drivePose = drive.getPose();
+ return Math.atan2(pose.getY()-drivePose.getY(), pose.getX()-drivePose.getX());
+ }
+
+ /**
+ * Gets the angle relative to the front of the robot (0 is in front, positive counterclockwise)
+ * @return The relative angle in radians
+ */
+ public double getRelativeAngle(){
+ double angle = getAngle()-drive.getYaw().getRadians();
+ return MathUtil.angleModulus(angle);
+ }
+
+ /**
+ * Gets the angle of the object relative to the robot's velocity (0 is in front, positive counterclockwise)
+ * @return The relative angle in radians
+ */
+ public double getVelocityRelativeAngle(){
+ ChassisSpeeds speeds = drive.getChassisSpeeds();
+ double angle = getRelativeAngle() - Math.atan2(speeds.vyMetersPerSecond, speeds.vxMetersPerSecond);
+ return MathUtil.angleModulus(angle);
+ }
+
+ public String toString(){
+ return type+" at ("+pose.getX()+", "+pose.getY()+", "+pose.getZ()+")";
+ }
+}
--- /dev/null
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.util.Vision;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
+import frc.robot.constants.Constants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.SwerveStuff.SwerveSetpoint;
+import frc.robot.util.SwerveStuff.SwerveSetpointGenerator;
+
+/**
+ * A util class to assist the driver drive to a pose
+ */
+public class DriverAssist {
+ // The amount to correct the driver's inpus by
+ // 0 = return unchanged driver inputs, 1 = return a value much closer to the calculated speed, sometimes equal to it
+ // This can be greater than 1 to fully correct more of the time, like from farther away
+ private static final double CORRECTION_FACTOR = 1;
+
+
+ // Variables used for first method
+ // The setpoint generator, which limits the acceleration
+ private static final SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator();
+ private static final TrapezoidProfile xProfile = new TrapezoidProfile(new Constraints(DriveConstants.MAX_SPEED, DriveConstants.MAX_LINEAR_ACCEL));
+ private static final TrapezoidProfile yProfile = new TrapezoidProfile(new Constraints(DriveConstants.MAX_SPEED, DriveConstants.MAX_LINEAR_ACCEL));
+ private static final TrapezoidProfile angleProfile = new TrapezoidProfile(new Constraints(DriveConstants.MAX_ANGULAR_SPEED, DriveConstants.MAX_ANGULAR_ACCEL));
+
+ /**
+ * Combines the driver input with a speed calculated using a trapezoidal profile <p>
+ * Called when VisionConstants.DRIVER_ASSIST_MODE is 2
+ * @param drive The drivetrain
+ * @param driverInput The driver input speed
+ * @param desiredPose The pose to drive to
+ * @param keepAngle True to use the angle in the pose, false to point hte robot toward the pose
+ * @return The new speed
+ */
+ private static ChassisSpeeds calculate2(Drivetrain drive, ChassisSpeeds driverInput, Pose2d desiredPose, boolean keepAngle) {
+ // Do nothing if there is no pose
+ if(desiredPose == null){
+ return driverInput;
+ }
+
+ // Store current states
+ Pose2d currentPose = drive.getPose();
+ Rotation2d yaw = drive.getYaw();
+ ChassisSpeeds driveSpeeds = drive.getChassisSpeeds();
+ driveSpeeds = ChassisSpeeds. fromFieldRelativeSpeeds(driveSpeeds,yaw); // Changing this does not cause problems because getChassisSpeeds() creates a new object
+ State xState = new State(currentPose.getX(), driveSpeeds.vxMetersPerSecond);
+ State yState = new State(currentPose.getY(), driveSpeeds.vyMetersPerSecond);
+ State angleState = new State(currentPose.getRotation().getRadians(), driveSpeeds.omegaRadiansPerSecond);
+
+ // Store goal states
+ State xGoal = new State(desiredPose.getX(), 0);
+ State yGoal = new State(desiredPose.getY(), 0);
+ Translation2d difference = desiredPose.getTranslation().minus(currentPose.getTranslation());
+ double rotation = keepAngle ? desiredPose.getRotation().getRadians() : difference.getAngle().getRadians();
+ if(rotation - currentPose.getRotation().getRadians() > Math.PI){
+ rotation -= 2*Math.PI;
+ }else if(rotation - currentPose.getRotation().getRadians() < -Math.PI){
+ rotation += 2*Math.PI;
+ }
+ State angleGoal = new State(rotation, 0);
+
+ // Calculate ideal speeds for next frame
+ ChassisSpeeds goal = new ChassisSpeeds(
+ xProfile.calculate(Constants.LOOP_TIME, xState, xGoal).velocity,
+ yProfile.calculate(Constants.LOOP_TIME, yState, yGoal).velocity,
+ angleProfile.calculate(Constants.LOOP_TIME, angleState, angleGoal).velocity
+ );
+ // Robot-relataive goal
+ ChassisSpeeds goalRobot = goal.times(1);
+ goalRobot = ChassisSpeeds. fromRobotRelativeSpeeds(goalRobot, yaw);
+
+ // This calculates the actual acceleration we can get
+ // This is the only thing that needs to be robot relative
+ SwerveSetpoint nextSetpoint = setpointGenerator.generateSetpoint(
+ DriveConstants.MODULE_LIMITS,
+ 0,
+ drive.getCurrSetpoint(), goalRobot,
+ Constants.LOOP_TIME);
+ ChassisSpeeds nextChassisSpeed = nextSetpoint.chassisSpeeds();
+ nextChassisSpeed = ChassisSpeeds.fromRobotRelativeSpeeds(nextChassisSpeed, yaw);
+
+ // Robot relative driver inputs
+ ChassisSpeeds driverInputRobot = driverInput.times(1); // Copy so original doesn't change
+ driverInputRobot = ChassisSpeeds.fromFieldRelativeSpeeds(driverInputRobot, yaw);
+ // This is the speed the driver will be able to get next frame
+ // Both speeds need to be obtainable in 1 frame or the driver speed will always be farther away
+ SwerveSetpoint driverSetpoint = setpointGenerator.generateSetpoint(
+ DriveConstants.MODULE_LIMITS,
+ 0,
+ drive.getCurrSetpoint(), driverInputRobot,
+ Constants.LOOP_TIME);
+ ChassisSpeeds driverSpeeds = driverSetpoint.chassisSpeeds();
+ driverSpeeds= ChassisSpeeds.fromRobotRelativeSpeeds(driverSpeeds,yaw);
+
+ // The difference between the 2 speeds
+ ChassisSpeeds error = nextChassisSpeed.minus(driverSpeeds);
+
+ // 1.2*1.2^-distance decreases the amount it correct by as distance increases
+ double distanceFactor = 1.2*Math.pow(1.2, -currentPose.getTranslation().getDistance(desiredPose.getTranslation()));
+
+ // Driver input speed
+ double driverInputSpeed = Math.hypot(driverInput.vxMetersPerSecond, driverInput.vyMetersPerSecond);
+
+ // The amount to correct by
+ ChassisSpeeds correction = error.times(Math.min(CORRECTION_FACTOR * distanceFactor * driverInputSpeed / DriveConstants.MAX_SPEED, 1));
+
+ return driverSpeeds.plus(correction);
+ // return nextChassisSpeed.times(CORRECTION_FACTOR).plus(driverInput.times(1-CORRECTION_FACTOR));
+ }
+
+ // Constants used for second method
+ public static final double MAX_VELOCITY_ANGLE_ERROR = Math.PI/4;
+ public static final double MAX_DISTANCE_ERROR = 2;
+ public static final double ROTATION_CORRECTION_FACTOR = 0.1;
+ public static final double MAX_ROTATION_ERROR = Math.PI/3;
+
+ /**
+ * Combines the driver input with a calculated correction speed
+ * @param drive The drivetrain
+ * @param driverInput The driver input speed
+ * @param desiredPose The pose to drive to
+ * @param keepAngle True to use the angle in the pose, false to point hte robot toward the pose
+ * @return The new speed
+ */
+ @SuppressWarnings("unused") // Needed because some code might not run for some values of DRIVER_ASSIST_MODE
+ public static ChassisSpeeds calculate(Drivetrain drive, ChassisSpeeds driverInput, Pose2d desiredPose, boolean keepAngle) {
+ if(VisionConstants.DRIVER_ASSIST_MODE < 2 || desiredPose == null){
+ return driverInput;
+ }else if(VisionConstants.DRIVER_ASSIST_MODE == 2){
+ return calculate2(drive, driverInput, desiredPose, keepAngle);
+ }
+ // Combines the driver input with a speed perpendicular to the input
+ Pose2d drivePose = drive.getPose();
+ Translation2d difference = desiredPose.getTranslation().minus(drivePose.getTranslation());
+ double distance = difference.getNorm();
+ double velocityAngle = difference.getAngle().getRadians();
+ double targetAngle = keepAngle ? desiredPose.getRotation().getRadians() : MathUtil.angleModulus(velocityAngle + Math.PI/2);
+ double inputSpeed = Math.hypot(driverInput.vxMetersPerSecond, driverInput.vyMetersPerSecond);
+ double driverAngle = Math.atan2(driverInput.vyMetersPerSecond, driverInput.vxMetersPerSecond);
+ double velocityAngleError = MathUtil.angleModulus(velocityAngle - driverAngle);
+ if(Math.abs(velocityAngleError) > MAX_VELOCITY_ANGLE_ERROR){
+ return driverInput;
+ }
+ double perpendicularDist = Math.abs(distance * Math.sin(velocityAngleError));
+ if(perpendicularDist > MAX_DISTANCE_ERROR){
+ return driverInput;
+ }
+ double perpendicularAngle = MathUtil.angleModulus(driverAngle + Math.PI/2*Math.signum(velocityAngleError));
+ // Different options for calculation.
+ double correctionSpeed = 0;
+ switch(VisionConstants.DRIVER_ASSIST_MODE){
+ case 3:
+ correctionSpeed = Math.min(CORRECTION_FACTOR * inputSpeed * Math.pow(2, -perpendicularDist), Math.abs(Math.tan(velocityAngleError)*inputSpeed));
+ break;
+ case 4:
+ correctionSpeed = Math.min(CORRECTION_FACTOR * Math.pow(1.5, -perpendicularDist), 1) * Math.abs(Math.tan(velocityAngleError)*inputSpeed);
+ break;
+ case 5:
+ correctionSpeed = Math.min(CORRECTION_FACTOR * inputSpeed * Math.pow(2, -perpendicularDist) * Math.pow(1.2, -distance+1), Math.abs(Math.tan(velocityAngleError)*inputSpeed));
+ break;
+ }
+ double rotationError = MathUtil.angleModulus(targetAngle-drivePose.getRotation().getRadians());
+ if(Math.abs(rotationError) > MAX_ROTATION_ERROR){
+ return driverInput;
+ }
+ // We want to set the current angular velocity so that we can decelerate to 0rad/s at the setpoint
+ // Since 0=v0^2+2ax, v0=√(2ax)
+ // High correction factors will also ignore the driver's input more
+ double rotationalSpeed = ROTATION_CORRECTION_FACTOR * Math.signum(rotationError)*Math.sqrt(2*DriveConstants.MAX_ANGULAR_ACCEL*Math.abs(rotationError)) - ROTATION_CORRECTION_FACTOR * driverInput.omegaRadiansPerSecond;
+ return driverInput.plus(new ChassisSpeeds(correctionSpeed*Math.cos(perpendicularAngle), correctionSpeed*Math.sin(perpendicularAngle), rotationalSpeed));
+ }
+}
--- /dev/null
+package frc.robot.util.Vision;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose3d;
+import frc.robot.constants.GyroBiasConstants;
+
+/**
+ * estimates gyro bias by comparing vision-derived yaw to gyro yaw.
+ *
+ * when the robot observes April tags, PhotonVision calculates what the robot heading
+ * SHOULD be based on known tag positions vs observed angles. This can be compared to
+ * the gyro reading to detect and correct drift.
+ *
+ */
+public class GyroBiasEstimator {
+
+ private double weightedBiasSum = 0.0;
+ private double totalWeight = 0.0;
+ private int sampleCount = 0;
+
+ // exponential moving average
+ private double emaBias = 0.0;
+ private boolean emaInitialized = false;
+
+ /**
+ * process a new observation comparing vision pose to gyro reading.
+ *
+ * @param visionPose the pose estimated by vision (from PhotonVision)
+ * @param gyroYaw current gyro reading in radians
+ * @param visionWeight weight for observation (0.0 to 1.0, higher is more trusted)
+ * @return true if bias should be applied (has enough samples)
+ */
+ public boolean addObservation(Pose3d visionPose, double gyroYaw, double visionWeight) {
+ if (visionPose == null) {
+ return false;
+ }
+
+ // get yaw from vision
+ double visionYaw = visionPose.getRotation().getZ();
+
+ return addObservation(visionYaw, gyroYaw, visionWeight);
+ }
+
+ /**
+ * process a new observation with just yaw values.
+ *
+ * @param visionYaw yaw from vision pose in radians
+ * @param gyroYaw current gyro reading in radians
+ * @param visionWeight weight for this observation (0.0 to 1.0, higher is more trusted)
+ * @return true if bias should be applied
+ */
+ public boolean addObservation(double visionYaw, double gyroYaw, double visionWeight) {
+ // normalize to [-PI, PI]
+ double diff = normalizeAngle(visionYaw - gyroYaw);
+
+ // reject outliers
+ if (Math.abs(diff) > GyroBiasConstants.MAX_ANGLE_DIFF_RAD) {
+ return false;
+ }
+
+ // clamp weight
+ double weight = Math.max(0.0, Math.min(1.0, visionWeight));
+
+ // accumulate weighted bias
+ weightedBiasSum += diff * weight;
+ totalWeight += weight;
+ sampleCount++;
+
+ // update exponential moving average
+ if (!emaInitialized) {
+ emaBias = diff;
+ emaInitialized = true;
+ } else {
+ emaBias = emaBias * (1.0 - GyroBiasConstants.EMA_ALPHA) + diff * GyroBiasConstants.EMA_ALPHA;
+ }
+
+ return sampleCount >= GyroBiasConstants.MIN_SAMPLES;
+ }
+
+ /**
+ * process new observation with default weight of 1.0.
+ * maintains backward compatibility.
+ */
+ public boolean addObservation(Pose3d visionPose, double gyroYaw) {
+ return addObservation(visionPose, gyroYaw, 1.0);
+ }
+
+ /**
+ * process new observation with default weight of 1.0.
+ * maintains backward compatibility
+ */
+ public boolean addObservation(double visionYaw, double gyroYaw) {
+ return addObservation(visionYaw, gyroYaw, 1.0);
+ }
+
+ /**
+ * get average bias and reset
+ *
+ * @return average bias in radians to apply, or 0 if not enough samples
+ */
+ public double getAndResetBias() {
+ if (sampleCount < GyroBiasConstants.MIN_SAMPLES) {
+ return 0.0;
+ }
+
+ // use weighted average
+ double avgBias = weightedBiasSum / totalWeight;
+
+ // reset
+ weightedBiasSum = 0.0;
+ totalWeight = 0.0;
+ sampleCount = 0;
+ emaInitialized = false;
+
+ return avgBias;
+ }
+
+ /**
+ * apply partial correction to avoid sudden jumps.
+ *
+ * @param fullBias the full calculated bias
+ * @return partial correction to apply
+ */
+ public double applyPartialCorrection(double fullBias) {
+ double clampedBias = fullBias;
+ if (clampedBias > GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD) {
+ clampedBias = GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD;
+ } else if (clampedBias < -GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD) {
+ clampedBias = -GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD;
+ }
+
+ return clampedBias * GyroBiasConstants.CORRECTION_FRACTION;
+ }
+
+ /**
+ * normalize angle to [-PI, PI]
+ */
+ private double normalizeAngle(double angle) {
+ return MathUtil.angleModulus(angle);
+ }
+
+ /**
+ * get sample count for debugging
+ */
+ public int getSampleCount() {
+ return sampleCount;
+ }
+
+ /**
+ * get current accumulated bias without resetting
+ */
+ public double getCurrentBias() {
+ if (sampleCount == 0) {
+ return 0.0;
+ }
+ if (totalWeight > 0) {
+ return weightedBiasSum / totalWeight;
+ }
+ return emaBias;
+ }
+
+ /**
+ * get current total weight for debugging
+ */
+ public double getTotalWeight() {
+ return totalWeight;
+ }
+
+ /**
+ * reset accumulated state
+ */
+ public void reset() {
+ weightedBiasSum = 0.0;
+ totalWeight = 0.0;
+ sampleCount = 0;
+ emaInitialized = false;
+ }
+}
--- /dev/null
+package frc.robot.util.Vision;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Optional;
+import java.util.function.DoubleUnaryOperator;
+
+import org.littletonrobotics.junction.Logger;
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.simulation.PhotonCameraSim;
+import org.photonvision.simulation.VisionSystemSim;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.constants.Constants;
+import frc.robot.constants.FieldConstants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.util.MathUtils;
+
+// Vision and it's commands are adapted from Iron Claw's FRC2023
+public class Vision {
+ private NetworkTable objectDetectionTable;
+
+ private NetworkTableEntry xOffset;
+ private NetworkTableEntry yOffset;
+ private NetworkTableEntry objectDistance;
+ private NetworkTableEntry objectClass;
+ private NetworkTableEntry cameraIndex;
+
+ // A list of the cameras on the robot.
+ private ArrayList<VisionCamera> cameras = new ArrayList<>();
+
+ private VisionSystemSim visionSim;
+
+ private boolean sawTag = false;
+
+ // Array of tags to use, null or empty array to use all tags
+ private int[] onlyUse = null;
+
+ /**
+ * Creates a new instance of Vision and sets up the cameras and field layout
+ */
+ public Vision(ArrayList<Pair<String, Transform3d>> camList) {
+ // Initialize object_detection NetworkTable
+ objectDetectionTable = NetworkTableInstance.getDefault().getTable("object_detection");
+
+ // From the object detection NetworkTable, get the entries
+ objectDistance = objectDetectionTable.getEntry("distance");
+ xOffset = objectDetectionTable.getEntry("x_offset");
+ yOffset = objectDetectionTable.getEntry("y_offset");
+ objectClass = objectDetectionTable.getEntry("class");
+ cameraIndex = objectDetectionTable.getEntry("index");
+
+ // Start NetworkTables server
+ NetworkTableInstance.getDefault().startServer();
+
+ // Sets the origin to the right side of the blue alliance wall
+ FieldConstants.field.setOrigin(OriginPosition.kBlueAllianceWallRightSide);
+
+ if(VisionConstants.ENABLED){
+ // Puts the cameras in an array list
+ for (int i = 0; i < camList.size(); i++) {
+ cameras.add(new VisionCamera(camList.get(i).getFirst(), camList.get(i).getSecond()));
+ }
+
+ if(RobotBase.isSimulation()){
+ visionSim = new VisionSystemSim("Vision");
+ visionSim.addAprilTags(FieldConstants.field);
+ for(VisionCamera c : cameras){
+ PhotonCameraSim cameraSim = new PhotonCameraSim(c.camera);
+ cameraSim.enableDrawWireframe(true);
+ cameraSim.prop.setAvgLatencyMs(30);
+ cameraSim.prop.setCalibration(1280, 720, Rotation2d.fromDegrees(78));
+ visionSim.addCamera(cameraSim, c.photonPoseEstimator.getRobotToCameraTransform());
+ }
+ }
+ }
+
+ Pose3d[] tags = new Pose3d[FieldConstants.field.getTags().size()];
+ for (int i = 0; i < FieldConstants.field.getTags().size(); i++) {
+ tags[i] = (FieldConstants.field.getTagPose(i+1).get());
+ }
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("AprilTags", tags);
+ }
+ }
+
+
+ /**
+ * Get the horizontal offsets from the crosshair to the targets
+ * @return An array of offsets in degrees
+ */
+ public double[] getHorizontalOffset(){
+ if(!VisionConstants.OBJECT_DETECTION_ENABLED){
+ return new double[0];
+ }
+ return xOffset.getDoubleArray(new double[0]);
+ }
+
+ /**
+ * Get the vertical offsets from the crosshair to the targets
+ * @return An array of offsets in degrees
+ */
+ public double[] getVerticalOffset(){
+ if(!VisionConstants.OBJECT_DETECTION_ENABLED){
+ return new double[0];
+ }
+ return yOffset.getDoubleArray(new double[0]);
+ }
+
+ /**
+ * Get the target distances
+ * @return Distance in meters
+ */
+ @SuppressWarnings("unused")
+ public double[] getDistance(){
+ if(!VisionConstants.OBJECT_DETECTION_ENABLED || true){
+ return new double[0];
+ }
+ return objectDistance.getDoubleArray(new double[0]);
+ }
+
+ /**
+ * Returns whether or not a valid object is detected
+ * @return true or false
+ */
+ public boolean validObjectDetected(){
+ return getHorizontalOffset().length > 0;
+ }
+
+ /**
+ * Returns what types of object are detected
+ * @return The object types as a String array
+ */
+ @SuppressWarnings("unused")
+ public String[] getDetectedObjectClass(){
+ if(!VisionConstants.OBJECT_DETECTION_ENABLED || true){
+ return new String[0];
+ }
+ return objectClass.getStringArray(new String[0]);
+ }
+
+ /**
+ * Gets the camera indices (which camera sees the object)
+ * @return The indices as a long array (method returns long array instead of int array)
+ */
+ @SuppressWarnings("unused")
+ public long[] getCameraIndex(){
+ if(!VisionConstants.OBJECT_DETECTION_ENABLED || true){
+ return new long[0];
+ }
+ return cameraIndex.getIntegerArray(new long[0]);
+ }
+
+ /**
+ * Stores all of the detected objects in an array
+ * @return The array of DetectedObjects
+ */
+ public DetectedObject[] getDetectedObjects(){
+ if(!VisionConstants.OBJECT_DETECTION_ENABLED){
+ return new DetectedObject[0];
+ }
+ double[] xOffset = getHorizontalOffset();
+ double[] yOffset = getVerticalOffset();
+ // double[] distance = getDistance();
+ String[] objectClass = getDetectedObjectClass();
+ // long[] cameraIndex = getCameraIndex();
+ DetectedObject[] objects = new DetectedObject[Math.min(xOffset.length, yOffset.length)];
+ for(int i = 0; i < objects.length; i++){
+ objects[i] = new DetectedObject(
+ Units.degreesToRadians(xOffset[i]),
+ -Units.degreesToRadians(yOffset[i]),
+ // distance[i],
+ objectClass[i],
+ // VisionConstants.OBJECT_DETECTION_CAMERAS.get((int)cameraIndex[i]).getSecond()
+ VisionConstants.OBJECT_DETECTION_CAMERAS.get(0)
+ );
+ }
+ return objects;
+ }
+
+ /**
+ * Returns the closest game piece in front of the robot
+ * @param maxAngle The maximum angle between the angle to the object and the robot's heading or rotation to use, in radians
+ * @param relativeToVelocity Whether to compare the angle to the robot's heading or rotation, true for heading
+ * @return The best DetectedObject
+ */
+ public DetectedObject getBestGamePiece(double maxAngle, boolean relativeToVelocity){
+ DetectedObject[] objects = getDetectedObjects();
+ DetectedObject best = null;
+ double closest = Double.POSITIVE_INFINITY;
+ for(DetectedObject object : objects){
+ double dist = object.getDistance();
+ if(object.isGamePiece() && Math.abs(relativeToVelocity ? object.getVelocityRelativeAngle() : object.getAngle()) < maxAngle && dist < closest){
+ closest = dist;
+ best = object;
+ }
+ }
+ return best;
+ }
+
+ /**
+ * Gets the pose as a Pose2d using PhotonVision
+ * @param referencePoses The reference poses in order of preference, null poses will be skipped
+ * @return The pose of the robot, or null if it can't see april tags
+ */
+ public Pose2d getPose2d(Pose2d... referencePoses){
+ Pose2d referencePose = new Pose2d();
+ for (Pose2d checkReferencePose:referencePoses){
+ if (checkReferencePose != null) {
+ referencePose = checkReferencePose;
+ break;
+ }
+ }
+ ArrayList<EstimatedRobotPose> estimatedPoses = getEstimatedPoses(referencePose);
+
+ if (estimatedPoses.size() == 0) return null;
+
+ if (estimatedPoses.size() == 1) return estimatedPoses.get(0).estimatedPose.toPose2d();
+
+ if (estimatedPoses.size() == 2) {
+ return new Pose2d(
+ estimatedPoses.get(0).estimatedPose.toPose2d().getTranslation()
+ .plus(estimatedPoses.get(1).estimatedPose.toPose2d().getTranslation())
+ .div(2),
+
+ new Rotation2d(MathUtils.modulusMidpoint(
+ estimatedPoses.get(0).estimatedPose.toPose2d().getRotation().getRadians(),
+ estimatedPoses.get(1).estimatedPose.toPose2d().getRotation().getRadians(),
+ -Math.PI, Math.PI)
+ )
+ );
+ }
+
+ // The average translation is just the average of all of the translations (sum divided by total)
+ // Average angle is similar, except every step needs to use a modulus, since -π is the same angle as π
+ // This calculation is essentially newAverage = (oldAverage * valuesInOldAverage + nextValue) / newNumberOfValues
+ Translation2d translation = new Translation2d();
+ double angle = 0;
+ for(int i = 0; i < estimatedPoses.size(); i ++){
+ translation = translation.plus(estimatedPoses.get(i).estimatedPose.toPose2d().getTranslation());
+ angle = MathUtils.modulusInterpolate(angle, estimatedPoses.get(i).estimatedPose.toPose2d().getRotation().getRadians(), 1.0/(i+1), -Math.PI, Math.PI);
+ }
+
+ return new Pose2d(translation.div(estimatedPoses.size()), new Rotation2d(angle));
+ }
+
+ public AprilTagFieldLayout getAprilTagFieldLayout(){
+ return FieldConstants.field;
+ }
+
+ /**
+ * Gets the pose of an april tag
+ * @param id AprilTag id (1-8)
+ * @return Pose3d of the AprilTag
+ */
+ public Pose3d getTagPose(int id){
+ if(id < 1 || id > getAprilTagFieldLayout().getTags().size()){
+ System.out.println("Tried to find the pose of april tag "+id);
+ return null;
+ }
+ return getAprilTagFieldLayout().getTags().get(id-1).pose;
+ }
+
+ /**
+ * Returns where it thinks the robot is
+ * @param referencePose The pose to use as a reference, usually the previous robot pose
+ * @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time
+ * @return An array list of estimated poses, one for each camera that can see an april tag
+ */
+ public ArrayList<EstimatedRobotPose> getEstimatedPoses(Pose2d referencePose) {
+ return getEstimatedPoses(referencePose, ignoree->referencePose.getRotation().getRadians());
+ }
+
+ /**
+ * Returns where it thinks the robot is
+ * @param referencePose The pose to use as a reference, usually the previous robot pose
+ * @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time
+ * @return An array list of estimated poses, one for each camera that can see an april tag
+ */
+ public ArrayList<EstimatedRobotPose> getEstimatedPoses(Pose2d referencePose, DoubleUnaryOperator yawFunction) {
+ ArrayList<EstimatedRobotPose> estimatedPoses = new ArrayList<>();
+ for (int i = 0; i < cameras.size(); i++) {
+ if(VisionConstants.USE_MANUAL_CALCULATIONS){
+ for(EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(yawFunction)){
+ if(pose != null){
+ estimatedPoses.add(pose);
+ }
+ }
+ }else{
+ for(EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(referencePose)){
+ // If the camera can see an april tag that exists, add it to the array list
+ // April tags that don't exist might return a result that is present but doesn't have a pose
+ if (pose.estimatedPose != null) {
+ estimatedPoses.add(pose);
+
+ }
+ }
+ }
+ }
+ if(estimatedPoses.size() > 1){
+ Translation2d average = new Translation2d();
+ for(EstimatedRobotPose pose : estimatedPoses){
+ average = average.plus(pose.estimatedPose.getTranslation().toTranslation2d());
+ }
+ average = average.div(estimatedPoses.size());
+ for(int i = estimatedPoses.size()-1; i>=0; i--){
+ if(estimatedPoses.get(i).estimatedPose.getTranslation().toTranslation2d().getDistance(average) > VisionConstants.MAX_POSE_DIFFERENCE/2){
+ estimatedPoses.remove(i);
+ }
+ }
+ }
+ return estimatedPoses;
+ }
+
+ /**
+ * Updates the robot's odometry with vision
+ * @param poseEstimator The pose estimator to update
+ * @param yawFunction A function that returns the yaw as a double given the timestamp
+ * @param slipped True if the wheels have slipped, false otherwise
+ * @return The list of estimated robot poses from vision
+ */
+ public ArrayList<EstimatedRobotPose> updateOdometry(SwerveDrivePoseEstimator poseEstimator, DoubleUnaryOperator yawFunction, boolean slipped){
+ // Simulate vision
+ // 2 ifs to avoid warning
+ if(VisionConstants.ENABLED_SIM){
+ if(RobotBase.isSimulation()){
+ visionSim.update(poseEstimator.getEstimatedPosition());
+ }
+ }
+
+ sawTag = false;
+
+ // An array list of poses returned by different cameras
+ ArrayList<EstimatedRobotPose> estimatedPoses = getEstimatedPoses(poseEstimator.getEstimatedPosition(), yawFunction);
+ for (EstimatedRobotPose estimatedPose : estimatedPoses) {
+ // Continue if this pose doesn't exist
+ if(estimatedPose.timestampSeconds < 0 || !onField(estimatedPose.estimatedPose.toPose2d()) || Timer.getFPGATimestamp() < estimatedPose.timestampSeconds || Timer.getFPGATimestamp() > estimatedPose.timestampSeconds + 1){
+ continue;
+ }
+
+ poseEstimator.addVisionMeasurement(
+ estimatedPose.estimatedPose.toPose2d(),
+ estimatedPose.timestampSeconds,
+ slipped ? VisionConstants.VISION_STD_DEVS_2 : VisionConstants.VISION_STD_DEVS
+ );
+ sawTag = true;
+ }
+ return estimatedPoses;
+ }
+
+ /**
+ * Updates each camera's inputs for logging
+ */
+ public void updateInputs(){
+ for(VisionCamera c : cameras){
+ c.updateInputs();
+ }
+ }
+
+ /**
+ * If vision saw any April tags last frame
+ * @return If vision saw an April tag last frame
+ */
+ public boolean canSeeTag(){
+ return sawTag;
+ }
+
+ /**
+ * Enable or disable a single camera
+ * @param index The camera index
+ * @param enabled If it should be enabled or disabled
+ */
+ public void enableCamera(int index, boolean enabled){
+ try{
+ cameras.get(index).enable(enabled);
+ }catch(IndexOutOfBoundsException e){
+ DriverStation.reportWarning("Camera index "+index+" is out of bounds", false);
+ }
+ }
+ /**
+ * Sets the cameras to only use April tag in the specified array
+ * @param ids The ids of the tags to use, null or empty array to use all
+ */
+ public void onlyUse(int[] ids){
+ onlyUse = ids;
+ }
+
+ /**
+ * Checks if one or more cameras are disconnected
+ * @return true if at least one camera is disconnected, false otherwise
+ */
+ public boolean oneCameraDisconnected(){
+ for(VisionCamera camera : cameras){
+ if(!camera.inputs.connected){
+ return true;
+ }
+ }
+ return false;
+ }
+
+ /**
+ * Checks if a pose is on the field
+ * @param pose The pose to check
+ * @return If the pose is on the field
+ */
+ public static boolean onField(Pose2d pose){
+ return pose!=null && pose.getX()>0 && pose.getX()<FieldConstants.field.getFieldLength() && pose.getY()>0 && pose.getY()<FieldConstants.field.getFieldWidth();
+ }
+
+ /**
+ * Checks if a pose is on or near the field
+ * @param pose The pose to check
+ * @return If the pose is within an area with twice the length and width of the field
+ */
+ public static boolean nearField(Pose2d pose){
+ return pose!=null && pose.getX()>-FieldConstants.field.getFieldLength()/2 && pose.getX()<FieldConstants.field.getFieldLength()*1.5 && pose.getY()>-FieldConstants.field.getFieldWidth()/2 && pose.getY()<FieldConstants.field.getFieldWidth()*1.5;
+ }
+
+ private class VisionCamera implements VisionIO {
+ private PhotonCamera camera;
+ private PhotonPoseEstimator photonPoseEstimator;
+ private Pose2d lastPose;
+ private double lastTimestamp = 0;
+ private boolean enabled = true;
+ private final VisionIOInputs inputs = new VisionIOInputs();
+
+ /**
+ * Stores information about a camera
+ * @param cameraName The name of the camera on PhotonVision
+ * @param robotToCam The transformation from the robot to the camera
+ */
+ public VisionCamera(String cameraName, Transform3d robotToCam) {
+ camera = new PhotonCamera(cameraName);
+ photonPoseEstimator = new PhotonPoseEstimator(
+ FieldConstants.field,
+ robotToCam
+ );
+ lastPose = null;
+ }
+
+ /**
+ * Gets the estimated poses from the camera
+ * @param referencePose Pose to use for reference, usually the previous estimated robot pose
+ * @return estimated robot poses
+ */
+ public ArrayList<EstimatedRobotPose> getEstimatedPose(Pose2d referencePose) {
+
+ ArrayList<EstimatedRobotPose> list = new ArrayList<>();
+
+ if(!enabled){
+ return list;
+ }
+
+ for(PhotonPipelineResult cameraResult : inputs.results){
+ if(!cameraResult.hasTargets() || cameraResult.getTimestampSeconds()<0){
+ continue;
+ }
+
+ // if there is a target detected and the timestamp exists,
+ // check the ambiguity isn't too high
+ List<PhotonTrackedTarget> targetsUsed = cameraResult.targets;
+ for (int i = targetsUsed.size()-1; i >= 0; i--) {
+ // Remove it from the list if it should not be used or if it has too high of an ambiguity
+ if(!useTag(targetsUsed.get(i).getFiducialId()) || targetsUsed.get(i).getPoseAmbiguity() > VisionConstants.HIGHEST_AMBIGUITY || targetsUsed.get(i).bestCameraToTarget.getTranslation().getNorm() > VisionConstants.MAX_DISTANCE){
+ targetsUsed.remove(i);
+ }
+ }
+
+ // If there are no targets, the timestamp doesn't exist, or there there is only 1 tag and the constant is set to only use 2 tags, continue
+ if(targetsUsed.size() == 0 || cameraResult.getTimestampSeconds()<0 || targetsUsed.size()==1 && VisionConstants.ONLY_USE_2_TAGS){
+ continue;
+ }
+
+ // Set strategy to single tag if there is only 1 good tag and update
+ PhotonPoseEstimator.PoseStrategy poseStrategy = targetsUsed.size() > 1 ? VisionConstants.POSE_STRATEGY : VisionConstants.MULTITAG_FALLBACK_STRATEGY;
+ Optional<EstimatedRobotPose> pose;
+ switch (poseStrategy) {
+ case AVERAGE_BEST_TARGETS:
+ pose = photonPoseEstimator.estimateAverageBestTargetsPose(cameraResult);
+ break;
+ case CLOSEST_TO_CAMERA_HEIGHT:
+ pose = photonPoseEstimator.estimateClosestToCameraHeightPose(cameraResult);
+ break;
+ case CLOSEST_TO_REFERENCE_POSE:
+ pose = photonPoseEstimator.estimateClosestToReferencePose(cameraResult, new Pose3d(referencePose));
+ break;
+ case LOWEST_AMBIGUITY:
+ pose = photonPoseEstimator.estimateLowestAmbiguityPose(cameraResult);
+ break;
+ case MULTI_TAG_PNP_ON_COPROCESSOR:
+ pose = photonPoseEstimator.estimateCoprocMultiTagPose(cameraResult);
+ break;
+ case PNP_DISTANCE_TRIG_SOLVE:
+ pose = photonPoseEstimator.estimatePnpDistanceTrigSolvePose(cameraResult);
+ break;
+ case CLOSEST_TO_LAST_POSE:
+ case CONSTRAINED_SOLVEPNP:
+ case MULTI_TAG_PNP_ON_RIO:
+ default:
+ throw new RuntimeException("Pose estimation method " + poseStrategy.toString() + " is not supported.");
+ }
+
+ if(pose.isPresent() && pose.get()!=null && onField(pose.get().estimatedPose.toPose2d())){
+ double timestamp = cameraResult.getTimestampSeconds();
+
+ // If the pose moved too much, don't use it
+ if(lastPose==null || lastPose.getTranslation().getDistance(pose.get().estimatedPose.toPose2d().getTranslation()) > DriveConstants.MAX_SPEED*1.25*(timestamp-lastTimestamp) || timestamp < lastTimestamp){
+ lastPose = pose.get().estimatedPose.toPose2d();
+ lastTimestamp = timestamp;
+ continue;
+ }
+
+ // Otherwise, add the pose to the list
+ lastPose = pose.get().estimatedPose.toPose2d();
+ lastTimestamp = timestamp;
+ list.add(pose.get());
+ }
+ }
+ return list;
+ }
+
+ /**
+ * Updates the VisionIOInputs object with the results from PhotonVision for logging
+ */
+ @Override
+ public void updateInputs() {
+ inputs.connected = camera.isConnected();
+ inputs.results = camera.getAllUnreadResults();
+
+ Logger.processInputs("Vision/"+camera.getName(), inputs);
+ }
+
+ /**
+ * Gets the pose using manual calculations
+ * @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time
+ * @return A list of estimated poses as EstimatedRobotPoses
+ */
+ public ArrayList<EstimatedRobotPose> getEstimatedPose(DoubleUnaryOperator yawFunction){
+ ArrayList<EstimatedRobotPose> list = new ArrayList<>();
+
+ // Do nothing if this camera is disabled
+ if(!enabled){
+ return list;
+ }
+
+ // The latest camera results
+ for(PhotonPipelineResult result : inputs.results){
+ // TODO: This could be improved by averaging all targets instead of only using 1
+
+ // Continue if the target doesn't exist or it should be ignored
+ if (!result.hasTargets()) continue;
+ // Gets the best target to use for the calculations
+ PhotonTrackedTarget target = result.getBestTarget();
+ // I don't know why this would happen, but keep it in just in case
+ if(target==null){
+ continue;
+ }
+ // Continue if the id is too high or too low
+ int id = target.getFiducialId();
+ if(!useTag(id) || target.bestCameraToTarget.getTranslation().getNorm() > VisionConstants.MAX_DISTANCE || target.poseAmbiguity > VisionConstants.HIGHEST_AMBIGUITY){
+ continue;
+ }
+ // Stores target pose and robot to camera transformation for easy access later
+ Pose3d targetPose = FieldConstants.field.getTagPose(id).get();
+ Transform3d robotToCamera = photonPoseEstimator.getRobotToCameraTransform();
+
+ double timestamp = result.getTimestampSeconds();
+ double yaw = yawFunction.applyAsDouble(timestamp);
+
+ // Get the tag position relative to the robot, assuming the robot is on the ground
+ Translation3d translation = target.getBestCameraToTarget().getTranslation()
+ .rotateBy(robotToCamera.getRotation());
+ translation = translation//.times((targetPose.getZ()-robotToCamera.getZ())/translation.getZ())
+ .plus(robotToCamera.getTranslation())
+ .rotateBy(new Rotation3d(0, 0, yaw))
+
+ // Invert it to get the robot position relative to the April tag
+ // Multiply by a constant. I don't know why this works, but it was consistently 10% off in 2023 Fall Semester
+ .times(-VisionConstants.DISTANCE_SCALE)
+ // Get the field relative robot pose
+ .plus(targetPose.getTranslation());
+ try{
+ // Adds an EstimatedRobotPose
+ list.add(new EstimatedRobotPose(
+ new Pose3d(translation.getX(), translation.getY(), 0, new Rotation3d(0, 0, yaw)),
+ timestamp,
+ List.of(target),
+ VisionConstants.POSE_STRATEGY
+ ));
+ }catch(Exception e){
+ DriverStation.reportError("Error creating EstimatedRobotPose", true);
+ }
+ }
+ return list;
+ }
+
+ public boolean useTag(int id){
+ // Never use tags that don't exist
+ if(id <= 0 || id > FieldConstants.field.getTags().size()){
+ return false;
+ }
+ // Return false if it is in the list of tags to ignore
+ for(int id2 : VisionConstants.TAGS_TO_IGNORE){
+ if(id == id2){
+ return false;
+ }
+ }
+ // If it's in the array to only use and not in the array to ignore, return true
+ for(int j = 0; onlyUse != null && j < onlyUse.length; j++){
+ if(id == onlyUse[j]){
+ return true;
+ }
+ }
+ // If it isn't in the array to only use, only reutrn true if the array is empty/null
+ return onlyUse == null || onlyUse.length == 0;
+ }
+
+ /**
+ * Enables or disables this camera
+ * @param enable If it should be enabled or disabled
+ */
+ public void enable(boolean enable){
+ enabled = enable;
+ }
+ }
+}
--- /dev/null
+// Copyright 2021-2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.util.Vision;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+import org.photonvision.targeting.PhotonPipelineResult;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public interface VisionIO {
+ public static class VisionIOInputs implements LoggableInputs {
+ public boolean connected = false;
+ public List<PhotonPipelineResult> results = new ArrayList<>();
+
+ // PhotonVision should never return more than 5 results, except possibly for very long loop overruns
+ private static final int maxLength = 5;
+
+ private boolean intitalized = false;
+
+ @Override
+ public void toLog(LogTable table) {
+ // LogTable does not easily allow removal of logs, especially ProtobufSerializables, so extra values will need to be ignored
+ // This is not very efficient, since unused values are still taking up memory, but there is no easy way to remove them
+ if (!intitalized){
+ for(int i = 0; i < maxLength; i++){
+ table.put("Results"+i, new PhotonPipelineResult());
+ intitalized = true;
+ }
+ }
+ table.put("Connected", connected);
+ double length = Math.min(results.size(), maxLength);
+ table.put("Length", length);
+ for(int i = 0; i < length; i++){
+ table.put("Results"+i, results.get(i));
+ }
+ }
+
+ @Override
+ public void fromLog(LogTable table) {
+ connected = table.get("Connected", false);
+ int length = table.get("Length", 0);
+ results = new ArrayList<>(length);
+ // Java gets confused when null is used for a generic type argument
+ PhotonPipelineResult nullResult = null;
+ for(int i = 0; i < length; i++){
+ PhotonPipelineResult result = table.get("Results"+i, nullResult);
+ if(result != null){
+ results.add(result);
+ }
+ }
+ }
+ }
+
+ /** Represents the angle to a simple target, not used for pose estimation. */
+ public static record TargetObservation(Rotation2d tx, Rotation2d ty) {}
+
+ /** Represents a robot pose sample used for pose estimation. */
+ public static record PoseObservation(
+ double timestamp,
+ Pose3d pose,
+ double ambiguity,
+ int tagCount,
+ double averageTagDistance) {}
+
+ public default void updateInputs() {}
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.util;
+
+public record Zone(
+ double x_center,
+ double y_center,
+ double width,
+ double length) {
+
+ public boolean isInside(double x, double y) {
+ if (x > x_center + (length / 2) || x < x_center - (length / 2)) {
+ return false;
+ }
+ if (y > y_center + (width / 2) || y < y_center - (width / 2)) {
+ return false;
+ }
+ return true;
+ }
+}
--- /dev/null
+package lib;
+
+import edu.wpi.first.math.util.Units;
+
+/* Contains values and required settings for common COTS swerve modules. */
+public class COTSFalconSwerveConstants {
+ public final double wheelDiameter;
+ public final double wheelCircumference;
+ public final double angleGearRatio;
+ public final double driveGearRatio;
+ public final double angleKP;
+ public final double angleKI;
+ public final double angleKD;
+ public final double angleKF;
+ public final boolean driveMotorInvert;
+ public final boolean angleMotorInvert;
+ public final boolean canCoderInvert;
+
+ public COTSFalconSwerveConstants(double wheelDiameter, double angleGearRatio, double driveGearRatio, double angleKP, double angleKI, double angleKD, double angleKF, boolean driveMotorInvert, boolean angleMotorInvert, boolean canCoderInvert) {
+ this.wheelDiameter = wheelDiameter;
+ this.wheelCircumference = wheelDiameter * Math.PI;
+ this.angleGearRatio = angleGearRatio;
+ this.driveGearRatio = driveGearRatio;
+ this.angleKP = angleKP;
+ this.angleKI = angleKI;
+ this.angleKD = angleKD;
+ this.angleKF = angleKF;
+ this.driveMotorInvert = driveMotorInvert;
+ this.angleMotorInvert = angleMotorInvert;
+ this.canCoderInvert = canCoderInvert;
+ }
+
+ /**
+ * Swerve Drive Specialties - MK3 Module
+ */
+ public static COTSFalconSwerveConstants SDSMK3(double driveGearRatio) {
+ double wheelDiameter = Units.inchesToMeters(4.0);
+
+ /** 12.8 : 1 */
+ double angleGearRatio = (12.8);
+
+ double angleKP = 0.2;
+ double angleKI = 0.0;
+ double angleKD = 0.0;
+ double angleKF = 0.0;
+
+ boolean driveMotorInvert = false;
+ boolean angleMotorInvert = false;
+ boolean canCoderInvert = false;
+ return new COTSFalconSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, angleKF, driveMotorInvert, angleMotorInvert, canCoderInvert);
+ }
+
+ /**
+ * Swerve Drive Specialties - MK4 Module
+ */
+ public static COTSFalconSwerveConstants SDSMK4(double driveGearRatio) {
+ double wheelDiameter = Units.inchesToMeters(4.0);
+
+ /** 12.8 : 1 */
+ double angleGearRatio = (12.8);
+
+ double angleKP = 0.2;
+ double angleKI = 0.0;
+ double angleKD = 0.0;
+ double angleKF = 0.0;
+
+ boolean driveMotorInvert = false;
+ boolean angleMotorInvert = false;
+ boolean canCoderInvert = false;
+ return new COTSFalconSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, angleKF, driveMotorInvert, angleMotorInvert, canCoderInvert);
+ }
+
+ /**
+ * Swerve Drive Specialties - MK4i Module
+ */
+ public static COTSFalconSwerveConstants SDSMK4i(double driveGearRatio) {
+ double wheelDiameter = Units.inchesToMeters(4.0);
+
+ /** (150 / 7) : 1 */
+ double angleGearRatio = ((150.0 / 7.0));
+
+ double angleKP = 0.3;
+ double angleKI = 0.0;
+ double angleKD = 0.0;
+ double angleKF = 0.0;
+
+ boolean driveMotorInvert = false;
+ boolean angleMotorInvert = true;
+ boolean canCoderInvert = false;
+ return new COTSFalconSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, angleKF, driveMotorInvert, angleMotorInvert, canCoderInvert);
+ }
+
+ /**
+ * Swerve Drive Specialties - MK5n Module
+ */
+ public static COTSFalconSwerveConstants SDSMK5n(double driveGearRatio) {
+ double wheelDiameter = Units.inchesToMeters(4.0);
+
+ /** (287 / 11) : 1 */
+ double angleGearRatio = ((287.0 / 11.0));
+
+ double angleKP = 0.3;
+ double angleKI = 0.0;
+ double angleKD = 0.0;
+ double angleKF = 0.0;
+
+ boolean driveMotorInvert = false;
+ boolean angleMotorInvert = true;
+ boolean canCoderInvert = false;
+ return new COTSFalconSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, angleKF, driveMotorInvert, angleMotorInvert, canCoderInvert);
+ }
+
+ /* Drive Gear Ratios for all supported modules */
+ public static class DriveGearRatios {
+ /* SDS MK3 */
+ /**
+ * SDS MK3 - 8.16 : 1
+ */
+ public static final double SDSMK3_Standard = (8.16);
+ /**
+ * SDS MK3 - 6.86 : 1
+ */
+ public static final double SDSMK3_Fast = (6.86);
+
+ /* SDS MK4 */
+ /**
+ * SDS MK4 - 8.14 : 1
+ */
+ public static final double SDSMK4_L1 = (8.14);
+ /**
+ * SDS MK4 - 6.75 : 1
+ */
+ public static final double SDSMK4_L2 = (6.75);
+ /**
+ * SDS MK4 - 6.12 : 1
+ */
+ public static final double SDSMK4_L3 = (6.12);
+ /**
+ * SDS MK4 - 5.14 : 1
+ */
+ public static final double SDSMK4_L4 = (5.14);
+
+ /* SDS MK4i */
+ /**
+ * SDS MK4i - 8.14 : 1
+ */
+ public static final double SDSMK4i_L1 = (8.14);
+ /**
+ * SDS MK4i - 6.75 : 1
+ */
+ public static final double SDSMK4i_L2 = (6.75);
+ /**
+ * SDS MK4i - 6.12 : 1
+ */
+ public static final double SDSMK4i_L3 = (6.12);
+
+ /* SDS MK5n */
+ /**
+ * SDS MK5n - 7.13 : 1
+ */
+ public static final double SDSMK5n_L1_PLUS = (8.13);
+ /**
+ * SDS MK4i - 5.9 : 1
+ */
+ public static final double SDSMK5n_L2_PLUS = (5.9);
+ /**
+ * SDS MK4i - 5.36 : 1
+ */
+ public static final double SDSMK5n_L3_PLUS = (5.35);
+ }
+}
+
+
\ No newline at end of file
--- /dev/null
+package lib;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+
+public class CTREModuleState {
+
+ /**
+ * Minimize the change in heading the desired swerve module state would require by potentially
+ * reversing the direction the wheel spins. Customized from WPILib's version to include placing
+ * in appropriate scope for CTRE onboard control.
+ *
+ * @param desiredState The desired state.
+ * @param currentAngle The current module angle.
+ */
+ public static SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle) {
+ double targetAngle = placeInAppropriate0To360Scope(currentAngle.getDegrees(), desiredState.angle.getDegrees());
+ double targetSpeed = desiredState.speedMetersPerSecond;
+ double delta = targetAngle - currentAngle.getDegrees();
+ if (Math.abs(delta) > 90) {
+ targetSpeed = -targetSpeed;
+ if (delta > 90) {
+ targetAngle -= 180;
+ } else {
+ targetAngle += 180;
+ }
+ }
+ return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle));
+ }
+
+ /**
+ * @param scopeReference Current Angle
+ * @param newAngle Target Angle
+ * @return Closest angle within scope
+ */
+ private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) {
+ double lowerBound;
+ double upperBound;
+ double lowerOffset = scopeReference % 360;
+ if (lowerOffset >= 0) {
+ lowerBound = scopeReference - lowerOffset;
+ upperBound = scopeReference + (360 - lowerOffset);
+ } else {
+ upperBound = scopeReference - lowerOffset;
+ lowerBound = scopeReference - (360 + lowerOffset);
+ }
+ while (newAngle < lowerBound) {
+ newAngle += 360;
+ }
+ while (newAngle > upperBound) {
+ newAngle -= 360;
+ }
+ if (newAngle - scopeReference > 180) {
+ newAngle -= 360;
+ } else if (newAngle - scopeReference < -180) {
+ newAngle += 360;
+ }
+ return newAngle;
+ }
+}
--- /dev/null
+package lib;
+
+import Jama.Matrix;
+import Jama.QRDecomposition;
+
+
+// NOTE: This file is available at
+// http://algs4.cs.princeton.edu/14analysis/PolynomialRegression.java.html
+
+/**
+ * The {@code PolynomialRegression} class performs a polynomial regression on an set of <em>N</em>
+ * data points (<em>y<sub>i</sub></em>, <em>x<sub>i</sub></em>). That is, it fits a polynomial
+ * <em>y</em> = β<sub>0</sub> + β<sub>1</sub> <em>x</em> + β<sub>2</sub>
+ * <em>x</em><sup>2</sup> + ... + β<sub><em>d</em></sub> <em>x</em><sup><em>d</em></sup> (where
+ * <em>y</em> is the response variable, <em>x</em> is the predictor variable, and the
+ * β<sub><em>i</em></sub> are the regression coefficients) that minimizes the sum of squared
+ * residuals of the multiple regression model. It also computes associated the coefficient of
+ * determination <em>R</em><sup>2</sup>.
+ *
+ * <p>This implementation performs a QR-decomposition of the underlying Vandermonde matrix, so it is
+ * neither the fastest nor the most numerically stable way to perform the polynomial regression.
+ *
+ * @author Robert Sedgewick
+ * @author Kevin Wayne
+ */
+public class PolynomialRegression implements Comparable<PolynomialRegression> {
+ private final String variableName; // name of the predictor variable
+ private int degree; // degree of the polynomial regression
+ private final Matrix beta; // the polynomial regression coefficients
+ private final double sse; // sum of squares due to error
+ private double sst; // total sum of squares
+
+ /**
+ * Performs a polynomial regression on the data points {@code (y[i], x[i])}. Uses n as the name
+ * of the predictor variable.
+ *
+ * @param x the values of the predictor variable
+ * @param y the corresponding values of the response variable
+ * @param degree the degree of the polynomial to fit
+ * @throws IllegalArgumentException if the lengths of the two arrays are not equal
+ */
+ public PolynomialRegression(double[] x, double[] y, int degree) {
+ this(x, y, degree, "n");
+ }
+
+ /**
+ * Performs a polynomial regression on the data points {@code (y[i], x[i])}.
+ *
+ * @param x the values of the predictor variable
+ * @param y the corresponding values of the response variable
+ * @param degree the degree of the polynomial to fit
+ * @param variableName the name of the predictor variable
+ * @throws IllegalArgumentException if the lengths of the two arrays are not equal
+ */
+ public PolynomialRegression(double[] x, double[] y, int degree, String variableName) {
+ this.degree = degree;
+ this.variableName = variableName;
+
+ int n = x.length;
+ QRDecomposition qr = null;
+ Matrix matrixX = null;
+
+ // in case Vandermonde matrix does not have full rank, reduce degree until it
+ // does
+ while (true) {
+
+ // build Vandermonde matrix
+ double[][] vandermonde = new double[n][this.degree + 1];
+ for (int i = 0; i < n; i++) {
+ for (int j = 0; j <= this.degree; j++) {
+ vandermonde[i][j] = Math.pow(x[i], j);
+ }
+ }
+ matrixX = new Matrix(vandermonde);
+
+ // find least squares solution
+ qr = new QRDecomposition(matrixX);
+ if (qr.isFullRank()) break;
+
+ // decrease degree and try again
+ this.degree--;
+ }
+
+ // create matrix from vector
+ Matrix matrixY = new Matrix(y, n);
+
+ // linear regression coefficients
+ beta = qr.solve(matrixY);
+
+ // mean of y[] values
+ double sum = 0.0;
+ for (int i = 0; i < n; i++) sum += y[i];
+ double mean = sum / n;
+
+ // total variation to be accounted for
+ for (int i = 0; i < n; i++) {
+ double dev = y[i] - mean;
+ sst += dev * dev;
+ }
+
+ // variation not accounted for
+ Matrix residuals = matrixX.times(beta).minus(matrixY);
+ sse = residuals.norm2() * residuals.norm2();
+ }
+
+ /**
+ * Returns the {@code j}th regression coefficient.
+ *
+ * @param j the index
+ * @return the {@code j}th regression coefficient
+ */
+ public double beta(int j) {
+ // to make -0.0 print as 0.0
+ if (Math.abs(beta.get(j, 0)) < 1E-4) return 0.0;
+ return beta.get(j, 0);
+ }
+
+ /**
+ * Returns the degree of the polynomial to fit.
+ *
+ * @return the degree of the polynomial to fit
+ */
+ public int degree() {
+ return degree;
+ }
+
+ /**
+ * Returns the coefficient of determination <em>R</em><sup>2</sup>.
+ *
+ * @return the coefficient of determination <em>R</em><sup>2</sup>, which is a real number between
+ * 0 and 1
+ */
+ public double R2() {
+ if (sst == 0.0) return 1.0; // constant function
+ return 1.0 - sse / sst;
+ }
+
+ /**
+ * Returns the expected response {@code y} given the value of the predictor variable {@code x}.
+ *
+ * @param x the value of the predictor variable
+ * @return the expected response {@code y} given the value of the predictor variable {@code x}
+ */
+ public double predict(double x) {
+ // horner's method
+ double y = 0.0;
+ for (int j = degree; j >= 0; j--) y = beta(j) + (x * y);
+ return y;
+ }
+
+ /**
+ * Returns a string representation of the polynomial regression model.
+ *
+ * @return a string representation of the polynomial regression model, including the best-fit
+ * polynomial and the coefficient of determination <em>R</em><sup>2</sup>
+ */
+ public String toString() {
+ StringBuilder s = new StringBuilder();
+ int j = degree;
+
+ // ignoring leading zero coefficients
+ while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--;
+
+ // create remaining terms
+ while (j >= 0) {
+ if (j == 0) s.append(String.format("%.4f ", beta(j)));
+ else if (j == 1) s.append(String.format("%.4f %s + ", beta(j), variableName));
+ else s.append(String.format("%.4f %s^%d + ", beta(j), variableName, j));
+ j--;
+ }
+ s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")");
+
+ // replace "+ -2n" with "- 2n"
+ return s.toString().replace("+ -", "- ");
+ }
+
+ /**
+ * Compare lexicographically.
+ */
+ public int compareTo(PolynomialRegression that) {
+ double EPSILON = 1E-5;
+ int maxDegree = Math.max(this.degree(), that.degree());
+ for (int j = maxDegree; j >= 0; j--) {
+ double term1 = 0.0;
+ double term2 = 0.0;
+ if (this.degree() >= j) term1 = this.beta(j);
+ if (that.degree() >= j) term2 = that.beta(j);
+ if (Math.abs(term1) < EPSILON) term1 = 0.0;
+ if (Math.abs(term2) < EPSILON) term2 = 0.0;
+ if (term1 < term2) return -1;
+ else if (term1 > term2) return +1;
+ }
+ return 0;
+ }
+}
--- /dev/null
+package lib.controllers;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
+import java.util.function.BooleanSupplier;
+
+public class Controller {
+ protected final Joystick controller;
+
+ public Controller(int port) {
+ this.controller = new Joystick(port);
+ }
+
+ public Trigger get(BooleanSupplier sup) {
+ return new Trigger(sup);
+ }
+}
--- /dev/null
+package lib.controllers;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
+public class Ex3DProController extends Controller {
+ public Ex3DProController(int port) {
+ super(port);
+ }
+
+ public enum Ex3DProButton {
+ B1(1),
+ B2(2),
+ B3(3),
+ B4(4),
+ B6(6),
+ B7(7),
+ B8(8),
+ B9(9),
+ B10(10),
+ B11(11),
+ B12(12);
+
+ public final int id;
+
+ Ex3DProButton(final int id) {
+ this.id = id;
+ }
+ }
+
+ public enum Ex3DProAxis {
+ X(0),
+ Y(1),
+ Z(2),
+ SLIDER(3);
+
+ public final int id;
+
+ Ex3DProAxis(final int id) {
+ this.id = id;
+ }
+ }
+
+ public enum Ex3DProHatSwitch {
+ UNPRESSED(-1),
+ UP(0),
+ UP_RIGHT(45),
+ RIGHT(90),
+ DOWN_RIGHT(135),
+ DOWN(180),
+ DOWN_LEFT(235),
+ LEFT(270),
+ UP_LEFT(315);
+
+ public final int angle;
+
+ Ex3DProHatSwitch(final int angle) {
+ this.angle = angle;
+ }
+ }
+
+ public Trigger get(Ex3DProButton button) {
+ return new Trigger(() -> controller.getRawButton(button.id));
+ }
+
+ public double get(Ex3DProAxis axis) {
+ return controller.getRawAxis(axis.id);
+ }
+
+ public Trigger get(Ex3DProHatSwitch hatSwitch) {
+ return new Trigger(() -> controller.getPOV() == hatSwitch.angle);
+ }
+
+ public Joystick get() {
+ return controller;
+ }
+}
--- /dev/null
+package lib.controllers;
+
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
+import java.util.function.BooleanSupplier;
+
+public class GameController extends Controller {
+ // These are the different controller triggers
+ public final BooleanSupplier LEFT_TRIGGER_BUTTON = () -> get(Axis.LEFT_TRIGGER) > 0.5,
+ RIGHT_TRIGGER_BUTTON = () -> get(Axis.RIGHT_TRIGGER) > 0.5;
+ public final Trigger ALL_UP = get(DPad.UP).or(get(DPad.UP_LEFT)).or(get(DPad.UP_RIGHT)),
+ ALL_DOWN = get(DPad.DOWN).or(get(DPad.DOWN_LEFT)).or(get(DPad.DOWN_RIGHT)),
+ ALL_LEFT = get(DPad.LEFT).or(get(DPad.UP_LEFT)).or(get(DPad.DOWN_LEFT)),
+ ALL_RIGHT = get(DPad.RIGHT).or(get(DPad.UP_RIGHT)).or(get(DPad.DOWN_RIGHT));
+
+ public final BooleanSupplier
+ LEFT_STICK_LEFT = () -> get(Axis.LEFT_X) < -0.75,
+ LEFT_STICK_RIGHT = () -> get(Axis.LEFT_X) > 0.75,
+ LEFT_STICK_UP = () -> get(Axis.LEFT_Y) < -0.75,
+ LEFT_STICK_DOWN = () -> get(Axis.LEFT_Y) > 0.75;
+ public final BooleanSupplier
+ RIGHT_STICK_LEFT = () -> get(Axis.RIGHT_X) < -0.75,
+ RIGHT_STICK_RIGHT = () -> get(Axis.RIGHT_X) > 0.75,
+ RIGHT_STICK_UP = () -> get(Axis.RIGHT_Y) < -0.75,
+ RIGHT_STICK_DOWN = () -> get(Axis.RIGHT_Y) > 0.75;
+
+ public GameController(int port) {
+ super(port);
+ }
+
+ public enum Button {
+ A(1),
+ B(2),
+ X(3),
+ Y(4),
+ LB(5),
+ RB(6),
+ BACK(7),
+ START(8),
+ LEFT_JOY(9),
+ RIGHT_JOY(10);
+
+ public final int id;
+
+ Button(final int id) {
+ this.id = id;
+ }
+ }
+
+ public enum Axis {
+ LEFT_X(0),
+ LEFT_Y(1),
+ LEFT_TRIGGER(2),
+ RIGHT_TRIGGER(3),
+ RIGHT_X(4),
+ RIGHT_Y(5);
+
+ public final int id;
+
+ Axis(final int id) {
+ this.id = id;
+ }
+ }
+
+ public enum DPad {
+ UNPRESSED(-1),
+ UP(0),
+ UP_RIGHT(45),
+ RIGHT(90),
+ DOWN_RIGHT(135),
+ DOWN(180),
+ DOWN_LEFT(235),
+ LEFT(270),
+ UP_LEFT(315);
+
+ public final int angle;
+
+ DPad(final int angle) {
+ this.angle = angle;
+ }
+ }
+
+ public enum RumbleStatus {
+ RUMBLE_ON(0.7),
+ RUMBLE_OFF(0);
+
+ public final double rumbleValue;
+
+ RumbleStatus(final double rumbleValue) {
+ this.rumbleValue = rumbleValue;
+ }
+ }
+
+ public Trigger get(Button button) {
+ return new Trigger(() -> controller.getRawButton(button.id));
+ }
+
+ public double get(Axis axis) {
+ return controller.getRawAxis(axis.id);
+ }
+
+ public Trigger get(DPad dPad) {
+ return new Trigger(() -> controller.getPOV() == dPad.angle);
+ }
+
+ public Joystick get() {
+ return controller;
+ }
+
+ public void setRumble(RumbleStatus rumbleStatus) {
+ controller.setRumble(RumbleType.kLeftRumble, rumbleStatus.rumbleValue);
+ controller.setRumble(RumbleType.kRightRumble, rumbleStatus.rumbleValue);
+ }
+}
\ No newline at end of file
--- /dev/null
+package lib.controllers;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
+public class MadCatzController extends Controller {
+ public final Trigger
+ ALL_UP = get(MadCatzHatSwitch.UP).or(get(MadCatzHatSwitch.UP_LEFT)).or(get(MadCatzHatSwitch.UP_RIGHT)),
+ ALL_DOWN = get(MadCatzHatSwitch.DOWN).or(get(MadCatzHatSwitch.DOWN_LEFT)).or(get(MadCatzHatSwitch.DOWN_RIGHT)),
+ ALL_LEFT = get(MadCatzHatSwitch.LEFT).or(get(MadCatzHatSwitch.UP_LEFT)).or(get(MadCatzHatSwitch.DOWN_LEFT)),
+ ALL_RIGHT = get(MadCatzHatSwitch.RIGHT).or(get(MadCatzHatSwitch.UP_RIGHT)).or(get(MadCatzHatSwitch.DOWN_RIGHT));
+
+ public MadCatzController(int port) {
+ super(port);
+ }
+
+ public enum MadCatzButton {
+ B1(1),
+ B2(2),
+ B3(3),
+ B4(4),
+ B6(6),
+ B7(7);
+
+ public final int id;
+
+ MadCatzButton(final int id) {
+ this.id = id;
+ }
+ }
+
+ public enum MadCatzAxis {
+ X(0),
+ Y(1),
+ SLIDER(2),
+ ZROTATE(3);
+
+ public final int id;
+
+ MadCatzAxis(final int id) {
+ this.id = id;
+ }
+ }
+
+ public enum MadCatzHatSwitch {
+ UNPRESSED(-1),
+ UP(0),
+ UP_RIGHT(45),
+ RIGHT(90),
+ DOWN_RIGHT(135),
+ DOWN(180),
+ DOWN_LEFT(235),
+ LEFT(270),
+ UP_LEFT(315);
+
+ public final int angle;
+
+ MadCatzHatSwitch(final int angle) {
+ this.angle = angle;
+ }
+ }
+
+ public Trigger get(MadCatzButton button) {
+ return new Trigger(() -> controller.getRawButton(button.id));
+ }
+
+ public double get(MadCatzAxis axis) {
+ return controller.getRawAxis(axis.id);
+ }
+
+ public Trigger get(MadCatzHatSwitch hatSwitch) {
+ return new Trigger(() -> controller.getPOV() == hatSwitch.angle);
+ }
+
+ public Joystick get() {
+ return controller;
+ }
+}
--- /dev/null
+package lib.controllers;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
+import java.util.function.BooleanSupplier;
+
+public class PS5Controller extends Controller {
+ // These are the different controller triggers
+ public final Trigger ALL_UP = get(DPad.UP).or(get(DPad.UP_LEFT)).or(get(DPad.UP_RIGHT)),
+ ALL_DOWN = get(DPad.DOWN).or(get(DPad.DOWN_LEFT)).or(get(DPad.DOWN_RIGHT)),
+ ALL_LEFT = get(DPad.LEFT).or(get(DPad.UP_LEFT)).or(get(DPad.DOWN_LEFT)),
+ ALL_RIGHT = get(DPad.RIGHT).or(get(DPad.UP_RIGHT)).or(get(DPad.DOWN_RIGHT));
+
+ public final BooleanSupplier
+ LEFT_STICK_LEFT = () -> get(PS5Axis.LEFT_X) < -0.75,
+ LEFT_STICK_RIGHT = () -> get(PS5Axis.LEFT_X) > 0.75,
+ LEFT_STICK_UP = () -> get(PS5Axis.LEFT_Y) < -0.75,
+ LEFT_STICK_DOWN = () -> get(PS5Axis.LEFT_Y) > 0.75;
+ public final BooleanSupplier
+ RIGHT_STICK_LEFT = () -> get(PS5Axis.RIGHT_X) < -0.75,
+ RIGHT_STICK_RIGHT = () -> get(PS5Axis.RIGHT_X) > 0.75,
+ RIGHT_STICK_UP = () -> get(PS5Axis.RIGHT_Y) < -0.75,
+ RIGHT_STICK_DOWN = () -> get(PS5Axis.RIGHT_Y) > 0.75;
+
+ public PS5Controller(int port) {
+ super(port);
+ }
+
+ public enum PS5Button {
+ SQUARE(1),
+ CROSS(2),
+ CIRCLE(3),
+ TRIANGLE(4),
+ LB(5),
+ RB(6),
+ LEFT_TRIGGER(7),
+ RIGHT_TRIGGER(8),
+ CREATE(9),
+ OPTIONS(10),
+ LEFT_JOY(11),
+ RIGHT_JOY(12),
+ PS(13),
+ TOUCHPAD(14),
+ MUTE(15);
+
+ public final int id;
+
+ PS5Button(final int id) {
+ this.id = id;
+ }
+ }
+
+ public enum PS5Axis {
+ LEFT_X(0),
+ LEFT_Y(1),
+ RIGHT_X(2),
+ /**
+ * note: ps5 controller trigger goes from -1 when unpressed, to 1 when fully
+ * pressed
+ */
+ LEFT_TRIGGER(3),
+ /**
+ * note: ps5 controller trigger goes from -1 when unpressed, to 1 when fully
+ * pressed
+ */
+ RIGHT_TRIGGER(4),
+ RIGHT_Y(5);
+
+ public final int id;
+
+ PS5Axis(final int id) {
+ this.id = id;
+ }
+ }
+
+ public enum DPad {
+ UNPRESSED(-1),
+ UP(0),
+ UP_RIGHT(45),
+ RIGHT(90),
+ DOWN_RIGHT(135),
+ DOWN(180),
+ DOWN_LEFT(235),
+ LEFT(270),
+ UP_LEFT(315);
+
+ public final int angle;
+
+ DPad(final int angle) {
+ this.angle = angle;
+ }
+ }
+
+ public Trigger get(PS5Button button) {
+ return new Trigger(() -> controller.getRawButton(button.id));
+ }
+
+ public double get(PS5Axis axis) {
+ return controller.getRawAxis(axis.id);
+ }
+
+ public Trigger get(DPad dPad) {
+ return new Trigger(() -> controller.getPOV() == dPad.angle);
+ }
+
+ public Joystick get() {
+ return controller;
+ }
+}
\ No newline at end of file
--- /dev/null
+package lib.controllers;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
+public class PistolController extends Controller {
+ public final Trigger TOP_BACK_ONLY = get(Button.TOP_BACK).and(get(Button.TOP_FRONT).negate()),
+ TOP_FRONT_ONLY = get(Button.TOP_FRONT).and(get(Button.TOP_BACK).negate()),
+ BOTTOM_BACK_ONLY = get(Button.BOTTOM_BACK).and(get(Button.BOTTOM_FRONT).negate()),
+ BOTTOM_FRONT_ONLY = get(Button.BOTTOM_FRONT).and(get(Button.BOTTOM_BACK).negate());
+
+ public PistolController(int port) {
+ super(port);
+ }
+
+ public enum Button {
+ TOP_BACK(1),
+ TOP_FRONT(2),
+ BOTTOM_FRONT(3),
+ BOTTOM_BACK(4),
+ BOTTOM(5);
+
+ public final int id;
+
+ Button(final int id) {
+ this.id = id;
+ }
+ }
+
+ public enum Axis {
+ WHEEL(0),
+ TRIGGER(1);
+
+ public final int id;
+
+ Axis(final int id) {
+ this.id = id;
+ }
+ }
+
+ public Trigger get(Button button) {
+ return new Trigger(() -> controller.getRawButton(button.id));
+ }
+
+ public double get(Axis axis) {
+ return controller.getRawAxis(axis.id);
+ }
+
+ public Joystick get() {
+ return controller;
+ }
+}
--- /dev/null
+package frc.robot.constants;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import frc.robot.util.Vision.Vision;
+
+/**
+ * Tests if all of the AprilTags are in the right spot
+ */
+public class AprilTagPoseTest {
+ /**
+ * Tests if there are the right number of AprilTags, that the tags in Vision match the ones in FieldConstants, and that they are on the right side of the field
+ */
+ @Test
+ public void testTagPoses() {
+ // Construct the vision instance
+ // makes the field layout
+ Vision vision = new Vision(new ArrayList<Pair<String, Transform3d>>());
+
+ // we should have 32 tags
+ assertEquals(32, FieldConstants.field.getTags().size());
+ assertEquals(32, vision.getAprilTagFieldLayout().getTags().size());
+
+ // Check each tag in the field layout
+ for (int i = 0; i < vision.getAprilTagFieldLayout().getTags().size(); i++) {
+ // The expected tagId. The ArrayList is zero-based and our tags start at 1, so the tagId is i+1.
+ int tagId = i + 1;
+
+ // Get the poses from the two sources
+ // From the ArrayList<AprilTag> source
+ AprilTag apriltag1 = FieldConstants.field.getTags().get(i);
+ Pose3d p1 = apriltag1.pose;
+ // From the vision source
+ Pose3d p2 = vision.getTagPose(tagId);
+
+ // Check the tag id in the ArrayList
+ assertEquals(tagId, apriltag1.ID);
+
+ // Make sure the points match
+ assertEquals(p1.getX(), p2.getX(), 0.0001);
+ assertEquals(p1.getY(), p2.getY(), 0.0001);
+ assertEquals(p1.getZ(), p2.getZ(), 0.0001);
+
+ // Make sure the rotations match
+ assertEquals(p1.getRotation().getX(), p2.getRotation().getX(), 0.0001);
+ assertEquals(p1.getRotation().getY(), p2.getRotation().getY(), 0.0001);
+ assertEquals(p1.getRotation().getZ(), p2.getRotation().getZ(), 0.0001);
+
+ // 1-16 should be on the right, and 17-36 should be on the left
+ if(tagId > 16){
+ assertTrue(p1.getX() < FieldConstants.field.getFieldLength()/2);
+ }else{
+ assertTrue(p1.getX() > FieldConstants.field.getFieldLength()/2);
+ }
+ }
+ }
+
+ @Test
+ public void testReefTags(){
+ List<Pose3d> redPoses = FieldConstants.field.getTags().subList(1, 16).stream().map(tag->tag.pose).toList();
+ List<Pose3d> bluePoses = FieldConstants.field.getTags().subList(17, 32).stream().map(tag->tag.pose).toList();
+ Pose3d redCenter = findCenter(redPoses);
+ Pose3d blueCenter = findCenter(bluePoses);
+
+ // The tags should be symmetrical, so the total rotation should be 0
+ assertEquals(redCenter.getRotation().getX(), 0, 0.0001);
+ assertEquals(blueCenter.getRotation().getX(), 0, 0.0001);
+ assertEquals(redCenter.getRotation().getY(), 0, 0.0001);
+ assertEquals(blueCenter.getRotation().getY(), 0, 0.0001);
+ assertEquals(MathUtil.angleModulus(redCenter.getRotation().getZ()), Math.PI, 0.0001);
+ assertEquals(MathUtil.angleModulus(blueCenter.getRotation().getZ()), 0, 0.0001);
+
+ // Y are symmetrical diagonally
+ assertEquals(redCenter.getY(), FieldConstants.field.getFieldWidth() - blueCenter.getY(), 0.01);
+ // Z should be equal
+ assertEquals(redCenter.getZ(), blueCenter.getZ(), 0.0001);
+
+ // X should be mirrored
+ assertEquals(redCenter.getX(), FieldConstants.field.getFieldLength()-blueCenter.getX(), 0.01);
+
+ // Compare each matching pair of tags
+ for(int i = 1; i < 17; i++){
+ Pose3d red = FieldConstants.field.getTagPose(i).get();
+ Pose3d blue = FieldConstants.field.getTagPose(i+16).get();
+ assertEquals(red.getY(), FieldConstants.field.getFieldWidth() - blue.getY(), 0.01);
+ assertEquals(red.getZ(), blue.getZ(), 0.0001);
+ assertEquals(red.getX(), FieldConstants.field.getFieldLength()-blue.getX(), 0.01);
+ assertEquals(MathUtil.angleModulus(red.getRotation().getZ()), MathUtil.angleModulus(blue.getRotation().getZ() + Math.PI), 0.0001);
+ }
+ }
+
+ /**
+ * Gets the center pose with the sum of the rotations, used for checking the reef
+ * @param poses The poses to find the center of
+ * @return A pose with the translation at the center and the sum of the rotations
+ */
+ private Pose3d findCenter(List<Pose3d> poses){
+ double x = 0;
+ double y = 0;
+ double z = 0;
+ Rotation3d rot = new Rotation3d();
+ for(Pose3d pose : poses){
+ x += pose.getX();
+ y += pose.getY();
+ z += pose.getZ();
+ rot = rot.plus(pose.getRotation());
+ }
+ x /= poses.size();
+ y /= poses.size();
+ z /= poses.size();
+ return new Pose3d(x, y, z, rot);
+ }
+}
--- /dev/null
+package frc.robot.constants;
+
+import edu.wpi.first.math.util.Units;
+import frc.robot.constants.swerve.DriveConstants;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+/**
+ * Check some robot constants/parameters.
+ */
+public class ConstantsTest {
+
+ @Test
+ public void testRobotSize() {
+ // The competition robot frame width and length is 26 inches.
+ // It has 3/16 inch plates on all sides,
+ // so frame width is 26.375!
+ double widthFrame = Units.inchesToMeters(26.0);
+ double widthFrameAndPlates = widthFrame + 2.0 * Units.inchesToMeters(3.0 / 16.0);
+
+ // At Port Hueneme, the frame perimeter was 105 inches
+ // frame perimeter is 105.5...
+ assertEquals(105.0, 4 * Units.metersToInches(widthFrameAndPlates), 0.501);
+
+ // Bumpers.
+ // The backing board is 0.75 inches.
+ // The noodles are 2.5 inches.
+ // Board + noodles = 3.25 inches
+ // Measure the red bumpers, and they are 3.5 inches.
+ //
+ // The latch studs are centered on the 1x2 frame rails. That's 0.5 inches
+ // The bumper latch bracket holes for the studs are 3/4 in from the inside surface of the bumpers.
+ // That puts the bumper inside surface at 3/4 - (0.5) = 1/4 inch out from widthFrame
+ double thickBumpers = Units.inchesToMeters(3.5 + 0.25);
+
+ // so width with bumpers is
+ @SuppressWarnings("unused")
+ double widthFrameWithBumpers = widthFrame + 2 * thickBumpers;
+
+ // check with values in DriveConstants
+ // System.out.printf("widthFrameWithBumpers = %8f %8f\n", widthFrameWithBumpers, Units.metersToInches(widthFrameWithBumpers));
+ // System.out.printf("kRobotWidthWithBumpers = %8f %8f\n", DriveConstants.kRobotWidthWithBumpers, Units.metersToInches(DriveConstants.kRobotWidthWithBumpers));
+ // assertEquals(widthFrameWithBumpers, DriveConstants.kRobotWidthWithBumpers, 0.001);
+ }
+
+ /**
+ * MK4i module
+ * https://www.swervedrivespecialties.com/products/mk4i-swerve-module
+ */
+ enum SwerveDriveSpecialties {
+ // Gearbox ratios from the SDS webpage
+ // https://www.swervedrivespecialties.com/products/mk4i-swerve-module
+ L1((50.0 / 14.0) * (19.0 / 25.0) * (45.0 / 15.0)),
+ L2((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)),
+ L3((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)),
+ // 16-tooth pinion
+ // https://www.swervedrivespecialties.com/products/kit-adapter-16t-drive-pinion-gear-mk4i
+ L1P((50.0 / 16.0) * (19.0 / 25.0) * (45.0 / 15.0)),
+ L2P((50.0 / 16.0) * (17.0 / 27.0) * (45.0 / 15.0)),
+ L3P((50.0 / 16.0) * (16.0 / 28.0) * (45.0 / 15.0));
+
+ /**
+ * Drive gear ratio varies for each module
+ */
+ final double driveRatio;
+ /**
+ * Steering Gear ratio (same for all MK4i modules)
+ */
+ final double steerRatio = 150.0 / 7.0;
+
+ SwerveDriveSpecialties(double drive) {
+ this.driveRatio = drive;
+ }
+ }
+
+ @Test
+ public void testSwerveRatios() {
+ // check the mroe exact ratios against the published-to-2-digits ratios
+ assertEquals(8.14, SwerveDriveSpecialties.L1.driveRatio, 0.01);
+ assertEquals(6.75, SwerveDriveSpecialties.L2.driveRatio, 0.01);
+ assertEquals(6.12, SwerveDriveSpecialties.L3.driveRatio, 0.01);
+
+ // The drive ratio could be more accurate, but does not hurt
+ assertEquals(SwerveDriveSpecialties.L2P.driveRatio, DriveConstants.DRIVE_GEAR_RATIO, 0.01);
+
+ // The steer ratio
+ // print the relative error: 0.6e-4. After 100 rotations, error would be 0.6e-2 rotations (about 1.5 degrees)
+ // System.out.println((DriveConstants.kSteerGearRatio - SwerveDriveSpecialties.L2.steerRatio) / SwerveDriveSpecialties.L2.steerRatio);
+ assertEquals(SwerveDriveSpecialties.L2P.steerRatio, DriveConstants.STEER_GEAR_RATIO, 0.01);
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Disabled;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+/**
+ * Example of a JUnit test class.
+ * This test should run everytime someone builds the robot code.
+ * See https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-simulation/unit-testing.html
+ * <p>
+ * To disable a test, annotate with Disabled
+ */
+public class ArithTest {
+
+ @BeforeEach
+ public void prepare() {
+ }
+
+ @AfterEach
+ public void cleanup() {
+ }
+
+ /**
+ * Test if floating point addition works.
+ */
+ @Test
+ public void testSimpleArith() {
+ assertEquals(5.0, 2.0 + 3.0, 0.0001);
+ }
+
+ /**
+ * Here is a disabled test
+ */
+ @Disabled
+ @Test
+ public void testNaught() {
+ assertEquals(0, 0);
+ }
+
+}
--- /dev/null
+package frc.robot.util;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import java.util.Random;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import frc.robot.util.Vision.DetectedObject;
+import frc.robot.util.Vision.DetectedObject.ObjectType;
+
+/**
+ * Tests DetectedObject
+ */
+public class DetectedObjectTest {
+
+ @BeforeEach
+ public void prepare() {
+ DetectedObject.setDrive(null);
+ }
+
+ @AfterEach
+ public void cleanup() {}
+
+ /**
+ * Tests if the objec pose is correct
+ */
+ @Test
+ public void testObjectPose() {
+ DetectedObject object = new DetectedObject(
+ Units.degreesToRadians(45),
+ 0,
+ 1,
+ ObjectType.NONE,
+ new Transform3d(new Translation3d(0, 0, 1), new Rotation3d(0, -Math.PI/2, Math.PI/2))
+ );
+ Translation3d expected = new Translation3d(Math.sqrt(2)/2, 0, Math.sqrt(2)/2+1);
+ assertEquals(expected.getX(), object.pose.getX(), 0.001);
+ assertEquals(expected.getY(), object.pose.getY(), 0.001);
+ assertEquals(expected.getZ(), object.pose.getZ(), 0.001);
+ }
+
+ /**
+ * Tests the position of an object when distance is not specified
+ */
+ @Test
+ public void testObjectPoseWithoutDistance(){
+ DetectedObject object = new DetectedObject(
+ 0,
+ -Units.degreesToRadians(20),
+ ObjectType.NONE,
+ new Transform3d(new Translation3d(0, 0, 1), new Rotation3d(0, Units.degreesToRadians(25), 0))
+ );
+ Translation3d expected = new Translation3d(1, 0, 0);
+ assertEquals(expected.getX(), object.pose.getX(), 0.001);
+ assertEquals(expected.getY(), object.pose.getY(), 0.001);
+ assertEquals(expected.getZ(), object.pose.getZ(), 0.001);
+ }
+
+ /**
+ * Tests if the object is on the ground using random offsets
+ */
+ @Test
+ public void testObjectOnGround(){
+ Random random = new Random();
+ DetectedObject object = new DetectedObject(
+ random.nextDouble(-Math.PI, Math.PI),
+ random.nextDouble(0.001, Math.PI/4),
+ ObjectType.NONE,
+ new Transform3d(new Translation3d(
+ random.nextDouble(0, 100),
+ random.nextDouble(0, 100),
+ random.nextDouble(0.1, 100)
+ ), new Rotation3d(
+ 0, random.nextDouble(0.001, Math.PI/4), random.nextDouble(-Math.PI, Math.PI)
+ ))
+ );
+ assertEquals(object.pose.getZ(), 0, 0.001);
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import org.junit.jupiter.api.BeforeAll;
+import org.junit.jupiter.api.Test;
+
+import com.pathplanner.lib.auto.NamedCommands;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+/**
+ * Simple check on PathPlanner path
+ */
+public class PathCheck {
+
+ /**
+ * Register placeholder commands for testing.
+ * These are normally registered in RobotContainer.registerCommands(),
+ * but the test doesn't instantiate RobotContainer.
+ */
+ @BeforeAll
+ public static void registerPlaceholderCommands() {
+ NamedCommands.registerCommand("Extend intake", new InstantCommand());
+ NamedCommands.registerCommand("Intake", new InstantCommand());
+ }
+
+ /**
+ * Load the path groups.
+ * <p>
+ * We have had problems with syntax errors in a path.
+ */
+ @Test
+ public void pathGroupLoaderTest() {
+ // load the paths
+ // may throw a ParseException; that error will fail this test
+ PathGroupLoader.loadPathGroups();
+ }
+}
--- /dev/null
+package frc.robot.util;
+
+import lib.PolynomialRegression;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class PolynomialRegressionTest {
+
+ /**
+ * Unit tests the {@code PolynomialRegression} data type.
+ */
+ @Test
+ public void testRegression() {
+ double[] x = {10, 20, 40, 80, 160, 200};
+ double[] y = {100, 350, 1500, 6700, 20160, 40000};
+ PolynomialRegression regression = new PolynomialRegression(x, y, 3);
+
+ assertEquals(regression.beta(3), 0.0092, 0.0001);
+ assertEquals(regression.beta(2), -1.6395, 0.0001);
+ assertEquals(regression.beta(1), 168.9232, 0.0001);
+ assertEquals(regression.beta(0), -2113.7306, 0.0001);
+ }
+
+}
--- /dev/null
+
+package frc.robot.util;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.util.ArrayList;
+import java.util.Random;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import frc.robot.constants.Constants;
+
+class ShooterPhysicsTest {
+ private static final double epsilon = .001;
+
+ @BeforeEach
+ public void prepare() {
+ }
+
+ @AfterEach
+ public void cleanup() {
+ }
+
+ @Test
+ public void basicImpulseTest() {
+ Translation3d transform = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero, Translation3d.kZero, 1);
+ assertEquals(0, transform.getX(), epsilon);
+ assertEquals(0, transform.getY(), epsilon);
+ assertEquals(4.427, transform.getZ(), epsilon);
+
+ Translation3d transform2 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero,
+ new Translation3d(10, 0, 0),
+ 1);
+ assertTrue(transform2.getX() > 0, transform2.toString());
+ assertEquals(0, transform2.getY(), epsilon);
+ assertEquals(4.427, transform2.getZ(), epsilon);
+
+ Translation3d transform3 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero,
+ new Translation3d(0, -6, 0), 1);
+ assertEquals(0, transform3.getX(), epsilon);
+ assertTrue(transform3.getY() < 0, transform3.toString());
+ assertEquals(4.427, transform3.getZ(), epsilon);
+
+ Translation3d transform4 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero,
+ new Translation3d(6, 3, 0), 2);
+ assertEquals(6. / 3, transform4.getX() / transform4.getY(), epsilon);
+
+ Translation3d transform5 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero,
+ new Translation3d(12.14, 3.21, 0), 2);
+ assertEquals(12.14 / 3.21, transform5.getX() / transform5.getY(), epsilon);
+
+ Translation3d transform6 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero,
+ new Translation3d(-16.32, 3.3, 0), 2);
+ assertEquals(-16.32 / 3.3, transform6.getX() / transform6.getY(), epsilon);
+
+ Translation3d transform7 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero,
+ new Translation3d(2, 0, 0), Constants.GRAVITY_ACCELERATION / 2);
+ assertEquals(1, transform7.getX(), epsilon, transform7.toString());
+ assertEquals(0, transform7.getY(), epsilon);
+ assertEquals(Constants.GRAVITY_ACCELERATION, transform7.getZ(), epsilon);
+ }
+
+ @Test
+ public void initialVelocityTest() {
+ Translation2d initialVelocityDiff = new Translation2d(5.1, -6.5);
+ Translation3d transform = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero,
+ new Translation3d(1, 2, 3), 4);
+ Translation3d transform2 = ShooterPhysics.getRequiredExitVelocity(initialVelocityDiff,
+ new Translation3d(1, 2, 3),
+ 4);
+ assertEquals(transform.getX(), transform2.getX() + initialVelocityDiff.getX(), epsilon);
+ assertEquals(transform.getY(), transform2.getY() + initialVelocityDiff.getY(), epsilon);
+ assertEquals(transform.getZ(), transform2.getZ(), epsilon);
+ }
+
+ @Test
+ public void yawTest() {
+ ShooterPhysics.TurretState state1 = ShooterPhysics.getShotParams(Translation2d.kZero,
+ new Translation3d(1, 0, 0), 1);
+ // different for this one because it's close to 0, so the angle wraps and
+ // assertEquals can't handle that
+ assertTrue(Math.abs((state1.yaw()).getRadians()) <= epsilon, state1.toString());
+
+ ShooterPhysics.TurretState state2 = ShooterPhysics.getShotParams(Translation2d.kZero,
+ new Translation3d(0, 1, 0), 1);
+ assertEquals(new Rotation2d(Math.PI / 2).getRadians(), state2.yaw().getRadians(), epsilon);
+
+ ShooterPhysics.TurretState state3 = ShooterPhysics.getShotParams(Translation2d.kZero,
+ new Translation3d(-1, 0, 0), 1);
+ assertEquals(new Rotation2d(Math.PI).getRadians(), state3.yaw().getRadians(), epsilon);
+
+ ShooterPhysics.TurretState state4 = ShooterPhysics.getShotParams(Translation2d.kZero,
+ new Translation3d(0, -1, 0), 1);
+ assertEquals(new Rotation2d(-Math.PI / 2).getRadians(), state4.yaw().getRadians(), epsilon);
+ }
+
+ @Test
+ public void pitchTest() {
+ // check random values are within range
+ ShooterPhysics.TurretState state1 = ShooterPhysics.getShotParams(Translation2d.kZero,
+ new Translation3d(1, 0, 0), 1);
+ assertTrue(state1.pitch() >= 0 && state1.pitch() <= Math.PI / 2, state1.toString());
+
+ ShooterPhysics.TurretState state2 = ShooterPhysics.getShotParams(new Translation2d(12.2, -1.3),
+ new Translation3d(1.1, 12, 11.1).minus(new Translation3d(.2, 1.2, 0)), 22.1);
+ assertTrue(state2.pitch() >= 0 && state2.pitch() <= Math.PI / 2, state2.toString());
+
+ ShooterPhysics.TurretState state3 = ShooterPhysics.getShotParams(new Translation2d(1.9, 9.1),
+ new Translation3d(11.2, -13.1, 4.1).minus(new Translation3d(-.3, -8.4, 0)), 5.6);
+ assertTrue(state3.pitch() >= 0 && state3.pitch() <= Math.PI / 2, state3.toString());
+
+ // try a steep shot
+ ShooterPhysics.TurretState state4 = ShooterPhysics.getShotParams(Translation2d.kZero,
+ new Translation3d(1, 0, 99), 100);
+ assertTrue(state4.pitch() >= Math.PI * 7 / 16 && state4.pitch() <= Math.PI / 2, state4.toString());
+
+ ShooterPhysics.TurretState state5 = ShooterPhysics.getShotParams(Translation2d.kZero,
+ new Translation3d(1, 0, 0), 100);
+ assertTrue(state5.pitch() >= Math.PI * 7 / 16 && state5.pitch() <= Math.PI / 2, state5.toString());
+
+ // try a shallow shot
+ ShooterPhysics.TurretState state6 = ShooterPhysics.getShotParams(Translation2d.kZero,
+ new Translation3d(100, 50, 1), 2);
+ assertTrue(state6.pitch() >= 0 && state6.pitch() <= Math.PI / 16, state6.toString());
+ }
+
+ // test using a simple physics simulation
+ @Test
+ public void simulatedTest() {
+ // test the simulation itself
+ checkTrajectory(Translation3d.kZero, new Translation3d(1, 0, Constants.GRAVITY_ACCELERATION),
+ new Translation3d(2, 0, .1), 2);
+
+ Random rng = new Random(972);
+
+ // compute 1000 random shots and simulate them
+ for (int i = 0; i < 1000; i++) {
+ Translation2d initPos = new Translation2d(rng.nextDouble() * 20 - 10, rng.nextDouble() * 20 - 10);
+ Translation3d target = new Translation3d(rng.nextDouble() * 20 - 10, rng.nextDouble() * 20 - 10,
+ rng.nextDouble() * 10 + 1);
+ Translation2d initVel = new Translation2d(rng.nextDouble() * 10 - 5, rng.nextDouble() * 10 - 5);
+ Translation3d robotToTarget = target.minus(new Translation3d(initPos));
+ double arcHeight = target.getZ() + rng.nextDouble() * 10;
+
+ Translation3d exitVel = ShooterPhysics.getRequiredExitVelocity(initVel, robotToTarget, arcHeight);
+
+ checkTrajectory(new Translation3d(initPos), exitVel.plus(new Translation3d(initVel)), target,
+ arcHeight);
+ }
+ }
+
+ private class PhysicsObject {
+ private Translation3d pos;
+ private Translation3d vel;
+
+ private PhysicsObject(Translation3d pos, Translation3d vel) {
+ this.pos = pos;
+ this.vel = vel;
+ }
+
+ private void step(double time) {
+ pos = pos.plus(vel.times(time));
+ vel = vel.plus(new Translation3d(0, 0, -Constants.GRAVITY_ACCELERATION).times(time));
+ }
+ }
+
+ // throws an error if something goes wrong
+ private void checkTrajectory(Translation3d initPos, Translation3d initVel, Translation3d endPos,
+ double requiredHeight) {
+ final double tolerance = .2;
+ PhysicsObject object = new PhysicsObject(initPos, initVel);
+ boolean achievedTargetHeight = false;
+ boolean firstLoop = true; // to allow starting from ground
+
+ // for diagnostics if something fails
+ var messages = new ArrayList<String>();
+ messages.add("Starting trajectory check from " + initPos + " to " + endPos + " with velocity " + initVel
+ + " and peak height " + requiredHeight + ".");
+ messages.add("position, velocity"); // column headers
+
+ while (object.pos.getZ() > 0 || firstLoop) {
+ messages.add("" + object.pos + ", " + object.vel);
+
+ if (object.pos.getZ() + tolerance >= requiredHeight)
+ achievedTargetHeight = true;
+
+ // hit the target, check we got the needed height as well
+ if (object.pos.getDistance(endPos) <= tolerance && achievedTargetHeight)
+ return;
+
+ object.step(.01);
+ firstLoop = false;
+ }
+
+ for (String i : messages)
+ System.out.println(i);
+
+ if (achievedTargetHeight)
+ throw new RuntimeException("Trajectory did not hit the target (" + endPos + ").");
+ else
+ throw new RuntimeException("Trajectory did not hit the target (" + endPos
+ + ") and did not attain required arc height (" + requiredHeight + ").");
+ }
+}
--- /dev/null
+.DS_Store
\ No newline at end of file
--- /dev/null
+MIT License
+
+Copyright (c) 2026 OLIVER427
+
+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.
\ No newline at end of file
--- /dev/null
+# Info!
+yes info. please read.
+
+## How to use
+Hopefully this is easy enough to figure out how to use, but I can explain quick details anyway.
+- Click on the file input button and select your ".path" file
+- Click the "Process Path File"
+- you have the option of checking the Mirror Vertically and/or Horizontally boxes
+- click "Export Mirrored Path" if you'd like to download the mirrored path you are currently previewing
+- click "Export All Mirrors" to download every possible mirror of the path you inputed.
+- also, remember to put your newly downloaded files in the paths folder in your pathplanner project directory
+
+if you are using the auto mirroring part, its a pretty similar process.
+
+## Specifications (THIS IS PRETTY IMPORTANT)
+The way the site calculates where the path is in the path mirrorer, (for the naming specifically) is that it detects which corner has a at least 50% of the line inside it.
+
+If you want to use the auto mirrorer, make sure that you've already used the paths in the path mirrorer because literally the ONLY thing that the auto mirror does is rename the path names inside of it to have a prefix that fits the naming of the path mirror (for example, "TopRight New Auto").
+
+The visualization of the path in the path mirrorer just visualizes the direct lines from the startpoint to endpoint of the path because the I cannot figure out how the curved lines are supposed to come out of the path file (its so weird. They calculate it somehow in the app and the curve values you put in there are just calculated to one single number in the path file.)
+
+# well met!
+Well, thats pretty much it, Have fun mirroring!
\ No newline at end of file
--- /dev/null
+html {
+ color: white;
+ font-family: "SUSE" !important;
+}
+
+body {
+ background-color: #1e1f2a;
+}
+
+p {
+ font-size: 1vw;
+ text-align: center;
+}
+
+label {
+ font-size: 1vw;
+}
+
+pre {
+ font-size: 1vw;
+ width: 50%;
+ overflow: hidden;
+ text-overflow: ellipsis;
+}
+
+button,
+#pathInput::file-selector-button,
+#autoInput::file-selector-button {
+ font-size: 1vw;
+ font-family: "SUSE";
+ border: 0.2vw solid #8089ff;
+ border-radius: 0.5vw;
+ background-color: #8089ff;
+ color: #181982;
+ transition-duration: 150ms;
+ transition-property: background-color, border;
+}
+
+button:hover,
+#pathInput::file-selector-button:hover,
+#autoInput::file-selector-button:hover {
+ cursor: pointer;
+ background-color: #6f76dc;
+ border: 0.2vw solid #6f76dc;
+}
+
+#toggleModeBtn {
+ border: 0.2vw solid #ff80f9;
+ background-color: #ff80f9;
+ color: #8c1a86;
+ margin-left: 0.7vw;
+}
+
+#toggleModeBtn:hover {
+ background-color: #cf6aca;
+ border: 0.2vw solid #cf6aca;
+}
+
+#infoText {
+ font-size: 1vw !important;
+ color: #3c3d4d;
+ transition-duration: 150ms;
+ transition-property: opacity;
+}
+
+#downloadBtn,
+#downloadAllBtn,
+#exportAuto {
+ /* transition-duration: 250ms; */
+ transition-property: opacity, background-color, border;
+ opacity: 0;
+ visibility: hidden;
+}
+
+#exportAuto {
+ transition-property: opacity, background-color, border;
+ /* opacity: 0; */
+ visibility: hidden;
+}
+
+#downloadAllBtn {
+ margin-left: 1vw;
+}
+
+#horizontal,
+#vertical {
+ width: 1vw;
+ height: auto;
+}
+
+
+#centerDiv,
+#centerDiv2 {
+ background-color: #12121d;
+ position: fixed;
+ transform: translate(-50%, -50%);
+ left: 50%;
+ top: 50%;
+ display: flex;
+ flex-direction: column;
+ justify-content: start;
+ align-items: center;
+ width: 80vw;
+ height: 40vw;
+ border-radius: 2vw;
+ box-shadow: #0e0e17 0vw 0vw 3vw;
+ transition-duration: 200ms;
+ transition-property: opacity;
+}
+
+#centerDiv>*,
+#centerDiv2>* {
+ margin-bottom: 0.7vw;
+}
+
+#centerDiv2 {
+ overflow: scroll;
+}
+
+#graph,
+#autoList {
+ background-color: #08080d;
+ border-radius: 1vw;
+ width: 41.25vw;
+ height: 20vw;
+ display: flex;
+ align-items: start;
+ overflow-y: scroll;
+ overflow-x: hidden;
+}
+
+#pathInput,
+#autoInput {
+ color: white;
+ font-size: 1vw;
+ font-style: italic;
+ /* margin-right: -5vw; */
+ margin-right: 0vw;
+ width: 14vw;
+ font-family: "SUSE"
+}
\ No newline at end of file
--- /dev/null
+<!DOCTYPE html>
+<html lang="en">
+
+<head>
+ <meta charset="UTF-8">
+ <meta name="viewport" content="width=device-width, initial-scale=1.0">
+ <link rel="stylesheet" href="index.css">
+ <link rel="icon" href="https://team2480.org/favicon.svg">
+ <link rel="preconnect" href="https://fonts.googleapis.com">
+ <link rel="preconnect" href="https://fonts.gstatic.com" crossorigin>
+ <link rel="stylesheet"
+ href="https://fonts.googleapis.com/css2?family=Playwrite+AU+QLD:wght@100..400&family=SUSE:ital,wght@0,100..900;1,100..900&display=swap">
+ <title>2026 Path Mirrorer</title>
+</head>
+
+<body>
+ <div id="centerDiv">
+ <p id="infoText">Preview (no curved lines) of path before mirroring it</p>
+ <canvas id="graph" width="1650" height="800"></canvas>
+ <span>
+ <input type="file" id="pathInput" accept=".path">
+ <button title="Finish loading in the .path file" id="visualizeBtn"
+ onclick="if (fileLoaded){visualizePath()}"
+ style="visibility: hidden; width:0vw; font-size: 0vw;">Process Path File</button>
+ <button id="toggleModeBtn" onclick="switchModes()">Mirror Autos Instead</button>
+ </span>
+ <label>
+ <input type="checkbox" id="vertical" onclick="if (fileVisualized){visualizePath()}">
+ Mirror Vertically</label>
+ <label>
+ <input type="checkbox" id="horizontal" onclick="if (fileVisualized){visualizePath()}">Mirror
+ Horizontally</label>
+ <span>
+ <button onclick="exportPath()" id="downloadBtn"
+ title="Downloads the currently displayed mirror of the path">Export Mirrored Path</button>
+ <button onclick="exportAllMirrors()" id="downloadAllBtn"
+ title="Downloads all possible mirrors of the current path.">Export All Mirrors</button>
+ </span>
+ </div>
+ <div id="centerDiv2" style="visibility: hidden; opacity: 0;">
+ <p id="infoText">(WARNING: The auto mirrorer is experimental and will not work with autos with paths in
+ different corners)</p>
+ <div id="autoList">
+ <pre>
+
+ </pre>
+ <pre>
+
+ </pre>
+ </div>
+ <span>
+ <input type="file" id="autoInput" accept=".auto"><button title="Finish loading in the .auto file"
+ id="visualizeBtn2" onclick="if (autoLoaded){visualizeAuto()}"
+ style="visibility: hidden; width:0vw; font-size: 0vw;">Process Auto File</button><button
+ id="toggleModeBtn" onclick="switchModes()">Mirror Paths Instead</button>
+ </span>
+ <p>(you must have already used the paths in this auto in the path mirrorer and have not renamed
+ them)<br />Select the area you are
+ converting to:</p>
+ <span id="selectArea">
+ <label>
+ <input name="area" type="radio" id="bottomR" onclick="if (autoVisualized){visualizeAuto()}">Bottom
+ Right</label>
+ <label>
+ <input name="area" type="radio" id="bottomL" onclick="if (autoVisualized){visualizeAuto()}">Bottom
+ Left</label>
+ <label>
+ <input name="area" type="radio" id="topR" onclick="if (autoVisualized){visualizeAuto()}">Top
+ Right</label>
+ <label>
+ <input name="area" type="radio" id="topL" onclick="if (autoVisualized){visualizeAuto()}">Top
+ Left</label>
+ </span>
+ <button onclick="exportAuto()" id="exportAuto"
+ title="Exports the auto with all path names modified as they were from the path mirrorer">Export Modified
+ Auto</button>
+ </div>
+ <p id="input"></p>
+ <br />
+ <div id="result"></div>
+ <a id="hiddenDownloader" style="display: none;"></a>
+</body>
+<script src="index.js"></script>
+
+</html>
\ No newline at end of file
--- /dev/null
+let pathJSON
+let autoJSON
+let importedPath
+let importedAuto
+let reader = new FileReader()
+let anotherReader = new FileReader()
+let fileLoaded = false
+let fileVisualized = false
+let autoLoaded = false
+let autoVisualized = false
+
+let firstTimeInAutos = true
+
+
+let pathMode = true
+
+let Ystatus
+let Xstatus
+
+const canvas = document.getElementById("graph");
+const ctx = canvas.getContext("2d");
+
+const fieldWidth = 8.071326
+// ctx.strokeStyle = "white";
+ctx.lineWidth = 5
+
+// ctx.beginPath();
+// ctx.moveTo(0,0);
+// ctx.lineTo(1650, 800);
+// ctx.stroke()
+
+document.getElementById("pathInput").addEventListener("change", async () => {
+ [importedPath] = document.getElementById("pathInput").files
+ reader.readAsText(importedPath)
+ fileLoaded = true
+
+ document.getElementById("visualizeBtn").style.visibility = "visible"
+ document.getElementById("visualizeBtn").style.fontSize = ""
+ document.getElementById("visualizeBtn").style.width = ""
+
+ // setTimeout(() => {
+ // visualizePath()
+ // }, 100);
+})
+
+document.getElementById("autoInput").addEventListener("change", async () => {
+ [importedAuto] = document.getElementById("autoInput").files
+ anotherReader.readAsText(importedAuto)
+ autoLoaded = true
+
+ document.getElementById("visualizeBtn2").style.visibility = "visible"
+ document.getElementById("visualizeBtn2").style.fontSize = ""
+ document.getElementById("visualizeBtn2").style.width = ""
+})
+
+function visualizePath() {
+ if (fileVisualized == false && fileLoaded) {
+ document.getElementById("downloadBtn").style.visibility = "visible"
+ document.getElementById("downloadBtn").style.opacity = 1
+ document.getElementById("downloadAllBtn").style.visibility = "visible"
+ document.getElementById("downloadAllBtn").style.opacity = 1
+ fileVisualized = true
+ }
+ ctx.strokeStyle = "white";
+ let startX = (JSON.parse(reader.result).waypoints[0].anchor.x) * 100
+ let startY = ((1 - (Number(JSON.parse(reader.result).waypoints[0].anchor.y) / fieldWidth)) * fieldWidth) * 100
+ let endX = (JSON.parse(reader.result).waypoints[1].anchor.x) * 100
+ let endY = ((1 - (Number(JSON.parse(reader.result).waypoints[1].anchor.y) / fieldWidth)) * fieldWidth) * 100
+ ctx.clearRect(0, 0, canvas.width, canvas.height)
+
+ ctx.beginPath();
+ ctx.moveTo(startX, startY);
+ ctx.lineTo(endX, endY);
+ ctx.stroke()
+
+ if (document.getElementById("horizontal").checked) {
+ startX = ((1 - (Number(JSON.parse(reader.result).waypoints[0].anchor.x) / 16.5)) * 16.5) * 100
+ endX = ((1 - (Number(JSON.parse(reader.result).waypoints[1].anchor.x) / 16.5)) * 16.5) * 100
+ }
+ if (document.getElementById("vertical").checked) {
+ // ctx.strokeStyle = "#d934eba4";
+ startY = (JSON.parse(reader.result).waypoints[0].anchor.y) * 100
+ endY = (JSON.parse(reader.result).waypoints[1].anchor.y) * 100
+ }
+
+ if (startX < 825) {
+ ctx.strokeStyle = "#5234eba4";
+ // Xstatus = "Blue"
+ // console.log(startX)
+ } else if (startX >= 825) {
+ ctx.strokeStyle = "#eb3434a4";
+ // Xstatus = "Red"
+ }
+
+ ctx.beginPath();
+ ctx.moveTo(startX, startY);
+ ctx.lineTo(endX, endY);
+ ctx.stroke()
+}
+
+function exportPath() {
+ //"reader.result" is just the text file
+ // let [file] = document.getElementById("pathInput").files
+ // let reader = new FileReader()
+
+ //X and Y are normal for this
+ // the X is [0, 16.5]
+ // the Y is [0, 8]
+ pathJSON = JSON.parse(reader.result)
+ console.log(pathJSON)
+ // document.getElementById("input").innerHTML = JSON.stringify(pathJSON)
+ console.log("START X: " + pathJSON.waypoints[0].anchor.x)
+ console.log("START Y: " + pathJSON.waypoints[0].anchor.y)
+ console.log("END X: " + pathJSON.waypoints[1].anchor.x)
+ console.log("END Y: " + pathJSON.waypoints[1].anchor.y)
+
+ //normal X and Y point flipping
+ // Loop through all waypoints
+ for (let i = 0; i < pathJSON.waypoints.length; i++) {
+ let waypoint = pathJSON.waypoints[i];
+
+ if (document.getElementById("horizontal").checked) {
+ waypoint.anchor.x = ((1 - (Number(waypoint.anchor.x) / 16.5)) * 16.5)
+
+ // Update nextControl if it exists
+ if (waypoint.nextControl != null) {
+ waypoint.nextControl.x = ((1 - (Number(waypoint.nextControl.x) / 16.5)) * 16.5)
+ }
+ // Update prevControl if it exists
+ if (waypoint.prevControl != null) {
+ waypoint.prevControl.x = ((1 - (Number(waypoint.prevControl.x) / 16.5)) * 16.5)
+ }
+ }
+
+ if (document.getElementById("vertical").checked) {
+ waypoint.anchor.y = ((1 - (Number(waypoint.anchor.y) / fieldWidth)) * fieldWidth)
+
+ // Update nextControl if it exists
+ if (waypoint.nextControl != null) {
+ waypoint.nextControl.y = ((1 - (Number(waypoint.nextControl.y) / fieldWidth)) * fieldWidth)
+ }
+ // Update prevControl if it exists
+ if (waypoint.prevControl != null) {
+ waypoint.prevControl.y = ((1 - (Number(waypoint.prevControl.y) / fieldWidth)) * fieldWidth)
+ }
+ }
+ }
+
+ // next stuff is for point towards zones
+ for (let i = 0; i < pathJSON.pointTowardsZones.length; i++) {
+ if (document.getElementById("horizontal").checked) {
+ pathJSON.pointTowardsZones[i].fieldPosition.x = ((1 - (Number(pathJSON.pointTowardsZones[i].fieldPosition.x) / 16.5)) * 16.5)
+ }
+ if (document.getElementById("vertical").checked) {
+ pathJSON.pointTowardsZones[i].fieldPosition.y = ((1 - (Number(pathJSON.pointTowardsZones[i].fieldPosition.y) / fieldWidth)) * fieldWidth)
+ }
+
+ if (document.getElementById("horizontal").checked && document.getElementById("vertical").checked == false) {
+ pathJSON.pointTowardsZones[i].rotationOffset = Number(pathJSON.pointTowardsZones[i].rotationOffset) * -1
+ } else if (document.getElementById("horizontal").checked && document.getElementById("vertical").checked) {
+ pathJSON.pointTowardsZones[i].rotationOffset = Number(pathJSON.pointTowardsZones[i].rotationOffset)
+ } else if (document.getElementById("horizontal").checked == false && document.getElementById("vertical").checked) {
+ pathJSON.pointTowardsZones[i].rotationOffset = -Number(pathJSON.pointTowardsZones[i].rotationOffset)
+ }
+ console.log("point towards zone: " + i)
+ }
+
+ // now into the starting state and end state rotation craziness
+ if (document.getElementById("horizontal").checked && document.getElementById("vertical").checked == false) {
+ pathJSON.idealStartingState.rotation = 180 - Number(pathJSON.idealStartingState.rotation)
+ pathJSON.goalEndState.rotation = 180 - Number(pathJSON.goalEndState.rotation)
+ } else if (document.getElementById("horizontal").checked && document.getElementById("vertical").checked) {
+ pathJSON.idealStartingState.rotation = 180 + Number(pathJSON.idealStartingState.rotation)
+ pathJSON.goalEndState.rotation = 180 + Number(pathJSON.goalEndState.rotation)
+ } else if (document.getElementById("horizontal").checked == false && document.getElementById("vertical").checked) {
+ pathJSON.idealStartingState.rotation = -Number(pathJSON.idealStartingState.rotation)
+ pathJSON.goalEndState.rotation = -Number(pathJSON.goalEndState.rotation)
+ }
+
+ //this is for the mid-path rotations
+ for (let i = 0; i < pathJSON.rotationTargets.length; i++) {
+ if (document.getElementById("horizontal").checked && document.getElementById("vertical").checked == false) {
+ pathJSON.rotationTargets[i].rotationDegrees = 180 - Number(pathJSON.rotationTargets[i].rotationDegrees)
+ } else if (document.getElementById("horizontal").checked && document.getElementById("vertical").checked) {
+ pathJSON.rotationTargets[i].rotationDegrees = 180 + Number(pathJSON.rotationTargets[i].rotationDegrees)
+ } else if (document.getElementById("horizontal").checked == false && document.getElementById("vertical").checked) {
+ pathJSON.rotationTargets[i].rotationDegrees = -Number(pathJSON.rotationTargets[i].rotationDegrees)
+ }
+ // console.log("rotation target: " + i)
+ }
+
+ console.log("CONVERTED START X: " + pathJSON.waypoints[0].anchor.x)
+ console.log("CONVERTED START Y: " + pathJSON.waypoints[0].anchor.y)
+ console.log("CONVERTED END X: " + pathJSON.waypoints[1].anchor.x)
+ console.log("CONVERTED END Y: " + pathJSON.waypoints[1].anchor.y)
+
+ if (pathJSON.waypoints[0].anchor.x < 8.25) {
+ Xstatus = "Left"
+ } else if (pathJSON.waypoints[0].anchor.x >= 8.25) {
+ Xstatus = "Right"
+ }
+ if (pathJSON.waypoints[0].anchor.y < 4 && ((pathJSON.waypoints[1].anchor.y + pathJSON.waypoints[0].anchor.y) / 2) < 4) {
+ Ystatus = "Bottom"
+ } else if (pathJSON.waypoints[0].anchor.y >= 4 && ((pathJSON.waypoints[1].anchor.y + pathJSON.waypoints[0].anchor.y) / 2) >= 4) {
+ Ystatus = "Top"
+ } else {
+ Ystatus = "bro what did you do for this to happen"
+ }
+
+ pathJSON.waypoints[0].linkedName = null
+ pathJSON.waypoints[1].linkedName = null
+ // document.getElementById("result").innerHTML = JSON.stringify(pathJSON)
+
+ let pathMirror = new File(["\ufeff" + JSON.stringify(pathJSON)], Ystatus + Xstatus + " " + importedPath.name);
+ document.getElementById("hiddenDownloader").href = window.URL.createObjectURL(pathMirror);
+ document.getElementById("hiddenDownloader").download = pathMirror.name
+ document.getElementById("hiddenDownloader").click()
+
+}
+
+function exportAllMirrors() {
+ document.getElementById("vertical").checked = true
+ document.getElementById("horizontal").checked = false
+ exportPath()
+ document.getElementById("vertical").checked = true
+ document.getElementById("horizontal").checked = true
+ exportPath()
+ document.getElementById("vertical").checked = false
+ document.getElementById("horizontal").checked = true
+ exportPath()
+ document.getElementById("vertical").checked = false
+ document.getElementById("horizontal").checked = false
+
+ alert(`Mirrored paths have been downloaded.\n \nMake sure to put these in your project's "pathplanner/paths" directory`)
+}
+
+function visualizeAuto() {
+ document.getElementById("exportAuto").style.visibility = "visible"
+ document.getElementById("exportAuto").style.opacity = 1
+
+ document.getElementById("autoList").querySelectorAll("pre")[0].innerHTML = ""
+ document.getElementById("autoList").querySelectorAll("pre")[1].innerHTML = ""
+
+ for (let i = 0; i < (JSON.parse(anotherReader.result).command.data.commands.length); i++) {
+ let checked
+ let textColor
+ console.log(i)
+
+ document.getElementById("autoList").querySelectorAll("pre")[0].innerHTML += `
+ `+ JSON.parse(anotherReader.result).command.data.commands[i].data.pathName + ``
+
+ if (document.getElementById("bottomR").checked) {
+ checked = "BottomRight"
+ textColor = "#eb3434a4"
+ } else if (document.getElementById("bottomL").checked) {
+ checked = "BottomLeft"
+ textColor = "#5234eba4"
+ } else if (document.getElementById("topR").checked) {
+ checked = "TopRight"
+ textColor = "#fb0505a4"
+ } else if (document.getElementById("topL").checked) {
+ checked = "TopLeft"
+ textColor = "#5234eba4"
+ }
+ document.getElementById("autoList").querySelectorAll("pre")[1].innerHTML += `<span style="color:` + textColor + `">
+ `+ checked + " " + JSON.parse(anotherReader.result).command.data.commands[i].data.pathName + `</span>`
+
+ }
+ if (autoVisualized == false) {
+ autoVisualized = true
+ }
+}
+
+function exportAuto() {
+ autoJSON = JSON.parse(anotherReader.result)
+ let checked
+
+ for (let i = 0; i < (autoJSON.command.data.commands.length); i++) {
+
+ if (document.getElementById("bottomR").checked) {
+ checked = "BottomRight"
+ } else if (document.getElementById("bottomL").checked) {
+ checked = "BottomLeft"
+ } else if (document.getElementById("topR").checked) {
+ checked = "TopRight"
+ } else if (document.getElementById("topL").checked) {
+ checked = "TopLeft"
+ }
+
+ autoJSON.command.data.commands[i].data.pathName = checked + " " + autoJSON.command.data.commands[i].data.pathName
+ }
+
+ let autoMirror = new File(["\ufeff" + JSON.stringify(autoJSON)], checked + " " + importedAuto.name);
+ document.getElementById("hiddenDownloader").href = window.URL.createObjectURL(autoMirror);
+ document.getElementById("hiddenDownloader").download = autoMirror.name
+ document.getElementById("hiddenDownloader").click()
+}
+
+let modeCooldown = false
+function switchModes() {
+ if (modeCooldown == false) {
+ pathMode = !pathMode
+
+ if (pathMode) {
+ document.getElementById("centerDiv2").style.opacity = 0
+ setTimeout(() => {
+ document.getElementById("centerDiv2").style.visibility = "hidden"
+ document.getElementById("centerDiv2").style.top = "1000%"
+ }, 200);
+ document.getElementById("centerDiv").style.visibility = "visible"
+ document.getElementById("centerDiv").style.opacity = 1
+ } else if (pathMode == false) {
+ document.getElementById("centerDiv").style.opacity = 0
+ setTimeout(() => {
+ document.getElementById("centerDiv").style.visibility = "hidden"
+ }, 200);
+ document.getElementById("centerDiv2").style.top = "50%"
+ document.getElementById("centerDiv2").style.visibility = "visible"
+ document.getElementById("centerDiv2").style.opacity = 1
+ if (firstTimeInAutos) {
+ alert('This is EXTREMELY experimental and will not work on most autos. If you are using it and think MAYBE it will work, make sure the paths you are using with it are in the same corner and have already been exported from the path mirrorer (this literally just renames the paths in the auto file for you.)') //basically its for lazy people
+ firstTimeInAutos = false
+ }
+ }
+ modeCooldown = true
+ setTimeout(() => {
+ modeCooldown = false
+ }, 1000);
+ }
+}
\ No newline at end of file
--- /dev/null
+{
+ "fileName": "AdvantageKit.json",
+ "name": "AdvantageKit",
+ "version": "26.0.2",
+ "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
+ "frcYear": "2026",
+ "mavenUrls": [
+ "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"
+ ],
+ "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
+ "javaDependencies": [
+ {
+ "groupId": "org.littletonrobotics.akit",
+ "artifactId": "akit-java",
+ "version": "26.0.2"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "org.littletonrobotics.akit",
+ "artifactId": "akit-wpilibio",
+ "version": "26.0.2",
+ "skipInvalidPlatforms": false,
+ "isJar": false,
+ "validPlatforms": [
+ "linuxathena",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal",
+ "windowsx86-64"
+ ]
+ }
+ ],
+ "cppDependencies": []
+}
\ No newline at end of file
--- /dev/null
+{
+ "fileName": "ChoreoLib2026.json",
+ "name": "ChoreoLib",
+ "version": "2026.0.3",
+ "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca",
+ "frcYear": "2026",
+ "mavenUrls": [
+ "https://frcmaven.wpi.edu/artifactory/sleipnirgroup-mvn-release/",
+ "https://repo1.maven.org/maven2"
+ ],
+ "jsonUrl": "https://choreo.autos/lib/ChoreoLib2026.json",
+ "javaDependencies": [
+ {
+ "groupId": "choreo",
+ "artifactId": "ChoreoLib-java",
+ "version": "2026.0.3"
+ },
+ {
+ "groupId": "com.google.code.gson",
+ "artifactId": "gson",
+ "version": "2.11.0"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "choreo",
+ "artifactId": "ChoreoLib-cpp",
+ "version": "2026.0.3",
+ "libName": "ChoreoLib",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal",
+ "linuxathena",
+ "linuxarm32",
+ "linuxarm64"
+ ]
+ }
+ ]
+}
\ No newline at end of file
--- /dev/null
+{
+ "fileName": "PathplannerLib.json",
+ "name": "PathplannerLib",
+ "version": "2026.1.2",
+ "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
+ "frcYear": "2026",
+ "mavenUrls": [
+ "https://3015rangerrobotics.github.io/pathplannerlib/repo"
+ ],
+ "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
+ "javaDependencies": [
+ {
+ "groupId": "com.pathplanner.lib",
+ "artifactId": "PathplannerLib-java",
+ "version": "2026.1.2"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "com.pathplanner.lib",
+ "artifactId": "PathplannerLib-cpp",
+ "version": "2026.1.2",
+ "libName": "PathplannerLib",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal",
+ "linuxathena",
+ "linuxarm32",
+ "linuxarm64"
+ ]
+ }
+ ]
+}
\ No newline at end of file
--- /dev/null
+{
+ "fileName": "Phoenix6-frc2026-latest.json",
+ "name": "CTRE-Phoenix (v6)",
+ "version": "26.2.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-frc2026-latest.json",
+ "conflictsWith": [
+ {
+ "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af",
+ "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.",
+ "offlineFileName": "Phoenix6-replay-frc2026-latest.json"
+ }
+ ],
+ "javaDependencies": [
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "wpiapi-java",
+ "version": "26.2.0"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "api-cpp",
+ "version": "26.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "tools",
+ "version": "26.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "api-cpp-sim",
+ "version": "26.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "tools-sim",
+ "version": "26.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simTalonSRX",
+ "version": "26.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simVictorSPX",
+ "version": "26.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simPigeonIMU",
+ "version": "26.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProTalonFX",
+ "version": "26.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProTalonFXS",
+ "version": "26.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANcoder",
+ "version": "26.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProPigeon2",
+ "version": "26.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANrange",
+ "version": "26.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANdi",
+ "version": "26.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANdle",
+ "version": "26.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "wpiapi-cpp",
+ "version": "26.2.0",
+ "libName": "CTRE_Phoenix6_WPI",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "tools",
+ "version": "26.2.0",
+ "libName": "CTRE_PhoenixTools",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "wpiapi-cpp-sim",
+ "version": "26.2.0",
+ "libName": "CTRE_Phoenix6_WPISim",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "tools-sim",
+ "version": "26.2.0",
+ "libName": "CTRE_PhoenixTools_Sim",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simTalonSRX",
+ "version": "26.2.0",
+ "libName": "CTRE_SimTalonSRX",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simVictorSPX",
+ "version": "26.2.0",
+ "libName": "CTRE_SimVictorSPX",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simPigeonIMU",
+ "version": "26.2.0",
+ "libName": "CTRE_SimPigeonIMU",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProTalonFX",
+ "version": "26.2.0",
+ "libName": "CTRE_SimProTalonFX",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProTalonFXS",
+ "version": "26.2.0",
+ "libName": "CTRE_SimProTalonFXS",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANcoder",
+ "version": "26.2.0",
+ "libName": "CTRE_SimProCANcoder",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProPigeon2",
+ "version": "26.2.0",
+ "libName": "CTRE_SimProPigeon2",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANrange",
+ "version": "26.2.0",
+ "libName": "CTRE_SimProCANrange",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANdi",
+ "version": "26.2.0",
+ "libName": "CTRE_SimProCANdi",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANdle",
+ "version": "26.2.0",
+ "libName": "CTRE_SimProCANdle",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ }
+ ]
+}
\ No newline at end of file
--- /dev/null
+{
+ "fileName": "REVLib.json",
+ "name": "REVLib",
+ "version": "2026.0.5",
+ "frcYear": "2026",
+ "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
+ "mavenUrls": [
+ "https://maven.revrobotics.com/"
+ ],
+ "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json",
+ "javaDependencies": [
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-java",
+ "version": "2026.0.5"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-driver",
+ "version": "2026.0.5",
+ "skipInvalidPlatforms": true,
+ "isJar": false,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ },
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "RevLibBackendDriver",
+ "version": "2026.0.5",
+ "skipInvalidPlatforms": true,
+ "isJar": false,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ },
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "RevLibWpiBackendDriver",
+ "version": "2026.0.5",
+ "skipInvalidPlatforms": true,
+ "isJar": false,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-cpp",
+ "version": "2026.0.5",
+ "libName": "REVLib",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ },
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-driver",
+ "version": "2026.0.5",
+ "libName": "REVLibDriver",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ },
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "RevLibBackendDriver",
+ "version": "2026.0.5",
+ "libName": "BackendDriver",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ },
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "RevLibWpiBackendDriver",
+ "version": "2026.0.5",
+ "libName": "REVLibWpi",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ }
+ ]
+}
\ No newline at end of file
--- /dev/null
+{
+ "fileName": "WPILibNewCommands.json",
+ "name": "WPILib-New-Commands",
+ "version": "1.0.0",
+ "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
+ "frcYear": "2026",
+ "mavenUrls": [],
+ "jsonUrl": "",
+ "javaDependencies": [
+ {
+ "groupId": "edu.wpi.first.wpilibNewCommands",
+ "artifactId": "wpilibNewCommands-java",
+ "version": "wpilib"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "edu.wpi.first.wpilibNewCommands",
+ "artifactId": "wpilibNewCommands-cpp",
+ "version": "wpilib",
+ "libName": "wpilibNewCommands",
+ "headerClassifier": "headers",
+ "sourcesClassifier": "sources",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "linuxsystemcore",
+ "linuxathena",
+ "linuxarm32",
+ "linuxarm64",
+ "windowsx86-64",
+ "windowsx86",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ]
+}
\ No newline at end of file
--- /dev/null
+{
+ "fileName": "libgrapplefrc2026.json",
+ "name": "libgrapplefrc",
+ "version": "2026.0.0",
+ "frcYear": "2026",
+ "uuid": "8ef3423d-9532-4665-8339-206dae1d7168",
+ "mavenUrls": [ "https://storage.googleapis.com/grapple-frc-maven" ],
+ "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2026.json",
+ "javaDependencies": [
+ {
+ "groupId": "au.grapplerobotics",
+ "artifactId": "libgrapplefrcjava",
+ "version": "2026.0.0"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "au.grapplerobotics",
+ "artifactId": "libgrapplefrcdriver",
+ "version": "2026.0.0",
+ "skipInvalidPlatforms": true,
+ "isJar": false,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "au.grapplerobotics",
+ "artifactId": "libgrapplefrccpp",
+ "version": "2026.0.0",
+ "libName": "grapplefrc",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ },
+ {
+ "groupId": "au.grapplerobotics",
+ "artifactId": "libgrapplefrcdriver",
+ "version": "2026.0.0",
+ "libName": "grapplefrcdriver",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ }
+ ]
+}
\ No newline at end of file
--- /dev/null
+{
+ "fileName": "photonlib.json",
+ "name": "photonlib",
+ "version": "v2026.3.4",
+ "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+ "frcYear": "2026",
+ "mavenUrls": [
+ "https://maven.photonvision.org/repository/internal",
+ "https://maven.photonvision.org/repository/snapshots"
+ ],
+ "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
+ "jniDependencies": [
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photontargeting-cpp",
+ "version": "v2026.3.4",
+ "skipInvalidPlatforms": true,
+ "isJar": false,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxathena",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photonlib-cpp",
+ "version": "v2026.3.4",
+ "libName": "photonlib",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxathena",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ },
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photontargeting-cpp",
+ "version": "v2026.3.4",
+ "libName": "photontargeting",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxathena",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ],
+ "javaDependencies": [
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photonlib-java",
+ "version": "v2026.3.4"
+ },
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photontargeting-java",
+ "version": "v2026.3.4"
+ }
+ ]
+}
\ No newline at end of file