worky

2026-06-08 16:28:02 -06:00 · 2024-01-05 13:56:01 -07:00
parent 3b5080a334
commit 5b2e8f7e98
25 changed files with 1493 additions and 729 deletions
@@ -1,4 +1,5 @@
-# Created by https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode
+# This gitignore has been specially created by the WPILib team.
+# If you remove items from this file, intellisense might break.

 ### C++ ###
 # Prerequisites
@@ -151,11 +152,21 @@ gradle-app.setting
 # gradle/wrapper/gradle-wrapper.properties

 # # VS Code Specific Java Settings
+# DO NOT REMOVE .classpath and .project
 .classpath
 .project
 .settings/
 bin/
-imgui.ini

+# IntelliJ
+*.iml
+*.ipr
+*.iws
+.idea/
+out/

-# End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode
+# Fleet
+.fleet
+
+# Simulation GUI and other tools window save file
+*-window.json
@@ -1,5 +1,6 @@
 {
  "java.configuration.updateBuildConfiguration": "automatic",
+  "java.server.launchMode": "Standard",
  "files.exclude": {
    "**/.git": true,
    "**/.svn": true,
@@ -10,6 +11,19 @@
    "**/.classpath": true,
    "**/.project": true,
    "**/.settings": true,
-    "**/.factorypath": 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"
 }
@@ -1,6 +1,6 @@
 {
    "enableCppIntellisense": false,
    "currentLanguage": "java",
-    "projectYear": "2020",
+    "projectYear": "2023",
    "teamNumber": 4388
 }
@@ -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.
@@ -1,6 +1,6 @@
 plugins {
    id "java"
-    id "edu.wpi.first.GradleRIO" version "2020.3.2"
+    id "edu.wpi.first.GradleRIO" version "2023.4.3"
 }

 sourceCompatibility = JavaVersion.VERSION_11
@@ -9,61 +9,91 @@ 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.
+// This is added by GradleRIO's backing project DeployUtils.
 deploy {
    targets {
-        roboRIO("roborio") {
+        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 = 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'
+            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 = true
+def includeDesktopSupport = false

 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
-// Also defines JUnit 4.
+// Also defines JUnit 5.
 dependencies {
-    implementation wpi.deps.wpilib()
-    nativeZip wpi.deps.wpilibJni(wpi.platforms.roborio)
-    nativeDesktopZip wpi.deps.wpilibJni(wpi.platforms.desktop)
+    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)

-    implementation wpi.deps.vendor.java()
-    nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio)
-    nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop)
+    roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
+    roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)

-    testImplementation 'junit:junit:4.12'
-    testCompile "org.mockito:mockito-core:2.+"
+    nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
+    nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
+    simulationDebug wpi.sim.enableDebug()

-    // Enable simulation gui support. Must check the box in vscode to enable support
-    // upon debugging
-    simulation wpi.deps.sim.gui(wpi.platforms.desktop, false)
+    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'
 }
@@ -1,5 +1,5 @@
 distributionBase=GRADLE_USER_HOME
 distributionPath=permwrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-6.0.1-bin.zip
+distributionUrl=https\://services.gradle.org/distributions/gradle-7.5.1-bin.zip
 zipStoreBase=GRADLE_USER_HOME
 zipStorePath=permwrapper/dists
@@ -1,7 +1,7 @@
-#!/usr/bin/env sh
+#!/bin/sh

 #
-# Copyright 2015 the original author or authors.
+# 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.
@@ -17,78 +17,113 @@
 #

 ##############################################################################
-##
-##  Gradle start up script for UN*X
-##
+#
+#   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
-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
+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
-SAVED="`pwd`"
-cd "`dirname \"$PRG\"`/" >/dev/null
-APP_HOME="`pwd -P`"
-cd "$SAVED" >/dev/null
+
+APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit

 APP_NAME="Gradle"
-APP_BASE_NAME=`basename "$0"`
+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"
+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
-    ;;
-  MINGW* )
-    msys=true
-    ;;
-  NONSTOP* )
-    nonstop=true
-    ;;
+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"
+        JAVACMD=$JAVA_HOME/jre/sh/java
    else
-        JAVACMD="$JAVA_HOME/bin/java"
+        JAVACMD=$JAVA_HOME/bin/java
    fi
    if [ ! -x "$JAVACMD" ] ; then
        die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
