Merge branch 'robot-logger' into swerve

2026-06-09 00:38:05 -06:00 · 2022-03-03 22:14:46 -07:00
parent 4f39a33a8b 14912c5743
commit c6041324d9
48 changed files with 2568 additions and 115151 deletions
@@ -1,11 +0,0 @@
-{
-  "SysId": {
-    "Analysis Type": "General Mechanism",
-    "AnalyzerJSONLocation": "C:\\Users\\Ridgebotics\\Documents\\GitHub\\2022NoWayHome\\sysid\\sysid_data20220128-205132.json",
-    "NetworkTables Settings": {
-      "serverTeam": "4388"
-    },
-    "mode": "Client",
-    "serverTeam": "4388"
-  }
-}
@@ -159,4 +159,6 @@ gradle-app.setting
 bin/

 # Simulation GUI and other tools window save file
-*-window.json
+simgui*.json
+
+src/main/deploy/config.json
@@ -8,13 +8,14 @@
      "type": "wpilib",
      "name": "WPILib Desktop Debug",
      "request": "launch",
-      "desktop": true,
+      "desktop": true
    },
    {
      "type": "wpilib",
      "name": "WPILib roboRIO Debug",
      "request": "launch",
      "desktop": false,
+      "postDebugTask": "downloadDeployDirectory"
    }
  ]
 }
@@ -26,5 +26,8 @@
    },
  ],
  "java.test.defaultConfig": "WPIlibUnitTests",
-  "java.dependency.packagePresentation": "hierarchical"
+  "java.dependency.packagePresentation": "hierarchical",
+  "files.associations": {
+    "*.path": "json"
+  }
 }
