diff --git a/.vscode/settings.json b/.vscode/settings.json index dccbc7c..8c1e215 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -56,5 +56,7 @@ "edu.wpi.first.math.proto.*", "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", - ] + ], + "git.enabled": false, + "java.debug.settings.onBuildFailureProceed": true } diff --git a/build.gradle b/build.gradle index 48a2abe..c7af5b2 100644 --- a/build.gradle +++ b/build.gradle @@ -49,6 +49,12 @@ 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 { @@ -72,6 +78,9 @@ dependencies { 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 { diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 index ec19934..f5feea6 --- a/gradlew +++ b/gradlew @@ -1,252 +1,252 @@ -#!/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" "$@" +#!/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" "$@" diff --git a/src/main/deploy/choreo/Test Path.traj b/src/main/deploy/choreo/Test Path.traj index bd7480f..71e1aa4 100644 --- a/src/main/deploy/choreo/Test Path.traj +++ b/src/main/deploy/choreo/Test Path.traj @@ -3,84 +3,475 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":2.776813268661499, "y":6.4990668296813965, "heading":0.0, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.8668832778930664, "y":6.168924331665039, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":7.228333473205566, "y":1.429280161857605, "heading":3.141592653589793, "intervals":72, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.31170654296875, "y":2.665813684463501, "heading":2.148697788119801, "intervals":100, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.4835398197174072, "y":0.747221827507019, "heading":-2.2769954387744447, "intervals":94, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.662995576858521, "y":2.645204782485962, "heading":0.9827933672865794, "intervals":74, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.684542179107666, "y":1.9651113748550415, "heading":2.506565824735817, "intervals":85, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.9004666805267334, "y":4.005391597747803, "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":17.548, "h":8.052}}, "enabled":false}, - {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, - {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":2.0}}, "enabled":true}, - {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":3.14}}, "enabled":true}], - "targetDt":0.05 + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.06182670593261719, "y":-0.041217803955078125, "w":17.548, "h":8.052}}, "enabled":true}, + {"from":1, "to":3, "data":{"type":"KeepOutCircle", "props":{"x":4.492737703025341, "y":4.018733609467745, "r":1.1122491970515056}}, "enabled":true}, + {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":5, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":false}], + "targetDt":0.02 }, "params":{ "waypoints":[ - {"x":{"exp":"2.776813268661499 m", "val":2.776813268661499}, "y":{"exp":"6.4990668296813965 m", "val":6.4990668296813965}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.8668832778930664 m", "val":3.8668832778930664}, "y":{"exp":"6.168924331665039 m", "val":6.168924331665039}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"7.228333473205566 m", "val":7.228333473205566}, "y":{"exp":"1.429280161857605 m", "val":1.429280161857605}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":72, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.31170654296875 m", "val":5.31170654296875}, "y":{"exp":"2.665813684463501 m", "val":2.665813684463501}, "heading":{"exp":"2.148697788119801 rad", "val":2.148697788119801}, "intervals":100, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.4835398197174072 m", "val":1.4835398197174072}, "y":{"exp":"0.747221827507019 m", "val":0.747221827507019}, "heading":{"exp":"-2.2769954387744447 rad", "val":-2.2769954387744447}, "intervals":94, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.6629955768585205 m", "val":3.662995576858521}, "y":{"exp":"2.645204782485962 m", "val":2.645204782485962}, "heading":{"exp":"0.9827933672865794 rad", "val":0.9827933672865794}, "intervals":74, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.684542179107666 m", "val":1.684542179107666}, "y":{"exp":"1.9651113748550415 m", "val":1.9651113748550415}, "heading":{"exp":"2.506565824735817 rad", "val":2.506565824735817}, "intervals":85, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.9004666805267334 m", "val":2.9004666805267334}, "y":{"exp":"4.005391597747803 m", "val":4.005391597747803}, "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":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, - {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"2 m / s ^ 2", "val":2.0}}}, "enabled":true}, - {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"3.14 rad / s", "val":3.14}}}, "enabled":true}], + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"61.82670593261719 mm", "val":0.06182670593261719}, "y":{"exp":"-41.217803955078125 mm", "val":-0.041217803955078125}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":true}, + {"from":1, "to":3, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"4.492737703025341 m", "val":4.492737703025341}, "y":{"exp":"4.018733609467745 m", "val":4.018733609467745}, "r":{"exp":"1.1122491970515056 m", "val":1.1122491970515056}}}, "enabled":true}, + {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":5, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":false}], "targetDt":{ - "exp":"0.05 s", - "val":0.05 + "exp":"0.02 s", + "val":0.02 } }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.63951], + "waypoints":[0.0,1.01317,2.45035,3.64108,4.58893,5.63143], "samples":[ - {"t":0.0, "x":2.77681, "y":6.49907, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":1.91306, "ay":-0.57932, "alpha":7.38842, "fx":[13.61343,13.60518,50.77241,52.17125], "fy":[11.47904,-32.1109,-29.06627,10.28203]}, - {"t":0.03564, "x":2.77803, "y":6.4987, "heading":0.0, "vx":0.06818, "vy":-0.02065, "omega":0.26333, "ax":1.91357, "ay":-0.57947, "alpha":5.79719, "fx":[17.82414,17.57744,46.8726,47.92275], "fy":[6.69007,-27.16601,-25.09639,6.14613]}, - {"t":0.07128, "x":2.78167, "y":6.49759, "heading":0.00939, "vx":0.13639, "vy":-0.0413, "omega":0.46996, "ax":1.91352, "ay":-0.57944, "alpha":4.53752, "fx":[20.98742,20.76525,43.92393,44.51707], "fy":[2.79227,-23.38723,-21.69932,2.86956]}, - {"t":0.10692, "x":2.78775, "y":6.49575, "heading":0.02614, "vx":0.20459, "vy":-0.06195, "omega":0.63168, "ax":1.91346, "ay":-0.57942, "alpha":3.47289, "fx":[23.42923,23.84767,41.17862,41.73428], "fy":[-0.38941,-20.28797,-18.88425,0.1386]}, - {"t":0.14257, "x":2.79626, "y":6.49318, "heading":0.04865, "vx":0.27279, "vy":-0.0826, "omega":0.75546, "ax":1.91339, "ay":-0.57939, "alpha":2.67282, "fx":[25.47821,25.85797,39.53361,39.31539], "fy":[-2.81823,-18.02397,-16.59322,-1.98547]}, - {"t":0.17821, "x":2.8072, "y":6.48987, "heading":0.07558, "vx":0.34098, "vy":-0.10325, "omega":0.85072, "ax":1.91331, "ay":-0.57935, "alpha":2.02149, "fx":[26.88001,27.82058,37.97749,37.5014], "fy":[-4.7304,-16.11009,-14.83815,-3.73978]}, - {"t":0.21385, "x":2.82056, "y":6.48582, "heading":0.1059, "vx":0.40918, "vy":-0.1239, "omega":0.92277, "ax":1.91321, "ay":-0.5793, "alpha":1.53752, "fx":[28.27209,28.93069,36.78107,36.18857], "fy":[-6.10483,-14.76961,-13.46571,-5.07485]}, - {"t":0.24949, "x":2.83636, "y":6.48103, "heading":0.13879, "vx":0.47737, "vy":-0.14455, "omega":0.97757, "ax":1.91307, "ay":-0.57924, "alpha":1.14034, "fx":[29.07833,30.24492,35.75202,35.0879], "fy":[-7.19019,-13.55503,-12.48213,-6.18378]}, - {"t":0.28513, "x":2.85459, "y":6.47551, "heading":0.17363, "vx":0.54555, "vy":-0.1652, "omega":1.01821, "ax":1.91289, "ay":-0.57916, "alpha":0.87154, "fx":[29.9973,30.66255,35.0672,34.42386], "fy":[-7.91504,-12.80918,-11.68487,-6.99638]}, - {"t":0.32077, "x":2.87525, "y":6.46926, "heading":0.20992, "vx":0.61373, "vy":-0.18584, "omega":1.04928, "ax":1.91264, "ay":-0.57905, "alpha":0.62479, "fx":[30.39849,31.66687,34.34293,33.72525], "fy":[-8.52093,-12.00068,-11.17976,-7.69655]}, - {"t":0.35642, "x":2.89834, "y":6.46227, "heading":0.24732, "vx":0.6819, "vy":-0.20648, "omega":1.07155, "ax":1.91227, "ay":-0.57881, "alpha":0.49138, "fx":[31.04587,31.5889,34.00773,33.4662], "fy":[-8.85776,-11.65897,-10.70855,-8.15652]}, - {"t":0.39206, "x":2.92386, "y":6.45454, "heading":0.28551, "vx":0.75005, "vy":-0.22711, "omega":1.08906, "ax":1.9116, "ay":-0.57859, "alpha":0.32468, "fx":[31.12444,32.59811,33.41245,32.92814], "fy":[-9.20882,-11.05837,-10.49032,-8.60902]}, - {"t":0.4277, "x":2.95181, "y":6.44608, "heading":0.32432, "vx":0.81819, "vy":-0.24773, "omega":1.10063, "ax":1.91045, "ay":-0.57742, "alpha":0.2844, "fx":[31.64766,32.04106,33.34892,32.94688], "fy":[-9.28696,-11.03893,-10.15936,-8.80153]}, - {"t":0.46334, "x":2.98218, "y":6.43688, "heading":0.36355, "vx":0.88628, "vy":-0.26831, "omega":1.11077, "ax":1.90565, "ay":-0.57731, "alpha":0.14417, "fx":[31.27842,33.52518,32.60306,32.25187], "fy":[-9.52904,-10.49728,-10.13185,-9.12141]}, - {"t":0.49898, "x":3.01498, "y":6.42695, "heading":0.40314, "vx":0.9542, "vy":-0.28888, "omega":1.11591, "ax":0.06801, "ay":-0.04108, "alpha":0.13866, "fx":[0.61226,0.93783,1.70099,1.37607], "fy":[-0.54444,-1.05567,-0.97169,-0.22297]}, - {"t":0.53462, "x":3.04903, "y":6.41663, "heading":0.44291, "vx":0.95662, "vy":-0.29035, "omega":1.12085, "ax":0.00468, "ay":0.01542, "alpha":0.12818, "fx":[-0.14132,-0.59185,0.6537,0.39782], "fy":[0.40762,-0.14478,0.11649,0.66989]}, - {"t":0.57027, "x":3.08313, "y":6.40629, "heading":0.48286, "vx":0.95679, "vy":-0.2898, "omega":1.12542, "ax":-0.00042, "ay":-0.00139, "alpha":0.09299, "fx":[-0.31223,-0.10231,0.29793,0.08796], "fy":[0.10835,-0.439,-0.07674,0.31297]}, - {"t":0.60591, "x":3.11723, "y":6.39596, "heading":0.52297, "vx":0.95677, "vy":-0.28985, "omega":1.12873, "ax":0.00078, "ay":0.00259, "alpha":0.06461, "fx":[-0.1568,-0.19788,0.28484,0.12322], "fy":[0.1051,-0.18363,-0.01701,0.2716]}, - {"t":0.64155, "x":3.15133, "y":6.38563, "heading":0.5632, "vx":0.9568, "vy":-0.28976, "omega":1.13103, "ax":0.00021, "ay":0.0007, "alpha":0.0429, "fx":[-0.16483,-0.03439,0.17202,0.04158], "fy":[0.04139,-0.12941,-0.03219,0.1678]}, - {"t":0.67719, "x":3.18544, "y":6.37531, "heading":0.60352, "vx":0.95681, "vy":-0.28973, "omega":1.13256, "ax":0.00007, "ay":0.00023, "alpha":0.03226, "fx":[-0.1046,-0.05259,0.12854,0.03336], "fy":[0.02599,-0.11666,-0.01827,0.12437]}, - {"t":0.71283, "x":3.21954, "y":6.36498, "heading":0.64388, "vx":0.95681, "vy":-0.28972, "omega":1.13371, "ax":-0.00036, "ay":-0.00117, "alpha":0.01686, "fx":[-0.08776,-0.01767,0.07566,0.00557], "fy":[-0.02604,-0.04701,-0.04849,0.04172]}, - {"t":0.74847, "x":3.25364, "y":6.35465, "heading":0.68429, "vx":0.9568, "vy":-0.28977, "omega":1.13431, "ax":-0.00006, "ay":-0.00021, "alpha":0.0124, "fx":[-0.04526,-0.0107,0.04649,0.00526], "fy":[0.00132,-0.05095,-0.00829,0.04398]}, - {"t":0.78411, "x":3.28774, "y":6.34432, "heading":0.72472, "vx":0.9568, "vy":-0.28977, "omega":1.13476, "ax":-0.00035, "ay":-0.00115, "alpha":0.003, "fx":[-0.02265,-0.00696,0.01075,-0.00494], "fy":[-0.02375,-0.02055,-0.02571,-0.00853]}, - {"t":0.81976, "x":3.32184, "y":6.334, "heading":0.76516, "vx":0.95679, "vy":-0.28981, "omega":1.13486, "ax":-0.00013, "ay":-0.00044, "alpha":-0.00347, "fx":[0.00991,0.00007,-0.01577,-0.00318], "fy":[-0.00769,0.00603,-0.00715,-0.02087]}, - {"t":0.8554, "x":3.35594, "y":6.32367, "heading":0.80561, "vx":0.95678, "vy":-0.28983, "omega":1.13474, "ax":-0.00013, "ay":-0.00043, "alpha":-0.00839, "fx":[0.04266,-0.00315,-0.04715,-0.00134], "fy":[0.00569,-0.00084,0.00395,-0.03834]}, - {"t":0.89104, "x":3.39005, "y":6.31334, "heading":0.84606, "vx":0.95678, "vy":-0.28984, "omega":1.13444, "ax":-0.00001, "ay":-0.00004, "alpha":-0.02041, "fx":[0.07122,0.00936,-0.08105,-0.0003], "fy":[0.00412,0.07841,-0.00548,-0.07977]}, - {"t":0.92668, "x":3.42415, "y":6.30301, "heading":0.88649, "vx":0.95678, "vy":-0.28985, "omega":1.13371, "ax":0.0002, "ay":0.00067, "alpha":-0.02395, "fx":[0.12295,-0.0087,-0.11611,0.01553], "fy":[0.05018,0.04501,0.02685,-0.07669]}, - {"t":0.96232, "x":3.45825, "y":6.29268, "heading":0.9269, "vx":0.95678, "vy":-0.28982, "omega":1.13286, "ax":0.00088, "ay":0.00291, "alpha":-0.04272, "fx":[0.15234,0.0628,-0.16948,0.01431], "fy":[0.07315,0.21621,0.02571,-0.11732]}, - {"t":0.99796, "x":3.49235, "y":6.28235, "heading":0.96727, "vx":0.95681, "vy":-0.28972, "omega":1.13134, "ax":0.00009, "ay":0.00031, "alpha":-0.05869, "fx":[0.22719,-0.0399,-0.22408,0.043], "fy":[0.04782,0.21502,-0.03213,-0.20993]}, - {"t":1.03361, "x":3.52645, "y":6.27202, "heading":1.0076, "vx":0.95682, "vy":-0.28971, "omega":1.12925, "ax":0.00281, "ay":0.00929, "alpha":-0.07387, "fx":[0.2419,0.26772,-0.33624,0.01799], "fy":[0.22571,0.45691,0.09067,-0.14091]}, - {"t":1.06925, "x":3.56056, "y":6.2617, "heading":1.04784, "vx":0.95692, "vy":-0.28938, "omega":1.12661, "ax":0.00466, "ay":0.01542, "alpha":-0.12596, "fx":[0.47092,-0.02575,-0.31251,0.18428], "fy":[0.28263,0.88694,0.07871,-0.19899]}, - {"t":1.10489, "x":3.59467, "y":6.2514, "heading":1.088, "vx":0.95708, "vy":-0.28883, "omega":1.12212, "ax":-0.08334, "ay":-0.01151, "alpha":-0.1115, "fx":[-1.28958,-0.44934,-2.29498,-1.63657], "fy":[-0.03409,0.32326,-0.35815,-0.71445]}, - {"t":1.14053, "x":3.62872, "y":6.2411, "heading":1.12799, "vx":0.95411, "vy":-0.28924, "omega":1.11815, "ax":-1.90538, "ay":0.57818, "alpha":-0.21063, "fx":[-31.78579,-32.61191,-33.03924,-32.20312], "fy":[9.94227,10.8803,9.46013,9.05579]}, - {"t":1.17617, "x":3.66152, "y":6.23115, "heading":1.16784, "vx":0.8862, "vy":-0.26863, "omega":1.11064, "ax":-1.90968, "ay":0.57995, "alpha":-0.27389, "fx":[-31.31111,-33.79386,-32.99277,-31.83438], "fy":[10.25314,10.70765,9.50149,8.99702]}, - {"t":1.21181, "x":3.69189, "y":6.22195, "heading":1.20743, "vx":0.81814, "vy":-0.24796, "omega":1.10088, "ax":-1.91121, "ay":0.57987, "alpha":-0.35122, "fx":[-31.38053,-32.97667,-33.64159,-32.0378], "fy":[10.34553,11.27034,9.23085,8.60727]}, - {"t":1.24745, "x":3.71984, "y":6.21348, "heading":1.24667, "vx":0.75002, "vy":-0.22729, "omega":1.08836, "ax":-1.91198, "ay":0.57976, "alpha":-0.48196, "fx":[-30.79146,-33.82486,-33.86244,-31.61038], "fy":[10.71562,11.43139,9.03196,8.26728]}, - {"t":1.2831, "x":3.74536, "y":6.20575, "heading":1.28546, "vx":0.68187, "vy":-0.20663, "omega":1.07118, "ax":-1.91242, "ay":0.57977, "alpha":-0.63496, "fx":[-30.4948,-33.57018,-34.56451,-31.48917], "fy":[11.04005,12.12765,8.59738,7.68192]}, - {"t":1.31874, "x":3.76844, "y":6.19875, "heading":1.32364, "vx":0.61371, "vy":-0.18596, "omega":1.04855, "ax":-1.91272, "ay":0.57974, "alpha":-0.8664, "fx":[-29.68047,-34.46448,-35.12187,-30.87217], "fy":[11.66038,12.64678,8.10394,7.03371]}, - {"t":1.35438, "x":3.7891, "y":6.19249, "heading":1.36101, "vx":0.54554, "vy":-0.1653, "omega":1.01767, "ax":-1.91292, "ay":0.57975, "alpha":-1.14278, "fx":[-28.99452,-34.70298,-36.07771,-30.37745], "fy":[12.3689,13.63652,7.33065,6.10968]}, - {"t":1.39002, "x":3.80733, "y":6.18697, "heading":1.39728, "vx":0.47736, "vy":-0.14464, "omega":0.97694, "ax":-1.91308, "ay":0.57973, "alpha":-1.53875, "fx":[-27.77566,-35.93074,-37.10754,-29.34958], "fy":[13.47312,14.60787,6.35543,5.00795]}, - {"t":1.42566, "x":3.82313, "y":6.18218, "heading":1.4321, "vx":0.40918, "vy":-0.12398, "omega":0.9221, "ax":-1.91319, "ay":0.57975, "alpha":-2.01933, "fx":[-26.55775,-36.80858,-38.52662,-28.27841], "fy":[14.84765,16.09033,4.97504,3.53221]}, - {"t":1.4613, "x":3.8365, "y":6.17813, "heading":1.46496, "vx":0.34099, "vy":-0.10331, "omega":0.85013, "ax":-1.91329, "ay":0.57973, "alpha":-2.68221, "fx":[-24.69371,-38.71738,-40.25977,-26.50728], "fy":[16.80668,17.68515,3.21897,1.73331]}, - {"t":1.49695, "x":3.84744, "y":6.17482, "heading":1.49526, "vx":0.27279, "vy":-0.08265, "omega":0.75453, "ax":-1.91337, "ay":0.57974, "alpha":-3.48835, "fx":[-22.71464,-40.53066,-42.41971,-24.51824], "fy":[19.29454,19.91281,0.85365,-0.61636]}, - {"t":1.53259, "x":3.85594, "y":6.17224, "heading":1.52216, "vx":0.2046, "vy":-0.06199, "omega":0.6302, "ax":-1.91343, "ay":0.57973, "alpha":-4.46246, "fx":[-20.45601,-42.54979,-45.06449,-22.11754], "fy":[22.55395,22.48437,-2.08159,-3.51273]}, - {"t":1.56823, "x":3.86202, "y":6.1704, "heading":1.54462, "vx":0.1364, "vy":-0.04133, "omega":0.47115, "ax":-1.91349, "ay":0.57973, "alpha":-5.83092, "fx":[-16.89739,-46.60792,-48.4303,-18.25582], "fy":[26.76499,25.59972,-5.76028,-7.16003]}, - {"t":1.60387, "x":3.86567, "y":6.16929, "heading":1.56141, "vx":0.0682, "vy":-0.02066, "omega":0.26333, "ax":-1.91353, "ay":0.57973, "alpha":-7.3882, "fx":[-13.01758,-50.64013,-52.36102,-14.17591], "fy":[32.08201,29.06241,-10.07519,-11.625]}, - {"t":1.63951, "x":3.86688, "y":6.16892, "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]}], + {"t":0.0, "x":7.22833, "y":1.42928, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.47413, "ay":4.8374, "alpha":-4.19434, "fx":[-125.43921,-142.16453,-132.78508,-108.1424], "fy":[87.90583,56.89109,75.97674,108.35744]}, + {"t":0.01407, "x":7.22759, "y":1.42976, "heading":3.14159, "vx":-0.10517, "vy":0.06807, "omega":-0.05902, "ax":-7.47418, "ay":4.83673, "alpha":-4.18467, "fx":[-125.44161,-142.13223,-132.76533,-108.19572], "fy":[87.88321,56.93843,75.97883,108.28526]}, + {"t":0.02814, "x":7.22537, "y":1.4312, "heading":3.14076, "vx":-0.21035, "vy":0.13613, "omega":-0.11791, "ax":-7.47422, "ay":4.83603, "alpha":-4.17475, "fx":[-125.43138,-142.09433,-132.76017,-108.25141], "fy":[87.8774,56.99781,75.95359,108.20936]}, + {"t":0.04222, "x":7.22167, "y":1.43359, "heading":3.1391, "vx":-0.31553, "vy":0.20418, "omega":-0.17665, "ax":-7.47423, "ay":4.8353, "alpha":-4.16453, "fx":[-125.40864,-142.05068,-132.76923,-108.30979], "fy":[87.88814,57.06944,75.90144,108.1293]}, + {"t":0.05629, "x":7.21649, "y":1.43694, "heading":3.13662, "vx":-0.4207, "vy":0.27223, "omega":-0.23526, "ax":-7.47423, "ay":4.83453, "alpha":-4.15396, "fx":[-125.3735,-142.00104,-132.79209,-108.37126], "fy":[87.91513,57.15359,75.82288,108.04454]}, + {"t":0.07036, "x":7.20983, "y":1.44125, "heading":3.13331, "vx":-0.52588, "vy":0.34026, "omega":-0.29371, "ax":-7.4742, "ay":4.83373, "alpha":-4.14295, "fx":[-125.32607,-141.94517,-132.82826,-108.43629], "fy":[87.95803,57.25061,75.71845,107.95443]}, + {"t":0.08443, "x":7.20169, "y":1.44652, "heading":3.12917, "vx":-0.63105, "vy":0.40828, "omega":-0.35201, "ax":-7.47414, "ay":4.83289, "alpha":-4.13144, "fx":[-125.26649,-141.88275,-132.87717,-108.50544], "fy":[88.01644,57.3609,75.58878,107.85822]}, + {"t":0.0985, "x":7.19207, "y":1.45274, "heading":3.12422, "vx":-0.73623, "vy":0.47628, "omega":-0.41015, "ax":-7.47405, "ay":4.83201, "alpha":-4.11935, "fx":[-125.19489,-141.81345,-132.93819,-108.57937], "fy":[88.08992,57.48496,75.43457,107.75503]}, + {"t":0.11257, "x":7.18097, "y":1.45992, "heading":3.11845, "vx":-0.8414, "vy":0.54428, "omega":-0.46811, "ax":-7.47393, "ay":4.83109, "alpha":-4.10656, "fx":[-125.11144,-141.73683,-133.01057,-108.65883], "fy":[88.17795,57.62335,75.25664,107.64383]}, + {"t":0.12665, "x":7.16839, "y":1.46806, "heading":3.11186, "vx":-0.94657, "vy":0.61226, "omega":-0.5259, "ax":-7.47377, "ay":4.83012, "alpha":-4.09299, "fx":[-125.01631,-141.65242,-133.09347,-108.7447], "fy":[88.27996,57.77673,75.05591,107.52347]}, + {"t":0.14072, "x":7.15433, "y":1.47715, "heading":3.10446, "vx":-1.05174, "vy":0.68023, "omega":-0.5835, "ax":-7.47357, "ay":4.82911, "alpha":-4.07848, "fx":[-124.90971,-141.55968,-133.18594,-108.83795], "fy":[88.39527,57.94585,74.83342,107.3926]}, + {"t":0.15479, "x":7.13879, "y":1.4872, "heading":3.09625, "vx":-1.15691, "vy":0.74819, "omega":-0.64089, "ax":-7.47332, "ay":4.82805, "alpha":-4.06288, "fx":[-124.79187,-141.45795,-133.28691,-108.93971], "fy":[88.52311,58.13155,74.59038,107.24966]}, + {"t":0.16886, "x":7.12177, "y":1.49821, "heading":3.08723, "vx":-1.26207, "vy":0.81612, "omega":-0.69806, "ax":-7.47302, "ay":4.82692, "alpha":-4.04602, "fx":[-124.66309,-141.34652,-133.39514,-109.05125], "fy":[88.6626,58.33479,74.32817,107.09289]}, + {"t":0.18293, "x":7.10327, "y":1.51017, "heading":3.07741, "vx":-1.36723, "vy":0.88405, "omega":-0.755, "ax":-7.47266, "ay":4.82574, "alpha":-4.02766, "fx":[-124.52368,-141.22452,-133.50925,-109.17402], "fy":[88.81268,58.55668,74.04839,106.92024]}, + {"t":0.19701, "x":7.08329, "y":1.52309, "heading":3.06678, "vx":-1.47239, "vy":0.95196, "omega":-0.81167, "ax":-7.47224, "ay":4.82449, "alpha":-4.00754, "fx":[-124.37404,-141.09096,-133.62765,-109.30966], "fy":[88.97215,58.79847,73.7529,106.72933]}, + {"t":0.21108, "x":7.06184, "y":1.53696, "heading":3.05536, "vx":-1.57754, "vy":1.01984, "omega":-0.86807, "ax":-7.47173, "ay":4.82316, "alpha":-3.98534, "fx":[-124.21465,-140.94468,-133.7485,-109.46004], "fy":[89.13956,59.06158,73.44383,106.51741]}, + {"t":0.22515, "x":7.0389, "y":1.55179, "heading":3.04315, "vx":-1.68268, "vy":1.08772, "omega":-0.92415, "ax":-7.47113, "ay":4.82174, "alpha":-3.96066, "fx":[-124.04607,-140.7843,-133.8697,-109.62731], "fy":[89.3132,59.34767,73.12371,106.28125]}, + {"t":0.23922, "x":7.01448, "y":1.56758, "heading":3.03014, "vx":-1.78781, "vy":1.15557, "omega":-0.97988, "ax":-7.47044, "ay":4.82022, "alpha":-3.93303, "fx":[-123.86899,-140.60817,-133.98877,-109.81395], "fy":[89.49103,59.65866,72.7955,106.01703]}, + {"t":0.25329, "x":6.98858, "y":1.58431, "heading":3.01635, "vx":-1.89293, "vy":1.2234, "omega":-1.03523, "ax":-7.46962, "ay":4.81857, "alpha":-3.90185, "fx":[-123.68428,-140.41431,-134.10281,-110.02283], "fy":[89.67054,59.99681,72.46268,105.72021]}, + {"t":0.26736, "x":6.9612, "y":1.60201, "heading":3.00179, "vx":-1.99804, "vy":1.2912, "omega":-1.09013, "ax":-7.46866, "ay":4.81678, "alpha":-3.86638, "fx":[-123.49299,-140.2003,-134.20839,-110.25733], "fy":[89.84868,60.36481,72.12946,105.38531]}, + {"t":0.28144, "x":6.93235, "y":1.62065, "heading":2.98645, "vx":-2.10314, "vy":1.35898, "omega":-1.14454, "ax":-7.46753, "ay":4.81481, "alpha":-3.82568, "fx":[-123.29644,-139.96311,-134.30133,-110.52146], "fy":[90.02162,60.76593,71.80087,105.00561]}, + {"t":0.29551, "x":6.90201, "y":1.64025, "heading":2.97034, "vx":-2.20822, "vy":1.42674, "omega":-1.19837, "ax":-7.4662, "ay":4.81261, "alpha":-3.77858, "fx":[-123.0963,-139.69895,-134.37654,-110.82001], "fy":[90.18455,61.20421,71.48308,104.57282]}, + {"t":0.30958, "x":6.8702, "y":1.66081, "heading":2.95348, "vx":-2.31329, "vy":1.49446, "omega":-1.25154, "ax":-7.46462, "ay":4.81014, "alpha":-3.72355, "fx":[-122.89475,-139.40293,-134.42763,-111.15887], "fy":[90.33125,61.68469,71.1837,104.07647]}, + {"t":0.32365, "x":6.83691, "y":1.68231, "heading":2.93587, "vx":-2.41833, "vy":1.56215, "omega":-1.30394, "ax":-7.46272, "ay":4.8073, "alpha":-3.65859, "fx":[-122.69456,-139.06859,-134.44649,-111.54532], "fy":[90.45354,62.21393,70.91234,103.50306]}, + {"t":0.33772, "x":6.80214, "y":1.70477, "heading":2.91752, "vx":-2.52334, "vy":1.62979, "omega":-1.35542, "ax":-7.46041, "ay":4.80398, "alpha":-3.58103, "fx":[-122.49947,-138.68726,-134.42251,-111.98864], "fy":[90.54045,62.80055,70.6813,102.83487]}, + {"t":0.3518, "x":6.76589, "y":1.72818, "heading":2.89844, "vx":-2.62832, "vy":1.69739, "omega":-1.40582, "ax":-7.45756, "ay":4.80002, "alpha":-3.48722, "fx":[-122.31453,-138.24686,-134.34144,-112.50083], "fy":[90.57677,63.45628,70.50674,102.04794]}, + {"t":0.36587, "x":6.72817, "y":1.75254, "heading":2.87866, "vx":-2.73327, "vy":1.76494, "omega":-1.45489, "ax":-7.45395, "ay":4.79517, "alpha":-3.37202, "fx":[-122.14675,-137.73006,-134.18354,-113.09795], "fy":[90.54078,64.19766,70.41046,101.10892]}, + {"t":0.37994, "x":6.68897, "y":1.77785, "heading":2.85819, "vx":-2.83816, "vy":1.83242, "omega":-1.50234, "ax":-7.44927, "ay":4.78905, "alpha":-3.22796, "fx":[-122.00623,-137.11094,-133.92039,-113.80206], "fy":[90.40021,65.04884,70.42281,99.9697]}, + {"t":0.39401, "x":6.64829, "y":1.80411, "heading":2.83705, "vx":-2.94298, "vy":1.89981, "omega":-1.54776, "ax":-7.44296, "ay":4.78105, "alpha":-3.04372, "fx":[-121.908,-136.34882,-133.50926,-114.64464], "fy":[90.10476,66.04678,70.58772,98.55787]}, + {"t":0.40808, "x":6.60614, "y":1.83132, "heading":2.81527, "vx":-3.04772, "vy":1.96708, "omega":-1.59059, "ax":-7.43408, "ay":4.77011, "alpha":-2.80116, "fx":[-121.87553,-135.37578,-132.88236,-115.67234], "fy":[89.57118,67.25123,70.97171,96.75867]}, + {"t":0.42216, "x":6.56252, "y":1.85947, "heading":2.79289, "vx":-3.15233, "vy":2.03421, "omega":-1.63001, "ax":-7.42075, "ay":4.75428, "alpha":-2.46928, "fx":[-121.94761,-134.06935,-131.92479,-116.95748], "fy":[88.65063,68.76581,71.68132,94.37812]}, + {"t":0.43623, "x":6.51743, "y":1.88857, "heading":2.76995, "vx":-3.25675, "vy":2.10111, "omega":-1.66476, "ax":-7.39899, "ay":4.72956, "alpha":-1.98994, "fx":[-122.19288,-132.18385,-130.42428,-118.61771], "fy":[87.04757,70.78727,72.89939,91.05979]}, + {"t":0.4503, "x":6.47087, "y":1.9186, "heading":2.74652, "vx":-3.36087, "vy":2.16766, "omega":-1.69276, "ax":-7.35874, "ay":4.68642, "alpha":-1.23885, "fx":[-122.74224,-129.14584,-127.94036,-120.85176], "fy":[84.08027,73.73882,74.96808,86.07149]}, + {"t":0.46437, "x":6.42284, "y":1.94957, "heading":2.7227, "vx":-3.46442, "vy":2.23361, "omega":-1.71019, "ax":-7.26758, "ay":4.5962, "alpha":0.11006, "fx":[-123.84901,-123.257,-123.39237,-123.97924], "fy":[77.75955,78.70942,78.59761,77.65387]}, + {"t":0.47844, "x":6.37337, "y":1.98146, "heading":2.69864, "vx":-3.56669, "vy":2.29829, "omega":-1.70864, "ax":-6.96138, "ay":4.32663, "alpha":3.30271, "fx":[-125.42771,-106.80586,-113.35378,-128.05718], "fy":[59.26208,89.22421,85.33729,60.5551]}, + {"t":0.49251, "x":6.3225, "y":2.01423, "heading":2.67459, "vx":-3.66465, "vy":2.35917, "omega":-1.66217, "ax":-4.95451, "ay":2.90281, "alpha":12.40074, "fx":[-94.41051,-37.65607,-85.61572,-119.41719], "fy":[-9.4431,92.12024,88.33206,26.49473]}, + {"t":0.50659, "x":6.27044, "y":2.04771, "heading":2.6512, "vx":-3.73437, "vy":2.40002, "omega":-1.48767, "ax":4.81508, "ay":-2.837, "alpha":-12.68993, "fx":[92.12784,34.03646,83.29007,118.15833], "fy":[12.09548,-90.00144,-88.57896,-26.5414]}, + {"t":0.52066, "x":6.21836, "y":2.0812, "heading":2.63027, "vx":-3.66661, "vy":2.3601, "omega":-1.66624, "ax":6.98024, "ay":-4.3859, "alpha":-2.18514, "fx":[123.70018,111.52593,114.60036,125.10103], "fy":[-64.80473,-84.23606,-83.18164,-66.18949]}, + {"t":0.53473, "x":6.16746, "y":2.11398, "heading":2.60682, "vx":-3.56839, "vy":2.29838, "omega":-1.69699, "ax":7.25843, "ay":-4.61849, "alpha":0.41837, "fx":[122.4571,124.71781,124.50401,122.17641], "fy":[-80.34103,-76.77273,-76.72801,-80.39505]}, + {"t":0.5488, "x":6.11796, "y":2.14586, "heading":2.58294, "vx":-3.46625, "vy":2.23339, "omega":-1.6911, "ax":7.34853, "ay":-4.69862, "alpha":1.53762, "fx":[121.21203,129.34554,129.23123,120.19705], "fy":[-86.37881,-73.63444,-72.77572,-86.89962]}, + {"t":0.56287, "x":6.06992, "y":2.17683, "heading":2.55914, "vx":-3.36284, "vy":2.16727, "omega":-1.66946, "ax":7.39081, "ay":-4.73757, "alpha":2.1684, "fx":[120.26726,131.59231,132.037,118.96554], "fy":[-89.69676,-72.07537,-70.0747,-90.49216]}, + {"t":0.57695, "x":6.02333, "y":2.20686, "heading":2.53565, "vx":-3.25884, "vy":2.1006, "omega":-1.63895, "ax":7.41471, "ay":-4.76017, "alpha":2.57521, "fx":[119.51226,132.85917,133.92185,118.19482], "fy":[-91.87156,-71.24877,-68.06878,-92.68766]}, + {"t":0.59102, "x":5.9782, "y":2.23594, "heading":2.51259, "vx":-3.1545, "vy":2.03362, "omega":-1.60271, "ax":7.42983, "ay":-4.7748, "alpha":2.85972, "fx":[118.87785,133.63075,135.29162,117.71713], "fy":[-93.45534,-70.81799,-66.49203,-94.10649]}, + {"t":0.60509, "x":5.93455, "y":2.26409, "heading":2.49004, "vx":-3.04995, "vy":1.96643, "omega":-1.56247, "ax":7.44017, "ay":-4.78499, "alpha":3.06974, "fx":[118.32544,134.11846,136.34177,117.43493], "fy":[-94.69047,-70.62306,-65.20296,-95.049]}, + {"t":0.61916, "x":5.89237, "y":2.29128, "heading":2.46805, "vx":-2.94525, "vy":1.8991, "omega":-1.51927, "ax":7.44764, "ay":-4.79249, "alpha":3.2308, "fx":[117.83264,134.42923,137.17819,117.28856], "fy":[-95.69992,-70.57799,-64.11943,-95.67851]}, + {"t":0.63323, "x":5.85166, "y":2.31753, "heading":2.44667, "vx":-2.84045, "vy":1.83166, "omega":-1.47381, "ax":7.45327, "ay":-4.79824, "alpha":3.35786, "fx":[117.38567,134.62304,137.86336,117.23956], "fy":[-96.5527,-70.63231,-63.19027,-96.09175]}, + {"t":0.6473, "x":5.81243, "y":2.34283, "heading":2.42593, "vx":-2.73557, "vy":1.76414, "omega":-1.42656, "ax":7.45766, "ay":-4.80279, "alpha":3.46029, "fx":[116.97567,134.73622,138.43668,117.2619], "fy":[-97.2906,-70.75425,-62.38173,-96.34995]}, + {"t":0.66138, "x":5.77467, "y":2.36718, "heading":2.40586, "vx":-2.63062, "vy":1.69655, "omega":-1.37787, "ax":7.46118, "ay":-4.80648, "alpha":3.54431, "fx":[116.59662,134.79214,138.92433,117.33725], "fy":[-97.94047,-70.92277,-61.67044,-96.49394]}, + {"t":0.67545, "x":5.73839, "y":2.39058, "heading":2.38647, "vx":-2.52563, "vy":1.62892, "omega":-1.32799, "ax":7.46408, "ay":-4.80954, "alpha":3.61417, "fx":[116.2443,134.80655,139.34448,117.45221], "fy":[-98.52038,-71.1233,-61.03955,-96.55219]}, + {"t":0.68952, "x":5.70359, "y":2.41303, "heading":2.36778, "vx":-2.4206, "vy":1.56124, "omega":-1.27713, "ax":7.46651, "ay":-4.81211, "alpha":3.67294, "fx":[115.91566,134.79051,139.7102,117.59669], "fy":[-99.04302,-71.3454,-60.47648,-96.54532]}, + {"t":0.70359, "x":5.67027, "y":2.43452, "heading":2.34981, "vx":-2.31553, "vy":1.49352, "omega":-1.22545, "ax":7.46859, "ay":-4.8143, "alpha":3.72286, "fx":[115.60837,134.75205,140.03118,117.76291], "fy":[-99.51758,-71.58135,-59.97151,-96.48869]}, + {"t":0.71766, "x":5.63842, "y":2.45506, "heading":2.33256, "vx":-2.21043, "vy":1.42578, "omega":-1.17306, "ax":7.4704, "ay":-4.81618, "alpha":3.76563, "fx":[115.32063,134.69713,140.31479,117.94469], "fy":[-99.95092,-71.82533,-59.51695,-96.39413]}, + {"t":0.73174, "x":5.60806, "y":2.47465, "heading":2.31606, "vx":-2.10531, "vy":1.358, "omega":-1.12007, "ax":7.47198, "ay":-4.81781, "alpha":3.80257, "fx":[115.05101,134.63032,140.56681,118.13707], "fy":[-100.34831,-72.07282,-59.10657,-96.27089]}, + {"t":0.74581, "x":5.57917, "y":2.49328, "heading":2.3003, "vx":-2.00017, "vy":1.29021, "omega":-1.06656, "ax":7.4734, "ay":-4.81924, "alpha":3.8347, "fx":[114.79833,134.55521,140.7918,118.33599], "fy":[-100.71389,-72.32028,-58.73521,-96.12644]}, + {"t":0.75988, "x":5.55177, "y":2.51096, "heading":2.28529, "vx":-1.895, "vy":1.22239, "omega":-1.0126, "ax":7.47467, "ay":-4.8205, "alpha":3.86283, "fx":[114.56161,134.47462,140.99346,118.53806], "fy":[-101.05096,-72.56485,-58.39853,-95.96685]}, + {"t":0.77395, "x":5.52584, "y":2.52768, "heading":2.27104, "vx":-1.78982, "vy":1.15456, "omega":-0.95824, "ax":7.47582, "ay":-4.8216, "alpha":3.88763, "fx":[114.34,134.39085,141.17483,118.74045], "fy":[-101.36222,-72.80424,-58.09282,-95.79719]}, + {"t":0.78802, "x":5.50139, "y":2.54345, "heading":2.25755, "vx":-1.68462, "vy":1.08671, "omega":-0.90354, "ax":7.47687, "ay":-4.82258, "alpha":3.90965, "fx":[114.13277,134.30578,141.33845,118.94076], "fy":[-101.64989,-73.03657,-57.81487,-95.62169]}, + {"t":0.80209, "x":5.47843, "y":2.55826, "heading":2.24484, "vx":-1.57941, "vy":1.01885, "omega":-0.84852, "ax":7.47784, "ay":-4.82345, "alpha":3.92931, "fx":[113.93928,134.22097,141.48644,119.13692], "fy":[-101.91587,-73.26026,-57.56187,-95.44399]}, + {"t":0.81617, "x":5.45694, "y":2.57212, "heading":2.2329, "vx":-1.47418, "vy":0.95097, "omega":-0.79323, "ax":7.47873, "ay":-4.82422, "alpha":3.947, "fx":[113.75893,134.13773,141.62061,119.32717], "fy":[-102.16174,-73.47401,-57.33136,-95.26719]}, + {"t":0.83024, "x":5.43694, "y":2.58503, "heading":2.22174, "vx":-1.36894, "vy":0.88309, "omega":-0.73769, "ax":7.47956, "ay":-4.8249, "alpha":3.96302, "fx":[113.59123,134.05718,141.74251,119.50998], "fy":[-102.38889,-73.67673,-57.12116,-95.09398]}, + {"t":0.84431, "x":5.41842, "y":2.59698, "heading":2.21136, "vx":-1.26369, "vy":0.81519, "omega":-0.68192, "ax":7.48034, "ay":-4.82551, "alpha":3.97763, "fx":[113.43568,133.98025,141.85348,119.68402], "fy":[-102.59849,-73.86746,-56.92932,-94.92674]}, + {"t":0.85838, "x":5.40137, "y":2.60797, "heading":2.20176, "vx":-1.15843, "vy":0.74729, "omega":-0.62595, "ax":7.48106, "ay":-4.82605, "alpha":3.99105, "fx":[113.29187,133.90778,141.95469,119.84813], "fy":[-102.79158,-74.04542,-56.75411,-94.76751]}, + {"t":0.87245, "x":5.38581, "y":2.61801, "heading":2.19295, "vx":-1.05316, "vy":0.67938, "omega":-0.56979, "ax":7.48173, "ay":-4.82652, "alpha":4.00346, "fx":[113.1594,133.84047,142.04715,120.00128], "fy":[-102.96909,-74.20991,-56.59398,-94.61811]}, + {"t":0.88653, "x":5.37173, "y":2.62709, "heading":2.18493, "vx":-0.94787, "vy":0.61146, "omega":-0.51345, "ax":7.48236, "ay":-4.82695, "alpha":4.01503, "fx":[113.03793,133.77893,142.13173,120.14261], "fy":[-103.1318,-74.36034,-56.44753,-94.48017]}, + {"t":0.9006, "x":5.35914, "y":2.63522, "heading":2.17771, "vx":-0.84258, "vy":0.54354, "omega":-0.45695, "ax":7.48295, "ay":-4.82732, "alpha":4.02589, "fx":[112.92712,133.7237,142.20923,120.27131], "fy":[-103.28042,-74.49619,-56.31353,-94.3551]}, + {"t":0.91467, "x":5.34802, "y":2.64239, "heading":2.17128, "vx":-0.73728, "vy":0.47561, "omega":-0.4003, "ax":7.4835, "ay":-4.82765, "alpha":4.03616, "fx":[112.8267,133.67527,142.28033,120.38671], "fy":[-103.41558,-74.61701,-56.19087,-94.24416]}, + {"t":0.92874, "x":5.33839, "y":2.6486, "heading":2.16565, "vx":-0.63198, "vy":0.40767, "omega":-0.3435, "ax":7.48402, "ay":-4.82794, "alpha":4.04593, "fx":[112.73641,133.63404,142.34561,120.4882], "fy":[-103.53782,-74.7224,-56.07857,-94.14848]}, + {"t":0.94281, "x":5.33024, "y":2.65386, "heading":2.16081, "vx":-0.52666, "vy":0.33973, "omega":-0.28657, "ax":7.48451, "ay":-4.82819, "alpha":4.05531, "fx":[112.65602,133.60038,142.40562,120.57525], "fy":[-103.64762,-74.81201,-55.97577,-94.06904]}, + {"t":0.95689, "x":5.32357, "y":2.65816, "heading":2.15678, "vx":-0.42134, "vy":0.27179, "omega":-0.2295, "ax":7.48496, "ay":-4.82841, "alpha":4.06436, "fx":[112.58534,133.57461,142.46079,120.64738], "fy":[-103.74538,-74.88554,-55.8817,-94.00672]}, + {"t":0.97096, "x":5.31838, "y":2.66151, "heading":2.15355, "vx":-0.31602, "vy":0.20385, "omega":-0.17231, "ax":7.48538, "ay":-4.8286, "alpha":4.07315, "fx":[112.52419,133.55702,142.51153,120.70418], "fy":[-103.83148,-74.94271,-55.79572,-93.96229]}, + {"t":0.98503, "x":5.31467, "y":2.6639, "heading":2.15113, "vx":-0.21068, "vy":0.1359, "omega":-0.11499, "ax":7.48578, "ay":-4.82876, "alpha":4.08174, "fx":[112.47244,133.54785,142.55818,120.74528], "fy":[-103.90621,-74.98327,-55.71727,-93.93641]}, + {"t":0.9991, "x":5.31245, "y":2.66534, "heading":2.14951, "vx":-0.10534, "vy":0.06795, "omega":-0.05756, "ax":7.48614, "ay":-4.82889, "alpha":4.09019, "fx":[112.42999,133.54732,142.60102,120.77037], "fy":[-103.96981,-75.007,-55.64587,-93.92968]}, + {"t":1.01317, "x":5.31171, "y":2.66581, "heading":2.1487, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.15617, "ay":-5.94845, "alpha":9.14467, "fx":[-89.86712,-56.09431,-138.55297,-134.34458], "fy":[-124.0909,-142.35869,-64.65956,-73.61628]}, + {"t":1.02754, "x":5.31107, "y":2.6652, "heading":2.1487, "vx":-0.08848, "vy":-0.08549, "omega":0.13143, "ax":-6.19466, "ay":-5.90111, "alpha":9.19356, "fx":[-90.45766,-56.98006,-139.25765,-134.78208], "fy":[-123.64792,-141.98577,-63.08093,-72.7901]}, + {"t":1.04192, "x":5.30916, "y":2.66336, "heading":2.15059, "vx":-0.1775, "vy":-0.1703, "omega":0.26355, "ax":-6.23435, "ay":-5.851, "alpha":9.24918, "fx":[-91.01309,-57.93832,-140.02241,-135.2041], "fy":[-123.22551,-141.57462,-61.31546,-71.97982]}, + {"t":1.05629, "x":5.30596, "y":2.66031, "heading":2.15437, "vx":-0.2671, "vy":-0.25439, "omega":0.39648, "ax":-6.27537, "ay":-5.79791, "alpha":9.31108, "fx":[-91.53667,-58.97961,-140.84099,-135.61196], "fy":[-122.82181,-141.11877,-59.35902,-71.18368]}, + {"t":1.07066, "x":5.30148, "y":2.65605, "heading":2.16007, "vx":-0.35729, "vy":-0.33772, "omega":0.5303, "ax":-6.31789, "ay":-5.74162, "alpha":9.37857, "fx":[-92.0322,-60.11715,-141.70561,-136.00691], "fy":[-122.43444,-140.60999,-57.20891,-70.39985]}, + {"t":1.08503, "x":5.29569, "y":2.65061, "heading":2.16769, "vx":-0.44809, "vy":-0.42023, "omega":0.66509, "ax":-6.36208, "ay":-5.68188, "alpha":9.45058, "fx":[-92.50411,-61.36733,-142.60709,-136.39019], "fy":[-122.06039,-140.03784,-54.86411,-69.62637]}, + {"t":1.0994, "x":5.28859, "y":2.64398, "heading":2.17725, "vx":-0.53953, "vy":-0.50189, "omega":0.80091, "ax":-6.40819, "ay":-5.61843, "alpha":9.52561, "fx":[-92.95766,-62.75015,-143.53482,-136.76307], "fy":[-121.69589,-139.38909,-52.32562,-68.86113]}, + {"t":1.11378, "x":5.28018, "y":2.63619, "heading":2.18876, "vx":-0.63162, "vy":-0.58264, "omega":0.93781, "ax":-6.4565, "ay":-5.55097, "alpha":9.60161, "fx":[-93.3991,-64.28994,-144.47693,-137.12687], "fy":[-121.33621,-138.64695,-49.59678,-68.1017]}, + {"t":1.12815, "x":5.27043, "y":2.62724, "heading":2.20224, "vx":-0.72441, "vy":-0.66242, "omega":1.0758, "ax":-6.50739, "ay":-5.47914, "alpha":9.6759, "fx":[-93.83596,-66.01604,-145.42048,-137.48302], "fy":[-120.97545,-137.79002,-46.68373,-67.34525]}, + {"t":1.14252, "x":5.25935, "y":2.61716, "heading":2.2177, "vx":-0.81794, "vy":-0.74116, "omega":1.21486, "ax":-6.56134, "ay":-5.40252, "alpha":9.745, "fx":[-94.27737,-67.96372,-146.35163,-137.83311], "fy":[-120.60623,-136.79088,-43.59577,-66.58841]}, + {"t":1.15689, "x":5.24692, "y":2.60595, "heading":2.23516, "vx":-0.91224, "vy":-0.81881, "omega":1.35491, "ax":-6.61893, "ay":-5.32059, "alpha":9.80451, "fx":[-94.73451,-70.17525,-147.25603,-138.17896], "fy":[-120.21928,-135.61421,-40.34595,-65.82713]}, + {"t":1.17126, "x":5.23312, "y":2.59363, "heading":2.25463, "vx":-1.00736, "vy":-0.89527, "omega":1.49582, "ax":-6.68095, "ay":-5.23267, "alpha":9.84898, "fx":[-95.22123,-72.70097,-148.11911,-138.52257], "fy":[-119.8028,-134.21406,-36.95175,-65.05656]}, + {"t":1.18563, "x":5.21796, "y":2.58022, "heading":2.27613, "vx":-1.10338, "vy":-0.97048, "omega":1.63737, "ax":-6.74832, "ay":-5.13794, "alpha":9.87167, "fx":[-95.75488,-75.60061,-148.92639,-138.86619], "fy":[-119.34172,-132.53027,-33.43647,-64.27104]}, + {"t":1.20001, "x":5.2014, "y":2.56574, "heading":2.29966, "vx":-1.20036, "vy":-1.04432, "omega":1.77924, "ax":-6.82222, "ay":-5.03528, "alpha":9.86466, "fx":[-96.357,-78.94252,-149.66423,-139.21226], "fy":[-118.81677,-130.48435,-29.82971,-63.46392]}, + {"t":1.21438, "x":5.18345, "y":2.55021, "heading":2.32524, "vx":-1.29841, "vy":-1.11668, "omega":1.92102, "ax":-6.90399, "ay":-4.92329, "alpha":9.81885, "fx":[-97.05428,-82.80204,-150.32023,-139.56329], "fy":[-118.20335,-127.97425,-26.16963,-62.62789]}, + {"t":1.22875, "x":5.16407, "y":2.53366, "heading":2.35284, "vx":-1.39764, "vy":-1.18744, "omega":2.06213, "ax":-6.99518, "ay":-4.80026, "alpha":9.72364, "fx":[-97.88087,-87.25942,-150.88304,-139.92074], "fy":[-117.46895,-124.86667,-22.51148,-61.75718]}, + {"t":1.24312, "x":5.14326, "y":2.5161, "heading":2.38248, "vx":-1.49817, "vy":-1.25643, "omega":2.20188, "ax":-7.09737, "ay":-4.66442, "alpha":9.5661, "fx":[-98.88139,-92.39159,-151.34174,-140.28228], "fy":[-116.56935,-120.99003,-18.94839,-60.85393]}, + {"t":1.25749, "x":5.121, "y":2.49756, "heading":2.41413, "vx":-1.60017, "vy":-1.32346, "omega":2.33936, "ax":-7.21184, "ay":-4.51518, "alpha":9.32714, "fx":[-100.1175,-98.25346,-151.68242,-140.63173], "fy":[-115.44091,-116.1326,-15.68253,-59.9519]}, + {"t":1.27187, "x":5.09726, "y":2.47807, "heading":2.44775, "vx":-1.70382, "vy":-1.38836, "omega":2.47341, "ax":-7.33809, "ay":-4.36013, "alpha":8.95824, "fx":[-101.69088,-104.82897,-151.86671,-140.88855], "fy":[-113.97547,-110.07228,-13.37315,-59.23739]}, + {"t":1.28624, "x":5.07201, "y":2.45767, "heading":2.48329, "vx":-1.80928, "vy":-1.45102, "omega":2.60216, "ax":-7.46342, "ay":-4.25168, "alpha":8.28853, "fx":[-103.76951,-111.67201,-151.69999,-140.66074], "fy":[-111.98383,-102.98854,-14.67391,-59.63334]}, + {"t":1.30061, "x":5.04524, "y":2.43637, "heading":2.52069, "vx":-1.91654, "vy":-1.51212, "omega":2.72128, "ax":-7.55583, "ay":-4.20516, "alpha":7.53749, "fx":[-105.75816,-117.12582,-151.2262,-139.97983], "fy":[-109.98655,-96.62548,-18.4597,-61.04251]}, + {"t":1.31498, "x":5.01691, "y":2.41421, "heading":2.5598, "vx":-2.02513, "vy":-1.57256, "omega":2.8296, "ax":-7.63488, "ay":-4.1789, "alpha":6.73589, "fx":[-108.12073,-121.75565,-150.46165,-139.13069], "fy":[-107.51966,-90.61484,-23.44279,-62.75]}, + {"t":1.32935, "x":4.98702, "y":2.39118, "heading":2.60047, "vx":-2.13486, "vy":-1.63262, "omega":2.92641, "ax":-7.70816, "ay":-4.15802, "alpha":5.86562, "fx":[-111.15323,-125.73627,-149.33857,-138.22629], "fy":[-104.20421,-84.90382,-29.32602,-64.4726]}, + {"t":1.34372, "x":4.95554, "y":2.36728, "heading":2.64253, "vx":-2.24564, "vy":-1.69238, "omega":3.01071, "ax":-7.77715, "ay":-4.13856, "alpha":4.88842, "fx":[-115.05027,-129.05561,-147.72369,-137.31863], "fy":[-99.66423,-79.67221,-36.16531,-66.08131]}, + {"t":1.3581, "x":4.92246, "y":2.34253, "heading":2.68579, "vx":-2.35741, "vy":-1.75185, "omega":3.08097, "ax":-7.84166, "ay":-4.1171, "alpha":3.7444, "fx":[-119.97426,-131.69622,-145.40576,-136.46167], "fy":[-93.39859,-75.13634,-44.14058,-67.44736]}, + {"t":1.37247, "x":4.88777, "y":2.31693, "heading":2.73007, "vx":-2.47011, "vy":-1.81102, "omega":3.13478, "ax":-7.8991, "ay":-4.08791, "alpha":2.35016, "fx":[-126.00784,-133.65754,-142.04701,-135.73358], "fy":[-84.71876,-71.5017,-53.53007,-68.38609]}, + {"t":1.38684, "x":4.85146, "y":2.29048, "heading":2.77513, "vx":-2.58364, "vy":-1.86978, "omega":3.16856, "ax":-7.94146, "ay":-4.04127, "alpha":0.60011, "fx":[-133.02175,-134.95049,-137.09142,-135.26417], "fy":[-72.72587,-68.94324,-64.71462,-68.5795]}, + {"t":1.40121, "x":4.81351, "y":2.26319, "heading":2.82066, "vx":-2.69777, "vy":-1.92786, "omega":3.17718, "ax":-7.95007, "ay":-3.96195, "alpha":-1.62285, "fx":[-140.43564,-135.58519,-129.60143,-135.2911], "fy":[-56.42055,-67.59643,-78.15813,-67.39125]}, + {"t":1.41558, "x":4.77391, "y":2.23507, "heading":2.86633, "vx":-2.81203, "vy":-1.9848, "omega":3.15386, "ax":-7.88953, "ay":-3.82516, "alpha":-4.42183, "fx":[-146.94102,-135.55892,-118.00357,-136.29133], "fy":[-35.15325,-67.55228,-94.25508,-63.2989]}, + {"t":1.42996, "x":4.73269, "y":2.20615, "heading":2.91165, "vx":-2.92541, "vy":-2.03977, "omega":3.09031, "ax":-7.7099, "ay":-3.56881, "alpha":-7.88092, "fx":[-150.5646,-134.85058,-99.92472,-139.23279], "fy":[-9.62803,-68.84676,-112.73023,-51.61272]}, + {"t":1.44433, "x":4.68985, "y":2.17647, "heading":2.95607, "vx":-3.03622, "vy":-2.09106, "omega":2.97704, "ax":-7.35418, "ay":-2.91813, "alpha":-12.47941, "fx":[-149.8423,-133.44257,-73.78647,-143.29889], "fy":[16.38885,-71.40061,-130.86229,-12.67187]}, + {"t":1.4587, "x":4.64545, "y":2.14612, "heading":2.99885, "vx":-3.14191, "vy":-2.133, "omega":2.79769, "ax":-6.93454, "ay":-2.1636, "alpha":-16.58561, "fx":[-148.36024,-131.68159,-58.0017,-133.77482], "fy":[25.74998,-74.28284,-138.14365,39.46751]}, + {"t":1.47307, "x":4.59958, "y":2.11524, "heading":3.03906, "vx":-3.24157, "vy":-2.16409, "omega":2.55933, "ax":-6.87123, "ay":-2.18504, "alpha":-16.9234, "fx":[-148.62343,-130.04227,-56.43304,-132.41187], "fy":[22.46073,-76.61369,-138.26885,43.75447]}, + {"t":1.48744, "x":4.55228, "y":2.08391, "heading":3.07584, "vx":-3.34033, "vy":-2.1955, "omega":2.31611, "ax":-6.8325, "ay":-2.25677, "alpha":-16.96499, "fx":[-148.76944,-128.44441,-55.63544,-132.02638], "fy":[18.94508,-78.65526,-137.956,44.11785]}, + {"t":1.50181, "x":4.50357, "y":2.05212, "heading":3.10913, "vx":-3.43852, "vy":-2.22793, "omega":2.07229, "ax":-6.77949, "ay":-2.31533, "alpha":-17.03678, "fx":[-148.68545,-126.8602,-54.6208,-131.10258], "fy":[15.93704,-80.44206,-137.57163,44.54409]}, + {"t":1.51619, "x":4.45345, "y":2.01987, "heading":3.13891, "vx":-3.53596, "vy":-2.26121, "omega":1.82744, "ax":-6.71101, "ay":-2.35978, "alpha":-17.13224, "fx":[-148.36583,-125.27222,-53.38716,-129.58439], "fy":[13.40278,-81.95488,-137.04666,45.04169]}, + {"t":1.53056, "x":4.40194, "y":1.98712, "heading":-3.11801, "vx":-3.6324, "vy":-2.29512, "omega":1.58122, "ax":-6.62585, "ay":-2.3906, "alpha":-17.22723, "fx":[-147.77191,-123.65534,-52.00212,-127.38566], "fy":[11.28989,-83.14146,-136.24112,45.43883]}, + {"t":1.54493, "x":4.34905, "y":1.95389, "heading":-3.09529, "vx":-3.72763, "vy":-2.32948, "omega":1.33363, "ax":-6.52022, "ay":-2.40659, "alpha":-17.28845, "fx":[-146.80379,-121.96309,-50.56719,-124.29413], "fy":[9.56183,-83.89654,-134.91713,45.51047]}, + {"t":1.5593, "x":4.29481, "y":1.92017, "heading":-3.07612, "vx":-3.82134, "vy":-2.36407, "omega":1.08517, "ax":-6.38481, "ay":-2.40273, "alpha":-17.26451, "fx":[-145.2375,-120.09476,-49.2252,-119.85752], "fy":[8.20978,-84.00709,-132.62939,44.94798]}, + {"t":1.57367, "x":4.23923, "y":1.88594, "heading":-3.06052, "vx":-3.9131, "vy":-2.3986, "omega":0.83704, "ax":-6.19657, "ay":-2.36383, "alpha":-17.05912, "fx":[-142.54059,-117.79733,-48.1777,-113.09209], "fy":[7.28404,-82.99209,-128.40256,43.27844]}, + {"t":1.58805, "x":4.18235, "y":1.85122, "heading":-3.04849, "vx":-4.00215, "vy":-2.43257, "omega":0.59187, "ax":-5.89088, "ay":-2.23934, "alpha":-16.43427, "fx":[-137.20072,-114.28493,-47.67115,-101.65175], "fy":[7.01726,-79.51823,-119.56241,39.70096]}, + {"t":1.60242, "x":4.12422, "y":1.81603, "heading":-3.03999, "vx":-4.08682, "vy":-2.46475, "omega":0.35568, "ax":-5.22743, "ay":-1.80447, "alpha":-14.52013, "fx":[-123.26264,-105.98794,-47.13462,-79.28286], "fy":[8.52107,-68.36258,-95.96123,33.02859]}, + {"t":1.61679, "x":4.06495, "y":1.78042, "heading":-3.03488, "vx":-4.16194, "vy":-2.49069, "omega":0.147, "ax":-2.94137, "ay":0.0298, "alpha":-6.85258, "fx":[-67.86192,-64.83258,-31.75879,-35.67411], "fy":[17.07785,-20.38194,-19.13817,24.46964]}, + {"t":1.63116, "x":4.00483, "y":1.74463, "heading":-3.03276, "vx":-4.20422, "vy":-2.49026, "omega":0.04852, "ax":-0.87353, "ay":0.98951, "alpha":-0.68968, "fx":[-16.86509,-16.55087,-12.84082,-13.177], "fy":[18.42018,14.7634,15.23685,18.90499]}, + {"t":1.64553, "x":3.94432, "y":1.70894, "heading":-3.03207, "vx":-4.21677, "vy":-2.47604, "omega":0.03861, "ax":-0.50675, "ay":0.83271, "alpha":-0.04376, "fx":[-8.74892,-8.72555,-8.49045,-8.51389], "fy":[14.26617,14.03416,14.06211,14.29415]}, + {"t":1.6599, "x":3.88366, "y":1.67344, "heading":-3.03151, "vx":-4.22405, "vy":-2.46407, "omega":0.03798, "ax":-0.37879, "ay":0.65121, "alpha":0.0026, "fx":[-6.43539,-6.43684,-6.45075,-6.4493], "fy":[11.07088,11.08468,11.08307,11.06927]}, + {"t":1.67428, "x":3.82292, "y":1.6381, "heading":-3.03096, "vx":-4.2295, "vy":-2.45471, "omega":0.03801, "ax":-0.30576, "ay":0.53033, "alpha":0.00531, "fx":[-5.18511,-5.18815,-5.21657,-5.21352], "fy":[9.00821,9.03647,9.03322,9.00496]}, + {"t":1.68865, "x":3.7621, "y":1.60287, "heading":-3.03042, "vx":-4.23389, "vy":-2.44709, "omega":0.03809, "ax":-0.26694, "ay":0.46514, "alpha":0.00516, "fx":[-4.52531,-4.5283,-4.55589,-4.55289], "fy":[7.8997,7.92717,7.92402,7.89655]}, + {"t":1.70302, "x":3.70122, "y":1.56775, "heading":-3.02987, "vx":-4.23773, "vy":-2.4404, "omega":0.03816, "ax":-0.25705, "ay":0.44956, "alpha":0.00505, "fx":[-4.35743,-4.36038,-4.38738,-4.38443], "fy":[7.6349,7.6618,7.6587,7.63181]}, + {"t":1.71739, "x":3.64029, "y":1.53273, "heading":-3.02932, "vx":-4.24142, "vy":-2.43394, "omega":0.03824, "ax":-0.27502, "ay":0.48261, "alpha":0.00522, "fx":[-4.66254,-4.66559,-4.69348,-4.69043], "fy":[8.19681,8.22458,8.22135,8.19359]}, + {"t":1.73176, "x":3.57931, "y":1.4978, "heading":-3.02877, "vx":-4.24538, "vy":-2.42701, "omega":0.03831, "ax":-0.32304, "ay":0.56897, "alpha":0.00573, "fx":[-5.47782,-5.48115,-5.5118,-5.50847], "fy":[9.66459,9.69505,9.69146,9.661]}, + {"t":1.74614, "x":3.51826, "y":1.46298, "heading":-3.02822, "vx":-4.25002, "vy":-2.41883, "omega":0.03839, "ax":-0.40649, "ay":0.71928, "alpha":0.0067, "fx":[-6.89447,-6.8983,-6.93422,-6.93039], "fy":[12.21909,12.25464,12.25033,12.21478]}, + {"t":1.76051, "x":3.45714, "y":1.42829, "heading":-3.02767, "vx":-4.25586, "vy":-2.40849, "omega":0.03849, "ax":-0.53378, "ay":0.95045, "alpha":0.00832, "fx":[-9.05483,-9.0594,-9.10415,-9.09959], "fy":[16.14765,16.1916,16.18601,16.14206]}, + {"t":1.77488, "x":3.39592, "y":1.39377, "heading":-3.02712, "vx":-4.26353, "vy":-2.39483, "omega":0.03861, "ax":-0.71535, "ay":1.28488, "alpha":0.01089, "fx":[-12.13563,-12.14109,-12.20015,-12.19469], "fy":[21.83082,21.88796,21.88006,21.82292]}, + {"t":1.78925, "x":3.33457, "y":1.35948, "heading":-3.02656, "vx":-4.27381, "vy":-2.37637, "omega":0.03877, "ax":-0.96042, "ay":1.74626, "alpha":0.01516, "fx":[-16.29171,-16.29797,-16.3813,-16.37505], "fy":[29.67042,29.74876,29.73634,29.65799]}, + {"t":1.80362, "x":3.27305, "y":1.32551, "heading":-3.02601, "vx":-4.28762, "vy":-2.35127, "omega":0.03898, "ax":-1.26892, "ay":2.34888, "alpha":0.02547, "fx":[-21.50882,-21.51545,-21.65893,-21.65237], "fy":[39.90209,40.03027,40.00531,39.87714]}, + {"t":1.81799, "x":3.21129, "y":1.29196, "heading":-3.02545, "vx":-4.30585, "vy":-2.31751, "omega":0.03935, "ax":-1.61062, "ay":3.08526, "alpha":0.08523, "fx":[-27.14517,-27.14776,-27.64686,-27.64525], "fy":[52.3266,52.73597,52.63204,52.22262]}, + {"t":1.83237, "x":3.14925, "y":1.25897, "heading":-3.02488, "vx":-4.329, "vy":-2.27317, "omega":0.04058, "ax":-1.82174, "ay":3.96211, "alpha":0.59806, "fx":[-29.16815,-29.0667,-32.77397,-32.94041], "fy":[66.51972,69.15102,68.2758,65.63091]}, + {"t":1.84674, "x":3.08684, "y":1.22671, "heading":-3.0243, "vx":-4.35518, "vy":-2.21623, "omega":0.04917, "ax":-0.99969, "ay":5.14069, "alpha":4.0496, "fx":[-1.19058,-4.15247,-30.51644,-32.15826], "fy":[82.62768,96.73594,92.65857,77.74472]}, + {"t":1.86111, "x":3.02415, "y":1.19539, "heading":-3.02359, "vx":-4.36955, "vy":-2.14235, "omega":0.10737, "ax":1.63013, "ay":5.74434, "alpha":11.30931, "fx":[85.62282,51.83565,-15.03498,-11.51141], "fy":[70.66216,114.29112,120.4364,85.44851]}, + {"t":1.87548, "x":2.96152, "y":1.1652, "heading":-3.02205, "vx":-4.34612, "vy":-2.05979, "omega":0.26991, "ax":3.04194, "ay":5.52549, "alpha":14.59245, "fx":[123.17932,76.85434,-7.67305,14.60968], "fy":[45.42653,114.89857,133.53712,82.08546]}, + {"t":1.88985, "x":2.89937, "y":1.13617, "heading":-3.01817, "vx":-4.3024, "vy":-1.98038, "omega":0.47963, "ax":3.74834, "ay":5.1948, "alpha":16.08673, "fx":[135.70596,86.68675,-6.30045,38.94039], "fy":[31.79492,114.30035,139.55255,67.80076]}, + {"t":1.90423, "x":2.83792, "y":1.10824, "heading":-3.01127, "vx":-4.24853, "vy":-1.90572, "omega":0.71082, "ax":4.28219, "ay":4.78348, "alpha":17.15725, "fx":[141.22497,91.47337,-6.05164,64.70846], "fy":[24.66563,113.9373,142.83626,44.0235]}, + {"t":1.9186, "x":2.77731, "y":1.08135, "heading":-3.00106, "vx":-4.18699, "vy":-1.83697, "omega":0.9574, "ax":4.72258, "ay":4.36189, "alpha":18.08998, "fx":[144.14495,94.1865,-5.4619,88.44955], "fy":[21.08452,113.75674,144.87175,17.06473]}, + {"t":1.93297, "x":2.71762, "y":1.0554, "heading":-2.9873, "vx":-4.11912, "vy":-1.77429, "omega":1.21739, "ax":5.02898, "ay":4.02793, "alpha":18.9037, "fx":[145.90336,95.82459,-4.66488,105.10277], "fy":[19.38112,113.73504,146.2408,-5.30088]}, + {"t":1.94734, "x":2.65894, "y":1.03031, "heading":-2.9698, "vx":-4.04684, "vy":-1.7164, "omega":1.48907, "ax":5.21683, "ay":3.78317, "alpha":19.64955, "fx":[147.06982,96.7693,-4.0942,115.20245], "fy":[18.6173,113.8912,147.20666,-22.31239]}, + {"t":1.96171, "x":2.60132, "y":1.00603, "heading":-2.9484, "vx":-3.97187, "vy":-1.66203, "omega":1.77147, "ax":5.32274, "ay":3.60012, "alpha":20.36848, "fx":[147.89133,97.20581,-3.92704,120.9828], "fy":[18.38697,114.23153,147.9096,-35.57973]}, + {"t":1.97608, "x":2.54478, "y":0.98252, "heading":-2.92294, "vx":-3.89537, "vy":-1.61029, "omega":2.0642, "ax":5.46615, "ay":3.6072, "alpha":20.24979, "fx":[148.332,97.53822,-1.7542,127.79455], "fy":[19.68672,114.48606,148.46277,-37.20551]}, + {"t":1.99046, "x":2.48936, "y":0.95975, "heading":-2.89328, "vx":-3.81681, "vy":-1.55844, "omega":2.35523, "ax":6.18706, "ay":4.76857, "alpha":13.17141, "fx":[145.69449,101.84012,33.6919,139.73409], "fy":[35.92834,110.9773,144.97388,32.56877]}, + {"t":2.00483, "x":2.43515, "y":0.93784, "heading":-2.85943, "vx":-3.72789, "vy":-1.48991, "omega":2.54452, "ax":6.74156, "ay":5.2133, "alpha":7.26274, "fx":[139.36938,108.5375,77.98416,132.7969], "fy":[56.60014,104.64178,127.59814,65.86657]}, + {"t":2.0192, "x":2.38227, "y":0.91697, "heading":-2.82286, "vx":-3.631, "vy":-1.41499, "omega":2.6489, "ax":7.06113, "ay":5.232, "alpha":3.32224, "fx":[132.07416,115.24472,105.39856,127.71405], "fy":[72.67175,97.41997,107.17814,78.70969]}, + {"t":2.03357, "x":2.33081, "y":0.89717, "heading":-2.78479, "vx":-3.52952, "vy":-1.33979, "omega":2.69665, "ax":7.22078, "ay":5.15881, "alpha":0.59241, "fx":[125.03826,121.63906,120.51942,124.09684], "fy":[84.68933,89.54655,90.88204,85.88171]}, + {"t":2.04794, "x":2.28083, "y":0.87845, "heading":-2.74603, "vx":-3.42575, "vy":-1.26565, "omega":2.70516, "ax":7.29632, "ay":5.07095, "alpha":-1.4112, "fx":[118.72937,127.50978,128.99383,121.20027], "fy":[93.6899,81.23921,79.27933,90.81338]}, + {"t":2.06232, "x":2.23235, "y":0.86079, "heading":-2.70716, "vx":-3.32089, "vy":-1.19277, "omega":2.68488, "ax":7.32855, "ay":4.98488, "alpha":-2.97921, "fx":[113.25745,132.70314,133.97597,118.68977], "fy":[100.5116,72.75432,71.26559,94.63424]}, + {"t":2.07669, "x":2.18538, "y":0.84416, "heading":-2.66857, "vx":-3.21556, "vy":-1.12113, "omega":2.64207, "ax":7.33664, "ay":4.9045, "alpha":-4.2689, "fx":[108.59052,137.13643,137.03949,116.40989], "fy":[105.7516,64.35642,65.77771,97.81056]}, + {"t":2.09106, "x":2.13993, "y":0.82855, "heading":-2.6306, "vx":-3.11012, "vy":-1.05064, "omega":2.58071, "ax":7.3299, "ay":4.83136, "alpha":-5.36302, "fx":[104.65056,140.79951,138.98453,114.28367], "fy":[109.82328,56.2818,62.04948,100.56534]}, + {"t":2.10543, "x":2.09599, "y":0.81395, "heading":-2.59351, "vx":-3.00478, "vy":-0.98121, "omega":2.50364, "ax":7.31353, "ay":4.76642, "alpha":-6.30556, "fx":[101.35152,143.74107,140.23977,112.27152], "fy":[113.01374,48.71302,59.55852,103.01678]}, + {"t":2.1198, "x":2.05356, "y":0.80034, "heading":-2.55753, "vx":-2.89967, "vy":-0.91271, "omega":2.41302, "ax":7.29085, "ay":4.71018, "alpha":-7.1214, "fx":[98.61242,146.04724,141.04857,110.35294], "fy":[115.52577,41.76868,57.94821,105.23268]}, + {"t":2.13417, "x":2.01264, "y":0.78771, "heading":-2.52285, "vx":-2.79488, "vy":-0.84501, "omega":2.31067, "ax":7.26432, "ay":4.66259, "alpha":-7.82665, "fx":[96.36057,147.82043,141.55708,108.51775], "fy":[117.50606,35.50657,56.97049,107.25446]}, + {"t":2.14855, "x":1.97322, "y":0.77605, "heading":-2.48964, "vx":-2.69048, "vy":-0.778, "omega":2.19818, "ax":7.23577, "ay":4.62319, "alpha":-8.43379, "fx":[94.53109,149.16326,141.85782,106.76148], "fy":[119.0637,29.93481,56.44883,109.10904]}, + {"t":2.16292, "x":1.9353, "y":0.76535, "heading":-2.45805, "vx":-2.58649, "vy":-0.71156, "omega":2.07698, "ax":7.20661, "ay":4.59117, "alpha":-8.95391, "fx":[93.06545,150.1686,142.01228,105.08285], "fy":[120.28231,25.02581,56.25469,110.81486]}, + {"t":2.17729, "x":1.89887, "y":0.75559, "heading":-2.4282, "vx":-2.48292, "vy":-0.64558, "omega":1.94829, "ax":7.17782, "ay":4.56556, "alpha":-9.3976, "fx":[91.90996,150.91507,142.0632,103.48235], "fy":[121.2281,20.72936,56.29255,112.38536]}, + {"t":2.19166, "x":1.86393, "y":0.74679, "heading":-2.4002, "vx":-2.37976, "vy":-0.57996, "omega":1.81323, "ax":7.15008, "ay":4.54534, "alpha":-9.77512, "fx":[91.01448,151.46619,142.04154,101.96132], "fy":[121.95533,16.98326,56.49011,113.83077]}, + {"t":2.20603, "x":1.83047, "y":0.73892, "heading":-2.37414, "vx":-2.277, "vy":-0.51464, "omega":1.67274, "ax":7.1238, "ay":4.5295, "alpha":-10.0963, "fx":[90.33157,151.87154,141.97057,100.52155], "fy":[122.50994,13.72093,56.7918,115.15934]}, + {"t":2.22041, "x":1.79848, "y":0.73199, "heading":-2.3501, "vx":-2.17462, "vy":-0.44954, "omega":1.52764, "ax":7.09915, "ay":4.51713, "alpha":-10.37043, "fx":[89.8158,152.16889,141.86833,99.16493], "fy":[122.93207,10.87638,57.15431,116.37788]}, + {"t":2.23478, "x":1.76796, "y":0.726, "heading":-2.32814, "vx":-2.07259, "vy":-0.38462, "omega":1.3786, "ax":7.07614, "ay":4.50744, "alpha":-10.60616, "fx":[89.42334,152.38648,141.74922,97.89331], "fy":[123.25789,8.38712,57.54344,117.4923]}, + {"t":2.24915, "x":1.7389, "y":0.72094, "heading":-2.30833, "vx":-1.97089, "vy":-0.31984, "omega":1.22617, "ax":7.05465, "ay":4.49972, "alpha":-10.8114, "fx":[89.11203,152.54511,141.62494,96.70845], "fy":[123.5205,6.19593,57.9319,118.50767]}, + {"t":2.26352, "x":1.7113, "y":0.7168, "heading":-2.29071, "vx":-1.86951, "vy":-0.25517, "omega":1.07079, "ax":7.03449, "ay":4.49343, "alpha":-10.99328, "fx":[88.84134,152.65989,141.50524,95.61207], "fy":[123.75061,4.25146,58.29768,119.42839]}, + {"t":2.27789, "x":1.68516, "y":0.7136, "heading":-2.27532, "vx":-1.76841, "vy":-0.19059, "omega":0.9128, "ax":7.01538, "ay":4.48812, "alpha":-11.15818, "fx":[88.57249,152.74172,141.39827,94.60586], "fy":[123.97691,2.50831,58.62283,120.25829]}, + {"t":2.29226, "x":1.66047, "y":0.71133, "heading":-2.2622, "vx":-1.66758, "vy":-0.12609, "omega":0.75243, "ax":6.99702, "ay":4.48341, "alpha":-11.3118, "fx":[88.26856,152.79831,141.31097,93.69149], "fy":[124.22606,0.92669,58.89248,121.00063]}, + {"t":2.30664, "x":1.63723, "y":0.70998, "heading":-2.25138, "vx":-1.56702, "vy":-0.06165, "omega":0.58986, "ax":6.9791, "ay":4.47901, "alpha":-11.45918, "fx":[87.89475,152.83507,141.24925,92.87068], "fy":[124.52262,-0.52803,59.0942,121.65812]}, + {"t":2.32101, "x":1.61543, "y":0.70955, "heading":-2.24291, "vx":-1.46672, "vy":0.00272, "omega":0.42517, "ax":6.96128, "ay":4.4747, "alpha":-11.60482, "fx":[87.41849,152.85568,141.21819,92.14524], "fy":[124.88871,-1.88584,59.21737,122.23293]}, + {"t":2.33538, "x":1.59507, "y":0.71005, "heading":-2.2368, "vx":-1.36668, "vy":0.06703, "omega":0.25839, "ax":6.94326, "ay":4.47025, "alpha":-11.75269, "fx":[86.80982,152.86255,141.22213,91.51714], "fy":[125.34361,-3.17261,59.25277,122.72669]}, + {"t":2.34975, "x":1.57614, "y":0.71148, "heading":-2.23308, "vx":-1.26689, "vy":0.13127, "omega":0.08948, "ax":6.92475, "ay":4.46547, "alpha":-11.90637, "fx":[86.04161,152.85717,141.26476,90.98853], "fy":[125.90318,-4.41055,59.19224,123.14048]}, + {"t":2.36412, "x":1.55865, "y":0.71383, "heading":-2.2318, "vx":-1.16737, "vy":0.19545, "omega":-0.08163, "ax":6.90549, "ay":4.46016, "alpha":-12.06903, "fx":[85.09004,152.84027,141.3492,90.5618], "fy":[126.57938,-5.61863,59.02836,123.47479]}, + {"t":2.3785, "x":1.54259, "y":0.7171, "heading":-2.23297, "vx":-1.06812, "vy":0.25955, "omega":-0.25509, "ax":6.88525, "ay":4.45408, "alpha":-12.24348, "fx":[83.9351,152.81209,141.47801,90.23961], "fy":[127.37963,-6.81291,58.75426,123.72951]}, + {"t":2.39287, "x":1.52795, "y":0.72129, "heading":-2.23664, "vx":-0.96917, "vy":0.32356, "omega":-0.43105, "ax":6.8639, "ay":4.44697, "alpha":-12.43221, "fx":[82.56123,152.77242,141.65325,90.02494], "fy":[128.30628,-8.00684,58.36341,123.9039]}, + {"t":2.40724, "x":1.51473, "y":0.7264, "heading":-2.24283, "vx":-0.87052, "vy":0.38747, "omega":-0.60972, "ax":6.84133, "ay":4.43851, "alpha":-12.6373, "fx":[80.95822,152.72077,141.87646,89.92109], "fy":[129.35621,-9.21137,57.84947,123.99656]}, + {"t":2.42161, "x":1.50292, "y":0.73242, "heading":-2.25159, "vx":-0.7722, "vy":0.45126, "omega":-0.79134, "ax":6.81756, "ay":4.42831, "alpha":-12.86035, "fx":[79.12213,152.65641,142.14871,89.93172], "fy":[130.52052,-10.43514,57.20618,124.00538]}, + {"t":2.43598, "x":1.49253, "y":0.73937, "heading":-2.26297, "vx":-0.67422, "vy":0.51491, "omega":-0.97617, "ax":6.79268, "ay":4.41593, "alpha":-13.1024, "fx":[77.0564,152.57849,142.47051,90.06084], "fy":[131.78454,-11.68442,56.42726,123.92748]}, + {"t":2.45035, "x":1.48354, "y":0.74722, "heading":-2.277, "vx":-0.5766, "vy":0.57837, "omega":-1.16447, "ax":6.87212, "ay":4.41173, "alpha":-12.59023, "fx":[80.36534,152.72861,142.49539,91.98156], "fy":[129.69682,-8.21363,56.23946,122.4467]}, + {"t":2.46302, "x":1.47679, "y":0.7549, "heading":-2.29175, "vx":-0.48955, "vy":0.63426, "omega":-1.32396, "ax":6.89489, "ay":4.40118, "alpha":-12.49804, "fx":[80.64271,152.75097,142.72703,92.99975], "fy":[129.51519,-7.34245,55.61704,121.6617]}, + {"t":2.47569, "x":1.47114, "y":0.76329, "heading":-2.30852, "vx":-0.40221, "vy":0.69001, "omega":-1.48227, "ax":6.91835, "ay":4.38968, "alpha":-12.40502, "fx":[80.86445,152.76888,142.98685,94.09654], "fy":[129.36811,-6.41186,54.91102,120.80126]}, + {"t":2.48836, "x":1.4666, "y":0.77238, "heading":-2.32729, "vx":-0.31457, "vy":0.74561, "omega":-1.63941, "ax":6.9428, "ay":4.37732, "alpha":-12.30779, "fx":[81.05724,152.78182,143.27133,95.2699], "fy":[129.23909,-5.40138,54.12675,119.86312]}, + {"t":2.50102, "x":1.46317, "y":0.78218, "heading":-2.34806, "vx":-0.22662, "vy":0.80106, "omega":-1.79532, "ax":6.96853, "ay":4.36424, "alpha":-12.20273, "fx":[81.24802,152.78866,143.57671,96.51774], "fy":[129.11122,-4.28819,53.27028,118.84476]}, + {"t":2.51369, "x":1.46086, "y":0.79268, "heading":-2.3708, "vx":-0.13835, "vy":0.85635, "omega":-1.9499, "ax":6.99582, "ay":4.35064, "alpha":-12.08593, "fx":[81.46366,152.78747,143.8989,97.83784], "fy":[128.96736,-3.047,52.34858,117.74349]}, + {"t":2.52636, "x":1.45967, "y":0.80387, "heading":-2.3955, "vx":-0.04973, "vy":0.91146, "omega":-2.10299, "ax":7.02492, "ay":4.33673, "alpha":-11.95324, "fx":[81.73069,152.77525,144.23352,99.22774], "fy":[128.79033,-1.65002,51.3697,116.55643]}, + {"t":2.53903, "x":1.4596, "y":0.81577, "heading":-2.42214, "vx":0.03926, "vy":0.96639, "omega":-2.25441, "ax":7.05601, "ay":4.32282, "alpha":-11.80032, "fx":[82.075,152.74752,144.57588,100.6847], "fy":[128.56293,-0.06709,50.34307,115.28065]}, + {"t":2.55169, "x":1.46067, "y":0.82835, "heading":-2.4507, "vx":0.12864, "vy":1.02115, "omega":-2.40389, "ax":7.08927, "ay":4.30923, "alpha":-11.62271, "fx":[82.52144,152.69794,144.92099,102.20559], "fy":[128.26805,1.7339,49.27972,113.91324]}, + {"t":2.56436, "x":1.46286, "y":0.84164, "heading":-2.48115, "vx":0.21844, "vy":1.07574, "omega":-2.55112, "ax":7.12478, "ay":4.29635, "alpha":-11.41596, "fx":[83.09372,152.61778,145.26347,103.78679], "fy":[127.88851,3.78643,48.19263,112.45135]}, + {"t":2.57703, "x":1.4662, "y":0.85561, "heading":-2.51347, "vx":0.30869, "vy":1.13016, "omega":-2.69573, "ax":7.16254, "ay":4.28462, "alpha":-11.17576, "fx":[83.81414,152.49534,145.59762,105.42416], "fy":[127.40689,6.12432,47.09713,110.89236]}, + {"t":2.5897, "x":1.47069, "y":0.87027, "heading":-2.54761, "vx":0.39942, "vy":1.18444, "omega":-2.83729, "ax":7.20249, "ay":4.27448, "alpha":-10.89813, "fx":[84.70347,152.31543,145.91728,107.11298], "fy":[126.80533,8.78007,46.01136,109.2339]}, + {"t":2.60236, "x":1.47633, "y":0.88561, "heading":-2.58355, "vx":0.49066, "vy":1.23858, "omega":-2.97534, "ax":7.24444, "ay":4.26636, "alpha":-10.5797, "fx":[85.7808,152.05899,146.21585,108.84782], "fy":[126.06516,11.78247,44.95675,107.47403]}, + {"t":2.61503, "x":1.48312, "y":0.90165, "heading":-2.62124, "vx":0.58243, "vy":1.29263, "omega":-3.10936, "ax":7.28812, "ay":4.26066, "alpha":-10.21798, "fx":[87.06352,151.70299,146.48611,110.62257], "fy":[125.16661,15.15346,43.95865,105.6114]}, + {"t":2.6277, "x":1.49108, "y":0.91836, "heading":-2.66063, "vx":0.67475, "vy":1.3466, "omega":-3.2388, "ax":7.33314, "ay":4.25764, "alpha":-9.81164, "fx":[88.56729,151.22102,146.72012,112.43033], "fy":[124.08821,18.90403,43.04707,103.64531]}, + {"t":2.64036, "x":1.50022, "y":0.93576, "heading":-2.70166, "vx":0.76764, "vy":1.40053, "omega":-3.36308, "ax":7.37907, "ay":4.25741, "alpha":-9.36073, "fx":[90.30623,150.5847,146.90888,114.26343], "fy":[122.80621,23.0295,42.25755,101.57593]}, + {"t":2.65303, "x":1.51054, "y":0.95384, "heading":-2.74426, "vx":0.86111, "vy":1.45446, "omega":-3.48166, "ax":7.42539, "ay":4.25985, "alpha":-8.86682, "fx":[92.29301,149.76645,147.04198,116.11339], "fy":[121.29378,27.50458,41.6322,99.40444]}, + {"t":2.6657, "x":1.52204, "y":0.97261, "heading":-2.78836, "vx":0.95517, "vy":1.50842, "omega":-3.59398, "ax":7.47162, "ay":4.26452, "alpha":-8.3328, "fx":[94.53923,148.74358,147.10689,117.97094], "fy":[119.51995,32.27888,41.221,97.13317]}, + {"t":2.67837, "x":1.53474, "y":0.99206, "heading":-2.83389, "vx":1.04982, "vy":1.56244, "omega":-3.69953, "ax":7.51737, "ay":4.27067, "alpha":-7.76239, "fx":[97.05571,147.50369,147.08802,119.82601], "fy":[117.4482,37.27404,41.08334,94.76581]}, + {"t":2.69103, "x":1.54864, "y":1.01219, "heading":-2.88075, "vx":1.14504, "vy":1.61654, "omega":-3.79786, "ax":7.56239, "ay":4.2772, "alpha":-7.15919, "fx":[99.85292,146.05074,146.96518,121.66773], "fy":[115.03461,42.38367,41.28996,92.3076]}, + {"t":2.7037, "x":1.56375, "y":1.03301, "heading":-2.92886, "vx":1.24084, "vy":1.67072, "omega":-3.88855, "ax":7.60665, "ay":4.28274, "alpha":-6.52516, "fx":[102.94135,144.41045,146.71135,123.4843], "fy":[112.22526,47.4768,41.9251,89.76562]}, + {"t":2.71637, "x":1.58008, "y":1.05452, "heading":-2.97812, "vx":1.33719, "vy":1.72497, "omega":-3.9712, "ax":7.6503, "ay":4.28573, "alpha":-5.85889, "fx":[106.33176,142.63374,146.2893,125.2629], "fy":[108.95275,52.40521,43.08906,87.14921]}, + {"t":2.72904, "x":1.59763, "y":1.07672, "heading":-3.02842, "vx":1.4341, "vy":1.77926, "omega":-4.04542, "ax":7.69366, "ay":4.28455, "alpha":-5.1536, "fx":[110.03506,140.7966,145.64664,126.98932], "fy":[105.13127,57.01329,44.90063,84.47055]}, + {"t":2.7417, "x":1.61642, "y":1.0996, "heading":-3.07967, "vx":1.53156, "vy":1.83353, "omega":-4.1107, "ax":7.73695, "ay":4.2776, "alpha":-4.39536, "fx":[114.06139,138.99593,144.70864,128.64735], "fy":[100.64978,61.14878,47.49891,81.74567]}, + {"t":2.75437, "x":1.63644, "y":1.12317, "heading":-3.13174, "vx":1.62957, "vy":1.88772, "omega":-4.16638, "ax":7.78005, "ay":4.26335, "alpha":-3.56175, "fx":[118.41778,137.34167,143.36828,130.21775], "fy":[95.36242,64.67188,51.0429,78.99607]}, + {"t":2.76704, "x":1.6577, "y":1.14742, "heading":3.09867, "vx":1.72812, "vy":1.94172, "omega":-4.2115, "ax":7.82198, "ay":4.24013, "alpha":-2.62128, "fx":[123.10277,135.94667,141.47303,131.67634], "fy":[89.07518,67.46085,55.70616,76.25146]}, + {"t":2.77971, "x":1.68022, "y":1.17236, "heading":3.04532, "vx":1.8272, "vy":1.99543, "omega":-4.2447, "ax":7.86039, "ay":4.20579, "alpha":-1.53394, "fx":[128.09543,134.9162,138.80969,132.99054], "fy":[81.52762,69.41343,61.66137,73.55455]}, + {"t":2.79237, "x":1.704, "y":1.19797, "heading":2.99155, "vx":1.92678, "vy":2.04871, "omega":-4.26413, "ax":7.89073, "ay":4.15691, "alpha":-0.25339, "fx":[133.33358,134.33849,135.09171,134.11271], "fy":[72.3696,70.44458,69.04715,70.97004]}, + {"t":2.80504, "x":1.72904, "y":1.22426, "heading":2.93754, "vx":2.02673, "vy":2.10137, "omega":-4.26734, "ax":7.90544, "ay":4.0878, "alpha":1.26809, "fx":[138.67188,134.27707,129.96175,134.96613], "fy":[61.13631,70.48222,77.90782,68.60276]}, + {"t":2.81771, "x":1.75535, "y":1.2512, "heading":2.88348, "vx":2.12687, "vy":2.15315, "omega":-4.25128, "ax":7.89279, "ay":3.9895, "alpha":3.07057, "fx":[143.80468,134.76449,123.03486,135.41259], "fy":[47.23791,69.46329,88.10167,66.63818]}, + {"t":2.83037, "x":1.78292, "y":1.2788, "heading":2.82963, "vx":2.22685, "vy":2.20369, "omega":-4.21239, "ax":7.83557, "ay":3.85057, "alpha":5.1725, "fx":[148.13854,135.79617,114.02217,135.16622], "fy":[30.01674,67.33402,99.19213,65.44546]}, + {"t":2.84304, "x":1.81176, "y":1.30702, "heading":2.77627, "vx":2.32611, "vy":2.25246, "omega":-4.14686, "ax":7.70833, "ay":3.66457, "alpha":7.55332, "fx":[150.6402,137.32099,102.98319,133.52191], "fy":[9.02193,64.06263,110.36628,65.88251]}, + {"t":2.85571, "x":1.84184, "y":1.33585, "heading":2.72374, "vx":2.42375, "vy":2.29888, "omega":-4.05118, "ax":7.46969, "ay":3.45876, "alpha":10.14547, "fx":[149.90535,139.21838,90.8046,128.30049], "fy":[-15.10611,59.69526,120.40109,70.34012]}, + {"t":2.86838, "x":1.87314, "y":1.36525, "heading":2.67242, "vx":2.51837, "vy":2.3427, "omega":-3.92267, "ax":7.04721, "ay":3.36748, "alpha":12.87106, "fx":[145.5609,141.20784,80.55096,112.16412], "fy":[-37.9517,54.62052,127.34818,85.10249]}, + {"t":2.88104, "x":1.90561, "y":1.39519, "heading":2.62273, "vx":2.60764, "vy":2.38535, "omega":-3.75962, "ax":6.69032, "ay":3.42447, "alpha":14.64387, "fx":[142.31253,142.67906,79.2648,90.94546], "fy":[-47.86657,50.25136,127.98882,102.62314]}, + {"t":2.89371, "x":1.93918, "y":1.42569, "heading":2.57511, "vx":2.69239, "vy":2.42873, "omega":-3.57413, "ax":6.62573, "ay":3.39813, "alpha":15.13013, "fx":[141.10098,143.74096,82.7001,83.26532], "fy":[-50.30708,46.53465,125.57623,109.40104]}, + {"t":2.90638, "x":1.97382, "y":1.45672, "heading":2.52983, "vx":2.77632, "vy":2.47178, "omega":-3.38247, "ax":6.61687, "ay":3.31935, "alpha":15.42225, "fx":[139.91744,144.67187,86.27962,79.33549], "fy":[-52.38437,42.7956,122.86814,112.56517]}, + {"t":2.91905, "x":2.00952, "y":1.4883, "heading":2.48698, "vx":2.86014, "vy":2.51382, "omega":-3.18711, "ax":6.60893, "ay":3.20687, "alpha":15.73922, "fx":[138.37107,145.51156,89.52332,76.25827], "fy":[-55.113,38.87112,120.18015,114.25347]}, + {"t":2.93171, "x":2.04628, "y":1.5204, "heading":2.44661, "vx":2.94386, "vy":2.55445, "omega":-2.98773, "ax":6.59514, "ay":3.06366, "alpha":16.09468, "fx":[136.35532,146.25331,92.49337,73.62357], "fy":[-58.58648,34.68835,117.47429,114.87199]}, + {"t":2.94438, "x":2.0841, "y":1.55301, "heading":2.40877, "vx":3.0274, "vy":2.59326, "omega":-2.78386, "ax":6.57462, "ay":2.88759, "alpha":16.47613, "fx":[133.77584,146.87582,95.28765,71.39037], "fy":[-62.79401,30.17582,114.66168,114.42464]}, + {"t":2.95705, "x":2.12297, "y":1.58609, "heading":2.3735, "vx":3.11068, "vy":2.62983, "omega":-2.57515, "ax":6.54771, "ay":2.67254, "alpha":16.86158, "fx":[130.51903,147.34302,97.9996,69.63713], "fy":[-67.70007,25.24586,111.62282,112.66822]}, + {"t":2.96972, "x":2.1629, "y":1.61961, "heading":2.34088, "vx":3.19362, "vy":2.66369, "omega":-2.36156, "ax":6.51505, "ay":2.407, "alpha":17.21874, "fx":[126.44286,147.59653,100.71757,68.51934], "fy":[-73.25016,19.78172,108.19377,109.04411]}, + {"t":2.98238, "x":2.20388, "y":1.65355, "heading":2.31097, "vx":3.27615, "vy":2.69418, "omega":-2.14344, "ax":6.47642, "ay":2.06961, "alpha":17.50177, "fx":[121.36491,147.53953,103.52186,68.22185], "fy":[-79.36639,13.62132,104.13784,102.42097]}, + {"t":2.99505, "x":2.2459, "y":1.68784, "heading":2.28381, "vx":3.35819, "vy":2.72039, "omega":-1.92174, "ax":6.42763, "ay":1.61886, "alpha":17.6466, "fx":[115.05363,147.00225,106.46936,68.80355], "fy":[-85.91412,6.54485,99.09182,90.42279]}, + {"t":3.00772, "x":2.28895, "y":1.72243, "heading":2.25947, "vx":3.43961, "vy":2.7409, "omega":-1.69821, "ax":6.36519, "ay":0.96843, "alpha":17.49909, "fx":[107.42445,145.66596,109.63985,70.34984], "fy":[-92.38077,-1.66758,92.33547,67.60388]}, + {"t":3.02038, "x":2.33304, "y":1.75723, "heading":2.23796, "vx":3.52024, "vy":2.75317, "omega":-1.47654, "ax":6.34107, "ay":-0.03422, "alpha":16.45778, "fx":[99.59228,142.89001,113.69445,75.26258], "fy":[-96.75576,-11.17293,81.40586,24.19477]}, + {"t":3.03305, "x":2.37814, "y":1.7921, "heading":2.21926, "vx":3.60057, "vy":2.75274, "omega":-1.26806, "ax":6.29739, "ay":-1.38134, "alpha":14.08336, "fx":[92.59987,136.93782,119.73283,79.19636], "fy":[-97.17919,-24.06024,58.15757,-30.90279]}, + {"t":3.04572, "x":2.42425, "y":1.82686, "heading":2.20319, "vx":3.68034, "vy":2.73524, "omega":-1.08966, "ax":5.78737, "ay":-3.24271, "alpha":9.25614, "fx":[82.05712,119.88733,118.9556,72.86609], "fy":[-95.94553,-49.71488,-3.01261,-71.9572]}, + {"t":3.05839, "x":2.47134, "y":1.86125, "heading":2.18939, "vx":3.75365, "vy":2.69416, "omega":-0.97241, "ax":3.21208, "ay":-5.82367, "alpha":-2.44152, "fx":[61.3772,43.0178,48.99544,65.1561], "fy":[-91.5758,-102.42237,-105.98786,-96.25011]}, + {"t":3.07105, "x":2.51914, "y":1.89491, "heading":2.17707, "vx":3.79434, "vy":2.62039, "omega":-1.00334, "ax":-0.57544, "ay":-6.04735, "alpha":-12.9736, "fx":[3.74392,-81.49996,-17.65417,56.25809], "fy":[-75.67534,-92.92754,-130.7819,-112.07028]}, + {"t":3.08372, "x":2.56716, "y":1.92762, "heading":2.16436, "vx":3.78705, "vy":2.54379, "omega":-1.16768, "ax":-2.98569, "ay":-5.12898, "alpha":-17.57048, "fx":[-83.71982,-117.35635,-46.01373,43.94725], "fy":[-24.43746,-69.08931,-131.61627,-123.82677]}, + {"t":3.09639, "x":2.61489, "y":1.95943, "heading":2.14957, "vx":3.74923, "vy":2.47882, "omega":-1.39025, "ax":-4.09936, "ay":-4.67464, "alpha":-18.83219, "fx":[-115.6106,-129.25569,-61.95562,27.90642], "fy":[1.53645,-57.95576,-129.09144,-132.54638]}, + {"t":3.10906, "x":2.66206, "y":1.99046, "heading":2.13196, "vx":3.6973, "vy":2.4196, "omega":-1.62881, "ax":-4.74634, "ay":-4.44813, "alpha":-18.8228, "fx":[-127.49975,-135.35193,-72.78341,12.69925], "fy":[11.85089,-50.79065,-125.88947,-137.81665]}, + {"t":3.12172, "x":2.70851, "y":2.02075, "heading":2.11133, "vx":3.63718, "vy":2.36325, "omega":-1.86724, "ax":-5.20138, "ay":-4.27809, "alpha":-18.49726, "fx":[-133.36243,-139.1906,-80.86326,-0.47942], "fy":[17.45587,-45.24708,-122.62314,-140.66172]}, + {"t":3.13439, "x":2.75417, "y":2.05034, "heading":2.08767, "vx":3.57129, "vy":2.30906, "omega":-2.10155, "ax":-5.5524, "ay":-4.13802, "alpha":-18.04762, "fx":[-136.8183,-141.84166,-87.27497,-11.84426], "fy":[20.56524,-40.69932,-119.4387,-141.97303]}, + {"t":3.14706, "x":2.79896, "y":2.07926, "heading":2.06105, "vx":3.50095, "vy":2.25664, "omega":-2.33017, "ax":-5.83802, "ay":-4.01847, "alpha":-17.54311, "fx":[-139.14443,-143.78698,-92.60203,-21.67851], "fy":[22.05019,-36.81528,-116.35088,-142.29623]}, + {"t":3.15973, "x":2.84284, "y":2.10752, "heading":2.03154, "vx":3.427, "vy":2.20574, "omega":-2.55239, "ax":-6.07848, "ay":-3.91481, "alpha":-17.01294, "fx":[-140.88131,-145.27848,-97.18474,-30.22857], "fy":[22.34772,-33.39058,-113.34142,-141.97459]}, + {"t":3.17239, "x":2.88576, "y":2.13515, "heading":1.9992, "vx":3.35, "vy":2.15615, "omega":-2.7679, "ax":-6.30808, "ay":-3.8484, "alpha":-16.26145, "fx":[-142.57543,-146.37241,-101.38167,-38.86521], "fy":[20.03794,-30.69667,-110.24597,-140.93576]}, + {"t":3.18506, "x":2.92769, "y":2.16215, "heading":1.96414, "vx":3.2701, "vy":2.1074, "omega":-2.97389, "ax":-6.69597, "ay":-4.00855, "alpha":-13.55349, "fx":[-145.48007,-146.33759,-106.56069,-57.20791], "fy":[1.26236,-32.54325,-105.75137,-135.70488]}, + {"t":3.19773, "x":2.96858, "y":2.18853, "heading":1.92647, "vx":3.18528, "vy":2.05662, "omega":-3.14558, "ax":-7.17657, "ay":-4.40013, "alpha":-8.24452, "fx":[-142.88564,-143.52527,-113.74755,-88.12703], "fy":[-37.27286,-44.13361,-98.34006,-119.63313]}, + {"t":3.21039, "x":3.00835, "y":2.21423, "heading":1.88662, "vx":3.09437, "vy":2.00089, "omega":-3.25001, "ax":-7.40838, "ay":-4.59296, "alpha":-4.33079, "fx":[-136.10432,-138.8137,-120.07089,-109.06856], "fy":[-61.32733,-57.86835,-90.82608,-102.47807]}, + {"t":3.22306, "x":3.04696, "y":2.2392, "heading":1.84546, "vx":3.00052, "vy":1.94271, "omega":-3.30487, "ax":-7.49946, "ay":-4.66892, "alpha":-1.64117, "fx":[-130.80823,-133.00382,-124.95976,-121.4826], "fy":[-74.00529,-70.72045,-84.2668,-88.67568]}, + {"t":3.23573, "x":3.08436, "y":2.26344, "heading":1.80359, "vx":2.90553, "vy":1.88356, "omega":-3.32566, "ax":-7.52478, "ay":-4.6978, "alpha":0.35678, "fx":[-127.4188,-126.69797,-128.60072,-129.25977], "fy":[-80.92124,-81.93267,-78.87122,-77.90781]}, + {"t":3.2484, "x":3.12056, "y":2.28692, "heading":1.76146, "vx":2.81021, "vy":1.82405, "omega":-3.32114, "ax":-7.51828, "ay":-4.7044, "alpha":1.92088, "fx":[-125.41712,-120.3872,-131.24206,-134.48849], "fy":[-84.78865,-91.34476,-74.64334,-69.30534]}, + {"t":3.26106, "x":3.15556, "y":2.30965, "heading":1.71939, "vx":2.71497, "vy":1.76446, "omega":-3.29681, "ax":-7.49697, "ay":-4.69859, "alpha":3.17504, "fx":[-124.35377,-114.42196,-133.09112,-138.21864], "fy":[-86.90215,-99.05388,-71.52086,-62.20947]}, + {"t":3.27373, "x":3.18935, "y":2.33162, "heading":1.67763, "vx":2.62, "vy":1.70494, "omega":-3.25659, "ax":-7.46991, "ay":-4.68564, "alpha":4.18902, "fx":[-123.92357,-109.01237,-134.30393,-141.00459], "fy":[-87.93108,-105.26995,-69.42009,-56.18473]}, + {"t":3.2864, "x":3.22194, "y":2.35284, "heading":1.63638, "vx":2.52538, "vy":1.64559, "omega":-3.20353, "ax":-7.4418, "ay":-4.66918, "alpha":5.01012, "fx":[-123.92486,-104.25514,-134.99274,-143.15849], "fy":[-88.25107,-110.23421,-68.25276,-50.94746]}, + {"t":3.29907, "x":3.25333, "y":2.37331, "heading":1.5958, "vx":2.43111, "vy":1.58644, "omega":-3.14006, "ax":-7.41479, "ay":-4.65185, "alpha":5.67508, "fx":[-124.22239,-100.1684,-135.23596,-144.86717], "fy":[-88.08837,-114.17623,-67.93174,-46.30998]}, + {"t":3.31173, "x":3.28353, "y":2.39304, "heading":1.55602, "vx":2.33719, "vy":1.52752, "omega":-3.06817, "ax":-7.38962, "ay":-4.63559, "alpha":6.21456, "fx":[-124.72328,-96.72239,-135.08724,-146.24867], "fy":[-87.58757,-117.29553,-68.37217,-42.14512]}, + {"t":3.3244, "x":3.31254, "y":2.41202, "heading":1.51716, "vx":2.24358, "vy":1.4688, "omega":-2.98945, "ax":-7.36621, "ay":-4.62174, "alpha":6.65484, "fx":[-125.36224,-93.86222,-134.58286,-147.38125], "fy":[-86.84572,-119.75711,-69.49061,-38.36448]}, + {"t":3.33707, "x":3.34037, "y":2.43025, "heading":1.47929, "vx":2.15027, "vy":1.41025, "omega":-2.90515, "ax":-7.34407, "ay":-4.61109, "alpha":7.0186, "fx":[-126.09246,-91.52266,-133.74783,-148.31903], "fy":[-85.93079,-121.69372,-71.20378,-34.90517]}, + {"t":3.34974, "x":3.36702, "y":2.44774, "heading":1.44249, "vx":2.05724, "vy":1.35184, "omega":-2.81625, "ax":-7.32256, "ay":-4.60401, "alpha":7.32527, "fx":[-126.87981,-89.63699,-132.601,-149.10086], "fy":[-84.89204,-123.21064,-73.42746,-31.7214]}, + {"t":3.3624, "x":3.39249, "y":2.4645, "heading":1.40681, "vx":1.96448, "vy":1.29352, "omega":-2.72345, "ax":-7.30106, "ay":-4.6005, "alpha":7.59127, "fx":[-127.69904,-88.14186,-131.15917,-149.75554], "fy":[-83.76637,-124.39087,-76.07598,-28.77922]}, + {"t":3.37507, "x":3.41679, "y":2.48052, "heading":1.37232, "vx":1.872, "vy":1.23524, "omega":-2.62729, "ax":-7.27902, "ay":-4.60028, "alpha":7.83017, "fx":[-128.53127,-86.97948,-129.44046,-150.30509], "fy":[-82.58216,-125.29988,-79.06249,-26.05293]}, + {"t":3.38774, "x":3.43992, "y":2.49579, "heading":1.33903, "vx":1.77979, "vy":1.17697, "omega":-2.52811, "ax":-7.25606, "ay":-4.60287, "alpha":8.05282, "fx":[-129.36225,-86.09841,-127.46686,-150.7668], "fy":[-81.36184,-125.98954,-82.30002,-23.52279]}, + {"t":3.4004, "x":3.46189, "y":2.51033, "heading":1.30701, "vx":1.68788, "vy":1.11866, "omega":-2.4261, "ax":-7.23197, "ay":-4.60768, "alpha":8.26746, "fx":[-130.18116,-85.45348,-125.26572,-151.15451], "fy":[-80.12353,-126.50117,-85.70329,-21.17336]}, + {"t":3.41307, "x":3.48269, "y":2.52413, "heading":1.27628, "vx":1.59627, "vy":1.0603, "omega":-2.32137, "ax":-7.20669, "ay":-4.61403, "alpha":8.47991, "fx":[-130.97971,-85.00535,-122.87041,-151.47958], "fy":[-78.88224,-126.8681,-89.19091,-18.99233]}, + {"t":3.42574, "x":3.50233, "y":2.5372, "heading":1.24687, "vx":1.50498, "vy":1.00185, "omega":-2.21395, "ax":-7.18035, "ay":-4.62127, "alpha":8.69376, "fx":[-131.75156,-84.71985,-120.31988,-151.75144], "fy":[-77.6506,-127.11743,-92.68783,-16.96973]}, + {"t":3.43841, "x":3.52082, "y":2.54952, "heading":1.21883, "vx":1.41402, "vy":0.94331, "omega":-2.10383, "ax":-7.15319, "ay":-4.62876, "alpha":8.91068, "fx":[-132.49182,-84.56731,-117.65746,-151.9781], "fy":[-76.4395,-127.27153,-96.12747,-15.09721]}, + {"t":3.45107, "x":3.53816, "y":2.56109, "heading":1.19218, "vx":1.32341, "vy":0.88468, "omega":-1.99095, "ax":-7.12554, "ay":-4.636, "alpha":9.13079, "fx":[-133.19677,-84.52158,-114.92882,-152.16642], "fy":[-75.2584,-127.34929,-99.45365,-13.36706]}, + {"t":3.46374, "x":3.55435, "y":2.57193, "heading":1.16696, "vx":1.23315, "vy":0.82595, "omega":-1.87529, "ax":-7.0978, "ay":-4.64258, "alpha":9.35298, "fx":[-133.86361,-84.56,-112.17996,-152.3223], "fy":[-74.11561,-127.36665,-102.62136,-11.77249]}, + {"t":3.47641, "x":3.5694, "y":2.58202, "heading":1.1432, "vx":1.14324, "vy":0.76714, "omega":-1.75681, "ax":-7.07036, "ay":-4.64823, "alpha":9.57527, "fx":[-134.49021,-84.6628,-109.45507,-152.45084], "fy":[-73.01859,-127.33723,-105.59695,-10.30739]}, + {"t":3.48908, "x":3.58331, "y":2.59136, "heading":1.12095, "vx":1.05367, "vy":0.70826, "omega":-1.63552, "ax":-7.04361, "ay":-4.65279, "alpha":9.79519, "fx":[-135.07496,-84.81271,-106.79482,-152.55643], "fy":[-71.974,-127.27283,-108.35747,-8.96605]}, + {"t":3.50174, "x":3.5961, "y":2.59996, "heading":1.10023, "vx":0.96445, "vy":0.64932, "omega":-1.51144, "ax":-7.01789, "ay":-4.65623, "alpha":10.01007, "fx":[-135.61674,-84.99459,-104.23498,-152.64289], "fy":[-70.9879,-127.18374,-110.88952,-7.74307]}, + {"t":3.51441, "x":3.60775, "y":2.60781, "heading":1.08109, "vx":0.87555, "vy":0.59034, "omega":-1.38464, "ax":-6.99349, "ay":-4.6586, "alpha":10.21729, "fx":[-136.11474,-85.19513,-101.80573,-152.71354], "fy":[-70.06573,-127.07902,-113.18768,-6.63323]}, + {"t":3.52708, "x":3.61828, "y":2.61492, "heading":1.06355, "vx":0.78696, "vy":0.53133, "omega":-1.25521, "ax":-6.97063, "ay":-4.66003, "alpha":10.41445, "fx":[-136.56841,-85.40265,-99.5314,-152.77121], "fy":[-69.2124,-126.96672,-115.25285,-5.63142]}, + {"t":3.53975, "x":3.62769, "y":2.62127, "heading":1.04765, "vx":0.69866, "vy":0.4723, "omega":-1.12329, "ax":-6.94946, "ay":-4.66071, "alpha":10.59944, "fx":[-136.97745,-85.60689,-97.43066,-152.81835], "fy":[-68.43234,-126.85397,-117.09057,-4.73264]}, + {"t":3.55241, "x":3.63598, "y":2.62688, "heading":1.03342, "vx":0.61063, "vy":0.41326, "omega":-0.98902, "ax":-6.93008, "ay":-4.66084, "alpha":10.77054, "fx":[-137.34169,-85.79883,-95.5171,-152.85705], "fy":[-67.72948,-126.7471,-118.70951,-3.932]}, + {"t":3.56508, "x":3.64316, "y":2.63174, "heading":1.02089, "vx":0.52285, "vy":0.35422, "omega":-0.85259, "ax":-6.91253, "ay":-4.66063, "alpha":10.9264, "fx":[-137.66109,-85.9706,-93.79996,-152.88905], "fy":[-67.10731,-126.65174,-120.12012,-3.22478]}, + {"t":3.57775, "x":3.64923, "y":2.63586, "heading":1.01009, "vx":0.43528, "vy":0.29518, "omega":-0.71418, "ax":-6.89682, "ay":-4.6603, "alpha":11.06598, "fx":[-137.93567,-86.11532,-92.28499,-152.91585], "fy":[-66.56889,-126.57284,-121.33357,-2.60648]}, + {"t":3.59042, "x":3.65419, "y":2.63922, "heading":1.00104, "vx":0.34792, "vy":0.23615, "omega":-0.574, "ax":-6.88293, "ay":-4.66006, "alpha":11.18858, "fx":[-138.16551,-86.22705,-90.97536,-152.93864], "fy":[-66.11689,-126.51474,-122.36089,-2.07287]}, + {"t":3.60308, "x":3.65804, "y":2.64184, "heading":0.99377, "vx":0.26073, "vy":0.17712, "omega":-0.43227, "ax":-6.87081, "ay":-4.66009, "alpha":11.29369, "fx":[-138.35067,-86.30068,-89.87243,-152.95842], "fy":[-65.75355,-126.48116,-123.2123,-1.62012]}, + {"t":3.61575, "x":3.6608, "y":2.64371, "heading":0.98829, "vx":0.1737, "vy":0.11809, "omega":-0.28921, "ax":-6.86043, "ay":-4.66054, "alpha":11.381, "fx":[-138.49119,-86.33188,-88.97656,-152.97601], "fy":[-65.48078,-126.47527,-123.89675,-1.24483]}, + {"t":3.62842, "x":3.66245, "y":2.64483, "heading":0.98463, "vx":0.08679, "vy":0.05905, "omega":-0.14505, "ax":-6.85173, "ay":-4.66153, "alpha":11.45033, "fx":[-138.5871,-86.31702,-88.28773,-152.99202], "fy":[-65.3001,-126.49968,-124.42158,-0.94412]}, + {"t":3.64108, "x":3.663, "y":2.6452, "heading":0.98279, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.83456, "ay":-4.90728, "alpha":10.40142, "fx":[-136.46628,-87.03151,-89.20957,-152.30812], "fy":[-69.6045,-126.00644,-123.88398,-14.39097]}, + {"t":3.65389, "x":3.66243, "y":2.6448, "heading":0.98279, "vx":-0.08754, "vy":-0.06286, "omega":0.13323, "ax":-6.84214, "ay":-4.90019, "alpha":10.37327, "fx":[-136.49551,-87.1929,-89.55089,-152.29205], "fy":[-69.52432,-125.88022,-123.60667,-14.39245]}, + {"t":3.6667, "x":3.66075, "y":2.6436, "heading":0.9845, "vx":-0.17518, "vy":-0.12562, "omega":0.2661, "ax":-6.85093, "ay":-4.89321, "alpha":10.33254, "fx":[-136.48696,-87.31421,-90.056,-152.2726], "fy":[-69.51696,-125.78032,-123.20548,-14.42541]}, + {"t":3.67951, "x":3.65795, "y":2.64158, "heading":0.98791, "vx":-0.26293, "vy":-0.1883, "omega":0.39845, "ax":-6.861, "ay":-4.88623, "alpha":10.27912, "fx":[-136.44102,-87.3978,-90.7264,-152.24957], "fy":[-69.5815,-125.70519,-122.67587,-14.49107]}, + {"t":3.69232, "x":3.65402, "y":2.63877, "heading":0.99301, "vx":-0.35082, "vy":-0.25088, "omega":0.53011, "ax":-6.87242, "ay":-4.87916, "alpha":10.21292, "fx":[-136.35808,-87.44648,-91.56412,-152.22271], "fy":[-69.71676,-125.65283,-122.01172,-14.59116]}, + {"t":3.70513, "x":3.64896, "y":2.63516, "heading":0.9998, "vx":-0.43884, "vy":-0.31338, "omega":0.66092, "ax":-6.88526, "ay":-4.87185, "alpha":10.13389, "fx":[-136.23855,-87.46367,-92.57149,-152.19165], "fy":[-69.92125,-125.62081,-121.20532,-14.72793]}, + {"t":3.71794, "x":3.64277, "y":2.63074, "heading":1.00827, "vx":-0.52703, "vy":-0.37578, "omega":0.79073, "ax":-6.89963, "ay":-4.86415, "alpha":10.04205, "fx":[-136.08285,-87.4534,-93.75089,-152.15594], "fy":[-70.19317,-125.60616,-120.24749,-14.90406]}, + {"t":3.73075, "x":3.63546, "y":2.62553, "heading":1.0184, "vx":-0.61541, "vy":-0.43809, "omega":0.91935, "ax":-6.91563, "ay":-4.85584, "alpha":9.9375, "fx":[-135.89146,-87.42041,-95.10435,-152.11504], "fy":[-70.53032,-125.6054,-119.12758,-15.1226]}, + {"t":3.74355, "x":3.62701, "y":2.61952, "heading":1.03017, "vx":-0.70399, "vy":-0.50028, "omega":1.04664, "ax":-6.93334, "ay":-4.84672, "alpha":9.82049, "fx":[-135.66495,-87.37024,-96.63318,-152.0683], "fy":[-70.93015,-125.61442,-117.83366,-15.3869]}, + {"t":3.75636, "x":3.61742, "y":2.61272, "heading":1.04358, "vx":-0.7928, "vy":-0.56236, "omega":1.17243, "ax":-6.95288, "ay":-4.83652, "alpha":9.69147, "fx":[-135.40403,-87.30935,-98.33743,-152.01497], "fy":[-71.38961,-125.62846,-116.35272,-15.70055]}, + {"t":3.76917, "x":3.60669, "y":2.60512, "heading":1.0586, "vx":-0.88186, "vy":-0.62431, "omega":1.29656, "ax":-6.97431, "ay":-4.82497, "alpha":9.55109, "fx":[-135.10958,-87.24522,-100.21531,-151.95417], "fy":[-71.90518,-125.64198,-114.67101,-16.06725]}, + {"t":3.78198, "x":3.59483, "y":2.59672, "heading":1.0752, "vx":-0.97119, "vy":-0.68612, "omega":1.4189, "ax":-6.99772, "ay":-4.81176, "alpha":9.40028, "fx":[-134.78273,-87.1866,-102.26255,-151.88491], "fy":[-72.47276,-125.64856,-112.77452,-16.49076]}, + {"t":3.79479, "x":3.58181, "y":2.58754, "heading":1.09338, "vx":-1.06082, "vy":-0.74775, "omega":1.53931, "ax":-7.02314, "ay":-4.79656, "alpha":9.24027, "fx":[-134.4249,-87.14361,-104.47173,-151.80602], "fy":[-73.0876,-125.64075,-110.64956,-16.97483]}, + {"t":3.8076, "x":3.56765, "y":2.57757, "heading":1.11309, "vx":-1.15078, "vy":-0.80919, "omega":1.65767, "ax":-7.05059, "ay":-4.77905, "alpha":9.07255, "fx":[-134.0379,-87.1281,-106.83164,-151.71623], "fy":[-73.74419,-125.60985,-108.28363,-17.52312]}, + {"t":3.82041, "x":3.55233, "y":2.56681, "heading":1.13433, "vx":-1.24109, "vy":-0.8704, "omega":1.77387, "ax":-7.08005, "ay":-4.75886, "alpha":8.89891, "fx":[-133.62405,-87.1539,-109.32667,-151.61405], "fy":[-74.43614,-125.54566,-105.66638,-18.13913]}, + {"t":3.83322, "x":3.53585, "y":2.55527, "heading":1.15705, "vx":-1.33178, "vy":-0.93136, "omega":1.88786, "ax":-7.11149, "ay":-4.73566, "alpha":8.7213, "fx":[-133.18631,-87.23727,-111.93633,-151.49782], "fy":[-75.15601,-125.43607,-102.79089,-18.8262]}, + {"t":3.84602, "x":3.51821, "y":2.54296, "heading":1.18123, "vx":-1.42286, "vy":-0.99201, "omega":1.99957, "ax":-7.14484, "ay":-4.70913, "alpha":8.54176, "fx":[-132.7284,-87.39743,-114.63511,-151.36562], "fy":[-75.89507,-125.26656,-99.65498,-19.5875]}, + {"t":3.85883, "x":3.4994, "y":2.52986, "heading":1.20684, "vx":-1.51438, "vy":-1.05233, "omega":2.10898, "ax":-7.18002, "ay":-4.67896, "alpha":8.36216, "fx":[-132.25509,-87.65721,-117.39249,-151.21531], "fy":[-76.64298,-125.01952,-96.26268,-20.42596]}, + {"t":3.87164, "x":3.47941, "y":2.516, "heading":1.23385, "vx":-1.60635, "vy":-1.11226, "omega":2.21609, "ax":-7.21697, "ay":-4.64486, "alpha":8.18395, "fx":[-131.77248,-88.04399,-120.17351,-151.0444], "fy":[-77.38739,-124.67323,-92.62565,-21.34444]}, + {"t":3.88445, "x":3.45825, "y":2.50137, "heading":1.26224, "vx":-1.69879, "vy":-1.17176, "omega":2.32091, "ax":-7.25569, "ay":-4.60654, "alpha":8.00779, "fx":[-131.28838,-88.59086,-122.93965,-150.85003], "fy":[-78.11323,-124.20043,-88.76451,-22.34565]}, + {"t":3.89726, "x":3.43589, "y":2.48599, "heading":1.29197, "vx":-1.79173, "vy":-1.23076, "omega":2.42348, "ax":-7.29627, "ay":-4.56372, "alpha":7.83305, "fx":[-130.8129,-89.33814,-125.6502,-150.62887], "fy":[-78.80188,-123.56635,-84.70981,-23.43236]}, + {"t":3.91007, "x":3.41234, "y":2.46985, "heading":1.32301, "vx":-1.88518, "vy":-1.28922, "omega":2.52381, "ax":-7.33898, "ay":-4.51603, "alpha":7.65725, "fx":[-130.35928,-90.33542,-128.26392,-150.37694], "fy":[-79.42975,-122.72572,-80.50254,-24.60748]}, + {"t":3.92288, "x":3.38759, "y":2.45296, "heading":1.35534, "vx":-1.97919, "vy":-1.34706, "omega":2.62189, "ax":-7.3843, "ay":-4.46294, "alpha":7.47533, "fx":[-129.94504,-91.64419,-130.74088,-150.0895], "fy":[-79.96622,-121.61843,-76.19416,-25.87423]}, + {"t":3.93569, "x":3.36164, "y":2.43534, "heading":1.38892, "vx":-2.07377, "vy":-1.40423, "omega":2.71764, "ax":-7.4331, "ay":-4.4036, "alpha":7.27884, "fx":[-129.59372,-93.34123,-133.0443,-149.76069], "fy":[-80.37048,-120.16292,-71.84608,-27.23636]}, + {"t":3.9485, "x":3.33447, "y":2.417, "heading":1.42373, "vx":-2.16898, "vy":-1.46063, "omega":2.81088, "ax":-7.48669, "ay":-4.33663, "alpha":7.05486, "fx":[-129.3375,-95.52282,-135.14206,-149.38313], "fy":[-80.58639,-118.24606,-67.52856,-28.69846]}, + {"t":3.9613, "x":3.30607, "y":2.39793, "heading":1.45973, "vx":-2.26487, "vy":-1.51618, "omega":2.90124, "ax":-7.54695, "ay":-4.25973, "alpha":6.78462, "fx":[-129.22128,-98.30954,-137.00759,-148.94724], "fy":[-80.53395,-115.70741,-63.31947,-30.26638]}, + {"t":3.97411, "x":3.27644, "y":2.37816, "heading":1.49689, "vx":-2.36154, "vy":-1.57074, "omega":2.98814, "ax":-7.61653, "ay":-4.16908, "alpha":6.44156, "fx":[-129.30906,-101.85058,-138.62014,-148.44008], "fy":[-80.09462,-112.31412,-59.30293,-31.94784]}, + {"t":3.98692, "x":3.24557, "y":2.3577, "heading":1.53517, "vx":-2.4591, "vy":-1.62414, "omega":3.07065, "ax":-7.69893, "ay":-4.05838, "alpha":5.98866, "fx":[-129.69435,-106.32442,-139.96386,-147.84349], "fy":[-79.08443,-107.72103,-55.56838,-33.75341]}, + {"t":3.99973, "x":3.21344, "y":2.33656, "heading":1.5745, "vx":-2.55771, "vy":-1.67613, "omega":3.14736, "ax":-7.79849, "ay":-3.91714, "alpha":5.37445, "fx":[-130.51713,-111.92648,-141.02591,-147.13051], "fy":[-77.20165,-101.40773,-52.21023,-35.6981]}, + {"t":4.01254, "x":3.18004, "y":2.31477, "heading":1.61481, "vx":-2.6576, "vy":-1.7263, "omega":3.2162, "ax":-7.91993, "ay":-3.72779, "alpha":4.52731, "fx":[-131.99106,-118.8205,-141.79314,-146.25854], "fy":[-73.91718,-92.5852,-49.3284,-37.80398]}, + {"t":4.02535, "x":3.14535, "y":2.29235, "heading":1.65601, "vx":-2.75905, "vy":-1.77405, "omega":3.27419, "ax":-8.06658, "ay":-3.46036, "alpha":3.34694, "fx":[-134.44004,-126.99858,-142.24734,-145.15498], "fy":[-68.22302,-80.0832,-47.02867,-40.10407]}, + {"t":4.03816, "x":3.10934, "y":2.26935, "heading":1.69795, "vx":-2.86237, "vy":-1.81837, "omega":3.31706, "ax":-8.23479, "ay":-3.0626, "alpha":1.68725, "fx":[-138.29583,-135.94599,-142.35946,-143.68435], "fy":[-57.99609,-62.31732,-45.41763,-42.64447]}, + {"t":4.05097, "x":3.07201, "y":2.24581, "heading":1.74044, "vx":-2.96785, "vy":-1.8576, "omega":3.33867, "ax":-8.3972, "ay":-2.43993, "alpha":-0.70556, "fx":[-143.64436,-144.03416,-142.08887,-141.56844], "fy":[-38.28209,-37.71159,-44.56423,-45.45247]}, + {"t":4.06377, "x":3.0333, "y":2.22181, "heading":1.7832, "vx":-3.07541, "vy":-1.88885, "omega":3.32963, "ax":-8.44035, "ay":-1.42178, "alpha":-4.41758, "fx":[-146.48701,-148.10874,-141.43847,-138.23747], "fy":[2.09729,-6.46734,-44.24264,-48.12384]}, + {"t":4.07658, "x":2.99322, "y":2.1975, "heading":1.82585, "vx":-3.18352, "vy":-1.90706, "omega":3.27305, "ax":-8.08483, "ay":0.04973, "alpha":-9.34422, "fx":[-129.12361,-145.55949,-141.08039,-134.31936], "fy":[64.27161,24.76105,-41.6326,-44.01619]}, + {"t":4.08939, "x":2.95178, "y":2.17308, "heading":1.86777, "vx":-3.28707, "vy":-1.90643, "omega":3.15336, "ax":-7.77728, "ay":1.62742, "alpha":-9.36503, "fx":[-110.01043,-139.84346,-143.22074,-136.08242], "fy":[91.86054,43.86492,-23.84816,-1.14958]}, + {"t":4.1022, "x":2.90904, "y":2.14879, "heading":1.90816, "vx":-3.38669, "vy":-1.88558, "omega":3.03341, "ax":-7.36881, "ay":3.4519, "alpha":-6.03686, "fx":[-105.24866,-130.76083,-141.04722,-124.30845], "fy":[95.87002,61.64066,19.77322,57.57944]}, + {"t":4.11501, "x":2.86505, "y":2.12492, "heading":1.94702, "vx":-3.48108, "vy":-1.84137, "omega":2.95608, "ax":-6.29534, "ay":5.3683, "alpha":-1.05251, "fx":[-102.68389,-108.92665,-111.51689,-105.20043], "fy":[96.672,90.18068,85.72782,92.67253]}, + {"t":4.12782, "x":2.81995, "y":2.10178, "heading":1.98488, "vx":-3.56171, "vy":-1.77261, "omega":2.9426, "ax":-3.59632, "ay":7.46226, "alpha":0.93286, "fx":[-65.42475,-56.86565,-57.16429,-65.23497], "fy":[124.45196,128.55542,129.28332,125.43283]}, + {"t":4.14063, "x":2.77403, "y":2.07969, "heading":2.02257, "vx":-3.60778, "vy":-1.67702, "omega":2.95455, "ax":0.02222, "ay":8.42938, "alpha":-1.55512, "fx":[3.74814,-9.82439,-3.48012,11.06792], "fy":[144.21537,143.53075,142.81779,142.96177]}, + {"t":4.15344, "x":2.72782, "y":2.0589, "heading":2.06042, "vx":-3.60749, "vy":-1.56905, "omega":2.93463, "ax":2.41833, "ay":8.16641, "alpha":-3.80658, "fx":[42.03003,14.50545,40.79301,67.21182], "fy":[141.26917,145.65778,138.69358,130.0124]}, + {"t":4.16624, "x":2.68181, "y":2.03947, "heading":2.09801, "vx":-3.57652, "vy":-1.46445, "omega":2.88587, "ax":3.79143, "ay":7.64975, "alpha":-5.45409, "fx":[60.26637,26.68678,74.33903,96.67205], "fy":[136.48594,145.52489,125.76321,112.70586]}, + {"t":4.17905, "x":2.63631, "y":2.02134, "heading":2.13497, "vx":-3.52795, "vy":-1.36647, "omega":2.81601, "ax":4.57572, "ay":7.19845, "alpha":-6.63421, "fx":[69.28572,33.8918,97.21971,110.92974], "fy":[133.30305,145.04456,110.79058,100.63582]}, + {"t":4.19186, "x":2.5915, "y":2.00443, "heading":2.17104, "vx":-3.46934, "vy":-1.27426, "omega":2.73104, "ax":5.0519, "ay":6.84792, "alpha":-7.54403, "fx":[74.16072,38.77793,112.29295,118.49408], "fy":[131.39584,144.51795,97.08654,92.92412]}, + {"t":4.20467, "x":2.54748, "y":1.98867, "heading":2.20602, "vx":-3.40463, "vy":-1.18655, "omega":2.63441, "ax":5.35843, "ay":6.58155, "alpha":-8.2558, "fx":[76.94348,42.44817,122.30608,122.88338], "fy":[130.29239,143.99093,85.55093,87.9671]}, + {"t":4.21748, "x":2.50431, "y":1.97401, "heading":2.23976, "vx":-3.336, "vy":-1.10225, "omega":2.52866, "ax":5.56607, "ay":6.3787, "alpha":-8.81051, "fx":[78.56006,45.42729,129.1255,125.59629], "fy":[129.68928,143.46305,76.11528,84.73171]}, + {"t":4.23029, "x":2.46203, "y":1.96041, "heading":2.27215, "vx":-3.2647, "vy":-1.02055, "omega":2.41581, "ax":5.71315, "ay":6.22267, "alpha":-9.23981, "fx":[79.47894,47.99004,133.90543,127.34177], "fy":[129.40286,142.92693,68.44938,82.60387]}, + {"t":4.2431, "x":2.42068, "y":1.94785, "heading":2.3031, "vx":-3.19153, "vy":-0.94084, "omega":2.29746, "ax":5.82152, "ay":6.10115, "alpha":-9.56927, "fx":[79.96,50.28924,137.35007,128.48998], "fy":[129.31974,142.37655,62.20835,81.21082]}, + {"t":4.25591, "x":2.38028, "y":1.9363, "heading":2.33252, "vx":-3.11696, "vy":-0.86269, "omega":2.17489, "ax":5.90423, "ay":6.00528, "alpha":-9.81942, "fx":[80.15812,52.41268,139.89576,129.25072], "fy":[129.36762,141.8086,57.09917,80.3172]}, + {"t":4.26871, "x":2.34084, "y":1.92574, "heading":2.36038, "vx":-3.04133, "vy":-0.78577, "omega":2.04911, "ax":5.96942, "ay":5.92864, "alpha":-10.00673, "fx":[80.17095,54.41089,141.81973,129.75115], "fy":[129.49895,141.22242,52.88826,79.76854]}, + {"t":4.28152, "x":2.30238, "y":1.91616, "heading":2.38663, "vx":-2.96487, "vy":-0.70983, "omega":1.92094, "ax":6.0223, "ay":5.86656, "alpha":-10.14442, "fx":[80.06287,56.31197,143.30305,130.07271], "fy":[129.68165,140.61958,49.3931,79.45981]}, + {"t":4.29433, "x":2.26489, "y":1.90755, "heading":2.41123, "vx":-2.88773, "vy":-0.63469, "omega":1.791, "ax":6.06631, "ay":5.81559, "alpha":-10.2432, "fx":[79.87784,58.13006,144.46713,130.27002], "fy":[129.89357,140.0035,46.47141,79.31745]}, + {"t":4.30714, "x":2.2284, "y":1.8999, "heading":2.43417, "vx":-2.81003, "vy":-0.5602, "omega":1.6598, "ax":6.10377, "ay":5.77315, "alpha":-10.31181, "fx":[79.64676,59.87036,145.39543,130.38116], "fy":[130.11922,139.37906,44.01176,79.2886]}, + {"t":4.31995, "x":2.19291, "y":1.8932, "heading":2.45543, "vx":-2.73185, "vy":-0.48625, "omega":1.52772, "ax":6.13626, "ay":5.73732, "alpha":-10.3574, "fx":[79.39185,61.53242,146.14661,130.43353], "fy":[130.34757,138.75233,41.92622,79.33443]}, + {"t":4.33276, "x":2.15842, "y":1.88744, "heading":2.475, "vx":-2.65325, "vy":-0.41276, "omega":1.39505, "ax":6.16489, "ay":5.70662, "alpha":-10.38585, "fx":[79.12941,63.11223,146.76278,130.44743], "fy":[130.57071,138.13024,40.1448,79.42585]}, + {"t":4.34557, "x":2.12494, "y":1.88262, "heading":2.49287, "vx":-2.57429, "vy":-0.33967, "omega":1.26202, "ax":6.19041, "ay":5.67992, "alpha":-10.40201, "fx":[78.87158,64.60367,147.27471,130.43822], "fy":[130.78289,137.52035,38.61136,79.54076]}, + {"t":4.35838, "x":2.09248, "y":1.87874, "heading":2.50904, "vx":-2.495, "vy":-0.26692, "omega":1.12878, "ax":6.21336, "ay":5.65637, "alpha":-10.40989, "fx":[78.62756,65.99949,147.70526,130.41781], "fy":[130.97992,136.93061,37.28046,79.66207]}, + {"t":4.37119, "x":2.06103, "y":1.87578, "heading":2.52349, "vx":-2.41541, "vy":-0.19447, "omega":0.99544, "ax":6.23414, "ay":5.6353, "alpha":-10.41278, "fx":[78.40442,67.29202,148.07168,130.39553], "fy":[131.15872,136.36911,36.11512,79.7764]}, + {"t":4.38399, "x":2.0306, "y":1.87375, "heading":2.53625, "vx":-2.33556, "vy":-0.12228, "omega":0.86207, "ax":6.25301, "ay":5.61619, "alpha":-10.41337, "fx":[78.20766,68.47365,148.38716,130.37885], "fy":[131.31695,135.84388,35.08503,79.87314]}, + {"t":4.3968, "x":2.0012, "y":1.87265, "heading":2.54729, "vx":-2.25547, "vy":-0.05035, "omega":0.72869, "ax":6.27016, "ay":5.59863, "alpha":-10.41389, "fx":[78.04164,69.53713,148.66189,130.37379], "fy":[131.45285,135.36269,34.16524,79.94377]}, + {"t":4.40961, "x":1.97282, "y":1.87246, "heading":2.55662, "vx":-2.17515, "vy":0.02136, "omega":0.5953, "ax":6.28574, "ay":5.58232, "alpha":-10.41615, "fx":[77.90989,70.47585,148.90384,130.38526], "fy":[131.56502,134.93285,33.33514,79.98137]}, + {"t":4.42242, "x":1.94548, "y":1.87319, "heading":2.56425, "vx":-2.09464, "vy":0.09287, "omega":0.46188, "ax":6.29987, "ay":5.56698, "alpha":-10.42162, "fx":[77.81533,71.28392,149.11928,130.41731], "fy":[131.65232,134.56107,32.57767,79.9802]}, + {"t":4.43523, "x":1.91917, "y":1.87484, "heading":2.57016, "vx":-2.01395, "vy":0.16417, "omega":0.32839, "ax":6.31262, "ay":5.55243, "alpha":-10.4315, "fx":[77.76045,71.95631,149.31317,130.47326], "fy":[131.71376,134.2533,31.87872,79.93547]}, + {"t":4.44804, "x":1.89389, "y":1.8774, "heading":2.57437, "vx":-1.93309, "vy":0.23529, "omega":0.19478, "ax":6.32406, "ay":5.53849, "alpha":-10.44677, "fx":[77.74742,72.48889,149.48946,130.55589], "fy":[131.74842,134.01461,31.22665,79.8431]}, + {"t":4.46085, "x":1.86965, "y":1.88087, "heading":2.57686, "vx":-1.85209, "vy":0.30623, "omega":0.06097, "ax":6.33425, "ay":5.52502, "alpha":-10.46821, "fx":[77.77823,72.87846,149.65129,130.66747], "fy":[131.75542,133.84907,30.61191,79.69958]}, + {"t":4.47366, "x":1.84644, "y":1.88524, "heading":2.57764, "vx":-1.77095, "vy":0.377, "omega":-0.07312, "ax":6.34327, "ay":5.51188, "alpha":-10.49646, "fx":[77.8547,73.1228,149.80114,130.80992], "fy":[131.73384,133.75967,30.02678,79.50178]}, + {"t":4.48646, "x":1.82428, "y":1.89052, "heading":2.57671, "vx":-1.6897, "vy":0.4476, "omega":-0.20757, "ax":6.35115, "ay":5.49896, "alpha":-10.53203, "fx":[77.97862,73.22072,149.94097,130.98476], "fy":[131.68272,133.74825,29.4651,79.24692]}, + {"t":4.49927, "x":1.80316, "y":1.89671, "heading":2.57405, "vx":-1.60835, "vy":0.51804, "omega":-0.34247, "ax":6.35798, "ay":5.48614, "alpha":-10.5753, "fx":[78.15175,73.17206,150.07229,131.19326], "fy":[131.60099,133.81541,28.92217,78.93245]}, + {"t":4.51208, "x":1.78308, "y":1.90379, "heading":2.56966, "vx":-1.52691, "vy":0.58831, "omega":-0.47793, "ax":6.36381, "ay":5.47332, "alpha":-10.62654, "fx":[78.37585,72.97784,150.19621,131.43639], "fy":[131.48746,133.96053,28.39461,78.55599]}, + {"t":4.52489, "x":1.76404, "y":1.91178, "heading":2.56354, "vx":-1.4454, "vy":0.65842, "omega":-0.61404, "ax":6.36873, "ay":5.46038, "alpha":-10.68594, "fx":[78.65275,72.64024,150.31354,131.71489], "fy":[131.34082,134.18168,27.88025,78.11531]}, + {"t":4.5377, "x":1.74605, "y":1.92066, "heading":2.55568, "vx":-1.36382, "vy":0.72836, "omega":-0.75091, "ax":6.37285, "ay":5.44721, "alpha":-10.75353, "fx":[78.98432,72.16279,150.42479,132.02927], "fy":[131.15959,134.47565,27.37815,77.60826]}, + {"t":4.55051, "x":1.7291, "y":1.93044, "heading":2.54606, "vx":-1.2822, "vy":0.79813, "omega":-0.88865, "ax":6.37625, "ay":5.43368, "alpha":-10.82923, "fx":[79.37249,71.55043,150.53022,132.37981], "fy":[130.9421,134.83798,26.88855,77.03276]}, + {"t":4.56332, "x":1.7132, "y":1.94111, "heading":2.53467, "vx":-1.20052, "vy":0.86773, "omega":-1.02736, "ax":6.37908, "ay":5.41969, "alpha":-10.91278, "fx":[79.81929,70.80967,150.62984,132.76659], "fy":[130.68652,135.263,26.41292,76.38678]}, + {"t":4.57613, "x":1.69835, "y":1.95266, "heading":2.52152, "vx":-1.11882, "vy":0.93715, "omega":-1.16714, "ax":6.38148, "ay":5.4051, "alpha":-11.00374, "fx":[80.32682,69.9487,150.72343,133.18946], "fy":[130.39078,135.74389,25.95402,75.66835]}, + {"t":4.58893, "x":1.68454, "y":1.96511, "heading":2.50657, "vx":-1.03708, "vy":1.00638, "omega":-1.30809, "ax":6.43502, "ay":5.41083, "alpha":-10.59183, "fx":[82.1219,72.29582,150.15383,133.2596], "fy":[129.24406,134.49225,28.90715,75.50302]}, + {"t":4.6012, "x":1.67231, "y":1.97786, "heading":2.49052, "vx":-0.95815, "vy":1.07274, "omega":-1.43799, "ax":6.45237, "ay":5.4022, "alpha":-10.52391, "fx":[83.0848,72.33897,149.9951,133.59266], "fy":[128.61397,134.45536,29.60334,74.88683]}, + {"t":4.61346, "x":1.66104, "y":1.99142, "heading":2.47289, "vx":-0.87902, "vy":1.139, "omega":-1.56706, "ax":6.47069, "ay":5.393, "alpha":-10.45105, "fx":[84.11194,72.37713,149.81776,133.95127], "fy":[127.93065,134.42101,30.36566,74.21618]}, + {"t":4.62573, "x":1.65075, "y":2.0058, "heading":2.45367, "vx":-0.79966, "vy":1.20514, "omega":-1.69524, "ax":6.49011, "ay":5.38329, "alpha":-10.37128, "fx":[85.20275,72.42617,149.61807,134.3325], "fy":[127.19198,134.38058,31.20627,73.49439]}, + {"t":4.63799, "x":1.64143, "y":2.02098, "heading":2.43288, "vx":-0.72006, "vy":1.27117, "omega":-1.82244, "ax":6.51075, "ay":5.37317, "alpha":-10.28252, "fx":[86.35664,72.50238,149.39167,134.73315], "fy":[126.39566,134.32516,32.13821,72.72527]}, + {"t":4.65026, "x":1.63309, "y":2.03698, "heading":2.41052, "vx":-0.64021, "vy":1.33707, "omega":-1.94855, "ax":6.53272, "ay":5.36272, "alpha":-10.18259, "fx":[87.57289,72.62235,149.13351,135.14975], "fy":[125.53923,134.2456,33.1753,71.91326]}, + {"t":4.66252, "x":1.62572, "y":2.05378, "heading":2.38663, "vx":-0.56009, "vy":1.40284, "omega":-2.07344, "ax":6.5561, "ay":5.35206, "alpha":-10.06921, "fx":[88.85062,72.80285,148.83774,135.57854], "fy":[124.62014,134.13252,34.33197,71.0635]}, + {"t":4.67479, "x":1.61935, "y":2.07139, "heading":2.3612, "vx":-0.47968, "vy":1.46848, "omega":-2.19693, "ax":6.58098, "ay":5.34131, "alpha":-9.94006, "fx":[90.18879,73.06075,148.49765,136.01545], "fy":[123.63569,133.97624,35.62299,70.18198]}, + {"t":4.68705, "x":1.61396, "y":2.0898, "heading":2.33425, "vx":-0.39896, "vy":1.53399, "omega":-2.31884, "ax":6.60741, "ay":5.33061, "alpha":-9.79282, "fx":[91.58613,73.41289,148.10558,136.45603], "fy":[122.5831,133.76675,37.06305,69.2757]}, + {"t":4.69932, "x":1.60956, "y":2.10902, "heading":2.30581, "vx":-0.31793, "vy":1.59937, "omega":-2.43895, "ax":6.63541, "ay":5.32008, "alpha":-9.62526, "fx":[93.04111,73.87596,147.65292,136.89545], "fy":[121.45953,133.49363,38.66627,68.35283]}, + {"t":4.71158, "x":1.60616, "y":2.12903, "heading":2.2759, "vx":-0.23654, "vy":1.66462, "omega":-2.557, "ax":6.66497, "ay":5.30985, "alpha":-9.43529, "fx":[94.55192,74.4665,147.13015,137.32845], "fy":[120.26209,133.14585,40.44542,67.42295]}, + {"t":4.72385, "x":1.60376, "y":2.14985, "heading":2.24454, "vx":-0.1548, "vy":1.72974, "omega":-2.67272, "ax":6.69608, "ay":5.30003, "alpha":-9.22104, "fx":[96.11642,75.20078,146.52695,137.74924], "fy":[118.98786,132.71166,42.41106,66.49721]}, + {"t":4.73611, "x":1.60237, "y":2.17146, "heading":2.21176, "vx":-0.07268, "vy":1.79474, "omega":-2.78581, "ax":6.72867, "ay":5.29067, "alpha":-8.98097, "fx":[97.73215,76.09482,145.83253,138.15149], "fy":[117.6339,132.17827,44.57037,65.58868]}, + {"t":4.74837, "x":1.60198, "y":2.19387, "heading":2.17759, "vx":0.00985, "vy":1.85963, "omega":-2.89596, "ax":6.76268, "ay":5.2818, "alpha":-8.71395, "fx":[99.39629,77.16443,145.03612,138.52817], "fy":[116.1973,131.53159,46.92583,64.71263]}, + {"t":4.76064, "x":1.60261, "y":2.21708, "heading":2.14207, "vx":0.09279, "vy":1.92441, "omega":-3.00284, "ax":6.79803, "ay":5.27333, "alpha":-8.41928, "fx":[101.10569,78.4253,144.12773,138.87142], "fy":[114.67514,130.75583,49.47374,63.88691]}, + {"t":4.7729, "x":1.60426, "y":2.24107, "heading":2.10524, "vx":0.17617, "vy":1.98908, "omega":-3.1061, "ax":6.83465, "ay":5.26512, "alpha":-8.09677, "fx":[102.85685,79.89315,143.09926,139.17236], "fy":[113.06448,129.83294,52.20265,63.13244]}, + {"t":4.78517, "x":1.60694, "y":2.26587, "heading":2.06715, "vx":0.25999, "vy":2.05366, "omega":-3.2054, "ax":6.87249, "ay":5.25685, "alpha":-7.7466, "fx":[104.64596,81.58401,141.94589,139.42078], "fy":[111.36235,128.74197,55.09184,62.47373]}, + {"t":4.79743, "x":1.61064, "y":2.29145, "heading":2.02784, "vx":0.34428, "vy":2.11813, "omega":-3.30041, "ax":6.91158, "ay":5.24808, "alpha":-7.36922, "fx":[106.46894,83.5145,140.66789,139.60479], "fy":[109.56564,127.45815,58.11007,61.9396]}, + {"t":4.8097, "x":1.61539, "y":2.31782, "heading":1.98736, "vx":0.42905, "vy":2.1825, "omega":-3.39079, "ax":6.95201, "ay":5.23821, "alpha":-6.96502, "fx":[108.32152,85.70229,139.27249,139.71019], "fy":[107.67103,125.95176,61.21476,61.56399]}, + {"t":4.82196, "x":1.62117, "y":2.34498, "heading":1.94577, "vx":0.51431, "vy":2.24674, "omega":-3.47621, "ax":6.99397, "ay":5.22643, "alpha":-6.53393, "fx":[110.19927,88.1666,137.77597,139.71971], "fy":[105.67476,124.18644,64.35186,61.38706]}, + {"t":4.83423, "x":1.628, "y":2.37293, "heading":1.90314, "vx":0.60009, "vy":2.31084, "omega":-3.55635, "ax":7.0378, "ay":5.21176, "alpha":-6.07473, "fx":[112.09775,90.92885,136.20533,139.61183], "fy":[103.57242,122.11709,67.45646,61.45649]}, + {"t":4.84649, "x":1.63589, "y":2.40166, "heading":1.85952, "vx":0.68641, "vy":2.37476, "omega":-3.63085, "ax":7.08396, "ay":5.19304, "alpha":-5.5843, "fx":[114.01258,94.01331,134.59952,139.35913], "fy":[101.35851,119.68675,70.45426,61.82905]}, + {"t":4.85876, "x":1.64484, "y":2.43118, "heading":1.81499, "vx":0.77329, "vy":2.43846, "omega":-3.69934, "ax":7.13303, "ay":5.16888, "alpha":-5.05667, "fx":[115.93961,97.4477,133.00978,138.9259], "fy":[99.02584,116.82236,73.26368,62.57259]}, + {"t":4.87102, "x":1.65487, "y":2.46148, "heading":1.76962, "vx":0.86077, "vy":2.50185, "omega":-3.76136, "ax":7.18563, "ay":5.13765, "alpha":-4.48197, "fx":[117.8751,101.2636,131.49879,138.26469], "fy":[96.56465,113.42867,75.79824,63.76828]}, + {"t":4.88329, "x":1.66596, "y":2.49255, "heading":1.72348, "vx":0.9489, "vy":2.56486, "omega":-3.81633, "ax":7.24236, "ay":5.09742, "alpha":-3.84521, "fx":[119.81597,105.49613,130.13877,137.31133], "fy":[93.96108,109.37955,77.96877,65.513]}, + {"t":4.89555, "x":1.67815, "y":2.52439, "heading":1.67668, "vx":1.03773, "vy":2.62738, "omega":-3.86349, "ax":7.3036, "ay":5.04574, "alpha":-3.12511, "fx":[121.76018,110.18205,129.00835,135.97783], "fy":[91.19477,104.50544,79.68503,67.92148]}, + {"t":4.90782, "x":1.69142, "y":2.55699, "heading":1.62929, "vx":1.1273, "vy":2.68926, "omega":-3.90182, "ax":7.36921, "ay":4.97941, "alpha":-2.29278, "fx":[123.70729,115.35441,128.18863,134.14234], "fy":[88.23469,98.57552,80.85612,71.1271]}, + {"t":4.92008, "x":1.7058, "y":2.59035, "heading":1.58144, "vx":1.21768, "vy":2.75033, "omega":-3.92994, "ax":7.43815, "ay":4.89387, "alpha":-1.31059, "fx":[125.65933,121.0297,127.75854,131.63563], "fy":[85.03161,91.27291,81.38979,75.27917]}, + {"t":4.93234, "x":1.7213, "y":2.62445, "heading":1.53324, "vx":1.30891, "vy":2.81036, "omega":-3.94601, "ax":7.50771, "ay":4.7824, "alpha":-0.13146, "fx":[127.62221,127.17988,127.78945,128.22454], "fy":[81.50357,82.16284,81.19068,80.53209]}, + {"t":4.94461, "x":1.73791, "y":2.65928, "heading":1.48484, "vx":1.40099, "vy":2.86901, "omega":-3.94763, "ax":7.57241, "ay":4.63476, "alpha":1.29999, "fx":[129.60783,133.67429,128.33893,123.59675], "fy":[77.5063,70.66121,80.15837,87.01754]}, + {"t":4.95687, "x":1.75567, "y":2.69481, "heading":1.43643, "vx":1.49386, "vy":2.92585, "omega":-3.93168, "ax":7.62225, "ay":4.43532, "alpha":3.04136, "fx":[131.63601,140.16798,129.44232,117.36298], "fy":[72.76763,56.03514,78.18747,94.78383]}, + {"t":4.96914, "x":1.77456, "y":2.73103, "heading":1.38821, "vx":1.58735, "vy":2.98025, "omega":-3.89438, "ax":7.64083, "ay":4.16118, "alpha":5.1341, "fx":[133.72898,145.91878,131.09963,109.12624], "fy":[66.73068,37.54019,75.17532,103.67553]}, + {"t":4.9814, "x":1.7946, "y":2.7679, "heading":1.34044, "vx":1.68106, "vy":3.03129, "omega":-3.83141, "ax":7.60567, "ay":3.782, "alpha":7.558, "fx":[135.8466,149.61516,133.24957,98.76955], "fy":[58.17225,14.98707,71.05673,113.10663]}, + {"t":4.99367, "x":1.81579, "y":2.80536, "heading":1.29345, "vx":1.77434, "vy":3.07767, "omega":-3.73872, "ax":7.50082, "ay":3.27368, "alpha":10.12148, "fx":[137.43514,149.74999,135.70032,87.46207], "fy":[44.7396,-9.55188,65.94422,121.60554]}, + {"t":5.00593, "x":1.83812, "y":2.84335, "heading":1.2476, "vx":1.86633, "vy":3.11782, "omega":-3.61458, "ax":7.36927, "ay":2.749, "alpha":12.10423, "fx":[136.27991,146.98076,137.96558,80.17023], "fy":[28.87263,-28.49467,60.57016,126.09074]}, + {"t":5.0182, "x":1.86156, "y":2.8818, "heading":1.20327, "vx":1.95672, "vy":3.15154, "omega":-3.46613, "ax":7.30747, "ay":2.44763, "alpha":12.82999, "fx":[132.92521,144.42258,139.7208,80.12303], "fy":[23.06438,-37.75121,55.6024,125.61813]}, + {"t":5.03046, "x":1.88611, "y":2.92063, "heading":1.16076, "vx":2.04634, "vy":3.18156, "omega":-3.30877, "ax":7.29732, "ay":2.21479, "alpha":13.03985, "fx":[129.93679,142.14854,141.24856,83.16726], "fy":[20.99135,-43.70268,50.4353,122.96807]}, + {"t":5.04273, "x":1.91176, "y":2.95982, "heading":1.12017, "vx":2.13584, "vy":3.20872, "omega":-3.14884, "ax":7.31869, "ay":1.92028, "alpha":13.09585, "fx":[127.86963,139.69717,142.65827,87.72998], "fy":[16.12974,-48.92588,44.59862,118.85111]}, + {"t":5.05499, "x":1.9385, "y":2.99932, "heading":1.08156, "vx":2.2256, "vy":3.23227, "omega":-2.98823, "ax":7.37053, "ay":1.51317, "alpha":13.00294, "fx":[126.74034,136.92592,143.92235,93.89395], "fy":[6.3879,-53.84552,37.68565,112.72663]}, + {"t":5.06726, "x":1.96635, "y":3.03907, "heading":1.04491, "vx":2.316, "vy":3.25083, "omega":-2.82875, "ax":7.44667, "ay":0.95601, "alpha":12.67937, "fx":[125.80982,133.71146,144.92647,102.21493], "fy":[-8.55829,-58.58216,29.01953,103.16642]}, + {"t":5.07952, "x":1.99532, "y":3.07902, "heading":1.01021, "vx":2.40733, "vy":3.26256, "omega":-2.67324, "ax":7.52705, "ay":0.19157, "alpha":11.95285, "fx":[123.60669,129.74351,145.3447,113.4372], "fy":[-27.71292,-63.46037,17.2232,86.98397]}, + {"t":5.09179, "x":2.02541, "y":3.11905, "heading":0.97743, "vx":2.49964, "vy":3.26491, "omega":-2.52664, "ax":7.55618, "ay":-0.92553, "alpha":10.36038, "fx":[118.52343,124.20189,144.06079,127.32751], "fy":[-49.3385,-69.37425,-0.78128,56.52185]}, + {"t":5.10405, "x":2.05664, "y":3.15902, "heading":0.94644, "vx":2.59232, "vy":3.25355, "omega":-2.39958, "ax":7.30068, "ay":-2.70977, "alpha":6.63177, "fx":[110.35455,115.58751,136.40066,134.38731], "fy":[-70.46642,-77.32502,-32.04116,-4.53691]}, + {"t":5.11631, "x":2.08898, "y":3.19872, "heading":0.91701, "vx":2.68186, "vy":3.22032, "omega":-2.31824, "ax":6.04123, "ay":-5.05627, "alpha":-0.4993, "fx":[104.54926,103.97674,100.92228,101.59014], "fy":[-84.34181,-84.03423,-87.72305,-87.92357]}, + {"t":5.12858, "x":2.12233, "y":3.23783, "heading":0.88858, "vx":2.75595, "vy":3.15831, "omega":-2.32437, "ax":3.49894, "ay":-6.57157, "alpha":-8.99314, "fx":[100.47361,83.89859,3.6811,50.0107], "fy":[-93.9038,-90.25927,-132.71225,-130.24663]}, + {"t":5.14084, "x":2.15639, "y":3.27608, "heading":0.86007, "vx":2.79886, "vy":3.07771, "omega":-2.43466, "ax":1.08471, "ay":-6.79403, "alpha":-13.86843, "fx":[95.46462,31.15439,-68.81984,16.00302], "fy":[-102.55008,-95.97847,-121.11244,-142.61706]}, + {"t":5.15311, "x":2.1908, "y":3.31331, "heading":0.83021, "vx":2.81217, "vy":2.99438, "omega":-2.60475, "ax":-1.59337, "ay":-7.17863, "alpha":-12.76148, "fx":[61.54057,-65.40571,-90.85083,-13.69517], "fy":[-127.32942,-104.93111,-111.24737,-144.91775]}, + {"t":5.16537, "x":2.22517, "y":3.3495, "heading":0.79826, "vx":2.79263, "vy":2.90634, "omega":-2.76127, "ax":-3.38515, "ay":-7.52417, "alpha":-6.87808, "fx":[-11.72521,-81.74568,-91.69352,-45.15755], "fy":[-143.4155,-115.24092,-113.64342,-139.63595]}, + {"t":5.17764, "x":2.25916, "y":3.38458, "heading":0.76439, "vx":2.75111, "vy":2.81406, "omega":-2.84563, "ax":-4.45647, "ay":-7.37379, "alpha":-2.4928, "fx":[-61.2947,-84.10642,-88.50202,-69.31024], "fy":[-133.35592,-119.83742,-118.03047,-130.48012]}, + {"t":5.1899, "x":2.29257, "y":3.41853, "heading":0.72949, "vx":2.69645, "vy":2.72362, "omega":-2.8762, "ax":-5.03014, "ay":-7.14125, "alpha":0.12719, "fx":[-86.21665,-85.17405,-84.90088,-85.95318], "fy":[-121.02808,-121.77587,-121.91509,-121.16331]}, + {"t":5.20217, "x":2.32526, "y":3.4514, "heading":0.69422, "vx":2.63476, "vy":2.63604, "omega":-2.87464, "ax":-5.36168, "ay":-6.94201, "alpha":1.83186, "fx":[-99.85276,-86.14225,-81.55634,-97.25137], "fy":[-111.54405,-122.55443,-125.08726,-113.14095]}, + {"t":5.21443, "x":2.35717, "y":3.48321, "heading":0.65896, "vx":2.569, "vy":2.55089, "omega":-2.85217, "ax":-5.57124, "ay":-6.78259, "alpha":3.03569, "fx":[-108.2844,-87.15608,-78.58526,-105.0352], "fy":[-104.41569,-122.74643,-127.65755,-106.65993]}, + {"t":5.2267, "x":2.38826, "y":3.51399, "heading":0.62398, "vx":2.50067, "vy":2.46771, "omega":-2.81494, "ax":-5.71352, "ay":-6.6549, "alpha":3.93102, "fx":[-114.02704,-88.22044,-75.98544,-110.50826], "fy":[-98.86822,-122.60436,-129.75146,-101.56755]}, + {"t":5.23896, "x":2.4185, "y":3.54375, "heading":0.58946, "vx":2.4306, "vy":2.38609, "omega":-2.76673, "ax":-5.81575, "ay":-6.55119, "alpha":4.6189, "fx":[-118.22734,-89.3196,-73.72676,-114.42321], "fy":[-94.38039,-122.25564,-131.46954,-97.62979]}, + {"t":5.25123, "x":2.44788, "y":3.57252, "heading":0.55552, "vx":2.35927, "vy":2.30574, "omega":-2.71008, "ax":-5.89267, "ay":-6.46568, "alpha":5.15857, "fx":[-121.46554,-90.43738,-71.77403,-117.25324], "fy":[-90.6278,-121.77186,-132.88814,-94.62934]}, + {"t":5.26349, "x":2.47637, "y":3.60032, "heading":0.52229, "vx":2.287, "vy":2.22644, "omega":-2.64681, "ax":-5.95276, "ay":-6.39422, "alpha":5.58781, "fx":[-124.06219,-91.56046,-70.09324,-119.30292], "fy":[-87.40609,-121.19685,-134.06522,-92.38702]}, + {"t":5.27575, "x":2.50397, "y":3.62714, "heading":0.48982, "vx":2.21399, "vy":2.14802, "omega":-2.57828, "ax":-6.00117, "ay":-6.33384, "alpha":5.93227, "fx":[-126.20741,-92.67837,-68.65313,-120.77415], "fy":[-84.58256,-120.55926,-135.04538,-90.76024]}, + {"t":5.28802, "x":2.53067, "y":3.65301, "heading":0.4582, "vx":2.14039, "vy":2.07034, "omega":-2.50552, "ax":-6.04117, "ay":-6.28239, "alpha":6.21029, "fx":[-128.02074,-93.78301,-67.42543,-121.80486], "fy":[-82.0681,-119.87895,-135.86346,-89.63612]}, + {"t":5.30028, "x":2.55647, "y":3.67793, "heading":0.42747, "vx":2.06629, "vy":1.99328, "omega":-2.42935, "ax":-6.07485, "ay":-6.23824, "alpha":6.43553, "fx":[-129.58104,-94.86808,-66.38477,-122.49225], "fy":[-79.80097,-119.17029,-136.54705,-88.92461]}, + {"t":5.31255, "x":2.58136, "y":3.70191, "heading":0.39768, "vx":1.99179, "vy":1.91677, "omega":-2.35043, "ax":-6.10367, "ay":-6.20017, "alpha":6.61849, "fx":[-130.9425,-95.92873,-65.50837,-122.90683], "fy":[-77.73714,-118.44411,-137.11836,-88.55289]}, + {"t":5.32481, "x":2.60532, "y":3.72495, "heading":0.36885, "vx":1.91693, "vy":1.84073, "omega":-2.26925, "ax":-6.12859, "ay":-6.16721, "alpha":6.76744, "fx":[-132.1437,-96.96115,-64.77585,-123.1013], "fy":[-75.84434,-117.70882,-137.59541,-88.46119]}, + {"t":5.33708, "x":2.62837, "y":3.74706, "heading":0.34102, "vx":1.84176, "vy":1.76509, "omega":-2.18625, "ax":-6.15032, "ay":-6.13858, "alpha":6.88903, "fx":[-133.21293,-97.96243,-64.16897,-123.11621], "fy":[-74.0984,-116.9711,-137.99302,-88.59969]}, + {"t":5.34934, "x":2.6505, "y":3.76825, "heading":0.31421, "vx":1.76633, "vy":1.68981, "omega":-2.10176, "ax":-6.16937, "ay":-6.11367, "alpha":6.98868, "fx":[-134.17151,-98.9303,-63.67141,-122.98365], "fy":[-72.48072,-116.23643,-138.32348,-88.92629]}, + {"t":5.36161, "x":2.6717, "y":3.78851, "heading":0.28843, "vx":1.69067, "vy":1.61482, "omega":-2.01605, "ax":-6.18613, "ay":-6.09193, "alpha":7.07082, "fx":[-135.03586,-99.86303,-63.26859,-122.72976], "fy":[-70.97676,-115.50934,-138.59707,-89.40495]}, + {"t":5.37387, "x":2.69197, "y":3.80786, "heading":0.2637, "vx":1.6148, "vy":1.54011, "omega":-1.92933, "ax":-6.2009, "ay":-6.07294, "alpha":7.1391, "fx":[-135.81892,-100.75933,-62.94747,-122.37639], "fy":[-69.57491,-114.79367,-138.82247,-90.00457]}, + {"t":5.38614, "x":2.71131, "y":3.82629, "heading":0.24004, "vx":1.53874, "vy":1.46563, "omega":-1.84177, "ax":-6.21392, "ay":-6.0563, "alpha":7.19653, "fx":[-136.53107,-101.61823,-62.69642,-121.94222], "fy":[-68.26574,-114.0927,-139.00704,-90.69806]}, + {"t":5.3984, "x":2.72971, "y":3.84381, "heading":0.21745, "vx":1.46253, "vy":1.39135, "omega":-1.7535, "ax":-6.22539, "ay":-6.04169, "alpha":7.24556, "fx":[-137.18075,-102.43901,-62.50502,-121.44363], "fy":[-67.04144,-113.40926,-139.15709,-91.46172]}, + {"t":5.41067, "x":2.74718, "y":3.86042, "heading":0.19594, "vx":1.38618, "vy":1.31725, "omega":-1.66464, "ax":-6.23549, "ay":-6.02882, "alpha":7.28821, "fx":[-137.77495,-103.22119,-62.364,-120.89516], "fy":[-65.89549,-112.74583,-139.27805,-92.27473]}, + {"t":5.42293, "x":2.76371, "y":3.87612, "heading":0.17553, "vx":1.3097, "vy":1.24331, "omega":-1.57525, "ax":-6.24436, "ay":-6.01745, "alpha":7.32608, "fx":[-138.3195,-103.96444,-62.26503,-120.30998], "fy":[-64.82234,-112.10462,-139.37462,-93.1188]}, + {"t":5.4352, "x":2.77931, "y":3.89092, "heading":0.15621, "vx":1.23312, "vy":1.1695, "omega":-1.4854, "ax":-6.25214, "ay":-6.00735, "alpha":7.36044, "fx":[-138.81934,-104.66856,-62.20069,-119.70006], "fy":[-63.81721,-111.48757,-139.4509,-93.97781]}, + {"t":5.44746, "x":2.79396, "y":3.90481, "heading":0.13799, "vx":1.15644, "vy":1.09583, "omega":-1.39513, "ax":-6.25897, "ay":-5.99834, "alpha":7.3923, "fx":[-139.27872,-105.33344,-62.16432,-119.07644], "fy":[-62.87595,-110.89644,-139.5105,-94.83761]}, + {"t":5.45972, "x":2.80767, "y":3.9178, "heading":0.12088, "vx":1.07967, "vy":1.02226, "omega":-1.30446, "ax":-6.26494, "ay":-5.99025, "alpha":7.42241, "fx":[-139.70124,-105.95905,-62.14996,-118.44931], "fy":[-61.99493,-110.3328,-139.55657,-95.68578]}, + {"t":5.47199, "x":2.82044, "y":3.92989, "heading":0.10488, "vx":1.00284, "vy":0.94879, "omega":-1.21343, "ax":-6.27018, "ay":-5.98294, "alpha":7.45132, "fx":[-140.09008,-106.54542,-62.15228,-117.82809], "fy":[-61.17094,-109.79809,-139.59192,-96.51145]}, + {"t":5.48425, "x":2.83227, "y":3.94107, "heading":0.09, "vx":0.92594, "vy":0.87541, "omega":-1.12204, "ax":-6.27478, "ay":-5.97627, "alpha":7.47943, "fx":[-140.44797,-107.09262,-62.16647,-117.22152], "fy":[-60.4011,-109.29361,-139.61905,-97.3051]}, + {"t":5.49652, "x":2.84316, "y":3.95136, "heading":0.07624, "vx":0.84898, "vy":0.80212, "omega":-1.03031, "ax":-6.27882, "ay":-5.97015, "alpha":7.507, "fx":[-140.77732,-107.60071,-62.18826,-116.63768], "fy":[-59.68288,-108.82056,-139.64016,-98.05846]}, + {"t":5.50878, "x":2.8531, "y":3.96075, "heading":0.0636, "vx":0.77197, "vy":0.72889, "omega":-0.93824, "ax":-6.28241, "ay":-5.96447, "alpha":7.53417, "fx":[-141.08026,-108.06982,-62.21377,-116.08405], "fy":[-59.01398,-108.38003,-139.65724,-98.76431]}, + {"t":5.52105, "x":2.86209, "y":3.96924, "heading":0.05209, "vx":0.69492, "vy":0.65574, "omega":-0.84583, "ax":-6.28561, "ay":-5.95915, "alpha":7.561, "fx":[-141.35866,-108.50003,-62.23955,-115.56749], "fy":[-58.39234,-107.97302,-139.67207,-99.41638]}, + {"t":5.53331, "x":2.87014, "y":3.97683, "heading":0.04172, "vx":0.61783, "vy":0.58265, "omega":-0.7531, "ax":-6.2885, "ay":-5.95413, "alpha":7.58752, "fx":[-141.61415,-108.89144,-62.2625,-115.09434], "fy":[-57.81612,-107.60045,-139.68621,-100.00921]}, + {"t":5.54558, "x":2.87725, "y":3.98353, "heading":0.03248, "vx":0.5407, "vy":0.50963, "omega":-0.66004, "ax":-6.29115, "ay":-5.94934, "alpha":7.61368, "fx":[-141.8482,-109.24416,-62.27984,-114.67036], "fy":[-57.28368,-107.26316,-139.7011,-100.53803]}, + {"t":5.55784, "x":2.88341, "y":3.98934, "heading":0.02439, "vx":0.46354, "vy":0.43666, "omega":-0.56666, "ax":-6.29361, "ay":-5.94472, "alpha":7.63943, "fx":[-142.06208,-109.55826,-62.28908,-114.30083], "fy":[-56.79354,-106.96192,-139.718,-100.99866]}, + {"t":5.57011, "x":2.88862, "y":3.99424, "heading":0.01744, "vx":0.38635, "vy":0.36375, "omega":-0.47297, "ax":-6.29595, "ay":-5.94024, "alpha":7.6647, "fx":[-142.25693,-109.8338,-62.288,-113.99049], "fy":[-56.34441,-106.69743,-139.73803,-101.38739]}, + {"t":5.58237, "x":2.89288, "y":3.99826, "heading":0.01164, "vx":0.30914, "vy":0.2909, "omega":-0.37896, "ax":-6.29821, "ay":-5.93585, "alpha":7.68942, "fx":[-142.43374,-110.07083,-62.27465,-113.74366], "fy":[-55.93515,-106.47032,-139.7622,-101.70091]}, + {"t":5.59464, "x":2.8962, "y":4.00138, "heading":0.00699, "vx":0.23189, "vy":0.2181, "omega":-0.28466, "ax":-6.30043, "ay":-5.93152, "alpha":7.71354, "fx":[-142.59335,-110.26935,-62.24729,-113.56417], "fy":[-55.56477,-106.28118,-139.79137,-101.9362]}, + {"t":5.6069, "x":2.89857, "y":4.00361, "heading":0.0035, "vx":0.15462, "vy":0.14535, "omega":-0.19005, "ax":-6.30266, "ay":-5.9272, "alpha":7.73702, "fx":[-142.73651,-110.42936,-62.2044,-113.45543], "fy":[-55.23246,-106.1305,-139.82632,-102.09047]}, + {"t":5.61916, "x":2.89999, "y":4.00495, "heading":0.00117, "vx":0.07732, "vy":0.07265, "omega":-0.09516, "ax":-6.30415, "ay":-5.92382, "alpha":7.75896, "fx":[-142.85562,-110.54194,-62.13581,-113.39352], "fy":[-54.95899,-106.02805,-139.8717,-102.19128]}, + {"t":5.63143, "x":2.90047, "y":4.00539, "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":[] diff --git a/src/main/java/frc/robot/CONSTANTS.java b/src/main/java/frc/robot/CONSTANTS.java index 6f36164..63de4d9 100644 --- a/src/main/java/frc/robot/CONSTANTS.java +++ b/src/main/java/frc/robot/CONSTANTS.java @@ -1,63 +1,385 @@ -package frc.robot; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Translation2d; - -public class CONSTANTS { - //RobotContainer - public static final int CONTROLLER_PORT = 0; - - // Vision Constants - public static AprilTagFieldLayout APRILTAG_FIELD_LAYOUT - = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); - - //Drivetrain - public static final Translation2d FRONT_LEFT_MODULE_LOCATION - = new Translation2d(0.33, 0.23); - public static final Translation2d FRONT_RIGHT_MODULE_LOCATION - = new Translation2d(0.33, -0.23); - public static final Translation2d BACK_LEFT_MODULE_LOCATION - = new Translation2d(-0.33, 0.23); - public static final Translation2d BACK_RIGHT_MODULE_LOCATION - = new Translation2d(-0.33, -0.23); - - public static final int FRONT_LEFT_STEERING_CAN_ID = 5; - public static final int FRONT_LEFT_DRIVE_CAN_ID = 1; - public static final int FRONT_LEFT_ENCODER_CAN_ID = 1; - - public static final int FRONT_RIGHT_STEERING_CAN_ID = 6; - public static final int FRONT_RIGHT_DRIVE_CAN_ID = 2; - public static final int FRONT_RIGHT_ENCODER_CAN_ID = 2; - - public static final int BACK_LEFT_STEERING_CAN_ID = 4; - public static final int BACK_LEFT_DRIVE_CAN_ID = 8; - public static final int BACK_LEFT_ENCODER_CAN_ID = 4; - - public static final int BACK_RIGHT_STEERING_CAN_ID = 3; - public static final int BACK_RIGHT_DRIVE_CAN_ID = 7; - public static final int BACK_RIGHT_ENCODER_CAN_ID = 3; - - // TODO: This should be set to the robot's actual max speeds and then we should set the controller's - // response curves. Determine these experimentally. - public static final double MAX_SPEED_METERS_PER_SEC = 5.0; - public static final double MAX_ANGULAR_RAD_PER_SEC = 3*Math.PI; - public static final double DEADBAND = 0.08; - - public final static long SWERVE_ENCODER_SET_FREQUECY_SECONDS = 1; - - public static final double STEERING_GEAR_RATIO = 150.0/7.0; // from SDS website - - // TODO: Determine this experimentally and not using math, just change everything - public static final double EFFECTIVE_GEAR_RATIO = 21.0; - - //Localization - public static final int GYRO_CAN_ID = 9; - - // Path Following Constants - public static final double PATH_FOLLOWER_P_X = 2.0; - public static final double PATH_FOLLOWER_P_Y = 2.0; - public static final double PATH_FOLLOWER_P_THETA = 4.0; -} - - +package frc.robot; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.MomentOfInertia; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.RobotBase; + +public class CONSTANTS { + + //RobotContainer + public static final int CONTROLLER_PORT = 0; + + // Vision Constants + public static AprilTagFieldLayout APRILTAG_FIELD_LAYOUT = + AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + + //Localization + public static final int GYRO_CAN_ID = 9; + + // Path Following Constants + public static final double PATH_FOLLOWER_P_X = 2.0; + public static final double PATH_FOLLOWER_P_Y = 2.0; + public static final double PATH_FOLLOWER_P_THETA = 4.0; + + public static final double ROBOT_LOOP_PERIOD = 0.02; + public static final Mode SIM_MODE = Mode.SIM; + public static final Mode CURRENT_MODE = RobotBase.isReal() + ? Mode.REAL + : SIM_MODE; + + public static enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + + /** Replaying from a log file. */ + REPLAY, + } + + // NEW DRIVETRAIN CONSTANTS + public static class DriveConstants { + + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs STEER_GAINS = new Slot0Configs() + .withKP(100) + .withKI(0) + .withKD(0.5) + .withKS(0.1) + .withKV(1.91) + .withKA(0) + .withStaticFeedforwardSign( + StaticFeedforwardSignValue.UseClosedLoopSign + ); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs DRIVE_GAINS = new Slot0Configs() + .withKP(0.1) + .withKI(0) + .withKD(0) + .withKS(0) + .withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType STEER_CLOSED_LOOP_OUTPUT = + ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType DRIVE_CLOSED_LOOP_OUTPUT = + ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement DRIVE_MOTOR_TYPE = + DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement STEER_MOTOR_TYPE = + SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder + private static final SteerFeedbackType STEER_FEEDBACK_TYPE = + SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current SLIP_CURRENT = Amps.of(120.0); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration DRIVE_INITIAL_CONFIGS = + new TalonFXConfiguration(); + private static final TalonFXConfiguration STEER_INITIAL_CONFIGS = + new TalonFXConfiguration().withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively + // low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + ); + private static final CANcoderConfiguration ENCODER_INITIAL_CONFIGS = + new CANcoderConfiguration(); + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus CAN_BUS = new CANBus( + "canivore", + "./logs/example.hoot" + ); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity SPEED_AT_12_VOLTS = + MetersPerSecond.of(4.0); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double COUPLE_RATIO = 3.8181818181818183; + + private static final double DRIVE_GEAR_RATIO = 7.363636363636365; + private static final double STEER_GEAR_RATIO = 150.0 / 7.0; // ~21.43 + public static final Distance WHEEL_RADIUS = Inches.of(2.0); + + private static final boolean INVERT_LEFT_SIDE = false; + private static final boolean INVERT_RIGHT_SIDE = true; + + // These are only used for simulation + private static final MomentOfInertia STEER_INERTIA = + KilogramSquareMeters.of(0.004); + private static final MomentOfInertia DRIVE_INERTIA = + KilogramSquareMeters.of(0.025); + // Simulated voltage necessary to overcome friction + private static final Voltage STEER_FRICTION_VOLTAGE = Volts.of(0.2); + private static final Voltage DRIVE_FRICTION_VOLTAGE = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DRIVETRAIN_CONSTANTS = + new SwerveDrivetrainConstants().withCANBusName(CAN_BUS.getName()); + + private static final SwerveModuleConstantsFactory< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > ConstantCreator = new SwerveModuleConstantsFactory< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + >() + .withDriveMotorGearRatio(DRIVE_GEAR_RATIO) + .withSteerMotorGearRatio(STEER_GEAR_RATIO) + .withCouplingGearRatio(COUPLE_RATIO) + .withWheelRadius(WHEEL_RADIUS) + .withSteerMotorGains(STEER_GAINS) + .withDriveMotorGains(DRIVE_GAINS) + .withSteerMotorClosedLoopOutput(STEER_CLOSED_LOOP_OUTPUT) + .withDriveMotorClosedLoopOutput(DRIVE_CLOSED_LOOP_OUTPUT) + .withSlipCurrent(SLIP_CURRENT) + .withSpeedAt12Volts(SPEED_AT_12_VOLTS) + .withDriveMotorType(DRIVE_MOTOR_TYPE) + .withSteerMotorType(STEER_MOTOR_TYPE) + .withFeedbackSource(STEER_FEEDBACK_TYPE) + .withDriveMotorInitialConfigs(DRIVE_INITIAL_CONFIGS) + .withSteerMotorInitialConfigs(STEER_INITIAL_CONFIGS) + .withEncoderInitialConfigs(ENCODER_INITIAL_CONFIGS) + .withSteerInertia(STEER_INERTIA) + .withDriveInertia(DRIVE_INERTIA) + .withSteerFrictionVoltage(STEER_FRICTION_VOLTAGE) + .withDriveFrictionVoltage(DRIVE_FRICTION_VOLTAGE); + + // Front Left + private static final int FRONT_LEFT_DRIVE_MOTOR_ID = 1; + private static final int FRONT_LEFT_STEER_MOTOR_ID = 5; + private static final int FRONT_LEFT_ENCODER_ID = 1; + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of( + 0.15234375 + ); + private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; + private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; + + private static final Distance FRONT_LEFT_X_POS = Meters.of(0.33); + private static final Distance FRONT_LEFT_Y_POS = Meters.of(0.23); + + // Front Right + private static final int FRONT_RIGHT_DRIVE_MOTOR_ID = 2; + private static final int FRONT_RIGHT_STEER_MOTOR_ID = 6; + private static final int FRONT_RIGHT_ENCODER_ID = 2; + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of( + -0.4873046875 + ); + private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; + private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; + + private static final Distance FRONT_RIGHT_X_POS = Meters.of(0.33); + private static final Distance FRONT_RIGHT_Y_POS = Meters.of(-0.23); + + // Back Left + private static final int BACK_LEFT_DRIVE_MOTOR_ID = 8; + private static final int BACK_LEFT_STEER_MOTOR_ID = 4; + private static final int BACK_LEFT_ENCODER_ID = 4; + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of( + -0.219482421875 + ); + private static final boolean BACK_LELFT_STEER_MOTOR_INVERTED = true; + private static final boolean BACK_LEFT_ENCODER_INVERTED = false; + + private static final Distance BACK_LEFT_X_POS = Meters.of(-0.33); + private static final Distance BACK_LEFT_Y_POS = Meters.of(0.23); + + // Back Right + private static final int BACK_RIGHT_DRIVE_MOTOR_ID = 7; + private static final int BACK_RIGHT_STEER_MOTOR_ID = 3; + private static final int BACK_RIGHT_ENCODER_ID = 3; + private static final Angle BACK_RIGHT_ENOCDER_OFFSET = Rotations.of( + 0.17236328125 + ); + private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; + private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; + + private static final Distance BACK_RIGHT_X_POS = Meters.of(-0.33); + private static final Distance BACK_RIGHT_Y_POS = Meters.of(-0.23); + + public static final SwerveModuleConstants< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > FRONT_LEFT = ConstantCreator.createModuleConstants( + FRONT_LEFT_STEER_MOTOR_ID, + FRONT_LEFT_DRIVE_MOTOR_ID, + FRONT_LEFT_ENCODER_ID, + FRONT_LEFT_ENCODER_OFFSET, + FRONT_LEFT_X_POS, + FRONT_LEFT_Y_POS, + INVERT_LEFT_SIDE, + FRONT_LEFT_STEER_MOTOR_INVERTED, + FRONT_LEFT_ENCODER_INVERTED + ); + public static final SwerveModuleConstants< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > FRONT_RIGHT = ConstantCreator.createModuleConstants( + FRONT_RIGHT_STEER_MOTOR_ID, + FRONT_RIGHT_DRIVE_MOTOR_ID, + FRONT_RIGHT_ENCODER_ID, + FRONT_RIGHT_ENCODER_OFFSET, + FRONT_RIGHT_X_POS, + FRONT_RIGHT_Y_POS, + INVERT_RIGHT_SIDE, + FRONT_RIGHT_STEER_MOTOR_INVERTED, + FRONT_RIGHT_ENCODER_INVERTED + ); + public static final SwerveModuleConstants< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > BACK_LEFT = ConstantCreator.createModuleConstants( + BACK_LEFT_STEER_MOTOR_ID, + BACK_LEFT_DRIVE_MOTOR_ID, + BACK_LEFT_ENCODER_ID, + BACK_LEFT_ENCODER_OFFSET, + BACK_LEFT_X_POS, + BACK_LEFT_Y_POS, + INVERT_LEFT_SIDE, + BACK_LELFT_STEER_MOTOR_INVERTED, + BACK_LEFT_ENCODER_INVERTED + ); + public static final SwerveModuleConstants< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > BACK_RIGHT = ConstantCreator.createModuleConstants( + BACK_RIGHT_STEER_MOTOR_ID, + BACK_RIGHT_DRIVE_MOTOR_ID, + BACK_RIGHT_ENCODER_ID, + BACK_RIGHT_ENOCDER_OFFSET, + BACK_RIGHT_X_POS, + BACK_RIGHT_Y_POS, + INVERT_RIGHT_SIDE, + BACK_RIGHT_STEER_MOTOR_INVERTED, + BACK_RIGHT_ENCODER_INVERTED + ); + + public static final double ODOMETRY_FREQUENCY = new CANBus( + DriveConstants.DRIVETRAIN_CONSTANTS.CANBusName + ).isNetworkFD() + ? 250.0 + : 100.0; + + public static final double DRIVE_BASE_RADIUS = Math.max( + Math.max( + Math.hypot( + DriveConstants.FRONT_LEFT.LocationX, + DriveConstants.FRONT_LEFT.LocationY + ), + Math.hypot( + DriveConstants.FRONT_RIGHT.LocationX, + DriveConstants.FRONT_RIGHT.LocationY + ) + ), + Math.max( + Math.hypot( + DriveConstants.BACK_LEFT.LocationX, + DriveConstants.BACK_LEFT.LocationY + ), + Math.hypot( + DriveConstants.BACK_RIGHT.LocationX, + DriveConstants.BACK_RIGHT.LocationY + ) + ) + ); + + // PathPlanner config constants + public static final double ROBOT_MASS_KG = 74.088; + public static final double ROBOT_MOI = 6.883; + public static final double WHEEL_COF = 1.2; + + public static final double DEADBAND = 0.1; + public static final double ANGLE_KP = 5.0; + public static final double ANGLE_KD = 0.4; + public static final double ANGLE_MAX_VELOCITY = 8.0; + public static final double ANGLE_MAX_ACCELERATION = 20.0; + public static final double FF_START_DELAY = 2.0; // Secs + public static final double FF_RAMP_RATE = 0.1; // Volts/Sec + public static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec + public static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 + + public static final double PATH_PLANNER_KP = 5.0; + public static final double PATH_PLANNER_KI = 0.0; + public static final double PATH_PLANNER_KD = 0.0; + + public static final double MM_CRUISE_VELOCITY = + 100.0 / STEER_GEAR_RATIO; + public static final double MM_ACCELERATION = MM_CRUISE_VELOCITY / 0.100; + public static final double MM_EXPO_KV = 0.12 * STEER_GEAR_RATIO; + public static final double MM_EXPO_KA = 0.1; + + public static final int GYRO_CAN_ID = 9; + + public static final double DRIVE_CAN_FRAME_FREQUENCY = 50.0; + public static final double DRIVE_CAN_FRAME_PERIOD = + 1.0 / DRIVE_CAN_FRAME_FREQUENCY; + public static final double DRIVE_CAN_FRAME_PERIOD_SEC = + DRIVE_CAN_FRAME_PERIOD / 1000.0; + public static final double GRYO_CAN_FRAME_FREQUENCY = 0.01; + } + + public static class Timeouts { + + public static final double STD_DEBOUNCE_TIME = 0.5; // Secs + public static final int STD_RETRY_ATTEMPTS = 5; + public static final double STD_TIMEOUT = 0.1; // Secs + public static final double STD_TIMEOUT_LONG = 0.25; // Secs + public static final double SYSID_TIMEOUT = 1.0; // Secs + } +} diff --git a/src/main/java/frc/robot/FollowPath.java b/src/main/java/frc/robot/FollowPath.java index b2bea7f..c1815b4 100644 --- a/src/main/java/frc/robot/FollowPath.java +++ b/src/main/java/frc/robot/FollowPath.java @@ -1,7 +1,5 @@ package frc.robot; -import static frc.robot.CONSTANTS.PATH_FOLLOWER_P_THETA; - import java.util.Optional; import choreo.trajectory.SwerveSample; @@ -21,7 +19,6 @@ public class FollowPath extends Command { private final Trajectory trajectory; private final Drivetrain drivetrain; - private final PoseEstimator8736 poseEstimator; private final boolean resetPose; private final Timer timer = new Timer(); @@ -30,16 +27,14 @@ public class FollowPath extends Command { public FollowPath( Trajectory trajectory, Drivetrain drivetrain, - PoseEstimator8736 poseEstimator, boolean resetPose) { this.trajectory = trajectory; this.drivetrain = drivetrain; - this.poseEstimator = poseEstimator; this.resetPose = resetPose; Constraints thetaProfile = new TrapezoidProfile.Constraints( - CONSTANTS.MAX_ANGULAR_RAD_PER_SEC, CONSTANTS.MAX_ANGULAR_RAD_PER_SEC * 2); + CONSTANTS.DriveConstants.ANGLE_MAX_ACCELERATION, CONSTANTS.DriveConstants.ANGLE_MAX_ACCELERATION); ProfiledPIDController thetaController = new ProfiledPIDController(CONSTANTS.PATH_FOLLOWER_P_THETA, 0, 0, thetaProfile); @@ -71,10 +66,7 @@ public void initialize() { // TODO: Why would this ever happen? Should we handle it differently? throw new IllegalStateException("Trajectory has no initial pose!"); } - this.drivetrain.setModulesToEncoders(); - this.poseEstimator.initialize( - initialPose.get(), - this.drivetrain); + this.drivetrain.resetPose(initialPose.get()); } } @@ -100,7 +92,7 @@ public void execute() { ChassisSpeeds commandedSpeeds = this.holonomicController.calculate( - this.poseEstimator.getPose(), + this.drivetrain.getPose(), swerveSample.get().getPose(), desiredLinearVelocity, swerveSample.get().getPose().getRotation() diff --git a/src/main/java/frc/robot/PoseEstimator8736.java b/src/main/java/frc/robot/PoseEstimator8736.java index ea1ad50..b784ab9 100644 --- a/src/main/java/frc/robot/PoseEstimator8736.java +++ b/src/main/java/frc/robot/PoseEstimator8736.java @@ -1,72 +1,158 @@ package frc.robot; -import com.reduxrobotics.sensors.canandgyro.Canandgyro; +import edu.wpi.first.math.Matrix; 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.Twist2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.subsystems.drivetrain.Drivetrain; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; -/* - * TEST PLAN: - * - * Determine where to initialize the pose estimator. - * Test output of getModulePositions to make sure they are correct. +/** + * Utility class for managing swerve drive pose estimation. Encapsulates the SwerveDrivePoseEstimator + * and handles odometry updates, vision measurements, and gyro integration. */ - public class PoseEstimator8736 { - private static final int GYRO_CAN_ID = 9; - private final Canandgyro gyro = new Canandgyro(GYRO_CAN_ID); - private SwerveDrivePoseEstimator swervePoseEstimator; - private Drivetrain drivetrain; + private final SwerveDriveKinematics kinematics; + private final SwerveDrivePoseEstimator poseEstimator; + + private Rotation2d rawGyroRotation = Rotation2d.kZero; + private SwerveModulePosition[] lastModulePositions = // For delta tracking + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + }; - // TODO: Where do we call this from and do we need to make sure drivetrain encoders are set first? - // Can we just call this whenever we call setModulesToEncoders? Should they be called together? - public void initialize(Pose2d pose, Drivetrain drivetrain) { - double yawInRotations = pose.getRotation().getDegrees() / 360.0; // between 0.0 and 1.0 - this.gyro.setYaw(yawInRotations); - this.drivetrain = drivetrain; - this.swervePoseEstimator = new SwerveDrivePoseEstimator( - drivetrain.getKinematics(), - pose.getRotation(), // initial gyro angle TODO: Should I use the actual gyro angle instead? - drivetrain.getModulePositions(), - pose); + /** + * Creates a new PoseEstimator. + * + * @param kinematics The swerve drive kinematics + * @param initialGyroRotation The initial gyro rotation + * @param initialPose The initial pose estimate + */ + public PoseEstimator8736( + SwerveDriveKinematics kinematics, + Rotation2d initialGyroRotation, + Pose2d initialPose + ) { + this.kinematics = kinematics; + this.rawGyroRotation = initialGyroRotation; + this.poseEstimator = new SwerveDrivePoseEstimator( + kinematics, + rawGyroRotation, + lastModulePositions, + initialPose + ); } - public void zeroGyro() { - // also resets the pose estimator, if it has been previously initialized - - Pose2d startingPose = null; - if (this.swervePoseEstimator != null) { - startingPose = getPose(); - startingPose = new Pose2d( - startingPose.getX(), - startingPose.getY(), - new Rotation2d(0.0)); + /** + * Updates the pose estimator with odometry data. + * + * @param modulePositions The current module positions + * @param gyroRotation The current gyro rotation (or null to use kinematics) + * @param timestamp The timestamp of this sample + */ + public void updateOdometry( + SwerveModulePosition[] modulePositions, + Rotation2d gyroRotation, + double timestamp + ) { + // Calculate module deltas + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + moduleDeltas[moduleIndex] = new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters - + lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle + ); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; } - this.gyro.setYaw(0.0); - - if (startingPose != null && this.drivetrain != null) { - initialize(startingPose, this.drivetrain); + // Update gyro angle + if (gyroRotation != null) { + // Use the real gyro angle + rawGyroRotation = gyroRotation; + } else { + // Use the angle delta from the kinematics and module deltas + Twist2d twist = kinematics.toTwist2d(moduleDeltas); + rawGyroRotation = rawGyroRotation.plus( + new Rotation2d(twist.dtheta) + ); } + + // Apply update to pose estimator + poseEstimator.updateWithTime( + timestamp, + rawGyroRotation, + modulePositions + ); } - - public Pose2d getPose() { - return this.swervePoseEstimator.getEstimatedPosition(); + + /** + * Adds a vision measurement to the pose estimator. + * + * @param visionRobotPoseMeters The vision-measured robot pose + * @param timestampSeconds The timestamp of the vision measurement + * @param visionMeasurementStdDevs The standard deviations of the vision measurement + */ + public void addVisionMeasurement( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs + ) { + poseEstimator.addVisionMeasurement( + visionRobotPoseMeters, + timestampSeconds, + visionMeasurementStdDevs + ); + } + + /** + * Adds a vision measurement to the pose estimator. + * + * @param visionRobotPoseMeters The vision-measured robot pose + * @param timestampSeconds The timestamp of the vision measurement + */ + public void addVisionMeasurement( + Pose2d visionRobotPoseMeters, + double timestampSeconds + ) { + poseEstimator.addVisionMeasurement( + visionRobotPoseMeters, + timestampSeconds + ); } - public double getGyroYaw() { - return this.gyro.getYaw(); // returns yaw in rotations + /** + * Resets the pose estimator to a specific pose. + * + * @param pose The new pose + * @param modulePositions The current module positions + */ + public void resetPose(Pose2d pose, SwerveModulePosition[] modulePositions) { + poseEstimator.resetPosition(rawGyroRotation, modulePositions, pose); } - public void addVisionMeasurement(Pose2d pose, double timestampSeconds) { - this.swervePoseEstimator.addVisionMeasurement(pose, timestampSeconds); + /** + * Returns the current estimated pose. + * + * @return The current pose estimate + */ + public Pose2d getEstimatedPose() { + return poseEstimator.getEstimatedPosition(); } - public void addOdometryMeasurement(SwerveModulePosition[] positions) { - this.swervePoseEstimator.update(this.gyro.getRotation2d(), positions); + /** + * Returns the current raw gyro rotation. + * + * @return The raw gyro rotation + */ + public Rotation2d getRawGyroRotation() { + return rawGyroRotation; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3e26c13..dde293a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,21 +4,49 @@ package frc.robot; -import java.time.Duration; import java.time.Instant; -import edu.wpi.first.wpilibj.TimedRobot; +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 edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import static frc.robot.CONSTANTS.*; -public class Robot extends TimedRobot { +public class Robot extends LoggedRobot { public Instant lastSwerveModuleSetTime = Instant.MIN; private Command autonomousCommand; private final RobotContainer robotContainer; public Robot() { + 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; + } + + // Start AdvantageKit logger + Logger.start(); + this.robotContainer = new RobotContainer(); } @@ -32,20 +60,10 @@ public void disabledInit() {} @Override public void disabledPeriodic() { - // set the swerve modules to the external encoders periodically - Instant now = Instant.now(); - Duration duration = Duration.between(this.lastSwerveModuleSetTime, now); - if (duration.compareTo(Duration.ofSeconds(SWERVE_ENCODER_SET_FREQUECY_SECONDS)) >= 0) { - this.robotContainer.setSwerveModulesToEncoders(); - this.lastSwerveModuleSetTime = now; - } } @Override public void disabledExit() { - // Zero the gyro when we enable. We will probably have to start setting this - // differently at the start of autos. This clearly will have to change. - this.robotContainer.zeroGyro(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ccaad78..de7dbe9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,98 +4,151 @@ package frc.robot; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static frc.robot.CONSTANTS.*; +import static frc.robot.CONSTANTS.DriveConstants; + +import choreo.Choreo; +import choreo.trajectory.SwerveSample; +import choreo.trajectory.Trajectory; +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.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.drivetrain.DrivetrainController; -import choreo.Choreo; -import choreo.trajectory.Trajectory; -import choreo.trajectory.SwerveSample; -import static frc.robot.CONSTANTS.*; - +import frc.robot.subsystems.drivetrain.GyroIO; +import frc.robot.subsystems.drivetrain.GyroIORedux; +import frc.robot.subsystems.drivetrain.ModuleIOSim; +import frc.robot.subsystems.drivetrain.ModuleIOTalonFX; +import frc.robot.subsystems.drivetrain.ModuleIOTalonFXRedux; import java.util.Optional; public class RobotContainer { - private final Drivetrain drivetrain = new Drivetrain(); - private final PoseEstimator8736 poseEstimator = new PoseEstimator8736(); - private final DrivetrainController drivetrainController = new DrivetrainController(poseEstimator); - - private final CommandPS4Controller controller = new CommandPS4Controller(CONTROLLER_PORT); - - public RobotContainer() { - // TODO: Think about where to initialize all of this properly - this.poseEstimator.initialize(new Pose2d(), this.drivetrain); - this.drivetrain.setPoseEstimator(this.poseEstimator); - configureBindings(); - generateAutos(); - } - - public void setSwerveModulesToEncoders() { - this.drivetrain.setModulesToEncoders(); - } - - public void zeroGyro() { - this.poseEstimator.zeroGyro(); - } - - private void configureBindings() { - controller.cross().onTrue(new InstantCommand( - () -> { - this.poseEstimator.zeroGyro(); - } - )); - - this.drivetrain.setDefaultCommand( - new RunCommand( - () -> { - double forward = -this.controller.getLeftY(); // Negative to match FRC convention - double strafe = -this.controller.getLeftX(); - double rotation = -this.controller.getRightX(); - - // apply deadbands and scaling - - forward = Math.abs(forward) > DEADBAND ? forward : 0.0; - strafe = Math.abs(strafe) > DEADBAND ? strafe : 0.0; - rotation = Math.abs(rotation) > DEADBAND ? rotation : 0.0; - - ChassisSpeeds speeds = new ChassisSpeeds( - forward*forward*forward*MAX_SPEED_METERS_PER_SEC, - strafe*strafe*strafe* MAX_SPEED_METERS_PER_SEC, - rotation*rotation*rotation*MAX_ANGULAR_RAD_PER_SEC - ); - - // convert to robot-oriented coordinates and pass to swerve subsystem - ChassisSpeeds robotOriented = this.drivetrainController.fieldToRobotChassisSpeeds(speeds); - this.drivetrain.setDesiredState(robotOriented); - }, - this.drivetrain - ) - ); - } + private final Drivetrain drivetrain; + private final DrivetrainController drivetrainController; - private Command testAuto = null; - - private void generateAutos() { - Optional> trajectory = Choreo.loadTrajectory("Test Path"); - this.testAuto = new FollowPath( - trajectory.get(), - this.drivetrain, - this.poseEstimator, - true + private final CommandPS4Controller controller = new CommandPS4Controller( + CONTROLLER_PORT ); - System.out.println("*** Loaded Test Path autonomous ***"); - } - - public Command getAutonomousCommand() { - return testAuto; - //return Commands.print("No autonomous command configured"); - } + public RobotContainer() { + // TODO: Think about where to initialize all of this properly + if (CONSTANTS.CURRENT_MODE == CONSTANTS.SIM_MODE) { + this.drivetrain = new Drivetrain( + new GyroIORedux(), + new ModuleIOSim(DriveConstants.FRONT_LEFT), + new ModuleIOSim(DriveConstants.FRONT_RIGHT), + new ModuleIOSim(DriveConstants.BACK_LEFT), + new ModuleIOSim(DriveConstants.BACK_RIGHT) + ); + } else { + this.drivetrain = new Drivetrain( + new GyroIO() {}, + new ModuleIOTalonFXRedux(DriveConstants.FRONT_LEFT), + new ModuleIOTalonFXRedux(DriveConstants.FRONT_RIGHT), + new ModuleIOTalonFXRedux(DriveConstants.BACK_LEFT), + new ModuleIOTalonFXRedux(DriveConstants.BACK_RIGHT) + ); + } + this.drivetrainController = new DrivetrainController(this.drivetrain); + configureBindings(); + generateAutos(); + } + + private void configureBindings() { + controller + .cross() + .onTrue( + new InstantCommand(() -> { + this.drivetrain.zeroGyro(); + }) + ); + + this.drivetrain.setDefaultCommand( + new RunCommand( + () -> { + double forward = -this.controller.getLeftY(); // Negative to match FRC convention + double strafe = -this.controller.getLeftX(); + Translation2d driveSpeeds = getDriveVelocity( + forward, + strafe + ); + double rotation; + if (CONSTANTS.CURRENT_MODE == CONSTANTS.Mode.SIM) { + rotation = -this.controller.getRawAxis(3); // Why is sim different then driverstation? + } else { + rotation = -this.controller.getRightX(); + } + + // apply deadbands and scaling + rotation = MathUtil.applyDeadband( + rotation, + CONSTANTS.DriveConstants.DEADBAND + ); + + rotation = Math.copySign(rotation * rotation, rotation); + + ChassisSpeeds speeds = new ChassisSpeeds( + driveSpeeds.getX() * + CONSTANTS.DriveConstants.SPEED_AT_12_VOLTS.in( + MetersPerSecond + ), + driveSpeeds.getY() * + CONSTANTS.DriveConstants.SPEED_AT_12_VOLTS.in( + MetersPerSecond + ), + (rotation * + (CONSTANTS.DriveConstants.SPEED_AT_12_VOLTS.in( + MetersPerSecond + ))) / + CONSTANTS.DriveConstants.DRIVE_BASE_RADIUS + ); + + // convert to robot-oriented coordinates and pass to swerve subsystem + ChassisSpeeds robotOriented = + this.drivetrainController.fieldToRobotChassisSpeeds( + speeds + ); + this.drivetrain.setDesiredState(robotOriented); + }, + this.drivetrain + ) + ); + } + + private Command testAuto = null; + + private void generateAutos() { + Optional> trajectory = Choreo.loadTrajectory( + "Test Path" + ); + this.testAuto = new FollowPath(trajectory.get(), this.drivetrain, true); + + System.out.println("*** Loaded Test Path autonomous ***"); + } + + public Command getAutonomousCommand() { + return testAuto; + //return Commands.print("No autonomous command configured"); + } + + private static Translation2d getDriveVelocity(double x, double y) { + double linearMag = MathUtil.applyDeadband( + Math.hypot(x, y), + DriveConstants.DEADBAND + ); + Rotation2d direction = new Rotation2d(Math.atan2(y, x)); + linearMag = linearMag * linearMag; + return new Pose2d(Translation2d.kZero, direction) + .transformBy(new Transform2d(linearMag, 0.0, Rotation2d.kZero)) + .getTranslation(); + } } - diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 4918bd9..d882831 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -1,149 +1,205 @@ package frc.robot.subsystems.drivetrain; +import static frc.robot.CONSTANTS.CONTROLLER_PORT; + 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.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.CONSTANTS; +import frc.robot.CONSTANTS.DriveConstants; import frc.robot.PoseEstimator8736; -import static frc.robot.CONSTANTS.*; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; public class Drivetrain extends SubsystemBase { - // Per WPILib documentation +X is forward and +Y is left (oriented to the robot) - // Positive rotation is counterclockwise - - - - SwerveDriveKinematics kinematics; - ChassisSpeeds desiredChassisSpeeds; - PoseEstimator8736 poseEstimator; - //private final StructArrayPublisher publisher; - - private final SwerveModule frontLeftModule = new SwerveModule( - FRONT_LEFT_STEERING_CAN_ID, FRONT_LEFT_DRIVE_CAN_ID, FRONT_LEFT_ENCODER_CAN_ID); - - private final SwerveModule frontRightModule = new SwerveModule( - FRONT_RIGHT_STEERING_CAN_ID, FRONT_RIGHT_DRIVE_CAN_ID, FRONT_RIGHT_ENCODER_CAN_ID); - - private final SwerveModule backLeftModule = new SwerveModule( - BACK_LEFT_STEERING_CAN_ID, BACK_LEFT_DRIVE_CAN_ID, BACK_LEFT_ENCODER_CAN_ID); - - private final SwerveModule backRightModule = new SwerveModule( - BACK_RIGHT_STEERING_CAN_ID, BACK_RIGHT_DRIVE_CAN_ID, BACK_RIGHT_ENCODER_CAN_ID); - - /** - * Remember that the front of the robot is +X and the left side of the robot is - * +Y. - */ - public Drivetrain() { - this.kinematics = new SwerveDriveKinematics( - FRONT_LEFT_MODULE_LOCATION, FRONT_RIGHT_MODULE_LOCATION, - BACK_LEFT_MODULE_LOCATION, BACK_RIGHT_MODULE_LOCATION); - - this.desiredChassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); - - // this.publisher = NetworkTableInstance.getDefault().getStructArrayTopic( - // "/SwerveStates", SwerveModuleState.struct).publish(); - } - - public void setDesiredState(ChassisSpeeds desiredChassisSpeeds) { - this.desiredChassisSpeeds = desiredChassisSpeeds; - } - - public ChassisSpeeds getDesiredState() { - return this.desiredChassisSpeeds; - } - - public SwerveDriveKinematics getKinematics() { - return this.kinematics; - } - - public SwerveModulePosition[] getModulePositions() { - return new SwerveModulePosition[] { - this.frontLeftModule.getModulePosition(), - this.frontRightModule.getModulePosition(), - this.backLeftModule.getModulePosition(), - this.backRightModule.getModulePosition() - }; - } - - public void setModulesToEncoders() { - this.frontLeftModule.setModuleToEncoder(); - this.frontRightModule.setModuleToEncoder(); - this.backLeftModule.setModuleToEncoder(); - this.backRightModule.setModuleToEncoder(); - } - - public void setPoseEstimator(PoseEstimator8736 poseEstimator) { - this.poseEstimator = poseEstimator; - } - - @Override - public void periodic() { - // update the pose estimator - - this.poseEstimator.addOdometryMeasurement(this.getModulePositions()); - - Pose2d pose = this.poseEstimator.getPose(); - SmartDashboard.putNumber("Pose Estimator/X", pose.getX()); - SmartDashboard.putNumber("Pose Estimator/Y", pose.getY()); - SmartDashboard.putNumber("Pose Estimator/Rotation Degrees", pose.getRotation().getDegrees()); - - // send the new desired states down to the modules - - SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates( - this.desiredChassisSpeeds); - - SwerveModuleState frontLeftState = moduleStates[0]; - SwerveModuleState frontRightState = moduleStates[1]; - SwerveModuleState backLeftState = moduleStates[2]; - SwerveModuleState backRightState = moduleStates[3]; - - // publisher.set(new SwerveModuleState[] { - // frontLeftState, - // frontRightState, - // backLeftState, - // backRightState - // }); - - this.frontLeftModule.setModuleState(frontLeftState); - this.frontRightModule.setModuleState(frontRightState); - this.backLeftModule.setModuleState(backLeftState); - this.backRightModule.setModuleState(backRightState); - } - - /** - * Example command factory method. - * - * @return a command - */ - public Command exampleMethodCommand() { - // Inline construction of command goes here. - // Subsystem::RunOnce implicitly requires `this` subsystem. - return runOnce( - () -> { - /* one-time action goes here */ - }); - } - - /** - * An example method querying a boolean state of the subsystem (for example, a - * digital sensor). - * - * @return value of some boolean subsystem state, such as a digital sensor. - */ - public boolean exampleCondition() { - // Query some boolean state, such as a digital sensor. - return false; - } - - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - this.periodic(); - } + + // Per WPILib documentation +X is forward and +Y is left (oriented to the robot) + // Positive rotation is counterclockwise + + SwerveDriveKinematics kinematics; + ChassisSpeeds desiredChassisSpeeds; + PoseEstimator8736 poseEstimator; + //private final StructArrayPublisher publisher; + + private final SwerveModule frontLeftModule; + private final SwerveModule frontRightModule; + private final SwerveModule backLeftModule; + private final SwerveModule backRightModule; + + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged gyroInputs = + new GyroIOInputsAutoLogged(); + + static final Lock odometryLock = new ReentrantLock(); + + /** + * Remember that the front of the robot is +X and the left side of the robot is + * +Y. + */ + public Drivetrain( + GyroIO gyroIO, + ModuleIO frontLeftModuleIO, + ModuleIO frontRightModuleIO, + ModuleIO backLeftModuleIO, + ModuleIO backRightModuleIO + ) { + this.gyroIO = gyroIO; + + this.frontLeftModule = new SwerveModule( + frontLeftModuleIO, + "Front Left" + ); + this.frontRightModule = new SwerveModule( + frontRightModuleIO, + "Front Right" + ); + this.backLeftModule = new SwerveModule(backLeftModuleIO, "Back Left"); + this.backRightModule = new SwerveModule( + backRightModuleIO, + "Back Right" + ); + + this.kinematics = new SwerveDriveKinematics( + new Translation2d( + DriveConstants.FRONT_LEFT.LocationX, + DriveConstants.FRONT_LEFT.LocationY + ), + new Translation2d( + DriveConstants.FRONT_RIGHT.LocationX, + DriveConstants.FRONT_RIGHT.LocationY + ), + new Translation2d( + DriveConstants.BACK_LEFT.LocationX, + DriveConstants.BACK_LEFT.LocationY + ), + new Translation2d( + DriveConstants.BACK_RIGHT.LocationX, + DriveConstants.BACK_RIGHT.LocationY + ) + ); + PhoenixOdometryThread.getInstance().start(); + + this.poseEstimator = new PoseEstimator8736( + this.kinematics, + Rotation2d.kZero, + Pose2d.kZero + ); + + this.desiredChassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); + } + + public void setDesiredState(ChassisSpeeds desiredChassisSpeeds) { + this.desiredChassisSpeeds = desiredChassisSpeeds; + } + + public ChassisSpeeds getDesiredState() { + return this.desiredChassisSpeeds; + } + + public SwerveDriveKinematics getKinematics() { + return this.kinematics; + } + + public SwerveModulePosition[] getModulePositions() { + return new SwerveModulePosition[] { + this.frontLeftModule.getModulePosition(), + this.frontRightModule.getModulePosition(), + this.backLeftModule.getModulePosition(), + this.backRightModule.getModulePosition(), + }; + } + + @Override + public void periodic() { + odometryLock.lock(); + gyroIO.updateInputs(gyroInputs); + Logger.processInputs("Drive/Gyro", gyroInputs); + + // update module inputs + this.frontLeftModule.periodic(); + this.frontRightModule.periodic(); + this.backLeftModule.periodic(); + this.backRightModule.periodic(); + + // Update odometry + double[] sampleTimestamps = + this.frontLeftModule.getOdometryTimestamps(); // All signals are sampled together + int sampleCount = sampleTimestamps.length; + for (int i = 0; i < sampleCount; i++) { + // Read wheel positions from each module + SwerveModulePosition[] modulePositions = + new SwerveModulePosition[4]; + modulePositions[0] = this.frontLeftModule.getOdometryPositions()[i]; + modulePositions[1] = + this.frontRightModule.getOdometryPositions()[i]; + modulePositions[2] = this.backLeftModule.getOdometryPositions()[i]; + modulePositions[3] = this.backRightModule.getOdometryPositions()[i]; + + // Update pose estimator with gyro rotation (or null to use kinematics) + Rotation2d gyroRotation = this.gyroInputs.connected + ? this.gyroInputs.odometryYawPositions[i] + : null; + this.poseEstimator.updateOdometry( + modulePositions, + gyroRotation, + sampleTimestamps[i] + ); + } + + // send the new desired states down to the modules + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize( + this.desiredChassisSpeeds, + CONSTANTS.ROBOT_LOOP_PERIOD + ); + SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates( + this.desiredChassisSpeeds + ); + SwerveDriveKinematics.desaturateWheelSpeeds( + moduleStates, + CONSTANTS.DriveConstants.SPEED_AT_12_VOLTS + ); + + Logger.recordOutput("SwerveStates/Setpoints", moduleStates); + Logger.recordOutput("SwerveChassisSpeeds/Setpoints", discreteSpeeds); + + SwerveModuleState frontLeftState = moduleStates[0]; + SwerveModuleState frontRightState = moduleStates[1]; + SwerveModuleState backLeftState = moduleStates[2]; + SwerveModuleState backRightState = moduleStates[3]; + + this.frontLeftModule.setModuleState(frontLeftState); + this.frontRightModule.setModuleState(frontRightState); + this.backLeftModule.setModuleState(backLeftState); + this.backRightModule.setModuleState(backRightState); + } + + public void zeroGyro() { + this.resetPose( + new Pose2d( + this.poseEstimator.getEstimatedPose().getTranslation(), + Rotation2d.kZero + ) + ); + } + + public void resetPose(Pose2d pose) { + this.poseEstimator.resetPose(pose, getModulePositions()); + } + + @AutoLogOutput(key = "Odometry/Robot") + public Pose2d getPose() { + return this.poseEstimator.getEstimatedPose(); + } + + @Override + public void simulationPeriodic() { + this.periodic(); + } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainController.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainController.java index 377f76d..f80ce0d 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainController.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainController.java @@ -1,25 +1,21 @@ package frc.robot.subsystems.drivetrain; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import frc.robot.PoseEstimator8736; public class DrivetrainController { - private final PoseEstimator8736 poseEstimator; - public DrivetrainController(PoseEstimator8736 poseEstimator) { - this.poseEstimator = poseEstimator; - } - - public ChassisSpeeds fieldToRobotChassisSpeeds(ChassisSpeeds fieldOriented) - { - double angle = 2*Math.PI*poseEstimator.getGyroYaw(); + private final Drivetrain drivetrain; - double cosA = Math.cos(angle); - double sinA = Math.sin(angle); + public DrivetrainController(Drivetrain drivetrain) { + this.drivetrain = drivetrain; + } - double vxRobot = fieldOriented.vxMetersPerSecond*cosA + fieldOriented.vyMetersPerSecond*sinA; - double vyRobot = -fieldOriented.vxMetersPerSecond*sinA + fieldOriented.vyMetersPerSecond*cosA; - - return new ChassisSpeeds(vxRobot, vyRobot, fieldOriented.omegaRadiansPerSecond); + public ChassisSpeeds fieldToRobotChassisSpeeds( + ChassisSpeeds fieldOriented + ) { + return ChassisSpeeds.fromFieldRelativeSpeeds( + fieldOriented, + drivetrain.getPose().getRotation() + ); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java b/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java new file mode 100644 index 0000000..c508c89 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java @@ -0,0 +1,18 @@ +package frc.robot.subsystems.drivetrain; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface GyroIO { + @AutoLog + public static class GyroIOInputs { + + public boolean connected = false; + public Rotation2d yawPosition = Rotation2d.kZero; + public double yawVelocityRadPerSec = 0.0; + public double[] odometryYawTimestamps = new double[] {}; + public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; + } + + public default void updateInputs(GyroIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/GyroIORedux.java b/src/main/java/frc/robot/subsystems/drivetrain/GyroIORedux.java new file mode 100644 index 0000000..b9bfdaf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/GyroIORedux.java @@ -0,0 +1,65 @@ +package frc.robot.subsystems.drivetrain; + +import com.reduxrobotics.sensors.canandgyro.Canandgyro; +import com.reduxrobotics.sensors.canandgyro.CanandgyroSettings; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import frc.robot.CONSTANTS; +import frc.robot.CONSTANTS.DriveConstants; +import frc.robot.CONSTANTS.Timeouts; +import java.util.Queue; + +public class GyroIORedux implements GyroIO { + + private final Canandgyro gyro = new Canandgyro(CONSTANTS.GYRO_CAN_ID); + + private final Queue yawTimestampQueue; + private final Queue yawPositionQueue; + + public GyroIORedux() { + // Configure the gyro + CanandgyroSettings settings = new CanandgyroSettings() + .setYawFramePeriod(1.0 / DriveConstants.ODOMETRY_FREQUENCY) + .setAngularPositionFramePeriod( + DriveConstants.GRYO_CAN_FRAME_FREQUENCY + ) + .setAngularVelocityFramePeriod( + DriveConstants.GRYO_CAN_FRAME_FREQUENCY + ); + gyro.setSettings( + settings, + Timeouts.STD_TIMEOUT_LONG, + Timeouts.STD_RETRY_ATTEMPTS + ); + gyro.setYaw(0.0, Timeouts.STD_TIMEOUT); + gyro.clearStickyFaults(); + + // Register the gyro signals + yawTimestampQueue = + PhoenixOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal( + gyro::getYaw + ); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = gyro.isConnected(); + inputs.yawPosition = Rotation2d.fromRotations(gyro.getYaw()); + inputs.yawVelocityRadPerSec = Units.rotationsToRadians( + gyro.getAngularVelocityYaw() + ); + + inputs.odometryYawTimestamps = yawTimestampQueue + .stream() + .mapToDouble((Double value) -> value) + .toArray(); + inputs.odometryYawPositions = yawPositionQueue + .stream() + .map(Rotation2d::fromRotations) + .toArray(Rotation2d[]::new); + + yawTimestampQueue.clear(); + yawPositionQueue.clear(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/ModuleIO.java b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIO.java new file mode 100644 index 0000000..1227ac3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIO.java @@ -0,0 +1,43 @@ +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 = Rotation2d.kZero; + public Rotation2d turnPosition = Rotation2d.kZero; + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double turnCurrentAmps = 0.0; + + public double[] odometryTimestamps = new double[] {}; + public double[] odometryDrivePositionsRad = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} + + /** Run the drive motor at the specified open loop value. */ + public default void setDriveOpenLoop(double output) {} + + /** Run the turn motor at the specified open loop value. */ + public default void setTurnOpenLoop(double output) {} + + /** Run the drive motor at the specified velocity. */ + public default void setDriveVelocity(double velocityRadPerSec) {} + + /** Run the turn motor to the specified rotation. */ + public default void setTurnPosition(Rotation2d rotation) {} +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIOSim.java new file mode 100644 index 0000000..e47c416 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIOSim.java @@ -0,0 +1,162 @@ +package frc.robot.subsystems.drivetrain; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +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.Timer; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.CONSTANTS; + +/** + * Physics sim implementation of module IO. The sim models are configured using a set of module + * constants from Phoenix. Simulation is always based on voltage control. + */ +public class ModuleIOSim implements ModuleIO { + + // TunerConstants doesn't support separate sim constants, so they are declared locally + private static final double DRIVE_KP = 0.05; + private static final double DRIVE_KD = 0.0; + private static final double DRIVE_KS = 0.0; + private static final double DRIVE_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation + private static final double DRIVE_KV = + 1.0 / Units.rotationsToRadians(1.0 / DRIVE_KV_ROT); + private static final double TURN_KP = 8.0; + private static final double TURN_KD = 0.0; + private static final DCMotor DRIVE_GEARBOX = DCMotor.getKrakenX60Foc(1); + private static final DCMotor TURN_GEARBOX = DCMotor.getKrakenX60Foc(1); + + private final DCMotorSim driveSim; + private final DCMotorSim turnSim; + + private boolean driveClosedLoop = false; + private boolean turnClosedLoop = false; + private PIDController driveController = new PIDController( + DRIVE_KP, + 0, + DRIVE_KD + ); + private PIDController turnController = new PIDController( + TURN_KP, + 0, + TURN_KD + ); + private double driveFFVolts = 0.0; + private double driveAppliedVolts = 0.0; + private double turnAppliedVolts = 0.0; + + public ModuleIOSim( + SwerveModuleConstants< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > constants + ) { + // Create drive and turn sim models + driveSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DRIVE_GEARBOX, + constants.DriveInertia, + constants.DriveMotorGearRatio + ), + DRIVE_GEARBOX + ); + turnSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem( + TURN_GEARBOX, + constants.SteerInertia, + constants.SteerMotorGearRatio + ), + TURN_GEARBOX + ); + + // Enable wrapping for turn PID + turnController.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Run closed-loop control + if (driveClosedLoop) { + driveAppliedVolts = + driveFFVolts + + driveController.calculate( + driveSim.getAngularVelocityRadPerSec() + ); + } else { + driveController.reset(); + } + if (turnClosedLoop) { + turnAppliedVolts = turnController.calculate( + turnSim.getAngularPositionRad() + ); + } else { + turnController.reset(); + } + + // Update simulation state + driveSim.setInputVoltage( + MathUtil.clamp(driveAppliedVolts, -12.0, 12.0) + ); + turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0)); + driveSim.update(CONSTANTS.ROBOT_LOOP_PERIOD); + turnSim.update(CONSTANTS.ROBOT_LOOP_PERIOD); + + // Update drive inputs + inputs.driveConnected = true; + inputs.drivePositionRad = driveSim.getAngularPositionRad(); + inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); + + // Update turn inputs + inputs.turnConnected = true; + inputs.turnEncoderConnected = true; + inputs.turnAbsolutePosition = new Rotation2d( + turnSim.getAngularPositionRad() + ); + inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); + inputs.turnAppliedVolts = turnAppliedVolts; + inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); + + // Update odometry inputs (50Hz because high-frequency odometry in sim doesn't matter) + inputs.odometryTimestamps = new double[] { Timer.getFPGATimestamp() }; + inputs.odometryDrivePositionsRad = new double[] { + inputs.drivePositionRad, + }; + inputs.odometryTurnPositions = new Rotation2d[] { inputs.turnPosition }; + } + + @Override + public void setDriveOpenLoop(double output) { + driveClosedLoop = false; + driveAppliedVolts = output; + } + + @Override + public void setTurnOpenLoop(double output) { + turnClosedLoop = false; + turnAppliedVolts = output; + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + driveClosedLoop = true; + driveFFVolts = + DRIVE_KS * Math.signum(velocityRadPerSec) + + DRIVE_KV * velocityRadPerSec; + driveController.setSetpoint(velocityRadPerSec); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + turnClosedLoop = true; + turnController.setSetpoint(rotation.getRadians()); + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIOTalonFX.java new file mode 100644 index 0000000..69d97e9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIOTalonFX.java @@ -0,0 +1,348 @@ +package frc.robot.subsystems.drivetrain; + +import static frc.robot.util.PhoenixUtil.tryUntilOk; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import edu.wpi.first.math.filter.Debouncer; +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.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.CONSTANTS.DriveConstants; +import frc.robot.CONSTANTS.Timeouts; +import java.util.Queue; + +/** + * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and + * CANcoder. Configured using a set of module constants from Phoenix. + * + *

Device configuration and other behaviors not exposed by TunerConstants can be customized here. + */ +public class ModuleIOTalonFX implements ModuleIO { + + private final SwerveModuleConstants< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > constants; + + // Hardware objects + private final TalonFX driveTalon; + private final TalonFX turnTalon; + private final CANcoder cancoder; + + // Voltage control requests + private final VoltageOut voltageRequest = new VoltageOut(0); + private final PositionVoltage positionVoltageRequest = new PositionVoltage( + 0.0 + ); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage( + 0.0 + ); + + // Torque-current control requests + private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC( + 0 + ); + private final PositionTorqueCurrentFOC positionTorqueCurrentRequest = + new PositionTorqueCurrentFOC(0.0); + private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + new VelocityTorqueCurrentFOC(0.0); + + // Timestamp inputs from Phoenix thread + private final Queue timestampQueue; + + // Inputs from drive motor + private final StatusSignal drivePosition; + private final Queue drivePositionQueue; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + // Inputs from turn motor + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnPosition; + private final Queue turnPositionQueue; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + // Connection debouncers + private final Debouncer driveConnectedDebounce = new Debouncer( + Timeouts.STD_DEBOUNCE_TIME, + Debouncer.DebounceType.kFalling + ); + private final Debouncer turnConnectedDebounce = new Debouncer( + Timeouts.STD_DEBOUNCE_TIME, + Debouncer.DebounceType.kFalling + ); + private final Debouncer turnEncoderConnectedDebounce = new Debouncer( + Timeouts.STD_DEBOUNCE_TIME, + Debouncer.DebounceType.kFalling + ); + + public ModuleIOTalonFX( + SwerveModuleConstants< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > constants + ) { + this.constants = constants; + this.driveTalon = new TalonFX( + constants.DriveMotorId, + DriveConstants.DRIVETRAIN_CONSTANTS.CANBusName + ); + this.turnTalon = new TalonFX( + constants.SteerMotorId, + DriveConstants.DRIVETRAIN_CONSTANTS.CANBusName + ); + this.cancoder = new CANcoder( + constants.EncoderId, + DriveConstants.DRIVETRAIN_CONSTANTS.CANBusName + ); + + // Configure drive motor + var driveConfig = constants.DriveMotorInitialConfigs; + driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + driveConfig.Slot0 = constants.DriveMotorGains; + driveConfig.Feedback.SensorToMechanismRatio = + constants.DriveMotorGearRatio; + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = + constants.SlipCurrent; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = + -constants.SlipCurrent; + driveConfig.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent; + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.MotorOutput.Inverted = constants.DriveMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + tryUntilOk(5, () -> + this.driveTalon.getConfigurator().apply(driveConfig, 0.25) + ); + tryUntilOk(5, () -> this.driveTalon.setPosition(0.0, 0.25)); + + // Configure turn motor + var turnConfig = new TalonFXConfiguration(); + turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + turnConfig.Slot0 = constants.SteerMotorGains; + turnConfig.Feedback.FeedbackRemoteSensorID = constants.EncoderId; + turnConfig.Feedback.FeedbackSensorSource = switch ( + constants.FeedbackSource + ) { + case RemoteCANcoder -> FeedbackSensorSourceValue.RemoteCANcoder; + case FusedCANcoder -> FeedbackSensorSourceValue.FusedCANcoder; + case SyncCANcoder -> FeedbackSensorSourceValue.SyncCANcoder; + default -> throw new RuntimeException( + "You have selected a turn feedback source that is not supported by the default implementation of ModuleIOTalonFX. Please check the AdvantageKit documentation for more information on alternative configurations: https://docs.advantagekit.org/getting-started/template-projects/talonfx-swerve-template#custom-module-implementations" + ); + }; + turnConfig.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicCruiseVelocity = + DriveConstants.MM_CRUISE_VELOCITY; + turnConfig.MotionMagic.MotionMagicAcceleration = + DriveConstants.MM_ACCELERATION; + turnConfig.MotionMagic.MotionMagicExpo_kV = DriveConstants.MM_EXPO_KV; + turnConfig.MotionMagic.MotionMagicExpo_kA = DriveConstants.MM_EXPO_KA; + turnConfig.ClosedLoopGeneral.ContinuousWrap = true; // ALWAYS TRUE FOR STEER MOTOR + turnConfig.MotorOutput.Inverted = constants.SteerMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + tryUntilOk(5, () -> + this.turnTalon.getConfigurator().apply(turnConfig, 0.25) + ); + + // Configure CANCoder + CANcoderConfiguration cancoderConfig = constants.EncoderInitialConfigs; + cancoderConfig.MagnetSensor.MagnetOffset = constants.EncoderOffset; + cancoderConfig.MagnetSensor.SensorDirection = constants.EncoderInverted + ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive; + this.cancoder.getConfigurator().apply(cancoderConfig); + + // Create timestamp queue + this.timestampQueue = + PhoenixOdometryThread.getInstance().makeTimestampQueue(); + + // Create drive status signals + this.drivePosition = this.driveTalon.getPosition(); + this.drivePositionQueue = + PhoenixOdometryThread.getInstance().registerSignal( + this.drivePosition.clone() + ); + this.driveVelocity = this.driveTalon.getVelocity(); + this.driveAppliedVolts = this.driveTalon.getMotorVoltage(); + this.driveCurrent = this.driveTalon.getStatorCurrent(); + + // Create turn status signals + this.turnAbsolutePosition = this.cancoder.getAbsolutePosition(); + this.turnPosition = this.turnTalon.getPosition(); + this.turnPositionQueue = + PhoenixOdometryThread.getInstance().registerSignal( + this.turnPosition.clone() + ); + this.turnVelocity = this.turnTalon.getVelocity(); + this.turnAppliedVolts = this.turnTalon.getMotorVoltage(); + this.turnCurrent = this.turnTalon.getStatorCurrent(); + + // Configure periodic frames + BaseStatusSignal.setUpdateFrequencyForAll( + DriveConstants.ODOMETRY_FREQUENCY, + this.drivePosition, + this.turnPosition + ); + BaseStatusSignal.setUpdateFrequencyForAll( + DriveConstants.DRIVE_CAN_FRAME_FREQUENCY, + this.driveVelocity, + this.driveAppliedVolts, + this.driveCurrent, + this.turnAbsolutePosition, + this.turnVelocity, + this.turnAppliedVolts, + this.turnCurrent + ); + ParentDevice.optimizeBusUtilizationForAll( + this.driveTalon, + this.turnTalon + ); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Refresh all signals + var driveStatus = BaseStatusSignal.refreshAll( + this.drivePosition, + this.driveVelocity, + this.driveAppliedVolts, + this.driveCurrent + ); + var turnStatus = BaseStatusSignal.refreshAll( + this.turnPosition, + this.turnVelocity, + this.turnAppliedVolts, + this.turnCurrent + ); + var turnEncoderStatus = BaseStatusSignal.refreshAll( + this.turnAbsolutePosition + ); + + // Update drive inputs + inputs.driveConnected = this.driveConnectedDebounce.calculate( + driveStatus.isOK() + ); + inputs.drivePositionRad = Units.rotationsToRadians( + this.drivePosition.getValueAsDouble() + ); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians( + this.driveVelocity.getValueAsDouble() + ); + inputs.driveAppliedVolts = this.driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = this.driveCurrent.getValueAsDouble(); + + // Update turn inputs + inputs.turnConnected = this.turnConnectedDebounce.calculate( + turnStatus.isOK() + ); + inputs.turnEncoderConnected = + this.turnEncoderConnectedDebounce.calculate( + turnEncoderStatus.isOK() + ); + inputs.turnAbsolutePosition = Rotation2d.fromRotations( + this.turnAbsolutePosition.getValueAsDouble() + ); + inputs.turnPosition = Rotation2d.fromRotations( + this.turnPosition.getValueAsDouble() + ); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians( + this.turnVelocity.getValueAsDouble() + ); + inputs.turnAppliedVolts = this.turnAppliedVolts.getValueAsDouble(); + inputs.turnCurrentAmps = this.turnCurrent.getValueAsDouble(); + + // Update odometry inputs + inputs.odometryTimestamps = this.timestampQueue.stream() + .mapToDouble((Double value) -> value) + .toArray(); + inputs.odometryDrivePositionsRad = this.drivePositionQueue.stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value)) + .toArray(); + inputs.odometryTurnPositions = this.turnPositionQueue.stream() + .map((Double value) -> Rotation2d.fromRotations(value)) + .toArray(Rotation2d[]::new); + this.timestampQueue.clear(); + this.drivePositionQueue.clear(); + this.turnPositionQueue.clear(); + } + + @Override + public void setDriveOpenLoop(double output) { + this.driveTalon.setControl( + switch (this.constants.DriveMotorClosedLoopOutput) { + case Voltage -> this.voltageRequest.withOutput(output); + case TorqueCurrentFOC -> this.torqueCurrentRequest.withOutput( + output + ); + } + ); + } + + @Override + public void setTurnOpenLoop(double output) { + this.turnTalon.setControl( + switch (this.constants.SteerMotorClosedLoopOutput) { + case Voltage -> this.voltageRequest.withOutput(output); + case TorqueCurrentFOC -> this.torqueCurrentRequest.withOutput( + output + ); + } + ); + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); + this.driveTalon.setControl( + switch (this.constants.DriveMotorClosedLoopOutput) { + case Voltage -> this.velocityVoltageRequest.withVelocity( + velocityRotPerSec + ); + case TorqueCurrentFOC -> this.velocityTorqueCurrentRequest.withVelocity( + velocityRotPerSec + ); + } + ); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + this.turnTalon.setControl( + switch (this.constants.SteerMotorClosedLoopOutput) { + case Voltage -> this.positionVoltageRequest.withPosition( + rotation.getRotations() + ); + case TorqueCurrentFOC -> this.positionTorqueCurrentRequest.withPosition( + rotation.getRotations() + ); + } + ); + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/ModuleIOTalonFXRedux.java b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIOTalonFXRedux.java new file mode 100644 index 0000000..4ae0baf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIOTalonFXRedux.java @@ -0,0 +1,364 @@ +package frc.robot.subsystems.drivetrain; + +import static frc.robot.util.PhoenixUtil.tryUntilOk; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.reduxrobotics.frames.Frame; +import com.reduxrobotics.frames.FrameData; +import com.reduxrobotics.sensors.canandmag.Canandmag; +import com.reduxrobotics.sensors.canandmag.CanandmagSettings; +import edu.wpi.first.math.filter.Debouncer; +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.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.CONSTANTS; +import frc.robot.CONSTANTS.DriveConstants; +import frc.robot.CONSTANTS.Timeouts; +import java.util.Queue; + +/** + * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and + * CANcoder. Configured using a set of module constants from Phoenix. + * + * This class has been modifed to use the Redux Can encoder rather than a ctre encoder. + */ +public class ModuleIOTalonFXRedux implements ModuleIO { + + private final SwerveModuleConstants< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > constants; + + // Hardware objects + private final TalonFX driveTalon; + private final TalonFX turnTalon; + private final Canandmag cancoder; + + // Voltage control requests + private final VoltageOut voltageRequest = new VoltageOut(0); + private final PositionVoltage positionVoltageRequest = new PositionVoltage( + 0.0 + ); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage( + 0.0 + ); + + // Torque-current control requests + private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC( + 0 + ); + private final PositionTorqueCurrentFOC positionTorqueCurrentRequest = + new PositionTorqueCurrentFOC(0.0); + private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + new VelocityTorqueCurrentFOC(0.0); + + // Timestamp inputs from Phoenix thread + private final Queue timestampQueue; + + // Inputs from drive motor + private final StatusSignal drivePosition; + private final Queue drivePositionQueue; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + // Inputs from turn motor + private final Frame turnAbsolutePosition; + private final StatusSignal turnPosition; + private final Queue turnPositionQueue; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + // Connection debouncers + private final Debouncer driveConnectedDebounce = new Debouncer( + Timeouts.STD_DEBOUNCE_TIME, + Debouncer.DebounceType.kFalling + ); + private final Debouncer turnConnectedDebounce = new Debouncer( + Timeouts.STD_DEBOUNCE_TIME, + Debouncer.DebounceType.kFalling + ); + private final Debouncer turnEncoderConnectedDebounce = new Debouncer( + Timeouts.STD_DEBOUNCE_TIME, + Debouncer.DebounceType.kFalling + ); + + public ModuleIOTalonFXRedux( + SwerveModuleConstants< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > constants + ) { + this.constants = constants; + this.driveTalon = new TalonFX( + constants.DriveMotorId, + DriveConstants.DRIVETRAIN_CONSTANTS.CANBusName + ); + this.turnTalon = new TalonFX( + constants.SteerMotorId, + DriveConstants.DRIVETRAIN_CONSTANTS.CANBusName + ); + this.cancoder = new Canandmag(constants.EncoderId); + + // Configure drive motor + var driveConfig = constants.DriveMotorInitialConfigs; + driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + driveConfig.Slot0 = constants.DriveMotorGains; + driveConfig.Feedback.SensorToMechanismRatio = + constants.DriveMotorGearRatio; + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = + constants.SlipCurrent; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = + -constants.SlipCurrent; + driveConfig.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent; + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.MotorOutput.Inverted = constants.DriveMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + tryUntilOk(5, () -> + this.driveTalon.getConfigurator().apply(driveConfig, 0.25) + ); + tryUntilOk(5, () -> this.driveTalon.setPosition(0.0, 0.25)); + + // Configure turn motor + var turnConfig = new TalonFXConfiguration(); + turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + turnConfig.Slot0 = constants.SteerMotorGains; + turnConfig.Feedback.FeedbackRemoteSensorID = constants.EncoderId; + + // Use just the steering motor encoder, as CTRE does not support uing fused encoders + // with third party encoders + turnConfig.Feedback.FeedbackSensorSource = + FeedbackSensorSourceValue.RotorSensor; + + // Update to use the gear ration of the steer motor rather than the steer ration + // after the can encoder + turnConfig.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicCruiseVelocity = + DriveConstants.MM_CRUISE_VELOCITY; + turnConfig.MotionMagic.MotionMagicAcceleration = + DriveConstants.MM_ACCELERATION; + turnConfig.MotionMagic.MotionMagicExpo_kV = DriveConstants.MM_EXPO_KV; + turnConfig.MotionMagic.MotionMagicExpo_kA = DriveConstants.MM_EXPO_KA; + turnConfig.ClosedLoopGeneral.ContinuousWrap = true; // ALWAYS TRUE FOR STEER MOTOR + turnConfig.MotorOutput.Inverted = constants.SteerMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + tryUntilOk(5, () -> + this.turnTalon.getConfigurator().apply(turnConfig, 0.25) + ); + + // Configure CANCoder + cancoder.setAbsPosition( + constants.EncoderOffset, + CONSTANTS.Timeouts.STD_TIMEOUT_LONG + ); + CanandmagSettings settings = new CanandmagSettings(); + settings.setInvertDirection(constants.EncoderInverted); + settings.setPositionFramePeriod( + DriveConstants.DRIVE_CAN_FRAME_PERIOD_SEC + ); + cancoder.setSettings(settings, CONSTANTS.Timeouts.STD_TIMEOUT_LONG); + + // Create timestamp queue + this.timestampQueue = + PhoenixOdometryThread.getInstance().makeTimestampQueue(); + + // Create drive status signals + this.drivePosition = this.driveTalon.getPosition(); + this.drivePositionQueue = + PhoenixOdometryThread.getInstance().registerSignal( + this.drivePosition.clone() + ); + this.driveVelocity = this.driveTalon.getVelocity(); + this.driveAppliedVolts = this.driveTalon.getMotorVoltage(); + this.driveCurrent = this.driveTalon.getStatorCurrent(); + + // Create turn status signals + this.turnAbsolutePosition = this.cancoder.getAbsPositionFrame(); + this.turnPosition = this.turnTalon.getPosition(); + this.turnPositionQueue = + PhoenixOdometryThread.getInstance().registerSignal( + this.turnPosition.clone() + ); + this.turnVelocity = this.turnTalon.getVelocity(); + this.turnAppliedVolts = this.turnTalon.getMotorVoltage(); + this.turnCurrent = this.turnTalon.getStatorCurrent(); + + // Configure periodic frames + BaseStatusSignal.setUpdateFrequencyForAll( + DriveConstants.ODOMETRY_FREQUENCY, + this.drivePosition, + this.turnPosition + ); + BaseStatusSignal.setUpdateFrequencyForAll( + DriveConstants.DRIVE_CAN_FRAME_FREQUENCY, + this.driveVelocity, + this.driveAppliedVolts, + this.driveCurrent, + this.turnVelocity, + this.turnAppliedVolts, + this.turnCurrent + ); + ParentDevice.optimizeBusUtilizationForAll( + this.driveTalon, + this.turnTalon + ); + + // Reset the turn motor position based on the can encoder + // Cast is safe as defined in the reducx docs + FrameData data = (FrameData) (Frame.waitForFrames( + Timeouts.STD_TIMEOUT_LONG, + this.turnAbsolutePosition + )[0]); + tryUntilOk(10, () -> + turnTalon.setPosition( + data.getValue(), + CONSTANTS.Timeouts.STD_TIMEOUT_LONG + ) + ); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Refresh all signals + var driveStatus = BaseStatusSignal.refreshAll( + this.drivePosition, + this.driveVelocity, + this.driveAppliedVolts, + this.driveCurrent + ); + var turnStatus = BaseStatusSignal.refreshAll( + this.turnPosition, + this.turnVelocity, + this.turnAppliedVolts, + this.turnCurrent + ); + var turnEncoderStatus = Frame.waitForFrames( + Timeouts.STD_TIMEOUT, + this.turnAbsolutePosition + ); + + // Update drive inputs + inputs.driveConnected = this.driveConnectedDebounce.calculate( + driveStatus.isOK() + ); + inputs.drivePositionRad = Units.rotationsToRadians( + this.drivePosition.getValueAsDouble() + ); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians( + this.driveVelocity.getValueAsDouble() + ); + inputs.driveAppliedVolts = this.driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = this.driveCurrent.getValueAsDouble(); + + // Update turn inputs + inputs.turnConnected = this.turnConnectedDebounce.calculate( + turnStatus.isOK() + ); + inputs.turnEncoderConnected = + this.turnEncoderConnectedDebounce.calculate( + turnEncoderStatus != null + ); + inputs.turnAbsolutePosition = Rotation2d.fromRotations( + this.turnAbsolutePosition.getValue() + ); + inputs.turnPosition = Rotation2d.fromRotations( + this.turnPosition.getValueAsDouble() + ); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians( + this.turnVelocity.getValueAsDouble() + ); + inputs.turnAppliedVolts = this.turnAppliedVolts.getValueAsDouble(); + inputs.turnCurrentAmps = this.turnCurrent.getValueAsDouble(); + + // Update odometry inputs + inputs.odometryTimestamps = this.timestampQueue.stream() + .mapToDouble((Double value) -> value) + .toArray(); + inputs.odometryDrivePositionsRad = this.drivePositionQueue.stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value)) + .toArray(); + inputs.odometryTurnPositions = this.turnPositionQueue.stream() + .map((Double value) -> Rotation2d.fromRotations(value)) + .toArray(Rotation2d[]::new); + this.timestampQueue.clear(); + this.drivePositionQueue.clear(); + this.turnPositionQueue.clear(); + } + + @Override + public void setDriveOpenLoop(double output) { + this.driveTalon.setControl( + switch (this.constants.DriveMotorClosedLoopOutput) { + case Voltage -> this.voltageRequest.withOutput(output); + case TorqueCurrentFOC -> this.torqueCurrentRequest.withOutput( + output + ); + } + ); + } + + @Override + public void setTurnOpenLoop(double output) { + this.turnTalon.setControl( + switch (this.constants.SteerMotorClosedLoopOutput) { + case Voltage -> this.voltageRequest.withOutput(output); + case TorqueCurrentFOC -> this.torqueCurrentRequest.withOutput( + output + ); + } + ); + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); + this.driveTalon.setControl( + switch (this.constants.DriveMotorClosedLoopOutput) { + case Voltage -> this.velocityVoltageRequest.withVelocity( + velocityRotPerSec + ); + case TorqueCurrentFOC -> this.velocityTorqueCurrentRequest.withVelocity( + velocityRotPerSec + ); + } + ); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + this.turnTalon.setControl( + switch (this.constants.SteerMotorClosedLoopOutput) { + case Voltage -> this.positionVoltageRequest.withPosition( + rotation.getRotations() + ); + case TorqueCurrentFOC -> this.positionTorqueCurrentRequest.withPosition( + rotation.getRotations() + ); + } + ); + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drivetrain/PhoenixOdometryThread.java new file mode 100644 index 0000000..c93cd9c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/PhoenixOdometryThread.java @@ -0,0 +1,179 @@ +// Copyright (c) 2021-2025 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.drivetrain; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.CONSTANTS.DriveConstants; +import java.util.ArrayList; +import java.util.List; +import java.util.Queue; +import java.util.concurrent.ArrayBlockingQueue; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.DoubleSupplier; + +/** + * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. + * + *

This version is intended for Phoenix 6 devices on both the RIO and CANivore buses. When using + * a CANivore, the thread uses the "waitForAll" blocking method to enable more consistent sampling. + * This also allows Phoenix Pro users to benefit from lower latency between devices using CANivore + * time synchronization. + */ +public class PhoenixOdometryThread extends Thread { + + private final Lock signalsLock = new ReentrantLock(); // Prevents conflicts when registering signals + private BaseStatusSignal[] phoenixSignals = new BaseStatusSignal[0]; + private final List genericSignals = new ArrayList<>(); + private final List> phoenixQueues = new ArrayList<>(); + private final List> genericQueues = new ArrayList<>(); + private final List> timestampQueues = new ArrayList<>(); + + private static boolean isCANFD = new CANBus( + DriveConstants.DRIVETRAIN_CONSTANTS.CANBusName + ).isNetworkFD(); + 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 (this.timestampQueues.size() > 0) { + super.start(); + } + } + + /** Registers a Phoenix signal to be read from the thread. */ + public Queue registerSignal(StatusSignal signal) { + Queue queue = new ArrayBlockingQueue<>(20); + this.signalsLock.lock(); + Drivetrain.odometryLock.lock(); + try { + BaseStatusSignal[] newSignals = + new BaseStatusSignal[this.phoenixSignals.length + 1]; + System.arraycopy( + this.phoenixSignals, + 0, + newSignals, + 0, + this.phoenixSignals.length + ); + newSignals[this.phoenixSignals.length] = signal; + this.phoenixSignals = newSignals; + this.phoenixQueues.add(queue); + } finally { + this.signalsLock.unlock(); + Drivetrain.odometryLock.unlock(); + } + return queue; + } + + /** Registers a generic signal to be read from the thread. */ + public Queue registerSignal(DoubleSupplier signal) { + Queue queue = new ArrayBlockingQueue<>(20); + this.signalsLock.lock(); + Drivetrain.odometryLock.lock(); + try { + this.genericSignals.add(signal); + this.genericQueues.add(queue); + } finally { + this.signalsLock.unlock(); + Drivetrain.odometryLock.unlock(); + } + return queue; + } + + /** Returns a new queue that returns timestamp values for each sample. */ + public Queue makeTimestampQueue() { + Queue queue = new ArrayBlockingQueue<>(20); + Drivetrain.odometryLock.lock(); + try { + this.timestampQueues.add(queue); + } finally { + Drivetrain.odometryLock.unlock(); + } + return queue; + } + + @Override + public void run() { + while (true) { + // Wait for updates from all signals + this.signalsLock.lock(); + try { + if (isCANFD && this.phoenixSignals.length > 0) { + BaseStatusSignal.waitForAll( + 2.0 / DriveConstants.ODOMETRY_FREQUENCY, + this.phoenixSignals + ); + } else { + // "waitForAll" does not support blocking on multiple signals with a bus + // that is not CAN FD, regardless of Pro licensing. No reasoning for this + // behavior is provided by the documentation. + Thread.sleep( + (long) (1000.0 / DriveConstants.ODOMETRY_FREQUENCY) + ); + if (this.phoenixSignals.length > 0) { + BaseStatusSignal.refreshAll(this.phoenixSignals); + } + } + } catch (InterruptedException e) { + e.printStackTrace(); + } finally { + this.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 : this.phoenixSignals) { + totalLatency += signal.getTimestamp().getLatency(); + } + if (this.phoenixSignals.length > 0) { + timestamp -= totalLatency / this.phoenixSignals.length; + } + + // Add new samples to queues + for (int i = 0; i < this.phoenixSignals.length; i++) { + this.phoenixQueues.get(i).offer( + this.phoenixSignals[i].getValueAsDouble() + ); + } + for (int i = 0; i < this.genericSignals.size(); i++) { + this.genericQueues.get(i).offer( + this.genericSignals.get(i).getAsDouble() + ); + } + for (int i = 0; i < this.timestampQueues.size(); i++) { + this.timestampQueues.get(i).offer(timestamp); + } + } finally { + Drivetrain.odometryLock.unlock(); + } + } + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java index 7b84e06..2f1fe4d 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -1,107 +1,69 @@ package frc.robot.subsystems.drivetrain; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.ControlRequest; -import com.ctre.phoenix6.controls.PositionDutyCycle; -import com.ctre.phoenix6.controls.VelocityDutyCycle; -import com.ctre.phoenix6.hardware.TalonFX; -import com.reduxrobotics.sensors.canandmag.Canandmag; -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.smartdashboard.SmartDashboard; +import static edu.wpi.first.units.Units.Meters; import static frc.robot.CONSTANTS.*; -public class SwerveModule { - private static final boolean OUTPUT_TO_SMART_DASH = true; - - private final int steeringMotorCANId; - - private final TalonFX steeringMotor; - private final TalonFX driveMotor; - - private final Canandmag encoder; - - public SwerveModule(int steeringMotorCANId, int driveMotorCANId, int encoderCANId) { - this.steeringMotorCANId = steeringMotorCANId; - - this.steeringMotor = new TalonFX(steeringMotorCANId); - this.driveMotor = new TalonFX(driveMotorCANId); - this.encoder = new Canandmag(encoderCANId); +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import frc.robot.CONSTANTS; - var steeringConfigs = new TalonFXConfiguration(); - steeringConfigs.Slot0.kP = 0.16; - steeringConfigs.Slot0.kI = 0.0; - steeringConfigs.Slot0.kD = 0.0; - steeringConfigs.Slot0.kV = 0.0; // feedforward term +import org.littletonrobotics.junction.Logger; - //experimenting with motion magic to smooth steering - /* var motionMagicConfigs = steeringConfigs.MotionMagic; - motionMagicConfigs.MotionMagicCruiseVelocity = 500; //Target cruise velocity of 500rps - motionMagicConfigs.MotionMagicAcceleration = 1000; //Target acceleration of 1000rps/s (0.5 sec) - motionMagicConfigs.MotionMagicJerk = 0; //Target jerk of 0 rps/s/s (0 sec) */ +public class SwerveModule { - this.steeringMotor.getConfigurator().apply(steeringConfigs); + private final ModuleIO io; + private final ModuleIOInputsAutoLogged inputs = + new ModuleIOInputsAutoLogged(); + private final String name; - var driveConfigs = new TalonFXConfiguration(); - driveConfigs.Slot0.kP = 0.02; - driveConfigs.Slot0.kI = 0.0; - driveConfigs.Slot0.kD = 0.0; - driveConfigs.Slot0.kV = 0.0; // feedforward term - this.driveMotor.getConfigurator().apply(driveConfigs); + public SwerveModule(ModuleIO io, String name) { + this.io = io; + this.name = name; } - public void setModuleToEncoder() { - // set the internal encoder based on the absolution position of the external encoder - double encPosition = this.encoder.getAbsPosition(); // 0.0 to 1.0, inclusive, increasing counterclockwise - this.steeringMotor.setPosition(STEERING_GEAR_RATIO*encPosition); - - // set the steering motor to the current position so it doesn't try to move - double positionInRotations = STEERING_GEAR_RATIO*encPosition; - ControlRequest steeringControlRequest = new PositionDutyCycle(positionInRotations); - this.steeringMotor.setControl(steeringControlRequest); + public void periodic() { + // Poll for new hardware inputs + io.updateInputs(inputs); + Logger.processInputs("Drive/Module" + name, inputs); } public SwerveModulePosition getModulePosition() { - double positionOfSteeringRad = 2*Math.PI*this.steeringMotor.getPosition().getValueAsDouble() / STEERING_GEAR_RATIO; - double wheelRotations = this.driveMotor.getPosition().getValueAsDouble(); return new SwerveModulePosition( - wheelRotations / EFFECTIVE_GEAR_RATIO, new Rotation2d(-positionOfSteeringRad)); + inputs.drivePositionRad * CONSTANTS.DriveConstants.WHEEL_RADIUS.in(Meters), + inputs.turnPosition + ); } public void setModuleState(SwerveModuleState state) { - // get the current position of the steering motor and optimize the state - SwerveModulePosition currentPosition = getModulePosition(); - state.optimize(currentPosition.angle); - - // set the position of the steering motor - // remember that angle is the negative of what the motors want, hence the minus - double positionInRotations = -STEERING_GEAR_RATIO*state.angle.getDegrees()/360.0; - ControlRequest steeringControlRequest = new PositionDutyCycle(positionInRotations); - this.steeringMotor.setControl(steeringControlRequest); + // get the current position and optimize the state + state.optimize(inputs.turnPosition); // calculate a speed scale factor (cosine compensation) - double scaleFactor = state.angle.minus(currentPosition.angle).getCos(); - - // set the speed of the drive motor - ControlRequest driveControlRequest = new VelocityDutyCycle( - EFFECTIVE_GEAR_RATIO*state.speedMetersPerSecond*scaleFactor); - this.driveMotor.setControl(driveControlRequest); + double scaleFactor = state.angle.minus(inputs.turnPosition).getCos(); - // this is probably not the best place for this code, but this is a sandbox project + // set the drive velocity (convert m/s to rad/s) + double driveVelocityRadPerSec = + (state.speedMetersPerSecond * scaleFactor) / DriveConstants.WHEEL_RADIUS.in(Meters); + io.setDriveVelocity(driveVelocityRadPerSec); - // if (OUTPUT_TO_SMART_DASH) { - // SmartDashboard.putNumber( - // "Swerve States/" + this.steeringMotorCANId + "/Drive/demand_wheelRotationsPerSecond", wheelRotationsPerSecond); - - // SmartDashboard.putNumber( - // "Swerve States/" + this.steeringMotorCANId + "/Steering/demand_positionInRotations", positionInRotations); - // SmartDashboard.putNumber( - // "Swerve States/" + this.steeringMotorCANId + "/Steering/actual_positionOfSteering", positionOfSteeringRad); + // set the turn position + io.setTurnPosition(state.angle); + } + + public double[] getOdometryTimestamps() { + return inputs.odometryTimestamps; + } - // double encPosition = encoder.getAbsPosition(); // 0.0 to 1.0, inclusive, increasing counterclockwise - // SmartDashboard.putNumber( - // "Swerve States/" + this.steeringMotorCANId + "/external_encoderPosition", encPosition); - // } + public SwerveModulePosition[] getOdometryPositions() { + int sampleCount = inputs.odometryDrivePositionsRad.length; + SwerveModulePosition[] positions = + new SwerveModulePosition[sampleCount]; + for (int i = 0; i < sampleCount; i++) { + positions[i] = new SwerveModulePosition( + inputs.odometryDrivePositionsRad[i] * DriveConstants.WHEEL_RADIUS.in(Meters), + inputs.odometryTurnPositions[i] + ); + } + return positions; } } diff --git a/src/main/java/frc/robot/util/PhoenixUtil.java b/src/main/java/frc/robot/util/PhoenixUtil.java new file mode 100644 index 0000000..a1d0acf --- /dev/null +++ b/src/main/java/frc/robot/util/PhoenixUtil.java @@ -0,0 +1,15 @@ +package frc.robot.util; + +import java.util.function.Supplier; + +import com.ctre.phoenix6.StatusCode; + +public class PhoenixUtil { + /** Attempts to run the command until no error is produced. */ + public static void tryUntilOk(int maxAttempts, Supplier command) { + for (int i = 0; i < maxAttempts; i++) { + var error = command.get(); + if (error.isOK()) break; + } + } +} \ No newline at end of file diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..bef4a15 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,35 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "4.1.2", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2025", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "4.1.2" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "4.1.2", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file