@@ -97,7 +132,7 @@ Please set the JAVA_HOME variable in your environment to match the
 location of your Java installation."
    fi
 else
-    JAVACMD="java"
+    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
@@ -105,79 +140,101 @@ 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" ;;
+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

-# 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, 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.

-# 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"
+# 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" "$@"
@@ -14,7 +14,7 @@
@rem limitations under the License.
@rem

-@if "%DEBUG%" == "" @echo off
+@if "%DEBUG%"=="" @echo off
@rem ##########################################################################
@rem
@rem  Gradle startup script for Windows
@@ -25,10 +25,13 @@
 if "%OS%"=="Windows_NT" setlocal

 set DIRNAME=%~dp0
-if "%DIRNAME%" == "" set DIRNAME=.
+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"

@@ -37,7 +40,7 @@ if defined JAVA_HOME goto findJavaFromJavaHome

 set JAVA_EXE=java.exe
 %JAVA_EXE% -version >NUL 2>&1
-if "%ERRORLEVEL%" == "0" goto init
+if %ERRORLEVEL% equ 0 goto execute

 echo.
 echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
@@ -51,7 +54,7 @@ goto fail
 set JAVA_HOME=%JAVA_HOME:"=%
 set JAVA_EXE=%JAVA_HOME%/bin/java.exe

-if exist "%JAVA_EXE%" goto init
+if exist "%JAVA_EXE%" goto execute

 echo.
 echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
@@ -61,38 +64,26 @@ 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%
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*

 :end
