Initial commit

.gitattributes

@@ -0,0 +1,2 @@
+# Auto detect text files and perform LF normalization
+* text=auto

.github/workflows/gradle.yml

@@ -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

.gitignore

@@ -0,0 +1,161 @@
+# Created by https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode
+
+### 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
+.classpath
+.project
+.settings/
+bin/
+imgui.ini
+
+
+# End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode

.templates/CommandTest.java

@@ -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);
+    }
+}

.templates/SubsystemTest.java

@@ -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);
+    }
+}

.templates/UtilityTest.java

@@ -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);
+    }
+}

.vscode/launch.json

@@ -0,0 +1,21 @@
+{
+  // Use IntelliSense to learn about possible attributes.
+  // Hover to view descriptions of existing attributes.
+  // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+  "version": "0.2.0",
+  "configurations": [
+
+    {
+      "type": "wpilib",
+      "name": "WPILib Desktop Debug",
+      "request": "launch",
+      "desktop": true,
+    },
+    {
+      "type": "wpilib",
+      "name": "WPILib roboRIO Debug",
+      "request": "launch",
+      "desktop": false,
+    }
+  ]
+}

.vscode/settings.json

@@ -0,0 +1,15 @@
+{
+  "java.configuration.updateBuildConfiguration": "automatic",
+  "files.exclude": {
+    "**/.git": true,
+    "**/.svn": true,
+    "**/.hg": true,
+    "**/CVS": true,
+    "**/.DS_Store": true,
+    "bin/": true,
+    "**/.classpath": true,
+    "**/.project": true,
+    "**/.settings": true,
+    "**/.factorypath": true
+  }
+}

.wpilib/wpilib_preferences.json

@@ -0,0 +1,6 @@
+{
+    "enableCppIntellisense": false,
+    "currentLanguage": "java",
+    "projectYear": "2020",
+    "teamNumber": 4388
+}

LICENSE

@@ -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.

README.md

@@ -0,0 +1,2 @@
+# Robot-Essentials
+ Basic code for any Ridgebotics robot project

build.gradle

@@ -0,0 +1,69 @@
+plugins {
+    id "java"
+    id "edu.wpi.first.GradleRIO" version "2020.3.2"
+}
+
+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 EmbeddedTools.
+deploy {
+    targets {
+        roboRIO("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 = frc.getTeamNumber()
+        }
+    }
+    artifacts {
+        frcJavaArtifact('frcJava') {
+            targets << "roborio"
+            // Debug can be overridden by command line, for use with VSCode
+            debug = frc.getDebugOrDefault(false)
+        }
+        // Built in artifact to deploy arbitrary files to the roboRIO.
+        fileTreeArtifact('frcStaticFileDeploy') {
+            // The directory below is the local directory to deploy
+            files = fileTree(dir: 'src/main/deploy')
+            // Deploy to RoboRIO target, into /home/lvuser/deploy
+            targets << "roborio"
+            directory = '/home/lvuser/deploy'
+        }
+    }
+}
+
+// Set this to true to enable desktop support.
+def includeDesktopSupport = true
+
+// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+// Also defines JUnit 4.
+dependencies {
+    implementation wpi.deps.wpilib()
+    nativeZip wpi.deps.wpilibJni(wpi.platforms.roborio)
+    nativeDesktopZip wpi.deps.wpilibJni(wpi.platforms.desktop)
+
+
+    implementation wpi.deps.vendor.java()
+    nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio)
+    nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop)
+
+    testImplementation 'junit:junit:4.12'
+    testCompile "org.mockito:mockito-core:2.+"
+
+    // Enable simulation gui support. Must check the box in vscode to enable support
+    // upon debugging
+    simulation wpi.deps.sim.gui(wpi.platforms.desktop, false)
+}
+
+// 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)
+}

gradle/wrapper/gradle-wrapper.properties

@@ -0,0 +1,5 @@
+distributionBase=GRADLE_USER_HOME
+distributionPath=permwrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-6.0.1-bin.zip
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=permwrapper/dists

gradlew