@@ -0,0 +1,34 @@
+{
+  "version": "2.0.0",
+  "tasks": [
+    {
+      "label": "downloadDeployDirectory",
+      "type": "shell",
+      "command": "scp -prv lvuser@roboRIO-4388-FRC.local:/home/lvuser/deploy ./src/main/",
+      "presentation": {
+        "echo": true,
+        "reveal": "always",
+        "focus": false,
+        "panel": "dedicated",
+        "showReuseMessage": true,
+        "clear": true
+      },
+      "problemMatcher": []
+    },
+    {
+      "label": "pathPlanner",
+      "type": "shell",
+      "osx": {
+        "command": "open -n /Applications/PathPlanner.app"
+      },
+      "presentation": {
+        "echo": false,
+        "reveal": "silent",
+        "focus": false,
+        "panel": "dedicated",
+        "showReuseMessage": false,
+        "clear": true
+      }
+    }
+  ]
+}
@@ -0,0 +1,18 @@
+{
+	"folders": [
+		{
+			"path": "."
+		},
+		{
+			"path": "NetworkTablesDesktopClient"
+		}
+	],
+	"settings": {
+		"java.configuration.updateBuildConfiguration": "automatic",
+		"java.server.launchMode": "Standard",
+		"java.dependency.packagePresentation": "hierarchical",
+		"files.associations": {
+			"*.path": "json"
+		}
+	}
+}
@@ -0,0 +1,2 @@
+build/
+bin/
@@ -0,0 +1,12 @@
+{
+  "version": "0.2.0",
+  "configurations": [
+    {
+      "type": "java",
+      "name": "Launch NetworkTablesDesktopClient",
+      "request": "launch",
+      "mainClass": "NetworkTablesDesktopClient",
+      "projectName": "NetworkTablesDesktopClient"
+    }
+  ]
+}
@@ -0,0 +1,24 @@
+apply plugin: 'java'
+
+sourceCompatibility = JavaVersion.VERSION_11
+targetCompatibility = JavaVersion.VERSION_11
+
+repositories {
+    maven { url "https://frcmaven.wpi.edu/artifactory/release/" }
+}
+
+dependencies {
+    // Add ntcore-java
+    implementation "edu.wpi.first.ntcore:ntcore-java:2022.2.1"
+
+    // Add wpiutil-java
+    implementation "edu.wpi.first.wpiutil:wpiutil-java:2022.2.1"
+
+    // Add ntcore-jni for runtime. We are adding all supported platforms
+    // so that our application will work on all supported platforms.
+    implementation "edu.wpi.first.ntcore:ntcore-jni:2022.2.1:windowsx86"
+    implementation "edu.wpi.first.ntcore:ntcore-jni:2022.2.1:windowsx86-64"
+    implementation "edu.wpi.first.ntcore:ntcore-jni:2022.2.1:linuxx86-64"
+    implementation "edu.wpi.first.ntcore:ntcore-jni:2022.2.1:linuxraspbian"
+    implementation "edu.wpi.first.ntcore:ntcore-jni:2022.2.1:osxx86-64"
+}
@@ -0,0 +1,5 @@
+distributionBase=GRADLE_USER_HOME
+distributionPath=wrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-7.4-bin.zip
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=wrapper/dists
@@ -0,0 +1,234 @@
+#!/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.
+#
+
+##############################################################################
+#
+#   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/master/subprojects/plugins/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
+
+APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit
+
+APP_NAME="Gradle"
+APP_BASE_NAME=${0##*/}
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+
+# 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
+    which java >/dev/null 2>&1 || 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
+
+# Increase the maximum file descriptors if we can.
+if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
+    case $MAX_FD in #(
+      max*)
+        MAX_FD=$( ulimit -H -n ) ||
+            warn "Could not query maximum file descriptor limit"
+    esac
+    case $MAX_FD in  #(
+      '' | soft) :;; #(
+      *)
+        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
+
+# Collect all arguments for the java command;
+#   * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of
+#     shell script including quotes and variable substitutions, so put them in
+#     double quotes to make sure that they get re-expanded; and
+#   * put everything else in single quotes, so that it's not re-expanded.
+
+set -- \
+        "-Dorg.gradle.appname=$APP_BASE_NAME" \
+        -classpath "$CLASSPATH" \
+        org.gradle.wrapper.GradleWrapperMain \
+        "$@"
+
+# 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" "$@"
@@ -0,0 +1,89 @@
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem      https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+
+@if "%DEBUG%" == "" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%" == "" set DIRNAME=.
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Resolve any "." and ".." in APP_HOME to make it shorter.
+for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if "%ERRORLEVEL%" == "0" goto execute
+
+echo.
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto execute
+
+echo.
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+
+:end
+@rem End local scope for the variables with windows NT shell
+if "%ERRORLEVEL%"=="0" goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+if  not "" == "%GRADLE_EXIT_CONSOLE%" exit 1
+exit /b 1
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega
@@ -0,0 +1,33 @@
+import java.io.File;
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.util.Scanner;
+
+import edu.wpi.first.networktables.EntryListenerFlags;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+public class NetworkTablesDesktopClient {
+  public static void main(String[] args) {
+    NetworkTableInstance instance = NetworkTableInstance.getDefault();
+    instance.getTable("Recording").addEntryListener((table, key, entry, value, flags) -> {
+      Path path = Path.of("..", "src", "main", "deploy", "pathplanner", key);
+      File file = path.toFile();
+      try {
+        System.err.println("Creating path " + key);
+        file.createNewFile();
+        Files.writeString(file.toPath(), value.getString());
+        System.err.println("Recorded path to " + file.getPath());
+      } catch (IOException exception) {
+        exception.printStackTrace();
+      }
+    }, EntryListenerFlags.kNew);
+    // instance.startClientTeam(4388);
+    instance.startClient("localhost");
+    instance.startDSClient();
+    try (Scanner scanner = new Scanner(System.in)) {
+      System.err.println("Press enter to stop...");
+      scanner.nextLine();
+    }
+  }
+}
@@ -55,6 +55,8 @@ def includeDesktopSupport = true
 dependencies {
    implementation wpi.java.deps.wpilib()
    implementation wpi.java.vendor.java()
+    implementation 'com.diffplug.durian:durian:3.4.0'
+    implementation 'com.fasterxml.jackson.datatype:jackson-datatype-jdk8:2.13.1'
    implementation 'org.fusesource.jansi:jansi:2.4.0'

    roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
@@ -1,101 +0,0 @@
-{
-  "keyboardJoysticks": [
-    {
-      "axisConfig": [
-        {
-          "decKey": 65,
-          "incKey": 68
-        },
-        {
-          "decKey": 87,
-          "incKey": 83
-        },
-        {
-          "decKey": 69,
-          "decayRate": 0.0,
-          "incKey": 82,
-          "keyRate": 0.009999999776482582
-        }
-      ],
-      "axisCount": 3,
-      "buttonCount": 4,
-      "buttonKeys": [
-        90,
-        88,
-        67,
-        86
-      ],
-      "povConfig": [
-        {
-          "key0": 328,
-          "key135": 323,
-          "key180": 322,
-          "key225": 321,
-          "key270": 324,
-          "key315": 327,
-          "key45": 329,
-          "key90": 326
-        }
-      ],
-      "povCount": 1
-    },
-    {
-      "axisConfig": [
-        {
-          "decKey": 74,
-          "incKey": 76
-        },
-        {
-          "decKey": 73,
-          "incKey": 75
-        }
-      ],
-      "axisCount": 2,
-      "buttonCount": 4,
-      "buttonKeys": [
-        77,
-        44,
-        46,
-        47
-      ],
-      "povCount": 0
-    },
-    {
-      "axisConfig": [
-        {
-          "decKey": 263,
-          "incKey": 262
-        },
-        {
-          "decKey": 265,
-          "incKey": 264
-        }
-      ],
-      "axisCount": 2,
-      "buttonCount": 6,
-      "buttonKeys": [
-        260,
-        268,
-        266,
-        261,
-        269,
-        267
-      ],
-      "povCount": 0
-    },
-    {
-      "axisCount": 0,
-      "buttonCount": 0,
-      "povCount": 0
-    }
-  ],
-  "robotJoysticks": [
-    {
-//      "guid": "78696e70757401000000000000000000"
-//    },
-//    {
-      "guid": "78696e70757401000000000000000000",
-      "useGamepad": true
-    }
-  ]
-}
@@ -1,54 +0,0 @@
-{
-  "HALProvider": {
-    "Other Devices": {
-      "SPARK MAX [10]": {
-      "Talon FX[2]": {
-        "header": {
-          "open": true
-        }
-      },
-      "SPARK MAX [30]": {
-      "Talon FX[3]": {
-        "header": {
-          "open": true
-        }
-      },
-      "SPARK MAX [31]": {
-      "Talon FX[4]": {
-        "header": {
-          "open": true
-        }
-      }
-    }
-  },
-  "NTProvider": {
-    "types": {
-      "/FMSInfo": "FMSInfo",
-      "/LiveWindow/BoomBoom": "Subsystem",
-      "/LiveWindow/Hood": "Subsystem",
-      "/LiveWindow/LED": "Subsystem",
-      "/LiveWindow/SwerveDrive": "Subsystem",
-      "/LiveWindow/SwerveModule": "Subsystem",
-      "/LiveWindow/Turret": "Subsystem",
-      "/LiveWindow/Ungrouped/Scheduler": "Scheduler",
-      "/LiveWindow/Vision": "Subsystem"
-      "/SmartDashboard/Driver Controller": "Mechanism2d",
-      "/SmartDashboard/Field": "Field2d"
-    },
-    "windows": {
-      "/SmartDashboard/Field": {
-        "window": {
-          "visible": true
-        }
-      }
-    }
-  },
-  "NetworkTables": {
-    "SmartDashboard": {
-      "Recording": {
-        "open": true
-      },
-      "open": true
-    }
-  }
-}
@@ -1,3 +0,0 @@
-Files placed in this directory will be deployed to the RoboRIO into the
-'deploy' directory in the home folder. Use the 'FileUtilities.getFilePath' wpilib function
-to get a proper path relative to the deploy directory.
@@ -71,11 +71,13 @@ public final class Constants {
    // swerve auto constants
    public static final PIDController X_CONTROLLER = new PIDController(0.5, 0.0, 0.0);
    public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0);
-    public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(
-            15.0, 0.1, 0.3, new TrapezoidProfile.Constraints(Math.PI, Math.PI));
-    
-    public static final double MAX_VEL = 5.0;
-    public static final double MAX_ACC = 5.0;
+    public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController (15.0, 0.1, 0.3, new TrapezoidProfile.Constraints(Math.PI, Math.PI));
+    public static final boolean PATH_RECORD_VELOCITY = true;
+    public static final double PATH_MAX_VELOCITY = 5.0;
+    public static final double PATH_MAX_ACCELERATION = 5.0;
+    public static final double MIN_WAYPOINT_ANGLE = 20;
+    public static final double MIN_WAYPOINT_DISTANCE = 0.1;
+    public static final double MIN_WAYPOINT_VELOCITY = 0.1;

    // swerve configuration
    public static final double NEUTRAL_DEADBAND = 0.04;
@@ -127,7 +129,7 @@ public final class Constants {
    /* PID Constants Shooter */
    public static final int CLOSED_LOOP_TIME_MS = 1;

-   public static final int SHOOTER_TIMEOUT_MS = 32;
+    public static final int SHOOTER_TIMEOUT_MS = 32;
    public static final int SHOOTER_SLOT_IDX = 0;
    public static final int SHOOTER_PID_LOOP_IDX = 1;
    public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5);
@@ -173,11 +175,11 @@ public final class Constants {

  }
  public static final class VisionConstants {
-      public static final double TURN_P_VALUE = 0.8;
-      public static final double X_ANGLE_ERROR = 0.5;
-      public static final double GRAV = 385.83;
-      public static final double TARGET_HEIGHT = 67.5;
-      public static final double FOV = 29.8; //Field of view limelight
-      public static final double LIME_ANGLE = 24.7;
-    }
+    public static final double TURN_P_VALUE = 0.8;
+    public static final double X_ANGLE_ERROR = 0.5;
+    public static final double GRAV = 385.83;
+    public static final double TARGET_HEIGHT = 67.5;
+    public static final double FOV = 29.8; //Field of view limelight
+    public static final double LIME_ANGLE = 24.7;
+  }
 }
@@ -5,6 +5,12 @@
 package frc4388.robot;


+import java.util.logging.Level;
+import java.util.logging.Logger;
+
+import com.diffplug.common.base.DurianPlugins;
+import com.diffplug.common.base.Errors;
+
 import edu.wpi.first.wpilibj.RobotBase;
 import frc4388.utility.AnsiLogging;

@@ -24,6 +30,7 @@ public final class Main {
   */
  public static void main(String... args) {
    AnsiLogging.systemInstall();
+    DurianPlugins.register(Errors.Plugins.Log.class, e -> Logger.getLogger(e.getStackTrace()[0].getClassName().substring(e.getStackTrace()[0].getClassName().lastIndexOf('.') + 1)).log(Level.SEVERE, e, e::getLocalizedMessage));
    RobotBase.startRobot(Robot::new);
  }
 }
@@ -4,14 +4,22 @@

 package frc4388.robot;

+import java.io.File;
+import java.nio.file.Files;
+import java.nio.file.Path;
 import java.util.logging.Level;
 import java.util.logging.Logger;
+import java.util.stream.IntStream;
+
+import com.diffplug.common.base.Errors;

 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc4388.utility.RobotTime;

 /**
@@ -22,7 +30,7 @@ import frc4388.utility.RobotTime;
 * project.
 */
 public class Robot extends TimedRobot {
-  private static final Logger LOGGER = Logger.getLogger(Robot.class.getName());
+  private static final Logger LOGGER = Logger.getLogger(Robot.class.getSimpleName());
  Command m_autonomousCommand;
  
  private RobotTime m_robotTime = RobotTime.getInstance();
@@ -34,20 +42,51 @@ public class Robot extends TimedRobot {
   */
  @Override
  public void robotInit() {
-    if (org.fusesource.jansi.Ansi.isEnabled()) {
-      LOGGER.log(Level.ALL, "Logging Test 1/8");
-      LOGGER.log(Level.SEVERE, "Logging Test 2/8");
-      LOGGER.log(Level.WARNING, "Logging Test 3/8");
-      LOGGER.log(Level.INFO, "Logging Test 4/8");
-      LOGGER.log(Level.CONFIG, "Logging Test 5/8");
-      LOGGER.log(Level.FINE, "Logging Test 6/8");
-      LOGGER.log(Level.FINER, "Logging Test 7/8");
-      LOGGER.log(Level.FINEST, "Logging Test 8/8");
-    }
+    LOGGER.log(Level.ALL, "Logging Test 1/8");
+    LOGGER.log(Level.SEVERE, "Logging Test 2/8");
+    LOGGER.log(Level.WARNING, "Logging Test 3/8");
+    LOGGER.log(Level.INFO, "Logging Test 4/8");
+    LOGGER.log(Level.CONFIG, "Logging Test 5/8");
+    LOGGER.log(Level.FINE, "Logging Test 6/8");
+    LOGGER.log(Level.FINER, "Logging Test 7/8");
+    LOGGER.log(Level.FINEST, "Logging Test 8/8");
+    Errors.log().run(() -> { throw new Throwable("Exception Test"); });
+
+    // var path = PathPlannerUtil.Path.read(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("Move Forward.path").toFile());
+    // LOGGER.finest(path::toString);
    LOGGER.fine("robotInit()");
+    // LOGGER.fine("Sssssssssh.");
+    // DriverStation.silenceJoystickConnectionWarning(true);
    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
    // autonomous chooser on the dashboard.
    m_robotContainer = new RobotContainer();
+    // addPeriodic(m_robotContainer::recordPeriodic, kDefaultPeriod);
+    SmartDashboard.putData(CommandScheduler.getInstance());
+    SmartDashboard.putData("JVM Memory", new RunCommand(() -> {}) {
+      @Override public boolean runsWhenDisabled() { return true; }
+      @Override public String getName() {
+        if (isScheduled()) {
+          Runtime runtime = Runtime.getRuntime();
+          long totalMemory = runtime.totalMemory() / 1_000_000;
+          long freeMemory = runtime.freeMemory() / 1_000_000;
+          long maxMemory = runtime.maxMemory() / 1_000_000;
+          return totalMemory - freeMemory + " MB / " + totalMemory + " MB / " + maxMemory + " MB";
+        }
+        return "Not Running";
+      }
+    });
+    SmartDashboard.putData("Usable Deploy Space", new RunCommand(() -> {}) {
+      @Override public boolean runsWhenDisabled() { return true; }
+      @Override public String getName() {
+        if (isScheduled()) {
+          File deploy = Filesystem.getDeployDirectory();
+          long usedSpace = Errors.suppress().getWithDefault(() -> Files.walk(deploy.toPath()).map(Path::toFile).filter(File::isFile).mapToLong(File::length).sum(), 0l) / 1_000_000;
+          long usableSpace = deploy.getUsableSpace() / 1_000_000;
+          return usedSpace + " MB / " + usableSpace + " MB";
+        }
+        return "Not Running";
+      }
+    });
  }

  /**
@@ -82,6 +121,15 @@ public class Robot extends TimedRobot {
  public void disabledInit() {
    LOGGER.fine("disabledInit()");
    m_robotTime.endMatchTime();
+    if (isTest()) {
+      // IMPORTANT: Had to chown the pathplanner folder in order to save autos.
+      File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("recording." + System.currentTimeMillis() + ".path").toFile();
+      if (Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
+        m_robotContainer.createPath(null, null, false).write(outputFile);
+        LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath());
+      } else
+        LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
+    }
  }

  @Override
@@ -132,6 +180,11 @@ public class Robot extends TimedRobot {
  public void teleopPeriodic() {
  }

+  @Override
+  public void testInit() {
+    CommandScheduler.getInstance().cancelAll();
+  }
+
  /**
   * This function is called periodically during test mode.
   */
@@ -4,36 +4,59 @@

 package frc4388.robot;

-import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
-import com.ctre.phoenix.motorcontrol.can.TalonFX;
-
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-
+import java.io.File;
+import java.io.IOException;
+import java.io.StringWriter;
+import java.nio.file.FileSystems;
+import java.nio.file.Path;
+import java.nio.file.StandardWatchEventKinds;
+import java.nio.file.WatchEvent;
+import java.nio.file.WatchKey;
+import java.time.Clock;
+import java.time.ZoneId;
+import java.time.ZonedDateTime;
+import java.time.format.DateTimeFormatter;
 import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Comparator;
+import java.util.List;
 import java.util.Objects;
+import java.util.Optional;
+import java.util.function.Function;
+import java.util.logging.Level;
+import java.util.logging.Logger;
+import java.util.regex.Matcher;
+import java.util.regex.Pattern;
+import java.util.stream.Collectors;

-import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
-import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.diffplug.common.base.Errors;
 import com.pathplanner.lib.PathPlanner;
 import com.pathplanner.lib.PathPlannerTrajectory;
 import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
 import com.pathplanner.lib.commands.PPSwerveControllerCommand;

+import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 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.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.NotifierCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-
-import frc4388.robot.Constants.*;
+import frc4388.robot.Constants.LEDConstants;
+import frc4388.robot.Constants.OIConstants;
+import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.commands.AimToCenter;
 import frc4388.robot.subsystems.BoomBoom;
 import frc4388.robot.subsystems.Hood;
@@ -42,6 +65,9 @@ import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.Turret;
 import frc4388.robot.subsystems.Vision;
 import frc4388.utility.LEDPatterns;
+import frc4388.utility.ListeningSendableChooser;
+import frc4388.utility.PathPlannerUtil;
+import frc4388.utility.PathPlannerUtil.Path.Waypoint;
 import frc4388.utility.controller.DeadbandedXboxController;

 /**
@@ -52,17 +78,12 @@ import frc4388.utility.controller.DeadbandedXboxController;
 * commands, and button mappings) should be declared here.
 */
 public class RobotContainer {
+  private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName());
  /* RobotMap */
  private final RobotMap m_robotMap = new RobotMap();

  /* Subsystems */
-  public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, 
-                                                                m_robotMap.leftBack, 
-                                                                m_robotMap.rightFront, 
-                                                                m_robotMap.rightBack, 
-                                                                m_robotMap.gyro);
-
-  private final TalonFX m_testMotor = new TalonFX(23);
+  public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);

  private final LED m_robotLED = new LED(m_robotMap.LEDController);
  private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
@@ -73,6 +94,18 @@ public class RobotContainer {
  private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
  private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);

+  /* Autonomous */
+  private PathPlannerTrajectory loadedPathTrajectory = null;
+  private final ListeningSendableChooser<File> autoChooser = new ListeningSendableChooser<>(this::loadPath);
+  private final List<Waypoint> pathPoints = new ArrayList<>();
+  private final NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault();
+  private final NetworkTable recordingNetworkTable = networkTableInstance.getTable("Recording");
+
+  private static final DateTimeFormatter RECORDING_FILE_NAME_FORMATTER = DateTimeFormatter.ofPattern("'Recording' yyyy-MM-dd HH:mm:ss.SSS'.path'");
+  private static final Clock SYSTEM_CLOCK = Clock.system(ZoneId.systemDefault());
+  private static final Path PATHPLANNER_DIRECTORY = Filesystem.getDeployDirectory().toPath().resolve("pathplanner");
+  private static final Function<CharSequence, String> PATH_EXTENSION_REMOVER = ((Function<CharSequence, Matcher>) Pattern.compile(".path")::matcher).andThen(m -> m.replaceFirst(""));
+
  /**
   * The container for the robot. Contains subsystems, OI devices, and commands.
   */
@@ -89,15 +122,17 @@ public class RobotContainer {

    m_robotSwerveDrive.setDefaultCommand(
        new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
-              getDriverController().getLeftX(),
-              getDriverController().getLeftY(),
-              getDriverController().getRightX(),
-              getDriverController().getRightY(),
-          true),
-           m_robotSwerveDrive));
+            getDriverController().getLeftX(),
+            getDriverController().getLeftY(),
+            getDriverController().getRightX(),
+            getDriverController().getRightY(),
+            true),
+            m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));

    // continually sends updates to the Blinkin LED controller to keep the lights on