@rem End local scope for the variables with windows NT shell
-if "%ERRORLEVEL%"=="0" goto mainEnd
+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!
-if  not "" == "%GRADLE_EXIT_CONSOLE%" exit 1
-exit /b 1
+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
@@ -4,7 +4,7 @@ pluginManagement {
    repositories {
        mavenLocal()
        gradlePluginPortal()
-        String frcYear = '2020'
+        String frcYear = '2023'
        File frcHome
        if (OperatingSystem.current().isWindows()) {
            String publicFolder = System.getenv('PUBLIC')
@@ -7,6 +7,8 @@

 package frc4388.robot;

+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+
 import frc4388.utility.LEDPatterns;
 import frc4388.utility.Gains;

@@ -19,52 +21,96 @@ import frc4388.utility.Gains;
 * constants are needed, to reduce verbosity.
 */
 public final class Constants {
-  public static final class SwerveDriveConstants {
-    public static final double ROTATION_SPEED = 0.1;
-    public static final double WHEEL_SPEED = 0.1;
-    public static final double WIDTH = 22;
-    public static final double HEIGHT = 22;
-    public static final double JOYSTICK_TO_METERS_PER_SECOND = 5;
-    public static final double MAX_SPEED_FEET_PER_SEC = 16;
-    public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20;
-    public static final int LEFT_FRONT_STEER_CAN_ID = 2;
-    public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
-    public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
-    public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;
-    public static final int LEFT_BACK_STEER_CAN_ID = 6;
-    public static final int LEFT_BACK_WHEEL_CAN_ID = 7;
-    public static final int RIGHT_BACK_STEER_CAN_ID = 8;
-    public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;
-    public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;
-    public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
-    public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
-    public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
-    // ofsets are in degrees
-    //ofsets are in degrees
-        // public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141;    
-        // public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281;   
-        // public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531;     
-        // public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594;
-        public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0;
-        public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0;
-        public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0;
-        public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0;
-
-    // swerve PID constants
-    public static final int SWERVE_SLOT_IDX = 0;
-    public static final int SWERVE_PID_LOOP_IDX = 1;
-    public static final int SWERVE_TIMEOUT_MS = 30;
-    public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0);
-
-    // swerve configuration
-    public static final double NEUTRAL_DEADBAND = 0.04;
-    public static final double OPEN_LOOP_RAMP_RATE = 0.2;
-    public static final int REMOTE_0 = 0;
-
-    // misc
-    public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
-  }
+    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 DriveConstants {
        public static final int DRIVE_PIGEON_ID = 6;

@@ -12,7 +12,7 @@ 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.motorcontrol.Spark;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import frc4388.robot.Constants.LEDConstants;
 import frc4388.utility.RobotGyro;
@@ -80,6 +80,6 @@ public class DiffDrive extends SubsystemBase {
    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);
+    //SmartDashboard.putData(m_gyro);
  }
 }
@@ -7,7 +7,7 @@

 package frc4388.robot.subsystems;

-import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;

@@ -4,312 +4,192 @@

 package frc4388.robot.subsystems;

-import com.ctre.phoenix.sensors.WPI_Pigeon2;
-
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.smartdashboard.Field2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
-import frc4388.utility.Gains;
+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 m_leftFront;
-  private SwerveModule m_leftBack;
-  private SwerveModule m_rightFront;
-  private SwerveModule m_rightBack;
+  private SwerveModule[] modules;

-  double halfWidth = SwerveDriveConstants.WIDTH / 2.d;
-  double halfHeight = SwerveDriveConstants.HEIGHT / 2.d;
+  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);

-  public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
+  private RobotGyro gyro;

-  Translation2d m_frontLeftLocation = 
-      new Translation2d(
-          Units.inchesToMeters(halfHeight), 
-          Units.inchesToMeters(halfWidth));
-  Translation2d m_frontRightLocation =
-       new Translation2d(
-          Units.inchesToMeters(halfHeight), 
-          Units.inchesToMeters(-halfWidth));
-  Translation2d m_backLeftLocation = 
-      new Translation2d(
-          Units.inchesToMeters(-halfHeight), 
-          Units.inchesToMeters(halfWidth));
-  Translation2d m_backRightLocation = 
-      new Translation2d(
-          Units.inchesToMeters(-halfHeight), 
-          Units.inchesToMeters(-halfWidth));
+  public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
+  
+  public double rotTarget = 0.0;
+  public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();

-  public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation,
-      m_backLeftLocation, m_backRightLocation);
-
-  public SwerveModule[] modules;
-  public WPI_Pigeon2 m_gyro;
-
-  public SwerveDriveOdometry m_odometry;
-  // public SwerveDriveOdometry m_odometry;
-
-  public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
-  public boolean ignoreAngles;
-  public Rotation2d rotTarget = new Rotation2d();
-  private ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
-
-  private final Field2d m_field = new Field2d();
-
-  public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack,
-      WPI_Pigeon2 gyro) {
-
-    m_leftFront = leftFront;
-    m_leftBack = leftBack;
-    m_rightFront = rightFront;
-    m_rightBack = rightBack;
-    m_gyro = gyro;
-
-    modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack};
+  /** 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;
    
-    // m_poseEstimator = new SwerveDrivePoseEstimator(
-    //     getRegGyro(),//m_gyro.getRotation2d(),
-    //     new Pose2d(),
-    //     m_kinematics,
-    //     VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)),  // TODO: tune
-    //     VecBuilder.fill(Units.degreesToRadians(1)),            // TODO: tune
-    //     VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune
-    
-    m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
+    this.gyro = gyro;

-    m_gyro.reset();
-    SmartDashboard.putData("Field", m_field);
+    this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
  }

-  public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative) {
-    Translation2d speed = new Translation2d(speedX, speedY);
-    driveWithInput(speed, rot, fieldRelative);
-  }
+  boolean stopped = false;
+  public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
+    if (fieldRelative) {

-  /**
-   * Method to drive the robot using joystick info.
-   * @link https://github.com/ZachOrr/MK3-Swerve-Example
-   * @param speeds[0]     Speed of the robot in the x direction (forward).
-   * @param speeds[1]     Speed of the robot in the y direction (sideways).
-   * @param rot           Angular rate of the robot.
-   * @param fieldRelative Whether the provided x and y speeds are relative to the
-   *                      field.
-   */
-  public void driveWithInput(Translation2d speed, double rot, boolean fieldRelative) {
-    ignoreAngles = (speed.getX() == 0) && (speed.getY() == 0) && (rot == 0);
+      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;
+        }

-    double mag = speed.getNorm();
-    speed = speed.times(mag * speedAdjust);
+        SmartDashboard.putBoolean("drift correction", true);
+        rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;

-    double xSpeedMetersPerSecond = speed.getX();
-    double ySpeedMetersPerSecond = speed.getY();
-    chassisSpeeds = fieldRelative
-        ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
-            -rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
-        : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond,
-            -rot * SwerveDriveConstants.ROTATION_SPEED * 2);
-    SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds);
-    setModuleStates(states);
-  }
+      }

