diff --git a/cpp/SwerveWithPathPlanner/.gitignore b/cpp/SwerveWithPathPlanner/.gitignore
new file mode 100644
index 00000000..375359c2
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/.gitignore
@@ -0,0 +1,184 @@
+# This gitignore has been specially created by the WPILib team.
+# If you remove items from this file, intellisense might break.
+
+### C++ ###
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+### Java ###
+# Compiled class file
+*.class
+
+# Log file
+*.log
+
+# BlueJ files
+*.ctxt
+
+# Mobile Tools for Java (J2ME)
+.mtj.tmp/
+
+# Package Files #
+*.jar
+*.war
+*.nar
+*.ear
+*.zip
+*.tar.gz
+*.rar
+
+# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml
+hs_err_pid*
+
+### Linux ###
+*~
+
+# temporary files which can be created if a process still has a handle open of a deleted file
+.fuse_hidden*
+
+# KDE directory preferences
+.directory
+
+# Linux trash folder which might appear on any partition or disk
+.Trash-*
+
+# .nfs files are created when an open file is removed but is still being accessed
+.nfs*
+
+### macOS ###
+# General
+.DS_Store
+.AppleDouble
+.LSOverride
+
+# Icon must end with two \r
+Icon
+
+# Thumbnails
+._*
+
+# Files that might appear in the root of a volume
+.DocumentRevisions-V100
+.fseventsd
+.Spotlight-V100
+.TemporaryItems
+.Trashes
+.VolumeIcon.icns
+.com.apple.timemachine.donotpresent
+
+# Directories potentially created on remote AFP share
+.AppleDB
+.AppleDesktop
+Network Trash Folder
+Temporary Items
+.apdisk
+
+### VisualStudioCode ###
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+
+### Windows ###
+# Windows thumbnail cache files
+Thumbs.db
+ehthumbs.db
+ehthumbs_vista.db
+
+# Dump file
+*.stackdump
+
+# Folder config file
+[Dd]esktop.ini
+
+# Recycle Bin used on file shares
+$RECYCLE.BIN/
+
+# Windows Installer files
+*.cab
+*.msi
+*.msix
+*.msm
+*.msp
+
+# Windows shortcuts
+*.lnk
+
+### Gradle ###
+.gradle
+/build/
+
+# Ignore Gradle GUI config
+gradle-app.setting
+
+# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored)
+!gradle-wrapper.jar
+
+# Cache of project
+.gradletasknamecache
+
+# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898
+# gradle/wrapper/gradle-wrapper.properties
+
+# # VS Code Specific Java Settings
+# DO NOT REMOVE .classpath and .project
+.classpath
+.project
+.settings/
+bin/
+
+# IntelliJ
+*.iml
+*.ipr
+*.iws
+.idea/
+out/
+
+# Fleet
+.fleet
+
+# Simulation GUI and other tools window save file
+networktables.json
+simgui.json
+*-window.json
+
+# Simulation data log directory
+logs/
+
+# Folder that has CTRE Phoenix Sim device config storage
+ctre_sim/
+
+# clangd
+/.cache
+compile_commands.json
diff --git a/cpp/SwerveWithPathPlanner/.wpilib/wpilib_preferences.json b/cpp/SwerveWithPathPlanner/.wpilib/wpilib_preferences.json
new file mode 100644
index 00000000..adf6ddda
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/.wpilib/wpilib_preferences.json
@@ -0,0 +1,6 @@
+{
+  "enableCppIntellisense": true,
+  "currentLanguage": "cpp",
+  "projectYear": "2025",
+  "teamNumber": 7762
+}
\ No newline at end of file
diff --git a/cpp/SwerveWithPathPlanner/WPILib-License.md b/cpp/SwerveWithPathPlanner/WPILib-License.md
new file mode 100644
index 00000000..645e5425
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/WPILib-License.md
@@ -0,0 +1,24 @@
+Copyright (c) 2009-2024 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.
diff --git a/cpp/SwerveWithPathPlanner/build.gradle b/cpp/SwerveWithPathPlanner/build.gradle
new file mode 100644
index 00000000..99861ab2
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/build.gradle
@@ -0,0 +1,99 @@
+plugins {
+    id "cpp"
+    id "google-test-test-suite"
+    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-1"
+}
+
+// Define my targets (RoboRIO) and artifacts (deployable files)
+// This is added by GradleRIO's backing project DeployUtils.
+deploy {
+    targets {
+        roborio(getTargetTypeClass('RoboRIO')) {
+            // Team number is loaded either from the .wpilib/wpilib_preferences.json
+            // or from command line. If not found an exception will be thrown.
+            // You can use getTeamOrDefault(team) instead of getTeamNumber if you
+            // want to store a team number in this file.
+            team = project.frc.getTeamNumber()
+            debug = project.frc.getDebugOrDefault(false)
+
+            artifacts {
+                // First part is artifact name, 2nd is artifact type
+                // getTargetTypeClass is a shortcut to get the class type using a string
+
+                frcCpp(getArtifactTypeClass('FRCNativeArtifact')) {
+                }
+
+                // Static files artifact
+                frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+                    files = project.fileTree('src/main/deploy')
+                    directory = '/home/lvuser/deploy'
+                }
+            }
+        }
+    }
+}
+
+def deployArtifact = deploy.targets.roborio.artifacts.frcCpp
+
+// Set this to true to enable desktop support.
+def includeDesktopSupport = true
+
+// Set to true to run simulation in debug mode
+wpi.cpp.debugSimulation = false
+
+// Default enable simgui
+wpi.sim.addGui().defaultEnabled = true
+// Enable DS but not by default
+wpi.sim.addDriverstation()
+
+model {
+    components {
+        frcUserProgram(NativeExecutableSpec) {
+            targetPlatform wpi.platforms.roborio
+            if (includeDesktopSupport) {
+                targetPlatform wpi.platforms.desktop
+            }
+
+            sources.cpp {
+                source {
+                    srcDir 'src/main/cpp'
+                    include '**/*.cpp', '**/*.cc'
+                }
+                exportedHeaders {
+                    srcDir 'src/main/include'
+                }
+            }
+
+            // Set deploy task to deploy this component
+            deployArtifact.component = it
+
+            // Enable run tasks for this component
+            wpi.cpp.enableExternalTasks(it)
+
+            // Enable simulation for this component
+            wpi.sim.enable(it)
+            // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+            wpi.cpp.vendor.cpp(it)
+            wpi.cpp.deps.wpilib(it)
+        }
+    }
+    testSuites {
+        frcUserProgramTest(GoogleTestTestSuiteSpec) {
+            testing $.components.frcUserProgram
+
+            sources.cpp {
+                source {
+                    srcDir 'src/test/cpp'
+                    include '**/*.cpp'
+                }
+            }
+
+            // Enable run tasks for this component
+            wpi.cpp.enableExternalTasks(it)
+
+            wpi.cpp.vendor.cpp(it)
+            wpi.cpp.deps.wpilib(it)
+            wpi.cpp.deps.googleTest(it)
+        }
+    }
+}
diff --git a/cpp/SwerveWithPathPlanner/gradle/wrapper/gradle-wrapper.jar b/cpp/SwerveWithPathPlanner/gradle/wrapper/gradle-wrapper.jar
new file mode 100644
index 00000000..a4b76b95
Binary files /dev/null and b/cpp/SwerveWithPathPlanner/gradle/wrapper/gradle-wrapper.jar differ
diff --git a/cpp/SwerveWithPathPlanner/gradle/wrapper/gradle-wrapper.properties b/cpp/SwerveWithPathPlanner/gradle/wrapper/gradle-wrapper.properties
new file mode 100644
index 00000000..fbacf711
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/gradle/wrapper/gradle-wrapper.properties
@@ -0,0 +1,7 @@
+distributionBase=GRADLE_USER_HOME
+distributionPath=permwrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.10.2-bin.zip
+networkTimeout=10000
+validateDistributionUrl=true
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=permwrapper/dists
diff --git a/cpp/SwerveWithPathPlanner/gradlew b/cpp/SwerveWithPathPlanner/gradlew
new file mode 100644
index 00000000..f5feea6d
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/gradlew
@@ -0,0 +1,252 @@
+#!/bin/sh
+
+#
+# Copyright © 2015-2021 the original authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#      https://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# SPDX-License-Identifier: Apache-2.0
+#
+
+##############################################################################
+#
+#   Gradle start up script for POSIX generated by Gradle.
+#
+#   Important for running:
+#
+#   (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is
+#       noncompliant, but you have some other compliant shell such as ksh or
+#       bash, then to run this script, type that shell name before the whole
+#       command line, like:
+#
+#           ksh Gradle
+#
+#       Busybox and similar reduced shells will NOT work, because this script
+#       requires all of these POSIX shell features:
+#         * functions;
+#         * expansions «$var», «${var}», «${var:-default}», «${var+SET}»,
+#           «${var#prefix}», «${var%suffix}», and «$( cmd )»;
+#         * compound commands having a testable exit status, especially «case»;
+#         * various built-in commands including «command», «set», and «ulimit».
+#
+#   Important for patching:
+#
+#   (2) This script targets any POSIX shell, so it avoids extensions provided
+#       by Bash, Ksh, etc; in particular arrays are avoided.
+#
+#       The "traditional" practice of packing multiple parameters into a
+#       space-separated string is a well documented source of bugs and security
+#       problems, so this is (mostly) avoided, by progressively accumulating
+#       options in "$@", and eventually passing that to Java.
+#
+#       Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS,
+#       and GRADLE_OPTS) rely on word-splitting, this is performed explicitly;
+#       see the in-line comments for details.
+#
+#       There are tweaks for specific operating systems such as AIX, CygWin,
+#       Darwin, MinGW, and NonStop.
+#
+#   (3) This script is generated from the Groovy template
+#       https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+#       within the Gradle project.
+#
+#       You can find Gradle at https://github.com/gradle/gradle/.
+#
+##############################################################################
+
+# Attempt to set APP_HOME
+
+# Resolve links: $0 may be a link
+app_path=$0
+
+# Need this for daisy-chained symlinks.
+while
+    APP_HOME=${app_path%"${app_path##*/}"}  # leaves a trailing /; empty if no leading path
+    [ -h "$app_path" ]
+do
+    ls=$( ls -ld "$app_path" )
+    link=${ls#*' -> '}
+    case $link in             #(
+      /*)   app_path=$link ;; #(
+      *)    app_path=$APP_HOME$link ;;
+    esac
+done
+
+# This is normally unused
+# shellcheck disable=SC2034
+APP_BASE_NAME=${0##*/}
+# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036)
+APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s
+' "$PWD" ) || exit
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD=maximum
+
+warn () {
+    echo "$*"
+} >&2
+
+die () {
+    echo
+    echo "$*"
+    echo
+    exit 1
+} >&2
+
+# OS specific support (must be 'true' or 'false').
+cygwin=false
+msys=false
+darwin=false
+nonstop=false
+case "$( uname )" in                #(
+  CYGWIN* )         cygwin=true  ;; #(
+  Darwin* )         darwin=true  ;; #(
+  MSYS* | MINGW* )  msys=true    ;; #(
+  NONSTOP* )        nonstop=true ;;
+esac
+
+CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
+
+
+# Determine the Java command to use to start the JVM.
+if [ -n "$JAVA_HOME" ] ; then
+    if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
+        # IBM's JDK on AIX uses strange locations for the executables
+        JAVACMD=$JAVA_HOME/jre/sh/java
+    else
+        JAVACMD=$JAVA_HOME/bin/java
+    fi
+    if [ ! -x "$JAVACMD" ] ; then
+        die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+    fi
+else
+    JAVACMD=java
+    if ! command -v java >/dev/null 2>&1
+    then
+        die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+    fi
+fi
+
+# Increase the maximum file descriptors if we can.
+if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
+    case $MAX_FD in #(
+      max*)
+        # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked.
+        # shellcheck disable=SC2039,SC3045
+        MAX_FD=$( ulimit -H -n ) ||
+            warn "Could not query maximum file descriptor limit"
+    esac
+    case $MAX_FD in  #(
+      '' | soft) :;; #(
+      *)
+        # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked.
+        # shellcheck disable=SC2039,SC3045
+        ulimit -n "$MAX_FD" ||
+            warn "Could not set maximum file descriptor limit to $MAX_FD"
+    esac
+fi
+
+# Collect all arguments for the java command, stacking in reverse order:
+#   * args from the command line
+#   * the main class name
+#   * -classpath
+#   * -D...appname settings
+#   * --module-path (only if needed)
+#   * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables.
+
+# For Cygwin or MSYS, switch paths to Windows format before running java
+if "$cygwin" || "$msys" ; then
+    APP_HOME=$( cygpath --path --mixed "$APP_HOME" )
+    CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" )
+
+    JAVACMD=$( cygpath --unix "$JAVACMD" )
+
+    # Now convert the arguments - kludge to limit ourselves to /bin/sh
+    for arg do
+        if
+            case $arg in                                #(
+              -*)   false ;;                            # don't mess with options #(
+              /?*)  t=${arg#/} t=/${t%%/*}              # looks like a POSIX filepath
+                    [ -e "$t" ] ;;                      #(
+              *)    false ;;
+            esac
+        then
+            arg=$( cygpath --path --ignore --mixed "$arg" )
+        fi
+        # Roll the args list around exactly as many times as the number of
+        # args, so each arg winds up back in the position where it started, but
+        # possibly modified.
+        #
+        # NB: a `for` loop captures its iteration list before it begins, so
+        # changing the positional parameters here affects neither the number of
+        # iterations, nor the values presented in `arg`.
+        shift                   # remove old arg
+        set -- "$@" "$arg"      # push replacement arg
+    done
+fi
+
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+
+# Collect all arguments for the java command:
+#   * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments,
+#     and any embedded shellness will be escaped.
+#   * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be
+#     treated as '${Hostname}' itself on the command line.
+
+set -- \
+        "-Dorg.gradle.appname=$APP_BASE_NAME" \
+        -classpath "$CLASSPATH" \
+        org.gradle.wrapper.GradleWrapperMain \
+        "$@"
+
+# Stop when "xargs" is not available.
+if ! command -v xargs >/dev/null 2>&1
+then
+    die "xargs is not available"
+fi
+
+# Use "xargs" to parse quoted args.
+#
+# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
+#
+# In Bash we could simply go:
+#
+#   readarray ARGS < <( xargs -n1 <<<"$var" ) &&
+#   set -- "${ARGS[@]}" "$@"
+#
+# but POSIX shell has neither arrays nor command substitution, so instead we
+# post-process each arg (as a line of input to sed) to backslash-escape any
+# character that might be a shell metacharacter, then use eval to reverse
+# that process (while maintaining the separation between arguments), and wrap
+# the whole thing up as a single "set" statement.
+#
+# This will of course break if any of these variables contains a newline or
+# an unmatched quote.
+#
+
+eval "set -- $(
+        printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
+        xargs -n1 |
+        sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
+        tr '\n' ' '
+    )" '"$@"'
+
+exec "$JAVACMD" "$@"
diff --git a/cpp/SwerveWithPathPlanner/gradlew.bat b/cpp/SwerveWithPathPlanner/gradlew.bat
new file mode 100644
index 00000000..9d21a218
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/gradlew.bat
@@ -0,0 +1,94 @@
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem      https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+@rem SPDX-License-Identifier: Apache-2.0
+@rem
+
+@if "%DEBUG%"=="" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%"=="" set DIRNAME=.
+@rem This is normally unused
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Resolve any "." and ".." in APP_HOME to make it shorter.
+for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if %ERRORLEVEL% equ 0 goto execute
+
+echo. 1>&2
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto execute
+
+echo. 1>&2
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
+
+goto fail
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+
+:end
+@rem End local scope for the variables with windows NT shell
+if %ERRORLEVEL% equ 0 goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+set EXIT_CODE=%ERRORLEVEL%
+if %EXIT_CODE% equ 0 set EXIT_CODE=1
+if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
+exit /b %EXIT_CODE%
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega
diff --git a/cpp/SwerveWithPathPlanner/settings.gradle b/cpp/SwerveWithPathPlanner/settings.gradle
new file mode 100644
index 00000000..969c7b09
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/settings.gradle
@@ -0,0 +1,30 @@
+import org.gradle.internal.os.OperatingSystem
+
+pluginManagement {
+    repositories {
+        mavenLocal()
+        gradlePluginPortal()
+        String frcYear = '2025'
+        File frcHome
+        if (OperatingSystem.current().isWindows()) {
+            String publicFolder = System.getenv('PUBLIC')
+            if (publicFolder == null) {
+                publicFolder = "C:\\Users\\Public"
+            }
+            def homeRoot = new File(publicFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        } else {
+            def userFolder = System.getProperty("user.home")
+            def homeRoot = new File(userFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        }
+        def frcHomeMaven = new File(frcHome, 'maven')
+        maven {
+            name 'frcHome'
+            url frcHomeMaven
+        }
+    }
+}
+
+Properties props = System.getProperties();
+props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true");
diff --git a/cpp/SwerveWithPathPlanner/src/main/cpp/Robot.cpp b/cpp/SwerveWithPathPlanner/src/main/cpp/Robot.cpp
new file mode 100644
index 00000000..2ed3f804
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/src/main/cpp/Robot.cpp
@@ -0,0 +1,55 @@
+// 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.
+
+#include "Robot.h"
+
+#include <frc2/command/CommandScheduler.h>
+
+Robot::Robot() {}
+
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
+
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+void Robot::DisabledExit() {}
+
+void Robot::AutonomousInit() {
+  m_autonomousCommand = m_container.GetAutonomousCommand();
+
+  if (m_autonomousCommand) {
+    m_autonomousCommand->Schedule();
+  }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::AutonomousExit() {}
+
+void Robot::TeleopInit() {
+  if (m_autonomousCommand) {
+    m_autonomousCommand->Cancel();
+  }
+}
+
+void Robot::TeleopPeriodic() {}
+
+void Robot::TeleopExit() {}
+
+void Robot::TestInit() {
+  frc2::CommandScheduler::GetInstance().CancelAll();
+}
+
+void Robot::TestPeriodic() {}
+
+void Robot::TestExit() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif
diff --git a/cpp/SwerveWithPathPlanner/src/main/cpp/RobotContainer.cpp b/cpp/SwerveWithPathPlanner/src/main/cpp/RobotContainer.cpp
new file mode 100644
index 00000000..e00ec701
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/src/main/cpp/RobotContainer.cpp
@@ -0,0 +1,53 @@
+// 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.
+
+#include "RobotContainer.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/Commands.h>
+#include <pathplanner/lib/auto/AutoBuilder.h>
+
+RobotContainer::RobotContainer()
+{
+    ConfigureBindings();
+
+    autoChooser = pathplanner::AutoBuilder::buildAutoChooser("Tests");
+    frc::SmartDashboard::PutData("Auto Mode", &autoChooser);
+}
+
+void RobotContainer::ConfigureBindings()
+{
+    // Note that X is defined as forward according to WPILib convention,
+    // and Y is defined as to the left according to WPILib convention.
+    drivetrain.SetDefaultCommand(
+        // Drivetrain will execute this command periodically
+        drivetrain.ApplyRequest([this] {
+            return drive.WithVelocityX(-joystick.GetLeftY() * MaxSpeed) // Drive forward with negative Y (forward)
+                .WithVelocityY(-joystick.GetLeftX() * MaxSpeed) // Drive left with negative X (left)
+                .WithRotationalRate(-joystick.GetRightX() * MaxAngularRate); // Drive counterclockwise with negative X (left)
+        })
+    );
+
+    joystick.A().WhileTrue(drivetrain.ApplyRequest([this] { return brake; }));
+    joystick.B().WhileTrue(drivetrain.ApplyRequest([this] {
+        return point.WithModuleDirection(frc::Rotation2d{-joystick.GetLeftY(), -joystick.GetLeftX()});
+    }));
+
+    // Run SysId routines when holding back/start and X/Y.
+    // Note that each routine should be run exactly once in a single log.
+    (joystick.Back() && joystick.Y()).WhileTrue(drivetrain.SysIdDynamic(frc2::sysid::Direction::kForward));
+    (joystick.Back() && joystick.X()).WhileTrue(drivetrain.SysIdDynamic(frc2::sysid::Direction::kReverse));
+    (joystick.Start() && joystick.Y()).WhileTrue(drivetrain.SysIdQuasistatic(frc2::sysid::Direction::kForward));
+    (joystick.Start() && joystick.X()).WhileTrue(drivetrain.SysIdQuasistatic(frc2::sysid::Direction::kReverse));
+
+    // reset the field-centric heading on left bumper press
+    joystick.LeftBumper().OnTrue(drivetrain.RunOnce([this] { drivetrain.SeedFieldRelative(); }));
+
+    drivetrain.RegisterTelemetry([this](auto const &state) { logger.Telemeterize(state); });
+}
+
+frc2::Command *RobotContainer::GetAutonomousCommand()
+{
+    return autoChooser.GetSelected();
+}
diff --git a/cpp/SwerveWithPathPlanner/src/main/cpp/Telemetry.cpp b/cpp/SwerveWithPathPlanner/src/main/cpp/Telemetry.cpp
new file mode 100644
index 00000000..749b62f8
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/src/main/cpp/Telemetry.cpp
@@ -0,0 +1,43 @@
+#include "Telemetry.h"
+#include <frc/smartdashboard/SmartDashboard.h>
+
+using namespace ctre::phoenix6;
+
+void Telemetry::Telemeterize(subsystems::CommandSwerveDrivetrain::SwerveDriveState const &state)
+{
+    /* Telemeterize the pose */
+    frc::Pose2d const pose = state.Pose;
+    fieldTypePub.Set("Field2d");
+    fieldPub.Set(std::array{
+        pose.X().value(),
+        pose.Y().value(),
+        pose.Rotation().Degrees().value()
+    });
+
+    /* Telemeterize the robot's general speeds */
+    units::second_t const currentTime = utils::GetCurrentTime();
+    units::second_t const diffTime = currentTime - lastTime;
+    lastTime = currentTime;
+
+    frc::Translation2d const distanceDiff = (pose - m_lastPose).Translation();
+    m_lastPose = pose;
+
+    frc::Translation2d const velocities = distanceDiff / diffTime.value();
+
+    speed.Set(velocities.Norm().value());
+    velocityX.Set(velocities.X().value());
+    velocityY.Set(velocities.Y().value());
+    odomFreq.Set((1.0 / state.OdometryPeriod).value());
+
+    /* Telemeterize the module's states */
+    for (size_t i = 0; i < m_moduleSpeeds.size(); ++i) {
+        m_moduleSpeeds[i]->SetAngle(state.ModuleStates[i].angle.Degrees());
+        m_moduleDirections[i]->SetAngle(state.ModuleStates[i].angle.Degrees());
+        m_moduleSpeeds[i]->SetLength(state.ModuleStates[i].speed / (2 * MaxSpeed));
+
+        frc::SmartDashboard::PutData("Module " + std::to_string(i), &m_moduleMechanisms[i]);
+    }
+
+    SignalLogger::WriteDoubleArray("odometry", {pose.X().value(), pose.Y().value(), pose.Rotation().Degrees().value()});
+    SignalLogger::WriteDouble("odom period", state.OdometryPeriod.value(), "seconds");
+}
diff --git a/cpp/SwerveWithPathPlanner/src/main/cpp/subsystems/CommandSwerveDrivetrain.cpp b/cpp/SwerveWithPathPlanner/src/main/cpp/subsystems/CommandSwerveDrivetrain.cpp
new file mode 100644
index 00000000..a8a9631a
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/src/main/cpp/subsystems/CommandSwerveDrivetrain.cpp
@@ -0,0 +1,79 @@
+#include "subsystems/CommandSwerveDrivetrain.h"
+#include <pathplanner/lib/auto/AutoBuilder.h>
+#include <pathplanner/lib/controllers/PPHolonomicDriveController.h>
+
+using namespace subsystems;
+
+void CommandSwerveDrivetrain::ConfigureAutoBuilder()
+{
+    auto config = pathplanner::RobotConfig::fromGUISettings();
+    pathplanner::AutoBuilder::configure(
+        // Supplier of current robot pose
+        [this] { return GetState().Pose; },
+        // Consumer for seeding pose against auto
+        [this](frc::Pose2d const &pose) { return SeedFieldRelative(pose); },
+        // Supplier of current robot speeds
+        [this] {
+            auto const speeds = GetState().Speeds;
+            return frc::ChassisSpeeds{speeds.vx, speeds.vy, speeds.omega};
+        },
+        // Consumer of ChassisSpeeds and feedforwards to drive the robot
+        [this](frc::ChassisSpeeds const &speeds, pathplanner::DriveFeedforwards const &feedforwards) {
+            swerve::impl::ChassisSpeeds robotSpeeds{speeds.vx, speeds.vy, speeds.omega};
+            return SetControl(
+                m_applyRobotSpeeds.WithSpeeds(robotSpeeds)
+                    .WithWheelForceFeedforwardsX(feedforwards.robotRelativeForcesX)
+                    .WithWheelForceFeedforwardsY(feedforwards.robotRelativeForcesY)
+            );
+        },
+        std::make_shared<pathplanner::PPHolonomicDriveController>(
+            // PID constants for translation
+            pathplanner::PIDConstants{10.0, 0.0, 0.0},
+            // PID constants for rotation
+            pathplanner::PIDConstants{10.0, 0.0, 0.0}
+        ),
+        std::move(config),
+        // Assume the path needs to be flipped for Red vs Blue, this is normally the case
+        [] {
+            auto const alliance = frc::DriverStation::GetAlliance().value_or(frc::DriverStation::Alliance::kBlue);
+            return alliance == frc::DriverStation::Alliance::kRed;
+        },
+        this // Subsystem for requirements
+    );
+}
+
+void CommandSwerveDrivetrain::Periodic()
+{
+    /*
+     * Periodically try to apply the operator perspective.
+     * If we haven't applied the operator perspective before, then we should apply it regardless of DS state.
+     * This allows us to correct the perspective in case the robot code restarts mid-match.
+     * Otherwise, only check and apply the operator perspective if the DS is disabled.
+     * This ensures driving behavior doesn't change until an explicit disable event occurs during testing.
+     */
+    if (!m_hasAppliedOperatorPerspective || frc::DriverStation::IsDisabled()) {
+        auto const allianceColor = frc::DriverStation::GetAlliance();
+        if (allianceColor) {
+            SetOperatorPerspectiveForward(
+                *allianceColor == frc::DriverStation::Alliance::kRed
+                    ? kRedAlliancePerspectiveRotation
+                    : kBlueAlliancePerspectiveRotation
+            );
+            m_hasAppliedOperatorPerspective = true;
+        }
+    }
+}
+
+void CommandSwerveDrivetrain::StartSimThread()
+{
+    m_lastSimTime = utils::GetCurrentTime();
+    m_simNotifier = std::make_unique<frc::Notifier>([this] {
+        units::second_t const currentTime = utils::GetCurrentTime();
+        auto const deltaTime = currentTime - m_lastSimTime;
+        m_lastSimTime = currentTime;
+
+        /* use the measured time delta, get battery voltage from WPILib */
+        UpdateSimState(deltaTime, frc::RobotController::GetBatteryVoltage());
+    });
+    m_simNotifier->StartPeriodic(kSimLoopPeriod);
+}
diff --git a/cpp/SwerveWithPathPlanner/src/main/deploy/example.txt b/cpp/SwerveWithPathPlanner/src/main/deploy/example.txt
new file mode 100644
index 00000000..bb82515d
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/src/main/deploy/example.txt
@@ -0,0 +1,3 @@
+Files placed in this directory will be deployed to the RoboRIO into the
+'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function
+to get a proper path relative to the deploy directory.
\ No newline at end of file
diff --git a/cpp/SwerveWithPathPlanner/src/main/deploy/pathplanner/Tests.path b/cpp/SwerveWithPathPlanner/src/main/deploy/pathplanner/Tests.path
new file mode 100644
index 00000000..9cb675ba
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/src/main/deploy/pathplanner/Tests.path
@@ -0,0 +1,234 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 7.855128995972278,
+        "y": 3.8506658441888315
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 9.51563917413549,
+        "y": 3.864980587104032
+      },
+      "holonomicAngle": 0.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 9.673101346202692,
+        "y": 4.909956819913639
+      },
+      "prevControl": {
+        "x": 9.658786603287492,
+        "y": 4.2657933887296355
+      },
+      "nextControl": {
+        "x": 9.658786603287492,
+        "y": 4.2657933887296355
+      },
+      "holonomicAngle": 0.0,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 9.744675060778693,
+        "y": 3.278076127580827
+      },
+      "prevControl": {
+        "x": 9.701730832033093,
+        "y": 3.922239558764832
+      },
+      "nextControl": {
+        "x": 9.701730832033093,
+        "y": 3.922239558764832
+      },
+      "holonomicAngle": 0,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 7.855128995972278,
+        "y": 3.8506658441888315
+      },
+      "prevControl": {
+        "x": 9.071882143764288,
+        "y": 3.879295330019232
+      },
+      "nextControl": {
+        "x": 9.071882143764288,
+        "y": 3.879295330019232
+      },
+      "holonomicAngle": 0,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": true,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 9.673101346202692,
+        "y": 4.89564207699844
+      },
+      "prevControl": {
+        "x": 9.687416089117892,
+        "y": 4.222849159984035
+      },
+      "nextControl": {
+        "x": 9.687416089117892,
+        "y": 4.222849159984035
+      },
+      "holonomicAngle": 125.92486296979446,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 9.730360317863493,
+        "y": 3.263761384665627
+      },
+      "prevControl": {
+        "x": 9.748383531854783,
+        "y": 4.107878917512221
+      },
+      "nextControl": {
+        "x": 9.748383531854783,
+        "y": 4.107878917512221
+      },
+      "holonomicAngle": -91.03738139006352,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 9.013854168059215,
+        "y": 3.3824178174672146
+      },
+      "prevControl": {
+        "x": 9.077332014313154,
+        "y": 4.307380720024598
+      },
+      "nextControl": {
+        "x": 9.077332014313154,
+        "y": 4.307380720024598
+      },
+      "holonomicAngle": 59.62087398863161,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 8.315597859265894,
+        "y": 3.3461447624649643
+      },
+      "prevControl": {
+        "x": 8.415348760522082,
+        "y": 3.863035796247031
+      },
+      "nextControl": {
+        "x": 8.415348760522082,
+        "y": 3.863035796247031
+      },
+      "holonomicAngle": -155.7255588655606,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 7.84404814423664,
+        "y": 3.863035796247031
+      },
+      "prevControl": {
+        "x": 8.78714757429515,
+        "y": 3.8086262137436555
+      },
+      "nextControl": null,
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "maxVelocity": 2.0,
+  "maxAcceleration": 3.0,
+  "isReversed": null,
+  "markers": [
+    {
+      "position": 3.4181818181818175,
+      "names": [
+        "marker"
+      ]
+    }
+  ]
+}
\ No newline at end of file
diff --git a/cpp/SwerveWithPathPlanner/src/main/deploy/pathplanner/autos/Tests.auto b/cpp/SwerveWithPathPlanner/src/main/deploy/pathplanner/autos/Tests.auto
new file mode 100644
index 00000000..e8c593b3
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/src/main/deploy/pathplanner/autos/Tests.auto
@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SimplePath"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/cpp/SwerveWithPathPlanner/src/main/deploy/pathplanner/navgrid.json b/cpp/SwerveWithPathPlanner/src/main/deploy/pathplanner/navgrid.json
new file mode 100644
index 00000000..bab0da93
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/cpp/SwerveWithPathPlanner/src/main/deploy/pathplanner/paths/SimplePath.path b/cpp/SwerveWithPathPlanner/src/main/deploy/pathplanner/paths/SimplePath.path
new file mode 100644
index 00000000..ca1e5b11
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/src/main/deploy/pathplanner/paths/SimplePath.path
@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.061722123444247,
+        "y": 5.038789506150441
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.204869552596248,
+        "y": 4.652291447440038
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.292790014151457,
+        "y": 4.494829275372837
+      },
+      "prevControl": {
+        "x": 4.5197938967306515,
+        "y": 4.4805145324576365
+      },
+      "nextControl": {
+        "x": 5.979928957731629,
+        "y": 4.507554070624321
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.151674589063464,
+        "y": 5.2964548786240435
+      },
+      "prevControl": {
+        "x": 6.065786131572263,
+        "y": 4.80975361950724
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 90.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3,
+    "maxAcceleration": 3,
+    "maxAngularVelocity": 540,
+    "maxAngularAcceleration": 720,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/cpp/SwerveWithPathPlanner/src/main/deploy/pathplanner/settings.json b/cpp/SwerveWithPathPlanner/src/main/deploy/pathplanner/settings.json
new file mode 100644
index 00000000..4281425f
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/src/main/deploy/pathplanner/settings.json
@@ -0,0 +1,21 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "robotMass": 75.0,
+  "robotMOI": 6.883,
+  "robotWheelbase": 0.5588,
+  "robotTrackwidth": 0.5588,
+  "driveWheelRadius": 0.05504,
+  "driveGearing": 7.363636,
+  "maxDriveSpeed": 4.7,
+  "driveMotorType": "krakenX60FOC",
+  "driveCurrentLimit": 120.0,
+  "wheelCOF": 1.7
+}
\ No newline at end of file
diff --git a/cpp/SwerveWithPathPlanner/src/main/include/Robot.h b/cpp/SwerveWithPathPlanner/src/main/include/Robot.h
new file mode 100644
index 00000000..a2fd3ba1
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/src/main/include/Robot.h
@@ -0,0 +1,35 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <optional>
+
+#include <frc/TimedRobot.h>
+#include <frc2/command/CommandPtr.h>
+
+#include "RobotContainer.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  Robot();
+  void RobotPeriodic() override;
+  void DisabledInit() override;
+  void DisabledPeriodic() override;
+  void DisabledExit() override;
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+  void AutonomousExit() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void TeleopExit() override;
+  void TestInit() override;
+  void TestPeriodic() override;
+  void TestExit() override;
+
+ private:
+  frc2::Command *m_autonomousCommand;
+
+  RobotContainer m_container;
+};
diff --git a/cpp/SwerveWithPathPlanner/src/main/include/RobotContainer.h b/cpp/SwerveWithPathPlanner/src/main/include/RobotContainer.h
new file mode 100644
index 00000000..aeacf1ce
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/src/main/include/RobotContainer.h
@@ -0,0 +1,47 @@
+// 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.
+
+#pragma once
+
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/CommandPtr.h>
+#include <frc2/command/button/CommandXboxController.h>
+#include "generated/TunerConstants.h"
+#include "Telemetry.h"
+
+class RobotContainer {
+private:
+    units::meters_per_second_t MaxSpeed = TunerConstants::kSpeedAt12Volts; // kSpeedAt12Volts desired top speed
+    units::radians_per_second_t MaxAngularRate = 0.75_tps; // 3/4 of a rotation per second max angular velocity
+
+    frc2::CommandXboxController joystick{0};
+
+    /* Note: This must be constructed before the drivetrain, otherwise we need to
+     *       define a destructor to un-register the telemetry from the drivetrain */
+    Telemetry logger{MaxSpeed};
+
+public:
+    subsystems::CommandSwerveDrivetrain drivetrain{TunerConstants::CreateDrivetrain()};
+
+private:
+    /* Setting up bindings for necessary control of the swerve drive platform */
+    swerve::requests::FieldCentric drive = swerve::requests::FieldCentric{}
+        .WithDeadband(MaxSpeed * 0.1).WithRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
+        .WithDriveRequestType(swerve::SwerveModule::DriveRequestType::OpenLoopVoltage); // Use open-loop control for drive motors
+    swerve::requests::SwerveDriveBrake brake{};
+    swerve::requests::PointWheelsAt point{};
+    swerve::requests::RobotCentric forwardStraight = swerve::requests::RobotCentric{}
+        .WithDriveRequestType(swerve::SwerveModule::DriveRequestType::OpenLoopVoltage);
+
+    /* Path follower */
+    frc::SendableChooser<frc2::Command *> autoChooser;
+
+public:
+    RobotContainer();
+
+    frc2::Command *GetAutonomousCommand();
+
+private:
+    void ConfigureBindings();
+};
diff --git a/cpp/SwerveWithPathPlanner/src/main/include/Telemetry.h b/cpp/SwerveWithPathPlanner/src/main/include/Telemetry.h
new file mode 100644
index 00000000..f6a9f808
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/src/main/include/Telemetry.h
@@ -0,0 +1,75 @@
+#pragma once
+
+#include "ctre/phoenix6/SignalLogger.hpp"
+#include <frc/smartdashboard/Mechanism2d.h>
+#include <frc/smartdashboard/MechanismLigament2d.h>
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/DoubleArrayTopic.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/StringTopic.h>
+
+#include "subsystems/CommandSwerveDrivetrain.h"
+
+class Telemetry {
+private:
+    units::meters_per_second_t MaxSpeed;
+
+    /* What to publish over networktables for telemetry */
+    nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();
+
+    /* Robot pose for field positioning */
+    std::shared_ptr<nt::NetworkTable> table = inst.GetTable("Pose");
+    nt::DoubleArrayPublisher fieldPub = table->GetDoubleArrayTopic("robotPose").Publish();
+    nt::StringPublisher fieldTypePub = table->GetStringTopic(".type").Publish();
+
+    /* Robot speeds for general checking */
+    std::shared_ptr<nt::NetworkTable> driveStats = inst.GetTable("Drive");
+    nt::DoublePublisher velocityX = driveStats->GetDoubleTopic("Velocity X").Publish();
+    nt::DoublePublisher velocityY = driveStats->GetDoubleTopic("Velocity Y").Publish();
+    nt::DoublePublisher speed = driveStats->GetDoubleTopic("Speed").Publish();
+    nt::DoublePublisher odomFreq = driveStats->GetDoubleTopic("Odometry Frequency").Publish();
+
+    /* Keep a reference of the last pose to calculate the speeds */
+    frc::Pose2d m_lastPose{};
+    units::second_t lastTime = ctre::phoenix6::utils::GetCurrentTime();
+
+    /* Mechanisms to represent the swerve module states */
+    std::array<frc::Mechanism2d, 4> m_moduleMechanisms{
+        frc::Mechanism2d{1, 1},
+        frc::Mechanism2d{1, 1},
+        frc::Mechanism2d{1, 1},
+        frc::Mechanism2d{1, 1},
+    };
+    /* A direction and length changing ligament for speed representation */
+    std::array<frc::MechanismLigament2d *, 4> m_moduleSpeeds{
+        m_moduleMechanisms[0].GetRoot("RootSpeed", 0.5, 0.5)->Append<frc::MechanismLigament2d>("Speed", 0.5, 0_deg),
+        m_moduleMechanisms[1].GetRoot("RootSpeed", 0.5, 0.5)->Append<frc::MechanismLigament2d>("Speed", 0.5, 0_deg),
+        m_moduleMechanisms[2].GetRoot("RootSpeed", 0.5, 0.5)->Append<frc::MechanismLigament2d>("Speed", 0.5, 0_deg),
+        m_moduleMechanisms[3].GetRoot("RootSpeed", 0.5, 0.5)->Append<frc::MechanismLigament2d>("Speed", 0.5, 0_deg),
+    };
+    /* A direction changing and length constant ligament for module direction */
+    std::array<frc::MechanismLigament2d *, 4> m_moduleDirections{
+        m_moduleMechanisms[0].GetRoot("RootDirection", 0.5, 0.5)
+            ->Append<frc::MechanismLigament2d>("Direction", 0.1, 0_deg, 0, frc::Color8Bit{frc::Color::kWhite}),
+        m_moduleMechanisms[1].GetRoot("RootDirection", 0.5, 0.5)
+            ->Append<frc::MechanismLigament2d>("Direction", 0.1, 0_deg, 0, frc::Color8Bit{frc::Color::kWhite}),
+        m_moduleMechanisms[2].GetRoot("RootDirection", 0.5, 0.5)
+            ->Append<frc::MechanismLigament2d>("Direction", 0.1, 0_deg, 0, frc::Color8Bit{frc::Color::kWhite}),
+        m_moduleMechanisms[3].GetRoot("RootDirection", 0.5, 0.5)
+            ->Append<frc::MechanismLigament2d>("Direction", 0.1, 0_deg, 0, frc::Color8Bit{frc::Color::kWhite}),
+    };
+
+public:
+    /**
+     * Construct a telemetry object with the specified max speed of the robot.
+     *
+     * \param maxSpeed Maximum speed
+     */
+    Telemetry(units::meters_per_second_t maxSpeed) : MaxSpeed{maxSpeed}
+    {
+        ctre::phoenix6::SignalLogger::Start();
+    }
+
+    /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */
+    void Telemeterize(subsystems::CommandSwerveDrivetrain::SwerveDriveState const &state);
+};
diff --git a/cpp/SwerveWithPathPlanner/src/main/include/generated/TunerConstants.h b/cpp/SwerveWithPathPlanner/src/main/include/generated/TunerConstants.h
new file mode 100644
index 00000000..26d32b82
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/src/main/include/generated/TunerConstants.h
@@ -0,0 +1,168 @@
+#pragma once
+
+#include "subsystems/CommandSwerveDrivetrain.h"
+
+using namespace ctre::phoenix6;
+
+// Generated by the Tuner X Swerve Project Generator
+// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
+class TunerConstants {
+    // Both sets of gains need to be tuned to your individual robot.
+
+    // The steer motor uses any SwerveModule.SteerRequestType control request with the
+    // output type specified by SwerveModuleConstants::SteerMotorClosedLoopOutput
+    static constexpr configs::Slot0Configs steerGains = configs::Slot0Configs{}
+        .WithKP(100).WithKI(0).WithKD(2.0)
+        .WithKS(0.2).WithKV(1.5).WithKA(0);
+    // When using closed-loop control, the drive motor uses the control
+    // output type specified by SwerveModuleConstants::DriveMotorClosedLoopOutput
+    static constexpr configs::Slot0Configs driveGains = configs::Slot0Configs{}
+        .WithKP(0.1).WithKI(0).WithKD(0)
+        .WithKS(0).WithKV(0.12);
+
+    // The closed-loop output type to use for the steer motors;
+    // This affects the PID/FF gains for the steer motors
+    static constexpr swerve::ClosedLoopOutputType kSteerClosedLoopOutput = swerve::ClosedLoopOutputType::Voltage;
+    // The closed-loop output type to use for the drive motors;
+    // This affects the PID/FF gains for the drive motors
+    static constexpr swerve::ClosedLoopOutputType kDriveClosedLoopOutput = swerve::ClosedLoopOutputType::Voltage;
+
+    // The remote sensor feedback type to use for the steer motors;
+    // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder
+    static constexpr swerve::SteerFeedbackType kSteerFeedbackType = swerve::SteerFeedbackType::FusedCANcoder;
+
+    // The stator current at which the wheels start to slip;
+    // This needs to be tuned to your individual robot
+    static constexpr units::ampere_t kSlipCurrent = 120_A;
+
+    // Initial configs for the drive and steer motors and the CANcoder; these cannot be null.
+    // Some configs will be overwritten; check the `With*InitialConfigs()` API documentation.
+    static constexpr configs::TalonFXConfiguration driveInitialConfigs{};
+    static constexpr configs::TalonFXConfiguration steerInitialConfigs = configs::TalonFXConfiguration{}
+        .WithCurrentLimits(
+            configs::CurrentLimitsConfigs{}
+                // Swerve azimuth does not require much torque output, so we can set a relatively low
+                // stator current limit to help avoid brownouts without impacting performance.
+                .WithStatorCurrentLimit(60_A)
+                .WithStatorCurrentLimitEnable(true)
+        );
+    static constexpr configs::CANcoderConfiguration cancoderInitialConfigs{};
+    // Configs for the Pigeon 2; leave this nullopt to skip applying Pigeon 2 configs
+    static constexpr std::optional<configs::Pigeon2Configuration> pigeonConfigs = std::nullopt;
+
+public:
+    // Theoretical free speed (m/s) at 12 V applied output;
+    // This needs to be tuned to your individual robot
+    static constexpr units::meters_per_second_t kSpeedAt12Volts = 4.70_mps;
+
+private:
+    // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
+    // This may need to be tuned to your individual robot
+    static constexpr units::scalar_t kCoupleRatio = 3.5;
+
+    static constexpr units::scalar_t kDriveGearRatio = 7.363636364;
+    static constexpr units::scalar_t kSteerGearRatio = 12.8;
+    static constexpr units::inch_t kWheelRadius = 2.167_in;
+
+    static constexpr bool kInvertLeftSide = false;
+    static constexpr bool kInvertRightSide = true;
+
+    static constexpr std::string_view kCANBusName = "rio";
+    static inline const CANBus kCANBus{kCANBusName, "./logs/example.hoot"};
+    static constexpr int kPigeonId = 1;
+
+
+    // These are only used for simulation
+    static constexpr units::kilogram_square_meter_t kSteerInertia = 0.00001_kg_sq_m;
+    static constexpr units::kilogram_square_meter_t kDriveInertia = 0.001_kg_sq_m;
+    // Simulated voltage necessary to overcome friction
+    static constexpr units::volt_t kSteerFrictionVoltage = 0.25_V;
+    static constexpr units::volt_t kDriveFrictionVoltage = 0.25_V;
+
+public:
+    static constexpr swerve::SwerveDrivetrainConstants DrivetrainConstants = swerve::SwerveDrivetrainConstants{}
+            .WithCANBusName(kCANBusName)
+            .WithPigeon2Id(kPigeonId)
+            .WithPigeon2Configs(pigeonConfigs);
+
+private:
+    static constexpr swerve::SwerveModuleConstantsFactory ConstantCreator = swerve::SwerveModuleConstantsFactory{}
+            .WithDriveMotorGearRatio(kDriveGearRatio)
+            .WithSteerMotorGearRatio(kSteerGearRatio)
+            .WithCouplingGearRatio(kCoupleRatio)
+            .WithWheelRadius(kWheelRadius)
+            .WithSteerMotorGains(steerGains)
+            .WithDriveMotorGains(driveGains)
+            .WithSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
+            .WithDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
+            .WithSlipCurrent(kSlipCurrent)
+            .WithSpeedAt12Volts(kSpeedAt12Volts)
+            .WithFeedbackSource(kSteerFeedbackType)
+            .WithDriveMotorInitialConfigs(driveInitialConfigs)
+            .WithSteerMotorInitialConfigs(steerInitialConfigs)
+            .WithCANcoderInitialConfigs(cancoderInitialConfigs)
+            .WithSteerInertia(kSteerInertia)
+            .WithDriveInertia(kDriveInertia)
+            .WithSteerFrictionVoltage(kSteerFrictionVoltage)
+            .WithDriveFrictionVoltage(kDriveFrictionVoltage);
+
+
+    // Front Left
+    static constexpr int kFrontLeftDriveMotorId = 5;
+    static constexpr int kFrontLeftSteerMotorId = 4;
+    static constexpr int kFrontLeftEncoderId = 2;
+    static constexpr units::turn_t kFrontLeftEncoderOffset = -0.83544921875_tr;
+    static constexpr bool kFrontLeftSteerMotorInverted = true;
+
+    static constexpr units::inch_t kFrontLeftXPos = 10.5_in;
+    static constexpr units::inch_t kFrontLeftYPos = 10.5_in;
+
+    // Front Right
+    static constexpr int kFrontRightDriveMotorId = 7;
+    static constexpr int kFrontRightSteerMotorId = 6;
+    static constexpr int kFrontRightEncoderId = 3;
+    static constexpr units::turn_t kFrontRightEncoderOffset = -0.15234375_tr;
+    static constexpr bool kFrontRightSteerMotorInverted = true;
+
+    static constexpr units::inch_t kFrontRightXPos = 10.5_in;
+    static constexpr units::inch_t kFrontRightYPos = -10.5_in;
+
+    // Back Left
+    static constexpr int kBackLeftDriveMotorId = 1;
+    static constexpr int kBackLeftSteerMotorId = 0;
+    static constexpr int kBackLeftEncoderId = 0;
+    static constexpr units::turn_t kBackLeftEncoderOffset = -0.4794921875_tr;
+    static constexpr bool kBackLeftSteerMotorInverted = true;
+
+    static constexpr units::inch_t kBackLeftXPos = -10.5_in;
+    static constexpr units::inch_t kBackLeftYPos = 10.5_in;
+
+    // Back Right
+    static constexpr int kBackRightDriveMotorId = 3;
+    static constexpr int kBackRightSteerMotorId = 2;
+    static constexpr int kBackRightEncoderId = 1;
+    static constexpr units::turn_t kBackRightEncoderOffset = -0.84130859375_tr;
+    static constexpr bool kBackRightSteerMotorInverted = true;
+
+    static constexpr units::inch_t kBackRightXPos = -10.5_in;
+    static constexpr units::inch_t kBackRightYPos = -10.5_in;
+
+public:
+    static constexpr swerve::SwerveModuleConstants FrontLeft = ConstantCreator.CreateModuleConstants(
+            kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted);
+    static constexpr swerve::SwerveModuleConstants FrontRight = ConstantCreator.CreateModuleConstants(
+            kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted);
+    static constexpr swerve::SwerveModuleConstants BackLeft = ConstantCreator.CreateModuleConstants(
+            kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted);
+    static constexpr swerve::SwerveModuleConstants BackRight = ConstantCreator.CreateModuleConstants(
+            kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted);
+
+    /**
+     * Creates a CommandSwerveDrivetrain instance.
+     * This should only be called once in your robot program.
+     */
+    static subsystems::CommandSwerveDrivetrain CreateDrivetrain()
+    {
+        return {DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight};
+    }
+};
diff --git a/cpp/SwerveWithPathPlanner/src/main/include/subsystems/CommandSwerveDrivetrain.h b/cpp/SwerveWithPathPlanner/src/main/include/subsystems/CommandSwerveDrivetrain.h
new file mode 100644
index 00000000..aa45f19b
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/src/main/include/subsystems/CommandSwerveDrivetrain.h
@@ -0,0 +1,233 @@
+#pragma once
+
+#include "ctre/phoenix6/swerve/SwerveDrivetrain.hpp"
+#include "ctre/phoenix6/SignalLogger.hpp"
+
+#include <frc/DriverStation.h>
+#include <frc/Notifier.h>
+#include <frc2/command/CommandPtr.h>
+#include <frc2/command/SubsystemBase.h>
+#include <frc2/command/sysid/SysIdRoutine.h>
+
+using namespace ctre::phoenix6;
+
+namespace subsystems {
+
+/**
+ * \brief Class that extends the Phoenix 6 SwerveDrivetrain class and implements
+ * Subsystem so it can easily be used in command-based projects.
+ */
+class CommandSwerveDrivetrain : public frc2::SubsystemBase, public swerve::SwerveDrivetrain {
+    static constexpr units::second_t kSimLoopPeriod = 5_ms;
+    std::unique_ptr<frc::Notifier> m_simNotifier;
+    units::second_t m_lastSimTime;
+
+    /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
+    static constexpr frc::Rotation2d kBlueAlliancePerspectiveRotation{0_deg};
+    /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
+    static constexpr frc::Rotation2d kRedAlliancePerspectiveRotation{180_deg};
+    /* Keep track if we've ever applied the operator perspective before or not */
+    bool m_hasAppliedOperatorPerspective = false;
+
+    /* Swerve request to apply during path following */
+    swerve::requests::ApplyChassisSpeeds m_applyRobotSpeeds;
+
+    /* Swerve requests to apply during SysId characterization */
+    swerve::requests::SysIdSwerveTranslation m_translationCharacterization;
+    swerve::requests::SysIdSwerveSteerGains m_steerCharacterization;
+    swerve::requests::SysIdSwerveRotation m_rotationCharacterization;
+
+    /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
+    frc2::sysid::SysIdRoutine m_sysIdRoutineTranslation{
+        frc2::sysid::Config{
+            std::nullopt, // Use default ramp rate (1 V/s)
+            4_V,          // Reduce dynamic step voltage to 4 V to prevent brownout
+            std::nullopt, // Use default timeout (10 s)
+            // Log state with SignalLogger class
+            [](frc::sysid::State state)
+            {
+                SignalLogger::WriteString("SysIdTranslation_State", frc::sysid::SysIdRoutineLog::StateEnumToString(state));
+            }
+        },
+        frc2::sysid::Mechanism{
+            [this](units::volt_t output) { SetControl(m_translationCharacterization.WithVolts(output)); },
+            {},
+            this
+        }
+    };
+
+    /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */
+    frc2::sysid::SysIdRoutine m_sysIdRoutineSteer{
+        frc2::sysid::Config{
+            std::nullopt, // Use default ramp rate (1 V/s)
+            7_V,          // Use dynamic voltage of 7 V
+            std::nullopt, // Use default timeout (10 s)
+            // Log state with SignalLogger class
+            [](frc::sysid::State state)
+            {
+                SignalLogger::WriteString("SysIdSteer_State", frc::sysid::SysIdRoutineLog::StateEnumToString(state));
+            }
+        },
+        frc2::sysid::Mechanism{
+            [this](units::volt_t output) { SetControl(m_steerCharacterization.WithVolts(output)); },
+            {},
+            this
+        }
+    };
+
+    /*
+     * SysId routine for characterizing rotation.
+     * This is used to find PID gains for the FieldCentricFacingAngle HeadingController.
+     * See the documentation of swerve::requests::SysIdSwerveRotation for info on importing the log to SysId.
+     */
+    frc2::sysid::SysIdRoutine m_sysIdRoutineRotation{
+        frc2::sysid::Config{
+            /* This is in radians per second², but SysId only supports "volts per second" */
+            units::constants::detail::PI_VAL / 6 * (1_V / 1_s),
+            /* This is in radians per second, but SysId only supports "volts" */
+            units::constants::detail::PI_VAL * 1_V,
+            std::nullopt, // Use default timeout (10 s)
+            // Log state with SignalLogger class
+            [](frc::sysid::State state)
+            {
+                SignalLogger::WriteString("SysIdRotation_State", frc::sysid::SysIdRoutineLog::StateEnumToString(state));
+            }
+        },
+        frc2::sysid::Mechanism{
+            [this](units::volt_t output)
+            {
+                /* output is actually radians per second, but SysId only supports "volts" */
+                SetControl(m_rotationCharacterization.WithRotationalRate(output * (1_rad_per_s / 1_V)));
+                /* also log the requested output for SysId */
+                SignalLogger::WriteValue("Rotational_Rate", output * (1_rad_per_s / 1_V));
+            },
+            {},
+            this
+        }
+    };
+
+    /* The SysId routine to test */
+    frc2::sysid::SysIdRoutine *m_sysIdRoutineToApply = &m_sysIdRoutineTranslation;
+
+public:
+    /**
+     * \brief Constructs a CTRE SwerveDrivetrain using the specified constants.
+     *
+     * This constructs the underlying hardware devices, so user should not construct
+     * the devices themselves. If they need the devices, they can access them
+     * through getters in the classes.
+     *
+     * \param drivetrainConstants Drivetrain-wide constants for the swerve drive
+     * \param modules             Constants for each specific module
+     */
+    template <std::same_as<swerve::SwerveModuleConstants>... ModuleConstants>
+    CommandSwerveDrivetrain(swerve::SwerveDrivetrainConstants const &driveTrainConstants, ModuleConstants const &... modules) :
+        SwerveDrivetrain{driveTrainConstants, modules...}
+    {
+        if (utils::IsSimulation()) {
+            StartSimThread();
+        }
+        ConfigureAutoBuilder();
+    }
+
+    /**
+     * \brief Constructs a CTRE SwerveDrivetrain using the specified constants.
+     *
+     * This constructs the underlying hardware devices, so user should not construct
+     * the devices themselves. If they need the devices, they can access them
+     * through getters in the classes.
+     *
+     * \param driveTrainConstants        Drivetrain-wide constants for the swerve drive
+     * \param odometryUpdateFrequency    The frequency to run the odometry loop. If
+     *                                   unspecified or set to 0 Hz, this is 250 Hz on
+     *                                   CAN FD, and 100 Hz on CAN 2.0.
+     * \param modules                    Constants for each specific module
+     */
+    template <std::same_as<swerve::SwerveModuleConstants>... ModuleConstants>
+    CommandSwerveDrivetrain(swerve::SwerveDrivetrainConstants const &driveTrainConstants, units::hertz_t odometryUpdateFrequency,
+                    ModuleConstants const &... modules) :
+        SwerveDrivetrain{driveTrainConstants, odometryUpdateFrequency, modules...}
+    {
+        if (utils::IsSimulation()) {
+            StartSimThread();
+        }
+        ConfigureAutoBuilder();
+    }
+
+    /**
+     * \brief Constructs a CTRE SwerveDrivetrain using the specified constants.
+     *
+     * This constructs the underlying hardware devices, so user should not construct
+     * the devices themselves. If they need the devices, they can access them
+     * through getters in the classes.
+     *
+     * \param driveTrainConstants        Drivetrain-wide constants for the swerve drive
+     * \param odometryUpdateFrequency    The frequency to run the odometry loop. If
+     *                                   unspecified or set to 0 Hz, this is 250 Hz on
+     *                                   CAN FD, and 100 Hz on CAN 2.0.
+     * \param odometryStandardDeviation  The standard deviation for odometry calculation
+     * \param visionStandardDeviation    The standard deviation for vision calculation
+     * \param modules                    Constants for each specific module
+     */
+    template <std::same_as<swerve::SwerveModuleConstants>... ModuleConstants>
+    CommandSwerveDrivetrain(swerve::SwerveDrivetrainConstants const &driveTrainConstants, units::hertz_t odometryUpdateFrequency,
+                    std::array<double, 3> const &odometryStandardDeviation, std::array<double, 3> const &visionStandardDeviation,
+                    ModuleConstants const &... modules) :
+        SwerveDrivetrain{driveTrainConstants, odometryUpdateFrequency,
+            odometryStandardDeviation, visionStandardDeviation, modules...}
+    {
+        if (utils::IsSimulation()) {
+            StartSimThread();
+        }
+        ConfigureAutoBuilder();
+    }
+
+    /**
+     * \brief Returns a command that applies the specified control request to this swerve drivetrain.
+     *
+     * \param request Function returning the request to apply
+     * \returns Command to run
+     */
+    template <typename RequestSupplier>
+        requires std::derived_from<
+            std::invoke_result_t<RequestSupplier>,
+            swerve::requests::SwerveRequest>
+    frc2::CommandPtr ApplyRequest(RequestSupplier request)
+    {
+        return Run([this, request=std::move(request)] {
+            return SetControl(request());
+        });
+    }
+
+    void Periodic() override;
+
+    /**
+     * \brief Runs the SysId Quasistatic test in the given direction for the routine
+     * specified by m_sysIdRoutineToApply.
+     *
+     * \param direction Direction of the SysId Quasistatic test
+     * \returns Command to run
+     */
+    frc2::CommandPtr SysIdQuasistatic(frc2::sysid::Direction direction)
+    {
+        return m_sysIdRoutineToApply->Quasistatic(direction);
+    }
+
+    /**
+     * \brief Runs the SysId Dynamic test in the given direction for the routine
+     * specified by m_sysIdRoutineToApply.
+     *
+     * \param direction Direction of the SysId Dynamic test
+     * \returns Command to run
+     */
+    frc2::CommandPtr SysIdDynamic(frc2::sysid::Direction direction)
+    {
+        return m_sysIdRoutineToApply->Dynamic(direction);
+    }
+
+private:
+    void ConfigureAutoBuilder();
+    void StartSimThread();
+};
+
+}
diff --git a/cpp/SwerveWithPathPlanner/src/test/cpp/main.cpp b/cpp/SwerveWithPathPlanner/src/test/cpp/main.cpp
new file mode 100644
index 00000000..b8b23d23
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/src/test/cpp/main.cpp
@@ -0,0 +1,10 @@
+#include <hal/HAL.h>
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}
diff --git a/cpp/SwerveWithPathPlanner/tuner-project.json b/cpp/SwerveWithPathPlanner/tuner-project.json
new file mode 100644
index 00000000..2e525562
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/tuner-project.json
@@ -0,0 +1 @@
+{"Version":"1.0.0.0","LastState":11,"Modules":[{"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"IsEncoderOffsetConfigured":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"ModuleName":"Front Left","ModuleId":0,"Encoder":null,"SteerMotor":null,"DriveMotor":null,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false},{"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"IsEncoderOffsetConfigured":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"ModuleName":"Front Right","ModuleId":1,"Encoder":null,"SteerMotor":null,"DriveMotor":null,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false},{"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"IsEncoderOffsetConfigured":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"ModuleName":"Back Left","ModuleId":2,"Encoder":null,"SteerMotor":null,"DriveMotor":null,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false},{"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"IsEncoderOffsetConfigured":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"ModuleName":"Back Right","ModuleId":3,"Encoder":null,"SteerMotor":null,"DriveMotor":null,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false}],"SwerveOptions":{"IsValidConfiguration":false,"Gyro":{"Id":1,"Name":"Pigeon 2 (Device ID 1)","Model":"Pigeon 2","CANbus":"","CANbusFriendly":""},"IsValidGyroCANbus":false,"VerticalTrackSizeInches":0.0,"HorizontalTrackSizeInches":0.0,"WheelRadiusInches":0.0,"IsLeftSideInverted":false,"IsRightSideInverted":true,"SwerveModuleType":5,"SwerveModuleConfiguration":{"_CouplingRatio":0.0,"_CustomName":null,"SteerRatio":15.42857142857143,"Flipped":0,"DriveRatio":0.0,"CouplingRatio":0.0,"CustomName":null},"HasVerifiedSteer":false,"HasVerifiedDrive":false}}
\ No newline at end of file
diff --git a/cpp/SwerveWithPathPlanner/vendordeps/PathplannerLib-beta.json b/cpp/SwerveWithPathPlanner/vendordeps/PathplannerLib-beta.json
new file mode 100644
index 00000000..8a53e682
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/vendordeps/PathplannerLib-beta.json
@@ -0,0 +1,38 @@
+{
+    "fileName": "PathplannerLib-beta.json",
+    "name": "PathplannerLib",
+    "version": "2025.0.0-beta-3.1",
+    "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
+    "frcYear": "2025",
+    "mavenUrls": [
+        "https://3015rangerrobotics.github.io/pathplannerlib/repo"
+    ],
+    "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib-beta.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-java",
+            "version": "2025.0.0-beta-3.1"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-cpp",
+            "version": "2025.0.0-beta-3.1",
+            "libName": "PathplannerLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "osxuniversal",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64"
+            ]
+        }
+    ]
+}
\ No newline at end of file
diff --git a/cpp/SwerveWithPathPlanner/vendordeps/Phoenix6.json b/cpp/SwerveWithPathPlanner/vendordeps/Phoenix6.json
new file mode 100644
index 00000000..41c701fd
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/vendordeps/Phoenix6.json
@@ -0,0 +1,337 @@
+{
+    "fileName": "Phoenix6.json",
+    "name": "CTRE-Phoenix (v6)",
+    "version": "25.0.0-beta-1",
+    "frcYear": 2025,
+    "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
+    "mavenUrls": [
+        "https://maven.ctr-electronics.com/release/"
+    ],
+    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json",
+    "conflictsWith": [
+        {
+            "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a",
+            "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.",
+            "offlineFileName": "Phoenix6And5.json"
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "wpiapi-java",
+            "version": "25.0.0-beta-1"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "api-cpp",
+            "version": "25.0.0-beta-1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "tools",
+            "version": "25.0.0-beta-1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "api-cpp-sim",
+            "version": "25.0.0-beta-1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "tools-sim",
+            "version": "25.0.0-beta-1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simTalonSRX",
+            "version": "25.0.0-beta-1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simVictorSPX",
+            "version": "25.0.0-beta-1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simPigeonIMU",
+            "version": "25.0.0-beta-1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simCANCoder",
+            "version": "25.0.0-beta-1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFX",
+            "version": "25.0.0-beta-1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANcoder",
+            "version": "25.0.0-beta-1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProPigeon2",
+            "version": "25.0.0-beta-1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "wpiapi-cpp",
+            "version": "25.0.0-beta-1",
+            "libName": "CTRE_Phoenix6_WPI",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "tools",
+            "version": "25.0.0-beta-1",
+            "libName": "CTRE_PhoenixTools",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "wpiapi-cpp-sim",
+            "version": "25.0.0-beta-1",
+            "libName": "CTRE_Phoenix6_WPISim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "tools-sim",
+            "version": "25.0.0-beta-1",
+            "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": "25.0.0-beta-1",
+            "libName": "CTRE_SimTalonSRX",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simVictorSPX",
+            "version": "25.0.0-beta-1",
+            "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": "25.0.0-beta-1",
+            "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": "25.0.0-beta-1",
+            "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": "25.0.0-beta-1",
+            "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": "25.0.0-beta-1",
+            "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": "25.0.0-beta-1",
+            "libName": "CTRE_SimProPigeon2",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ]
+}
\ No newline at end of file
diff --git a/cpp/SwerveWithPathPlanner/vendordeps/WPILibNewCommands.json b/cpp/SwerveWithPathPlanner/vendordeps/WPILibNewCommands.json
new file mode 100644
index 00000000..3718e0ac
--- /dev/null
+++ b/cpp/SwerveWithPathPlanner/vendordeps/WPILibNewCommands.json
@@ -0,0 +1,38 @@
+{
+  "fileName": "WPILibNewCommands.json",
+  "name": "WPILib-New-Commands",
+  "version": "1.0.0",
+  "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
+  "frcYear": "2025",
+  "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"
+      ]
+    }
+  ]
+}
diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/RobotContainer.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/RobotContainer.java
index d1606a08..34b7c00e 100644
--- a/java/SwerveWithPathPlanner/src/main/java/frc/robot/RobotContainer.java
+++ b/java/SwerveWithPathPlanner/src/main/java/frc/robot/RobotContainer.java
@@ -9,9 +9,11 @@
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
-import com.pathplanner.lib.commands.PathPlannerAuto;
+import com.pathplanner.lib.auto.AutoBuilder;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
@@ -39,7 +41,7 @@ public class RobotContainer {
             .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
 
     /* Path follower */
-    private Command runAuto = new PathPlannerAuto("Tests");
+    private final SendableChooser<Command> autoChooser;
 
     private void configureBindings() {
         // Note that X is defined as forward according to WPILib convention,
@@ -80,10 +82,13 @@ private void configureBindings() {
 
     public RobotContainer() {
         configureBindings();
+
+        autoChooser = AutoBuilder.buildAutoChooser("Tests");
+        SmartDashboard.putData("Auto Mode", autoChooser);
     }
 
     public Command getAutonomousCommand() {
         /* First put the drivetrain into auto run mode, then run the auto */
-        return runAuto;
+        return autoChooser.getSelected();
     }
 }
diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
index 140ed53f..1d9471b3 100644
--- a/java/SwerveWithPathPlanner/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
+++ b/java/SwerveWithPathPlanner/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
@@ -180,24 +180,24 @@ public CommandSwerveDrivetrain(
     }
 
     private void configureAutoBuilder() {
-        double driveBaseRadius = 0;
-        for (var moduleLocation : m_moduleLocations) {
-            driveBaseRadius = Math.max(driveBaseRadius, moduleLocation.getNorm());
-        }
-
         try {
             var config = RobotConfig.fromGUISettings();
             AutoBuilder.configure(
-                () -> this.getState().Pose,   // Supplier of current robot pose
-                this::seedFieldRelative,      // Consumer for seeding pose against auto
-                () -> this.getState().Speeds, // Supplier of current robot speeds
+                () -> getState().Pose,   // Supplier of current robot pose
+                this::seedFieldRelative, // Consumer for seeding pose against auto
+                () -> getState().Speeds, // Supplier of current robot speeds
                 // Consumer of ChassisSpeeds and feedforwards to drive the robot
-                (speeds, feedforwards) -> this.setControl(
+                (speeds, feedforwards) -> setControl(
                     m_applyRobotSpeeds.withSpeeds(speeds)
                         .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
                         .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
                 ),
-                new PPHolonomicDriveController(new PIDConstants(10, 0, 0), new PIDConstants(10, 0, 0)),
+                new PPHolonomicDriveController(
+                    // PID constants for translation
+                    new PIDConstants(10, 0, 0),
+                    // PID constants for rotation
+                    new PIDConstants(10, 0, 0)
+                ),
                 config,
                 // Assume the path needs to be flipped for Red vs Blue, this is normally the case
                 () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,