-    m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
+    m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED).withName("LED update defaultCommand"));
+    autoInit();
+    recordInit();
  }

  /**
@@ -110,21 +145,18 @@ public class RobotContainer {
    /* Driver Buttons */
    // "XboxController.Button.kBack" was undefined yet, 7 works just fine
    new JoystickButton(getDriverController(), 7)
-      .whenPressed(() -> m_robotSwerveDrive.resetGyro());
+      .whenPressed(m_robotSwerveDrive::resetGyro);

    new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
-    // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON)
-      .whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
-
+        // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON)
+        .whenPressed(() -> m_robotSwerveDrive.highSpeed(false));

    new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
-    // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.RIGHT_BUMPER_BUTTON)
-      .whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
-    
-    new JoystickButton(getDriverController(), XboxController.Button.kA.value)
+        // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.RIGHT_BUMPER_BUTTON)
+        .whenPressed(() -> m_robotSwerveDrive.highSpeed(true));

-      .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
-      //.whenPressed(this::resetOdometry);
+    new JoystickButton(getDriverController(), XboxController.Button.kA.value)
+        .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));

    /* Operator Buttons */
    // activates "Lit Mode"
@@ -141,81 +173,33 @@ public class RobotContainer {
        .whenReleased(() -> m_robotHood.runHood(0.d));
  }