-  public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) {
-    Translation2d speed = new Translation2d(leftX, leftY);
-    Translation2d head = new Translation2d(rightX, rightY);
-    driveWithInput(speed, head, fieldRelative);
-  }
+      // 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));

-  // new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180)))
-  public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {    
-    ignoreAngles = leftStick.getX() == 0 && leftStick.getY() == 0 && rightStick.getX() == 0 && rightStick.getY() == 0;
-    leftStick = leftStick.times(leftStick.getNorm() * speedAdjust);
-    if (Math.abs(rightStick.getX()) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightStick.getY()) > OIConstants.RIGHT_AXIS_DEADBAND)
-      rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0,1));
-    double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
-    if (ignoreAngles) {
-      rot = 0;
+      // 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);
    }
-    double xSpeedMetersPerSecond = leftStick.getX();
-    double ySpeedMetersPerSecond = leftStick.getY();
-    chassisSpeeds = fieldRelative
-        ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
-            rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
-        : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED * 2);
-    SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
-        chassisSpeeds);
-    // if (ignoreAngles) {
-    //   SwerveModuleState[] lockedStates = new SwerveModuleState[states.length];
-    //   for (int i = 0; i < states.length; i ++) {
-    //     SwerveModuleState state = states[i];
-    //     lockedStates[i]= new SwerveModuleState(0, state.angle);
-    //   }
-    //   setModuleStates(lockedStates);
-    // }
-    setModuleStates(states);
-    // SmartDashboard.putNumber("rot", rot);
-    // SmartDashboard.putNumber("rotarget", rotTarget.getDegrees());
+    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_SEC));
-    // int i = 2; {
+    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, ignoreAngles);
+      module.setDesiredState(state);
    }
-    // modules[0].setDesiredState(desiredStates[0], false);
  }

-  public void setModuleRotationsToAngle(double angle) {
-    for (int i = 0; i < modules.length; i++) {
-      SwerveModule module = modules[i];
-      module.rotateToAngle(angle);
+  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() {
-    updateOdometry();
-    updateSmartDash();
-
-    // SmartDashboard.putNumber("Pigeon getRotation2d", m_gyro.getRotation2d().getDegrees());
-    // SmartDashboard.putNumber("Pigeon getAngle", m_gyro.getAngle());
-    // SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
-    // SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360);
-
-    m_field.setRobotPose(getOdometry());
-    super.periodic();
+    // This method will be called once per scheduler run\
+    SmartDashboard.putNumber("Gyro", getGyroAngle());
  }

-  private void updateSmartDash() {
-    // odometry
-    SmartDashboard.putNumber("Odometry: X", getOdometry().getX());
-    SmartDashboard.putNumber("Odometry: Y", getOdometry().getY());
-    SmartDashboard.putNumber("Odometry: Theta", getOdometry().getRotation().getDegrees());
-
-    // chassis speeds
-    // TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds
-    // SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond);
-    // SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
-    // SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
-  }
-
-  /**
-   * Gets the current chassis speeds in m/s and rad/s.
-   * @return Current chassis speeds (vx, vy, ω)
-   */
-  public ChassisSpeeds getChassisSpeeds() {
-    return chassisSpeeds;
-  }
-
-  /**
-   * Gets the current pose of the robot.
-   * 
-   * @return Robot's current pose.
-   */
-  public Pose2d getOdometry() {
-    // return m_odometry.getPoseMeters();
-    return m_odometry.getPoseMeters();
-    // return m_poseEstimator.getEstimatedPosition();
-  }
-
-  public Pose2d getAutoOdo() {
-    Pose2d workingPose = getOdometry();
-    return new Pose2d(-workingPose.getX(), workingPose.getY(), workingPose.getRotation());
-  }
-
-  /**
-   * Gets the current gyro using regression formula.
-   * 
-   * @return Rotation2d object holding current gyro in radians
-   */
-  public Rotation2d getRegGyro() { 
-    // * test chassis regression
-    // double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527;
-    // * new robot regression
-    double regCur = 0.2507023948 + m_gyro.getRotation2d().getDegrees() * 0.999034743;
-    return new Rotation2d(Math.toRadians(regCur));
-  }
-
-  /**
-   * Resets the odometry of the robot to the given pose.
-   */
-  public void resetOdometry(Pose2d pose) {
-    m_odometry.resetPosition(pose, m_gyro.getRotation2d());
-  }
-
-  /**
-   * Updates the field relative position of the robot.
-   */
-  public void updateOdometry() {
-    Rotation2d actualDWI = new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2)); //+ (Math.PI/2));
-    Rotation2d actual =  new Rotation2d(m_gyro.getRotation2d().getRadians());
-
-    SmartDashboard.putNumber("AUTO ACTUAL GYRO", actual.getDegrees());
-    SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees());
-
-    m_odometry.update( actual,//m_gyro.getRotation2d(),//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
-                       modules[0].getState(),
-                       modules[1].getState(),
-                       modules[2].getState(),
-                       modules[3].getState());
-  }
-  
-  
-  /**
-   * Resets pigeon.
-   */
-  public void resetGyro() {
-    m_gyro.reset();
-    rotTarget = new Rotation2d(0);
-  }
-
-  /**
-   * Stop all four swerve modules.
-   */
-  public void stopModules() {
-    modules[0].stop();
-    modules[1].stop();
-    modules[2].stop();
-    modules[3].stop();
-  }
-
-  /**
-   * Switches speed modes.
-   * 
-   * @param shift True if fast mode, false if slow mode.
-   */
-  public void highSpeed(boolean shift) {
-    if (shift) {
-      speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_FAST;
+  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 {
-      speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
+      this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
    }
  }

-  public double getCurrent(){
-    return m_leftFront.getCurrent() + m_rightFront.getCurrent() + m_rightBack.getCurrent() + m_leftBack.getCurrent();
+  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 double getVoltage(){
-    return m_leftFront.getVoltage() + m_rightFront.getVoltage() + m_rightBack.getVoltage() + m_leftBack.getVoltage();
+  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;
+    }
+  }
+
+}
@@ -7,175 +7,155 @@ 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.TalonFXFeedbackDevice;
 import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