@@ -0,0 +1,183 @@
+#!/usr/bin/env sh
+
+#
+# Copyright 2015 the original author or 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 UN*X
+##
+##############################################################################
+
+# Attempt to set APP_HOME
+# Resolve links: $0 may be a link
+PRG="$0"
+# Need this for relative symlinks.
+while [ -h "$PRG" ] ; do
+    ls=`ls -ld "$PRG"`
+    link=`expr "$ls" : '.*-> \(.*\)$'`
+    if expr "$link" : '/.*' > /dev/null; then
+        PRG="$link"
+    else
+        PRG=`dirname "$PRG"`"/$link"
+    fi
+done
+SAVED="`pwd`"
+cd "`dirname \"$PRG\"`/" >/dev/null
+APP_HOME="`pwd -P`"
+cd "$SAVED" >/dev/null
+
+APP_NAME="Gradle"
+APP_BASE_NAME=`basename "$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 "$*"
+}
+
+die () {
+    echo
+    echo "$*"
+    echo
+    exit 1
+}
+
+# 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
+    ;;
+  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" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then
+    MAX_FD_LIMIT=`ulimit -H -n`
+    if [ $? -eq 0 ] ; then
+        if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then
+            MAX_FD="$MAX_FD_LIMIT"
+        fi
+        ulimit -n $MAX_FD
+        if [ $? -ne 0 ] ; then
+            warn "Could not set maximum file descriptor limit: $MAX_FD"
+        fi
+    else
+        warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT"
+    fi
+fi
+
+# For Darwin, add options to specify how the application appears in the dock
+if $darwin; then
+    GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\""
+fi
+
+# For Cygwin or MSYS, switch paths to Windows format before running java
+if [ "$cygwin" = "true" -o "$msys" = "true" ] ; then
+    APP_HOME=`cygpath --path --mixed "$APP_HOME"`
+    CLASSPATH=`cygpath --path --mixed "$CLASSPATH"`
+    JAVACMD=`cygpath --unix "$JAVACMD"`
+
+    # We build the pattern for arguments to be converted via cygpath
+    ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null`
+    SEP=""
+    for dir in $ROOTDIRSRAW ; do
+        ROOTDIRS="$ROOTDIRS$SEP$dir"
+        SEP="|"
+    done
+    OURCYGPATTERN="(^($ROOTDIRS))"
+    # Add a user-defined pattern to the cygpath arguments
+    if [ "$GRADLE_CYGPATTERN" != "" ] ; then
+        OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)"
+    fi
+    # Now convert the arguments - kludge to limit ourselves to /bin/sh
+    i=0
+    for arg in "$@" ; do
+        CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -`
+        CHECK2=`echo "$arg"|egrep -c "^-"`                                 ### Determine if an option
+
+        if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then                    ### Added a condition
+            eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"`
+        else
+            eval `echo args$i`="\"$arg\""
+        fi
+        i=`expr $i + 1`
+    done
+    case $i in
+        0) set -- ;;
+        1) set -- "$args0" ;;
+        2) set -- "$args0" "$args1" ;;
+        3) set -- "$args0" "$args1" "$args2" ;;
+        4) set -- "$args0" "$args1" "$args2" "$args3" ;;
+        5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;;
+        6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;;
+        7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;;
+        8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;;
+        9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;;
+    esac
+fi
+
+# Escape application args
+save () {
+    for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done
+    echo " "
+}
+APP_ARGS=`save "$@"`
+
+# Collect all arguments for the java command, following the shell quoting and substitution rules
+eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS"
+
+exec "$JAVACMD" "$@"

gradlew.bat

@@ -0,0 +1,100 @@
+@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 Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if "%ERRORLEVEL%" == "0" goto init
+
+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 init
+
+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
+
+:init
+@rem Get command-line arguments, handling Windows variants
+
+if not "%OS%" == "Windows_NT" goto win9xME_args
+
+:win9xME_args
+@rem Slurp the command line arguments.
+set CMD_LINE_ARGS=
+set _SKIP=2
+
+:win9xME_args_slurp
+if "x%~1" == "x" goto execute
+
+set CMD_LINE_ARGS=%*
+
+: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 %CMD_LINE_ARGS%
+
+:end
+@rem End local scope for the variables with windows NT shell
+if "%ERRORLEVEL%"=="0" goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+if  not "" == "%GRADLE_EXIT_CONSOLE%" exit 1
+exit /b 1
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega

settings.gradle

@@ -0,0 +1,27 @@
+import org.gradle.internal.os.OperatingSystem
+
+pluginManagement {
+    repositories {
+        mavenLocal()
+        gradlePluginPortal()
+        String frcYear = '2020'
+        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
+        }
+    }
+}

src/main/deploy/example.txt

@@ -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.