-  /**
-   * Generate autonomous
-   * @param maxVel max velocity for the path (null to override default value of 5.0)
-   * @param maxAccel max acceleration for the path (null to override default value of 5.0)
-   * @param inputs strings (path names) or commands you want to run (in order)
-   * @return array of commands, which can then be processed in a command group
-   */
-  public Command[] buildAuto(Double maxVel, Double maxAccel, Object... inputs) {
-
-    // default vel and acc
-    maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.MAX_VEL);
-    maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.MAX_ACC);
-
-    ArrayList<Command> commands = new ArrayList<Command>();
-    commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()));
-
-    // pids controlling the path
-    PIDController xController = SwerveDriveConstants.X_CONTROLLER;
-    PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
-    ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
-    thetaController.enableContinuousInput(-Math.PI, Math.PI);
-
-    // parse input
-    for (int i=0; i<inputs.length; i++) {
-
-      // if string, process as pathplanner trajectory
-      if (inputs[i] instanceof String) {
-
-        PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
-        PathPlannerState initState = (PathPlannerState) traj.sample(0);
-
-        commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation))));
-        commands.add(new PPSwerveControllerCommand(
-                          traj,
-                          m_robotSwerveDrive::getOdometry,
-                          m_robotSwerveDrive.m_kinematics,
-                          xController,
-                          yController,
-                          thetaController,
-                          m_robotSwerveDrive::setModuleStates,
-                          m_robotSwerveDrive));
-      }
-
-      // if command, just add it to the array
-      if (inputs[i] instanceof Command) {
-        commands.add((Command) inputs[i]);
-      }
-    }
-    
-    commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
-    Command[] ret = new Command[commands.size()];
-    ret = commands.toArray(ret);
-    return ret;
-  }
-
  /**
   * Use this to pass the autonomous command to the main {@link Robot} class.
   *
   * @return the command to run in autonomous
   */
  public Command getAutonomousCommand() {
-    // https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki
-    return new ParallelCommandGroup(
-            buildAuto(
-              null,
-              null,
-              new SequentialCommandGroup(buildAuto(0.5, 0.5, "Move Forward", "Move Down")), 
-              new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2))
-            )
-          );
+    if (loadedPathTrajectory != null) {
+      PIDController xController = SwerveDriveConstants.X_CONTROLLER;
+      PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
+      ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
+      thetaController.enableContinuousInput(-Math.PI, Math.PI);
+
+      PathPlannerState initialState = loadedPathTrajectory.getInitialState();
+      Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation);
+      return new SequentialCommandGroup(
+          new InstantCommand(m_robotSwerveDrive.m_gyro::reset),
+          new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)),
+          new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry,
+              m_robotSwerveDrive.m_kinematics, xController, yController, thetaController,
+              m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive),
+          new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path");
+    } else {
+      LOGGER.severe("No auto selected.");
+      return new RunCommand(() -> {}).withName("No Autonomous Path");
+    }
  }

-  /**
-   * Add your docs here.
-   */
  public XboxController getDriverController() {
    return m_driverXbox;
  }
@@ -235,10 +219,144 @@ public class RobotContainer {
  public void resetOdometry(Pose2d pose) {
    m_robotSwerveDrive.resetOdometry(pose);
  }
-  /**
-   * Add your docs here.
-   */
+
  public XboxController getOperatorController() {
    return m_operatorXbox;
  }
+
+  private void autoInit() {
+    try {
+      WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, StandardWatchEventKinds.ENTRY_DELETE);
+      // TODO: Store this and other commands as fields so they can be rescheduled.
+      new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) {
+        @Override
+        public boolean runsWhenDisabled() {
+          return true;
+        }
+      }.withName("Path Watcher").schedule();
+    } catch (IOException exception) {
+      LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception);
+    }
+    Arrays.stream(PATHPLANNER_DIRECTORY.toFile().listFiles())
+        .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified))
+        .forEachOrdered(file -> autoChooser.addOption(file.getName(), file));
+    SmartDashboard.putData("Auto Chooser", autoChooser);
+  }
+
+  public void recordInit() {
+    SmartDashboard.putData("Recording",
+        new RunCommand(this::recordPeriodic) {
+          @Override
+          public void end(boolean interupted) {
+            new InstantCommand(RobotContainer.this::saveRecording) {
+              @Override
+              public boolean runsWhenDisabled() {
+                return true;
+              }
+            }.withName("Save Recording").schedule();
+          }
+        }.withName("Record Path (Cancel to Save)"));
+  }
+
+  private void updateAutoChooser(WatchKey watchKey) {
+    List<WatchEvent<?>> watchEvents = watchKey.pollEvents();
+    if (!watchEvents.isEmpty()) {
+      List<WatchEvent<?>> pathWatchEvents = watchEvents.stream().filter(e -> e.kind().type().isAssignableFrom(Path.class)).collect(Collectors.toList());
+      for (WatchEvent<?> pathWatchEvent : pathWatchEvents) {
+        Path watchEventPath = (Path) pathWatchEvent.context();
+        File watchEventFile = watchEventPath.toFile();
+        String watchEventFileName = watchEventFile.getName();
+        if (watchEventFileName.endsWith(".path")) {
+          if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_CREATE)) {
+            LOGGER.log(Level.WARNING, "PathPlanner file {0} created. Options added to SendableChooser.", watchEventFileName);
+            autoChooser.addOption(watchEventFile.getName(), watchEventFile);
+          } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_MODIFY)) {
+            LOGGER.log(Level.WARNING, "PathPlanner file {0} modified.", watchEventFileName);
+            if (watchEventFileName.equals(autoChooser.getSelected().getName())) {
+              LOGGER.log(Level.SEVERE, "PathPlanner file {0} already selected. Reloading path.", watchEventFileName);
+              loadPath(watchEventFileName);
+            }
+          } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_DELETE)) {
+            LOGGER.log(Level.SEVERE, "PathPlanner file {0} deleted. Removing options from SendableChooser not yet implemented.", watchEventFileName);
+          }
+        }
+      }
+    }
+    if (!watchKey.reset())
+      LOGGER.severe("File watch key invalid.");
+  }
+
+  private void loadPath(String pathName) {
+    LOGGER.warning("Loading path " + pathName);
+    loadedPathTrajectory = null;
+    loadedPathTrajectory = PathPlanner.loadPath(PATH_EXTENSION_REMOVER.apply(Objects.requireNonNullElse(pathName, "")), SwerveDriveConstants.PATH_MAX_VELOCITY, SwerveDriveConstants.PATH_MAX_ACCELERATION);
+    LOGGER.info("Done loading");
+  }
+
+  private void saveRecording() {
+    // IMPORTANT: Had to chown the pathplanner folder in order to save autos.
+    File outputFile = PATHPLANNER_DIRECTORY.resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile();
+    LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath());
+    if (!pathPoints.isEmpty() && Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
+      // TODO: Change to use measured maximum velocity and acceleration.
+      var path = createPath(null, null, false);
+      if (RobotBase.isReal())
+        path.write(outputFile);
+      StringWriter writer = new StringWriter();
+      path.write(writer);
+      recordingNetworkTable.getEntry(outputFile.getName()).setString(writer.toString());
+      autoChooser.setDefaultOption(outputFile.getName(), outputFile);
+      LOGGER.log(Level.INFO, "Recorded path to {0}.", outputFile.getPath());
+    } else
+      LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
+  }
+
+  public void recordPeriodic() {
+    Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation();
+    Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
+    // FIXME: Chassis speeds are created from joystick inputs and do not reflect actual robot velocity.
+    Translation2d velocity = new Translation2d(m_robotSwerveDrive.chassisSpeeds.vxMetersPerSecond, m_robotSwerveDrive.chassisSpeeds.vyMetersPerSecond);
+    Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false, SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false);
+    pathPoints.add(waypoint);
+  }
+
+  public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) {
+    // Remove points whose angles to neighboring points are less than 10 degrees apart.
+    int j = 0;
+    for (int i = 1; i < pathPoints.size() - 1; i++) {
+      var prev = pathPoints.get(j).anchorPoint.orElseThrow();
+      var current = pathPoints.get(i).anchorPoint.orElseThrow();
+      var next = pathPoints.get(i + 1).anchorPoint.orElseThrow();
+      var fromPrevious = current.minus(prev);
+      var toNext = next.minus(current);
+      var angleFromPrevious = new Rotation2d(fromPrevious.getX(), fromPrevious.getY());
+      var angleToNext = new Rotation2d(toNext.getX(), toNext.getY());
+      if (Math.abs(angleFromPrevious.minus(angleToNext).getDegrees()) < SwerveDriveConstants.MIN_WAYPOINT_ANGLE || (next.getDistance(prev) < SwerveDriveConstants.MIN_WAYPOINT_DISTANCE && pathPoints.get(i).velOverride.map(v -> v < SwerveDriveConstants.MIN_WAYPOINT_VELOCITY).orElse(false)))
+        pathPoints.set(i, null);
+      else
+        j = i;
+    }
+    pathPoints.removeIf(Objects::isNull);
+    // Make control points
+    pathPoints.get(0).nextControl = Optional.of(makeControlPoints(null, pathPoints.get(0).anchorPoint.orElseThrow(), pathPoints.get(1).anchorPoint.orElseThrow()).getSecond());
+    for (int i = 1; i < pathPoints.size() - 1; i++) {
+      var controls = makeControlPoints(pathPoints.get(i - 1).anchorPoint.orElseThrow(), pathPoints.get(i).anchorPoint.orElseThrow(), pathPoints.get(i + 1).anchorPoint.orElseThrow());
+      pathPoints.get(i).prevControl = Optional.of(controls.getFirst());
+      pathPoints.get(i).nextControl = Optional.of(controls.getSecond());
+    }
+    pathPoints.get(pathPoints.size() - 1).prevControl = Optional.of(makeControlPoints(pathPoints.get(pathPoints.size() - 2).anchorPoint.orElseThrow(), pathPoints.get(pathPoints.size() - 1).anchorPoint.orElseThrow(), null).getFirst());
+    // Create the path
+    PathPlannerUtil.Path path = new PathPlannerUtil.Path();
+    path.waypoints = Optional.ofNullable(pathPoints.toArray(PathPlannerUtil.Path.Waypoint[]::new));
+    path.maxVelocity = Optional.ofNullable(maxVelocity);
+    path.maxAcceleration = Optional.ofNullable(maxAcceleration);
+    path.isReversed = Optional.ofNullable(isReversed);
+    pathPoints.clear();
+    return path;
+  }
+
+  private static Pair<Translation2d, Translation2d> makeControlPoints(Translation2d prev, Translation2d current, Translation2d next) {
+    var line = Objects.requireNonNullElse(next, current).minus(Objects.requireNonNullElse(prev, current)).div(4);
+    return Pair.of(current.minus(line), current.plus(line));
+  }
 }