-import com.ctre.phoenix.sensors.CANCoderConfiguration;

 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.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.Gains;

 public class SwerveModule extends SubsystemBase {
-    public WPI_TalonFX angleMotor;
-    public WPI_TalonFX driveMotor;
-    private CANCoder canCoder;
-    public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
-
-    private static double kEncoderTicksPerRotation = 4096;
-    private SwerveModuleState state;
-    private double canCoderFeedbackCoefficient;
-
-    public long m_currentTime;
-    public long m_lastTime;
-    public double m_deltaTime;
-
-    public double m_currentPos;
-    public double m_lastPos;
-
-    public SwerveModuleState lastState = new SwerveModuleState();
-    public SwerveModuleState currentState;
+    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 canCoder, double offset) {
+    public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) {
        this.driveMotor = driveMotor;
        this.angleMotor = angleMotor;
-        this.canCoder = canCoder;
-        canCoderFeedbackCoefficient = canCoder.configGetFeedbackCoefficient();
+        this.encoder = encoder;

-        TalonFXConfiguration angleTalonFXConfiguration = new TalonFXConfiguration();
+        TalonFXConfiguration angleConfig = new TalonFXConfiguration();
+        angleConfig.slot0.kP = swerveGains.kP;
+        angleConfig.slot0.kI = swerveGains.kI;
+        angleConfig.slot0.kD = swerveGains.kD;

-        angleTalonFXConfiguration.slot0.kP = m_swerveGains.kP;
-        angleTalonFXConfiguration.slot0.kI = m_swerveGains.kI;
-        angleTalonFXConfiguration.slot0.kD = m_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);

-        // Use the CANCoder as the remote sensor for the primary TalonFX PID
-        angleTalonFXConfiguration.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID();
-        angleTalonFXConfiguration.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
-        angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
-        angleMotor.configAllSettings(angleTalonFXConfiguration);
-        // angleMotor.setInverted(true);
-        // TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
-        // driveTalonFXConfiguration.slot0.kP = 0.05;
-        // driveTalonFXConfiguration.slot0.kI = 0.0;
-        // driveTalonFXConfiguration.slot0.kD = 0.0;
-        // driveTalonFXConfiguration.primaryPID.selectedFeedbackSensor =
-        // FeedbackDevice.IntegratedSensor;
-        driveMotor.configFactoryDefault();
-        driveMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30);
-        driveMotor.configNominalOutputForward(0, 30);
-        driveMotor.configNominalOutputReverse(0, 30);
-        driveMotor.configPeakOutputForward(1, 30);
-        driveMotor.configPeakOutputReverse(-1, 30);
-        driveMotor.configAllowableClosedloopError(0, 0, 30);
-        // driveMotor.setInverted(true);
-        driveMotor.config_kP(0, 0, 30);
-        driveMotor.config_kI(0, 0, 30);
-        driveMotor.config_kD(0, 0, 30);
+        encoder.configMagnetOffset(offset);
        