src/main/java/frc4388/robot/Constants.java

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* 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 frc4388.utility.LEDPatterns;
+
+/**
+ * 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 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;
+    }
+}

src/main/java/frc4388/robot/Main.java

@@ -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);
+  }
+}

src/main/java/frc4388/robot/Robot.java

@@ -0,0 +1,125 @@
+/*----------------------------------------------------------------------------*/
+/* 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.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() {
+  }
+
+  /**
+   * 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() {
+  }
+}

src/main/java/frc4388/robot/RobotContainer.java

@@ -0,0 +1,105 @@
+/*----------------------------------------------------------------------------*/
+/* 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.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
+
+        /* 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();
+    }
+}

src/main/java/frc4388/robot/RobotMap.java

@@ -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.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() {
+    }
+}

src/main/java/frc4388/robot/subsystems/DiffDrive.java

@@ -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);
+  }
+}

src/main/java/frc4388/robot/subsystems/LED.java

@@ -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.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;
+  }
+}

src/main/java/frc4388/utility/LEDPatterns.java

@@ -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;
+  }
+}

src/main/java/frc4388/utility/RobotGyro.java

@@ -0,0 +1,180 @@
+/*----------------------------------------------------------------------------*/
+/* 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.PigeonIMU;
+import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
+import com.kauailabs.navx.frc.AHRS;
+
+import edu.wpi.first.wpilibj.GyroBase;
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * Gyro class that allows for interchangeable use between a pigeon and a navX
+ */
+public class RobotGyro extends GyroBase {
+    private RobotTime m_robotTime = RobotTime.getInstance();
+
+    private PigeonIMU m_pigeon = null;
+    private AHRS m_navX = null;
+    public boolean m_isGyroAPigeon; //true if pigeon, false if navX
+
+    private double m_lastPigeonAngle;
+    private double m_deltaPigeonAngle;
+
+    /**
+     * Creates a Gyro based on a pigeon
+     * @param gyro the gyroscope to use for Gyro
+     */
+    public RobotGyro(PigeonIMU gyro) {
+        m_pigeon = gyro;
+        m_isGyroAPigeon = true;
+    }
+
+    /**
+     * Creates a Gyro based on a navX
+     * @param gyro the gyroscope to use for Gyro
+     */
+    public RobotGyro(AHRS gyro){
+        m_navX = gyro;
+        m_isGyroAPigeon = false;
+    }
+
+    /**
+     * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note
+     * that the getRate() method for a navX will likely be much more accurate than for a pigeon.
+     */
+    public void updatePigeonDeltas() {
+        double currentPigeonAngle = getAngle();
+        m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
+        m_lastPigeonAngle = currentPigeonAngle;
+    }
+
+    /**
+     * <p>NavX:
+     * <p>Calibrate the gyro by running for a number of samples and computing the center value. Then use
+     * the center value as the Accumulator center value for subsequent measurements. It's important to
+     * make sure that the robot is not moving while the centering calculations are in progress, this
+     * is typically done when the robot is first turned on while it's sitting at rest before the
+     * competition starts.
+     * 
+     * <p>Pigeon:
+     * <p>Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot
+     * into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon
+     * has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to
+     * make sure that the robot is not moving while the tempurature calculations are in progress, this
+     * is typically done when the robot is first turned on while it's sitting at rest before the
+     * competition starts.
+     */
+    @Override
+    public void calibrate() {
+        if (m_isGyroAPigeon) {
+            m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
+        } else {
+            m_navX.calibrate();
+        }
+    }
+
+    @Override
+    public void reset() {
+        if (m_isGyroAPigeon) {
+            m_pigeon.setYaw(0);
+        } else {
+            m_navX.reset();
+        }
+    }
+
+    /**
+	 * Get Yaw, Pitch, and Roll data.
+	 *
+	 * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
+	 *					Yaw is within [-368,640, +368,640] degrees.
+	 *					Pitch is within [-90,+90] degrees.
+	 *					Roll is within [-90,+90] degrees.
+     */
+    private double[] getPigeonAngles() {
+        double[] angles = new double[3];
+        m_pigeon.getYawPitchRoll(angles);
+        return angles;
+    }
+
+    @Override
+    public double getAngle() {
+        if (m_isGyroAPigeon) {
+            return getPigeonAngles()[0];
+        } else {
+            return m_navX.getAngle();
+        }
+    }
+
+    /**
+     * Gets an absolute heading of the robot
+     * @return heading from -180 to 180 degrees
+     */
+    public double getHeading() {
+        return getHeading(getAngle());
+    }
+
+    /**
+     * Gets an absolute heading of the robot
+     * @return heading from -180 to 180 degrees
+     */
+    public double getHeading(double angle) {
+        return Math.IEEEremainder(angle, 360);
+    }
+
+    /**
+     * Returns the current pitch value (in degrees, from -90 to 90)
+     * reported by the sensor.  Pitch is a measure of rotation around
+     * the Y Axis.
+     * @return The current pitch value in degrees (-90 to 90).
+     */
+    public double getPitch() {
+        if (m_isGyroAPigeon) {
+            return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
+        } else {
+            return MathUtil.clamp(m_navX.getPitch(), -90, 90);
+        }
+    }
+
+    /**
+     * Returns the current roll value (in degrees, from -90 to 90)
+     * reported by the sensor.  Roll is a measure of rotation around
+     * the X Axis.
+     * @return The current roll value in degrees (-90 to 90).
+     */
+    public double getRoll() {
+        if (m_isGyroAPigeon) {
+            return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
+        } else {
+            return MathUtil.clamp(m_navX.getRoll(), -90, 90);
+        }
+    }
+
+    @Override
+    public double getRate() {
+        if (m_isGyroAPigeon) {
+            return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
+        } else {
+            return m_navX.getRate();
+        }
+    }
+
+    public PigeonIMU getPigeon(){
+        return m_pigeon;
+    }
+
+    public AHRS getNavX(){
+        return m_navX;
+    }
+
+    @Override
+    public void close() throws Exception {
+
+    }
+}