@@ -10,6 +10,7 @@ import java.util.Comparator;
 import java.util.Map;
 import java.util.Optional;
 import java.util.function.Function;
+import java.util.logging.Logger;
 import java.util.regex.Pattern;
 import java.util.stream.IntStream;

@@ -24,82 +25,81 @@ import frc4388.utility.CSV;
 import frc4388.utility.Gains;

 public class BoomBoom extends SubsystemBase {
-public WPI_TalonFX m_shooterFalconLeft;
-public WPI_TalonFX m_shooterFalconRight;
-public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
-public static BoomBoom m_boomBoom;
+  private static final Logger LOGGER = Logger.getLogger(BoomBoom.class.getSimpleName());
+  public WPI_TalonFX m_shooterFalconLeft;
+  public WPI_TalonFX m_shooterFalconRight;
+  public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
+  public static BoomBoom m_boomBoom;

-double velP;
-double input;
+  double velP;
+  double input;

-public boolean m_isDrumReady = false;
-public double m_fireVel;
+  public boolean m_isDrumReady = false;
+  public double m_fireVel;

-public Hood m_hoodSubsystem;
-public Turret m_turretSubsystem;
+  public Hood m_hoodSubsystem;
+  public Turret m_turretSubsystem;

-// SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later
+  // SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values
+  // later

-public static class ShooterTableEntry {
-  public Double distance, hoodExt, drumVelocity;
-}
-
-private ShooterTableEntry[] m_shooterTable;
-
-/**
- * Creates a new BoomBoom subsystem, which has the drum shooter.
- * @param shooterFalconLeft The left drum motor.
- * @param shooterFalconRight The right drum motor.
- */
-public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
-  m_shooterFalconLeft = shooterFalconLeft;
-  m_shooterFalconRight = shooterFalconRight;
-
-  try {
-    CSV<ShooterTableEntry> csv = new CSV<>(ShooterTableEntry::new) {
-      private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)");
-
-      @Override
-      protected String headerSanitizer(final String header) {
-        return super.headerSanitizer(parentheses.matcher(header).replaceAll(""));
-      }
-
-    };
-    m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "Robot Data - Distances.csv").toPath());
-    new Thread(() -> System.out.println(CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start(); 
-  } catch (final IOException e) {
-    e.printStackTrace();
-    // throw new RuntimeException(e);
+  public static class ShooterTableEntry {
+    public Double distance, hoodExt, drumVelocity;
  }
-}

-public Double getVelocity(final Double distance) {
-  return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
-}
+  private ShooterTableEntry[] m_shooterTable;

-public Double getHood(final Double distance) {
-  return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
-}
+  /*
+  * Creates new BoomBoom subsystem, has drum shooter and angle adjuster
+  */
+  /** Creates a new BoomBoom. */
+  public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
+    m_shooterFalconLeft = shooterFalconLeft;
+    m_shooterFalconRight = shooterFalconRight;

-// TODO: @nathanrsxtn pls java doc your shooter tables code
+    try {
+      CSV<ShooterTableEntry> csv = new CSV<>(ShooterTableEntry::new) {
+        private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)");

-private static <E> Number linearInterpolate(final E[] table, final Number lookupValue, final Function<E, Number> lookupGetter, final Function<E, Number> targetGetter) {
-  final Map.Entry<Integer, E> closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1]));
-  final E closestRecord = closestEntry.getValue();
-  final int closestRecordIndex = closestEntry.getKey();
-  final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))];
-  return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord));
-}
+        @Override
+        protected String headerSanitizer(final String header) {
+          return super.headerSanitizer(parentheses.matcher(header).replaceAll(""));
+        }

-private static <E> Optional<Map.Entry<Integer, E>> lookup(final E[] table, final Number value, final Function<E, Number> valueGetter, final boolean exactMatch) {
-  final Optional<Map.Entry<Integer, E>> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue())));
-  return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty();
-}
+      };
+      m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath());
+      new Thread(() -> LOGGER.fine(CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start();
+    } catch (final IOException e) {
+      e.printStackTrace();
+      // throw new RuntimeException(e);
+    }
+  }

-private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) {
-  final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue());
-  return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue();
-}
+  public Double getVelocity(final Double distance) {
+    return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
+  }
+
+  public Double getHood(final Double distance) {
+    return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
+  }
+
+  private static <E> Number linearInterpolate(final E[] table, final Number lookupValue, final Function<E, Number> lookupGetter, final Function<E, Number> targetGetter) {
+    final Map.Entry<Integer, E> closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1]));
+    final E closestRecord = closestEntry.getValue();
+    final int closestRecordIndex = closestEntry.getKey();
+    final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))];
+    return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord));
+  }
+
+  private static <E> Optional<Map.Entry<Integer, E>> lookup(final E[] table, final Number value, final Function<E, Number> valueGetter, final boolean exactMatch) {
+    final Optional<Map.Entry<Integer, E>> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue())));
+    return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty();
+  }
+
+  private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) {
+    final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue());
+    return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue();
+  }

  @Override
  public void periodic() {
@@ -111,33 +111,34 @@ private static Number lerp2(final Number x, final Number x0, final Number y0, fi
    m_hoodSubsystem = subsystem0;
    m_turretSubsystem = subsystem1;
  }
-/**
- * Runs the Drum motor at a given speed
- * @param speed percent output form -1.0 to 1.0
- */
-public void runDrumShooter(double speed) {
-    m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed);
-    
-  }    

-public void setShooterGains() {
-  m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
-  m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
-  m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
-  m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
-  m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
-}
+  /**
+   * Runs the Drum motor at a given speed
+   * @param speed percent output form -1.0 to 1.0
+   */
+  public void runDrumShooter(double speed) {
+    m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed);
+
+  }
+
+  public void setShooterGains() {
+    m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
+    m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
+  }

  public void runDrumShooterVelocityPID(double targetVel) {
-    m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init
+    m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init
    m_shooterFalconRight.follow(m_shooterFalconLeft);
-    // New BoomBoom controller stuff 
-    //Controls a motor with the output of the BangBang controller
-    //Controls a motor with the output of the BangBang conroller and a feedforward
-    //Shrinks the feedforward slightly to avoid over speeding the shooter
-    
-    
-    // m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel));
+    // New BoomBoom controller stuff
+    // Controls a motor with the output of the BangBang controller
+    // Controls a motor with the output of the BangBang conroller and a feedforward
+    // Shrinks the feedforward slightly to avoid over speeding the shooter
+
+    // m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 *
+    // feedforward.calculate(targetVel));
    // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel));
  }
 }