-        // driveMotor.configAllSettings(driveTalonFXConfiguration);
-
-        CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
-        canCoderConfiguration.sensorCoefficient = 0.087890625;
-        canCoderConfiguration.magnetOffsetDegrees = offset;
-        canCoderConfiguration.sensorDirection = true;
-        canCoder.configAllSettings(canCoderConfiguration);
-
-        m_currentTime = System.currentTimeMillis();
-        m_lastTime = System.currentTimeMillis();
-
-        m_lastPos = driveMotor.getSelectedSensorPosition();
-    }
-
-    private Rotation2d getAngle() {
-        // ! Note: This assumes the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
-        return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
+        driveMotor.setSelectedSensorPosition(0);
+        driveMotor.config_kP(0, 0.2);
    }

    /**
-     * Set the speed + rotation of the swerve module from a SwerveModuleState object
-     *
-     * @param desiredState - A SwerveModuleState representing the desired new state
-     *                     of the module
+     * Get the drive motor of the SwerveModule
+     * @return the drive motor of the SwerveModule
     */
-    public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
-        Rotation2d currentRotation = getAngle();
-        // currentRotation.getDegrees());
-        state = SwerveModuleState.optimize(desiredState, currentRotation);
-
-        // Find the difference between our current rotational position + our new
-        // rotational position
-        Rotation2d rotationDelta = state.angle.minus(currentRotation);
-
-        // Find the new absolute position of the module based on the difference in
-        // rotation
-        double deltaTicks = (rotationDelta.getDegrees() / 360.) * kEncoderTicksPerRotation;
-        // Convert the CANCoder from it's position reading back to ticks
-        double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient;
-        double desiredTicks = currentTicks + deltaTicks;
-
-        if (!ignoreAngle) {
-            angleMotor.set(TalonFXControlMode.Position, desiredTicks);
-        }
-
-        // Please work
-        double ftPerSec = Units.metersToFeet(state.speedMetersPerSecond);
-        double normFtPerSec = ftPerSec / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC;
-        // double angleCorrection = angleMotor.getSelectedSensorVelocity() * 2.69;
-        driveMotor.set(normFtPerSec);// - angleMotor.get());
-        // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection); // Ratio
-        // between axis = 1/1.75 Ratio of wheel is 5.14/1 ratio of steer is 12.8/1
+    public WPI_TalonFX getDriveMotor() {
+        return this.driveMotor;
    }

    /**
-     * Get current module state.
-     *
-     * @return The current state of the module in m/s.
+     * Get the angle motor of the SwerveModule
+     * @return the angle motor of the SwerveModule
     */