src/main/java/frc4388/utility/RobotTime.java

@@ -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;
+    }
+}

src/main/java/frc4388/utility/RobotUnits.java

@@ -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;}
+}

src/main/java/frc4388/utility/controller/IHandController.java

@@ -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();
+}

src/main/java/frc4388/utility/controller/XboxController.java

@@ -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);
+	}
+}

src/main/java/frc4388/utility/controller/XboxTriggerButton.java

@@ -0,0 +1,69 @@
+package frc4388.utility.controller;
+
+import edu.wpi.first.wpilibj2.command.button.Button;
+import frc4388.utility.controller.XboxController;
+
+/**
+ * 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;
+	}
+}

src/test/java/frc4388/mocks/MockPigeonIMU.java

@@ -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;
+    }
+}

src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java

@@ -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);
+    }
+}

src/test/java/frc4388/utility/RobotGyroUtilityTest.java

@@ -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);
+    }
+}

src/test/java/frc4388/utility/RobotTimeUtilityTest.java

@@ -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) {}
+    }
+}

template.config.js

@@ -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;
+  }
+};

vendordeps/Phoenix.json

@@ -0,0 +1,180 @@
+{
+    "fileName": "Phoenix.json",
+    "name": "CTRE-Phoenix",
+    "version": "5.17.3",
+    "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
+    "mavenUrls": [
+        "http://devsite.ctr-electronics.com/maven/release/"
+    ],
+    "jsonUrl": "http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "api-java",
+            "version": "5.17.3"
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "wpiapi-java",
+            "version": "5.17.3"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "cci",
+            "version": "5.17.3",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "linuxathena",
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "diagnostics",
+            "version": "5.17.3",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "linuxathena",
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "canutils",
+            "version": "5.17.3",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "platform-stub",
+            "version": "5.17.3",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "core",
+            "version": "5.17.3",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "linuxathena",
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "wpiapi-cpp",
+            "version": "5.17.3",
+            "libName": "CTRE_Phoenix_WPI",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena",
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "api-cpp",
+            "version": "5.17.3",
+            "libName": "CTRE_Phoenix",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena",
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "cci",
+            "version": "5.17.3",
+            "libName": "CTRE_PhoenixCCI",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena",
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "diagnostics",
+            "version": "5.17.3",
+            "libName": "CTRE_PhoenixDiagnostics",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena",
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "canutils",
+            "version": "5.17.3",
+            "libName": "CTRE_PhoenixCanutils",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "platform-stub",
+            "version": "5.17.3",
+            "libName": "CTRE_PhoenixPlatform",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "core",
+            "version": "5.17.3",
+            "libName": "CTRE_PhoenixCore",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena",
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        }
+    ]
+}

vendordeps/WPILibNewCommands.json

@@ -0,0 +1,37 @@
+{
+    "fileName": "WPILibNewCommands.json",
+    "name": "WPILib-New-Commands",
+    "version": "2020.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",
+                "linuxraspbian",
+                "linuxaarch64bionic",
+                "windowsx86-64",
+                "windowsx86",
+                "linuxx86-64",
+                "osxx86-64"
+            ]
+        }
+    ]
+}

vendordeps/navx_frc.json

@@ -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"
+            ]
+        }
+    ]
+}