@@ -27,7 +27,7 @@ public class LED extends SubsystemBase {
    m_LEDController = LEDController;
    setPattern(LEDConstants.DEFAULT_PATTERN);
    updateLED();
-    Logger.getLogger(LED.class.getName()).finer("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
+    Logger.getLogger(LED.class.getSimpleName()).finer("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
  }

  @Override
@@ -4,10 +4,8 @@

 package frc4388.robot.subsystems;

-import java.util.ArrayList;
-
-import com.ctre.phoenix.sensors.WPI_PigeonIMU;
 import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
+import com.ctre.phoenix.sensors.WPI_PigeonIMU;

 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
@@ -19,11 +17,9 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
 import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.Gains;
@@ -59,6 +55,7 @@ public class SwerveDrive extends SubsystemBase {
  public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
  public boolean ignoreAngles;
  public Rotation2d rotTarget = new Rotation2d();
+  public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();

  private final Field2d m_field = new Field2d();

@@ -112,6 +109,7 @@ public class SwerveDrive extends SubsystemBase {
              : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
    setModuleStates(states);
  }
+
  public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative)
  {
    ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
@@ -122,11 +120,12 @@ public class SwerveDrive extends SubsystemBase {
    double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
    double xSpeedMetersPerSecond = -speed.getX();
    double ySpeedMetersPerSecond = speed.getY();
+    chassisSpeeds = fieldRelative
+      ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
+      : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED);
    SwerveModuleState[] states =
        m_kinematics.toSwerveModuleStates(
-            fieldRelative
-              ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
-              : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED));
+            chassisSpeeds);
    setModuleStates(states);
  }

@@ -143,7 +142,7 @@ public class SwerveDrive extends SubsystemBase {
    }
    // modules[0].setDesiredState(desiredStates[0], false);
  }