-    public SwerveModuleState getState() {
-        // return state;
-        return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK
-                * SwerveDriveConstants.METERS_PER_INCH * 10, getAngle());
+    public WPI_TalonFX getAngleMotor() {
+        return this.angleMotor;
    }

    /**
-     * Stop the drive and steer motors of current module.
+     * 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) {
-        this.angleMotor.set(TalonFXControlMode.Position, angle);
+        angleMotor.set(TalonFXControlMode.Position, angle);
    }

-    @Override
-    public void periodic() {
-        currentState = this.getState();
-
-        Rotation2d currentRotation = getAngle();
-        SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
-        SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(),
-                ((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360);
-
-        lastState = currentState;
+    /**
+     * 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()
+        );
    }

-    public void reset() {
-        canCoder.setPositionToAbsolute();
-        // canCoder.configSensorInitializationStrategy(initializationStrategy)
+    /**
+     * 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());
    }
-    public double getCurrent(){
+
+    /**
+     * 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(){
+    public double getVoltage() {
        return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
    }
-}
+}
@@ -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);
+    }    
+}
@@ -7,31 +7,34 @@

 package frc4388.utility;

-import com.ctre.phoenix.sensors.PigeonIMU;
-import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
 import com.kauailabs.navx.frc.AHRS;

-import edu.wpi.first.wpilibj.GyroBase;
-import edu.wpi.first.wpiutil.math.MathUtil;
+// 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 extends GyroBase {
+public class RobotGyro implements Gyro {
    private RobotTime m_robotTime = RobotTime.getInstance();

-    private PigeonIMU m_pigeon = null;
+    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(PigeonIMU gyro) {
+    public RobotGyro(WPI_Pigeon2 gyro) {
        m_pigeon = gyro;
        m_isGyroAPigeon = true;
    }
@@ -45,6 +48,16 @@ public class RobotGyro extends GyroBase {
        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.
@@ -74,7 +87,7 @@ public class RobotGyro extends GyroBase {
    @Override
    public void calibrate() {
        if (m_isGyroAPigeon) {
-            m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
+            m_pigeon.calibrate();
        } else {
            m_navX.calibrate();
        }
@@ -82,6 +95,8 @@ public class RobotGyro extends GyroBase {

    @Override
    public void reset() {
+        resetZeroValues();
+
        if (m_isGyroAPigeon) {
            m_pigeon.setYaw(0);
        } else {
@@ -98,9 +113,10 @@ public class RobotGyro extends GyroBase {
 	 *					Roll is within [-90,+90] degrees.
     */
    private double[] getPigeonAngles() {
-        double[] angles = new double[3];
-        m_pigeon.getYawPitchRoll(angles);
-        return angles;
+        double[] ypr = new double[3];
+        m_pigeon.getYawPitchRoll(ypr);
+
+        return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)};
    }

    @Override
@@ -112,6 +128,10 @@ public class RobotGyro extends GyroBase {
        }
    }

+    public double getYaw() {
+       return this.getAngle();
+    }
+
    /**
     * Gets an absolute heading of the robot
     * @return heading from -180 to 180 degrees
@@ -165,7 +185,7 @@ public class RobotGyro extends GyroBase {
        }
    }

-    public PigeonIMU getPigeon(){
+    public WPI_Pigeon2 getPigeon(){
        return m_pigeon;
    }

@@ -1,7 +1,6 @@
 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
@@ -32,7 +31,7 @@ public class XboxTriggerButton extends Button {
 	}

 	/** {@inheritDoc} */
-	@Override
+	// @Override
 	public boolean get() {
 		if (m_trigger == RIGHT_TRIGGER) {
 			return m_controller.getRightTrigger();
@@ -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,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"
+            ]
+        }
+    ]
+}
@@ -1,180 +1,423 @@
 {
    "fileName": "Phoenix.json",
-    "name": "CTRE-Phoenix",
-    "version": "5.17.3",
+    "name": "CTRE-Phoenix (v5)",
+    "version": "5.31.0+23.2.2",
+    "frcYear": 2023,
    "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
    "mavenUrls": [
-        "http://devsite.ctr-electronics.com/maven/release/"
+        "https://maven.ctr-electronics.com/release/"
    ],
-    "jsonUrl": "http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json",
+    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json",
    "javaDependencies": [
        {
            "groupId": "com.ctre.phoenix",
            "artifactId": "api-java",
-            "version": "5.17.3"
+            "version": "5.31.0"
        },
        {
            "groupId": "com.ctre.phoenix",
            "artifactId": "wpiapi-java",
-            "version": "5.17.3"
+            "version": "5.31.0"
        }
    ],
    "jniDependencies": [
        {
            "groupId": "com.ctre.phoenix",
            "artifactId": "cci",
-            "version": "5.17.3",
+            "version": "5.31.0",
            "isJar": false,
            "skipInvalidPlatforms": true,
            "validPlatforms": [
-                "linuxathena",
                "windowsx86-64",
-                "linuxx86-64"
-            ]
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
        },
        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "diagnostics",
-            "version": "5.17.3",
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "cci-sim",
+            "version": "5.31.0",
            "isJar": false,
            "skipInvalidPlatforms": true,
            "validPlatforms": [
-                "linuxathena",
                "windowsx86-64",
-                "linuxx86-64"
-            ]
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
        },
        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "canutils",
-            "version": "5.17.3",
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "tools",
+            "version": "23.2.2",
            "isJar": false,
            "skipInvalidPlatforms": true,
            "validPlatforms": [
                "windowsx86-64",
-                "linuxx86-64"
-            ]
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
        },
        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "platform-stub",
