Initial commit

2026-06-09 00:38:02 -06:00 · 2024-01-08 11:38:08 -07:00
commit d861e109fc
62 changed files with 4618 additions and 0 deletions
@@ -0,0 +1,2 @@
+# Auto detect text files and perform LF normalization
+* text=auto
@@ -0,0 +1,23 @@
+name: Java CI
+
+on:
+  push:
+    branches:
+    - master
+  pull_request:
+    branches:
+    - master
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/checkout@v1
+    - name: Set up JDK 1.8
+      uses: actions/setup-java@v1
+      with:
+        java-version: 1.12
+    - name: Change wrapper permissions
+      run: chmod +x ./gradlew
+    - name: Build with Gradle
+      run: ./gradlew build
@@ -0,0 +1,172 @@
+# This gitignore has been specially created by the WPILib team.
+# If you remove items from this file, intellisense might break.
+
+### C++ ###
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+### Java ###
+# Compiled class file
+*.class
+
+# Log file
+*.log
+
+# BlueJ files
+*.ctxt
+
+# Mobile Tools for Java (J2ME)
+.mtj.tmp/
+
+# Package Files #
+*.jar
+*.war
+*.nar
+*.ear
+*.zip
+*.tar.gz
+*.rar
+
+# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml
+hs_err_pid*
+
+### Linux ###
+*~
+
+# temporary files which can be created if a process still has a handle open of a deleted file
+.fuse_hidden*
+
+# KDE directory preferences
+.directory
+
+# Linux trash folder which might appear on any partition or disk
+.Trash-*
+
+# .nfs files are created when an open file is removed but is still being accessed
+.nfs*
+
+### macOS ###
+# General
+.DS_Store
+.AppleDouble
+.LSOverride
+
+# Icon must end with two \r
+Icon
+
+# Thumbnails
+._*
+
+# Files that might appear in the root of a volume
+.DocumentRevisions-V100
+.fseventsd
+.Spotlight-V100
+.TemporaryItems
+.Trashes
+.VolumeIcon.icns
+.com.apple.timemachine.donotpresent
+
+# Directories potentially created on remote AFP share
+.AppleDB
+.AppleDesktop
+Network Trash Folder
+Temporary Items
+.apdisk
+
+### VisualStudioCode ###
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+
+### Windows ###
+# Windows thumbnail cache files
+Thumbs.db
+ehthumbs.db
+ehthumbs_vista.db
+
+# Dump file
+*.stackdump
+
+# Folder config file
+[Dd]esktop.ini
+
+# Recycle Bin used on file shares
+$RECYCLE.BIN/
+
+# Windows Installer files
+*.cab
+*.msi
+*.msix
+*.msm
+*.msp
+
+# Windows shortcuts
+*.lnk
+
+### Gradle ###
+.gradle
+/build/
+
+# Ignore Gradle GUI config
+gradle-app.setting
+
+# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored)
+!gradle-wrapper.jar
+
+# Cache of project
+.gradletasknamecache
+
+# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898
+# gradle/wrapper/gradle-wrapper.properties
+
+# # VS Code Specific Java Settings
+# DO NOT REMOVE .classpath and .project
+.classpath
+.project
+.settings/
+bin/
+
+# IntelliJ
+*.iml
+*.ipr
+*.iws
+.idea/
+out/
+
+# Fleet
+.fleet
+
+# Simulation GUI and other tools window save file
+*-window.json
@@ -0,0 +1,3 @@
+# Default ignored files
+/shelf/
+/workspace.xml
@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="GradleSettings">
+    <option name="linkedExternalProjectsSettings">
+      <GradleProjectSettings>
+        <option name="distributionType" value="DEFAULT_WRAPPED" />
+        <option name="externalProjectPath" value="$PROJECT_DIR$" />
+        <option name="gradleJvm" value="temurin-11" />
+        <option name="modules">
+          <set>
+            <option value="$PROJECT_DIR$" />
+          </set>
+        </option>
+      </GradleProjectSettings>
+    </option>
+  </component>
+</project>
@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ExternalStorageConfigurationManager" enabled="true" />
+</project>
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="" vcs="Git" />
+  </component>
+</project>
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 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.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.*;
+import frc4388.robot.subsystems.*;
+import org.junit.*;
+
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.verify;
+
+public class CommandTest {
+    private CommandScheduler scheduler = null;
+
+    @Before
+    public void setup() {
+        scheduler = CommandScheduler.getInstance();
+    }
+
+    // TODO: Update this to use an actual command. Won't work with inline commands for some reason
+
+    @Test
+    public void testExample() {
+        // Arrange
+        Drive drive = mock(Drive.class);
+        RunCommand command = new RunCommand(() -> drive.driveWithInput(0, 0), drive);
+
+        // Act
+        scheduler.schedule(command);
+        scheduler.run();
+
+        // Assert
+        verify(drive).driveWithInput(0, 0);
+    }
+}
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* 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.robot.subsystems;
+
+import static org.junit.Assert.*;
+import static org.mockito.Mockito.*;
+
+import org.junit.*;
+
+import edu.wpi.first.wpilibj.Spark;
+import frc4388.robot.Constants.LEDConstants;
+import frc4388.utility.LEDPatterns;
+
+/**
+ * Based off the LEDSubsystemTest class
+ */
+public class SubsystemTest {
+    @Test
+    public void testConstructor() {
+        // Arrange
+        Spark ledController = mock(Spark.class);
+
+        // Act
+        LED led = new LED(ledController);
+
+        // Assert
+        assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001);
+    }
+
+    @Test
+    public void testPatterns() {
+        // Arrange
+        Spark ledController = mock(Spark.class);
+        LED led = new LED(ledController);
+
+        // Act
+        led.setPattern(LEDPatterns.RAINBOW_RAINBOW);
+
+        // Assert
+        assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001);
+
+        // Act
+        led.setPattern(LEDPatterns.BLUE_BREATH);
+
+        // Assert
+        assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001);
+
+        // Act
+        led.setPattern(LEDPatterns.SOLID_BLACK);
+
+        // Assert
+        assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001);
+    }
+}
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* 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 static org.junit.Assert.*;
+import static org.mockito.Mockito.*;
+
+import com.kauailabs.navx.frc.AHRS;
+
+import org.junit.*;
+
+import frc4388.mocks.MockPigeonIMU;
+import frc4388.robot.Constants.DriveConstants;
+
+/**
+ * Based on the RobotGyroUtilityTest class
+ */
+public class UtilityTest {
+    private RobotGyro gyroPigeon;
+    private RobotGyro gyroNavX;
+
+    @Test
+    public void testConstructor() {
+        // Arrange
+        MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
+        AHRS navX = mock(AHRS.class);
+        gyroPigeon = new RobotGyro(pigeon);
+        gyroNavX = new RobotGyro(navX);
+
+        // Assert
+        assertEquals(true, gyroPigeon.m_isGyroAPigeon);
+        assertEquals(pigeon, gyroPigeon.getPigeon());
+        assertEquals(null, gyroPigeon.getNavX());
+        assertEquals(false, gyroNavX.m_isGyroAPigeon);
+        assertEquals(navX, gyroNavX.getNavX());
+        assertEquals(null, gyroNavX.getPigeon());
+    }
+
+    @Test
+    public void testHeadingPigeon() {
+        // Arrange
+        MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
+        gyroPigeon = new RobotGyro(pigeon);
+
+        // Act & Assert
+        assertEquals(-90, gyroPigeon.getHeading(270), 0.0001);
+        assertEquals(-45, gyroPigeon.getHeading(315), 0.0001);
+        assertEquals(-60, gyroPigeon.getHeading(-60), 0.0001);
+        assertEquals(30, gyroPigeon.getHeading(30), 0.0001);
+        assertEquals(0, gyroPigeon.getHeading(0), 0.0001);
+        assertEquals(180, gyroPigeon.getHeading(180), 0.0001);
+        assertEquals(-180, gyroPigeon.getHeading(-180), 0.0001);
+        assertEquals(97, gyroPigeon.getHeading(1537), 0.0001);
+        assertEquals(99, gyroPigeon.getHeading(-2781), 0.0001);
+    }
+}
@@ -0,0 +1,21 @@
+{
+  // Use IntelliSense to learn about possible attributes.
+  // Hover to view descriptions of existing attributes.
+  // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+  "version": "0.2.0",
+  "configurations": [
+
+    {
+      "type": "wpilib",
+      "name": "WPILib Desktop Debug",
+      "request": "launch",
+      "desktop": true,
+    },
+    {
+      "type": "wpilib",
+      "name": "WPILib roboRIO Debug",
+      "request": "launch",
+      "desktop": false,
+    }
+  ]
+}
@@ -0,0 +1,29 @@
+{
+  "java.configuration.updateBuildConfiguration": "automatic",
+  "java.server.launchMode": "Standard",
+  "files.exclude": {
+    "**/.git": true,
+    "**/.svn": true,
+    "**/.hg": true,
+    "**/CVS": true,
+    "**/.DS_Store": true,
+    "bin/": true,
+    "**/.classpath": true,
+    "**/.project": true,
+    "**/.settings": true,
+    "**/.factorypath": true,
+    "**/*~": true
+  },
+  "java.test.config": [
+    {
+      "name": "WPIlibUnitTests",
+      "workingDirectory": "${workspaceFolder}/build/jni/release",
+      "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ],
+      "env": {
+        "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" ,
+        "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release"
+      }
+    },
+  ],
+  "java.test.defaultConfig": "WPIlibUnitTests"
+}
@@ -0,0 +1,6 @@
+{
+    "enableCppIntellisense": false,
+    "currentLanguage": "java",
+    "projectYear": "2023",
+    "teamNumber": 4388
+}
@@ -0,0 +1,24 @@
+Copyright (c) 2009-2018 FIRST
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the FIRST nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS``AS IS'' AND ANY
+EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@@ -0,0 +1,2 @@
+# Robot-Essentials
+ Basic code for any Ridgebotics robot project
@@ -0,0 +1,24 @@
+Copyright (c) 2009-2021 FIRST and other WPILib contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+   * Neither the name of FIRST, WPILib, nor the names of other WPILib
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@@ -0,0 +1,99 @@
+plugins {
+    id "java"
+    id "edu.wpi.first.GradleRIO" version "2023.4.3"
+}
+
+sourceCompatibility = JavaVersion.VERSION_11
+targetCompatibility = JavaVersion.VERSION_11
+
+def ROBOT_MAIN_CLASS = "frc4388.robot.Main"
+
+// Define my targets (RoboRIO) and artifacts (deployable files)
+// This is added by GradleRIO's backing project DeployUtils.
+deploy {
+    targets {
+        roborio(getTargetTypeClass('RoboRIO')) {
+            // Team number is loaded either from the .wpilib/wpilib_preferences.json
+            // or from command line. If not found an exception will be thrown.
+            // You can use getTeamOrDefault(team) instead of getTeamNumber if you
+            // want to store a team number in this file.
+            team = project.frc.getTeamNumber()
+            debug = project.frc.getDebugOrDefault(false)
+
+            artifacts {
+                // First part is artifact name, 2nd is artifact type
+                // getTargetTypeClass is a shortcut to get the class type using a string
+
+                frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
+                }
+
+                // Static files artifact
+                frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+                    files = project.fileTree('src/main/deploy')
+                    directory = '/home/lvuser/deploy'
+                }
+            }
+        }
+    }
+}
+
+def deployArtifact = deploy.targets.roborio.artifacts.frcJava
+
+// Set to true to use debug for JNI.
+wpi.java.debugJni = false
+
+// Set this to true to enable desktop support.
+def includeDesktopSupport = false
+
+// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+// Also defines JUnit 5.
+dependencies {
+    implementation wpi.java.deps.wpilib()
+    implementation wpi.java.vendor.java()
+
+    roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
+    roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
+
+    roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
+    roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)
+
+    nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
+    nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
+    simulationDebug wpi.sim.enableDebug()
+
+    nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop)
+    nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
+    simulationRelease wpi.sim.enableRelease()
+
+    testImplementation 'org.junit.jupiter:junit-jupiter-api:5.8.2'
+    testImplementation 'org.junit.jupiter:junit-jupiter-params:5.8.2'
+    testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.8.2'
+}
+
+test {
+    useJUnitPlatform()
+    systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
+}
+
+// Simulation configuration (e.g. environment variables).
+wpi.sim.addGui().defaultEnabled = true
+wpi.sim.addDriverstation()
+
+// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
+// in order to make them all available at runtime. Also adding the manifest so WPILib
+// knows where to look for our Robot Class.
+jar {
+    from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
+    manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
+    duplicatesStrategy = DuplicatesStrategy.INCLUDE
+}
+
+// Configure jar and deploy tasks
+deployArtifact.jarTask = jar
+wpi.java.configureExecutableTasks(jar)
+wpi.java.configureTestTasks(test)
+
+// Configure string concat to always inline compile
+tasks.withType(JavaCompile) {
+    options.compilerArgs.add '-XDstringConcat=inline'
+}
@@ -0,0 +1,5 @@
+distributionBase=GRADLE_USER_HOME
+distributionPath=permwrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-7.5.1-bin.zip
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=permwrapper/dists
@@ -0,0 +1,240 @@
+#!/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 \
+        "$@"
+
+# 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" "$@"
@@ -0,0 +1,91 @@
+@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% equ 0 goto execute
+
+echo.
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+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% equ 0 goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+set EXIT_CODE=%ERRORLEVEL%
+if %EXIT_CODE% equ 0 set EXIT_CODE=1
+if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
+exit /b %EXIT_CODE%
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega
@@ -0,0 +1,27 @@
+import org.gradle.internal.os.OperatingSystem
+
+pluginManagement {
+    repositories {
+        mavenLocal()
+        gradlePluginPortal()
+        String frcYear = '2023'
+        File frcHome
+        if (OperatingSystem.current().isWindows()) {
+            String publicFolder = System.getenv('PUBLIC')
+            if (publicFolder == null) {
+                publicFolder = "C:\\Users\\Public"
+            }
+            def homeRoot = new File(publicFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        } else {
+            def userFolder = System.getProperty("user.home")
+            def homeRoot = new File(userFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        }
+        def frcHomeMaven = new File(frcHome, 'maven')
+        maven {
+            name 'frcHome'
+            url frcHomeMaven
+        }
+    }
+}
@@ -0,0 +1,3 @@
+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.
@@ -0,0 +1,152 @@
+/*----------------------------------------------------------------------------*/
+/* 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.robot;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+
+import frc4388.utility.LEDPatterns;
+import frc4388.utility.Gains;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants.  This class should not be used for any other purpose.  All constants should be
+ * declared globally (i.e. public static).  Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+    public static final class SwerveDriveConstants {
+
+        public static final double MAX_ROT_SPEED        = 1.5;
+        public static final double MIN_ROT_SPEED        = 0.8;
+        public static       double ROTATION_SPEED       = MAX_ROT_SPEED;
+        public static       double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
+    
+        public static final double CORRECTION_MIN = 10;
+        public static final double CORRECTION_MAX = 50;
+    
+        public static final double SLOW_SPEED = 0.8;
+        public static final double FAST_SPEED = 1.0;
+        public static final double TURBO_SPEED = 4.0;
+    
+        public static final class IDs {
+          public static final int LEFT_FRONT_WHEEL_ID = 2;
+          public static final int LEFT_FRONT_STEER_ID = 3;
+          public static final int LEFT_FRONT_ENCODER_ID = 10;
+    
+          public static final int RIGHT_FRONT_WHEEL_ID = 4;
+          public static final int RIGHT_FRONT_STEER_ID = 5;
+          public static final int RIGHT_FRONT_ENCODER_ID = 11;
+    
+          public static final int LEFT_BACK_WHEEL_ID = 6;
+          public static final int LEFT_BACK_STEER_ID = 7;
+          public static final int LEFT_BACK_ENCODER_ID = 12;
+          
+          public static final int RIGHT_BACK_WHEEL_ID = 8;
+          public static final int RIGHT_BACK_STEER_ID = 9;
+          public static final int RIGHT_BACK_ENCODER_ID = 13;
+        }
+    
+        public static final class PIDConstants {
+          public static final int SWERVE_SLOT_IDX = 0;
+          public static final int SWERVE_PID_LOOP_IDX = 1;
+          public static final Gains SWERVE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0);
+        }
+    
+        public static final class AutoConstants {
+          public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
+          public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
+          public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0);
+          public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune
+    
+          public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
+          public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
+        }
+    
+        public static final class Conversions {
+          public static final int CANCODER_TICKS_PER_ROTATION = 4096;
+    
+          public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8;
+          public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8;
+    
+          public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
+          public static final double MOTOR_REV_PER_STEER_REV = 12.8;
+    
+          public static final double TICKS_PER_MOTOR_REV = 2048;
+          public static final double WHEEL_DIAMETER_INCHES = 3.9;
+          public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
+    
+          public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV;
+          public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV;
+          public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV;
+          public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH;
+    
+          public static final double TICK_TIME_TO_SECONDS = 10;
+          public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
+        }
+    
+        public static final class Configurations {
+          public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
+          public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
+          public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value
+        }
+    
+        public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value
+        public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value
+    
+        // dimensions
+        public static final double WIDTH = 18.5;
+        public static final double HEIGHT = 18.5;
+        public static final double HALF_WIDTH = WIDTH / 2.d;
+        public static final double HALF_HEIGHT = HEIGHT / 2.d;
+    
+        // misc
+        public static final int TIMEOUT_MS = 30;
+        public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
+      }
+    
+      public static final class VisionConstants {
+        public static final String NAME = "photonCamera";
+    
+        public static final int LIME_HIXELS = 640;
+        public static final int LIME_VIXELS = 480;
+    
+        public static final double H_FOV = 59.6;
+        public static final double V_FOV = 45.7;
+    
+        public static final double LIME_HEIGHT = 6.0;
+        public static final double LIME_ANGLE = 55.0;
+    
+        // public static final double HIGH_TARGET_HEIGHT = 46.0;
+        public static final double HIGH_TAPE_HEIGHT = 44.0;
+        
+        // public static final double MID_TARGET_HEIGHT = 34.0;
+        public static final double MID_TAPE_HEIGHT = 24.0;
+    
+        public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
+    
+      }
+    
+    public static final class DriveConstants {
+        public static final int DRIVE_PIGEON_ID = 6;
+
+        public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
+    }
+    
+    public static final class LEDConstants {
+        public static final int LED_SPARK_ID = 0;
+
+        public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
+    }
+
+    public static final class OIConstants {
+        public static final int XBOX_DRIVER_ID = 0;
+        public static final int XBOX_OPERATOR_ID = 1;
+    }
+}
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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.robot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}
@@ -0,0 +1,131 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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.robot;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc4388.utility.DeferredBlock;
+import frc4388.utility.RobotTime;
+
+/**
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as described in the TimedRobot
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+  Command m_autonomousCommand;
+  
+  private RobotTime m_robotTime = RobotTime.getInstance();
+  private RobotContainer m_robotContainer;
+
+  /**
+   * This function is run when the robot is first started up and should be
+   * used for any initialization code.
+   */
+  @Override
+  public void robotInit() {
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
+  }
+
+  /**
+   * This function is called every robot packet, no matter the mode. Use
+   * this for items like diagnostics that you want ran during disabled,
+   * autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before
+   * LiveWindow and SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {
+    m_robotTime.updateTimes();
+    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+    // commands, running already-scheduled commands, removing finished or interrupted commands,
+    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // block in order for anything in the Command-based framework to work.
+    CommandScheduler.getInstance().run();
+  }
+
+  /**
+   * This function is called once each time the robot enters Disabled mode.
+   * You can use it to reset any subsystem information you want to clear when
+   * the robot is disabled.
+   */
+  @Override
+  public void disabledInit() {
+    m_robotTime.endMatchTime();
+  }
+
+  @Override
+  public void disabledPeriodic() {
+  }
+
+  @Override
+  public void disabledExit() {
+    DeferredBlock.execute();
+  }
+
+  /**
+   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+   */
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    /*
+     * String autoSelected = SmartDashboard.getString("Auto Selector",
+     * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+     * = new MyAutoCommand(); break; case "Default Auto": default:
+     * autonomousCommand = new ExampleCommand(); break; }
+     */
+
+    // schedule the autonomous command (example)
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.schedule();
+    }
+    m_robotTime.startMatchTime();
+  }
+
+  /**
+   * This function is called periodically during autonomous.
+   */
+  @Override
+  public void autonomousPeriodic() {
+  }
+
+  @Override
+  public void teleopInit() {
+    // This makes sure that the autonomous stops running when
+    // teleop starts running. If you want the autonomous to
+    // continue until interrupted by another command, remove
+    // this line or comment it out.
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+    m_robotTime.startMatchTime();
+  }
+
+  /**
+   * This function is called periodically during operator control.
+   */
+  @Override
+  public void teleopPeriodic() {
+    
+  }
+
+  /**
+   * This function is called periodically during test mode.
+   */
+  @Override
+  public void testPeriodic() {
+  }
+}
@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* 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.robot;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc4388.robot.Constants.*;
+import frc4388.robot.commands.Swerve.JoystickPlayback;
+import frc4388.robot.commands.Swerve.JoystickRecorder;
+import frc4388.robot.subsystems.LED;
+import frc4388.utility.LEDPatterns;
+import frc4388.utility.controller.IHandController;
+import frc4388.utility.controller.XboxController;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+    /* RobotMap */
+    private final RobotMap m_robotMap = new RobotMap();
+
+    /* Subsystems */
+    private final LED m_robotLED = new LED(m_robotMap.LEDController);
+
+    /* Controllers */
+    private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
+    private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
+
+    /**
+     * The container for the robot. Contains subsystems, OI devices, and commands.
+     */
+    public RobotContainer() {
+        configureButtonBindings();
+
+        /* Default Commands */
+        // drives the robot with a two-axis input from the driver controller
+        // continually sends updates to the Blinkin LED controller to keep the lights on
+        m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
+    }
+
+    /**
+     * Use this method to define your button->command mappings. Buttons can be
+     * created by instantiating a {@link GenericHID} or one of its subclasses
+     * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
+     * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+     */
+    private void configureButtonBindings() {
+        /* Driver Buttons */
+        // test command to spin the robot while pressing A on the driver controller
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
+        //     .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
+        //                                     () -> getDeadbandedDriverController().getLeftX(),
+        //                                     () -> getDeadbandedDriverController().getLeftY(),
+        //                                     () -> getDeadbandedDriverController().getRightX(),
+        //                                     () -> getDeadbandedDriverController().getRightY(),
+        //                                     "Blue1Path.txt"))
+        //     .onFalse(new InstantCommand());
+
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
+        //     .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"))
+        //     .onFalse(new InstantCommand()); 
+
+        /* Operator Buttons */
+        // activates "Lit Mode"
+        new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
+                .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
+                .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
+    }
+
+    /**
+     * Use this to pass the autonomous command to the main {@link Robot} class.
+     *
+     * @return the command to run in autonomous
+     */
+    public Command getAutonomousCommand() {
+        // no auto
+        return new InstantCommand();
+    }
+
+    /**
+     * Add your docs here.
+     */
+    public IHandController getDriverController() {
+        return m_driverXbox;
+    }
+
+    /**
+     * Add your docs here.
+     */
+    public IHandController getOperatorController() {
+        return m_operatorXbox;
+    }
+
+    /**
+     * Add your docs here.
+     */
+    public Joystick getOperatorJoystick() {
+        return m_operatorXbox.getJoyStick();
+    }
+
+    /**
+     * Add your docs here.
+     */
+    public Joystick getDriverJoystick() {
+        return m_driverXbox.getJoyStick();
+    }
+}
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* 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.robot;
+
+import com.ctre.phoenix.motorcontrol.InvertType;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix.sensors.PigeonIMU;
+
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import frc4388.robot.Constants.LEDConstants;
+import frc4388.utility.RobotGyro;
+
+/**
+ * Defines and holds all I/O objects on the Roborio. This is useful for unit
+ * testing and modularization.
+ */
+public class RobotMap {
+
+    public RobotMap() {
+        configureLEDMotorControllers();
+        configureDriveMotorControllers();
+    }
+
+    /* LED Subsystem */
+    public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
+
+    void configureLEDMotorControllers() {
+        
+    }
+
+    void configureDriveMotorControllers() {
+    }
+}
@@ -0,0 +1,91 @@
+package frc4388.robot.commands.Autos;
+
+import java.io.File;
+import java.util.ArrayList;
+import java.util.HashMap;
+
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc4388.robot.commands.Swerve.JoystickPlayback;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class PlaybackChooser {
+    private final ArrayList<SendableChooser<Command>> m_choosers    = new ArrayList<>();
+    private       SendableChooser<Command>            m_playback    = null;
+    private final ArrayList<ComplexWidget>            m_widgets     = new ArrayList<>();
+    private final HashMap<String, Command>            m_commandPool = new HashMap<>();
+    
+    private final File        m_dir    = new File("/home/lvuser/autos/");
+    private       int         m_cmdNum = 0;
+    private final SwerveDrive m_swerve;
+
+    // commands
+    private Command m_noAuto = new InstantCommand();
+    
+    public PlaybackChooser(SwerveDrive swerve, Object... pool) {
+        m_swerve = swerve;
+    }
+
+    public PlaybackChooser addOption(String name, Command option) {
+        m_commandPool.put(name, option);
+        return this;
+    }
+
+    public PlaybackChooser buildDisplay() {
+        for (int i = 0; i < 10; i++) {
+            appendCommand();
+        }
+        m_playback = m_choosers.get(0);
+        nextChooser();
+
+        Shuffleboard.getTab("Auto Chooser")
+            .add("Add Sequence", new InstantCommand(() -> nextChooser()))
+            .withPosition(4, 0);
+        return this;
+    }
+
+    // This will be bound to a button for the time being
+    public void appendCommand() {
+        var chooser = new SendableChooser<Command>();
+        chooser.setDefaultOption("No Auto", m_noAuto);
+
+        m_choosers.add(chooser);
+        ComplexWidget widget = Shuffleboard.getTab("Auto Chooser")
+            .add("Command: " + m_choosers.size(), chooser)
+            .withSize(4, 1)
+            .withPosition(0, m_choosers.size() - 1)
+            .withWidget(BuiltInWidgets.kSplitButtonChooser);
+
+        m_widgets.add(widget);
+    }
+
+    public void nextChooser() {
+        SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
+
+        for (String auto : m_dir.list()) {
+            chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
+        }
+        for (var cmd_name : m_commandPool.keySet()) {
+            chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
+        }
+    }
+
+    public Command getCommand() {
+        Command command = m_playback.getSelected();
+        command = command == null ? m_noAuto : command.asProxy();
+        
+        Command[] commands = new Command[m_cmdNum - 1];
+        for (int i = 0; i < m_cmdNum - 1; i++) {
+            Command command2 = m_choosers.get(i + 1).getSelected();
+            command2 = command2 == null ? m_noAuto : command2.asProxy();
+
+            commands[i] = command2.asProxy();
+        }
+
+        return command.andThen(commands);
+    }
+}
@@ -0,0 +1,225 @@
+0.0,0.0,0.0,0.0,0
+0.0,0.0,0.0,0.0,0
+0.0,0.0,0.0,0.0,12
+0.0,0.0,0.0,0.0,26
+0.0,0.0,0.0,0.0,37
+0.0,0.0,0.0,0.0,50
+0.0,0.0,0.0,0.0,62
+0.0,0.0,0.0,0.0,73
+0.0,0.0,0.0,0.0,88
+0.0,0.0,0.0,0.0,103
+0.0,0.0,0.0,0.0,116
+0.0,0.0,0.0,0.0,160
+0.0,0.0,0.0,0.0,173
+0.0,0.0,0.0,0.0,185
+0.0,0.0,0.0,0.0,200
+0.0,0.0,0.0,0.0,211
+0.0,0.0,0.0,0.0,223
+0.0,0.0,0.0,0.0,235
+0.0,0.0,0.0,0.0,247
+0.0,0.0,0.0,0.0,263
+0.0,0.0,0.0,0.0,283
+0.0,0.0,0.0,0.0,303
+0.0,-0.109375,0.0,0.0,323
+0.0,-0.1484375,0.0,0.0,343
+0.0,-0.2109375,0.0,0.0,363
+0.0,-0.3671875,0.0,0.0,398
+0.0,-0.4140625,0.0,0.0,411
+0.0,-0.4765625,0.0,0.0,425
+0.0,-0.5078125,0.0,0.0,443
+0.0,-0.5078125,0.0,0.0,463
+0.0,-0.53125,0.0,0.0,483
+0.0,-0.5546875,0.0,0.0,504
+0.0,-0.5625,0.0,0.0,523
+0.0,-0.5625,0.0,0.0,544
+0.0,-0.5703125,0.0,0.0,563
+0.0,-0.5859375,0.0,0.0,584
+0.0,-0.5859375,0.0,0.0,603
+0.0,-0.5859375,0.0,0.0,640
+0.0,-0.59375,0.0,0.0,657
+0.0,-0.6015625,0.0,0.0,672
+0.0,-0.6015625,0.0,0.0,685
+0.0,-0.6015625,0.0,0.0,703
+0.0,-0.6015625,0.0,0.0,723
+0.0,-0.6015625,0.0,0.0,743
+0.0,-0.6015625,0.0,0.0,763
+0.0,-0.6015625,0.0,0.0,783
+0.0,-0.6015625,0.0,0.0,803
+0.0,-0.6015625,0.0,0.0,823
+0.0,-0.6015625,0.0,0.0,844
+0.0,-0.6015625,0.0,0.0,878
+0.0,-0.6015625,0.0,0.0,893
+0.0,-0.6015625,0.0,0.0,907
+0.0,-0.6015625,0.0,0.0,924
+0.0,-0.609375,0.0,0.0,943
+0.0,-0.609375,0.0,0.0,963
+0.0,-0.609375,0.0,0.0,983
+0.0,-0.609375,0.0,0.0,1004
+0.0,-0.609375,0.0,0.0,1023
+0.0,-0.609375,0.0,0.0,1043
+0.0,-0.609375,0.0,0.0,1064
+0.0,-0.609375,0.0,0.0,1083
+0.0,-0.609375,0.0,0.0,1156
+0.0,-0.609375,0.0,0.0,1172
+0.0,-0.609375,0.0,0.0,1185
+0.0,-0.609375,0.0,0.0,1200
+0.0,-0.609375,0.0,0.0,1215
+0.0,-0.609375,0.0,0.0,1225
+0.0,-0.609375,0.0,0.0,1236
+0.0,-0.609375,0.0,0.0,1249
+0.0,-0.609375,0.0,0.0,1263
+0.0,-0.609375,0.0,0.0,1283
+0.0,-0.609375,0.0,0.0,1303
+0.0,-0.609375,0.0,0.0,1323
+0.0,-0.609375,0.0,0.0,1363
+0.0,-0.6015625,0.0,0.0,1376
+0.0,-0.6015625,0.0,0.0,1394
+0.0,-0.6015625,0.0,0.0,1405
+0.0,-0.6015625,0.0,0.0,1423
+0.0,-0.6015625,0.0,0.0,1443
+0.0,-0.6015625,0.0,0.0,1463
+0.0,-0.6015625,0.0,0.0,1483
+0.0,-0.6015625,0.0,0.0,1503
+0.0,-0.6015625,0.0,0.0,1523
+0.0,-0.6015625,0.0,0.0,1543
+0.0,-0.6015625,0.0,0.0,1563
+0.0,-0.6015625,0.0,0.0,1597
+0.0,-0.6015625,0.0,0.0,1608
+0.0,-0.6015625,0.0,0.0,1624
+0.0,-0.6015625,0.0,0.0,1643
+0.0,-0.6015625,0.0,0.0,1664
+0.0,-0.5859375,0.0,0.0,1683
+0.0,-0.5859375,0.0,0.0,1703
+0.0,-0.5625,0.0,0.0,1723
+0.0,-0.5625,0.0,0.0,1743
+0.0,-0.5625,0.0,0.0,1763
+0.0,-0.5625,0.0,0.0,1783
+0.0,-0.5625,0.0,0.0,1803
+0.0,-0.5625,0.0,0.0,1843
+0.0,-0.5625,0.0,0.0,1855
+0.0,-0.5625,0.0,0.0,1868
+0.0,-0.5625,0.0,0.0,1883
+0.0,-0.5625,0.0,0.0,1903
+0.0,-0.5625,0.0,0.0,1923
+0.0,-0.5625,0.0,0.0,1943
+0.0,-0.5625,0.0,0.0,1963
+0.0,-0.5625,0.0,0.0,1983
+0.0,-0.5625,0.0,0.0,2003
+0.0,-0.5625,0.0,0.0,2024
+0.0,-0.5625,0.0,0.0,2043
+0.0,-0.5625,0.0,0.0,2081
+0.0,-0.5625,0.0,0.0,2093
+0.0,-0.5625,0.0,0.0,2105
+0.0,-0.5625,0.0,0.0,2123
+0.0,-0.5625,0.0,0.0,2143
+0.0,-0.5625,0.0,0.0,2163
+0.0,-0.5625,0.0,0.0,2183
+0.0,-0.5625,0.0,0.0,2203
+0.0,-0.5625,0.0,0.0,2223
+0.0,-0.5625,0.0,0.0,2243
+0.0,-0.5625,0.0,0.0,2263
+0.0,-0.5625,0.0,0.0,2283
+0.0,-0.5625,0.0,0.0,2366
+0.0,-0.5625,0.0,0.0,2377
+0.0,-0.5625,0.0,0.0,2394
+0.0,-0.5703125,0.0,0.0,2405
+0.0,-0.5703125,0.0,0.0,2418
+0.0,-0.5703125,0.0,0.0,2431
+0.0,-0.5703125,0.0,0.0,2444
+0.0,-0.5703125,0.0,0.0,2458
+0.0,-0.5703125,0.0,0.0,2470
+0.0,-0.5703125,0.0,0.0,2485
+0.0,-0.5703125,0.0,0.0,2503
+0.0,-0.5703125,0.0,0.0,2523
+0.0,-0.5703125,0.0,0.0,2563
+0.0,-0.5703125,0.0,0.0,2577
+0.0,-0.5703125,0.0,0.0,2591
+0.0,-0.5703125,0.0,0.0,2608
+0.0,-0.5703125,0.0,0.0,2624
+0.0,-0.5703125,0.0,0.0,2643
+0.0,-0.5703125,0.0,0.0,2677
+0.0,-0.5703125,0.0,0.0,2698
+0.0,-0.5703125,0.0,0.0,2711
+0.0,-0.5703125,0.0,0.0,2725
+0.0,-0.5703125,0.0,0.0,2743
+0.0,-0.5703125,0.0,0.0,2764
+0.0,-0.5703125,0.0,0.0,2810
+0.0,-0.5703125,0.0,0.0,2820
+0.0,-0.5703125,0.0,0.0,2833
+0.0,-0.5703125,0.0,0.0,2845
+0.0,-0.5703125,0.0,0.0,2863
+0.0,-0.5703125,0.0,0.0,2883
+0.0,-0.5703125,0.0,0.0,2904
+0.0,-0.5703125,0.0,0.0,2924
+0.0,-0.5703125,0.0,0.0,2943
+0.0,-0.5703125,0.0,0.0,2963
+0.0,-0.5703125,0.0,0.0,2983
+0.0,-0.5703125,0.0,0.0,3003
+0.0,-0.5703125,0.0,0.0,3033
+0.0,-0.5703125,0.0,0.0,3050
+0.0,-0.5703125,0.0,0.0,3065
+0.0,-0.5703125,0.0,0.0,3083
+0.0,-0.5703125,0.0,0.0,3103
+0.0,-0.5703125,0.0,0.0,3123
+0.0,-0.5703125,0.0,0.0,3144
+0.0,-0.5703125,0.0,0.0,3164
+0.0,-0.5703125,0.0,0.0,3184
+0.0,-0.5703125,0.0,0.0,3203
+0.0,-0.5703125,0.0,0.0,3223
+0.0,-0.5703125,0.0,0.0,3243
+0.0,-0.5703125,0.0,0.0,3272
+0.0,-0.5703125,0.0,0.0,3289
+0.0,-0.5703125,0.0,0.0,3303
+0.0,-0.5703125,0.0,0.0,3323
+0.0,-0.5703125,0.0,0.0,3343
+0.0,-0.5703125,0.0,0.0,3363
+0.0,-0.5703125,0.0,0.0,3383
+0.0,-0.5703125,0.0,0.0,3403
+0.0,-0.5703125,0.0,0.0,3423
+0.0,-0.5703125,0.0,0.0,3443
+0.0,-0.5703125,0.0,0.0,3463
+0.0,-0.5703125,0.0,0.0,3483
+0.0,-0.5703125,0.0,0.0,3566
+0.0,-0.5703125,0.0,0.0,3578
+0.0,-0.5703125,0.0,0.0,3596
+0.0,-0.5703125,0.0,0.0,3610
+0.0,-0.5703125,0.0,0.0,3623
+0.0,-0.5703125,0.0,0.0,3640
+0.0,-0.5703125,0.0,0.0,3651
+0.0,-0.5703125,0.0,0.0,3663
+0.0,-0.5703125,0.0,0.0,3678
+0.0,-0.5703125,0.0,0.0,3691
+0.0,-0.5703125,0.0,0.0,3706
+0.0,-0.5703125,0.0,0.0,3723
+0.0,-0.5703125,0.0,0.0,3766
+0.0,-0.5703125,0.0,0.0,3778
+0.0,-0.5703125,0.0,0.0,3792
+0.0,-0.5703125,0.0,0.0,3807
+0.0,-0.5703125,0.0,0.0,3823
+0.0,-0.5703125,0.0,0.0,3843
+0.0,-0.53125,0.0,0.0,3863
+0.0,-0.53125,0.0,0.0,3884
+0.0,-0.421875,0.0,0.0,3904
+0.0,0.0,0.0,0.0,3924
+0.0,0.0,0.0,0.0,3944
+0.0,0.0,0.0,0.0,3963
+0.0,0.0,0.0,0.0,3999
+0.0,0.0,0.0,0.0,4010
+0.0,0.0,0.0,0.0,4025
+0.0,0.0,0.0,0.0,4043
+0.0,0.0,0.0,0.0,4063
+0.0,0.0,0.0,0.0,4083
+0.0,0.0,0.0,0.0,4103
+0.0,0.0,0.0,0.0,4123
+0.0,0.0,0.0,0.0,4143
+0.0,0.0,0.0,0.0,4163
+0.0,0.0,0.0,0.0,4183
+0.0,0.0,0.0,0.0,4203
+0.0,0.0,0.0,0.0,4236
+0.0,0.0,0.0,0.0,4247
+0.0,0.0,0.0,0.0,4264
+0.0,0.0,0.0,0.0,4284
+0.0,0.0,0.0,0.0,4304
+0.0,0.0,0.0,0.0,4324
+0.0,0.0,0.0,0.0,4343
+0.0,0.0,0.0,0.0,4363
@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.utility.Gains;
+
+public abstract class PID extends CommandBase {
+	protected Gains  gains;
+	private   double output    = 0;
+	private   double tolerance = 0;
+
+	/** Creates a new PelvicInflammatoryDisease. */
+	public PID(double kp, double ki, double kd, double kf, double tolerance) {
+		gains          = new Gains(kp, ki, kd, kf, 0);
+		this.tolerance = tolerance;
+	}
+
+	public PID(Gains gains, double tolerance) {
+		this.gains     = gains;
+		this.tolerance = tolerance;
+	}
+
+	/** produces the error from the setpoint */
+	public abstract double getError();
+	
+	/** figure it out bitch */
+	public abstract void runWithOutput(double output);
+
+	// Called when the command is initially scheduled.
+	@Override 
+	public final void initialize() {
+		output = 0;
+	}
+
+	private double prevError, cumError = 0;
+	
+	// Called every time the scheduler runs while the command is scheduled.
+	@Override 
+	public final void execute() {
+		double error = getError();
+		cumError += error * .02; // 20 ms
+		double delta = error - prevError;
+
+		output = error * gains.kP;
+		output += cumError * gains.kI;
+		output += delta * gains.kD;
+		output += gains.kF;
+
+		runWithOutput(output);
+	}
+
+	// Returns true when the command should end.
+	@Override
+	public final boolean isFinished() {
+		return Math.abs(getError()) < tolerance;
+	}
+}
@@ -0,0 +1,141 @@
+// 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.robot.commands.Swerve;
+
+import java.io.File;
+import java.io.FileNotFoundException;
+import java.util.ArrayList;
+import java.util.Scanner;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.UtilityStructs.TimedOutput;
+
+public class JoystickPlayback extends CommandBase {
+  private final SwerveDrive            swerve;
+  private       String                 filename;
+  private       int                    mult       = 1;
+  private       Scanner                input;
+  private final ArrayList<TimedOutput> outputs      = new ArrayList<>();
+  private       int                    counter      = 0;
+  private       long                   startTime    = 0;
+  private       long                   playbackTime = 0;
+  private       int                    lastIndex;
+  private       boolean                m_finished   = false; // ! find a better way
+
+  /** Creates a new JoystickPlayback. */
+  public JoystickPlayback(SwerveDrive swerve, String filename, int mult) {
+    this.swerve = swerve;
+    this.filename = filename;
+    this.mult = mult;
+
+    addRequirements(this.swerve);
+  }
+
+  /** Creates a new JoystickPlayback. */
+  public JoystickPlayback(SwerveDrive swerve, String filename) {
+    this(swerve, filename, 1);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    outputs.clear();
+    m_finished = false;
+
+    startTime = System.currentTimeMillis();
+    playbackTime = 0;
+    lastIndex    = 0;
+    try {
+      input = new Scanner(new File("/home/lvuser/autos/" + filename));
+
+      String line = "";
+      while (input.hasNextLine()) {
+        line = input.nextLine();
+
+        if (line.isEmpty() || line.isBlank() || line.equals("\n")) {
+          continue;
+        }
+      
+        String[] values = line.split(",");
+        System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]);
+
+        var out = new TimedOutput();
+        out.leftX  = Double.parseDouble(values[0]) * mult;
+        out.leftY  = Double.parseDouble(values[1]);
+        out.rightX = Double.parseDouble(values[2]);
+        out.rightY = Double.parseDouble(values[3]);
+
+        out.timedOffset = Long.parseLong(values[4]);
+
+        outputs.add(out);
+      }
+
+      input.close();
+    } catch (FileNotFoundException e) {
+      e.printStackTrace();
+    }
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (counter == 0) {
+      startTime = System.currentTimeMillis();
+      playbackTime = 0;
+    } else {
+      playbackTime = System.currentTimeMillis() - startTime;
+    }
+
+    // skip to reasonable time frame
+    // too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing
+    {
+      int i = lastIndex == 0 ? 1 : lastIndex;
+      while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) {
+        i++;
+      }
+
+      if (i >= outputs.size()) {
+        m_finished = true; // ! kind of a hack
+        return;
+      }
+      lastIndex = i;
+    }
+
+    TimedOutput lastOut = outputs.get(lastIndex - 1);
+    TimedOutput out     = outputs.get(lastIndex);
+
+    double deltaTime     = out.timedOffset - lastOut.timedOffset;
+    double playbackDelta = playbackTime    - lastOut.timedOffset;
+
+    double lerpLX = lastOut.leftX  + (out.leftX  - lastOut.leftX)  * (playbackDelta / deltaTime);
+    double lerpLY = lastOut.leftY  + (out.leftY  - lastOut.leftY)  * (playbackDelta / deltaTime);
+    double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime);
+    double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime);
+
+    // this.swerve.driveWithInput(new Translation2d(out.leftX,   out.leftY),
+    //                              new Translation2d(out.rightX, out.rightY),
+    //                              true);
+    
+    this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY),
+                                new Translation2d(lerpRX, lerpRY),
+                                true);
+                             
+    counter++;
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    input.close();
+    swerve.stopModules();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return m_finished;
+  }
+}
@@ -0,0 +1,97 @@
+// 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.robot.commands.Swerve;
+
+import java.io.File;
+import java.io.IOException;
+import java.io.PrintWriter;
+import java.util.ArrayList;
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.UtilityStructs.TimedOutput;
+
+public class JoystickRecorder extends CommandBase {
+  public  final SwerveDrive            swerve;
+
+  public  final Supplier<Double>       leftX;
+  public  final Supplier<Double>       leftY;
+  public  final Supplier<Double>       rightX;
+  public  final Supplier<Double>       rightY;
+  private       String                 filename;
+  public  final ArrayList<TimedOutput> outputs   = new ArrayList<>();
+  private       long                   startTime = -1;
+
+
+  /** Creates a new JoystickRecorder. */
+  public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftX,  Supplier<Double> leftY,
+                                              Supplier<Double> rightX, Supplier<Double> rightY,
+                                              String filename)
+  {
+    this.swerve = swerve;
+    this.leftX  = leftX;
+    this.leftY  = leftY;
+    this.rightX = rightX;
+    this.rightY = rightY;
+    this.filename = filename;
+
+    addRequirements(this.swerve);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    outputs.clear();
+
+    this.startTime = System.currentTimeMillis();
+
+    outputs.add(new TimedOutput());
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    var inputs = new TimedOutput();
+    inputs.leftX       = leftX.get();
+    inputs.leftY       = leftY.get();
+    inputs.rightX      = rightX.get();
+    inputs.rightY      = rightY.get();
+    inputs.timedOffset = System.currentTimeMillis() - startTime;
+
+    outputs.add(inputs);
+
+    swerve.driveWithInput(new Translation2d(inputs.leftX,  inputs.leftY),
+                          new Translation2d(inputs.rightX, inputs.rightY),
+                          true);
+    
+    System.out.println("RECORDING");
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    File output = new File("/home/lvuser/autos/" + filename);
+
+    try (PrintWriter writer = new PrintWriter(output)) {
+      for (var input : outputs) {
+        writer.println( input.leftX  + "," + input.leftY  + "," +
+                        input.rightX + "," + input.rightY + "," +
+                        input.timedOffset);
+      }
+
+      writer.close();
+    } catch (IOException e) {
+        e.printStackTrace();
+    }
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
@@ -0,0 +1,35 @@
+// 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.robot.commands.Swerve;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import frc4388.robot.commands.PID;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class RotateToAngle extends PID {
+
+  SwerveDrive drive;
+  double targetAngle;
+
+  /** Creates a new RotateToAngle. */
+  public RotateToAngle(SwerveDrive drive, double targetAngle) {
+    super(0.3, 0.0, 0.0, 0.0, 1);
+    
+    this.drive = drive;
+    this.targetAngle = targetAngle;
+
+    addRequirements(drive);
+  }
+
+  @Override
+  public double getError() {
+    return targetAngle - drive.getGyroAngle();
+  }
+
+  @Override
+  public void runWithOutput(double output) {
+    drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true);    
+  }
+}
@@ -0,0 +1,36 @@
+package frc4388.robot.subsystems;
+
+//import edu.wpi.first.apriltag.AprilTag;
+//import edu.wpi.first.math.geometry.Pose3d;
+//import edu.wpi.first.math.geometry.Rotation3d;
+//import edu.wpi.first.networktables.NetworkTable;
+//import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+public class Apriltags {
+    public static class Tag {
+        public boolean visible = true;
+        public double x,  y,  z  = 0;
+        public double ry, rp, rr = 0;
+    }
+
+    public Tag getTagPosRot() {
+        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
+
+        final Tag tag = new Tag();
+        tag.visible = isAprilTag();
+        tag.x  = tagTable.getEntry("TagPosX").getDouble(0);
+        tag.y  = tagTable.getEntry("TagPosY").getDouble(0);
+        tag.z  = tagTable.getEntry("TagPosZ").getDouble(0);
+        tag.ry = tagTable.getEntry("TagRotY").getDouble(0);
+        tag.rp = tagTable.getEntry("TagRotP").getDouble(0);
+        tag.rr = tagTable.getEntry("TagRotR").getDouble(0);
+
+        return tag;
+    }
+
+    public boolean isAprilTag() {
+        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
+        return tagTable.getEntry("IsTag").getBoolean(false);
+    }
+}
@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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.robot.subsystems;
+
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import frc4388.robot.Constants.DriveConstants;
+import frc4388.utility.RobotGyro;
+import frc4388.utility.RobotTime;
+
+/**
+ * Add your docs here.
+ */
+public class DiffDrive extends SubsystemBase {
+  // Put methods for controlling this subsystem
+  // here. Call these from Commands.
+
+  private RobotTime m_robotTime = RobotTime.getInstance();
+
+  private WPI_TalonFX m_leftFrontMotor;
+  private WPI_TalonFX m_rightFrontMotor;
+  private WPI_TalonFX m_leftBackMotor;
+  private WPI_TalonFX m_rightBackMotor;
+  private DifferentialDrive m_driveTrain;
+  private RobotGyro m_gyro;
+
+  /**
+   * Add your docs here.
+   */
+  public DiffDrive(WPI_TalonFX leftFrontMotor, WPI_TalonFX rightFrontMotor, WPI_TalonFX leftBackMotor,
+      WPI_TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
+
+    m_leftFrontMotor = leftFrontMotor;
+    m_rightFrontMotor = rightFrontMotor;
+    m_leftBackMotor = leftBackMotor;
+    m_rightBackMotor = rightBackMotor;
+    m_driveTrain = driveTrain;
+    m_gyro = gyro;
+  }
+
+  @Override
+  public void periodic() {
+    m_gyro.updatePigeonDeltas();
+
+    if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
+      updateSmartDashboard();
+    }
+  }
+
+  /**
+   * Add your docs here.
+   */
+  public void driveWithInput(double move, double steer) {
+    m_driveTrain.arcadeDrive(move, steer);
+  }
+
+  /**
+   * Add your docs here.
+   */
+  public void tankDriveWithInput(double leftMove, double rightMove) {
+    m_leftFrontMotor.set(leftMove);
+    m_rightFrontMotor.set(rightMove);
+  }
+
+  /**
+   * Add your docs here.
+   */
+  private void updateSmartDashboard() {
+
+    // Examples of the functionality of RobotGyro
+    SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon);
+    SmartDashboard.putNumber("Turn Rate", m_gyro.getRate());
+    SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
+    //SmartDashboard.putData(m_gyro);
+  }
+}
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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.robot.subsystems;
+
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import frc4388.robot.Constants.LEDConstants;
+import frc4388.utility.LEDPatterns;
+
+/**
+ * Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED
+ * Driver
+ */
+public class LED extends SubsystemBase {
+
+  private LEDPatterns m_currentPattern;
+  private Spark m_LEDController;
+
+  /**
+   * Add your docs here.
+   */
+  public LED(Spark LEDController){
+    m_LEDController = LEDController;
+    setPattern(LEDConstants.DEFAULT_PATTERN);
+    updateLED();
+    System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
+  }
+
+  @Override
+  public void periodic(){
+    SmartDashboard.putNumber("LED", m_currentPattern.getValue());
+  }
+
+  /**
+   * Add your docs here.
+   */
+  public void updateLED(){
+    m_LEDController.set(m_currentPattern.getValue());
+  }
+
+  /**
+   * Add your docs here.
+   */
+  public void setPattern(LEDPatterns pattern){
+    m_currentPattern = pattern;
+    m_LEDController.set(m_currentPattern.getValue());
+  }
+
+  /**
+   * Add your docs here.
+   * @return
+   */
+  public LEDPatterns getPattern() {
+    return m_currentPattern;
+  }
+}
@@ -0,0 +1,165 @@
+// 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.robot.subsystems;
+
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Optional;
+
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.common.hardware.VisionLEDMode;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.targeting.TargetCorner;
+
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.VisionConstants;
+
+public class Limelight extends SubsystemBase {
+  
+  private PhotonCamera cam;
+  private PhotonPoseEstimator photonPoseEstimator;
+
+  private boolean lightOn;
+
+  /** Creates a new Limelight. */
+  public Limelight() {
+    cam = new PhotonCamera(VisionConstants.NAME);
+    cam.setDriverMode(false);
+  }
+
+  public void setLEDs(boolean on) {
+    lightOn = on;
+    cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
+  }
+
+  public void toggleLEDs() {
+    lightOn = !lightOn;
+    cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
+  }
+
+  public void setDriverMode(boolean driverMode) {
+    cam.setDriverMode(driverMode);
+  }
+
+  public void setToLimePipeline() {
+    cam.setPipelineIndex(1);
+    setLEDs(true);
+  }
+
+  public void setToAprilPipeline() {
+    cam.setPipelineIndex(0);
+    setLEDs(false);
+  }
+
+  public PhotonTrackedTarget getAprilPoint() {
+    if (!cam.isConnected()) return null;
+
+    PhotonPipelineResult result = cam.getLatestResult();
+
+    if (!result.hasTargets()) return null;
+
+    return result.getBestTarget();
+  }
+
+  private List<TargetCorner> getAprilCorners() {
+    if (!cam.isConnected()) return null;
+
+    PhotonPipelineResult result = cam.getLatestResult();
+
+    if (!result.hasTargets()) return null;
+
+    return result.getBestTarget().getDetectedCorners();
+  }
+
+  public double getAprilSkew() {
+    List<TargetCorner> corners = getAprilCorners();
+    ArrayList<TargetCorner> bottomSide = getAprilBottomSide(corners);
+
+    if (bottomSide == null) return 0;
+
+    TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1);
+    TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0);
+
+    return bottomLeft.y - bottomRight.y;
+  }
+
+  private ArrayList<TargetCorner> getAprilBottomSide(List<TargetCorner> box) {
+    if (box == null) return null;
+
+    ArrayList<TargetCorner> bottomSide = new ArrayList<>();
+
+    TargetCorner l1 = new TargetCorner(-1, -1);
+    TargetCorner l2 = new TargetCorner(-1, -1);
+    
+    for (TargetCorner c : box) {
+      if (c.y > l1.y) l1 = c;
+    }
+
+    for (TargetCorner c : box) {
+      if (c.y == l1.y) continue;
+      if (c.y > l2.y) l2 = c;
+    }
+
+    bottomSide.add(l1);
+    bottomSide.add(l2);
+
+    return bottomSide;
+  }
+
+  public double getDistanceToApril() {    
+    PhotonTrackedTarget aprilPoint = getAprilPoint();
+    if (aprilPoint == null) return -1;
+
+    double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT;
+    double theta = 35.0 + aprilPoint.getPitch();
+
+    double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta));
+    return distanceToApril;
+  }
+
+  public PhotonTrackedTarget getLowestTape() {
+    if (!cam.isConnected()) return null;
+
+    PhotonPipelineResult result = cam.getLatestResult();
+
+    if (!result.hasTargets()) return null;
+
+    ArrayList<PhotonTrackedTarget> points = (ArrayList<PhotonTrackedTarget>) result.getTargets();
+
+    PhotonTrackedTarget lowest = points.get(0);
+    for (PhotonTrackedTarget point : points) {
+      if (point.getPitch() < lowest.getPitch()) {
+        lowest = point;
+      }
+    }
+
+    return lowest;
+  }
+
+  public double getDistanceToTape() {    
+    PhotonTrackedTarget tapePoint = getLowestTape();
+    if (tapePoint == null) return -1;
+
+    double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT;
+    double theta = 35.0 + tapePoint.getPitch();
+
+    double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta));
+    return distanceToTape;
+  }
+
+  @Override
+  public void periodic() {}
+}
@@ -0,0 +1,195 @@
+// 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.robot.subsystems;
+
+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.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.utility.RobotGyro;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class SwerveDrive extends SubsystemBase {
+  
+  private SwerveModule leftFront;
+  private SwerveModule rightFront;
+  private SwerveModule leftBack;
+  private SwerveModule rightBack;
+
+  private SwerveModule[] modules;
+
+  private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
+  private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
+  private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
+  private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
+  
+  private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
+
+  private RobotGyro gyro;
+
+  public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
+  
+  public double rotTarget = 0.0;
+  public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
+
+  /** Creates a new SwerveDrive. */
+  public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
+    this.leftFront = leftFront;
+    this.rightFront = rightFront;
+    this.leftBack = leftBack;
+    this.rightBack = rightBack;
+    
+    this.gyro = gyro;
+
+    this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
+  }
+
+  boolean stopped = false;
+  public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
+    if (fieldRelative) {
+
+      double rot = 0;
+      
+      if (rightStick.getNorm() > 0.05) {
+        rotTarget = gyro.getAngle();
+        rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
+        SmartDashboard.putBoolean("drift correction", false);
+        stopped = false;
+      } else if(leftStick.getNorm() > 0.05) {
+        if (!stopped) {
+          stopModules();
+          stopped = true;
+        }
+
+        SmartDashboard.putBoolean("drift correction", true);
+        rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
+
+      }
+
+      // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
+      Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
+      // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
+
+      // Convert field-relative speeds to robot-relative speeds.
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
+    } else {
+      // Create robot-relative speeds.
+      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
+    }
+    setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
+  }
+
+  /**
+   * Set each module of the swerve drive to the corresponding desired state.
+   * @param desiredStates Array of module states to set.
+   */
+  public void setModuleStates(SwerveModuleState[] desiredStates) {
+    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
+    for (int i = 0; i < desiredStates.length; i++) {
+      SwerveModule module = modules[i];
+      SwerveModuleState state = desiredStates[i];
+      module.setDesiredState(state);
+    }
+  }
+
+  public boolean rotateToTarget(double angle) {
+    double currentAngle = getGyroAngle();
+    double error = angle - currentAngle;
+
+    driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true);
+
+    if (Math.abs(angle - getGyroAngle()) < 5.0) {
+      return true;
+    }
+
+    return false;
+  }
+
+  public double getGyroAngle() {
+    return gyro.getAngle();
+  }
+
+  public void resetGyro() {
+    gyro.reset();
+    rotTarget = 0.0;
+  }
+  
+  public void stopModules() {
+    for (SwerveModule module : this.modules) {
+      module.stop();
+    }
+  }
+
+  public SwerveDriveKinematics getKinematics() {
+    return this.kinematics;
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run\
+    SmartDashboard.putNumber("Gyro", getGyroAngle());
+  }
+
+  public void shiftDown() {
+    if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
+      
+    } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
+      this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
+    } else {
+      this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
+    }
+  }
+
+  public void setToSlow() {
+    this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
+    System.out.println("SLOW");
+    System.out.println("SLOW");
+    System.out.println("SLOW");
+    System.out.println("SLOW");
+    System.out.println("SLOW");
+  }
+
+  public void setToFast() {
+    this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
+    System.out.println("FAST");
+    System.out.println("FAST");
+    System.out.println("FAST");
+    System.out.println("FAST");
+    System.out.println("FAST");
+  }
+
+  public void setToTurbo() {
+    this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
+    System.out.println("TURBO");
+    System.out.println("TURBO");
+    System.out.println("TURBO");
+    System.out.println("TURBO");
+    System.out.println("TURBO");
+  }
+
+  public void shiftUp() {
+    if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
+      this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
+    } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
+      this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
+    } else {
+      
+    }
+  }
+
+  public void toggleGear(double angle) {
+    if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) {
+      this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
+      SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
+    } else {
+      this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
+      SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
+    }
+  }
+
+}
@@ -0,0 +1,161 @@
+// 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.robot.subsystems;
+
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
+import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix.sensors.CANCoder;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.utility.Gains;
+
+public class SwerveModule extends SubsystemBase {
+    private WPI_TalonFX driveMotor;
+    private WPI_TalonFX angleMotor;
+    private CANCoder encoder;
+
+    public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
+  
+    /** Creates a new SwerveModule. */
+    public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) {
+        this.driveMotor = driveMotor;
+        this.angleMotor = angleMotor;
+        this.encoder = encoder;
+
+        TalonFXConfiguration angleConfig = new TalonFXConfiguration();
+        angleConfig.slot0.kP = swerveGains.kP;
+        angleConfig.slot0.kI = swerveGains.kI;
+        angleConfig.slot0.kD = swerveGains.kD;
+
+        // use the CANcoder as the remote sensor for the primary TalonFX PID
+        angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
+        angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
+        angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
+        angleMotor.configAllSettings(angleConfig);
+
+        encoder.configMagnetOffset(offset);
+        
+        driveMotor.setSelectedSensorPosition(0);
+        driveMotor.config_kP(0, 0.2);
+    }
+
+    /**
+     * Get the drive motor of the SwerveModule
+     * @return the drive motor of the SwerveModule
+     */
+    public WPI_TalonFX getDriveMotor() {
+        return this.driveMotor;
+    }
+
+    /**
+     * Get the angle motor of the SwerveModule
+     * @return the angle motor of the SwerveModule
+     */
+    public WPI_TalonFX getAngleMotor() {
+        return this.angleMotor;
+    }
+
+    /**
+     * Get the CANcoder of the SwerveModule
+     * @return the CANcoder of the SwerveModule
+     */
+    public CANCoder getEncoder() {
+        return this.encoder;
+    }
+
+    /**
+     * Get the angle of a SwerveModule as a Rotation2d
+     * @return the angle of a SwerveModule as a Rotation2d
+     */
+    public Rotation2d getAngle() {
+        // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
+        return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
+    }
+    
+    public double getAngularVel() {
+        return this.angleMotor.getSelectedSensorVelocity();
+    }
+
+    public double getDrivePos() {
+        return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
+    }
+
+    public double getDriveVel() {
+        return this.driveMotor.getSelectedSensorVelocity(0);
+    }
+
+    public void stop() {
+        driveMotor.set(0);
+        angleMotor.set(0);
+    }
+
+    public void rotateToAngle(double angle) {
+        angleMotor.set(TalonFXControlMode.Position, angle);
+    }
+
+    /**
+     * Get state of swerve module
+     * @return speed in m/s and angle in degrees
+     */
+    public SwerveModuleState getState() {
+        return new SwerveModuleState(
+            Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, 
+            getAngle()
+        );
+    }
+
+    /**
+     * Returns the current position of the SwerveModule
+     * @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor.
+     */
+    public SwerveModulePosition getPosition() {
+        return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle());
+    }
+
+    /**
+     * Set the speed and rotation of the SwerveModule from a SwerveModuleState object
+     * @param desiredState a SwerveModuleState representing the desired new state of the module
+     */
+    public void setDesiredState(SwerveModuleState desiredState) {
+        Rotation2d currentRotation = this.getAngle();
+
+        SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
+
+        // calculate the difference between our current rotational position and our new rotational position
+        Rotation2d rotationDelta = state.angle.minus(currentRotation);
+
+        // calculate the new absolute position of the SwerveModule based on the difference in rotation
+        double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
+
+        // convert the CANCoder from its position reading to ticks
+        double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
+
+        angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
+
+        double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
+
+        driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
+    }
+
+    public void reset(double position) {
+        encoder.setPositionToAbsolute();
+    }
+
+    public double getCurrent() {
+        return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
+    }
+
+    public double getVoltage() {
+        return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
+    }
+}
@@ -0,0 +1,38 @@
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+public class Vision {
+    private final NetworkTableEntry m_isTags;
+    private final NetworkTableEntry m_xPoses;
+    private final NetworkTableEntry m_yPoses;
+    private final NetworkTableEntry m_zPoses;
+
+    public Vision() {
+        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
+
+        m_isTags = tagTable.getEntry("IsTag");
+        m_xPoses = tagTable.getEntry("TagPosX");
+        m_yPoses = tagTable.getEntry("TagPosY");
+        m_zPoses = tagTable.getEntry("TagPosZ");
+    }
+
+    public AprilTag[] getAprilTags() {
+        if (!m_isTags.getBoolean(false)) return new AprilTag[0];
+
+        double xarr[] = m_xPoses.getDoubleArray(new double[] {});
+        double yarr[] = m_yPoses.getDoubleArray(new double[] {});
+        double zarr[] = m_zPoses.getDoubleArray(new double[] {});
+
+        AprilTag tags[] = new AprilTag[xarr.length];
+        for (int i = 0; i < tags.length; i++) {
+            tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d()));
+        }
+
+        return tags;
+    }
+}
@@ -0,0 +1,13 @@
+package frc4388.utility;
+
+// This is a seperate class in case I want to encode rotation or other
+// information about the tag
+public class AprilTag {
+    public final double x, y, z;
+
+    public AprilTag(double _x, double _y, double _z) {
+        x = _x;
+        y = _y;
+        z = _z;
+    }
+}
@@ -0,0 +1,23 @@
+package frc4388.utility;
+
+import java.util.ArrayList;
+
+public class DeferredBlock {
+    private static ArrayList<Runnable> m_blocks = new ArrayList<>();
+    private static boolean             m_hasRun = false;
+
+    public DeferredBlock(Runnable block) {
+        m_blocks.add(block);
+    }
+
+    public static void execute() {
+        if (m_hasRun) return;
+
+        for (Runnable block : m_blocks) {
+            block.run();
+        }
+
+        m_blocks.clear(); // for garbage collection
+        m_hasRun = true;
+    }
+}
@@ -0,0 +1,83 @@
+// 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 Gains {
+    public double kP;
+    public double kI;
+    public double kD;
+    public double kF;
+    public int kIZone;
+    public double kPeakOutput;
+    public double kMaxOutput;
+    public double kMinOutput;
+
+    /**
+     * Creates Gains object for PIDs
+     * @param kP The P value.
+     * @param kI The I value.
+     * @param kD The D value.
+     * @param kF The F value.
+     * @param kIZone The zone of the I value.
+     * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0.
+     */
+    public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kF = kF;
+        this.kIZone = kIZone;
+        this.kPeakOutput = kPeakOutput;
+        this.kMaxOutput = kPeakOutput;
+        this.kMinOutput = -kPeakOutput;
+    }
+
+    /**
+     * Creates Gains object for PIDs
+     * @param kP The P value.
+     * @param kI The I value.
+     * @param kD The D value.
+     * @param kF The F value.
+     * @param kIZone The zone of the I value.
+     */
+    public Gains(double kP, double kI, double kD, double kF, int kIZone) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kF = kF;
+        this.kIZone = kIZone;
+        this.kPeakOutput = 1.0;
+        this.kMaxOutput = 1.0;
+        this.kMinOutput = -1.0;
+    }
+
+    public Gains(double kP, double kI, double kD) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+    }
+
+    /**
+     * Creates Gains object for PIDs
+     * @param kP The P value.
+     * @param kI The I value.
+     * @param kD The D value.
+     * @param kF The F value.
+     * @param kIZone The zone of the I value.
+     * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0.
+     * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0.
+     */
+    public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kF = kF;
+        this.kIZone = kIZone;
+        this.kMaxOutput = kMaxOutput;
+        this.kMinOutput = kMinOutput;
+        this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput);
+    }    
+}
@@ -0,0 +1,45 @@
+package frc4388.utility;
+
+/**
+ * Add your docs here.
+ */
+public enum LEDPatterns {
+  /* PALLETTE PATTERNS */
+  RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f),
+    RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f),
+    PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f),
+    PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f),
+    RAINBOW_TWINKLES(-0.55f), PARTY_TWINKLES(-0.53f), OCEAN_TWINKLES(-0.51f), LAVA_TWINKLES(-0.49f), FOREST_TWINKLES(-0.47f),
+    RAINBOW_WAVES(-0.45f), PARTY_WAVES(-0.43f), OCEAN_WAVES(-0.41f), LAVA_WAVES(-0.39f), FOREST_WAVES(-0.37f),
+    RED_SCANNER(-0.35f), GRAY_SCANNER(-0.33f), RED_CHASE(-0.31f), BLUE_CHASE(-0.29f), GRAY_CHASE(-0.27f), RED_HEARTBEAT(-0.25f),
+    BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f),
+    GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f),
+  
+  /* COLOR 1 PATTERNS */
+  C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f),
+    C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f),
+    
+  /* COLOR 2 PATTERNS */
+  C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f), 
+    C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f),
+    
+  /* COLOR 1 AND 2 PATTERNS */
+  C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f),
+    C1C2_WAVES(0.53f), C1C2_SINELON(0.55f),
+     
+  /* SOLID COLORS */
+  SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f),
+    SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f),
+    SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f),
+    SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f),
+    SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f);
+  
+  /* GETTERS/SETTERS */
+  private final float id;
+  LEDPatterns(float id) {
+    this.id = id;
+  }
+  public float getValue() {
+    return id;
+  }
+}
@@ -0,0 +1,200 @@
+/*----------------------------------------------------------------------------*/
+/* 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 com.ctre.phoenix.sensors.WPI_Pigeon2;
+import com.kauailabs.navx.frc.AHRS;
+
+// import edu.wpi.first.wpilibj.GyroBase;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.math.MathUtil;
+
+/**
+ * Gyro class that allows for interchangeable use between a pigeon and a navX
+ */
+public class RobotGyro implements Gyro {
+    private RobotTime m_robotTime = RobotTime.getInstance();
+
+    private WPI_Pigeon2 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;
+
+    private double pitchZero = 0;
+    private double rollZero = 0;
+
+    /**
+     * Creates a Gyro based on a pigeon
+     * @param gyro the gyroscope to use for Gyro
+     */
+    public RobotGyro(WPI_Pigeon2 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;
+    }
+
+    /**
+     * Resets yaw, pitch, and roll.
+     */
+    public void resetZeroValues() {
+        if (!m_isGyroAPigeon) return;
+
+        pitchZero = m_pigeon.getPitch();
+        rollZero =  m_pigeon.getRoll();
+    }
+
+    /**
+     * 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.calibrate();
+        } else {
+            m_navX.calibrate();
+        }
+    }
+
+    @Override
+    public void reset() {
+        resetZeroValues();
+
+        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[] ypr = new double[3];
+        m_pigeon.getYawPitchRoll(ypr);
+
+        return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)};
+    }
+
+    @Override
+    public double getAngle() {
+        if (m_isGyroAPigeon) {
+            return getPigeonAngles()[0];
+        } else {
+            return m_navX.getAngle();
+        }
+    }
+
+    public double getYaw() {
+       return this.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 WPI_Pigeon2 getPigeon(){
+        return m_pigeon;
+    }
+
+    public AHRS getNavX(){
+        return m_navX;
+    }
+
+    @Override
+    public void close() throws Exception {
+
+    }
+}
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+/**
+ * <p>Keeps track of Robot times like time passed, delta time, etc
+ * <p>All times are in milliseconds
+ */
+public class RobotTime {
+    private long m_currTime = System.currentTimeMillis();
+    public long m_deltaTime = 0;
+
+    private long m_startRobotTime = m_currTime;
+    public long m_robotTime = 0;
+    public long m_lastRobotTime = 0;
+
+    private long m_startMatchTime = 0;
+    public  long m_matchTime = 0;
+    public long m_lastMatchTime = 0;
+
+    public long m_frameNumber = 0;
+
+    /**
+     * Private constructor prevents other classes from instantiating
+     */
+    private RobotTime(){}
+
+    private static RobotTime instance = null;
+
+    /**
+     * Gets the instance of Robot Time. If there is no instance running one will be created.
+     * @return instance of Robot Time
+     */
+    public static RobotTime getInstance() {
+        if (instance == null) {
+            instance = new RobotTime();
+        }
+        return instance;
+    }
+
+    /**
+     * Call this once per periodic loop.
+     */
+    public void updateTimes() {
+        m_lastRobotTime = m_robotTime;
+        m_lastMatchTime = m_matchTime;
+
+        m_currTime = System.currentTimeMillis();
+        m_robotTime = m_currTime - m_startRobotTime;
+        m_deltaTime = m_robotTime - m_lastRobotTime;
+        m_frameNumber++;
+
+        if (m_startMatchTime != 0) {
+            m_matchTime = m_currTime - m_startMatchTime;
+        }
+    }
+
+    /**
+     * Call this in both the auto and periodic inits
+     */
+    public void startMatchTime() {
+        if (m_startMatchTime == 0) {
+            m_startMatchTime = m_currTime;
+        }
+    }
+
+    /**
+     * Call this in disabled init
+     */
+    public void endMatchTime() {
+        m_startMatchTime = 0;
+        m_matchTime = 0;
+    }
+}
@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.utility;
+
+/** Aarav's good units class (better than WPILib)
+ * @author Aarav Shah */
+
+public class RobotUnits {
+  // constants
+
+  // angle conversions
+  public static double degreesToRadians(final double degrees) {return degrees * Math.PI / 180;}
+
+  public static double radiansToDegrees(final double radians) {return radians / Math.PI * 180;}
+
+  // falcon conversions
+  public static double falconTicksToRotations(final double ticks) {return ticks / 2048;}
+
+  public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;}
+
+  // distance conversions
+  public static double metersToFeet(final double meters) {return meters * 3.28084;}
+
+  public static double feetToMeters(final double feet) {return feet / 3.28084;}
+}
@@ -0,0 +1,12 @@
+package frc4388.utility;
+
+public class UtilityStructs {
+    public static class TimedOutput {
+        public double leftX  = 0.0;
+        public double leftY  = 0.0;
+        public double rightX = 0.0;
+        public double rightY = 0.0;
+    
+        public long timedOffset = 0;
+    }
+}
@@ -0,0 +1,21 @@
+package frc4388.utility.controller;
+
+/**
+ * Add your docs here.
+ */
+public interface IHandController {
+
+	public double getLeftXAxis();
+
+	public double getLeftYAxis();
+
+	public double getRightXAxis();
+	
+	public double getRightYAxis();
+
+	public double getLeftTriggerAxis();
+
+	public double getRightTriggerAxis();
+
+	public int getDpadAngle();
+}
@@ -0,0 +1,218 @@
+package frc4388.utility.controller;
+
+import edu.wpi.first.wpilibj.Joystick;
+
+/**
+ * This is a wrapper for the WPILib Joystick class that represents an XBox 
+ * controller.
+ * @author frc1675
+ */
+public class XboxController implements IHandController
+{
+	public static final int LEFT_X_AXIS = 0;
+	public static final int LEFT_Y_AXIS = 1;
+	public static final int LEFT_TRIGGER_AXIS = 2;
+	public static final int RIGHT_TRIGGER_AXIS = 3;
+	public static final int RIGHT_X_AXIS = 4;
+	public static final int RIGHT_Y_AXIS = 5;
+	public static final int LEFT_RIGHT_DPAD_AXIS = 6;
+	public static final int TOP_BOTTOM_DPAD_AXIS = 6;
+
+	public static final int A_BUTTON = 1;
+	public static final int B_BUTTON = 2;
+	public static final int X_BUTTON = 3;
+	public static final int Y_BUTTON = 4;
+	public static final int LEFT_BUMPER_BUTTON = 5;
+	public static final int RIGHT_BUMPER_BUTTON = 6;
+	public static final int BACK_BUTTON = 7;
+	public static final int START_BUTTON = 8;
+
+	public static final int LEFT_JOYSTICK_BUTTON = 9;
+	public static final int RIGHT_JOYSTICK_BUTTON = 10;
+
+	private static final double LEFT_DPAD_TOLERANCE = -0.9;
+	private static final double RIGHT_DPAD_TOLERANCE = 0.9;
+	private static final double BOTTOM_DPAD_TOLERANCE = -0.9;
+	private static final double TOP_DPAD_TOLERANCE = 0.9;
+
+	private static final double LEFT_TRIGGER_TOLERANCE = 0.5;
+	private static final double RIGHT_TRIGGER_TOLERANCE = 0.5;
+
+	private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9;
+	private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9;
+	private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9;
+	private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9;
+
+	private static final double LEFT_AXIS_UP_TOLERANCE = -0.9;
+	private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9;
+	private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9;
+	private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9;
+
+	private static final double DEADZONE = 0.1;
+
+	private Joystick m_stick;
+
+	/**
+     * Add your docs here.
+     */
+	public XboxController(int portNumber){
+		m_stick = new Joystick(portNumber);
+	}
+
+	/**
+     * Add your docs here.
+     */
+	public Joystick getJoyStick() {
+		return m_stick;
+	}
+	
+	/**
+	 * Checks if the input falls within the deadzone.
+	 * @param input from an axis on the controller
+	 * @return true if input falls in deadzone, false if input falls outside deadzone
+	 */
+	private boolean inDeadZone(double input){
+		return (Math.abs(input) < DEADZONE);
+	}
+
+	/**
+	 * Updates an input to have a deadzone around the 0 position
+	 * @param input from an axis on the controller
+	 * @return updated input
+	 */
+	private double getAxisWithDeadZoneCheck(double input){
+		if(inDeadZone(input)){
+			return 0.0;       
+		} else {
+			return input;
+		}
+	}
+
+	public boolean getAButton(){
+		return m_stick.getRawButton(A_BUTTON);
+	}
+
+	public boolean getXButton(){
+		return m_stick.getRawButton(X_BUTTON);    
+	}
+
+	public boolean getBButton(){
+		return m_stick.getRawButton(B_BUTTON);
+	}
+
+	public boolean getYButton(){
+		return m_stick.getRawButton(Y_BUTTON);
+	} 
+
+	public boolean getBackButton(){
+		return m_stick.getRawButton(BACK_BUTTON);
+	}
+
+	public boolean getStartButton(){
+		return m_stick.getRawButton(START_BUTTON);
+	} 
+
+	public boolean getLeftBumperButton(){
+		return m_stick.getRawButton(LEFT_BUMPER_BUTTON);
+	}
+
+	public boolean getRightBumperButton(){
+		return m_stick.getRawButton(RIGHT_BUMPER_BUTTON);
+	}
+
+	public boolean getLeftJoystickButton(){
+		return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON);
+	}
+
+	public boolean getRightJoystickButton(){
+		return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
+	}
+
+	public double getLeftXAxis(){         
+		return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS)); 
+	}
+
+	public double getLeftYAxis(){
+		return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS));
+	}
+
+	public double getRightXAxis(){
+		return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS));
+	}
+
+	public double getRightYAxis(){
+		return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS)); 
+	}
+
+	public double getLeftTriggerAxis(){
+		return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS)); 
+	}
+
+	public double getRightTriggerAxis(){
+		return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS)); 
+	}
+
+	/**
+	 * Get the angle input from the dpad.
+	 * @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc.
+	 */
+	public int getDpadAngle() {
+		return m_stick.getPOV(0);
+	}
+
+	public boolean getDPadLeft(){
+      return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE);
+  	}
+
+  	public boolean getDPadRight(){
+      return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE);
+  	}
+
+  	public boolean getDPadTop(){
+      return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE);
+  	}
+
+  	public boolean getDPadBottom(){
+      return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE);
+  	}
+
+	public boolean getLeftTrigger(){
+		return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
+	}
+
+	public boolean getRightTrigger(){
+		return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
+	}   
+
+	public boolean getRightAxisUpTrigger(){
+		return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
+	}   
+
+	public boolean getRightAxisDownTrigger(){
+		return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
+	}   
+
+	public boolean getRightAxisLeftTrigger(){
+		return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
+	}   
+
+	public boolean getRightAxisRightTrigger(){
+		return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
+	}   
+
+	public boolean getLeftAxisUpTrigger(){
+		return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE);
+	}   
+
+	public boolean getLeftAxisDownTrigger(){
+		return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE);
+	}   
+
+	public boolean getLeftAxisLeftTrigger(){
+		return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE);
+	}   
+
+	public boolean getLeftAxisRightTrigger(){
+		return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE);
+	}
+}
@@ -0,0 +1,68 @@
+package frc4388.utility.controller;
+
+import edu.wpi.first.wpilibj2.command.button.Button;
+
+/**
+ * Mapping for the Xbox controller triggers to allow triggers to be defined as
+ * buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger 
+ * exceeds a tolerance defined in {@link XboxController}.
+ */
+public class XboxTriggerButton extends Button {
+	public static final int RIGHT_TRIGGER = 0;
+	public static final int LEFT_TRIGGER = 1;
+	public static final int RIGHT_AXIS_UP_TRIGGER = 2;
+	public static final int RIGHT_AXIS_DOWN_TRIGGER = 3;
+	public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4;
+	public static final int RIGHT_AXIS_LEFT_TRIGGER = 5;
+	public static final int LEFT_AXIS_UP_TRIGGER = 6;
+	public static final int LEFT_AXIS_DOWN_TRIGGER = 7;
+	public static final int LEFT_AXIS_RIGHT_TRIGGER = 8;
+	public static final int LEFT_AXIS_LEFT_TRIGGER = 9;
+
+	private XboxController m_controller;
+	private int m_trigger;
+
+	/**
+	 * Creates a Trigger-Button mapped to a specific Xbox controller and trigger
+	 */
+	public XboxTriggerButton(XboxController controller, int trigger) {
+		m_controller = controller;
+		m_trigger = trigger;
+	}
+
+	/** {@inheritDoc} */
+	// @Override
+	public boolean get() {
+		if (m_trigger == RIGHT_TRIGGER) {
+			return m_controller.getRightTrigger();
+		}
+		else if (m_trigger == LEFT_TRIGGER) {
+			return m_controller.getLeftTrigger();
+		}
+		else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) {
+			return m_controller.getRightAxisUpTrigger();
+		}
+		else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) {
+			return m_controller.getRightAxisDownTrigger();
+		}
+		else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) {
+			return m_controller.getRightAxisRightTrigger();
+		}
+		else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) {
+			return m_controller.getRightAxisLeftTrigger();
+		}
+		else if (m_trigger == LEFT_AXIS_UP_TRIGGER) {
+			return m_controller.getLeftAxisUpTrigger();
+		}
+		else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) {
+			return m_controller.getLeftAxisDownTrigger();
+		}
+		else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) {
+			return m_controller.getLeftAxisRightTrigger();
+		}
+		else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) {
+			return m_controller.getLeftAxisLeftTrigger();
+		}
+		return false;
+	}
+}
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* 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.mocks;
+
+import com.ctre.phoenix.ErrorCode;
+import com.ctre.phoenix.sensors.PigeonIMU;
+
+/**
+ * Add your docs here.
+ */
+public class MockPigeonIMU extends PigeonIMU {
+    public int m_deviceNumber;
+    public double currentYaw;
+    public double currentPitch;
+    public double currentRoll;
+
+    public MockPigeonIMU(int deviceNumber) {
+        super(deviceNumber);
+        m_deviceNumber = deviceNumber;
+    }
+
+    @Override
+    public ErrorCode setYaw(double angleDeg) {
+        currentYaw = angleDeg;
+        return ErrorCode.OK;
+    }
+
+    /**
+     * @param currentPitch the Pitch to set
+     */
+    public void setCurrentPitch(double currentPitch) {
+        this.currentPitch = currentPitch;
+    }
+
+    /**
+     * @param currentRoll the Roll to set
+     */
+    public void setCurrentRoll(double currentRoll) {
+        this.currentRoll = currentRoll;
+    }
+
+    @Override
+    public ErrorCode getYawPitchRoll(double[] ypr_deg) {
+        ypr_deg[0] = currentYaw;
+        ypr_deg[1] = currentPitch;
+        ypr_deg[2] = currentRoll;
+        return ErrorCode.OK;
+    }
+}
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* 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.robot.subsystems;
+
+import static org.junit.Assert.assertEquals;
+import static org.mockito.Mockito.mock;
+
+import org.junit.Test;
+
+import edu.wpi.first.wpilibj.*;
+import frc4388.robot.Constants.LEDConstants;
+import frc4388.utility.LEDPatterns;
+
+/**
+ * Add your docs here.
+ */
+public class LEDSubsystemTest {
+    @Test
+    public void testConstructor() {
+        // Arrange
+        Spark ledController = mock(Spark.class);
+
+        // Act
+        LED led = new LED(ledController);
+
+        // Assert
+        assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001);
+    }
+
+    @Test
+    public void testPatterns() {
+        // Arrange
+        Spark ledController = mock(Spark.class);
+        LED led = new LED(ledController);
+
+        // Act
+        led.setPattern(LEDPatterns.RAINBOW_RAINBOW);
+
+        // Assert
+        assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001);
+
+        // Act
+        led.setPattern(LEDPatterns.BLUE_BREATH);
+
+        // Assert
+        assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001);
+
+        // Act
+        led.setPattern(LEDPatterns.SOLID_BLACK);
+
+        // Assert
+        assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001);
+    }
+}
@@ -0,0 +1,184 @@
+/*----------------------------------------------------------------------------*/
+/* 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 static org.junit.Assert.*;
+import static org.mockito.Mockito.*;
+
+import com.kauailabs.navx.frc.AHRS;
+
+import org.junit.*;
+
+import frc4388.mocks.MockPigeonIMU;
+import frc4388.robot.Constants.DriveConstants;
+
+/**
+ * Add your docs here.
+ */
+public class RobotGyroUtilityTest {
+    // TODO UNTESTED: most functions for NavX
+
+    private RobotGyro gyroPigeon;
+    private RobotGyro gyroNavX;
+
+    @Test
+    public void testConstructor() {
+        // Arrange
+        MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
+        AHRS navX = mock(AHRS.class);
+        gyroPigeon = new RobotGyro(pigeon);
+        gyroNavX = new RobotGyro(navX);
+
+        // Assert
+        assertEquals(true, gyroPigeon.m_isGyroAPigeon);
+        assertEquals(pigeon, gyroPigeon.getPigeon());
+        assertEquals(null, gyroPigeon.getNavX());
+        assertEquals(false, gyroNavX.m_isGyroAPigeon);
+        assertEquals(navX, gyroNavX.getNavX());
+        assertEquals(null, gyroNavX.getPigeon());
+    }
+
+    @Test
+    public void testHeadingPigeon() {
+        // Arrange
+        MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
+        gyroPigeon = new RobotGyro(pigeon);
+
+        // Act & Assert
+        assertEquals(-90, gyroPigeon.getHeading(270), 0.0001);
+        assertEquals(-45, gyroPigeon.getHeading(315), 0.0001);
+        assertEquals(-60, gyroPigeon.getHeading(-60), 0.0001);
+        assertEquals(30, gyroPigeon.getHeading(30), 0.0001);
+        assertEquals(0, gyroPigeon.getHeading(0), 0.0001);
+        assertEquals(180, gyroPigeon.getHeading(180), 0.0001);
+        assertEquals(-180, gyroPigeon.getHeading(-180), 0.0001);
+        assertEquals(97, gyroPigeon.getHeading(1537), 0.0001);
+        assertEquals(99, gyroPigeon.getHeading(-2781), 0.0001);
+    }
+
+    @Test
+    public void testYawPitchRollPigeon() {
+        // Arrange
+        MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
+        gyroPigeon = new RobotGyro(pigeon);
+
+        // Assert
+        assertEquals(0, gyroPigeon.getAngle(), 0.0001);
+
+        // Act
+        pigeon.setYaw(40);
+
+        // Assert
+        assertEquals(40, gyroPigeon.getAngle(), 0.0001);
+
+        // Act
+        gyroPigeon.reset();
+
+        // Assert
+        assertEquals(0, gyroPigeon.getAngle(), 0.0001);
+
+        // Act
+        pigeon.setYaw(-1457);
+        pigeon.setCurrentPitch(100);
+        pigeon.setCurrentRoll(100);
+
+        // Assert
+        assertEquals(-1457, gyroPigeon.getAngle(), 0.0001);
+        assertEquals(90, gyroPigeon.getPitch(), 0.0001);
+        assertEquals(90, gyroPigeon.getRoll(), 0.0001);
+
+        // Act
+        pigeon.setCurrentPitch(45);
+        pigeon.setCurrentRoll(45);
+
+        // Assert
+        assertEquals(45, gyroPigeon.getPitch(), 0.0001);
+        assertEquals(45, gyroPigeon.getRoll(), 0.0001);
+
+        // Act
+        pigeon.setCurrentPitch(0);
+        pigeon.setCurrentRoll(0);
+
+        // Assert
+        assertEquals(0, gyroPigeon.getPitch(), 0.0001);
+        assertEquals(0, gyroPigeon.getRoll(), 0.0001);
+
+        // Act
+        pigeon.setCurrentPitch(-60);
+        pigeon.setCurrentRoll(-60);
+
+        // Assert
+        assertEquals(-60, gyroPigeon.getPitch(), 0.0001);
+        assertEquals(-60, gyroPigeon.getRoll(), 0.0001);
+
+        // Act
+        pigeon.setCurrentPitch(-90);
+        pigeon.setCurrentRoll(-90);
+
+        // Assert
+        assertEquals(-90, gyroPigeon.getPitch(), 0.0001);
+        assertEquals(-90, gyroPigeon.getRoll(), 0.0001);
+
+        // Act
+        pigeon.setCurrentPitch(-100);
+        pigeon.setCurrentRoll(-100);
+
+        // Assert
+        assertEquals(-90, gyroPigeon.getPitch(), 0.0001);
+        assertEquals(-90, gyroPigeon.getRoll(), 0.0001);
+    }
+
+    @Test
+    public void testRatesPigeon() {
+        // Arrange
+        MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
+        gyroPigeon = new RobotGyro(pigeon);
+        RobotTime robotTime = RobotTime.getInstance();
+        gyroPigeon.updatePigeonDeltas();
+
+        // Act
+        robotTime.m_deltaTime = 5;
+        pigeon.setYaw(0);
+        gyroPigeon.updatePigeonDeltas();
+
+        // Assert
+        assertEquals(0, gyroPigeon.getRate(), 1);
+
+        // Act
+        robotTime.m_deltaTime = 5;
+        pigeon.setYaw(90);
+        gyroPigeon.updatePigeonDeltas();
+
+        // Assert
+        assertEquals(18000, gyroPigeon.getRate(), 0.001);
+
+        // Act
+        robotTime.m_deltaTime = 5;
+        pigeon.setYaw(90);
+        gyroPigeon.updatePigeonDeltas();
+
+        // Assert
+        assertEquals(0, gyroPigeon.getRate(), 0.001);
+
+        // Act
+        robotTime.m_deltaTime = 3;
+        pigeon.setYaw(-30);
+        gyroPigeon.updatePigeonDeltas();
+        
+        // Assert
+        assertEquals(-40000, gyroPigeon.getRate(), 0.001);
+
+        // Act
+        robotTime.m_deltaTime = 6;
+        pigeon.setYaw(690);
+        gyroPigeon.updatePigeonDeltas();
+
+        // Assert
+        assertEquals(120000, gyroPigeon.getRate(), 0.001);
+    }
+}
@@ -0,0 +1,104 @@
+/*----------------------------------------------------------------------------*/
+/* 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 static org.junit.Assert.*;
+
+import org.junit.*;
+
+/**
+ * Add your docs here.
+ */
+public class RobotTimeUtilityTest {
+    RobotTime robotTime = RobotTime.getInstance();
+
+    @Test
+    public void testUpdateTimes() {
+        // Arrange
+        long lastTime;
+        robotTime.m_deltaTime = 0;
+        robotTime.m_robotTime = 0;
+        robotTime.m_lastRobotTime = 0;
+        robotTime.m_frameNumber = 0;
+        robotTime.endMatchTime();
+        robotTime.m_lastMatchTime = 0;
+
+        // Assert
+        assertEquals(0, robotTime.m_deltaTime);
+        assertEquals(0, robotTime.m_robotTime);
+        assertEquals(0, robotTime.m_lastRobotTime);
+        assertEquals(0, robotTime.m_frameNumber);
+        lastTime = robotTime.m_robotTime;
+
+        // Act
+        wait(1);
+        robotTime.updateTimes();
+
+        // Assert
+        assertEquals(true, robotTime.m_deltaTime > 0);
+        assertEquals(true, robotTime.m_robotTime > 0);
+        assertEquals(lastTime, robotTime.m_lastRobotTime);
+        assertEquals(1, robotTime.m_frameNumber);
+        lastTime = robotTime.m_robotTime;
+
+        // Act
+        wait(1);
+        robotTime.updateTimes();
+
+        // Assert
+        assertEquals(true, robotTime.m_deltaTime > 0);
+        assertEquals(true, robotTime.m_robotTime > 0);
+        assertEquals(lastTime, robotTime.m_lastRobotTime);
+        assertEquals(2, robotTime.m_frameNumber);
+    }
+
+    @Test
+    public void testMatchTime() {
+        // Arrange
+        long lastTime;
+
+        // Assert
+        assertEquals(0, robotTime.m_matchTime);
+        assertEquals(0, robotTime.m_lastMatchTime);
+        lastTime = robotTime.m_matchTime;
+
+        // Act
+        robotTime.startMatchTime();
+        wait(1);
+        robotTime.updateTimes();
+
+        // Assert
+        assertEquals(true, robotTime.m_matchTime > 0);
+        assertEquals(lastTime, robotTime.m_lastMatchTime);
+        lastTime = robotTime.m_matchTime;
+        
+        // Act
+        wait(1);
+        robotTime.updateTimes();
+        robotTime.endMatchTime();
+
+        // Assert
+        assertEquals(0, robotTime.m_matchTime);
+        assertEquals(lastTime, robotTime.m_lastMatchTime);
+        lastTime = robotTime.m_matchTime;
+
+        // Act
+        wait(1);
+        robotTime.updateTimes();
+
+        // Assert
+        assertEquals(0, robotTime.m_matchTime);
+        assertEquals(lastTime, robotTime.m_lastMatchTime);
+    }
+
+    private void wait(int millis) {
+        try {
+            Thread.sleep(millis);
+        } catch (Exception e) {}
+    }
+}
@@ -0,0 +1,31 @@
+/**
+ * This file is a configuration file generated by the `Template` extension on `vscode`
+ * @see https://marketplace.visualstudio.com/items?itemName=yongwoo.template
+ */
+module.exports = {
+  // You can change the template path to another path
+  templateRootPath: "./.templates",
+  // After copying the template file the `replaceFileTextFn` function is executed
+  replaceFileTextFn: (fileText, templateName, utils) => {
+    // @see https://www.npmjs.com/package/change-case
+    const { changeCase } = utils;
+    // You can change the text in the file
+    return fileText
+      .replace(/__templateName__/gm, templateName)
+      .replace(
+        /__templateNameToPascalCase__/gm,
+        changeCase.pascalCase(templateName)
+      )
+      .replace(
+        /__templateNameToParamCase__/gm,
+        changeCase.paramCase(templateName)
+      );
+  },
+  replaceFileNameFn: (fileName, templateName, utils) => {
+    const { path } = utils;
+    // @see https://nodejs.org/api/path.html#path_path_parse_path
+    const { base } = path.parse(fileName);
+    // You can change the file name
+    return base;
+  }
+};
@@ -0,0 +1,39 @@
+{
+    "fileName": "NavX.json",
+    "name": "KauaiLabs_navX_FRC",
+    "version": "2023.0.4",
+    "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+    "mavenUrls": [
+        "https://dev.studica.com/maven/release/2023/"
+    ],
+    "jsonUrl": "https://dev.studica.com/releases/2023/NavX.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.kauailabs.navx.frc",
+            "artifactId": "navx-frc-java",
+            "version": "2023.0.4"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.kauailabs.navx.frc",
+            "artifactId": "navx-frc-cpp",
+            "version": "2023.0.4",
+            "headerClassifier": "headers",
+            "sourcesClassifier": "sources",
+            "sharedLibrary": false,
+            "libName": "navx_frc",
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena",
+                "linuxraspbian",
+                "linuxarm32",
+                "linuxarm64",
+                "linuxx86-64",
+                "osxuniversal",
+                "windowsx86-64"
+            ]
+        }
+    ]
+}
@@ -0,0 +1,423 @@
+{
+    "fileName": "Phoenix.json",
+    "name": "CTRE-Phoenix (v5)",
+    "version": "5.31.0+23.2.2",
+    "frcYear": 2023,
+    "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
+    "mavenUrls": [
+        "https://maven.ctr-electronics.com/release/"
+    ],
+    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "api-java",
+            "version": "5.31.0"
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "wpiapi-java",
+            "version": "5.31.0"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "cci",
+            "version": "5.31.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "cci-sim",
+            "version": "5.31.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "tools",
+            "version": "23.2.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "tools-sim",
+            "version": "23.2.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simTalonSRX",
+            "version": "23.2.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simTalonFX",
+            "version": "23.2.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simVictorSPX",
+            "version": "23.2.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simPigeonIMU",
+            "version": "23.2.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simCANCoder",
+            "version": "23.2.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFX",
+            "version": "23.2.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANcoder",
+            "version": "23.2.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProPigeon2",
+            "version": "23.2.2",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "wpiapi-cpp",
+            "version": "5.31.0",
+            "libName": "CTRE_Phoenix_WPI",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "api-cpp",
+            "version": "5.31.0",
+            "libName": "CTRE_Phoenix",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "cci",
+            "version": "5.31.0",
+            "libName": "CTRE_PhoenixCCI",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "tools",
+            "version": "23.2.2",
+            "libName": "CTRE_PhoenixTools",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "wpiapi-cpp-sim",
+            "version": "5.31.0",
+            "libName": "CTRE_Phoenix_WPISim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "api-cpp-sim",
+            "version": "5.31.0",
+            "libName": "CTRE_PhoenixSim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "cci-sim",
+            "version": "5.31.0",
+            "libName": "CTRE_PhoenixCCISim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "tools-sim",
+            "version": "23.2.2",
+            "libName": "CTRE_PhoenixTools_Sim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simTalonSRX",
+            "version": "23.2.2",
+            "libName": "CTRE_SimTalonSRX",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simTalonFX",
+            "version": "23.2.2",
+            "libName": "CTRE_SimTalonFX",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simVictorSPX",
+            "version": "23.2.2",
+            "libName": "CTRE_SimVictorSPX",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simPigeonIMU",
+            "version": "23.2.2",
+            "libName": "CTRE_SimPigeonIMU",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simCANCoder",
+            "version": "23.2.2",
+            "libName": "CTRE_SimCANCoder",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFX",
+            "version": "23.2.2",
+            "libName": "CTRE_SimProTalonFX",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANcoder",
+            "version": "23.2.2",
+            "libName": "CTRE_SimProCANcoder",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProPigeon2",
+            "version": "23.2.2",
+            "libName": "CTRE_SimProPigeon2",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ]
+}
@@ -0,0 +1,37 @@
+{
+  "fileName": "WPILibNewCommands.json",
+  "name": "WPILib-New-Commands",
+  "version": "1.0.0",
+  "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
+  "mavenUrls": [],
+  "jsonUrl": "",
+  "javaDependencies": [
+    {
+      "groupId": "edu.wpi.first.wpilibNewCommands",
+      "artifactId": "wpilibNewCommands-java",
+      "version": "wpilib"
+    }
+  ],
+  "jniDependencies": [],
+  "cppDependencies": [
+    {
+      "groupId": "edu.wpi.first.wpilibNewCommands",
+      "artifactId": "wpilibNewCommands-cpp",
+      "version": "wpilib",
+      "libName": "wpilibNewCommands",
+      "headerClassifier": "headers",
+      "sourcesClassifier": "sources",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "linuxathena",
+        "linuxarm32",
+        "linuxarm64",
+        "windowsx86-64",
+        "windowsx86",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    }
+  ]
+}
@@ -0,0 +1,35 @@
+{
+    "fileName": "navx_frc.json",
+    "name": "KauaiLabs_navX_FRC",
+    "version": "3.1.413",
+    "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+    "mavenUrls": [
+        "https://repo1.maven.org/maven2/"
+    ],
+    "jsonUrl": "https://www.kauailabs.com/dist/frc/2020/navx_frc.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.kauailabs.navx.frc",
+            "artifactId": "navx-java",
+            "version": "3.1.413"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.kauailabs.navx.frc",
+            "artifactId": "navx-cpp",
+            "version": "3.1.413",
+            "headerClassifier": "headers",
+            "sourcesClassifier": "sources",
+            "sharedLibrary": false,
+            "libName": "navx_frc",
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena",
+                "linuxraspbian",
+                "windowsx86-64"
+            ]
+        }
+    ]
+}
@@ -0,0 +1,42 @@
+{
+    "fileName": "photonlib.json",
+    "name": "photonlib",
+    "version": "v2024.1.1-beta-3.2",
+    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+    "frcYear": "2024",
+    "mavenUrls": [
+        "https://maven.photonvision.org/repository/internal",
+        "https://maven.photonvision.org/repository/snapshots"
+    ],
+    "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json",
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonLib-cpp",
+            "version": "v2024.1.1-beta-3.2",
+            "libName": "Photon",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonLib-java",
+            "version": "v2024.1.1-beta-3.2"
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonTargeting-java",
+            "version": "v2024.1.1-beta-3.2"
+        }
+    ]
+}