-  
+
  @Override
  public void periodic() {

@@ -9,6 +9,8 @@ import java.io.PrintWriter;
 import java.io.StringWriter;
 import java.time.ZoneId;
 import java.time.ZonedDateTime;
+import java.util.Map;
+import java.util.Optional;
 import java.util.logging.ConsoleHandler;
 import java.util.logging.Formatter;
 import java.util.logging.Level;
@@ -24,68 +26,76 @@ import org.fusesource.jansi.AnsiConsole;
 public class AnsiLogging extends ConsoleHandler {
  public static void systemInstall() {
    try {
-      // Configures java.util.logging.Logger to output additional colored information.
+      // Configure java.util.logging.Logger to output additional colored information.
      LogManager.getLogManager().updateConfiguration(key -> (o, n) -> {
        switch (key) {
-          case ".level": return Level.ALL.getName();
-          case "handlers": return AnsiColorConsoleHandler.class.getName();
-          default: return n;
+          case ".level":
+            return Level.ALL.getName();
+          case "handlers":
+            return AnsiColorConsoleHandler.class.getName();
+          default:
+            return n;
        }
      });
-      // Replaces standard output streams with org.fusesource.jansi.AnsiPrintStreams.
+      // Replace standard output streams with org.fusesource.jansi.AnsiPrintStreams.
      AnsiConsole.systemInstall();
-      // Replaces standard output stream with java.util.logging.Logger.
+      // Replace standard output stream with java.util.logging.Logger.
      System.setOut(printStreamLogger(Logger.getGlobal(), Level.INFO));
-      // Replaces standard error output stream with java.util.logging.Logger.
+      // Replace standard error output stream with java.util.logging.Logger.
      System.setErr(printStreamLogger(Logger.getGlobal(), Level.SEVERE));
    } catch (IOException exception) {
      exception.printStackTrace(AnsiConsole.sysErr());
    }
  }
-  
+
  public static class AnsiColorConsoleHandler extends ConsoleHandler {
    @Override
    public void publish(LogRecord logRecord) {
      AnsiConsole.err().print(getFormatter().format(logRecord));
      AnsiConsole.err().flush();
    }
+
    @Override
    public Formatter getFormatter() {
      return formatter;
    }
+
    private static final Formatter formatter = new Formatter() {
+      private final ZoneId zoneId = ZoneId.systemDefault();
+      // Specify and prepare formats for messages
+      private final Map<Integer, String> levelColors = Map.of(
+        Level.OFF.intValue(), "", 
+        Level.SEVERE.intValue(), makeMessageFormatString(ansi().fgBright(Color.RED)), 
+        Level.WARNING.intValue(), makeMessageFormatString(ansi().fgBright(Color.YELLOW)), 
+        Level.INFO.intValue(), makeMessageFormatString(ansi().fg(Color.GREEN)), 
+        Level.CONFIG.intValue(), makeMessageFormatString(ansi().fgBright(Color.BLUE)), 
+        Level.FINE.intValue(), makeMessageFormatString(ansi().fg(Color.CYAN)), 
+        Level.FINER.intValue(), makeMessageFormatString(ansi().fg(Color.MAGENTA)), 
+        Level.FINEST.intValue(), makeMessageFormatString(ansi().fgBright(Color.BLACK)), 
+        Level.ALL.intValue(), makeMessageFormatString(ansi().fg(Color.DEFAULT))
+      );
+
+      private String makeMessageFormatString(Ansi base) {
+        return base.bold().a(Attribute.UNDERLINE).a("[%1$tb %1$td %1$tk:%1$tM:%1$tS.%1$tL] %2$s %3$s:").boldOff().a(Attribute.UNDERLINE_OFF).a("%4$s%5$s").a(Attribute.INTENSITY_FAINT).a("%6$s").reset().a("%n").toString();
+      }
+
+      private String makeStackTraceString(Throwable throwable) {
+        StringWriter stringWriter = new StringWriter();
+        try (PrintWriter printWriter = new PrintWriter(stringWriter)) {
+          printWriter.println();
+          throwable.printStackTrace(printWriter);
+        }
+        return stringWriter.toString();
+      }
+
      @Override
      public String format(LogRecord logRecord) {
-        ZonedDateTime zdt = ZonedDateTime.ofInstant(logRecord.getInstant(), ZoneId.systemDefault());
-        String source;
-        if (logRecord.getSourceClassName() != null && !logRecord.getSourceClassName().startsWith(AnsiLogging.class.getName())) {
-          source = logRecord.getSourceClassName();
-          if (logRecord.getSourceMethodName() != null) {
-            source += " " + logRecord.getSourceMethodName();
-          }
-        } else
-          source = logRecord.getLoggerName();
+        ZonedDateTime time = ZonedDateTime.ofInstant(logRecord.getInstant(), zoneId);
+        String source = Optional.ofNullable(logRecord.getLoggerName()).or(() -> Optional.ofNullable(logRecord.getSourceClassName())).map(s -> s + " ").orElse("") + Optional.ofNullable(logRecord.getSourceMethodName()).orElse("");
        String message = formatMessage(logRecord);
-        String throwable = "";
-        if (logRecord.getThrown() != null) {
-          StringWriter sw = new StringWriter();
-          try (PrintWriter pw = new PrintWriter(sw)) {
-            pw.println();
-            logRecord.getThrown().printStackTrace(pw);
-          }
-          throwable = sw.toString();
-        }
-        Ansi ansi;
-        if (logRecord.getLevel() == Level.SEVERE) ansi = ansi().fgBright(Color.RED);
-        else if (logRecord.getLevel() == Level.WARNING) ansi = ansi().fgBright(Color.YELLOW);
-        else if (logRecord.getLevel() == Level.INFO) ansi = ansi().fg(Color.GREEN);
-        else if (logRecord.getLevel() == Level.CONFIG) ansi = ansi().fgBright(Color.BLUE);
-        else if (logRecord.getLevel() == Level.FINE) ansi = ansi().fg(Color.CYAN);
-        else if (logRecord.getLevel() == Level.FINER) ansi = ansi().fg(Color.MAGENTA);
-        else if (logRecord.getLevel() == Level.FINEST) ansi = ansi().fgBright(Color.BLACK);
-        else ansi = ansi().fg(Color.DEFAULT);
-        String format = ansi.bold().a(Attribute.UNDERLINE).a("%1$tb %1$td, %1$tY %1$tl:%1$tM:%1$tS %1$Tp %2$s %4$s:").boldOff().a(Attribute.UNDERLINE_OFF).a("%n%5$s%6$s").reset().a("%n").toString();
-        return String.format(format, zdt, source, logRecord.getLoggerName(), logRecord.getLevel().getLocalizedName(), message, throwable);
+        String throwable = Optional.ofNullable(logRecord.getThrown()).map(this::makeStackTraceString).orElse("");
+        String format = levelColors.getOrDefault(logRecord.getLevel().intValue(), levelColors.get(Level.ALL.intValue()));
+        return String.format(format, time, source, logRecord.getLevel().getLocalizedName(), message.lines().count() > 1 ? System.lineSeparator() : " ", message.contains("\033") ? "\033[0m"  + message : message, throwable);
      }
    };
  }
@@ -95,7 +105,7 @@ public class AnsiLogging extends ConsoleHandler {
      private final StringBuilder stringBuilder = new StringBuilder();

      @Override
-      public final void write(int i) throws IOException {
+      public void write(int i) throws IOException {
        if (i == '\n') {
          logger.log(level, stringBuilder::toString);
          stringBuilder.setLength(0);
@@ -1,10 +1,3 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
 package frc4388.utility;

 import java.awt.Color;
@@ -1,12 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.utility;
-
-/** Add your docs here. */
-public class DataLogging {
-    public DataLogging() {
-        
-    }
-}
@@ -0,0 +1,182 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+// package edu.wpi.first.wpilibj.smartdashboard;
+package frc4388.utility;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.networktables.NTSendable;
+import edu.wpi.first.networktables.NTSendableBuilder;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import java.util.ArrayList;
+import java.util.LinkedHashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.concurrent.atomic.AtomicInteger;
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.function.Consumer;
+
+/**
+ * The {@link ListeningSendableChooser} class is a useful tool for presenting a selection of options to the
+ * {@link SmartDashboard}.
+ *
+ * <p>For instance, you may wish to be able to select between multiple autonomous modes. You can do
+ * this by putting every possible Command you want to run as an autonomous into a {@link
+ * ListeningSendableChooser} and then put it into the {@link SmartDashboard} to have a list of options appear
+ * on the laptop. Once autonomous starts, simply ask the {@link ListeningSendableChooser} what the selected
+ * value is.
+ *
+ * @param <V> The type of the values to be stored
+ */
+public class ListeningSendableChooser<V> implements NTSendable, AutoCloseable {
+  /** The key for the default value. */
+  private static final String DEFAULT = "default";
+  /** The key for the selected option. */
+  private static final String SELECTED = "selected";
+  /** The key for the active option. */
+  private static final String ACTIVE = "active";
+  /** The key for the option array. */
+  private static final String OPTIONS = "options";
+  /** The key for the instance number. */
+  private static final String INSTANCE = ".instance";
+  /** A map linking strings to the objects the represent. */
+  private final Map<String, V> m_map = new LinkedHashMap<>();
+
+  private String m_defaultChoice = "";
+  private final int m_instance;
+  private static final AtomicInteger s_instances = new AtomicInteger();
+
+  private Consumer<String> m_consumer;
+
+  /** Instantiates a {@link ListeningSendableChooser}. */
+  public ListeningSendableChooser(Consumer<String> consumer) {
+    m_consumer = consumer;
+    m_instance = s_instances.getAndIncrement();
+    SendableRegistry.add(this, "SendableChooser", m_instance);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * Adds the given object to the list of options. On the {@link SmartDashboard} on the desktop, the
+   * object will appear as the given name.
+   *
+   * @param name the name of the option
+   * @param object the option
+   */
+  public void addOption(String name, V object) {
+    m_map.put(name, object);
+  }
+
+  /**
+   * Adds the given object to the list of options.
+   *
+   * @param name the name of the option
+   * @param object the option
+   * @deprecated Use {@link #addOption(String, Object)} instead.
+   */
+  @Deprecated
+  public void addObject(String name, V object) {
+    addOption(name, object);
+  }
+
+  /**
+   * Adds the given object to the list of options and marks it as the default. Functionally, this is
+   * very close to {@link #addOption(String, Object)} except that it will use this as the default
+   * option if none other is explicitly selected.
+   *
+   * @param name the name of the option
+   * @param object the option
+   */
+  public void setDefaultOption(String name, V object) {
+    requireNonNullParam(name, "name", "setDefaultOption");
+
+    m_defaultChoice = name;
+    addOption(name, object);
+  }
+
+  /**
+   * Adds the given object to the list of options and marks it as the default.
+   *
+   * @param name the name of the option
+   * @param object the option
+   * @deprecated Use {@link #setDefaultOption(String, Object)} instead.
+   */
+  @Deprecated
+  public void addDefault(String name, V object) {
+    setDefaultOption(name, object);
+  }
+
+  /**
+   * Returns the selected option. If there is none selected, it will return the default. If there is
+   * none selected and no default, then it will return {@code null}.
+   *
+   * @return the option selected
+   */
+  public V getSelected() {
+    m_mutex.lock();
+    try {
+      if (m_selected != null) {
+        return m_map.get(m_selected);
+      } else {
+        return m_map.get(m_defaultChoice);
+      }
+    } finally {
+      m_mutex.unlock();
+    }
+  }
+
+  private String m_selected;
+  private final List<NetworkTableEntry> m_activeEntries = new ArrayList<>();
+  private final ReentrantLock m_mutex = new ReentrantLock();
+
+  @Override
+  public void initSendable(NTSendableBuilder builder) {
+    builder.setSmartDashboardType("String Chooser");
+    builder.getEntry(INSTANCE).setDouble(m_instance);
+    builder.addStringProperty(DEFAULT, () -> m_defaultChoice, null);
+    builder.addStringArrayProperty(OPTIONS, () -> m_map.keySet().toArray(new String[0]), null);
+    builder.addStringProperty(
+        ACTIVE,
+        () -> {
+          m_mutex.lock();
+          try {
+            if (m_selected != null) {
+              return m_selected;
+            } else {
+              return m_defaultChoice;
+            }
+          } finally {
+            m_mutex.unlock();
+          }
+        },
+        null);
+    m_mutex.lock();
+    try {
+      m_activeEntries.add(builder.getEntry(ACTIVE));
+    } finally {
+      m_mutex.unlock();
+    }
+    builder.addStringProperty(
+        SELECTED,
+        null,
+        val -> {
+          m_mutex.lock();
+          try {
+            m_selected = val;
+            m_consumer.accept(val);
+            for (NetworkTableEntry entry : m_activeEntries) {
+              entry.setString(val);
+            }
+          } finally {
+            m_mutex.unlock();
+          }
+        });
+  }
+}
@@ -0,0 +1,63 @@
+package frc4388.utility;
+
+import java.io.File;
+import java.io.OutputStream;
+import java.io.Writer;
+import java.util.Optional;
+
+import com.diffplug.common.base.Errors;
+import com.fasterxml.jackson.annotation.JsonInclude.Include;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import com.fasterxml.jackson.datatype.jdk8.Jdk8Module;
+
+import edu.wpi.first.math.geometry.Translation2d;
+
+public final class PathPlannerUtil {
+  public static final class Path {
+    public Optional<Waypoint[]> waypoints;
+    public Optional<Double> maxVelocity;
+    public Optional<Double> maxAcceleration;
+    public Optional<Boolean> isReversed;
+
+    private static final ObjectMapper objectMapper = new ObjectMapper();
+    static {
+      objectMapper.registerModule(new Jdk8Module());
+      objectMapper.setSerializationInclusion(Include.ALWAYS);
+    }
+
+    public static Path read(File src) {
+      return Errors.log().getWithDefault(() -> objectMapper.readValue(src, Path.class), null);
+    }
+
+    public void write(File resultFile) {
+      Errors.log().run(() -> objectMapper.writeValue(resultFile, this));
+    }
+
+    public void write(Writer writer) {
+      Errors.log().run(() -> objectMapper.writeValue(writer, this));
+    }
+
+    public static final class Waypoint {
+      public Optional<Translation2d> anchorPoint;
+      public Optional<Translation2d> prevControl;
+      public Optional<Translation2d> nextControl;
+      public Optional<Double> holonomicAngle;
+      public Optional<Boolean> isReversal;
+      public Optional<Double> velOverride;
+      public Optional<Boolean> isLocked;
+
+      public Waypoint() {
+      }
+
+      public Waypoint(Translation2d anchorPoint, Translation2d prevControl, Translation2d nextControl, Double holonomicAngle, Boolean isReversal, Double velOverride, Boolean isLocked) {
+        this.anchorPoint = Optional.ofNullable(anchorPoint);
+        this.prevControl = Optional.ofNullable(prevControl);
+        this.nextControl = Optional.ofNullable(nextControl);
+        this.holonomicAngle = Optional.ofNullable(holonomicAngle);
+        this.isReversal = Optional.ofNullable(isReversal);
+        this.velOverride = Optional.ofNullable(velOverride);
+        this.isLocked = Optional.ofNullable(isLocked);
+      }
+    }
+  }
+}
@@ -1,245 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.utility;
-
-import com.ctre.phoenix.sensors.PigeonIMU;
-import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
-import com.kauailabs.navx.frc.AHRS;
-
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.util.sendable.Sendable;
-import edu.wpi.first.util.sendable.SendableBuilder;
-import edu.wpi.first.wpilibj.PIDSource;
-import edu.wpi.first.wpilibj.PIDSourceType;
-import edu.wpi.first.wpilibj.interfaces.Gyro;
-
-/**
- * Gyro class that allows for interchangeable use between a pigeon and a navX
- */
-public class RobotGyro implements Gyro, PIDSource, Sendable {
-  private RobotTime m_robotTime = RobotTime.getInstance();
-
-  private PigeonIMU m_pigeon = null;
-  private AHRS m_navX = null;
-  public boolean m_isGyroAPigeon; // true if pigeon, false if navX
-
-  private double m_lastPigeonAngle;
-  private double m_deltaPigeonAngle;
-
-  /**
-   * Creates a Gyro based on a pigeon
-   * 
-   * @param gyro the gyroscope to use for Gyro
-   */
-  public RobotGyro(PigeonIMU gyro) {
-    m_pigeon = gyro;
-    m_isGyroAPigeon = true;
-  }
-
-  /**
-   * Creates a Gyro based on a navX
-   * 
-   * @param gyro the gyroscope to use for Gyro
-   */
-  public RobotGyro(AHRS gyro) {
-    m_navX = gyro;
-    m_isGyroAPigeon = false;
-  }
-
-  /**
-   * Run in periodic if you are using a pigeon. Updates a delta angle so that it
-   * can calculate getRate(). Note
-   * that the getRate() method for a navX will likely be much more accurate than
-   * for a pigeon.
-   */
-  public void updatePigeonDeltas() {
-    double currentPigeonAngle = getAngle();
-    m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
-    m_lastPigeonAngle = currentPigeonAngle;
-  }
-
-  /**
-   * <p>
-   * NavX:
-   * <p>
-   * Calibrate the gyro by running for a number of samples and computing the
-   * center value. Then use
-   * the center value as the Accumulator center value for subsequent measurements.
-   * It's important to
-   * make sure that the robot is not moving while the centering calculations are
-   * in progress, this
-   * is typically done when the robot is first turned on while it's sitting at
-   * rest before the
-   * competition starts.
-   * 
-   * <p>
-   * Pigeon:
-   * <p>
-   * Calibrate the gyro by collecting data at a range of tempuratures. Allow
-   * pigeon to cool, then boot
-   * into calibration mode. For faster calibration, use a heat lamp to heat up the
-   * pigeon. Once the pigeon
-   * has seen a reasonable range of tempuratures, it will exit calibration mode.
-   * It's important to
-   * make sure that the robot is not moving while the tempurature calculations are
-   * in progress, this
-   * is typically done when the robot is first turned on while it's sitting at
-   * rest before the
-   * competition starts.
-   */
-  @Override
-  public void calibrate() {
-    if (m_isGyroAPigeon)
-      m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
-    else
-      m_navX.calibrate();
-  }
-
-  @Override
-  public void reset() {
-    if (m_isGyroAPigeon)
-      m_pigeon.setYaw(0);
-    else
-      m_navX.reset();
-  }
-
-  /**
-   * Get Yaw, Pitch, and Roll data.
-   *
-   * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
-   *         Yaw is within [-368,640, +368,640] degrees.
-   *         Pitch is within [-90,+90] degrees.
-   *         Roll is within [-90,+90] degrees.
-   */
-  private double[] getPigeonAngles() {
-    double[] angles = new double[3];
-    m_pigeon.getYawPitchRoll(angles);
-    return angles;
-  }
-
-  @Override
-  public double getAngle() {
-    if (m_isGyroAPigeon) {
-      return getPigeonAngles()[0];
-    } else {
-      return m_navX.getAngle();
-    }
-  }
-
-  /**
-   * Gets an absolute heading of the robot
-   * 
-   * @return heading from -180 to 180 degrees
-   */
-  public double getHeading() {
-    return getHeading(getAngle());
-  }
-
-  /**
-   * Gets an absolute heading of the robot
-   * 
-   * @return heading from -180 to 180 degrees
-   */
-  public double getHeading(double angle) {
-    return Math.IEEEremainder(angle, 360);
-  }
-
-  /**
-   * Returns the current pitch value (in degrees, from -90 to 90)
-   * reported by the sensor. Pitch is a measure of rotation around
-   * the Y Axis.
-   * 
-   * @return The current pitch value in degrees (-90 to 90).
-   */
-  public double getPitch() {
-    if (m_isGyroAPigeon) {
-      return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
-    } else {
-      return MathUtil.clamp(m_navX.getPitch(), -90, 90);
-    }
-  }
-
-  /**
-   * Returns the current roll value (in degrees, from -90 to 90)
-   * reported by the sensor. Roll is a measure of rotation around
-   * the X Axis.
-   * 
-   * @return The current roll value in degrees (-90 to 90).
-   */
-  public double getRoll() {
-    if (m_isGyroAPigeon) {
-      return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
-    } else {
-      return MathUtil.clamp(m_navX.getRoll(), -90, 90);
-    }
-  }
-
-  @Override
-  public double getRate() {
-    if (m_isGyroAPigeon) {
-      return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
-    } else {
-      return m_navX.getRate();
-    }
-  }
-
-  public PigeonIMU getPigeon() {
-    return m_pigeon;
-  }
-
-  public AHRS getNavX() {
-    return m_navX;
-  }
-
-  @Override
-  public void close() throws Exception {
-
-  }
-
-  // Begin old GyroBase class
-  private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
-
-  /**
-   * Set which parameter of the gyro you are using as a process control variable.
-   * The Gyro class
-   * supports the rate and displacement parameters
-   *
-   * @param pidSource An enum to select the parameter.
-   */
-  @Override
-  public void setPIDSourceType(PIDSourceType pidSource) {
-    m_pidSource = pidSource;
-  }
-
-  @Override
-  public PIDSourceType getPIDSourceType() {
-    return m_pidSource;
-  }
-
-  /**
-   * Get the output of the gyro for use with PIDControllers. May be the angle or
-   * rate depending on
-   * the set PIDSourceType
-   *
-   * @return the output according to the gyro
-   */
-  @Override
-  public double pidGet() {
-    switch (m_pidSource) {
-      case kRate:
-        return getRate();
-      case kDisplacement:
-        return getAngle();
-      default:
-        return 0.0;
-    }
-  }
-
-  @Override
-  public void initSendable(SendableBuilder builder) {
-    builder.setSmartDashboardType("Gyro");
-    builder.addDoubleProperty("Value", this::getAngle, null);
-  }
-}
@@ -1,52 +0,0 @@
-{
-  "counts per rotation": 2048.0,
-  "encoder type": "Built-in",
-  "encoding": false,
-  "gearing denominator": 1.0,
-  "gearing numerator": 5.12,
-  "gyro": "Pigeon",
-  "gyro ctor": "14",
-  "is drivetrain": false,
-  "motor controllers": [
-    "TalonFX",
-    "TalonFX",
-    "TalonFX",
-    "TalonFX"
-  ],
-  "number of samples per average": 1,
-  "primary encoder inverted": false,
-  "primary encoder ports": [
-    10,
-    1
-  ],
-  "primary motor ports": [
-    3,
-    5,
-    7,
-    9
-  ],
-  "primary motors inverted": [
-    false,
-    false,
-    false,
-    false
-  ],
-  "secondary encoder inverted": false,
-  "secondary encoder ports": [
-    2,
-    13
-  ],
-  "secondary motor ports": [
-    5,
-    9,
-    5,
-    7
-  ],
-  "secondary motors inverted": [
-    false,
-    false,
-    false,
-    false
-  ],
-  "velocity measurement period": 100
-}
@@ -1,37 +0,0 @@
-{
-    "fileName": "WPILibOldCommands.json",
-    "name": "WPILib-Old-Commands",
-    "version": "2020.0.0",
-    "uuid": "b066afc2-5c18-43c4-b758-43381fcb275e",
-    "mavenUrls": [],
-    "jsonUrl": "",
-    "javaDependencies": [
-        {
-            "groupId": "edu.wpi.first.wpilibOldCommands",
-            "artifactId": "wpilibOldCommands-java",
-            "version": "wpilib"
-        }
-    ],
-    "jniDependencies": [],
-    "cppDependencies": [
-        {
-            "groupId": "edu.wpi.first.wpilibOldCommands",
-            "artifactId": "wpilibOldCommands-cpp",
-            "version": "wpilib",
-            "libName": "wpilibOldCommands",
-            "headerClassifier": "headers",
-            "sourcesClassifier": "sources",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "linuxathena",
-                "linuxraspbian",
-                "linuxaarch64bionic",
-                "windowsx86-64",
-                "windowsx86",
-                "linuxx86-64",
-                "osxx86-64"
-            ]
-        }
-    ]
-}