-            "version": "5.17.3",
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "tools-sim",
+            "version": "23.2.2",
            "isJar": false,
            "skipInvalidPlatforms": true,
            "validPlatforms": [
                "windowsx86-64",
-                "linuxx86-64"
-            ]
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
        },
        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "core",
-            "version": "5.17.3",
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simTalonSRX",
+            "version": "23.2.2",
            "isJar": false,
            "skipInvalidPlatforms": true,
            "validPlatforms": [
-                "linuxathena",
                "windowsx86-64",
-                "linuxx86-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.17.3",
+            "version": "5.31.0",
            "libName": "CTRE_Phoenix_WPI",
            "headerClassifier": "headers",
-            "sharedLibrary": false,
+            "sharedLibrary": true,
            "skipInvalidPlatforms": true,
            "binaryPlatforms": [
-                "linuxathena",
                "windowsx86-64",
-                "linuxx86-64"
-            ]
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
        },
        {
            "groupId": "com.ctre.phoenix",
            "artifactId": "api-cpp",
-            "version": "5.17.3",
+            "version": "5.31.0",
            "libName": "CTRE_Phoenix",
            "headerClassifier": "headers",
-            "sharedLibrary": false,
+            "sharedLibrary": true,
            "skipInvalidPlatforms": true,
            "binaryPlatforms": [
-                "linuxathena",
                "windowsx86-64",
-                "linuxx86-64"
-            ]
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
        },
        {
            "groupId": "com.ctre.phoenix",
            "artifactId": "cci",
-            "version": "5.17.3",
+            "version": "5.31.0",
            "libName": "CTRE_PhoenixCCI",
            "headerClassifier": "headers",
-            "sharedLibrary": false,
+            "sharedLibrary": true,
            "skipInvalidPlatforms": true,
            "binaryPlatforms": [
-                "linuxathena",
                "windowsx86-64",
-                "linuxx86-64"
-            ]
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
        },
        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "diagnostics",
-            "version": "5.17.3",
-            "libName": "CTRE_PhoenixDiagnostics",
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "tools",
+            "version": "23.2.2",
+            "libName": "CTRE_PhoenixTools",
            "headerClassifier": "headers",
-            "sharedLibrary": false,
+            "sharedLibrary": true,
            "skipInvalidPlatforms": true,
            "binaryPlatforms": [
-                "linuxathena",
                "windowsx86-64",
-                "linuxx86-64"
-            ]
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
        },
        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "canutils",
-            "version": "5.17.3",
-            "libName": "CTRE_PhoenixCanutils",
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "wpiapi-cpp-sim",
+            "version": "5.31.0",
+            "libName": "CTRE_Phoenix_WPISim",
            "headerClassifier": "headers",
-            "sharedLibrary": false,
+            "sharedLibrary": true,
            "skipInvalidPlatforms": true,
            "binaryPlatforms": [
                "windowsx86-64",
-                "linuxx86-64"
-            ]
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
        },
        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "platform-stub",
-            "version": "5.17.3",
-            "libName": "CTRE_PhoenixPlatform",
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "api-cpp-sim",
+            "version": "5.31.0",
+            "libName": "CTRE_PhoenixSim",
            "headerClassifier": "headers",
-            "sharedLibrary": false,
+            "sharedLibrary": true,
            "skipInvalidPlatforms": true,
            "binaryPlatforms": [
                "windowsx86-64",
-                "linuxx86-64"
-            ]
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
        },
        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "core",
-            "version": "5.17.3",
-            "libName": "CTRE_PhoenixCore",
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "cci-sim",
+            "version": "5.31.0",
+            "libName": "CTRE_PhoenixCCISim",
            "headerClassifier": "headers",
-            "sharedLibrary": false,
+            "sharedLibrary": true,
            "skipInvalidPlatforms": true,
            "binaryPlatforms": [
-                "linuxathena",
                "windowsx86-64",
-                "linuxx86-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"
        }
    ]
 }
@@ -1,37 +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"
-            ]
-        }
-    ]
-}
+  "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"
+      ]
+    }
+  ]
+}