.
diff --git a/README.md b/README.md
index 371a25dc..461bfa1d 100644
--- a/README.md
+++ b/README.md
@@ -1,2 +1,73 @@
+[](https://github.com/AZ-First/Az-RBSI/actions/workflows/main.yml)
+
+
+
+
# Az-RBSI
Arizona's Reference Build and Software Implementation for FRC Robots (read: "A-Z-ribsy")
+
+## Notes
+
+Will need to update the drive subsystem once AdvantageKit's 2025-beta is
+publicly available, as it nicely wraps CTRE's swerve library in the usual
+AK formalism (with all the logging!).
+
+https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/docs/docs/example-projects/talonfx-swerve-template.md
+
+
+## Purpose
+
+The purpose of Az-RBSI is to help Arizona FRC teams with:
+* Improving robot reliability / performance during “Autonomous Play”
+* Improving robot build & endurance, gameplay reliability and troubleshooting
+ skills
+* Providing a standardized robot “stack” to allow for quick software setup and
+ troubleshooting, and make it easier for Arizona teams to form effective
+ in-state alliances
+
+
+## Design Philosophy
+
+The Az-RBSI is centered around a "Reference Build" robot that allows for teams
+to communicate quickly and effectivly with each other about gameplay strategy
+and troubleshooting. Additionally, the consolidation around a standard robot
+design allows for easier swapping of spare parts and programming modules.
+
+The Az-RBSI software is an outline of an FRC robot program upon which teams can
+build with their particular mechanisms and designs. It weaves together popular
+and currently maintained FIRST- and community-sponsored software libraries to
+provide a baseline robot functionality that combines robust reliability with
+effective logging for troubleshooting.
+
+
+## Library Dependencies
+
+* [WPILib](https://docs.wpilib.org/en/stable/index.html) -- FIRST basic libraries
+* [AdvantageKit](https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/docs/WHAT-IS-ADVANTAGEKIT.md) -- Logging
+* [CTRE Phoenix6](https://v6.docs.ctr-electronics.com/en/stable/docs/api-reference/mechanisms/swerve/swerve-overview.html) / [YAGSL](https://yagsl.gitbook.io/yagsl) -- Swerve drive library
+* [PathPlanner](https://pathplanner.dev/home.html) / [Choreo](https://sleipnirgroup.github.io/Choreo/) -- Autonomous path planning
+* [PhotonVision](https://docs.photonvision.org/en/latest/) -- Robot vision / tracking
+
+
+## Basic Set Up Information
+
+The Az-RBSI supports two different drive train options, both based on
+AdvantageKit example projects. Each option has a different method of creating
+the necessary input files containing details of your setup (motor ID, gear
+ratios, etc.):
+
+* If you are using an entirely CTRE drivebase (*i.e.*, 8x TalonFX-controlled
+motors, 4x CANcoder, 1x Pigeon 2.0), you will use the CTRE Tuner X
+[Swerve Project Generator](https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html)
+to create the necessary ``TunerConstants.java`` file to replace the example
+in ``src/main/java/frc/robot/generated``.
+
+* For all other drivebases (all-REV, blended CTRE/REV, etc.), you will use the
+[YAGSL configuration tool](https://broncbotz3481.github.io/YAGSL-Example/) to
+create the necessary ``*.json`` files to replace the examples in
+``src/main/deploy/swerve``.
+
+When editing the ``Constants.java`` file, you will need to specify which
+drivetrain type is being used via the ``swerveType`` variable near the top of
+the file (options are ``SwerveType.PHOENIX6`` or ``SwerveType.YAGSL``) so that
+the code reads in the proper configuration file(s).
diff --git a/WPILib-License.md b/WPILib-License.md
new file mode 100644
index 00000000..645e5425
--- /dev/null
+++ b/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/build.gradle b/build.gradle
new file mode 100644
index 00000000..14021685
--- /dev/null
+++ b/build.gradle
@@ -0,0 +1,204 @@
+plugins {
+ id "java"
+ id "edu.wpi.first.GradleRIO" version "2024.3.2"
+ id "com.peterabeles.gversion" version "1.10.3"
+ id "com.diffplug.spotless" version "6.25.0"
+ id "io.freefair.lombok" version "8.11"
+ id "com.google.protobuf" version "0.9.4"
+}
+
+java {
+ sourceCompatibility = JavaVersion.VERSION_17
+ targetCompatibility = JavaVersion.VERSION_17
+}
+
+def ROBOT_MAIN_CLASS = "frc.robot.Main"
+
+// Define my targets (RoboRIO) and artifacts (deployable files)
+// This is added by GradleRIO's backing project DeployUtils.
+deploy {
+ targets {
+ roborio(getTargetTypeClass('RoboRIO')) {
+ // Team number is loaded either from the .wpilib/wpilib_preferences.json
+ // or from command line. If not found an exception will be thrown.
+ // You can use getTeamOrDefault(team) instead of getTeamNumber if you
+ // want to store a team number in this file.
+ team = project.frc.getTeamNumber()
+ debug = project.frc.getDebugOrDefault(false)
+
+ artifacts {
+ // First part is artifact name, 2nd is artifact type
+ // getTargetTypeClass is a shortcut to get the class type using a string
+
+ frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
+ }
+
+ // Static files artifact
+ frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+ files = project.fileTree('src/main/deploy')
+ directory = '/home/lvuser/deploy'
+ }
+ }
+ }
+ }
+}
+
+def deployArtifact = deploy.targets.roborio.artifacts.frcJava
+
+// Set to true to use debug for JNI.
+wpi.java.debugJni = false
+
+// Set this to true to enable desktop support.
+def includeDesktopSupport = true
+
+// Configuration for AdvantageKit
+repositories {
+ maven {
+ url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit")
+ credentials {
+ username = "Mechanical-Advantage-Bot"
+ password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051"
+ }
+ }
+ mavenLocal()
+}
+
+configurations.all {
+ exclude group: "edu.wpi.first.wpilibj"
+}
+
+task(checkAkitInstall, dependsOn: "classes", type: JavaExec) {
+ mainClass = "org.littletonrobotics.junction.CheckInstall"
+ classpath = sourceSets.main.runtimeClasspath
+}
+compileJava.finalizedBy checkAkitInstall
+
+// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+// Also defines JUnit 4.
+dependencies {
+ implementation wpi.java.deps.wpilib()
+ implementation wpi.java.vendor.java()
+
+ roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
+ roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
+
+ roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
+ roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)
+
+ nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
+ nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
+ simulationDebug wpi.sim.enableDebug()
+
+ nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop)
+ nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
+ simulationRelease wpi.sim.enableRelease()
+
+ testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
+ testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
+ def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
+ annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version"
+}
+
+test {
+ useJUnitPlatform()
+ systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
+}
+
+// Simulation configuration (e.g. environment variables).
+//
+// The sim GUI is *disabled* by default to support running
+// AdvantageKit log replay from the command line. Set the
+// value to "true" to enable the sim GUI by default (this
+// is the standard WPILib behavior).
+wpi.sim.addGui().defaultEnabled = false
+wpi.sim.addDriverstation()
+
+// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
+// in order to make them all available at runtime. Also adding the manifest so WPILib
+// knows where to look for our Robot Class.
+jar {
+ from {
+ configurations.runtimeClasspath.collect {
+ it.isDirectory() ? it : zipTree(it)
+ }
+ }
+ from sourceSets.main.allSource
+ manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
+ duplicatesStrategy = DuplicatesStrategy.INCLUDE
+}
+
+// Configure jar and deploy tasks
+deployArtifact.jarTask = jar
+wpi.java.configureExecutableTasks(jar)
+wpi.java.configureTestTasks(test)
+
+// Configure string concat to always inline compile
+tasks.withType(JavaCompile) {
+ options.compilerArgs << '-XDstringConcat=inline' << '-Xlint:unchecked'
+}
+
+// Create version file
+project.compileJava.dependsOn(createVersionFile)
+gversion {
+ srcDir = "src/main/java/"
+ classPackage = "frc.robot"
+ className = "BuildConstants"
+ dateFormat = "yyyy-MM-dd HH:mm:ss z"
+ timeZone = "America/Phoenix"
+ indent = " "
+}
+
+// Spotless formatting
+project.compileJava.dependsOn(spotlessApply)
+spotless {
+ enforceCheck true
+ java {
+ target fileTree('.') {
+ include '**/*.java'
+ exclude '**/build/**', '**/build-*/**'
+ }
+ importOrder()
+ toggleOffOn()
+ googleJavaFormat()
+ removeUnusedImports()
+ trimTrailingWhitespace()
+ endWithNewline()
+ }
+ groovyGradle {
+ target fileTree('.') {
+ include '**/*.gradle'
+ exclude '**/build/**', '**/build-*/**'
+ }
+ greclipse()
+ indentWithSpaces(4)
+ trimTrailingWhitespace()
+ endWithNewline()
+ }
+ json {
+ target fileTree('.'){
+ include '**/*.json'
+ exclude '**/build/**', '**/build-*/**'
+ }
+ gson().indentWithSpaces(2)
+ }
+ format 'xml', {
+ target fileTree('.') {
+ include '**/*.xml'
+ exclude '**/build/**', '**/build-*/**'
+ }
+ eclipseWtp('xml')
+ trimTrailingWhitespace()
+ indentWithSpaces(2)
+ endWithNewline()
+ }
+ format "misc", {
+ target fileTree(".") {
+ include "**/*.md", "**/.gitignore"
+ exclude "**/build/**", "**/build-*/**"
+ }
+ trimTrailingWhitespace()
+ indentWithSpaces(2)
+ endWithNewline()
+ }
+}
diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar
new file mode 100644
index 00000000..d64cd491
Binary files /dev/null and b/gradle/wrapper/gradle-wrapper.jar differ
diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties
new file mode 100644
index 00000000..7015f6be
--- /dev/null
+++ b/gradle/wrapper/gradle-wrapper.properties
@@ -0,0 +1,7 @@
+distributionBase=GRADLE_USER_HOME
+distributionPath=permwrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip
+networkTimeout=10000
+validateDistributionUrl=true
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=permwrapper/dists
diff --git a/gradlew b/gradlew
new file mode 100755
index 00000000..1aa94a42
--- /dev/null
+++ b/gradlew
@@ -0,0 +1,249 @@
+#!/bin/sh
+
+#
+# Copyright © 2015-2021 the original authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# https://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
+##############################################################################
+#
+# Gradle start up script for POSIX generated by Gradle.
+#
+# Important for running:
+#
+# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is
+# noncompliant, but you have some other compliant shell such as ksh or
+# bash, then to run this script, type that shell name before the whole
+# command line, like:
+#
+# ksh Gradle
+#
+# Busybox and similar reduced shells will NOT work, because this script
+# requires all of these POSIX shell features:
+# * functions;
+# * expansions «$var», «${var}», «${var:-default}», «${var+SET}»,
+# «${var#prefix}», «${var%suffix}», and «$( cmd )»;
+# * compound commands having a testable exit status, especially «case»;
+# * various built-in commands including «command», «set», and «ulimit».
+#
+# Important for patching:
+#
+# (2) This script targets any POSIX shell, so it avoids extensions provided
+# by Bash, Ksh, etc; in particular arrays are avoided.
+#
+# The "traditional" practice of packing multiple parameters into a
+# space-separated string is a well documented source of bugs and security
+# problems, so this is (mostly) avoided, by progressively accumulating
+# options in "$@", and eventually passing that to Java.
+#
+# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS,
+# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly;
+# see the in-line comments for details.
+#
+# There are tweaks for specific operating systems such as AIX, CygWin,
+# Darwin, MinGW, and NonStop.
+#
+# (3) This script is generated from the Groovy template
+# https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+# within the Gradle project.
+#
+# You can find Gradle at https://github.com/gradle/gradle/.
+#
+##############################################################################
+
+# Attempt to set APP_HOME
+
+# Resolve links: $0 may be a link
+app_path=$0
+
+# Need this for daisy-chained symlinks.
+while
+ APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path
+ [ -h "$app_path" ]
+do
+ ls=$( ls -ld "$app_path" )
+ link=${ls#*' -> '}
+ case $link in #(
+ /*) app_path=$link ;; #(
+ *) app_path=$APP_HOME$link ;;
+ esac
+done
+
+# 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 "${APP_HOME:-./}" > /dev/null && pwd -P ) || 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/gradlew.bat b/gradlew.bat
new file mode 100644
index 00000000..6689b85b
--- /dev/null
+++ b/gradlew.bat
@@ -0,0 +1,92 @@
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+
+@if "%DEBUG%"=="" @echo off
+@rem ##########################################################################
+@rem
+@rem Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%"=="" set DIRNAME=.
+@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.
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto execute
+
+echo.
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+
+:end
+@rem End local scope for the variables with windows NT shell
+if %ERRORLEVEL% equ 0 goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+set EXIT_CODE=%ERRORLEVEL%
+if %EXIT_CODE% equ 0 set EXIT_CODE=1
+if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
+exit /b %EXIT_CODE%
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega
diff --git a/settings.gradle b/settings.gradle
new file mode 100644
index 00000000..d94f73c6
--- /dev/null
+++ b/settings.gradle
@@ -0,0 +1,30 @@
+import org.gradle.internal.os.OperatingSystem
+
+pluginManagement {
+ repositories {
+ mavenLocal()
+ gradlePluginPortal()
+ String frcYear = '2024'
+ 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/src/main/deploy/README.md b/src/main/deploy/README.md
new file mode 100644
index 00000000..88549dc4
--- /dev/null
+++ b/src/main/deploy/README.md
@@ -0,0 +1,14 @@
+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.
+
+Currently expected directories:
+
+apriltags/
+ HHH
+
+pathplanner/
+ HHH
+
+swerve/
+ HHH
diff --git a/src/main/deploy/apriltags/2024-amps.json b/src/main/deploy/apriltags/2024-amps.json
new file mode 100644
index 00000000..ddaf4be3
--- /dev/null
+++ b/src/main/deploy/apriltags/2024-amps.json
@@ -0,0 +1,44 @@
+{
+ "tags": [
+ {
+ "ID": 5,
+ "pose": {
+ "translation": {
+ "x": 14.700757999999999,
+ "y": 8.2042,
+ "z": 1.355852
+ },
+ "rotation": {
+ "quaternion": {
+ "W": -0.7071067811865475,
+ "X": -0.0,
+ "Y": 0.0,
+ "Z": 0.7071067811865476
+ }
+ }
+ }
+ },
+ {
+ "ID": 6,
+ "pose": {
+ "translation": {
+ "x": 1.8415,
+ "y": 8.2042,
+ "z": 1.355852
+ },
+ "rotation": {
+ "quaternion": {
+ "W": -0.7071067811865475,
+ "X": -0.0,
+ "Y": 0.0,
+ "Z": 0.7071067811865476
+ }
+ }
+ }
+ }
+ ],
+ "field": {
+ "length": 16.541,
+ "width": 8.211
+ }
+}
diff --git a/src/main/deploy/apriltags/2024-official.json b/src/main/deploy/apriltags/2024-official.json
new file mode 100644
index 00000000..8cb837a5
--- /dev/null
+++ b/src/main/deploy/apriltags/2024-official.json
@@ -0,0 +1,296 @@
+{
+ "tags": [
+ {
+ "ID": 1,
+ "pose": {
+ "translation": {
+ "x": 15.079471999999997,
+ "y": 0.24587199999999998,
+ "z": 1.355852
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 0.5000000000000001,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.8660254037844386
+ }
+ }
+ }
+ },
+ {
+ "ID": 2,
+ "pose": {
+ "translation": {
+ "x": 16.185134,
+ "y": 0.883666,
+ "z": 1.355852
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 0.5000000000000001,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.8660254037844386
+ }
+ }
+ }
+ },
+ {
+ "ID": 3,
+ "pose": {
+ "translation": {
+ "x": 16.579342,
+ "y": 4.982717999999999,
+ "z": 1.4511020000000001
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 6.123233995736766e-17,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 1.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 4,
+ "pose": {
+ "translation": {
+ "x": 16.579342,
+ "y": 5.547867999999999,
+ "z": 1.4511020000000001
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 6.123233995736766e-17,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 1.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 5,
+ "pose": {
+ "translation": {
+ "x": 14.700757999999999,
+ "y": 8.2042,
+ "z": 1.355852
+ },
+ "rotation": {
+ "quaternion": {
+ "W": -0.7071067811865475,
+ "X": -0.0,
+ "Y": 0.0,
+ "Z": 0.7071067811865476
+ }
+ }
+ }
+ },
+ {
+ "ID": 6,
+ "pose": {
+ "translation": {
+ "x": 1.8415,
+ "y": 8.2042,
+ "z": 1.355852
+ },
+ "rotation": {
+ "quaternion": {
+ "W": -0.7071067811865475,
+ "X": -0.0,
+ "Y": 0.0,
+ "Z": 0.7071067811865476
+ }
+ }
+ }
+ },
+ {
+ "ID": 7,
+ "pose": {
+ "translation": {
+ "x": -0.038099999999999995,
+ "y": 5.547867999999999,
+ "z": 1.4511020000000001
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 1.0,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 8,
+ "pose": {
+ "translation": {
+ "x": -0.038099999999999995,
+ "y": 4.982717999999999,
+ "z": 1.4511020000000001
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 1.0,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 9,
+ "pose": {
+ "translation": {
+ "x": 0.356108,
+ "y": 0.883666,
+ "z": 1.355852
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 0.8660254037844387,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.49999999999999994
+ }
+ }
+ }
+ },
+ {
+ "ID": 10,
+ "pose": {
+ "translation": {
+ "x": 1.4615159999999998,
+ "y": 0.24587199999999998,
+ "z": 1.355852
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 0.8660254037844387,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.49999999999999994
+ }
+ }
+ }
+ },
+ {
+ "ID": 11,
+ "pose": {
+ "translation": {
+ "x": 11.904726,
+ "y": 3.7132259999999997,
+ "z": 1.3208
+ },
+ "rotation": {
+ "quaternion": {
+ "W": -0.8660254037844387,
+ "X": -0.0,
+ "Y": 0.0,
+ "Z": 0.49999999999999994
+ }
+ }
+ }
+ },
+ {
+ "ID": 12,
+ "pose": {
+ "translation": {
+ "x": 11.904726,
+ "y": 4.49834,
+ "z": 1.3208
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 0.8660254037844387,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.49999999999999994
+ }
+ }
+ }
+ },
+ {
+ "ID": 13,
+ "pose": {
+ "translation": {
+ "x": 11.220196,
+ "y": 4.105148,
+ "z": 1.3208
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 6.123233995736766e-17,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 1.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 14,
+ "pose": {
+ "translation": {
+ "x": 5.320792,
+ "y": 4.105148,
+ "z": 1.3208
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 1.0,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 15,
+ "pose": {
+ "translation": {
+ "x": 4.641342,
+ "y": 4.49834,
+ "z": 1.3208
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 0.5000000000000001,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.8660254037844386
+ }
+ }
+ }
+ },
+ {
+ "ID": 16,
+ "pose": {
+ "translation": {
+ "x": 4.641342,
+ "y": 3.7132259999999997,
+ "z": 1.3208
+ },
+ "rotation": {
+ "quaternion": {
+ "W": -0.4999999999999998,
+ "X": -0.0,
+ "Y": 0.0,
+ "Z": 0.8660254037844387
+ }
+ }
+ }
+ }
+ ],
+ "field": {
+ "length": 16.541,
+ "width": 8.211
+ }
+}
diff --git a/src/main/deploy/apriltags/2024-speakers.json b/src/main/deploy/apriltags/2024-speakers.json
new file mode 100644
index 00000000..36ecc9df
--- /dev/null
+++ b/src/main/deploy/apriltags/2024-speakers.json
@@ -0,0 +1,80 @@
+{
+ "tags": [
+ {
+ "ID": 3,
+ "pose": {
+ "translation": {
+ "x": 16.579342,
+ "y": 4.982717999999999,
+ "z": 1.4511020000000001
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 6.123233995736766e-17,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 1.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 4,
+ "pose": {
+ "translation": {
+ "x": 16.579342,
+ "y": 5.547867999999999,
+ "z": 1.4511020000000001
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 6.123233995736766e-17,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 1.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 7,
+ "pose": {
+ "translation": {
+ "x": -0.038099999999999995,
+ "y": 5.547867999999999,
+ "z": 1.4511020000000001
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 1.0,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 8,
+ "pose": {
+ "translation": {
+ "x": -0.038099999999999995,
+ "y": 4.982717999999999,
+ "z": 1.4511020000000001
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 1.0,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.0
+ }
+ }
+ }
+ }
+ ],
+ "field": {
+ "length": 16.541,
+ "width": 8.211
+ }
+}
diff --git a/src/main/deploy/apriltags/2024-wpi.json b/src/main/deploy/apriltags/2024-wpi.json
new file mode 100644
index 00000000..bbf6862c
--- /dev/null
+++ b/src/main/deploy/apriltags/2024-wpi.json
@@ -0,0 +1,116 @@
+{
+ "tags": [
+ {
+ "ID": 3,
+ "pose": {
+ "translation": {
+ "x": 16.579342,
+ "y": 4.982717999999999,
+ "z": 1.4511020000000001
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 6.123233995736766e-17,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 1.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 4,
+ "pose": {
+ "translation": {
+ "x": 16.579342,
+ "y": 5.547867999999999,
+ "z": 1.4511020000000001
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 6.123233995736766e-17,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 1.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 5,
+ "pose": {
+ "translation": {
+ "x": 14.700757999999999,
+ "y": 8.2042,
+ "z": 1.355852
+ },
+ "rotation": {
+ "quaternion": {
+ "W": -0.7071067811865475,
+ "X": -0.0,
+ "Y": 0.0,
+ "Z": 0.7071067811865476
+ }
+ }
+ }
+ },
+ {
+ "ID": 6,
+ "pose": {
+ "translation": {
+ "x": 1.8415,
+ "y": 8.2042,
+ "z": 1.355852
+ },
+ "rotation": {
+ "quaternion": {
+ "W": -0.7071067811865475,
+ "X": -0.0,
+ "Y": 0.0,
+ "Z": 0.7071067811865476
+ }
+ }
+ }
+ },
+ {
+ "ID": 7,
+ "pose": {
+ "translation": {
+ "x": -0.038099999999999995,
+ "y": 5.547867999999999,
+ "z": 1.4511020000000001
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 1.0,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.0
+ }
+ }
+ }
+ },
+ {
+ "ID": 8,
+ "pose": {
+ "translation": {
+ "x": -0.038099999999999995,
+ "y": 4.982717999999999,
+ "z": 1.4511020000000001
+ },
+ "rotation": {
+ "quaternion": {
+ "W": 1.0,
+ "X": 0.0,
+ "Y": 0.0,
+ "Z": 0.0
+ }
+ }
+ }
+ }
+ ],
+ "field": {
+ "length": 16.541,
+ "width": 8.211
+ }
+}
diff --git a/src/main/deploy/pathplanner/Tests.path b/src/main/deploy/pathplanner/Tests.path
new file mode 100644
index 00000000..9cb675ba
--- /dev/null
+++ b/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/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto
new file mode 100644
index 00000000..77a8433f
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Example Auto.auto
@@ -0,0 +1,31 @@
+{
+ "version": 1.0,
+ "startingPose": {
+ "position": {
+ "x": 2.0,
+ "y": 7.0
+ },
+ "rotation": 180.0
+ },
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Run Flywheel"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Example Path"
+ }
+ }
+ ]
+ }
+ },
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto
new file mode 100644
index 00000000..7fc8be27
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/New Auto.auto
@@ -0,0 +1,25 @@
+{
+ "version": 1.0,
+ "startingPose": {
+ "position": {
+ "x": 2.0,
+ "y": 7.0
+ },
+ "rotation": 0
+ },
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "New Path"
+ }
+ }
+ ]
+ }
+ },
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Tests.auto b/src/main/deploy/pathplanner/autos/Tests.auto
new file mode 100644
index 00000000..feb22dfb
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Tests.auto
@@ -0,0 +1,24 @@
+{
+ "version": 1.0,
+ "startingPose": {
+ "position": {
+ "x": 4.061722123444247,
+ "y": 5.038789506150441
+ },
+ "rotation": 0
+ },
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "SimplePath"
+ }
+ }
+ ]
+ }
+ },
+ "folder": null
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
new file mode 100644
index 00000000..690f5db2
--- /dev/null
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +1,1633 @@
+{
+ "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
+ ]
+ ]
+}
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
new file mode 100644
index 00000000..8f3609bb
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Example Path.path
@@ -0,0 +1,65 @@
+{
+ "version": 1.0,
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 2.0,
+ "y": 7.0
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 3.013282910175676,
+ "y": 6.5274984191074985
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.166769560390973,
+ "y": 5.0964860911203305
+ },
+ "prevControl": {
+ "x": 4.166769560390973,
+ "y": 6.0964860911203305
+ },
+ "nextControl": {
+ "x": 6.166769560390973,
+ "y": 4.0964860911203305
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.0,
+ "y": 1.0
+ },
+ "prevControl": {
+ "x": 6.75,
+ "y": 2.5
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 3.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0,
+ "rotateFast": false
+ },
+ "reversed": false,
+ "folder": null,
+ "previewStartingState": null,
+ "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
new file mode 100644
index 00000000..2012ffc1
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/New Path.path
@@ -0,0 +1,65 @@
+{
+ "version": 1.0,
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 2.0,
+ "y": 7.0
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 3.0,
+ "y": 6.5
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.0,
+ "y": 5.0
+ },
+ "prevControl": {
+ "x": 4.0,
+ "y": 6.0
+ },
+ "nextControl": {
+ "x": 6.0,
+ "y": 4.0
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 7.225280307703473,
+ "y": 6.012192024384049
+ },
+ "prevControl": {
+ "x": 6.975280307703473,
+ "y": 7.512192024384059
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 3.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0,
+ "rotateFast": false
+ },
+ "reversed": false,
+ "folder": null,
+ "previewStartingState": null,
+ "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/SimplePath.path b/src/main/deploy/pathplanner/paths/SimplePath.path
new file mode 100644
index 00000000..d070be0e
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/SimplePath.path
@@ -0,0 +1,64 @@
+{
+ "version": 1.0,
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.061722123444247,
+ "y": 5.038789506150441
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 4.204869552596248,
+ "y": 4.652291447440038
+ },
+ "isLocked": false
+ },
+ {
+ "anchor": {
+ "x": 5.292790014151457,
+ "y": 4.494829275372837
+ },
+ "prevControl": {
+ "x": 4.5197938967306515,
+ "y": 4.4805145324576365
+ },
+ "nextControl": {
+ "x": 5.979928957731629,
+ "y": 4.507554070624321
+ },
+ "isLocked": false
+ },
+ {
+ "anchor": {
+ "x": 6.151674589063464,
+ "y": 5.2964548786240435
+ },
+ "prevControl": {
+ "x": 6.065786131572263,
+ "y": 4.80975361950724
+ },
+ "nextControl": null,
+ "isLocked": false
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 1.0,
+ "rotationDegrees": 90.0
+ }
+ ],
+ "constraintZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3,
+ "maxAcceleration": 3,
+ "maxAngularVelocity": 540,
+ "maxAngularAcceleration": 720
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0
+ },
+ "reversed": false,
+ "folder": null
+}
\ No newline at end of file
diff --git a/src/main/deploy/swerve/controllerproperties.json b/src/main/deploy/swerve/controllerproperties.json
new file mode 100644
index 00000000..c5ab6446
--- /dev/null
+++ b/src/main/deploy/swerve/controllerproperties.json
@@ -0,0 +1,8 @@
+{
+ "angleJoystickRadiusDeadband": 0.5,
+ "heading": {
+ "p": 0.4,
+ "i": 0,
+ "d": 0.01
+ }
+}
diff --git a/src/main/deploy/swerve/example.txt b/src/main/deploy/swerve/example.txt
new file mode 100644
index 00000000..bb82515d
--- /dev/null
+++ b/src/main/deploy/swerve/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/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json
new file mode 100644
index 00000000..08f078a0
--- /dev/null
+++ b/src/main/deploy/swerve/modules/backleft.json
@@ -0,0 +1,27 @@
+{
+ "location": {
+ "front": -10.375,
+ "left": 10.375
+ },
+ "absoluteEncoderOffset": 0,
+ "drive": {
+ "type": "falcon",
+ "id": 7,
+ "canbus": "DriveTrain"
+ },
+ "inverted": {
+ "drive": true,
+ "angle": true
+ },
+ "angle": {
+ "type": "falcon",
+ "id": 8,
+ "canbus": "DriveTrain"
+ },
+ "encoder": {
+ "type": "cancoder",
+ "id": 9,
+ "canbus": "DriveTrain"
+ },
+ "absoluteEncoderInverted": false
+}
diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json
new file mode 100644
index 00000000..d48c3768
--- /dev/null
+++ b/src/main/deploy/swerve/modules/backright.json
@@ -0,0 +1,27 @@
+{
+ "location": {
+ "front": -10.375,
+ "left": -10.375
+ },
+ "absoluteEncoderOffset": 0,
+ "drive": {
+ "type": "falcon",
+ "id": 10,
+ "canbus": "DriveTrain"
+ },
+ "inverted": {
+ "drive": true,
+ "angle": true
+ },
+ "angle": {
+ "type": "falcon",
+ "id": 11,
+ "canbus": "DriveTrain"
+ },
+ "encoder": {
+ "type": "cancoder",
+ "id": 12,
+ "canbus": "DriveTrain"
+ },
+ "absoluteEncoderInverted": false
+}
diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json
new file mode 100644
index 00000000..54f9bcf5
--- /dev/null
+++ b/src/main/deploy/swerve/modules/frontleft.json
@@ -0,0 +1,27 @@
+{
+ "location": {
+ "front": 10.375,
+ "left": 10.375
+ },
+ "absoluteEncoderOffset": 0,
+ "drive": {
+ "type": "falcon",
+ "id": 1,
+ "canbus": "DriveTrain"
+ },
+ "inverted": {
+ "drive": true,
+ "angle": true
+ },
+ "angle": {
+ "type": "falcon",
+ "id": 2,
+ "canbus": "DriveTrain"
+ },
+ "encoder": {
+ "type": "cancoder",
+ "id": 3,
+ "canbus": "DriveTrain"
+ },
+ "absoluteEncoderInverted": false
+}
diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json
new file mode 100644
index 00000000..d1de2ab9
--- /dev/null
+++ b/src/main/deploy/swerve/modules/frontright.json
@@ -0,0 +1,27 @@
+{
+ "location": {
+ "front": 10.375,
+ "left": -10.375
+ },
+ "absoluteEncoderOffset": 0,
+ "drive": {
+ "type": "falcon",
+ "id": 4,
+ "canbus": "DriveTrain"
+ },
+ "inverted": {
+ "drive": true,
+ "angle": true
+ },
+ "angle": {
+ "type": "falcon",
+ "id": 5,
+ "canbus": "DriveTrain"
+ },
+ "encoder": {
+ "type": "cancoder",
+ "id": 6,
+ "canbus": "DriveTrain"
+ },
+ "absoluteEncoderInverted": false
+}
diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json
new file mode 100644
index 00000000..7708865f
--- /dev/null
+++ b/src/main/deploy/swerve/modules/physicalproperties.json
@@ -0,0 +1,23 @@
+{
+ "optimalVoltage": 12,
+ "wheelGripCoefficientOfFriction": 1.19,
+ "currentLimit": {
+ "drive": 120,
+ "angle": 40
+ },
+ "conversionFactors": {
+ "angle": {
+ "gearRatio": 21.42857,
+ "factor": 0
+ },
+ "drive": {
+ "diameter": 4,
+ "gearRatio": 6.74603,
+ "factor": 0
+ }
+ },
+ "rampRate": {
+ "drive": 0.25,
+ "angle": 0.25
+ }
+}
diff --git a/src/main/deploy/swerve/modules/pidfproperties.json b/src/main/deploy/swerve/modules/pidfproperties.json
new file mode 100644
index 00000000..b4650e38
--- /dev/null
+++ b/src/main/deploy/swerve/modules/pidfproperties.json
@@ -0,0 +1,16 @@
+{
+ "drive": {
+ "p": 0.0020645,
+ "i": 0,
+ "d": 0,
+ "f": 0,
+ "iz": 0
+ },
+ "angle": {
+ "p": 0.0020645,
+ "i": 0,
+ "d": 0,
+ "f": 0,
+ "iz": 0
+ }
+}
diff --git a/src/main/deploy/swerve/swervedrive.json b/src/main/deploy/swerve/swervedrive.json
new file mode 100644
index 00000000..26406d90
--- /dev/null
+++ b/src/main/deploy/swerve/swervedrive.json
@@ -0,0 +1,14 @@
+{
+ "imu": {
+ "type": "pigeon2",
+ "id": 13,
+ "canbus": "DriveTrain"
+ },
+ "invertedIMU": false,
+ "modules": [
+ "frontleft.json",
+ "frontright.json",
+ "backleft.json",
+ "backright.json"
+ ]
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
new file mode 100644
index 00000000..b5a72596
--- /dev/null
+++ b/src/main/java/frc/robot/Constants.java
@@ -0,0 +1,344 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import com.pathplanner.lib.util.PIDConstants;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
+import edu.wpi.first.wpilibj.RobotBase;
+import frc.robot.generated.TunerConstants;
+import frc.robot.util.Alert;
+import frc.robot.util.Alert.AlertType;
+import java.io.IOException;
+import java.nio.file.Path;
+import lombok.Getter;
+import swervelib.math.Matter;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+
+ /***************************************************************************/
+ /**
+ * Define the various multiple robots that use this same code (e.g., COMPBOT, DEVBOT, SIMBOT,
+ * etc.) and the operating modes of the code (REAL, SIM, or REPLAY)
+ */
+ private static RobotType robotType = RobotType.COMPBOT;
+
+ // Define swerve, auto, and vision types being used
+ private static SwerveType swerveType = SwerveType.PHOENIX6;
+ private static AutoType autoType = AutoType.PATHPLANNER;
+ private static VisionType visionType = VisionType.NONE;
+
+ public static boolean disableHAL = false;
+
+ /** Enumerate the robot types (add additional bots here) */
+ public static enum RobotType {
+ DEVBOT, // Development / Alpha / Practice Bot
+ COMPBOT, // Competition robot
+ SIMBOT // Simulated robot
+ }
+
+ /** Enumerate the robot operation modes */
+ public static enum Mode {
+ REAL, // REAL == Running on a real robot
+ REPLAY, // REPLAY == Replaying from a log file
+ SIM // SIM == Running a physics simulator
+ }
+
+ /** Get the current robot */
+ public static RobotType getRobot() {
+ if (!disableHAL && RobotBase.isReal() && robotType == RobotType.SIMBOT) {
+ new Alert("Invalid robot selected, using competition robot as default.", AlertType.ERROR)
+ .set(true);
+ robotType = RobotType.COMPBOT;
+ }
+ return robotType;
+ }
+
+ /** Get the current mode */
+ public static Mode getMode() {
+ return switch (robotType) {
+ case DEVBOT, COMPBOT -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY;
+ case SIMBOT -> Mode.SIM;
+ };
+ }
+
+ /** Disable the Hardware Abstraction Layer, if requested */
+ public static void disableHAL() {
+ disableHAL = true;
+ }
+
+ /** Checks whether the correct robot is selected when deploying. */
+ public static void main(String... args) {
+ if (robotType == RobotType.SIMBOT) {
+ System.err.println("Cannot deploy, invalid robot selected: " + robotType);
+ System.exit(1);
+ }
+ }
+
+ /** Enumerate the supported swerve drive types */
+ public static enum SwerveType {
+ PHOENIX6, // The all-CTRE Phoenix6 swerve generator
+ YAGSL // The generic YAGSL swerve generator
+ }
+
+ /** Get the current swerve drive type */
+ public static SwerveType getSwerveType() {
+ return swerveType;
+ }
+
+ /** Enumerate the supported autonomous path planning types */
+ public static enum AutoType {
+ PATHPLANNER, // PathPlanner (https://pathplanner.dev/home.html)
+ CHOREO // Choreo (https://sleipnirgroup.github.io/Choreo/)
+ }
+
+ /** Get the current autonomous path planning type */
+ public static AutoType getAutoType() {
+ return autoType;
+ }
+
+ /** Enumerate the supported vision types */
+ public static enum VisionType {
+ PHOTON, // PhotonVision (https://docs.photonvision.org/en/latest/)
+ NONE // No cameras
+ }
+
+ /** Get the current autonomous path planning type */
+ public static VisionType getVisionType() {
+ return visionType;
+ }
+
+ /***************************************************************************/
+ /* The remainder of this file contains physical and/or software constants for the various subsystems of the robot */
+
+ /** General Constants **************************************************** */
+ public static final double loopPeriodSecs = 0.02;
+
+ public static final boolean tuningMode = false;
+
+ /** Accelerometer Constants ********************************************** */
+ public static class AccelerometerConstants {
+ // Insert here the orientation (CCW == +) of the Rio and IMU from the robot
+ // An angle of "0." means the x-y-z markings on the device match the robot's intrinsic reference
+ // frame.
+ // NOTE: It is assumed that both the Rio and the IMU are mounted such that +Z is UP
+ public static final Rotation2d kRioOrientation =
+ switch (getRobot()) {
+ case COMPBOT -> Rotation2d.fromDegrees(0.);
+ case DEVBOT -> Rotation2d.fromDegrees(0.);
+ default -> Rotation2d.fromDegrees(0.);
+ };
+ // IMU can be one of Pigeon2 or NavX
+ public static final Rotation2d kIMUOrientation =
+ switch (getRobot()) {
+ case COMPBOT -> Rotation2d.fromDegrees(0.);
+ case DEVBOT -> Rotation2d.fromDegrees(0.);
+ default -> Rotation2d.fromDegrees(0.);
+ };
+ }
+
+ /** Physical Constants for Robot Operation ******************************* */
+ public static final class PhysicalConstants {
+ public static final double kRobotMass = (148 - 20.3) * 0.453592; // 32lbs * kg per pound
+ public static final Matter kChassis =
+ new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), PhysicalConstants.kRobotMass);
+ public static final double kLoopTime = 0.13; // s, 20ms + 110ms sprk max velocity lag
+ }
+
+ /** Deploy Directoy Location Constants *********************************** */
+ public static final class DeployConstants {
+ public static final String apriltagDir = "apriltags";
+ public static final String choreoDir = "choreo";
+ public static final String pathplannerDir = "pathplanner";
+ public static final String yagslDir = "swerve";
+ }
+
+ /** Power Distribution Constants ********************************** */
+ public static final class PowerConstants {
+
+ // Set this to either kRev or kCTRE for the type of Power Distribution Module
+ public static final ModuleType kPowerModule = ModuleType.kRev;
+
+ // Current Limits
+ public static final double kTotalMaxCurrent = 120.;
+ public static final double kMotorPortMaxCurrent = 40.;
+ public static final double kSmallPortMaxCurrent = 20.;
+ }
+
+ /** Autonomous Action Constants ****************************************** */
+ public static final class AutonConstants {
+
+ // Translation PID constants
+ public static final PIDConstants kAutoTranslatePID = new PIDConstants(0.7, 0, 0);
+ // Rotation PID constants
+ public static final PIDConstants kAutoAnglePID = new PIDConstants(0.4, 0, 0.01);
+ }
+
+ /** Choreo Autonomous Action Constants *********************************** */
+ public static final class AutoConstants {
+ public static final double kMaxSpeedMetersPerSecond = 3;
+ public static final double kMaxAccelerationMetersPerSecondSquared = 3;
+ public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
+ public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
+
+ public static final double kPXController = 1;
+ public static final double kPYController = 1;
+ public static final double kPThetaController = 1;
+
+ // Constraint for the motion profiled robot angle controller
+ public static final TrapezoidProfile.Constraints kThetaControllerConstraints =
+ new TrapezoidProfile.Constraints(
+ kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
+ }
+
+ /** Drive Base Constants ************************************************* */
+ public static final class DrivebaseConstants {
+
+ // Theoretical free speed (m/s) at 12v applied output;
+ // This needs to be tuned to your individual robot
+ // NOTE: If using SwerveType.PHOENIX6, adjust this in the Phoenix X Tuner Swerve Generator
+ public static final double kMaxLinearSpeed = TunerConstants.kSpeedAt12VoltsMps;
+ // Otherwise, set the maximum linear speed here
+ // public static final double kMaxLinearSpeed = 5.21;
+
+ // Set 3/4 of a rotation per second as the max angular velocity
+ public static final double kMaxAngularSpeed = 1.5 * Math.PI;
+
+ // Maximum chassis accelerations desired for robot motion -- metric / radians
+ public static final double kMaxLinearAccel = 4.0; // m/s/s
+ public static final double kMaxAngularAccel = Units.degreesToRadians(720); // deg/s/s
+
+ // Hold time on motor brakes when disabled
+ public static final double kWheelLockTime = 10; // seconds
+
+ // SysID characterization constants
+ public static final double kMaxV = 12.0; // Max volts
+ public static final double kDelay = 3.0; // seconds
+ public static final double kQuasiTimeout = 5.0; // seconds
+ public static final double kDynamicTimeout = 3.0; // seconds
+ }
+
+ /** Example Flywheel Mechanism Constants ********************************* */
+ public static final class FlywheelConstants {
+
+ // Mechanism motor gear ratio
+ public static final double kFlywheelGearRatio = 1.5;
+
+ // MODE == REAL / REPLAY
+ // Feedforward constants
+ public static final double kStaticGainReal = 0.1;
+ public static final double kVelocityGainReal = 0.05;
+ // Feedback (PID) constants
+ public static final double kPReal = 1.0;
+ public static final double kIReal = 0.0;
+ public static final double kDReal = 0.0;
+
+ // MODE == SIM
+ // Feedforward constants
+ public static final double kStaticGainSim = 0.0;
+ public static final double kVelocityGainSim = 0.03;
+ // Feedback (PID) constants
+ public static final double kPSim = 1.0;
+ public static final double kISim = 0.0;
+ public static final double kDSim = 0.0;
+ }
+
+ /** Operator Constants *************************************************** */
+ public static class OperatorConstants {
+
+ // Joystick Deadband
+ public static final double kLeftDeadband = 0.1;
+ public static final double kRightDeadband = 0.1;
+ public static final double kTurnConstant = 6;
+ }
+
+ /** Vision Constants (Assuming PhotonVision) ***************************** */
+ public static class VisionConstants {
+
+ // AprilTag Identification Constants
+ public static final double kAmbiguityThreshold = 0.4;
+ public static final double kTargetLogTimeSecs = 0.1;
+ public static final double kFieldBorderMargin = 0.5;
+ public static final double kZMargin = 0.75;
+ public static final double kXYZStdDevCoefficient = 0.005;
+ public static final double kThetaStdDevCoefficient = 0.01;
+ }
+
+ /** AprilTag Field Layout ************************************************ */
+ /* SEASON SPECIFIC! -- This section is for 2024 (Crescendo) */
+ public static class AprilTagConstants {
+
+ public static final double aprilTagWidth = Units.inchesToMeters(6.50);
+ public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL;
+
+ public static final AprilTagFieldLayout aprilTagFieldLayout =
+ AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
+
+ @Getter
+ public enum AprilTagLayoutType {
+ OFFICIAL("2024-official"),
+ SPEAKERS_ONLY("2024-speakers"),
+ AMPS_ONLY("2024-amps"),
+ WPI("2024-wpi");
+
+ private AprilTagLayoutType(String name) {
+ if (Constants.disableHAL) {
+ layout = null;
+ } else {
+ try {
+ layout =
+ new AprilTagFieldLayout(
+ Path.of(
+ Filesystem.getDeployDirectory().getPath(), "apriltags", name + ".json"));
+ } catch (IOException e) {
+ throw new RuntimeException(e);
+ }
+ }
+ if (layout == null) {
+ layoutString = "";
+ } else {
+ try {
+ layoutString = new ObjectMapper().writeValueAsString(layout);
+ } catch (JsonProcessingException e) {
+ throw new RuntimeException(
+ "Failed to serialize AprilTag layout JSON " + toString() + "for PhotonVision");
+ }
+ }
+ }
+
+ private final AprilTagFieldLayout layout;
+ private final String layoutString;
+ }
+ }
+}
diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java
new file mode 100644
index 00000000..750b769a
--- /dev/null
+++ b/src/main/java/frc/robot/Main.java
@@ -0,0 +1,34 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+ private Main() {}
+
+ /**
+ * Main initialization function. Do not perform any initialization here.
+ *
+ *
If you change your main robot class, change the parameter type.
+ */
+ public static void main(String... args) {
+ RobotBase.startRobot(Robot::new);
+ }
+}
diff --git a/src/main/java/frc/robot/README b/src/main/java/frc/robot/README
new file mode 100644
index 00000000..700dc4bf
--- /dev/null
+++ b/src/main/java/frc/robot/README
@@ -0,0 +1,79 @@
+Az-RBSI -- Java Root Directory
+
+--------------------
+This directory contains the basic Java files needed to define and run an FRC
+robot. The files that should exist in this directory are listed below, along
+with a brief description of their contents and which files MUST be edited to
+include information about the specific robot being controlled.
+
+BuildConstants.java
+ Not tracked by git, used by the build system to record information about
+ the latest build of the code. This file should NOT be altered, and will be
+ overwritten each time the code is built / compiled.
+
+Constants.java
+ A file for enumerating physical and software constants for the robot. The
+ main Constants class should contain subsystem-specific classes that list
+ the constants for each subsystem. This is useful for organization and
+ importing only the constants needed for a particular software module. This
+ file MUST be modified to include the proper robot constants in the code.
+
+Main.java
+ This is the file run by the RoboRIO virtual machine. Do NOT alter this
+ file.
+
+Robot.java
+ This file is called by ``Main.java`` and directs the operation of the robot
+ according to the currently commanded mode (disabled, autonomous, teleop,
+ test, and simulation). Care must be taken when modifying this module to
+ ensure proper operation of the robot. One section that would be useful to
+ modify is the test mode, where teams can implement pre-match systems
+ testing in a known sequence.
+
+RobotContainer.java
+ A file for holding robot subsystems and commands. This file is called
+ from ``Robot.java`` and is where all subsystems and commands should be
+ gathered and organized including button bindings for operator controls,
+ CANbus and DIO port enumeration for robot devices, and camera pose
+ information (with respect to robot center).
+
+
+--------------------
+Subdirectories:
+
+commands/
+ Commands build in separate modules (rather than in subsystem definitions or
+ inline commands in ``RobotContainer.java``) live in this directory.
+ Organization of files and subdirectories herein is left to the discrection
+ of teams.
+
+generated/
+ This directory holds the ``TunerConstants.java`` file produced by Phoenix
+ Tuner X's swerve generator (you must generate this file for your specific
+ drivetrain -- the one included in the Az-RBSI is an example only). It must
+ be modified as described in the AdvantageKit documentation (removal of
+ final function and final import; modification of ``kSteerInertia`` and
+ ``kDriveInertia`` constants to be ``0.004`` and ``0.025``, respectively;
+ optional "Pro" feature, set ``kSteerClosedLoopOutput`` and/or
+ ``kDriveClosedLoopOutput`` to ``ClosedLoopOutputType.TorqueCurrentFOC``;
+ characterize the drive and turn feedforward gains [``kS``,``kV``,``kA``]
+ using the auto routines included in the Az-RBSI).
+
+subsystems/
+ The hardware definition of subsystems is done in this directory.
+ Organization of files and subdirectories herein is left to the discrection
+ of teams.
+
+util/
+ Various utility functions used by the Az-RBSI are included in this
+ directory. Most teams will not need to modify these routines, but
+ additional utilities deemed necessary by individual teams may be placed
+ here for organizational purposes. The EXCEPTION is
+ ``OverrideSwitches.java``, which may be modified to meet the specific
+ design requirements of teams. ``OverrideSwitches.java`` can be modified
+ to match any manual override switches on an operator console as described
+ in that module.
+
+
+--------------------
+Last Modified: 06 Nov 2024, TPEB
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
new file mode 100644
index 00000000..6cd3c0c0
--- /dev/null
+++ b/src/main/java/frc/robot/Robot.java
@@ -0,0 +1,207 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.Constants.PowerConstants;
+import frc.robot.RobotContainer.Ports;
+import frc.robot.util.VirtualSubsystem;
+import org.littletonrobotics.junction.LogFileUtil;
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.inputs.LoggedPowerDistribution;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGReader;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends LoggedRobot {
+ private Command m_autonomousCommand;
+ private RobotContainer m_robotContainer;
+ private Timer m_disabledTimer;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Record metadata
+ Logger.recordMetadata("Robot", Constants.getRobot().toString());
+ Logger.recordMetadata("TuningMode", Boolean.toString(Constants.tuningMode));
+ Logger.recordMetadata("RuntimeType", getRuntimeType().toString());
+ Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
+ Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
+ Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
+ Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
+ Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
+ switch (BuildConstants.DIRTY) {
+ case 0:
+ Logger.recordMetadata("GitDirty", "All changes committed");
+ break;
+ case 1:
+ Logger.recordMetadata("GitDirty", "Uncomitted changes");
+ break;
+ default:
+ Logger.recordMetadata("GitDirty", "Unknown");
+ break;
+ }
+
+ // Set up data receivers & replay source
+ switch (Constants.getMode()) {
+ case REAL:
+ // Running on a real robot, log to a USB stick ("/U/logs")
+ Logger.addDataReceiver(new WPILOGWriter());
+ Logger.addDataReceiver(new NT4Publisher());
+ LoggedPowerDistribution.getInstance(
+ Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), PowerConstants.kPowerModule);
+ break;
+
+ case SIM:
+ // Running a physics simulator, log to NT
+ Logger.addDataReceiver(new NT4Publisher());
+ break;
+
+ case REPLAY:
+ // Replaying a log, set up replay source
+ setUseTiming(false); // Run as fast as possible
+ String logPath = LogFileUtil.findReplayLog();
+ Logger.setReplaySource(new WPILOGReader(logPath));
+ Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
+ break;
+ }
+
+ // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit.
+ // Logger.disableDeterministicTimestamps()
+
+ // Start AdvantageKit logger; disable protobuf warnings
+ Logger.start();
+ LogTable.disableProtobufWarning();
+
+ // Instantiate our RobotContainer. This will perform all our button bindings,
+ // and put our autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+
+ // Create a timer to disable motor brake a few seconds after disable. This will
+ // let the robot
+ // stop immediately when disabled, but then also let it be pushed more
+ m_disabledTimer = new Timer();
+ }
+
+ /** This function is called periodically during all modes. */
+ @Override
+ public void robotPeriodic() {
+ // Run all virtual subsystems each time through the loop
+ VirtualSubsystem.periodicAll();
+
+ // Runs the Scheduler. This is responsible for polling buttons, adding
+ // newly-scheduled commands, running already-scheduled commands, removing
+ // finished or interrupted commands, and running subsystem periodic() methods.
+ // This must be called from the robot's periodic block in order for anything in
+ // the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /** This function is called once when the robot is disabled. */
+ @Override
+ public void disabledInit() {
+ // Set the brakes to stop robot motion
+ m_robotContainer.setMotorBrake(true);
+ m_disabledTimer.reset();
+ m_disabledTimer.start();
+ }
+
+ /** This function is called periodically when disabled. */
+ @Override
+ public void disabledPeriodic() {
+ // After WHEEL_LOCK_TIME has elapsed, release the drive brakes
+ if (m_disabledTimer.hasElapsed(Constants.DrivebaseConstants.kWheelLockTime)) {
+ m_robotContainer.setMotorBrake(false);
+ m_disabledTimer.stop();
+ }
+ }
+
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+ @Override
+ public void autonomousInit() {
+ m_robotContainer.setMotorBrake(true);
+ switch (Constants.getAutoType()) {
+ case PATHPLANNER:
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+ break;
+ case CHOREO:
+ m_autonomousCommand = m_robotContainer.getAutonomousCommandChoreo();
+ break;
+ default:
+ throw new RuntimeException(
+ "Incorrect AUTO type selected in Constants: " + Constants.getAutoType());
+ }
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+ }
+
+ /** This function is called periodically during autonomous. */
+ @Override
+ public void autonomousPeriodic() {}
+
+ /** This function is called once when teleop is enabled. */
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ } else {
+ CommandScheduler.getInstance().cancelAll();
+ }
+ m_robotContainer.setDriveMode();
+ m_robotContainer.setMotorBrake(true);
+ }
+
+ /** This function is called periodically during operator control. */
+ @Override
+ public void teleopPeriodic() {}
+
+ /** This function is called once when test mode is enabled. */
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ m_robotContainer.setDriveMode();
+ }
+
+ /** This function is called periodically during test mode. */
+ @Override
+ public void testPeriodic() {}
+
+ /** This function is called once when the robot is first started up. */
+ @Override
+ public void simulationInit() {}
+
+ /** This function is called periodically whilst in simulation. */
+ @Override
+ public void simulationPeriodic() {}
+}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
new file mode 100644
index 00000000..71767867
--- /dev/null
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -0,0 +1,417 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+import com.choreo.lib.Choreo;
+import com.choreo.lib.ChoreoTrajectory;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.Constants.AprilTagConstants;
+import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType;
+import frc.robot.Constants.AutoConstants;
+import frc.robot.Constants.PowerConstants;
+import frc.robot.commands.DriveCommands;
+import frc.robot.subsystems.accelerometer.Accelerometer;
+import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.flywheel_example.Flywheel;
+import frc.robot.subsystems.flywheel_example.FlywheelIO;
+import frc.robot.subsystems.flywheel_example.FlywheelIOSim;
+import frc.robot.subsystems.vision.Vision;
+import frc.robot.subsystems.vision.VisionIO;
+import frc.robot.subsystems.vision.VisionIOPhoton;
+import frc.robot.util.Alert;
+import frc.robot.util.Alert.AlertType;
+import frc.robot.util.LoggedTunableNumber;
+import frc.robot.util.OverrideSwitches;
+import frc.robot.util.PowerMonitoring;
+import frc.robot.util.RobotDeviceId;
+import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
+
+/** This is the location for defining robot hardware, commands, and controller button bindings. */
+public class RobotContainer {
+
+ // Define the Driver and, optionally, the Operator/Co-Driver Controllers
+ // Replace with ``CommandPS4Controller`` or ``CommandJoystick`` if needed
+ final CommandXboxController driverXbox = new CommandXboxController(0);
+ final CommandXboxController operatorXbox = new CommandXboxController(1);
+ final OverrideSwitches overrides = new OverrideSwitches(2);
+
+ // Autonomous Things
+ Field2d m_field = new Field2d();
+ ChoreoTrajectory m_traj;
+
+ // Declare the robot subsystems here
+ private final Drive m_drivebase;
+ private final Flywheel m_flywheel;
+ private final Accelerometer m_accel;
+ private final Vision m_vision;
+ private final PowerMonitoring m_power;
+
+ // Dashboard inputs
+ private final LoggedDashboardChooser autoChooser;
+ // EXAMPLE TUNABLE FLYWHEEL SPEED INPUT FROM DASHBOARD
+ private final LoggedTunableNumber flywheelSpeedInput =
+ new LoggedTunableNumber("Flywheel Speed", 1500.0);
+
+ // Alerts
+ private final Alert aprilTagLayoutAlert = new Alert("", AlertType.INFO);
+
+ /** Returns the current AprilTag layout type. */
+ public AprilTagLayoutType getAprilTagLayoutType() {
+ return AprilTagConstants.defaultAprilTagType;
+ }
+
+ public static AprilTagLayoutType staticGetAprilTagLayoutType() {
+ return AprilTagConstants.defaultAprilTagType;
+ }
+
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
+ public RobotContainer() {
+
+ // Instantiate Robot Subsystems based on RobotType
+ switch (Constants.getMode()) {
+ case REAL:
+ // Real robot, instantiate hardware IO implementations
+ // YAGSL drivebase, get config from deploy directory
+ m_drivebase = new Drive();
+ m_flywheel = new Flywheel(new FlywheelIOSim()); // new Flywheel(new FlywheelIOTalonFX());
+ m_vision =
+ switch (Constants.getVisionType()) {
+ case PHOTON ->
+ new Vision(
+ this::getAprilTagLayoutType,
+ new VisionIOPhoton(this::getAprilTagLayoutType, "Photon_CAMNAME"));
+ case NONE -> new Vision(this::getAprilTagLayoutType);
+ default -> null;
+ };
+ m_accel = new Accelerometer(m_drivebase.getGyro());
+ break;
+
+ case SIM:
+ // Sim robot, instantiate physics sim IO implementations
+ m_drivebase = new Drive();
+ m_flywheel = new Flywheel(new FlywheelIOSim());
+ m_vision = new Vision(this::getAprilTagLayoutType);
+ m_accel = new Accelerometer(m_drivebase.getGyro());
+ break;
+
+ default:
+ // Replayed robot, disable IO implementations
+ m_drivebase = new Drive();
+ m_flywheel = new Flywheel(new FlywheelIO() {});
+ m_vision = new Vision(this::getAprilTagLayoutType, new VisionIO() {}, new VisionIO() {});
+ m_accel = new Accelerometer(m_drivebase.getGyro());
+ break;
+ }
+
+ // ``PowerMonitoring`` takes all the non-drivebase subsystems for which
+ // you wish to have power monitoring; DO NOT include ``m_drivebase``,
+ // as that is automatically monitored.
+ m_power = null; // new PowerMonitoring(m_flywheel);
+
+ // Set up the SmartDashboard Auto Chooser
+ autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
+
+ // Configure the trigger bindings
+ configureBindings();
+ // Define Auto commands
+ defineAutoCommands();
+ // Define SysIs Routines
+ definesysIdRoutines();
+ }
+
+ /** Use this method to define your Autonomous commands for use with PathPlanner / Choreo */
+ private void defineAutoCommands() {
+
+ NamedCommands.registerCommand("Zero", Commands.runOnce(() -> m_drivebase.zero()));
+ }
+
+ /** Set up the SysID routines from AdvantageKit */
+ private void definesysIdRoutines() {
+ // Drivebase characterization
+ autoChooser.addOption(
+ "Drive SysId (Quasistatic Forward)",
+ m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+ autoChooser.addOption(
+ "Drive SysId (Quasistatic Reverse)",
+ m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+ autoChooser.addOption(
+ "Drive SysId (Dynamic Forward)", m_drivebase.sysIdDynamic(SysIdRoutine.Direction.kForward));
+ autoChooser.addOption(
+ "Drive SysId (Dynamic Reverse)", m_drivebase.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+
+ // Example Flywheel SysId Characterization
+ autoChooser.addOption(
+ "Flywheel SysId (Quasistatic Forward)",
+ m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+ autoChooser.addOption(
+ "Flywheel SysId (Quasistatic Reverse)",
+ m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+ autoChooser.addOption(
+ "Flywheel SysId (Dynamic Forward)",
+ m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward));
+ autoChooser.addOption(
+ "Flywheel SysId (Dynamic Reverse)",
+ m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+ * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ */
+ private void configureBindings() {
+
+ // SET STANDARD DRIVING AS DEFAULT COMMAND FOR THE DRIVEBASE
+ // NOTE: Left joystick controls lateral translation, right joystick (X) controls rotation
+ m_drivebase.setDefaultCommand(
+ DriveCommands.fieldRelativeDrive(
+ m_drivebase,
+ () -> -driverXbox.getLeftY(),
+ () -> -driverXbox.getLeftX(),
+ () -> driverXbox.getRightX()));
+
+ // Example Commands
+ // Press B button while driving --> ROBOT-CENTRIC
+ driverXbox
+ .b()
+ .onTrue(
+ Commands.runOnce(
+ () ->
+ DriveCommands.robotRelativeDrive(
+ m_drivebase,
+ () -> -driverXbox.getLeftY(),
+ () -> -driverXbox.getLeftX(),
+ () -> driverXbox.getRightX()),
+ m_drivebase));
+
+ // Press A button -> BRAKE
+ driverXbox.a().whileTrue(Commands.runOnce(() -> m_drivebase.setMotorBrake(true), m_drivebase));
+
+ // Press X button --> Stop with wheels in X-Lock position
+ driverXbox.x().onTrue(Commands.runOnce(m_drivebase::stopWithX, m_drivebase));
+
+ // Press Y button --> Manually Re-Zero the Gyro
+ driverXbox.y().onTrue(Commands.runOnce(() -> m_drivebase.zero()));
+
+ // Press RIGHT BUMPER --> Run the example flywheel
+ driverXbox
+ .rightBumper()
+ .whileTrue(
+ Commands.startEnd(
+ () -> m_flywheel.runVelocity(flywheelSpeedInput.get()),
+ m_flywheel::stop,
+ m_flywheel));
+ }
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+
+ // An example command will be run in autonomous
+ // return m_drivebase.getAutoPath("Tests");
+ // Use the ``autoChooser`` to define your auto path from the SmartDashboard
+ return autoChooser.get();
+ }
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommandChoreo() {
+
+ m_traj = Choreo.getTrajectory("Trajectory");
+
+ m_field.getObject("traj").setPoses(m_traj.getInitialPose(), m_traj.getFinalPose());
+ m_field.getObject("trajPoses").setPoses(m_traj.getPoses());
+
+ var thetaController = new PIDController(AutoConstants.kPThetaController, 0, 0);
+ thetaController.enableContinuousInput(-Math.PI, Math.PI);
+
+ m_drivebase.setPose(m_traj.getInitialPose());
+
+ boolean isFlipped =
+ DriverStation.getAlliance().isPresent()
+ && DriverStation.getAlliance().get() == Alliance.Red;
+
+ Command swerveCommand =
+ Choreo.choreoSwerveCommand(
+ // Choreo trajectory from above
+ m_traj,
+ // A function that returns the current field-relative pose of the robot: your wheel or
+ // vision odometry
+ m_drivebase::getPose,
+ // PIDController for field-relative X translation (input: X error meters, output: m/s).
+ new PIDController(Constants.AutoConstants.kPXController, 0.0, 0.0),
+ // PIDController for field-relative Y translation (input: Y error meters, output: m/s).
+ new PIDController(Constants.AutoConstants.kPYController, 0.0, 0.0),
+ // PID constants to correct for rotation error
+ thetaController,
+ (ChassisSpeeds speeds) ->
+ m_drivebase.runVelocity(
+ ChassisSpeeds.fromRobotRelativeSpeeds(
+ speeds.vxMetersPerSecond,
+ speeds.vyMetersPerSecond,
+ speeds.omegaRadiansPerSecond,
+ isFlipped
+ ? m_drivebase.getRotation().plus(new Rotation2d(Math.PI))
+ : m_drivebase.getRotation())),
+ // Whether or not to mirror the path based on alliance (this assumes the path is created
+ // for the blue alliance)
+ () -> true,
+ // The subsystem(s) to require, typically your drive subsystem only
+ m_drivebase);
+
+ return Commands.sequence(
+ Commands.runOnce(() -> m_drivebase.setPose(m_traj.getInitialPose())),
+ swerveCommand,
+ m_drivebase.run(() -> m_drivebase.stop()));
+ }
+
+ public void setDriveMode() {
+ configureBindings();
+ }
+
+ /** Set the motor neutral mode to BRAKE / COAST for T/F */
+ public void setMotorBrake(boolean brake) {
+ m_drivebase.setMotorBrake(brake);
+ }
+
+ /** Updates the alerts. */
+ public void updateAlerts() {
+ // AprilTag layout alert
+ boolean aprilTagAlertActive = getAprilTagLayoutType() != AprilTagLayoutType.OFFICIAL;
+ aprilTagLayoutAlert.set(aprilTagAlertActive);
+ if (aprilTagAlertActive) {
+ aprilTagLayoutAlert.setText(
+ "Non-official AprilTag layout in use (" + getAprilTagLayoutType().toString() + ").");
+ }
+ }
+
+ /** List of Device CAN and Power Distribution Circuit IDs **************** */
+ public static class Ports {
+
+ /* DRIVETRAIN CAN DEVICE IDS */
+ // This is the default setup for the Az-RBSI swerve base
+ // Swerve Modules go:
+ // FL,FR,BL,BR
+ //
+ // 0 1
+ // 2 3
+ public static final RobotDeviceId FL_DRIVE = new RobotDeviceId(1, "DriveTrain", 18);
+ public static final RobotDeviceId FL_ROTATION = new RobotDeviceId(2, "DriveTrain", 19);
+ public static final RobotDeviceId FL_CANCODER = new RobotDeviceId(3, "DriveTrain", null);
+
+ public static final RobotDeviceId FR_DRIVE = new RobotDeviceId(4, "DriveTrain", 17);
+ public static final RobotDeviceId FR_ROTATION = new RobotDeviceId(5, "DriveTrain", 16);
+ public static final RobotDeviceId FR_CANCODER = new RobotDeviceId(6, "DriveTrain", null);
+
+ public static final RobotDeviceId BL_DRIVE = new RobotDeviceId(7, "DriveTrain", 1);
+ public static final RobotDeviceId BL_ROTATION = new RobotDeviceId(8, "DriveTrain", 0);
+ public static final RobotDeviceId BL_CANCODER = new RobotDeviceId(9, "DriveTrain", null);
+
+ public static final RobotDeviceId BR_DRIVE = new RobotDeviceId(10, "DriveTrain", 2);
+ public static final RobotDeviceId BR_ROTATION = new RobotDeviceId(11, "DriveTrain", 3);
+ public static final RobotDeviceId BR_CANCODER = new RobotDeviceId(12, "DriveTrain", null);
+
+ public static final RobotDeviceId PIGEON = new RobotDeviceId(13, "DriveTrain", null);
+
+ /* POWER DISTRIBUTION CAN ID (set by device type in PowerConstants) */
+ public static final RobotDeviceId POWER_CAN_DEVICE_ID =
+ switch (PowerConstants.kPowerModule) {
+ case kRev -> new RobotDeviceId(1, null);
+ case kCTRE -> new RobotDeviceId(0, null);
+ default -> null;
+ };
+
+ /* SUBSYSTEM CAN DEVICE IDS */
+ // This is where mechanism subsystem devices are defined
+ // Example:
+ public static final RobotDeviceId FLYWHEEL_LEADER = new RobotDeviceId(3, 8);
+ public static final RobotDeviceId FLYWHEEL_FOLLOWER = new RobotDeviceId(4, 9);
+
+ /* BEAM BREAK and/or LIMIT SWITCH DIO CHANNELS */
+ // This is where digital I/O feedback devices are defined
+ // Example:
+ // public static final int ELEVATOR_BOTTOM_LIMIT = 3;
+
+ /* LINEAR SERVO PWM CHANNELS */
+ // This is where PWM-controlled devices (actuators, servos, pneumatics, etc.)
+ // are defined
+ // Example:
+ // public static final int INTAKE_SERVO = 0;
+ }
+
+ /** Override and Console Toggle Switches ********************************* */
+ public static class Overrides {
+
+ // Assumes this controller: https://www.amazon.com/gp/product/B00UUROWWK
+ // Example from:
+ // https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2024-build-thread/442736/72
+ public static final int DRIVER_SWITCH_0 = 1;
+ public static final int DRIVER_SWITCH_1 = 2;
+ public static final int DRIVER_SWITCH_2 = 3;
+
+ public static final int OPERATOR_SWITCH_0 = 8;
+ public static final int OPERATOR_SWITCH_1 = 9;
+ public static final int OPERATOR_SWITCH_2 = 10;
+ public static final int OPERATOR_SWITCH_3 = 11;
+ public static final int OPERATOR_SWITCH_4 = 12;
+
+ public static final int[] MULTI_TOGGLE = {4, 5};
+ }
+
+ /** Vision Camera Posses ************************************************* */
+ public static class Cameras {
+
+ public static final Pose3d[] cameraPoses =
+ switch (Constants.getRobot()) {
+ case COMPBOT ->
+ new Pose3d[] {
+ // Camera #1
+ new Pose3d(
+ Units.inchesToMeters(-1.0),
+ Units.inchesToMeters(0),
+ Units.inchesToMeters(23.5),
+ new Rotation3d(0.0, Units.degreesToRadians(-20), 0.0)),
+ };
+ case DEVBOT -> new Pose3d[] {};
+ default -> new Pose3d[] {};
+ };
+ }
+}
diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java
new file mode 100644
index 00000000..35356f09
--- /dev/null
+++ b/src/main/java/frc/robot/commands/DriveCommands.java
@@ -0,0 +1,128 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.commands;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants.OperatorConstants;
+import frc.robot.subsystems.drive.Drive;
+import java.util.function.DoubleSupplier;
+
+public class DriveCommands {
+
+ private DriveCommands() {}
+
+ /**
+ * Field relative drive command using two joysticks (controlling linear and angular velocities).
+ */
+ public static Command fieldRelativeDrive(
+ Drive drive,
+ DoubleSupplier xSupplier,
+ DoubleSupplier ySupplier,
+ DoubleSupplier omegaSupplier) {
+ return Commands.run(
+ () -> {
+ // Get the Linear Velocity & Omega from inputs
+ Translation2d linearVelocity = getLinearVelocity(xSupplier, ySupplier);
+ double omega = getOmega(omegaSupplier);
+
+ // Convert to field relative speeds & send command
+ boolean isFlipped =
+ DriverStation.getAlliance().isPresent()
+ && DriverStation.getAlliance().get() == Alliance.Red;
+ drive.runVelocity(
+ ChassisSpeeds.fromFieldRelativeSpeeds(
+ linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
+ linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
+ omega * drive.getMaxAngularSpeedRadPerSec(),
+ isFlipped
+ ? drive.getRotation().plus(new Rotation2d(Math.PI))
+ : drive.getRotation()));
+ },
+ drive);
+ }
+
+ /**
+ * Robot relative drive command using two joysticks (controlling linear and angular velocities).
+ */
+ public static Command robotRelativeDrive(
+ Drive drive,
+ DoubleSupplier xSupplier,
+ DoubleSupplier ySupplier,
+ DoubleSupplier omegaSupplier) {
+ return Commands.run(
+ () -> {
+
+ // Get the Linear Velocity & Omega from inputs
+ Translation2d linearVelocity = getLinearVelocity(xSupplier, ySupplier);
+ double omega = getOmega(omegaSupplier);
+
+ // Convert to robot relative speeds & send command
+ boolean isFlipped =
+ DriverStation.getAlliance().isPresent()
+ && DriverStation.getAlliance().get() == Alliance.Red;
+ drive.runVelocity(
+ ChassisSpeeds.fromRobotRelativeSpeeds(
+ linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
+ linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
+ omega * drive.getMaxAngularSpeedRadPerSec(),
+ isFlipped
+ ? drive.getRotation().plus(new Rotation2d(Math.PI))
+ : drive.getRotation()));
+ },
+ drive);
+ }
+
+ /**
+ * Compute the new linear velocity from inputs, including applying deadbands and squaring for
+ * smoothness
+ */
+ private static Translation2d getLinearVelocity(
+ DoubleSupplier xSupplier, DoubleSupplier ySupplier) {
+
+ // Apply deadband
+ double linearMagnitude =
+ MathUtil.applyDeadband(
+ Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()),
+ OperatorConstants.kLeftDeadband);
+ Rotation2d linearDirection = new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble());
+
+ // Square values
+ linearMagnitude = linearMagnitude * linearMagnitude;
+
+ return new Pose2d(new Translation2d(), linearDirection)
+ .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d()))
+ .getTranslation();
+ }
+
+ /**
+ * Compute the new angular velocity from inputs, including applying deadbands and squaring for
+ * smoothness
+ */
+ private static double getOmega(DoubleSupplier omegaSupplier) {
+ double omega =
+ MathUtil.applyDeadband(omegaSupplier.getAsDouble(), OperatorConstants.kRightDeadband);
+ return Math.copySign(omega * omega, omega);
+ }
+}
diff --git a/src/main/java/frc/robot/generated/README b/src/main/java/frc/robot/generated/README
new file mode 100644
index 00000000..420d3fe4
--- /dev/null
+++ b/src/main/java/frc/robot/generated/README
@@ -0,0 +1,2 @@
+This directory, and in particular the TunerConstants.java file, are the
+Phoenix6 equivalent of the ``src/main/deploy/swerve`` directory for YAGSL.
diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java
new file mode 100644
index 00000000..f88d9bf1
--- /dev/null
+++ b/src/main/java/frc/robot/generated/TunerConstants.java
@@ -0,0 +1,188 @@
+package frc.robot.generated;
+
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.Pigeon2Configuration;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.ClosedLoopOutputType;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory;
+import edu.wpi.first.math.util.Units;
+
+// Generated by the Tuner X Swerve Project Generator
+// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
+public 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
+ public static final Slot0Configs steerGains =
+ new Slot0Configs().withKP(100).withKI(0).withKD(0.2).withKS(0).withKV(1.5).withKA(0);
+ // When using closed-loop control, the drive motor uses the control
+ // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
+ public static final Slot0Configs driveGains =
+ new Slot0Configs().withKP(3).withKI(0).withKD(0).withKS(0).withKV(0).withKA(0);
+
+ // The closed-loop output type to use for the steer motors;
+ // This affects the PID/FF gains for the steer motors
+ public static final ClosedLoopOutputType steerClosedLoopOutput = ClosedLoopOutputType.Voltage;
+ // The closed-loop output type to use for the drive motors;
+ // This affects the PID/FF gains for the drive motors
+ public static final ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage;
+
+ // The stator current at which the wheels start to slip;
+ // This needs to be tuned to your individual robot
+ public static final double kSlipCurrentA = 150.0;
+
+ // 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.
+ public static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
+ public static final TalonFXConfiguration steerInitialConfigs =
+ new TalonFXConfiguration()
+ .withCurrentLimits(
+ new 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)
+ .withStatorCurrentLimitEnable(true));
+ public static final CANcoderConfiguration cancoderInitialConfigs = new CANcoderConfiguration();
+ // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
+ public static final Pigeon2Configuration pigeonConfigs = null;
+
+ // Theoretical free speed (m/s) at 12v applied output;
+ // This needs to be tuned to your individual robot
+ public static final double kSpeedAt12VoltsMps = 4.73;
+
+ // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
+ // This may need to be tuned to your individual robot
+ public static final double kCoupleRatio = 3.5714285714285716;
+
+ public static final double kDriveGearRatio = 6.746031746031747;
+ public static final double kSteerGearRatio = 21.428571428571427;
+ public static final double kWheelRadiusInches = 2;
+
+ public static final boolean kInvertLeftSide = false;
+ public static final boolean kInvertRightSide = true;
+
+ public static final String kCANbusName = "DriveTrain";
+ public static final int kPigeonId = 13;
+
+ // These are only used for simulation
+ public static final double kSteerInertia = 0.004;
+ public static final double kDriveInertia = 0.025;
+ // Simulated voltage necessary to overcome friction
+ public static final double kSteerFrictionVoltage = 0.25;
+ public static final double kDriveFrictionVoltage = 0.25;
+
+ public static final SwerveDrivetrainConstants DrivetrainConstants =
+ new SwerveDrivetrainConstants()
+ .withCANbusName(kCANbusName)
+ .withPigeon2Id(kPigeonId)
+ .withPigeon2Configs(pigeonConfigs);
+
+ public static final SwerveModuleConstantsFactory ConstantCreator =
+ new SwerveModuleConstantsFactory()
+ .withDriveMotorGearRatio(kDriveGearRatio)
+ .withSteerMotorGearRatio(kSteerGearRatio)
+ .withWheelRadius(kWheelRadiusInches)
+ .withSlipCurrent(kSlipCurrentA)
+ .withSteerMotorGains(steerGains)
+ .withDriveMotorGains(driveGains)
+ .withSteerMotorClosedLoopOutput(steerClosedLoopOutput)
+ .withDriveMotorClosedLoopOutput(driveClosedLoopOutput)
+ .withSpeedAt12VoltsMps(kSpeedAt12VoltsMps)
+ .withSteerInertia(kSteerInertia)
+ .withDriveInertia(kDriveInertia)
+ .withSteerFrictionVoltage(kSteerFrictionVoltage)
+ .withDriveFrictionVoltage(kDriveFrictionVoltage)
+ .withFeedbackSource(SteerFeedbackType.FusedCANcoder)
+ .withCouplingGearRatio(kCoupleRatio)
+ .withDriveMotorInitialConfigs(driveInitialConfigs)
+ .withSteerMotorInitialConfigs(steerInitialConfigs)
+ .withCANcoderInitialConfigs(cancoderInitialConfigs);
+
+ // Front Left
+ public static final int kFrontLeftDriveMotorId = 1;
+ public static final int kFrontLeftSteerMotorId = 2;
+ public static final int kFrontLeftEncoderId = 3;
+ public static final double kFrontLeftEncoderOffset = -0.4736328125;
+ public static final boolean kFrontLeftSteerInvert = true;
+
+ public static final double kFrontLeftXPosInches = 10.375;
+ public static final double kFrontLeftYPosInches = 10.375;
+
+ // Front Right
+ public static final int kFrontRightDriveMotorId = 4;
+ public static final int kFrontRightSteerMotorId = 5;
+ public static final int kFrontRightEncoderId = 6;
+ public static final double kFrontRightEncoderOffset = -0.1123046875;
+ public static final boolean kFrontRightSteerInvert = true;
+
+ public static final double kFrontRightXPosInches = 10.375;
+ public static final double kFrontRightYPosInches = -10.375;
+
+ // Back Left
+ public static final int kBackLeftDriveMotorId = 7;
+ public static final int kBackLeftSteerMotorId = 8;
+ public static final int kBackLeftEncoderId = 9;
+ public static final double kBackLeftEncoderOffset = 0.4091796875;
+ public static final boolean kBackLeftSteerInvert = true;
+
+ public static final double kBackLeftXPosInches = -10.375;
+ public static final double kBackLeftYPosInches = 10.375;
+
+ // Back Right
+ public static final int kBackRightDriveMotorId = 10;
+ public static final int kBackRightSteerMotorId = 11;
+ public static final int kBackRightEncoderId = 12;
+ public static final double kBackRightEncoderOffset = 0.08544921875;
+ public static final boolean kBackRightSteerInvert = true;
+
+ public static final double kBackRightXPosInches = -10.375;
+ public static final double kBackRightYPosInches = -10.375;
+
+ public static final SwerveModuleConstants FrontLeft =
+ ConstantCreator.createModuleConstants(
+ kFrontLeftSteerMotorId,
+ kFrontLeftDriveMotorId,
+ kFrontLeftEncoderId,
+ kFrontLeftEncoderOffset,
+ Units.inchesToMeters(kFrontLeftXPosInches),
+ Units.inchesToMeters(kFrontLeftYPosInches),
+ kInvertLeftSide)
+ .withSteerMotorInverted(kFrontLeftSteerInvert);
+ public static final SwerveModuleConstants FrontRight =
+ ConstantCreator.createModuleConstants(
+ kFrontRightSteerMotorId,
+ kFrontRightDriveMotorId,
+ kFrontRightEncoderId,
+ kFrontRightEncoderOffset,
+ Units.inchesToMeters(kFrontRightXPosInches),
+ Units.inchesToMeters(kFrontRightYPosInches),
+ kInvertRightSide)
+ .withSteerMotorInverted(kFrontRightSteerInvert);
+ public static final SwerveModuleConstants BackLeft =
+ ConstantCreator.createModuleConstants(
+ kBackLeftSteerMotorId,
+ kBackLeftDriveMotorId,
+ kBackLeftEncoderId,
+ kBackLeftEncoderOffset,
+ Units.inchesToMeters(kBackLeftXPosInches),
+ Units.inchesToMeters(kBackLeftYPosInches),
+ kInvertLeftSide)
+ .withSteerMotorInverted(kBackLeftSteerInvert);
+ public static final SwerveModuleConstants BackRight =
+ ConstantCreator.createModuleConstants(
+ kBackRightSteerMotorId,
+ kBackRightDriveMotorId,
+ kBackRightEncoderId,
+ kBackRightEncoderOffset,
+ Units.inchesToMeters(kBackRightXPosInches),
+ Units.inchesToMeters(kBackRightYPosInches),
+ kInvertRightSide)
+ .withSteerMotorInverted(kBackRightSteerInvert);
+}
diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java
new file mode 100644
index 00000000..91519de3
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java
@@ -0,0 +1,140 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.accelerometer;
+
+import static frc.robot.Constants.AccelerometerConstants.*;
+
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.wpilibj.BuiltInAccelerometer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.Constants;
+import frc.robot.util.VirtualSubsystem;
+import frc.robot.util.YagslConstants;
+import org.littletonrobotics.junction.Logger;
+
+/**
+ * Accelerometer subsystem (built upon a virtual subsystem)
+ *
+ * This virtual subsystem pulls the acceleration values from both the RoboRIO and the swerve's
+ * IMU (either Pigeon2 or NavX) and logs them to both AdvantageKit and the SmartDashboard. In
+ * addition to the accelerations, the jerk (a-dot or x-tripple-dot) is computed from the delta
+ * accelerations.
+ */
+public class Accelerometer extends VirtualSubsystem {
+
+ private final BuiltInAccelerometer rioAccelerometer = new BuiltInAccelerometer();
+ private final Pigeon2 pigeonAccelerometer;
+ private final AHRS navXAccelerometer;
+
+ // Define the 3D vectors needed to hold values
+ private Translation3d rioAccVector;
+ private Translation3d imuAccVector;
+ private Translation3d prevRioAccel = new Translation3d();
+ private Translation3d prevImuAccel = new Translation3d();
+ private Translation3d rioJerkVector;
+ private Translation3d imuJerkVector;
+
+ /** Constructor method, takes the IMU accelerometer instance */
+ public Accelerometer(Object accelerometer) {
+ switch (Constants.getSwerveType()) {
+ case PHOENIX6:
+ // CTRE Tuner X drive bases must use a Pigeon2
+ pigeonAccelerometer = (Pigeon2) accelerometer;
+ navXAccelerometer = null;
+ break;
+
+ case YAGSL:
+ // Logic checking the type of IMU included in the parsed YAGSL:
+ if (YagslConstants.swerveDriveJson.imu.type == "pigeon2") {
+ pigeonAccelerometer = (Pigeon2) accelerometer;
+ navXAccelerometer = null;
+ break;
+ }
+
+ if (YagslConstants.swerveDriveJson.imu.type == "navx"
+ || YagslConstants.swerveDriveJson.imu.type == "navx_spi") {
+ navXAccelerometer = (AHRS) accelerometer;
+ pigeonAccelerometer = null;
+ break;
+ }
+
+ default:
+ // Otherwise kick a message to the console, and set to null
+ System.out.println("WARNING: Cannot initialize IMU's accelerometer for logging!");
+ this.pigeonAccelerometer = null;
+ this.navXAccelerometer = null;
+ }
+ }
+
+ /** Get accelerations, compute jerks, log everything */
+ public void periodic() {
+
+ // Log the execution time
+ long start = System.nanoTime();
+
+ // Compute the Rio's acceleration, rotated as needed in the XY plane (yaw)
+ // RoboRio provides accelerations in `g`, from -8 to +8; convert to m/s^2
+ rioAccVector =
+ new Translation3d(rioAccelerometer.getX(), rioAccelerometer.getY(), rioAccelerometer.getZ())
+ .rotateBy(new Rotation3d(0., 0., kRioOrientation.getRadians()))
+ .times(9.81);
+
+ // Compute the IMU's acceleration, rotated as needed
+ if (pigeonAccelerometer != null) {
+ // Pigeon provides accelerations in `g`, from -2 to +2; convert to m/s^2
+ imuAccVector =
+ new Translation3d(
+ pigeonAccelerometer.getAccelerationX().getValueAsDouble(),
+ pigeonAccelerometer.getAccelerationY().getValueAsDouble(),
+ pigeonAccelerometer.getAccelerationZ().getValueAsDouble());
+ } else if (navXAccelerometer != null) {
+ // NavX provides accelerations in ...
+ imuAccVector =
+ new Translation3d(
+ navXAccelerometer.getWorldLinearAccelX(),
+ navXAccelerometer.getWorldLinearAccelY(),
+ navXAccelerometer.getWorldLinearAccelZ());
+ } else {
+ imuAccVector = new Translation3d();
+ }
+ imuAccVector =
+ imuAccVector.rotateBy(new Rotation3d(0., 0., kIMUOrientation.getRadians())).times(9.81);
+
+ // Compute the jerks ((current - prev accel) / loop time)
+ rioJerkVector = rioAccVector.minus(prevRioAccel).div(Constants.loopPeriodSecs);
+ imuJerkVector = imuAccVector.minus(prevImuAccel).div(Constants.loopPeriodSecs);
+
+ // Log everything to both AdvantageKit and SmartDashboard
+ Logger.recordOutput("Acceleration/Rio/Accel_mss", rioAccVector);
+ Logger.recordOutput("Acceleration/Rio/Jerk_msss", rioJerkVector);
+ Logger.recordOutput("Acceleration/IMU/Accel_mss", imuAccVector);
+ Logger.recordOutput("Acceleration/IMU/Jerk_msss", imuJerkVector);
+ SmartDashboard.putNumber("RioXAccel", rioAccVector.getX());
+ SmartDashboard.putNumber("RioYAccel", rioAccVector.getY());
+ SmartDashboard.putNumber("IMUXAccel", imuAccVector.getX());
+ SmartDashboard.putNumber("IMUYAccel", imuAccVector.getY());
+
+ // Set the "previous" accelerations to the current for the next loop
+ prevRioAccel = rioAccVector;
+ prevImuAccel = imuAccVector;
+
+ // Quick logging to see how long this periodic takes
+ long finish = System.nanoTime();
+ long timeElapsed = finish - start;
+ Logger.recordOutput("LoggedRobot/AccelCodeMS", (double) timeElapsed / 1.e6);
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
new file mode 100644
index 00000000..99ce6ab4
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -0,0 +1,403 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drive;
+
+import static edu.wpi.first.units.Units.*;
+import static frc.robot.subsystems.drive.DriveConstants.*;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.pathfinding.Pathfinding;
+import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
+import com.pathplanner.lib.util.PathPlannerLogging;
+import com.pathplanner.lib.util.ReplanningConfig;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.Constants;
+import frc.robot.Constants.DrivebaseConstants;
+import frc.robot.util.LocalADStarAK;
+import frc.robot.util.YagslConstants;
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+public class Drive extends SubsystemBase {
+
+ private final GyroIO gyroIO;
+ private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
+ private final Module[] modules = new Module[4]; // FL, FR, BL, BR
+ private final SysIdRoutine sysId;
+
+ private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations());
+ private Rotation2d rawGyroRotation = new Rotation2d();
+ private SwerveModulePosition[] lastModulePositions = // For delta tracking
+ new SwerveModulePosition[] {
+ new SwerveModulePosition(),
+ new SwerveModulePosition(),
+ new SwerveModulePosition(),
+ new SwerveModulePosition()
+ };
+ private SwerveDrivePoseEstimator m_PoseEstimator =
+ new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d());
+
+ // Constructor
+ public Drive() {
+ switch (Constants.getSwerveType()) {
+ case PHOENIX6:
+ // This one is easy because it's all CTRE all the time
+ gyroIO = new GyroIOPigeon2();
+ for (int i = 0; i < 4; i++) {
+ modules[i] = new Module(new ModuleIOTalonFX(i), i);
+ }
+ break;
+
+ case YAGSL:
+ // First, choose the Gyro
+ switch (YagslConstants.swerveDriveJson.imu.type) {
+ case "pigeon2":
+ gyroIO = new GyroIOPigeon2();
+ break;
+ case "navx":
+ case "navx_spi":
+ gyroIO = new GyroIONavX();
+ break;
+ default:
+ throw new RuntimeException("Invalid IMU type");
+ }
+ // Then parse the module(s)
+ Byte modType = parseModuleType();
+ for (int i = 0; i < 4; i++) {
+ switch (modType) {
+ case 0b00000000: // ALL-CTRE
+ modules[i] = new Module(new ModuleIOTalonFX(i), i);
+ break;
+ case 0b00010000: // Blended Talon Drive / NEO Steer
+ modules[i] = new Module(new ModuleIOBlended(i), i);
+ break;
+ case 0b01010000: // NEO motors + CANcoder
+ modules[i] = new Module(new ModuleIOSparkCANcoder(i), i);
+ break;
+ case 0b01010100: // NEO motors + analog encoder
+ modules[i] = new Module(new ModuleIOSparkMax(i), i);
+ break;
+ default:
+ throw new RuntimeException("Invalid swerve module combination");
+ }
+ }
+
+ default:
+ throw new RuntimeException("Invalid Swerve Drive Type");
+ }
+
+ // Configure Autonomous Path Building based on `AutoType`
+ switch (Constants.getAutoType()) {
+ case PATHPLANNER:
+ // Configure AutoBuilder for PathPlanner
+ AutoBuilder.configureHolonomic(
+ this::getPose,
+ this::setPose,
+ () -> kinematics.toChassisSpeeds(getModuleStates()),
+ this::runVelocity,
+ new HolonomicPathFollowerConfig(
+ DrivebaseConstants.kMaxLinearSpeed,
+ Units.inchesToMeters(kDriveBaseRadius),
+ new ReplanningConfig()),
+ () ->
+ DriverStation.getAlliance().isPresent()
+ && DriverStation.getAlliance().get() == Alliance.Red,
+ this);
+ Pathfinding.setPathfinder(new LocalADStarAK());
+ PathPlannerLogging.setLogActivePathCallback(
+ (activePath) -> {
+ Logger.recordOutput(
+ "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
+ });
+ PathPlannerLogging.setLogTargetPoseCallback(
+ (targetPose) -> {
+ Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
+ });
+ break;
+ case CHOREO:
+ break;
+ default:
+ }
+ // Configure SysId
+ sysId =
+ new SysIdRoutine(
+ new SysIdRoutine.Config(
+ null,
+ null,
+ null,
+ (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())),
+ new SysIdRoutine.Mechanism(
+ (voltage) -> {
+ for (int i = 0; i < 4; i++) {
+ modules[i].runCharacterization(voltage.in(Volts));
+ }
+ },
+ null,
+ this));
+ }
+
+ public void periodic() {
+ gyroIO.updateInputs(gyroInputs);
+ Logger.processInputs("Drive/Gyro", gyroInputs);
+ for (var module : modules) {
+ module.periodic();
+ }
+
+ // Stop moving when disabled
+ if (DriverStation.isDisabled()) {
+ for (var module : modules) {
+ module.stop();
+ }
+ }
+ // Log empty setpoint states when disabled
+ if (DriverStation.isDisabled()) {
+ Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {});
+ Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {});
+ }
+
+ // Read wheel positions and deltas from each module
+ SwerveModulePosition[] modulePositions = getModulePositions();
+ SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4];
+ for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) {
+ moduleDeltas[moduleIndex] =
+ new SwerveModulePosition(
+ modulePositions[moduleIndex].distanceMeters
+ - lastModulePositions[moduleIndex].distanceMeters,
+ modulePositions[moduleIndex].angle);
+ lastModulePositions[moduleIndex] = modulePositions[moduleIndex];
+ }
+
+ // Update gyro angle
+ if (gyroInputs.connected) {
+ // Use the real gyro angle
+ rawGyroRotation = gyroInputs.yawPosition;
+ } else {
+ // Use the angle delta from the kinematics and module deltas
+ Twist2d twist = kinematics.toTwist2d(moduleDeltas);
+ rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta));
+ }
+
+ // Apply odometry update
+ m_PoseEstimator.update(rawGyroRotation, modulePositions);
+ }
+
+ /**
+ * Sets the swerve drive motors to brake/coast mode.
+ *
+ * @param brake True to set motors to brake mode, false for coast.
+ */
+ public void setMotorBrake(boolean brake) {
+ {
+ for (Module swerveModule : modules) {
+ swerveModule.setBrakeMode(brake);
+ }
+ }
+ }
+
+ /**
+ * Runs the drive at the desired velocity.
+ *
+ * @param speeds Speeds in meters/sec
+ */
+ public void runVelocity(ChassisSpeeds speeds) {
+ // Calculate module setpoints
+ ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02);
+ SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);
+ SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DrivebaseConstants.kMaxLinearSpeed);
+
+ // Send setpoints to modules
+ SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4];
+ for (int i = 0; i < 4; i++) {
+ // The module returns the optimized state, useful for logging
+ optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]);
+ }
+
+ // Log setpoint states
+ Logger.recordOutput("SwerveStates/Setpoints", setpointStates);
+ Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates);
+ }
+
+ /** Re-zero the gyro at the present heading */
+ public void zero() {
+ gyroIO.zero();
+ }
+
+ /** Stops the drive. */
+ public void stop() {
+ runVelocity(new ChassisSpeeds());
+ }
+
+ /**
+ * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will
+ * return to their normal orientations the next time a nonzero velocity is requested.
+ */
+ public void stopWithX() {
+ Rotation2d[] headings = new Rotation2d[4];
+ for (int i = 0; i < 4; i++) {
+ headings[i] = getModuleTranslations()[i].getAngle();
+ }
+ kinematics.resetHeadings(headings);
+ stop();
+ }
+
+ /** Returns a command to run a quasistatic test in the specified direction. */
+ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+ return sysId.quasistatic(direction);
+ }
+
+ /** Returns a command to run a dynamic test in the specified direction. */
+ public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+ return sysId.dynamic(direction);
+ }
+
+ /** Returns the module states (turn angles and drive velocities) for all of the modules. */
+ @AutoLogOutput(key = "SwerveStates/Measured")
+ private SwerveModuleState[] getModuleStates() {
+ SwerveModuleState[] states = new SwerveModuleState[4];
+ for (int i = 0; i < 4; i++) {
+ states[i] = modules[i].getState();
+ }
+ return states;
+ }
+
+ /** Returns the module positions (turn angles and drive positions) for all of the modules. */
+ private SwerveModulePosition[] getModulePositions() {
+ SwerveModulePosition[] states = new SwerveModulePosition[4];
+ for (int i = 0; i < 4; i++) {
+ states[i] = modules[i].getPosition();
+ }
+ return states;
+ }
+
+ /** Returns the current odometry pose. */
+ @AutoLogOutput(key = "Odometry/Robot")
+ public Pose2d getPose() {
+ return m_PoseEstimator.getEstimatedPosition();
+ }
+
+ /** Returns the current odometry rotation. */
+ public Rotation2d getRotation() {
+ return getPose().getRotation();
+ }
+
+ /** Resets the current odometry pose. */
+ public void setPose(Pose2d pose) {
+ m_PoseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose);
+ }
+
+ /**
+ * Adds a vision measurement to the pose estimator.
+ *
+ * @param visionPose The pose of the robot as measured by the vision camera.
+ * @param timestamp The timestamp of the vision measurement in seconds.
+ */
+ public void addVisionMeasurement(Pose2d visionPose, double timestamp) {
+ m_PoseEstimator.addVisionMeasurement(visionPose, timestamp);
+ }
+
+ /** Returns the maximum linear speed in meters per sec. */
+ public double getMaxLinearSpeedMetersPerSec() {
+ return DrivebaseConstants.kMaxLinearSpeed;
+ }
+
+ /** Returns the maximum angular speed in radians per sec. */
+ public double getMaxAngularSpeedRadPerSec() {
+ return DrivebaseConstants.kMaxAngularSpeed;
+ }
+
+ /** Returns an array of module translations. */
+ public static Translation2d[] getModuleTranslations() {
+ return new Translation2d[] {
+ new Translation2d(DriveConstants.kFrontLeftXPosInches, DriveConstants.kFrontLeftYPosInches),
+ new Translation2d(DriveConstants.kFrontRightXPosInches, DriveConstants.kFrontRightYPosInches),
+ new Translation2d(DriveConstants.kBackLeftXPosInches, DriveConstants.kBackLeftYPosInches),
+ new Translation2d(DriveConstants.kBackRightXPosInches, DriveConstants.kBackRightYPosInches)
+ };
+ }
+
+ public Object getGyro() {
+ return gyroIO.getGyro();
+ }
+
+ /**
+ * Parse the module type given the type information for the FL module
+ *
+ * @return Byte The module type as bits in a byte.
+ */
+ private Byte parseModuleType() {
+ // NOTE: This assumes all 4 modules have the same arrangement!
+ Byte b_drive; // [x,x,-,-,-,-,-,-]
+ Byte b_steer; // [-,-,x,x,-,-,-,-]
+ Byte b_encoder; // [-,-,-,-,x,x,-,-]
+ switch (kFrontLeftDriveType) {
+ case "falcon":
+ case "kraken":
+ case "talonfx":
+ // CTRE Drive Motor
+ b_drive = 0b00;
+ break;
+ case "sparkmax":
+ case "sparkflex":
+ // REV Drive Motor
+ b_drive = 0b01;
+ break;
+ default:
+ throw new RuntimeException("Invalid drive motor type");
+ }
+ switch (kFrontLeftSteerType) {
+ case "falcon":
+ case "kraken":
+ case "talonfx":
+ // CTRE Drive Motor
+ b_steer = 0b00;
+ break;
+ case "sparkmax":
+ case "sparkflex":
+ // REV Drive Motor
+ b_steer = 0b01;
+ break;
+ default:
+ throw new RuntimeException("Invalid steer motor type");
+ }
+ switch (kFrontLeftEncoderType) {
+ case "cancoder":
+ // CTRE CANcoder
+ b_encoder = 0b00;
+ break;
+ case "analog":
+ // Analog Encoder
+ b_encoder = 0b01;
+ break;
+ default:
+ throw new RuntimeException("Invalid swerve encoder type");
+ }
+ return (byte) (0b00000000 | b_drive << 6 | b_steer << 4 | b_encoder << 2);
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
new file mode 100644
index 00000000..d7f9463e
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -0,0 +1,302 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drive;
+
+import edu.wpi.first.math.util.Units;
+import frc.robot.Constants;
+import frc.robot.generated.TunerConstants;
+import frc.robot.util.YagslConstants;
+
+/**
+ * Holds the proper set of drive constants given the type of drive
+ *
+ *
Does some translation of how the two keep values to return a completely unified API. This file
+ * should not be modified.
+ */
+public class DriveConstants {
+
+ // Declare all the constants
+ public static final double kCoupleRatio;
+ public static final double kDriveGearRatio;
+ public static final double kSteerGearRatio;
+ public static final double kWheelRadiusInches;
+ public static final String kCANbusName;
+ public static final int kPigeonId;
+ public static final double kSteerInertia;
+ public static final double kDriveInertia;
+ public static final double kSteerFrictionVoltage;
+ public static final double kDriveFrictionVoltage;
+ public static final double kSteerCurrentLimit;
+ public static final double kDriveCurrentLimit;
+ public static final double kOptimalVoltage;
+ public static final int kFrontLeftDriveMotorId;
+ public static final int kFrontLeftSteerMotorId;
+ public static final int kFrontLeftEncoderId;
+ public static final String kFrontLeftDriveCanbus;
+ public static final String kFrontLeftSteerCanbus;
+ public static final String kFrontLeftEncoderCanbus;
+ public static final String kFrontLeftDriveType;
+ public static final String kFrontLeftSteerType;
+ public static final String kFrontLeftEncoderType;
+ public static final double kFrontLeftEncoderOffset; // In Radians
+ public static final boolean kFrontLeftDriveInvert;
+ public static final boolean kFrontLeftSteerInvert;
+ public static final boolean kFrontLeftEncoderInvert;
+ public static final double kFrontLeftXPosInches;
+ public static final double kFrontLeftYPosInches;
+ public static final int kFrontRightDriveMotorId;
+ public static final int kFrontRightSteerMotorId;
+ public static final int kFrontRightEncoderId;
+ public static final String kFrontRightDriveCanbus;
+ public static final String kFrontRightSteerCanbus;
+ public static final String kFrontRightEncoderCanbus;
+ public static final String kFrontRightDriveType;
+ public static final String kFrontRightSteerType;
+ public static final String kFrontRightEncoderType;
+ public static final double kFrontRightEncoderOffset; // In Radians
+ public static final boolean kFrontRightDriveInvert;
+ public static final boolean kFrontRightSteerInvert;
+ public static final boolean kFrontRightEncoderInvert;
+ public static final double kFrontRightXPosInches;
+ public static final double kFrontRightYPosInches;
+ public static final int kBackLeftDriveMotorId;
+ public static final int kBackLeftSteerMotorId;
+ public static final int kBackLeftEncoderId;
+ public static final String kBackLeftDriveCanbus;
+ public static final String kBackLeftSteerCanbus;
+ public static final String kBackLeftEncoderCanbus;
+ public static final String kBackLeftDriveType;
+ public static final String kBackLeftSteerType;
+ public static final String kBackLeftEncoderType;
+ public static final double kBackLeftEncoderOffset; // In Radians
+ public static final boolean kBackLeftDriveInvert;
+ public static final boolean kBackLeftSteerInvert;
+ public static final boolean kBackLeftEncoderInvert;
+ public static final double kBackLeftXPosInches;
+ public static final double kBackLeftYPosInches;
+ public static final int kBackRightDriveMotorId;
+ public static final int kBackRightSteerMotorId;
+ public static final int kBackRightEncoderId;
+ public static final String kBackRightDriveCanbus;
+ public static final String kBackRightSteerCanbus;
+ public static final String kBackRightEncoderCanbus;
+ public static final String kBackRightDriveType;
+ public static final String kBackRightSteerType;
+ public static final String kBackRightEncoderType;
+ public static final double kBackRightEncoderOffset; // In Radians
+ public static final boolean kBackRightDriveInvert;
+ public static final boolean kBackRightSteerInvert;
+ public static final boolean kBackRightEncoderInvert;
+ public static final double kBackRightXPosInches;
+ public static final double kBackRightYPosInches;
+ public static final double kDriveP;
+ public static final double kDriveI;
+ public static final double kDriveD;
+ public static final double kDriveF;
+ public static final double kDriveIZ;
+ public static final double kSteerP;
+ public static final double kSteerI;
+ public static final double kSteerD;
+ public static final double kSteerF;
+ public static final double kSteerIZ;
+
+ // Fill in the values from the proper source
+ static {
+ switch (Constants.getSwerveType()) {
+ case PHOENIX6:
+ kCoupleRatio = TunerConstants.kCoupleRatio;
+ kDriveGearRatio = TunerConstants.kDriveGearRatio;
+ kSteerGearRatio = TunerConstants.kSteerGearRatio;
+ kWheelRadiusInches = TunerConstants.kWheelRadiusInches;
+ kCANbusName = TunerConstants.kCANbusName;
+ kPigeonId = TunerConstants.kPigeonId;
+ kSteerInertia = TunerConstants.kSteerInertia;
+ kDriveInertia = TunerConstants.kDriveInertia;
+ kSteerFrictionVoltage = 0.0;
+ kDriveFrictionVoltage = 0.1;
+ kSteerCurrentLimit = 40.0; // Example from CTRE documentation
+ kDriveCurrentLimit = 120.0; // Example from CTRE documentation
+ kOptimalVoltage = 12.0; // Assumed Ideal
+ kFrontLeftDriveMotorId = TunerConstants.kFrontLeftDriveMotorId;
+ kFrontLeftSteerMotorId = TunerConstants.kFrontLeftSteerMotorId;
+ kFrontLeftEncoderId = TunerConstants.kFrontLeftEncoderId;
+ kFrontLeftDriveCanbus = TunerConstants.kCANbusName;
+ kFrontLeftSteerCanbus = TunerConstants.kCANbusName;
+ kFrontLeftEncoderCanbus = TunerConstants.kCANbusName;
+ kFrontLeftDriveType = "kraken";
+ kFrontLeftSteerType = "kraken";
+ kFrontLeftEncoderType = "cancoder";
+ kFrontLeftEncoderOffset =
+ -Units.rotationsToRadians(TunerConstants.kFrontLeftEncoderOffset) + Math.PI;
+ kFrontLeftDriveInvert = TunerConstants.kInvertLeftSide;
+ kFrontLeftSteerInvert = TunerConstants.kFrontLeftSteerInvert;
+ kFrontLeftEncoderInvert = false;
+ kFrontLeftXPosInches = TunerConstants.kFrontLeftXPosInches;
+ kFrontLeftYPosInches = TunerConstants.kFrontLeftYPosInches;
+ kFrontRightDriveMotorId = TunerConstants.kFrontRightDriveMotorId;
+ kFrontRightSteerMotorId = TunerConstants.kFrontRightSteerMotorId;
+ kFrontRightEncoderId = TunerConstants.kFrontRightEncoderId;
+ kFrontRightDriveCanbus = TunerConstants.kCANbusName;
+ kFrontRightSteerCanbus = TunerConstants.kCANbusName;
+ kFrontRightEncoderCanbus = TunerConstants.kCANbusName;
+ kFrontRightDriveType = "kraken";
+ kFrontRightSteerType = "kraken";
+ kFrontRightEncoderType = "cancoder";
+ kFrontRightEncoderOffset =
+ -Units.rotationsToRadians(TunerConstants.kFrontRightEncoderOffset);
+ kFrontRightDriveInvert = TunerConstants.kInvertRightSide;
+ kFrontRightSteerInvert = TunerConstants.kFrontRightSteerInvert;
+ kFrontRightEncoderInvert = false;
+ kFrontRightXPosInches = TunerConstants.kFrontRightXPosInches;
+ kFrontRightYPosInches = TunerConstants.kFrontRightYPosInches;
+ kBackLeftDriveMotorId = TunerConstants.kBackLeftDriveMotorId;
+ kBackLeftSteerMotorId = TunerConstants.kBackLeftSteerMotorId;
+ kBackLeftEncoderId = TunerConstants.kBackLeftEncoderId;
+ kBackLeftDriveCanbus = TunerConstants.kCANbusName;
+ kBackLeftSteerCanbus = TunerConstants.kCANbusName;
+ kBackLeftEncoderCanbus = TunerConstants.kCANbusName;
+ kBackLeftDriveType = "kraken";
+ kBackLeftSteerType = "kraken";
+ kBackLeftEncoderType = "cancoder";
+ kBackLeftEncoderOffset =
+ -Units.rotationsToRadians(TunerConstants.kBackLeftEncoderOffset) + Math.PI;
+ kBackLeftDriveInvert = TunerConstants.kInvertLeftSide;
+ kBackLeftSteerInvert = TunerConstants.kBackLeftSteerInvert;
+ kBackLeftEncoderInvert = false;
+ kBackLeftXPosInches = TunerConstants.kBackLeftXPosInches;
+ kBackLeftYPosInches = TunerConstants.kBackLeftYPosInches;
+ kBackRightDriveMotorId = TunerConstants.kBackRightDriveMotorId;
+ kBackRightSteerMotorId = TunerConstants.kBackRightSteerMotorId;
+ kBackRightEncoderId = TunerConstants.kBackRightEncoderId;
+ kBackRightDriveCanbus = TunerConstants.kCANbusName;
+ kBackRightSteerCanbus = TunerConstants.kCANbusName;
+ kBackRightEncoderCanbus = TunerConstants.kCANbusName;
+ kBackRightDriveType = "kraken";
+ kBackRightSteerType = "kraken";
+ kBackRightEncoderType = "cancoder";
+ kBackRightEncoderOffset = -Units.rotationsToRadians(TunerConstants.kBackRightEncoderOffset);
+ kBackRightDriveInvert = TunerConstants.kInvertRightSide;
+ kBackRightSteerInvert = TunerConstants.kBackRightSteerInvert;
+ kBackRightEncoderInvert = false;
+ kBackRightXPosInches = TunerConstants.kBackRightXPosInches;
+ kBackRightYPosInches = TunerConstants.kBackRightYPosInches;
+ // NOTE: The PIDF values from TunerConstants.java make WPILib/AK implemention go crazy
+ // These values are from the AK example sketches
+ kDriveP = 0.05;
+ kDriveI = 0.0;
+ kDriveD = 0.0;
+ kDriveF = 0.13;
+ kDriveIZ = 0.0;
+ kSteerP = 7.0;
+ kSteerI = 0.0;
+ kSteerD = 0.0;
+ kSteerF = 0.0;
+ kSteerIZ = 0.0;
+ break;
+
+ case YAGSL:
+ kCoupleRatio = YagslConstants.kCoupleRatio;
+ kDriveGearRatio = YagslConstants.kDriveGearRatio;
+ kSteerGearRatio = YagslConstants.kSteerGearRatio;
+ kWheelRadiusInches = YagslConstants.kWheelRadiusInches;
+ kCANbusName = YagslConstants.kCANbusName;
+ kPigeonId = YagslConstants.kPigeonId;
+ kSteerInertia = YagslConstants.kSteerInertia;
+ kDriveInertia = YagslConstants.kDriveInertia;
+ kSteerFrictionVoltage = YagslConstants.kSteerFrictionVoltage;
+ kDriveFrictionVoltage = YagslConstants.kDriveFrictionVoltage;
+ kSteerCurrentLimit = YagslConstants.kSteerCurrentLimit;
+ kDriveCurrentLimit = YagslConstants.kDriveCurrentLimit;
+ kOptimalVoltage = YagslConstants.kOptimalVoltage;
+ kFrontLeftDriveMotorId = YagslConstants.kFrontLeftDriveMotorId;
+ kFrontLeftSteerMotorId = YagslConstants.kFrontLeftSteerMotorId;
+ kFrontLeftEncoderId = YagslConstants.kFrontLeftEncoderId;
+ kFrontLeftDriveCanbus = YagslConstants.kFrontLeftDriveCanbus;
+ kFrontLeftSteerCanbus = YagslConstants.kFrontLeftSteerCanbus;
+ kFrontLeftEncoderCanbus = YagslConstants.kFrontLeftEncoderCanbus;
+ kFrontLeftDriveType = YagslConstants.kFrontLeftDriveType.toLowerCase();
+ kFrontLeftSteerType = YagslConstants.kFrontLeftSteerType.toLowerCase();
+ kFrontLeftEncoderType = YagslConstants.kFrontLeftEncoderType.toLowerCase();
+ kFrontLeftEncoderOffset = Units.degreesToRadians(YagslConstants.kFrontLeftEncoderOffset);
+ kFrontLeftDriveInvert = YagslConstants.kFrontLeftDriveInvert;
+ kFrontLeftSteerInvert = YagslConstants.kFrontLeftSteerInvert;
+ kFrontLeftEncoderInvert = YagslConstants.kFrontLeftEncoderInvert;
+ kFrontLeftXPosInches = YagslConstants.kFrontLeftXPosInches;
+ kFrontLeftYPosInches = YagslConstants.kFrontLeftYPosInches;
+ kFrontRightDriveMotorId = YagslConstants.kFrontRightDriveMotorId;
+ kFrontRightSteerMotorId = YagslConstants.kFrontRightSteerMotorId;
+ kFrontRightEncoderId = YagslConstants.kFrontRightEncoderId;
+ kFrontRightDriveCanbus = YagslConstants.kFrontRightDriveCanbus;
+ kFrontRightSteerCanbus = YagslConstants.kFrontRightSteerCanbus;
+ kFrontRightEncoderCanbus = YagslConstants.kFrontRightEncoderCanbus;
+ kFrontRightDriveType = YagslConstants.kFrontRightDriveType.toLowerCase();
+ kFrontRightSteerType = YagslConstants.kFrontRightSteerType.toLowerCase();
+ kFrontRightEncoderType = YagslConstants.kFrontRightEncoderType.toLowerCase();
+ kFrontRightEncoderOffset = Units.degreesToRadians(YagslConstants.kFrontRightEncoderOffset);
+ kFrontRightDriveInvert = YagslConstants.kFrontRightDriveInvert;
+ kFrontRightSteerInvert = YagslConstants.kFrontRightSteerInvert;
+ kFrontRightEncoderInvert = YagslConstants.kFrontRightEncoderInvert;
+ kFrontRightXPosInches = YagslConstants.kFrontRightXPosInches;
+ kFrontRightYPosInches = YagslConstants.kFrontRightYPosInches;
+ kBackLeftDriveMotorId = YagslConstants.kBackLeftDriveMotorId;
+ kBackLeftSteerMotorId = YagslConstants.kBackLeftSteerMotorId;
+ kBackLeftEncoderId = YagslConstants.kBackLeftEncoderId;
+ kBackLeftDriveCanbus = YagslConstants.kBackLeftDriveCanbus;
+ kBackLeftSteerCanbus = YagslConstants.kBackLeftSteerCanbus;
+ kBackLeftEncoderCanbus = YagslConstants.kBackLeftEncoderCanbus;
+ kBackLeftDriveType = YagslConstants.kBackLeftDriveType.toLowerCase();
+ kBackLeftSteerType = YagslConstants.kBackLeftSteerType.toLowerCase();
+ kBackLeftEncoderType = YagslConstants.kBackLeftEncoderType.toLowerCase();
+ kBackLeftEncoderOffset = Units.degreesToRadians(YagslConstants.kBackLeftEncoderOffset);
+ kBackLeftDriveInvert = YagslConstants.kBackLeftDriveInvert;
+ kBackLeftSteerInvert = YagslConstants.kBackLeftSteerInvert;
+ kBackLeftEncoderInvert = YagslConstants.kBackLeftEncoderInvert;
+ kBackLeftXPosInches = YagslConstants.kBackLeftXPosInches;
+ kBackLeftYPosInches = YagslConstants.kBackLeftYPosInches;
+ kBackRightDriveMotorId = YagslConstants.kBackRightDriveMotorId;
+ kBackRightSteerMotorId = YagslConstants.kBackRightSteerMotorId;
+ kBackRightEncoderId = YagslConstants.kBackRightEncoderId;
+ kBackRightDriveCanbus = YagslConstants.kBackRightDriveCanbus;
+ kBackRightSteerCanbus = YagslConstants.kBackRightSteerCanbus;
+ kBackRightEncoderCanbus = YagslConstants.kBackRightEncoderCanbus;
+ kBackRightDriveType = YagslConstants.kBackRightDriveType.toLowerCase();
+ kBackRightSteerType = YagslConstants.kBackRightSteerType.toLowerCase();
+ kBackRightEncoderType = YagslConstants.kBackRightEncoderType.toLowerCase();
+ kBackRightEncoderOffset = Units.degreesToRadians(YagslConstants.kBackRightEncoderOffset);
+ kBackRightDriveInvert = YagslConstants.kBackRightDriveInvert;
+ kBackRightSteerInvert = YagslConstants.kBackRightSteerInvert;
+ kBackRightEncoderInvert = YagslConstants.kBackRightEncoderInvert;
+ kBackRightXPosInches = YagslConstants.kBackRightXPosInches;
+ kBackRightYPosInches = YagslConstants.kBackRightYPosInches;
+ kDriveP = YagslConstants.kDriveP;
+ kDriveI = YagslConstants.kDriveI;
+ kDriveD = YagslConstants.kDriveD;
+ kDriveF = YagslConstants.kDriveF;
+ kDriveIZ = YagslConstants.kDriveIZ;
+ kSteerP = YagslConstants.kSteerP;
+ kSteerI = YagslConstants.kSteerI;
+ kSteerD = YagslConstants.kSteerD;
+ kSteerF = YagslConstants.kSteerF;
+ kSteerIZ = YagslConstants.kSteerIZ;
+ break;
+
+ default:
+ throw new RuntimeException("Invalid Swerve Drive Type");
+ }
+ }
+
+ // Computed quantities
+ public static final double kDriveBaseRadius =
+ Math.hypot(kFrontLeftXPosInches, kFrontLeftYPosInches);
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java
new file mode 100644
index 00000000..8b1b9f0c
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java
@@ -0,0 +1,34 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drive;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface GyroIO {
+ @AutoLog
+ public static class GyroIOInputs {
+ public boolean connected = false;
+ public Rotation2d yawPosition = new Rotation2d();
+ public double yawVelocityRadPerSec = 0.0;
+ }
+
+ public default void updateInputs(GyroIOInputs inputs) {}
+
+ public default void zero() {}
+
+ public abstract T getGyro();
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java
new file mode 100644
index 00000000..009846aa
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java
@@ -0,0 +1,73 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drive;
+
+import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.SPI;
+
+/** IO implementation for Pigeon2 */
+public class GyroIONavX implements GyroIO {
+ private final AHRS navx;
+
+ // Constructor, taking default values
+ public GyroIONavX() {
+ navx = new AHRS(SPI.Port.kMXP, (byte) 100.0);
+ navx.reset();
+ // Set Angle Adjustment based on alliance
+ if (DriverStation.getAlliance().get() == Alliance.Blue) {
+ navx.setAngleAdjustment(0.0);
+ } else {
+ navx.setAngleAdjustment(180.0);
+ }
+ }
+
+ // Return the Pigeon2 instance
+ @Override
+ public AHRS getGyro() {
+ return navx;
+ }
+
+ @Override
+ public void updateInputs(GyroIOInputs inputs) {
+ inputs.connected = navx.isConnected();
+ inputs.yawPosition = Rotation2d.fromDegrees(navx.getYaw());
+ inputs.yawVelocityRadPerSec = Units.degreesToRadians(navx.getRate());
+ }
+
+ /**
+ * Zero the NavX
+ *
+ * This method should always rezero the pigeon in ALWAYS-BLUE-ORIGIN orientation. Testing,
+ * however, shows that it's not doing what I think it should be doing. There is likely
+ * interference with something else in the odometry
+ */
+ @Override
+ public void zero() {
+ // With the Pigeon facing forward, forward depends on the alliance selected.
+ // Set Angle Adjustment based on alliance
+ if (DriverStation.getAlliance().get() == Alliance.Blue) {
+ navx.setAngleAdjustment(0.0);
+ } else {
+ navx.setAngleAdjustment(180.0);
+ }
+ System.out.println("Setting YAW to " + navx.getAngleAdjustment());
+ navx.zeroYaw();
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
new file mode 100644
index 00000000..f95ed38d
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
@@ -0,0 +1,80 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drive;
+
+import static frc.robot.subsystems.drive.DriveConstants.*;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.Pigeon2Configuration;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+
+/** IO implementation for Pigeon2 */
+public class GyroIOPigeon2 implements GyroIO {
+ private final Pigeon2 pigeon;
+ private final StatusSignal yaw;
+ private final StatusSignal yawVelocity;
+
+ // Constructor
+ public GyroIOPigeon2() {
+ pigeon = new Pigeon2(kPigeonId, kCANbusName);
+ pigeon.getConfigurator().apply(new Pigeon2Configuration());
+ pigeon.getConfigurator().setYaw(0.0);
+ yaw = pigeon.getYaw();
+ yawVelocity = pigeon.getAngularVelocityZWorld();
+ yaw.setUpdateFrequency(100.0);
+ yawVelocity.setUpdateFrequency(100.0);
+
+ pigeon.optimizeBusUtilization();
+ }
+
+ // Return the Pigeon2 instance
+ @Override
+ public Pigeon2 getGyro() {
+ return pigeon;
+ }
+
+ @Override
+ public void updateInputs(GyroIOInputs inputs) {
+ inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK);
+ inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
+ inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());
+ }
+
+ /**
+ * Zero the Pigeon2
+ *
+ * This method should always rezero the pigeon in ALWAYS-BLUE-ORIGIN orientation. Testing,
+ * however, shows that it's not doing what I think it should be doing. There is likely
+ * interference with something else in the odometry
+ */
+ @Override
+ public void zero() {
+ // With the Pigeon facing forward, forward depends on the alliance selected.
+ if (DriverStation.getAlliance().get() == Alliance.Blue) {
+ System.out.println("Alliance Blue: Setting YAW to 0");
+ pigeon.setYaw(0.0);
+ } else {
+ System.out.println("Alliance Red: Setting YAW to 180");
+ pigeon.setYaw(180.0);
+ }
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java
new file mode 100644
index 00000000..9334b159
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/Module.java
@@ -0,0 +1,178 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drive;
+
+import static frc.robot.subsystems.drive.DriveConstants.*;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
+import frc.robot.Constants;
+import org.littletonrobotics.junction.Logger;
+
+public class Module {
+ private static final double WHEEL_RADIUS = Units.inchesToMeters(kWheelRadiusInches);
+
+ private final ModuleIO io;
+ private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
+ private final int index;
+
+ private final SimpleMotorFeedforward driveFeedforward;
+ private final PIDController driveFeedback;
+ private final PIDController turnFeedback;
+ private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop
+ private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop
+ private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute
+
+ public Module(ModuleIO io, int index) {
+ this.io = io;
+ this.index = index;
+
+ // Switch constants based on mode (the physics simulator is treated as a
+ // separate robot with different tuning)
+ switch (Constants.getMode()) {
+ case REAL:
+ case REPLAY:
+ driveFeedforward = new SimpleMotorFeedforward(kDriveFrictionVoltage, kDriveF);
+ driveFeedback = new PIDController(kDriveP, kDriveI, kDriveD);
+ turnFeedback = new PIDController(kSteerP, kSteerI, kSteerD);
+ break;
+ case SIM:
+ driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13);
+ driveFeedback = new PIDController(0.1, 0.0, 0.0);
+ turnFeedback = new PIDController(10.0, 0.0, 0.0);
+ break;
+ default:
+ driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0);
+ driveFeedback = new PIDController(0.0, 0.0, 0.0);
+ turnFeedback = new PIDController(0.0, 0.0, 0.0);
+ break;
+ }
+
+ turnFeedback.enableContinuousInput(-Math.PI, Math.PI);
+ setBrakeMode(true);
+ }
+
+ public void periodic() {
+ io.updateInputs(inputs);
+ Logger.processInputs("Drive/Module" + Integer.toString(index), inputs);
+
+ // On first cycle, reset relative turn encoder
+ // Wait until absolute angle is nonzero in case it wasn't initialized yet
+ if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) {
+ turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition);
+ }
+
+ // Run closed loop turn control
+ if (angleSetpoint != null) {
+ io.setTurnVoltage(
+ turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians()));
+
+ // Run closed loop drive control
+ // Only allowed if closed loop turn control is running
+ if (speedSetpoint != null) {
+ // Scale velocity based on turn error
+ //
+ // When the error is 90°, the velocity setpoint should be 0. As the wheel turns
+ // towards the setpoint, its velocity should increase. This is achieved by
+ // taking the component of the velocity in the direction of the setpoint.
+ double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getPositionError());
+
+ // Run drive controller
+ double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS;
+ io.setDriveVoltage(
+ driveFeedforward.calculate(velocityRadPerSec)
+ + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec));
+ }
+ }
+ }
+
+ /** Runs the module with the specified setpoint state. Returns the optimized state. */
+ public SwerveModuleState runSetpoint(SwerveModuleState state) {
+ // Optimize state based on current angle
+ // Controllers run in "periodic" when the setpoint is not null
+ var optimizedState = SwerveModuleState.optimize(state, getAngle());
+
+ // Update setpoints, controllers run in "periodic"
+ angleSetpoint = optimizedState.angle;
+ speedSetpoint = optimizedState.speedMetersPerSecond;
+
+ return optimizedState;
+ }
+
+ /** Runs the module with the specified voltage while controlling to zero degrees. */
+ public void runCharacterization(double volts) {
+ // Closed loop turn control
+ angleSetpoint = new Rotation2d();
+
+ // Open loop drive control
+ io.setDriveVoltage(volts);
+ speedSetpoint = null;
+ }
+
+ /** Disables all outputs to motors. */
+ public void stop() {
+ io.setTurnVoltage(0.0);
+ io.setDriveVoltage(0.0);
+
+ // Disable closed loop control for turn and drive
+ angleSetpoint = null;
+ speedSetpoint = null;
+ }
+
+ /** Sets whether brake mode is enabled. */
+ public void setBrakeMode(boolean enabled) {
+ io.setDriveBrakeMode(enabled);
+ io.setTurnBrakeMode(enabled);
+ }
+
+ /** Returns the current turn angle of the module. */
+ public Rotation2d getAngle() {
+ if (turnRelativeOffset == null) {
+ return new Rotation2d();
+ } else {
+ return inputs.turnPosition.plus(turnRelativeOffset);
+ }
+ }
+
+ /** Returns the current drive position of the module in meters. */
+ public double getPositionMeters() {
+ return inputs.drivePositionRad * WHEEL_RADIUS;
+ }
+
+ /** Returns the current drive velocity of the module in meters per second. */
+ public double getVelocityMetersPerSec() {
+ return inputs.driveVelocityRadPerSec * WHEEL_RADIUS;
+ }
+
+ /** Returns the module position (turn angle and drive position). */
+ public SwerveModulePosition getPosition() {
+ return new SwerveModulePosition(getPositionMeters(), getAngle());
+ }
+
+ /** Returns the module state (turn angle and drive velocity). */
+ public SwerveModuleState getState() {
+ return new SwerveModuleState(getVelocityMetersPerSec(), getAngle());
+ }
+
+ /** Returns the drive velocity in radians/sec. */
+ public double getCharacterizationVelocity() {
+ return inputs.driveVelocityRadPerSec;
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java
new file mode 100644
index 00000000..98dde745
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java
@@ -0,0 +1,50 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drive;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ModuleIO {
+ @AutoLog
+ public static class ModuleIOInputs {
+ public double drivePositionRad = 0.0;
+ public double driveVelocityRadPerSec = 0.0;
+ public double driveAppliedVolts = 0.0;
+ public double[] driveCurrentAmps = new double[] {};
+
+ public Rotation2d turnAbsolutePosition = new Rotation2d();
+ public Rotation2d turnPosition = new Rotation2d();
+ public double turnVelocityRadPerSec = 0.0;
+ public double turnAppliedVolts = 0.0;
+ public double[] turnCurrentAmps = new double[] {};
+ }
+
+ /** Updates the set of loggable inputs. */
+ public default void updateInputs(ModuleIOInputs inputs) {}
+
+ /** Run the drive motor at the specified voltage. */
+ public default void setDriveVoltage(double volts) {}
+
+ /** Run the turn motor at the specified voltage. */
+ public default void setTurnVoltage(double volts) {}
+
+ /** Enable or disable brake mode on the drive motor. */
+ public default void setDriveBrakeMode(boolean enable) {}
+
+ /** Enable or disable brake mode on the turn motor. */
+ public default void setTurnBrakeMode(boolean enable) {}
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java
new file mode 100644
index 00000000..9dfbfb05
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java
@@ -0,0 +1,196 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright 2024 FRC 2486
+// https://github.com/Coconuts2486-FRC
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drive;
+
+import static frc.robot.subsystems.drive.DriveConstants.*;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+
+/**
+ * Module IO implementation for blended TalonFX drive motor controller, SparkMax turn motor
+ * controller (NEO or NEO 550), and CANcoder.
+ *
+ *
To calibrate the absolute encoder offsets, point the modules straight (such that forward
+ * motion on the drive motor will propel the robot forward) and copy the reported values from the
+ * absolute encoders using AdvantageScope. These values are logged under
+ * "/Drive/ModuleX/TurnAbsolutePositionRad"
+ */
+public class ModuleIOBlended implements ModuleIO {
+ // CAN Devices
+ private final TalonFX driveTalon;
+ private final CANSparkMax turnSparkMax;
+ private final CANcoder cancoder;
+
+ // Drive telemetry information
+ private final StatusSignal drivePosition;
+ private final StatusSignal driveVelocity;
+ private final StatusSignal driveAppliedVolts;
+ private final StatusSignal driveCurrent;
+
+ // Steer telemetry information
+ private final StatusSignal turnAbsolutePosition;
+ private final RelativeEncoder turnRelativeEncoder;
+
+ private final boolean isTurnMotorInverted;
+ private final Rotation2d absoluteEncoderOffset;
+
+ /*
+ * Blended Module I/O, using Falcon drive and NEO steer motors
+ * Based on the ModuleIOTalonFX module, with the SparkMax components
+ * added in appropriately.
+ */
+ public ModuleIOBlended(int index) {
+ switch (index) {
+ case 0:
+ // Front Left
+ driveTalon = new TalonFX(kFrontLeftDriveMotorId, kFrontLeftDriveCanbus);
+ turnSparkMax = new CANSparkMax(kFrontLeftSteerMotorId, MotorType.kBrushless);
+ cancoder = new CANcoder(kFrontLeftEncoderId, kFrontLeftEncoderCanbus);
+ absoluteEncoderOffset = new Rotation2d(kFrontLeftEncoderOffset);
+ isTurnMotorInverted = kFrontLeftSteerInvert;
+ break;
+
+ case 1:
+ // Front Right
+ driveTalon = new TalonFX(kFrontRightDriveMotorId, kFrontRightDriveCanbus);
+ turnSparkMax = new CANSparkMax(kFrontRightSteerMotorId, MotorType.kBrushless);
+ cancoder = new CANcoder(kFrontRightEncoderId, kFrontRightEncoderCanbus);
+ absoluteEncoderOffset = new Rotation2d(kFrontRightEncoderOffset);
+ isTurnMotorInverted = kFrontRightSteerInvert;
+ break;
+
+ case 2:
+ // Back Left
+ driveTalon = new TalonFX(kBackLeftDriveMotorId, kBackLeftDriveCanbus);
+ turnSparkMax = new CANSparkMax(kBackLeftSteerMotorId, MotorType.kBrushless);
+ cancoder = new CANcoder(kBackLeftEncoderId, kBackLeftEncoderCanbus);
+ absoluteEncoderOffset = new Rotation2d(kBackLeftEncoderOffset);
+ isTurnMotorInverted = kBackLeftSteerInvert;
+ break;
+
+ case 3:
+ // Back Right
+ driveTalon = new TalonFX(kBackRightDriveMotorId, kBackRightDriveCanbus);
+ turnSparkMax = new CANSparkMax(kBackRightSteerMotorId, MotorType.kBrushless);
+ cancoder = new CANcoder(kBackRightEncoderId, kBackRightEncoderCanbus);
+ absoluteEncoderOffset = new Rotation2d(kBackRightEncoderOffset);
+ isTurnMotorInverted = kBackRightSteerInvert;
+ break;
+
+ default:
+ throw new RuntimeException("Invalid module index");
+ }
+
+ // Drive Configuration
+ var driveConfig = new TalonFXConfiguration();
+ driveConfig.CurrentLimits.StatorCurrentLimit = kDriveCurrentLimit;
+ driveConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+ driveConfig.CurrentLimits.SupplyCurrentLimit = kDriveCurrentLimit * 0.6;
+ driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+ driveTalon.getConfigurator().apply(driveConfig);
+ setDriveBrakeMode(true);
+
+ // Steer Configuration
+ turnSparkMax.restoreFactoryDefaults();
+ turnSparkMax.setCANTimeout(250);
+ turnRelativeEncoder = turnSparkMax.getEncoder();
+ turnSparkMax.setInverted(isTurnMotorInverted);
+ turnSparkMax.setSmartCurrentLimit((int) kSteerCurrentLimit);
+ turnSparkMax.enableVoltageCompensation(kOptimalVoltage);
+ turnRelativeEncoder.setPosition(0.0);
+ turnRelativeEncoder.setMeasurementPeriod(10);
+ turnRelativeEncoder.setAverageDepth(2);
+ turnSparkMax.setCANTimeout(0);
+
+ // CANCoder Configuration
+ cancoder.getConfigurator().apply(new CANcoderConfiguration());
+
+ drivePosition = driveTalon.getPosition();
+ driveVelocity = driveTalon.getVelocity();
+ driveAppliedVolts = driveTalon.getMotorVoltage();
+ driveCurrent = driveTalon.getSupplyCurrent();
+ turnAbsolutePosition = cancoder.getAbsolutePosition();
+
+ BaseStatusSignal.setUpdateFrequencyForAll(
+ 100.0, drivePosition); // Required for odometry, use faster rate
+ BaseStatusSignal.setUpdateFrequencyForAll(
+ 50.0, driveVelocity, driveAppliedVolts, driveCurrent, turnAbsolutePosition);
+ driveTalon.optimizeBusUtilization();
+ turnSparkMax.burnFlash();
+ }
+
+ @Override
+ public void updateInputs(ModuleIOInputs inputs) {
+ BaseStatusSignal.refreshAll(
+ drivePosition, driveVelocity, driveAppliedVolts, driveCurrent, turnAbsolutePosition);
+
+ inputs.drivePositionRad =
+ Units.rotationsToRadians(drivePosition.getValueAsDouble()) / kDriveGearRatio;
+ inputs.driveVelocityRadPerSec =
+ Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / kDriveGearRatio;
+ inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
+ inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()};
+
+ inputs.turnAbsolutePosition =
+ Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble())
+ .minus(absoluteEncoderOffset);
+ inputs.turnPosition =
+ Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / kSteerGearRatio);
+ inputs.turnVelocityRadPerSec =
+ Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity())
+ / kSteerGearRatio;
+ inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage();
+ inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()};
+ }
+
+ @Override
+ public void setDriveVoltage(double volts) {
+ driveTalon.setControl(new VoltageOut(volts));
+ }
+
+ @Override
+ public void setTurnVoltage(double volts) {
+ turnSparkMax.setVoltage(volts);
+ }
+
+ @Override
+ public void setDriveBrakeMode(boolean enable) {
+ var config = new MotorOutputConfigs();
+ config.Inverted = InvertedValue.CounterClockwise_Positive;
+ config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast;
+ driveTalon.getConfigurator().apply(config);
+ }
+
+ @Override
+ public void setTurnBrakeMode(boolean enable) {
+ turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast);
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java
new file mode 100644
index 00000000..39cc551a
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java
@@ -0,0 +1,69 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drive;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+
+/**
+ * Physics sim implementation of module IO.
+ *
+ * Uses two flywheel sims for the drive and turn motors, with the absolute position initialized
+ * to a random value. The flywheel sims are not physically accurate, but provide a decent
+ * approximation for the behavior of the module.
+ */
+public class ModuleIOSim implements ModuleIO {
+ private static final double LOOP_PERIOD_SECS = 0.02;
+
+ private DCMotorSim driveSim = new DCMotorSim(DCMotor.getNEO(1), 6.75, 0.025);
+ private DCMotorSim turnSim = new DCMotorSim(DCMotor.getNEO(1), 150.0 / 7.0, 0.004);
+
+ private final Rotation2d turnAbsoluteInitPosition = new Rotation2d(Math.random() * 2.0 * Math.PI);
+ private double driveAppliedVolts = 0.0;
+ private double turnAppliedVolts = 0.0;
+
+ @Override
+ public void updateInputs(ModuleIOInputs inputs) {
+ driveSim.update(LOOP_PERIOD_SECS);
+ turnSim.update(LOOP_PERIOD_SECS);
+
+ inputs.drivePositionRad = driveSim.getAngularPositionRad();
+ inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec();
+ inputs.driveAppliedVolts = driveAppliedVolts;
+ inputs.driveCurrentAmps = new double[] {Math.abs(driveSim.getCurrentDrawAmps())};
+
+ inputs.turnAbsolutePosition =
+ new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition);
+ inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad());
+ inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec();
+ inputs.turnAppliedVolts = turnAppliedVolts;
+ inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())};
+ }
+
+ @Override
+ public void setDriveVoltage(double volts) {
+ driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0);
+ driveSim.setInputVoltage(driveAppliedVolts);
+ }
+
+ @Override
+ public void setTurnVoltage(double volts) {
+ turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0);
+ turnSim.setInputVoltage(turnAppliedVolts);
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java
new file mode 100644
index 00000000..557c7c88
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java
@@ -0,0 +1,170 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drive;
+
+import static frc.robot.subsystems.drive.DriveConstants.*;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+
+/**
+ * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO
+ * or NEO 550), and CTRE's CANcoder.
+ *
+ *
To calibrate the absolute encoder offsets, point the modules straight (such that forward
+ * motion on the drive motor will propel the robot forward) and copy the reported values from the
+ * absolute encoders using AdvantageScope. These values are logged under
+ * "/Drive/ModuleX/TurnAbsolutePositionRad"
+ */
+public class ModuleIOSparkCANcoder implements ModuleIO {
+ private final CANSparkMax driveSparkMax;
+ private final CANSparkMax turnSparkMax;
+
+ private final RelativeEncoder driveEncoder;
+ private final RelativeEncoder turnRelativeEncoder;
+ private final CANcoder cancoder;
+ private final StatusSignal turnAbsolutePosition;
+
+ private final boolean isTurnMotorInverted;
+ private final Rotation2d absoluteEncoderOffset;
+
+ public ModuleIOSparkCANcoder(int index) {
+ switch (index) {
+ case 0:
+ // Front Left
+ driveSparkMax = new CANSparkMax(kFrontLeftDriveMotorId, MotorType.kBrushless);
+ turnSparkMax = new CANSparkMax(kFrontLeftSteerMotorId, MotorType.kBrushless);
+ cancoder = new CANcoder(kFrontLeftEncoderId, kFrontLeftEncoderCanbus);
+ absoluteEncoderOffset = new Rotation2d(kFrontLeftEncoderOffset);
+ isTurnMotorInverted = kFrontLeftSteerInvert;
+ break;
+
+ case 1:
+ // Front Right
+ driveSparkMax = new CANSparkMax(kFrontRightDriveMotorId, MotorType.kBrushless);
+ turnSparkMax = new CANSparkMax(kFrontRightSteerMotorId, MotorType.kBrushless);
+ cancoder = new CANcoder(kFrontRightEncoderId, kFrontRightEncoderCanbus);
+ absoluteEncoderOffset = new Rotation2d(kFrontRightEncoderOffset);
+ isTurnMotorInverted = kFrontRightSteerInvert;
+ break;
+
+ case 2:
+ // Back Left
+ driveSparkMax = new CANSparkMax(kBackLeftDriveMotorId, MotorType.kBrushless);
+ turnSparkMax = new CANSparkMax(kBackLeftSteerMotorId, MotorType.kBrushless);
+ cancoder = new CANcoder(kBackLeftEncoderId, kBackLeftEncoderCanbus);
+ absoluteEncoderOffset = new Rotation2d(kBackLeftEncoderOffset);
+ isTurnMotorInverted = kBackLeftSteerInvert;
+ break;
+
+ case 3:
+ // Back Right
+ driveSparkMax = new CANSparkMax(kBackRightDriveMotorId, MotorType.kBrushless);
+ turnSparkMax = new CANSparkMax(kBackRightSteerMotorId, MotorType.kBrushless);
+ cancoder = new CANcoder(kBackRightEncoderId, kBackRightEncoderCanbus);
+ absoluteEncoderOffset = new Rotation2d(kBackRightEncoderOffset);
+ isTurnMotorInverted = kBackRightSteerInvert;
+ break;
+
+ default:
+ throw new RuntimeException("Invalid module index");
+ }
+
+ driveSparkMax.restoreFactoryDefaults();
+ turnSparkMax.restoreFactoryDefaults();
+
+ driveSparkMax.setCANTimeout(250);
+ turnSparkMax.setCANTimeout(250);
+
+ driveEncoder = driveSparkMax.getEncoder();
+ turnRelativeEncoder = turnSparkMax.getEncoder();
+
+ turnSparkMax.setInverted(isTurnMotorInverted);
+ driveSparkMax.setSmartCurrentLimit((int) kDriveCurrentLimit);
+ turnSparkMax.setSmartCurrentLimit((int) kSteerCurrentLimit);
+ driveSparkMax.enableVoltageCompensation(kOptimalVoltage);
+ turnSparkMax.enableVoltageCompensation(12.0);
+
+ cancoder.getConfigurator().apply(new CANcoderConfiguration());
+
+ driveEncoder.setPosition(0.0);
+ driveEncoder.setMeasurementPeriod(10);
+ driveEncoder.setAverageDepth(2);
+
+ turnRelativeEncoder.setPosition(0.0);
+ turnRelativeEncoder.setMeasurementPeriod(10);
+ turnRelativeEncoder.setAverageDepth(2);
+
+ turnAbsolutePosition = cancoder.getAbsolutePosition();
+ BaseStatusSignal.setUpdateFrequencyForAll(50.0, turnAbsolutePosition);
+ driveSparkMax.setCANTimeout(0);
+ turnSparkMax.setCANTimeout(0);
+
+ driveSparkMax.burnFlash();
+ turnSparkMax.burnFlash();
+ }
+
+ @Override
+ public void updateInputs(ModuleIOInputs inputs) {
+ BaseStatusSignal.refreshAll(turnAbsolutePosition);
+
+ inputs.drivePositionRad =
+ Units.rotationsToRadians(driveEncoder.getPosition()) / kDriveGearRatio;
+ inputs.driveVelocityRadPerSec =
+ Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / kDriveGearRatio;
+ inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage();
+ inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()};
+
+ inputs.turnAbsolutePosition =
+ Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble())
+ .minus(absoluteEncoderOffset);
+ inputs.turnPosition =
+ Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / kSteerGearRatio);
+ inputs.turnVelocityRadPerSec =
+ Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity())
+ / kSteerGearRatio;
+ inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage();
+ inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()};
+ }
+
+ @Override
+ public void setDriveVoltage(double volts) {
+ driveSparkMax.setVoltage(volts);
+ }
+
+ @Override
+ public void setTurnVoltage(double volts) {
+ turnSparkMax.setVoltage(volts);
+ }
+
+ @Override
+ public void setDriveBrakeMode(boolean enable) {
+ driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast);
+ }
+
+ @Override
+ public void setTurnBrakeMode(boolean enable) {
+ turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast);
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java
new file mode 100644
index 00000000..a64643b7
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java
@@ -0,0 +1,165 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drive;
+
+import static frc.robot.subsystems.drive.DriveConstants.*;
+
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.RobotController;
+
+/**
+ * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO
+ * or NEO 550), and analog absolute encoder connected to the RIO
+ *
+ * NOTE: This implementation should be used as a starting point and adapted to different hardware
+ * configurations (e.g. If using a CANcoder, copy from "ModuleIOTalonFX")
+ *
+ *
To calibrate the absolute encoder offsets, point the modules straight (such that forward
+ * motion on the drive motor will propel the robot forward) and copy the reported values from the
+ * absolute encoders using AdvantageScope. These values are logged under
+ * "/Drive/ModuleX/TurnAbsolutePositionRad"
+ */
+public class ModuleIOSparkMax implements ModuleIO {
+ private final CANSparkMax driveSparkMax;
+ private final CANSparkMax turnSparkMax;
+
+ private final RelativeEncoder driveEncoder;
+ private final RelativeEncoder turnRelativeEncoder;
+ private final AnalogInput turnAbsoluteEncoder;
+
+ private final boolean isTurnMotorInverted;
+ private final Rotation2d absoluteEncoderOffset;
+
+ public ModuleIOSparkMax(int index) {
+ switch (index) {
+ case 0:
+ // Front Left
+ driveSparkMax = new CANSparkMax(kFrontLeftDriveMotorId, MotorType.kBrushless);
+ turnSparkMax = new CANSparkMax(kFrontLeftSteerMotorId, MotorType.kBrushless);
+ turnAbsoluteEncoder = new AnalogInput(kFrontLeftEncoderId);
+ absoluteEncoderOffset = new Rotation2d(kFrontLeftEncoderOffset);
+ isTurnMotorInverted = kFrontLeftSteerInvert;
+ break;
+
+ case 1:
+ // Front Right
+ driveSparkMax = new CANSparkMax(kFrontRightDriveMotorId, MotorType.kBrushless);
+ turnSparkMax = new CANSparkMax(kFrontRightSteerMotorId, MotorType.kBrushless);
+ turnAbsoluteEncoder = new AnalogInput(kFrontRightEncoderId);
+ absoluteEncoderOffset = new Rotation2d(kFrontRightEncoderOffset);
+ isTurnMotorInverted = kFrontRightSteerInvert;
+ break;
+
+ case 2:
+ // Back Left
+ driveSparkMax = new CANSparkMax(kBackLeftDriveMotorId, MotorType.kBrushless);
+ turnSparkMax = new CANSparkMax(kBackLeftSteerMotorId, MotorType.kBrushless);
+ turnAbsoluteEncoder = new AnalogInput(kBackLeftEncoderId);
+ absoluteEncoderOffset = new Rotation2d(kBackLeftEncoderOffset);
+ isTurnMotorInverted = kBackLeftSteerInvert;
+ break;
+
+ case 3:
+ // Back Right
+ driveSparkMax = new CANSparkMax(kBackRightDriveMotorId, MotorType.kBrushless);
+ turnSparkMax = new CANSparkMax(kBackRightSteerMotorId, MotorType.kBrushless);
+ turnAbsoluteEncoder = new AnalogInput(kBackRightEncoderId);
+ absoluteEncoderOffset = new Rotation2d(kBackRightEncoderOffset);
+ isTurnMotorInverted = kBackRightSteerInvert;
+ break;
+
+ default:
+ throw new RuntimeException("Invalid module index");
+ }
+
+ driveSparkMax.restoreFactoryDefaults();
+ turnSparkMax.restoreFactoryDefaults();
+
+ driveSparkMax.setCANTimeout(250);
+ turnSparkMax.setCANTimeout(250);
+
+ driveEncoder = driveSparkMax.getEncoder();
+ turnRelativeEncoder = turnSparkMax.getEncoder();
+
+ turnSparkMax.setInverted(isTurnMotorInverted);
+ driveSparkMax.setSmartCurrentLimit((int) kDriveCurrentLimit);
+ turnSparkMax.setSmartCurrentLimit((int) kSteerCurrentLimit);
+ driveSparkMax.enableVoltageCompensation(kOptimalVoltage);
+ turnSparkMax.enableVoltageCompensation(12.0);
+
+ driveEncoder.setPosition(0.0);
+ driveEncoder.setMeasurementPeriod(10);
+ driveEncoder.setAverageDepth(2);
+
+ turnRelativeEncoder.setPosition(0.0);
+ turnRelativeEncoder.setMeasurementPeriod(10);
+ turnRelativeEncoder.setAverageDepth(2);
+
+ driveSparkMax.setCANTimeout(0);
+ turnSparkMax.setCANTimeout(0);
+
+ driveSparkMax.burnFlash();
+ turnSparkMax.burnFlash();
+ }
+
+ @Override
+ public void updateInputs(ModuleIOInputs inputs) {
+ inputs.drivePositionRad =
+ Units.rotationsToRadians(driveEncoder.getPosition()) / kDriveGearRatio;
+ inputs.driveVelocityRadPerSec =
+ Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / kDriveGearRatio;
+ inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage();
+ inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()};
+
+ inputs.turnAbsolutePosition =
+ new Rotation2d(
+ turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V() * 2.0 * Math.PI)
+ .minus(absoluteEncoderOffset);
+ inputs.turnPosition =
+ Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / kSteerGearRatio);
+ inputs.turnVelocityRadPerSec =
+ Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity())
+ / kSteerGearRatio;
+ inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage();
+ inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()};
+ }
+
+ @Override
+ public void setDriveVoltage(double volts) {
+ driveSparkMax.setVoltage(volts);
+ }
+
+ @Override
+ public void setTurnVoltage(double volts) {
+ turnSparkMax.setVoltage(volts);
+ }
+
+ @Override
+ public void setDriveBrakeMode(boolean enable) {
+ driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast);
+ }
+
+ @Override
+ public void setTurnBrakeMode(boolean enable) {
+ turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast);
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
new file mode 100644
index 00000000..b6180dfa
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
@@ -0,0 +1,209 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drive;
+
+import static frc.robot.subsystems.drive.DriveConstants.*;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+
+/**
+ * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and
+ * CANcoder
+ *
+ *
NOTE: This implementation should be used as a starting point and adapted to different hardware
+ * configurations (e.g. If using an analog encoder, copy from "ModuleIOSparkMax")
+ *
+ *
To calibrate the absolute encoder offsets, point the modules straight (such that forward
+ * motion on the drive motor will propel the robot forward) and copy the reported values from the
+ * absolute encoders using AdvantageScope. These values are logged under
+ * "/Drive/ModuleX/TurnAbsolutePositionRad"
+ */
+public class ModuleIOTalonFX implements ModuleIO {
+ private final TalonFX driveTalon;
+ private final TalonFX turnTalon;
+ private final CANcoder cancoder;
+
+ private final StatusSignal drivePosition;
+ private final StatusSignal driveVelocity;
+ private final StatusSignal driveAppliedVolts;
+ private final StatusSignal driveCurrent;
+
+ private final StatusSignal turnAbsolutePosition;
+ private final StatusSignal turnPosition;
+ private final StatusSignal turnVelocity;
+ private final StatusSignal turnAppliedVolts;
+ private final StatusSignal turnCurrent;
+
+ private final boolean isTurnMotorInverted;
+ private final Rotation2d absoluteEncoderOffset;
+
+ public ModuleIOTalonFX(int index) {
+ switch (index) {
+ case 0:
+ // Front Left
+ driveTalon = new TalonFX(kFrontLeftDriveMotorId, kFrontLeftDriveCanbus);
+ turnTalon = new TalonFX(kFrontLeftSteerMotorId, kFrontLeftSteerCanbus);
+ cancoder = new CANcoder(kFrontLeftEncoderId, kFrontLeftEncoderCanbus);
+ absoluteEncoderOffset = new Rotation2d(kFrontLeftEncoderOffset);
+ isTurnMotorInverted = kFrontLeftSteerInvert;
+ break;
+
+ case 1:
+ // Front Right
+ driveTalon = new TalonFX(kFrontRightDriveMotorId, kFrontRightDriveCanbus);
+ turnTalon = new TalonFX(kFrontRightSteerMotorId, kFrontRightSteerCanbus);
+ cancoder = new CANcoder(kFrontRightEncoderId, kFrontRightEncoderCanbus);
+ absoluteEncoderOffset = new Rotation2d(kFrontRightEncoderOffset);
+ isTurnMotorInverted = kFrontRightSteerInvert;
+ break;
+
+ case 2:
+ // Back Left
+ driveTalon = new TalonFX(kBackLeftDriveMotorId, kBackLeftDriveCanbus);
+ turnTalon = new TalonFX(kBackLeftSteerMotorId, kBackLeftSteerCanbus);
+ cancoder = new CANcoder(kBackLeftEncoderId, kBackLeftEncoderCanbus);
+ absoluteEncoderOffset = new Rotation2d(kBackLeftEncoderOffset);
+ isTurnMotorInverted = kBackLeftSteerInvert;
+ break;
+
+ case 3:
+ // Back Right
+ driveTalon = new TalonFX(kBackRightDriveMotorId, kBackRightDriveCanbus);
+ turnTalon = new TalonFX(kBackRightSteerMotorId, kBackRightSteerCanbus);
+ cancoder = new CANcoder(kBackRightEncoderId, kBackRightEncoderCanbus);
+ absoluteEncoderOffset = new Rotation2d(kBackRightEncoderOffset);
+ isTurnMotorInverted = kBackRightSteerInvert;
+ break;
+
+ default:
+ throw new RuntimeException("Invalid module index");
+ }
+
+ // Example values from
+ // https://v6.docs.ctr-electronics.com/en/stable/docs/hardware-reference/talonfx/improving-performance-with-current-limits.html
+ var driveConfig = new TalonFXConfiguration();
+ driveConfig.CurrentLimits.StatorCurrentLimit = kDriveCurrentLimit;
+ driveConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+ driveConfig.CurrentLimits.SupplyCurrentLimit = kDriveCurrentLimit * 0.6;
+ driveConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+ driveTalon.getConfigurator().apply(driveConfig);
+ setDriveBrakeMode(true);
+
+ var turnConfig = new TalonFXConfiguration();
+ turnConfig.CurrentLimits.StatorCurrentLimit = kSteerCurrentLimit;
+ turnConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+ turnTalon.getConfigurator().apply(turnConfig);
+ setTurnBrakeMode(true);
+
+ cancoder.getConfigurator().apply(new CANcoderConfiguration());
+
+ drivePosition = driveTalon.getPosition();
+ driveVelocity = driveTalon.getVelocity();
+ driveAppliedVolts = driveTalon.getMotorVoltage();
+ driveCurrent = driveTalon.getSupplyCurrent();
+
+ turnAbsolutePosition = cancoder.getAbsolutePosition();
+ turnPosition = turnTalon.getPosition();
+ turnVelocity = turnTalon.getVelocity();
+ turnAppliedVolts = turnTalon.getMotorVoltage();
+ turnCurrent = turnTalon.getSupplyCurrent();
+
+ BaseStatusSignal.setUpdateFrequencyForAll(
+ 100.0, drivePosition, turnPosition); // Required for odometry, use faster rate
+ BaseStatusSignal.setUpdateFrequencyForAll(
+ 50.0,
+ driveVelocity,
+ driveAppliedVolts,
+ driveCurrent,
+ turnAbsolutePosition,
+ turnVelocity,
+ turnAppliedVolts,
+ turnCurrent);
+ driveTalon.optimizeBusUtilization();
+ turnTalon.optimizeBusUtilization();
+ }
+
+ @Override
+ public void updateInputs(ModuleIOInputs inputs) {
+ BaseStatusSignal.refreshAll(
+ drivePosition,
+ driveVelocity,
+ driveAppliedVolts,
+ driveCurrent,
+ turnAbsolutePosition,
+ turnPosition,
+ turnVelocity,
+ turnAppliedVolts,
+ turnCurrent);
+
+ inputs.drivePositionRad =
+ Units.rotationsToRadians(drivePosition.getValueAsDouble()) / kDriveGearRatio;
+ inputs.driveVelocityRadPerSec =
+ Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / kDriveGearRatio;
+ inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
+ inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()};
+
+ inputs.turnAbsolutePosition =
+ Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble())
+ .minus(absoluteEncoderOffset);
+ inputs.turnPosition =
+ Rotation2d.fromRotations(turnPosition.getValueAsDouble() / kSteerGearRatio);
+ inputs.turnVelocityRadPerSec =
+ Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / kSteerGearRatio;
+ inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble();
+ inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()};
+ }
+
+ @Override
+ public void setDriveVoltage(double volts) {
+ driveTalon.setControl(new VoltageOut(volts));
+ }
+
+ @Override
+ public void setTurnVoltage(double volts) {
+ turnTalon.setControl(new VoltageOut(volts));
+ }
+
+ @Override
+ public void setDriveBrakeMode(boolean enable) {
+ var config = new MotorOutputConfigs();
+ config.Inverted = InvertedValue.CounterClockwise_Positive;
+ config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast;
+ driveTalon.getConfigurator().apply(config);
+ }
+
+ @Override
+ public void setTurnBrakeMode(boolean enable) {
+ var config = new MotorOutputConfigs();
+ config.Inverted =
+ isTurnMotorInverted
+ ? InvertedValue.Clockwise_Positive
+ : InvertedValue.CounterClockwise_Positive;
+ config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast;
+ turnTalon.getConfigurator().apply(config);
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/README b/src/main/java/frc/robot/subsystems/drive/README
new file mode 100644
index 00000000..1e84dc77
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/README
@@ -0,0 +1,21 @@
+The hardware-specific IO files included here are a baseline for what we expect
+teams to use. If, however, you are using a different combination of hardware,
+you will need to create a new ``ModuleIO*.java`` or ``GyroIO*.java`` file. We
+encourage you to submit a PR to the Az-RBSI repositiory with your new hardware
+file if you feel other teams may benefit from its inclusion. Additionally, the
+instantiation logic in ``Drive.java`` will need to be extended to include the
+new module type.
+
+Existing IO files:
+
+ - ``GyroIOPigeon2.java``: CTRE Pigeon2 as the swerve IMU
+ - ``GyroIONavX.java``: NavX as the swerve IMU, connected via SPI
+
+ - ``ModuleIOTalonFX.java``: 2x TalonFX motors + CANcoder
+ - ``ModuleIOSparkMax.java``: 2x Rev NEO motors + analog absolute encoder connected to the RIO
+ - ``ModuleIOSparkCANcoder.java``: 2x Rv NEO motors + CANcoder
+ - ``ModuleIOBlended.java``: TalonFX drive, Rev NEO steer motors + CANcoder
+
+
+--------------------
+Last Modified: 22 Nov 2024, TPEB
diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java
new file mode 100644
index 00000000..88d91026
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java
@@ -0,0 +1,116 @@
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.flywheel_example;
+
+import static edu.wpi.first.units.Units.*;
+import static frc.robot.Constants.FlywheelConstants.*;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.Constants;
+import frc.robot.util.RBSISubsystem;
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+public class Flywheel extends RBSISubsystem {
+ private final FlywheelIO io;
+ private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged();
+ private final SimpleMotorFeedforward ffModel;
+ private final SysIdRoutine sysId;
+
+ /** Creates a new Flywheel. */
+ public Flywheel(FlywheelIO io) {
+ this.io = io;
+
+ // Switch constants based on mode (the physics simulator is treated as a
+ // separate robot with different tuning)
+ switch (Constants.getMode()) {
+ case REAL:
+ case REPLAY:
+ ffModel = new SimpleMotorFeedforward(kStaticGainReal, kVelocityGainReal);
+ io.configurePID(kPReal, kIReal, kDReal);
+ break;
+ case SIM:
+ ffModel = new SimpleMotorFeedforward(kStaticGainSim, kVelocityGainSim);
+ io.configurePID(kPSim, kISim, kDSim);
+ break;
+ default:
+ ffModel = new SimpleMotorFeedforward(0.0, 0.0);
+ break;
+ }
+
+ // Configure SysId
+ sysId =
+ new SysIdRoutine(
+ new SysIdRoutine.Config(
+ null,
+ null,
+ null,
+ (state) -> Logger.recordOutput("Flywheel/SysIdState", state.toString())),
+ new SysIdRoutine.Mechanism((voltage) -> runVolts(voltage.in(Volts)), null, this));
+ }
+
+ @Override
+ public void periodic() {
+ io.updateInputs(inputs);
+ Logger.processInputs("Flywheel", inputs);
+ }
+
+ /** Run open loop at the specified voltage. */
+ public void runVolts(double volts) {
+ io.setVoltage(volts);
+ }
+
+ /** Run closed loop at the specified velocity. */
+ public void runVelocity(double velocityRPM) {
+ var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM);
+ io.setVelocity(velocityRadPerSec, ffModel.calculate(velocityRadPerSec));
+
+ // Log flywheel setpoint
+ Logger.recordOutput("Flywheel/SetpointRPM", velocityRPM);
+ }
+
+ /** Stops the flywheel. */
+ public void stop() {
+ io.stop();
+ }
+
+ /** Returns a command to run a quasistatic test in the specified direction. */
+ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+ return sysId.quasistatic(direction);
+ }
+
+ /** Returns a command to run a dynamic test in the specified direction. */
+ public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+ return sysId.dynamic(direction);
+ }
+
+ /** Returns the current velocity in RPM. */
+ @AutoLogOutput(key = "Mechanism/Flywheel")
+ public double getVelocityRPM() {
+ return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec);
+ }
+
+ /** Returns the current velocity in radians per second. */
+ public double getCharacterizationVelocity() {
+ return inputs.velocityRadPerSec;
+ }
+
+ @Override
+ public int[] getPowerPorts() {
+ return io.powerPorts;
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java
new file mode 100644
index 00000000..403c200e
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java
@@ -0,0 +1,45 @@
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.flywheel_example;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface FlywheelIO {
+
+ // IMPORTANT: Include here all devices that are part of this mechanism!
+ public final int[] powerPorts = {};
+
+ @AutoLog
+ public static class FlywheelIOInputs {
+ public double positionRad = 0.0;
+ public double velocityRadPerSec = 0.0;
+ public double appliedVolts = 0.0;
+ public double[] currentAmps = new double[] {};
+ }
+
+ /** Updates the set of loggable inputs. */
+ public default void updateInputs(FlywheelIOInputs inputs) {}
+
+ /** Run open loop at the specified voltage. */
+ public default void setVoltage(double volts) {}
+
+ /** Run closed loop at the specified velocity. */
+ public default void setVelocity(double velocityRadPerSec, double ffVolts) {}
+
+ /** Stop in open loop. */
+ public default void stop() {}
+
+ /** Set velocity PID constants. */
+ public default void configurePID(double kP, double kI, double kD) {}
+}
diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java
new file mode 100644
index 00000000..3a19b6b3
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java
@@ -0,0 +1,68 @@
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.flywheel_example;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+
+public class FlywheelIOSim implements FlywheelIO {
+ private FlywheelSim sim = new FlywheelSim(DCMotor.getNEO(1), 1.5, 0.004);
+ private PIDController pid = new PIDController(0.0, 0.0, 0.0);
+
+ private boolean closedLoop = false;
+ private double ffVolts = 0.0;
+ private double appliedVolts = 0.0;
+
+ @Override
+ public void updateInputs(FlywheelIOInputs inputs) {
+ if (closedLoop) {
+ appliedVolts =
+ MathUtil.clamp(pid.calculate(sim.getAngularVelocityRadPerSec()) + ffVolts, -12.0, 12.0);
+ sim.setInputVoltage(appliedVolts);
+ }
+
+ sim.update(0.02);
+
+ inputs.positionRad = 0.0;
+ inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec();
+ inputs.appliedVolts = appliedVolts;
+ inputs.currentAmps = new double[] {sim.getCurrentDrawAmps()};
+ }
+
+ @Override
+ public void setVoltage(double volts) {
+ closedLoop = false;
+ appliedVolts = volts;
+ sim.setInputVoltage(volts);
+ }
+
+ @Override
+ public void setVelocity(double velocityRadPerSec, double ffVolts) {
+ closedLoop = true;
+ pid.setSetpoint(velocityRadPerSec);
+ this.ffVolts = ffVolts;
+ }
+
+ @Override
+ public void stop() {
+ setVoltage(0.0);
+ }
+
+ @Override
+ public void configurePID(double kP, double kI, double kD) {
+ pid.setPID(kP, kI, kD);
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java
new file mode 100644
index 00000000..5fde4f02
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java
@@ -0,0 +1,98 @@
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.flywheel_example;
+
+import static frc.robot.Constants.FlywheelConstants.*;
+
+import com.revrobotics.CANSparkBase.ControlType;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkPIDController;
+import com.revrobotics.SparkPIDController.ArbFFUnits;
+import edu.wpi.first.math.util.Units;
+import frc.robot.RobotContainer.Ports;
+
+/**
+ * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with
+ * "CANSparkFlex".
+ */
+public class FlywheelIOSparkMax implements FlywheelIO {
+
+ // Define the leader / follower motors from the Ports section of RobotContainer
+ private final CANSparkMax leader =
+ new CANSparkMax(Ports.FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless);
+ private final CANSparkMax follower =
+ new CANSparkMax(Ports.FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless);
+ private final RelativeEncoder encoder = leader.getEncoder();
+ private final SparkPIDController pid = leader.getPIDController();
+ // IMPORTANT: Include here all devices listed above that are part of this mechanism!
+ public final int[] powerPorts = {
+ Ports.FLYWHEEL_LEADER.getPowerPort(), Ports.FLYWHEEL_FOLLOWER.getPowerPort()
+ };
+
+ public FlywheelIOSparkMax() {
+ leader.restoreFactoryDefaults();
+ follower.restoreFactoryDefaults();
+
+ leader.setCANTimeout(250);
+ follower.setCANTimeout(250);
+
+ leader.setInverted(false);
+ follower.follow(leader, false);
+
+ leader.enableVoltageCompensation(12.0);
+ leader.setSmartCurrentLimit(30);
+
+ leader.burnFlash();
+ follower.burnFlash();
+ }
+
+ @Override
+ public void updateInputs(FlywheelIOInputs inputs) {
+ inputs.positionRad = Units.rotationsToRadians(encoder.getPosition() / kFlywheelGearRatio);
+ inputs.velocityRadPerSec =
+ Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / kFlywheelGearRatio);
+ inputs.appliedVolts = leader.getAppliedOutput() * leader.getBusVoltage();
+ inputs.currentAmps = new double[] {leader.getOutputCurrent(), follower.getOutputCurrent()};
+ }
+
+ @Override
+ public void setVoltage(double volts) {
+ leader.setVoltage(volts);
+ }
+
+ @Override
+ public void setVelocity(double velocityRadPerSec, double ffVolts) {
+ pid.setReference(
+ Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * kFlywheelGearRatio,
+ ControlType.kVelocity,
+ 0,
+ ffVolts,
+ ArbFFUnits.kVoltage);
+ }
+
+ @Override
+ public void stop() {
+ leader.stopMotor();
+ }
+
+ @Override
+ public void configurePID(double kP, double kI, double kD) {
+ pid.setP(kP, 0);
+ pid.setI(kI, 0);
+ pid.setD(kD, 0);
+ pid.setFF(0, 0);
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java
new file mode 100644
index 00000000..2cb4d84d
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java
@@ -0,0 +1,108 @@
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.flywheel_example;
+
+import static frc.robot.Constants.FlywheelConstants.*;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.math.util.Units;
+import frc.robot.RobotContainer.Ports;
+
+public class FlywheelIOTalonFX implements FlywheelIO {
+
+ // Define the leader / follower motors from the Ports section of RobotContainer
+ private final TalonFX leader =
+ new TalonFX(Ports.FLYWHEEL_LEADER.getDeviceNumber(), Ports.FLYWHEEL_LEADER.getBus());
+ private final TalonFX follower =
+ new TalonFX(Ports.FLYWHEEL_FOLLOWER.getDeviceNumber(), Ports.FLYWHEEL_FOLLOWER.getBus());
+ // IMPORTANT: Include here all devices listed above that are part of this mechanism!
+ public final int[] powerPorts = {
+ Ports.FLYWHEEL_LEADER.getPowerPort(), Ports.FLYWHEEL_FOLLOWER.getPowerPort()
+ };
+
+ private final StatusSignal leaderPosition = leader.getPosition();
+ private final StatusSignal leaderVelocity = leader.getVelocity();
+ private final StatusSignal leaderAppliedVolts = leader.getMotorVoltage();
+ private final StatusSignal leaderCurrent = leader.getSupplyCurrent();
+ private final StatusSignal followerCurrent = follower.getSupplyCurrent();
+
+ public FlywheelIOTalonFX() {
+ var config = new TalonFXConfiguration();
+ config.CurrentLimits.SupplyCurrentLimit = 30.0;
+ config.CurrentLimits.SupplyCurrentLimitEnable = true;
+ config.MotorOutput.NeutralMode = NeutralModeValue.Coast;
+ leader.getConfigurator().apply(config);
+ follower.getConfigurator().apply(config);
+ follower.setControl(new Follower(leader.getDeviceID(), false));
+
+ BaseStatusSignal.setUpdateFrequencyForAll(
+ 50.0, leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent);
+ leader.optimizeBusUtilization();
+ follower.optimizeBusUtilization();
+ }
+
+ @Override
+ public void updateInputs(FlywheelIOInputs inputs) {
+ BaseStatusSignal.refreshAll(
+ leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent);
+ inputs.positionRad =
+ Units.rotationsToRadians(leaderPosition.getValueAsDouble()) / kFlywheelGearRatio;
+ inputs.velocityRadPerSec =
+ Units.rotationsToRadians(leaderVelocity.getValueAsDouble()) / kFlywheelGearRatio;
+ inputs.appliedVolts = leaderAppliedVolts.getValueAsDouble();
+ inputs.currentAmps =
+ new double[] {leaderCurrent.getValueAsDouble(), followerCurrent.getValueAsDouble()};
+ }
+
+ @Override
+ public void setVoltage(double volts) {
+ leader.setControl(new VoltageOut(volts));
+ }
+
+ @Override
+ public void setVelocity(double velocityRadPerSec, double ffVolts) {
+ leader.setControl(
+ new VelocityVoltage(
+ Units.radiansToRotations(velocityRadPerSec),
+ 0.0,
+ true,
+ ffVolts,
+ 0,
+ false,
+ false,
+ false));
+ }
+
+ @Override
+ public void stop() {
+ leader.stopMotor();
+ }
+
+ @Override
+ public void configurePID(double kP, double kI, double kD) {
+ var config = new Slot0Configs();
+ config.kP = kP;
+ config.kI = kI;
+ config.kD = kD;
+ leader.getConfigurator().apply(config);
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/README b/src/main/java/frc/robot/subsystems/flywheel_example/README
new file mode 100644
index 00000000..7cad088f
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/flywheel_example/README
@@ -0,0 +1,33 @@
+This directory contains an example generic flywheel subsystem, as distributed
+with AdvantageKit. The structure of the subsystem directory is:
+
+ * Flywheel.java
+ The base subsystem definitions and outward-facing (public) hardware-
+ agnostic functions for interacting with the subsystem. The idea is
+ that any of the hardware-specific (i.e. motor controller API calls)
+ functionality is insulated from the rest of the robot code within
+ the library-specific modules in this directory.
+
+ * FlywheelIO.java
+ The base subsystem I/O (input/output) interface that contains the
+ structure and class varibales needed by the library-specific modules.
+
+ * FlywheelIOSim.java
+ Simulated flywheel module, for use with robot simulations, does not
+ control actual hardware.
+
+ * FlywheelIOSparkMax.java
+ An example implementation of a flywheel using Rev SparkMax motor
+ controllers (and their associated library APIs).
+
+ * FlywheelIOTalonFX.java
+ An example implementation of a flywheel using CTRE TalonFX motor
+ controllers (and their associated library APIs).
+
+--------------------
+When creating a new mechanism subsystem, consider using this generic framework
+for public vs. private functions and API calls to make your code more modular
+and reusable from year to year. Additionally, the I/O framework given here
+interfaces with AdvantageKit's logging to allow for replay of inputs and
+testing of code changes in simulation.
+
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveSubsystem.java
new file mode 100644
index 00000000..d2eb761c
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveSubsystem.java
@@ -0,0 +1,382 @@
+// // Copyright (c) 2024 Az-FIRST
+// // http://github.com/AZ-First
+// //
+// // This program is free software; you can redistribute it and/or
+// // modify it under the terms of the GNU General Public License
+// // version 3 as published by the Free Software Foundation or
+// // available in the root directory of this project.
+// //
+// // This program is distributed in the hope that it will be useful,
+// // but WITHOUT ANY WARRANTY; without even the implied warranty of
+// // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// // GNU General Public License for more details.
+// //
+// // 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.
+// //
+// // NOTE: This module based on the CTRE Phoenix6 examples
+// // https://github.com/CrossTheRoadElec/Phoenix6-Examples
+
+// package frc.robot.subsystems.swervedrive_ignore;
+
+// import static edu.wpi.first.units.Units.Volts;
+// import static frc.robot.Constants.DrivebaseConstants.*;
+
+// import com.ctre.phoenix6.SignalLogger;
+// import com.ctre.phoenix6.Utils;
+// import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain;
+// import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants;
+// import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
+// import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants;
+// import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
+// import com.ctre.phoenix6.signals.NeutralModeValue;
+// import com.pathplanner.lib.auto.AutoBuilder;
+// import com.pathplanner.lib.commands.PathPlannerAuto;
+// import com.pathplanner.lib.path.PathConstraints;
+// import com.pathplanner.lib.path.PathPlannerPath;
+// import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
+// import com.pathplanner.lib.util.PIDConstants;
+// import com.pathplanner.lib.util.ReplanningConfig;
+// import edu.wpi.first.math.geometry.Pose2d;
+// import edu.wpi.first.math.geometry.Rotation2d;
+// import edu.wpi.first.math.kinematics.ChassisSpeeds;
+// import edu.wpi.first.wpilibj.DriverStation;
+// import edu.wpi.first.wpilibj.DriverStation.Alliance;
+// import edu.wpi.first.wpilibj.Notifier;
+// import edu.wpi.first.wpilibj.RobotController;
+// import edu.wpi.first.wpilibj2.command.Command;
+// import edu.wpi.first.wpilibj2.command.Subsystem;
+// import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+// import frc.robot.Constants.DrivebaseConstants;
+// import frc.robot.generated.TunerConstants;
+// import frc.robot.subsystems.vision.Vision;
+// import java.util.function.Supplier;
+// import org.littletonrobotics.junction.AutoLogOutput;
+// import org.littletonrobotics.junction.Logger;
+
+// /**
+// * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be
+// used
+// * in command-based projects easily.
+// */
+// public class SwerveSubsystem extends SwerveDrivetrain implements Subsystem {
+// /** CTRE constants */
+// private static final double kSimLoopPeriod = 0.005; // 5 ms
+
+// private Notifier m_simNotifier = null;
+// private double m_lastSimTime;
+
+// /** PhotonVision class to keep an accurate odometry */
+// private Vision vision;
+
+// /** Enable vision odometry updates while driving */
+// private final boolean visionDriveTest = false;
+
+// /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
+// private final Rotation2d BlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0);
+// /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
+// private final Rotation2d RedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180);
+// /* Keep track if we've ever applied the operator perspective before or not */
+// private boolean hasAppliedOperatorPerspective = false;
+
+// private final SwerveRequest.ApplyChassisSpeeds AutoRequest =
+// new SwerveRequest.ApplyChassisSpeeds();
+
+// private final SwerveRequest.SysIdSwerveTranslation TranslationCharacterization =
+// new SwerveRequest.SysIdSwerveTranslation();
+// private final SwerveRequest.SysIdSwerveRotation RotationCharacterization =
+// new SwerveRequest.SysIdSwerveRotation();
+// private final SwerveRequest.SysIdSwerveSteerGains SteerCharacterization =
+// new SwerveRequest.SysIdSwerveSteerGains();
+
+// /* Use one of these sysidroutines for your particular test */
+// private SysIdRoutine SysIdRoutineTranslation =
+// new SysIdRoutine(
+// new SysIdRoutine.Config(
+// null,
+// Volts.of(4),
+// null,
+// (state) -> SignalLogger.writeString("state", state.toString())),
+// new SysIdRoutine.Mechanism(
+// (volts) -> setControl(TranslationCharacterization.withVolts(volts)), null, this));
+
+// private final SysIdRoutine SysIdRoutineRotation =
+// new SysIdRoutine(
+// new SysIdRoutine.Config(
+// null,
+// Volts.of(4),
+// null,
+// (state) -> SignalLogger.writeString("state", state.toString())),
+// new SysIdRoutine.Mechanism(
+// (volts) -> setControl(RotationCharacterization.withVolts(volts)), null, this));
+// private final SysIdRoutine SysIdRoutineSteer =
+// new SysIdRoutine(
+// new SysIdRoutine.Config(
+// null,
+// Volts.of(7),
+// null,
+// (state) -> SignalLogger.writeString("state", state.toString())),
+// new SysIdRoutine.Mechanism(
+// (volts) -> setControl(SteerCharacterization.withVolts(volts)), null, this));
+
+// /* Change this to the sysid routine you want to test */
+// private final SysIdRoutine RoutineToApply = SysIdRoutineTranslation;
+
+// public SwerveSubsystem(
+// SwerveDrivetrainConstants driveTrainConstants,
+// double OdometryUpdateFrequency,
+// SwerveModuleConstants... modules) {
+// super(driveTrainConstants, OdometryUpdateFrequency, modules);
+// configurePathPlanner();
+// if (Utils.isSimulation()) {
+// startSimThread();
+// }
+// Logger.recordOutput("SwerveDrive/Comment1", "This is a comment!");
+// }
+
+// public SwerveSubsystem(
+// SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) {
+// super(driveTrainConstants, modules);
+// configurePathPlanner();
+// if (Utils.isSimulation()) {
+// startSimThread();
+// }
+// Logger.recordOutput("SwerveDrive/Comment2", "This is a comment!");
+// }
+
+// private void configurePathPlanner() {
+// double driveBaseRadius = 0;
+// for (var moduleLocation : m_moduleLocations) {
+// driveBaseRadius = Math.max(driveBaseRadius, moduleLocation.getNorm());
+// }
+
+// AutoBuilder.configureHolonomic(
+// () -> this.getState().Pose, // Supplier of current robot pose
+// this::seedFieldRelative, // Consumer for seeding pose against auto
+// this::getCurrentRobotChassisSpeeds,
+// (speeds) ->
+// this.setControl(
+// AutoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot
+// new HolonomicPathFollowerConfig(
+// new PIDConstants(10, 0, 0),
+// new PIDConstants(10, 0, 0),
+// TunerConstants.kSpeedAt12VoltsMps,
+// driveBaseRadius,
+// new ReplanningConfig()),
+// () ->
+// DriverStation.getAlliance().orElse(Alliance.Blue)
+// == Alliance
+// .Red, // Assume the path needs to be flipped for Red vs Blue, this is
+// normally
+// // the case
+// this); // Subsystem for requirements
+// }
+
+// /* Section for defining TeleOp Commands */
+// // Field-centric driving in open loop mode
+// public final SwerveRequest.FieldCentric drive =
+// new SwerveRequest.FieldCentric()
+// .withDeadband(DrivebaseConstants.kMaxLinearSpeed * 0.1)
+// .withRotationalDeadband(DrivebaseConstants.kMaxAngularSpeed * 0.1) // Add a 10%
+// deadband
+// .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
+
+// public final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
+// public final SwerveRequest.RobotCentric forwardStraight =
+// new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);
+// public final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
+
+// public Command applyRequest(Supplier requestSupplier) {
+// return run(() -> this.setControl(requestSupplier.get()));
+// }
+
+// public Command getAutoPath(String pathName) {
+// return new PathPlannerAuto(pathName);
+// }
+
+// /*
+// * Both the sysid commands are specific to one particular sysid routine, change
+// * which one you're trying to characterize
+// */
+// public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+// return RoutineToApply.quasistatic(direction);
+// }
+
+// public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+// return RoutineToApply.dynamic(direction);
+// }
+
+// public ChassisSpeeds getCurrentRobotChassisSpeeds() {
+// return m_kinematics.toChassisSpeeds(getState().ModuleStates);
+// }
+
+// private void startSimThread() {
+// m_lastSimTime = Utils.getCurrentTimeSeconds();
+
+// /* Run simulation at a faster rate so PID gains behave more reasonably */
+// m_simNotifier =
+// new Notifier(
+// () -> {
+// final double currentTime = Utils.getCurrentTimeSeconds();
+// double deltaTime = currentTime - m_lastSimTime;
+// m_lastSimTime = currentTime;
+
+// /* use the measured time delta, get battery voltage from WPILib */
+// updateSimState(deltaTime, RobotController.getBatteryVoltage());
+// });
+// m_simNotifier.startPeriodic(kSimLoopPeriod);
+// }
+
+// /** Periodic function -- update odometry and log everything */
+// @Override
+// public void 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 (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
+// DriverStation.getAlliance()
+// .ifPresent(
+// (allianceColor) -> {
+// this.setOperatorPerspectiveForward(
+// allianceColor == Alliance.Red
+// ? RedAlliancePerspectiveRotation
+// : BlueAlliancePerspectiveRotation);
+// hasAppliedOperatorPerspective = true;
+// });
+// }
+
+// // // When vision is enabled we must manually update odometry in SwerveDrive
+// // if (visionDriveTest) {
+// // vision.updatePoseEstimation(this);
+// // }
+
+// /** Log Telemetry Data to AdvantageKit */
+// Logger.recordOutput("SwerveDrive/Comment", "This is a comment!");
+// // Logger.recordOutput("SwerveDrive/Telemetry/moduleCount", this.ModuleCount);
+// // Logger.recordOutput("SwerveDrive/Telemetry/wheelLocations", this.m_moduleLocations);
+// Logger.recordOutput("SwerveDrive/Telemetry/measuredStates", getState().ModuleStates);
+// // Logger.recordOutput("SwerveDive/Telemetry/desiredStates",
+// // SwerveDriveTelemetry.desiredStates);
+// // Logger.recordOutput("SwerveDive/Telemetry/robotRotation",
+// // SwerveDriveTelemetry.robotRotation);
+// // Logger.recordOutput("SwerveDive/Telemetry/maxSpeed", SwerveDriveTelemetry.maxSpeed);
+// // Logger.recordOutput("SwerveDive/Telemetry/rotationUnit",
+// SwerveDriveTelemetry.rotationUnit);
+// // Logger.recordOutput("SwerveDive/Telemetry/sizeLeftRight",
+// // SwerveDriveTelemetry.sizeLeftRight);
+// // Logger.recordOutput("SwerveDive/Telemetry/sizeFrontBack",
+// // SwerveDriveTelemetry.sizeFrontBack);
+// // Logger.recordOutput(
+// // "SwerveDive/Telemetry/forwardDirection", SwerveDriveTelemetry.forwardDirection);
+// // Logger.recordOutput(
+// // "SwerveDive/Telemetry/maxAngularVelocity", SwerveDriveTelemetry.maxAngularVelocity);
+// // Logger.recordOutput(
+// // "SwerveDive/Telemetry/measuredChassisSpeeds",
+// // SwerveDriveTelemetry.measuredChassisSpeeds);
+// // Logger.recordOutput(
+// // "SwerveDive/Telemetry/desiredChassisSpeeds",
+// SwerveDriveTelemetry.desiredChassisSpeeds);
+
+// // /** Log Swerve Drive States to AdvantageKit */
+// // getModuleStates();
+// // getDesiredStates();
+// // Logger.recordOutput(
+// // "SwerveDive/States/RobotRotation",
+// // SwerveDriveTelemetry.rotationUnit == "degrees"
+// // ? Rotation2d.fromDegrees(SwerveDriveTelemetry.robotRotation)
+// // : Rotation2d.fromRadians(SwerveDriveTelemetry.robotRotation));
+// }
+
+// /************************************************************************* */
+// /* COMMAND SECTION -- Drivebase-only Commands */
+
+// /**
+// * Get the path follower with events.
+// *
+// * @param pathName PathPlanner path name.
+// * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command.
+// */
+// public Command getAutonomousCommand(String pathName) {
+// // Create a path following command using AutoBuilder. This will also trigger event markers.
+// return new PathPlannerAuto(pathName);
+// }
+
+// /**
+// * Use PathPlanner Path finding to go to a point on the field.
+// *
+// * @param pose Target {@link Pose2d} to go to.
+// * @return PathFinding command
+// */
+// public Command driveToPose(Pose2d pose) {
+// // Create the constraints to use while pathfinding
+// PathConstraints constraints =
+// new PathConstraints(kMaxLinearSpeed, kMaxLinearAccel, kMaxAngularSpeed,
+// kMaxAngularAccel);
+
+// // Since AutoBuilder is configured, we can use it to build pathfinding commands
+// return AutoBuilder.pathfindToPose(
+// pose,
+// constraints,
+// 0.0, // Goal end velocity in meters/sec
+// 0.0 // Rotation delay distance in meters. This is how far the robot should travel before
+// // attempting to rotate.
+// );
+// }
+
+// /************************************************************************* */
+// /* UTILITY SECTION -- Utility methods */
+
+// /**
+// * Gets the current pose (position and rotation) of the robot, as reported by odometry.
+// *
+// * @return The robot's pose
+// */
+// @AutoLogOutput(key = "Odometry/RobotPose")
+// public Pose2d getPose() {
+// return getState().Pose;
+// }
+
+// /**
+// * Sets the drive motors to brake/coast mode.
+// *
+// * @param brake True to set motors to brake mode, false for coast.
+// */
+// public void setMotorBrake(boolean brake) {
+// if (brake) {
+// this.configNeutralMode(NeutralModeValue.Brake);
+// } else {
+// this.configNeutralMode(NeutralModeValue.Coast);
+// }
+// }
+
+// // /** Returns the module states (turn angles and drive velocities) for all of the modules. */
+// // @AutoLogOutput(key = "SwerveDive/States/Measured")
+// // private SwerveModuleState[] getModuleStates() {
+// // SwerveModuleState[] states = new SwerveModuleState[4];
+// // for (int i = 0; i < 4; i++) {
+// // states[i] =
+// // new SwerveModuleState(
+// // SwerveDriveTelemetry.measuredStates[(i * 2) + 1],
+// // Rotation2d.fromDegrees(SwerveDriveTelemetry.measuredStates[i * 2]));
+// // }
+// // return states;
+// // }
+
+// // /** Returns the desired states (turn angles and drive velocities) for all of the modules. */
+// // @AutoLogOutput(key = "SwerveDive/States/Desired")
+// // private SwerveModuleState[] getDesiredStates() {
+// // SwerveModuleState[] states = new SwerveModuleState[4];
+// // for (int i = 0; i < 4; i++) {
+// // states[i] =
+// // new SwerveModuleState(
+// // SwerveDriveTelemetry.desiredStates[(i * 2) + 1],
+// // Rotation2d.fromDegrees(SwerveDriveTelemetry.desiredStates[i * 2]));
+// // }
+// // return states;
+// // }
+// }
diff --git a/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveTelemetry.java b/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveTelemetry.java
new file mode 100644
index 00000000..e697634e
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveTelemetry.java
@@ -0,0 +1,149 @@
+// // Copyright (c) 2024 Az-FIRST
+// // http://github.com/AZ-First
+// //
+// // This program is free software; you can redistribute it and/or
+// // modify it under the terms of the GNU General Public License
+// // version 3 as published by the Free Software Foundation or
+// // available in the root directory of this project.
+// //
+// // This program is distributed in the hope that it will be useful,
+// // but WITHOUT ANY WARRANTY; without even the implied warranty of
+// // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// // GNU General Public License for more details.
+// //
+// // 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.
+// //
+// // NOTE: This module based on the CTRE Phoenix6 examples
+// // https://github.com/CrossTheRoadElec/Phoenix6-Examples
+
+// package frc.robot.subsystems.swervedrive_ignore;
+
+// import com.ctre.phoenix6.SignalLogger;
+// import com.ctre.phoenix6.Utils;
+// import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState;
+// import edu.wpi.first.math.geometry.Pose2d;
+// import edu.wpi.first.math.geometry.Translation2d;
+// import edu.wpi.first.networktables.DoubleArrayPublisher;
+// import edu.wpi.first.networktables.DoublePublisher;
+// import edu.wpi.first.networktables.NetworkTable;
+// import edu.wpi.first.networktables.NetworkTableInstance;
+// import edu.wpi.first.networktables.StringPublisher;
+// import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+// import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+// import edu.wpi.first.wpilibj.util.Color;
+// import edu.wpi.first.wpilibj.util.Color8Bit;
+
+// public class SwerveTelemetry {
+// private final double MaxSpeed;
+
+// /**
+// * Construct a telemetry object, with the specified max speed of the robot
+// *
+// * @param maxSpeed Maximum speed in meters per second
+// */
+// public SwerveTelemetry(double maxSpeed) {
+// MaxSpeed = maxSpeed;
+// SignalLogger.start();
+// }
+
+// /* What to publish over networktables for telemetry */
+// private final NetworkTableInstance inst = NetworkTableInstance.getDefault();
+
+// /* Robot pose for field positioning */
+// private final NetworkTable table = inst.getTable("Pose");
+// private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish();
+// private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish();
+
+// /* Robot speeds for general checking */
+// private final NetworkTable driveStats = inst.getTable("Drive");
+// private final DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish();
+// private final DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish();
+// private final DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish();
+// private final DoublePublisher odomFreq =
+// driveStats.getDoubleTopic("Odometry Frequency").publish();
+
+// /* Keep a reference of the last pose to calculate the speeds */
+// private Pose2d m_lastPose = new Pose2d();
+// private double lastTime = Utils.getCurrentTimeSeconds();
+
+// /* Mechanisms to represent the swerve module states */
+// private final Mechanism2d[] m_moduleMechanisms =
+// new Mechanism2d[] {
+// new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1,
+// 1),
+// };
+// /* A direction and length changing ligament for speed representation */
+// private final MechanismLigament2d[] m_moduleSpeeds =
+// new MechanismLigament2d[] {
+// m_moduleMechanisms[0]
+// .getRoot("RootSpeed", 0.5, 0.5)
+// .append(new MechanismLigament2d("Speed", 0.5, 0)),
+// m_moduleMechanisms[1]
+// .getRoot("RootSpeed", 0.5, 0.5)
+// .append(new MechanismLigament2d("Speed", 0.5, 0)),
+// m_moduleMechanisms[2]
+// .getRoot("RootSpeed", 0.5, 0.5)
+// .append(new MechanismLigament2d("Speed", 0.5, 0)),
+// m_moduleMechanisms[3]
+// .getRoot("RootSpeed", 0.5, 0.5)
+// .append(new MechanismLigament2d("Speed", 0.5, 0)),
+// };
+// /* A direction changing and length constant ligament for module direction */
+// private final MechanismLigament2d[] m_moduleDirections =
+// new MechanismLigament2d[] {
+// m_moduleMechanisms[0]
+// .getRoot("RootDirection", 0.5, 0.5)
+// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new
+// Color8Bit(Color.kWhite))),
+// m_moduleMechanisms[1]
+// .getRoot("RootDirection", 0.5, 0.5)
+// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new
+// Color8Bit(Color.kWhite))),
+// m_moduleMechanisms[2]
+// .getRoot("RootDirection", 0.5, 0.5)
+// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new
+// Color8Bit(Color.kWhite))),
+// m_moduleMechanisms[3]
+// .getRoot("RootDirection", 0.5, 0.5)
+// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new
+// Color8Bit(Color.kWhite))),
+// };
+
+// /* Accept the swerve drive state and telemeterize it to smartdashboard */
+// public void telemeterize(SwerveDriveState state) {
+// /* Telemeterize the pose */
+// Pose2d pose = state.Pose;
+// fieldTypePub.set("Field2d");
+// fieldPub.set(new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()});
+
+// /* Telemeterize the robot's general speeds */
+// double currentTime = Utils.getCurrentTimeSeconds();
+// double diffTime = currentTime - lastTime;
+// lastTime = currentTime;
+// Translation2d distanceDiff = pose.minus(m_lastPose).getTranslation();
+// m_lastPose = pose;
+
+// Translation2d velocities = distanceDiff.div(diffTime);
+
+// speed.set(velocities.getNorm());
+// velocityX.set(velocities.getX());
+// velocityY.set(velocities.getY());
+// odomFreq.set(1.0 / state.OdometryPeriod);
+
+// /* Telemeterize the module's states */
+// for (int i = 0; i < 4; ++i) {
+// m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle);
+// m_moduleDirections[i].setAngle(state.ModuleStates[i].angle);
+// m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed));
+
+// SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]);
+// }
+
+// SignalLogger.writeDoubleArray(
+// "odometry", new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()});
+// SignalLogger.writeDouble("odom period", state.OdometryPeriod, "seconds");
+// }
+// }
diff --git a/src/main/java/frc/robot/subsystems/swervedrive_ignore/yagsl_old/YAGSLSwerve.java b/src/main/java/frc/robot/subsystems/swervedrive_ignore/yagsl_old/YAGSLSwerve.java
new file mode 100644
index 00000000..e2035742
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/swervedrive_ignore/yagsl_old/YAGSLSwerve.java
@@ -0,0 +1,766 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// 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.
+//
+// NOTE: This module based on the YAGSL Example Project
+
+package frc.robot.subsystems.swervedrive_ignore.yagsl_old;
+
+// import static frc.robot.Constants.DrivebaseConstants.*;
+
+// import com.pathplanner.lib.auto.AutoBuilder;
+// import com.pathplanner.lib.commands.PathPlannerAuto;
+// import com.pathplanner.lib.path.PathConstraints;
+// import com.pathplanner.lib.path.PathPlannerPath;
+// import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
+// import com.pathplanner.lib.util.ReplanningConfig;
+// import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+// import edu.wpi.first.math.geometry.Pose2d;
+// import edu.wpi.first.math.geometry.Rotation2d;
+// import edu.wpi.first.math.geometry.Translation2d;
+// import edu.wpi.first.math.kinematics.ChassisSpeeds;
+// import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+// import edu.wpi.first.math.kinematics.SwerveModuleState;
+// import edu.wpi.first.math.trajectory.Trajectory;
+// import edu.wpi.first.wpilibj.DriverStation;
+// import edu.wpi.first.wpilibj.Timer;
+// import edu.wpi.first.wpilibj2.command.Command;
+// import edu.wpi.first.wpilibj2.command.Commands;
+// import edu.wpi.first.wpilibj2.command.SubsystemBase;
+// import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
+// import frc.robot.Constants.AutonConstants;
+// import frc.robot.subsystems.vision.Vision;
+// import java.io.File;
+// import java.util.Arrays;
+// import java.util.function.DoubleSupplier;
+// import org.littletonrobotics.junction.AutoLogOutput;
+// import org.littletonrobotics.junction.Logger;
+// import org.photonvision.PhotonCamera;
+// import org.photonvision.targeting.PhotonPipelineResult;
+// import swervelib.SwerveController;
+// import swervelib.SwerveDrive;
+// import swervelib.SwerveDriveTest;
+// import swervelib.math.SwerveMath;
+// import swervelib.parser.SwerveControllerConfiguration;
+// import swervelib.parser.SwerveDriveConfiguration;
+// import swervelib.parser.SwerveParser;
+// import swervelib.telemetry.SwerveDriveTelemetry;
+// import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
+
+// /** YAGSLSwerve module */
+// public class YAGSLSwerve extends SubsystemBase {
+
+// /** Swerve drive object */
+// private final SwerveDrive swerveDrive;
+
+// /** PhotonVision class to keep an accurate odometry */
+// private Vision vision;
+
+// /** Enable vision odometry updates while driving */
+// private final boolean visionDriveTest = false;
+
+// /**
+// * Initialize {@link SwerveDrive} with the directory provided.
+// *
+// * @param directory Directory of swerve drive config files.
+// */
+// public YAGSLSwerve(File directory) {
+
+// // Angle conversion factor is 360 / (GEAR RATIO * ENCODER RESOLUTION)
+// double angleConversionFactor =
+// SwerveMath.calculateDegreesPerSteeringRotation(kTurnGearRatio);
+
+// // Motor conversion factor is (PI * WHEEL DIAMETER IN METERS) / (GEAR RATIO * ENCODER
+// // RESOLUTION)
+// double driveConversionFactor =
+// SwerveMath.calculateMetersPerRotation(kWheelRadius * 2.0, kDriveGearRatio);
+
+// // Output to console and to AdvantageKit
+// System.out.println("\"conversionFactors\": {");
+// System.out.println("\t\"angle\": {\"factor\": " + angleConversionFactor + " },");
+// System.out.println("\t\"drive\": {\"factor\": " + driveConversionFactor + " }");
+// System.out.println("}");
+// Logger.recordOutput("SwerveDive/ConversionFactors/Angle", angleConversionFactor);
+// Logger.recordOutput("SwerveDive/ConversionFactors/Drive", driveConversionFactor);
+
+ // // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary objects being
+ // // created.
+ // SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
+ // try {
+ // swerveDrive = new SwerveParser(directory).createSwerveDrive(kMaxLinearSpeed);
+ // // Alternative method if you don't want to supply the conversion factor via JSON files.
+ // // swerveDrive = new SwerveParser_RBSI(directory).createSwerveDrive(maximumSpeed,
+ // // angleConversionFactor, driveConversionFactor);
+ // } catch (Exception e) {
+ // throw new RuntimeException(e);
+ // }
+// // Heading correction should only be used while controlling the robot via angle.
+// swerveDrive.setHeadingCorrection(false);
+// // Disables cosine compensation for simulations since it causes discrepancies not seen in
+// real
+// // life.
+// if (SwerveDriveTelemetry.isSimulation) {
+// swerveDrive.setCosineCompensator(false);
+// } else {
+// swerveDrive.setCosineCompensator(true);
+// }
+
+// // Correct for skew that gets worse as angular velocity increases. Start with a coefficient
+// of
+// // 0.1
+// swerveDrive.setAngularVelocityCompensation(true, true, 0.1);
+// // Enable if you want to resynchronize your absolute encoders and motor encoders periodically
+// // when they are not moving.
+// swerveDrive.setModuleEncoderAutoSynchronize(false, 1);
+// // Set the absolute encoder to be used over the internal encoder and push the offsets onto
+// it.
+// // Throws warning if not possible
+// swerveDrive.pushOffsetsToEncoders();
+
+// if (visionDriveTest) {
+// setupPhotonVision();
+// // Stop the odometry thread if we are using vision that way we can synchronize updates
+// better.
+// swerveDrive.stopOdometryThread();
+// }
+// setupPathPlanner();
+// }
+
+// /**
+// * Construct the swerve drive.
+// *
+// * @param driveCfg SwerveDriveConfiguration for the swerve.
+// * @param controllerCfg Swerve Controller.
+// */
+// public YAGSLSwerve(
+// SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) {
+// swerveDrive = new SwerveDrive(driveCfg, controllerCfg, kMaxLinearSpeed);
+// }
+
+// /** Setup the photon vision class. */
+// public void setupPhotonVision() {
+// vision = new Vision(swerveDrive::getPose, swerveDrive.field);
+// }
+
+// /** Setup AutoBuilder for PathPlanner. */
+// public void setupPathPlanner() {
+// AutoBuilder.configureHolonomic(
+// this::getPose, // Robot pose supplier
+// this::resetOdometry, // Method to reset odometry (called if your auto has a starting
+// pose)
+// this::getRobotVelocity, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
+// this::setChassisSpeeds, // Driver method given ROBOT RELATIVE ChassisSpeeds
+// new HolonomicPathFollowerConfig(
+// AutonConstants.kAutoTranslatePID,
+// AutonConstants.kAutoAnglePID,
+// kMaxLinearSpeed,
+// kDriveBaseRadius,
+// new ReplanningConfig()),
+// () -> {
+// // Boolean supplier that controls when the path will be mirrored for the red alliance
+// // This will flip the path being followed to the red side of the field.
+// // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+// var alliance = DriverStation.getAlliance();
+// return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false;
+// },
+// this // Reference to this subsystem to set requirements
+// );
+// }
+
+// /** Periodic function -- update odometry and log everything */
+// @Override
+// public void periodic() {
+// // // When vision is enabled we must manually update odometry in SwerveDrive
+// // if (visionDriveTest) {
+// // swerveDrive.updateOdometry();
+// // vision.updatePoseEstimation(swerveDrive);
+// // }
+
+// /** Log Telemetry Data to AdvantageKit */
+// Logger.recordOutput("SwerveDive/Telemetry/moduleCount", SwerveDriveTelemetry.moduleCount);
+// Logger.recordOutput("SwerveDive/Telemetry/wheelLocations",
+// SwerveDriveTelemetry.wheelLocations);
+// Logger.recordOutput("SwerveDive/Telemetry/measuredStates",
+// SwerveDriveTelemetry.measuredStates);
+// Logger.recordOutput("SwerveDive/Telemetry/desiredStates",
+// SwerveDriveTelemetry.desiredStates);
+// Logger.recordOutput("SwerveDive/Telemetry/robotRotation",
+// SwerveDriveTelemetry.robotRotation);
+// Logger.recordOutput("SwerveDive/Telemetry/maxSpeed", SwerveDriveTelemetry.maxSpeed);
+// Logger.recordOutput("SwerveDive/Telemetry/rotationUnit", SwerveDriveTelemetry.rotationUnit);
+// Logger.recordOutput("SwerveDive/Telemetry/sizeLeftRight",
+// SwerveDriveTelemetry.sizeLeftRight);
+// Logger.recordOutput("SwerveDive/Telemetry/sizeFrontBack",
+// SwerveDriveTelemetry.sizeFrontBack);
+// Logger.recordOutput(
+// "SwerveDive/Telemetry/forwardDirection", SwerveDriveTelemetry.forwardDirection);
+// Logger.recordOutput(
+// "SwerveDive/Telemetry/maxAngularVelocity", SwerveDriveTelemetry.maxAngularVelocity);
+// Logger.recordOutput(
+// "SwerveDive/Telemetry/measuredChassisSpeeds",
+// SwerveDriveTelemetry.measuredChassisSpeeds);
+// Logger.recordOutput(
+// "SwerveDive/Telemetry/desiredChassisSpeeds", SwerveDriveTelemetry.desiredChassisSpeeds);
+
+// /** Log Swerve Drive States to AdvantageKit */
+// getModuleStates();
+// getDesiredStates();
+// Logger.recordOutput(
+// "SwerveDive/States/RobotRotation",
+// SwerveDriveTelemetry.rotationUnit == "degrees"
+// ? Rotation2d.fromDegrees(SwerveDriveTelemetry.robotRotation)
+// : Rotation2d.fromRadians(SwerveDriveTelemetry.robotRotation));
+// }
+
+// /** Simulation periodic function */
+// @Override
+// public void simulationPeriodic() {}
+
+// /**
+// * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a
+// rotation
+// * rate, and calculates and commands module states accordingly. Can use either open-loop or
+// * closed-loop velocity control for the wheel velocities. Also has field- and robot-relative
+// * modes, which affect how the translation vector is used.
+// *
+// * @param translation {@link Translation2d} that is the commanded linear velocity of the robot,
+// in
+// * meters per second. In robot-relative mode, positive x is torwards the bow (front) and
+// * positive y is torwards port (left). In field-relative mode, positive x is away from the
+// * alliance wall (field North) and positive y is torwards the left wall when looking
+// through
+// * the driver station glass (field West).
+// * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by
+// * field/robot relativity.
+// * @param fieldRelative Drive mode. True for field-relative, false for robot-relative.
+// */
+// public void drive(Translation2d translation, double rotation, boolean fieldRelative) {
+
+// // Open loop is disabled since it shouldn't be used most of the time.
+// swerveDrive.drive(translation, rotation, fieldRelative, false);
+// }
+
+// /**
+// * Drive the robot given a chassis field oriented velocity.
+// *
+// * @param velocity Velocity according to the field.
+// */
+// public void driveFieldOriented(ChassisSpeeds velocity) {
+// swerveDrive.driveFieldOriented(velocity);
+// }
+
+// /**
+// * Drive according to the chassis robot oriented velocity.
+// *
+// * @param velocity Robot oriented {@link ChassisSpeeds}
+// */
+// public void drive(ChassisSpeeds velocity) {
+// swerveDrive.drive(velocity);
+// }
+
+// /************************************************************************* */
+// /* COMMAND SECTION -- Swerve-only Commands */
+
+// /**
+// * Command to drive the robot using translative values and heading as a setpoint.
+// *
+// * @param translationX Translation in the X direction. Cubed for smoother controls.
+// * @param translationY Translation in the Y direction. Cubed for smoother controls.
+// * @param headingX Heading X to calculate angle of the joystick.
+// * @param headingY Heading Y to calculate angle of the joystick.
+// * @return Drive command.
+// */
+// public Command driveCommand(
+// DoubleSupplier translationX,
+// DoubleSupplier translationY,
+// DoubleSupplier headingX,
+// DoubleSupplier headingY) {
+// // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for
+// // this kind of control.
+// return run(
+// () -> {
+// Translation2d scaledInputs =
+// SwerveMath.scaleTranslation(
+// new Translation2d(translationX.getAsDouble(), translationY.getAsDouble()),
+// 0.8);
+
+// // Make the robot move
+// driveFieldOriented(
+// swerveDrive.swerveController.getTargetSpeeds(
+// scaledInputs.getX(),
+// scaledInputs.getY(),
+// headingX.getAsDouble(),
+// headingY.getAsDouble(),
+// swerveDrive.getOdometryHeading().getRadians(),
+// swerveDrive.getMaximumVelocity()));
+// });
+// }
+
+// /**
+// * Command to drive the robot using translative values and heading as angular velocity. NOTE:
+// * Alternate drive command constructor
+// *
+// * @param translationX Translation in the X direction. Cubed for smoother controls.
+// * @param translationY Translation in the Y direction. Cubed for smoother controls.
+// * @param angularRotationX Angular velocity of the robot to set. Cubed for smoother controls.
+// * @return Drive command.
+// */
+// public Command driveCommand(
+// DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX)
+// {
+// return run(
+// () -> {
+// // Make the robot move
+// swerveDrive.drive(
+// SwerveMath.scaleTranslation(
+// new Translation2d(
+// translationX.getAsDouble() * swerveDrive.getMaximumVelocity(),
+// translationY.getAsDouble() * swerveDrive.getMaximumVelocity()),
+// 0.8),
+// Math.pow(angularRotationX.getAsDouble(), 3) *
+// swerveDrive.getMaximumAngularVelocity(),
+// true,
+// false);
+// });
+// }
+
+// /**
+// * Command to drive the robot using translative values and heading as a setpoint.
+// *
+// * @param translationX Translation in the X direction.
+// * @param translationY Translation in the Y direction.
+// * @param rotation Rotation as a value between [-1, 1] converted to radians.
+// * @return Drive command.
+// */
+// public Command simDriveCommand(
+// DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier rotation) {
+// // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for
+// // this kind of control.
+// return run(
+// () -> {
+// // Make the robot move
+// driveFieldOriented(
+// swerveDrive.swerveController.getTargetSpeeds(
+// translationX.getAsDouble(),
+// translationY.getAsDouble(),
+// rotation.getAsDouble() * Math.PI,
+// swerveDrive.getOdometryHeading().getRadians(),
+// swerveDrive.getMaximumVelocity()));
+// });
+// }
+
+// /**
+// * Get the path follower with events.
+// *
+// * @param pathName PathPlanner path name.
+// * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command.
+// */
+// public Command getAutonomousCommand(String pathName) {
+// // Create a path following command using AutoBuilder. This will also trigger event markers.
+// return new PathPlannerAuto(pathName);
+// }
+
+// /**
+// * Use PathPlanner Path finding to go to a point on the field.
+// *
+// * @param pose Target {@link Pose2d} to go to.
+// * @return PathFinding command
+// */
+// public Command driveToPose(Pose2d pose) {
+// // Create the constraints to use while pathfinding
+// PathConstraints constraints =
+// new PathConstraints(kMaxLinearSpeed, kMaxLinearAccel, kMaxAngularSpeed,
+// kMaxAngularAccel);
+
+// // Since AutoBuilder is configured, we can use it to build pathfinding commands
+// return AutoBuilder.pathfindToPose(
+// pose,
+// constraints,
+// 0.0, // Goal end velocity in meters/sec
+// 0.0 // Rotation delay distance in meters. This is how far the robot should travel before
+// // attempting to rotate.
+// );
+// }
+
+// /**
+// * Command to characterize the robot drive motors using SysId
+// *
+// * @return SysId Drive Command
+// */
+// public Command sysIdDriveMotorCommand() {
+// return SwerveDriveTest.generateSysIdCommand(
+// SwerveDriveTest.setDriveSysIdRoutine(new Config(), this, swerveDrive, kMaxV),
+// kDelay,
+// kQuasiTimeout,
+// kDynamicTimeout);
+// }
+
+// /**
+// * Command to characterize the robot angle motors using SysId
+// *
+// * @return SysId Angle Command
+// */
+// public Command sysIdAngleMotorCommand() {
+// return SwerveDriveTest.generateSysIdCommand(
+// SwerveDriveTest.setAngleSysIdRoutine(new Config(), this, swerveDrive),
+// kDelay,
+// kQuasiTimeout,
+// kDynamicTimeout);
+// }
+
+// /**
+// * Returns a Command that centers the modules of the SwerveDrive subsystem.
+// *
+// * @return a Command that centers the modules of the SwerveDrive subsystem
+// */
+// public Command centerModulesCommand() {
+// return run(() -> Arrays.asList(swerveDrive.getModules()).forEach(it -> it.setAngle(0.0)));
+// }
+
+// /**
+// * Returns a Command that drives the swerve drive to a specific distance at a given speed.
+// *
+// * @param distanceInMeters the distance to drive in meters
+// * @param speedInMetersPerSecond the speed at which to drive in meters per second
+// * @return a Command that drives the swerve drive to a specific distance at a given speed
+// */
+// public Command driveToDistanceCommand(double distanceInMeters, double speedInMetersPerSecond) {
+// return Commands.deferredProxy(
+// () ->
+// Commands.run(() -> drive(new ChassisSpeeds(speedInMetersPerSecond, 0, 0)), this)
+// .until(
+// () ->
+// swerveDrive.getPose().getTranslation().getDistance(new Translation2d(0,
+// 0))
+// > distanceInMeters));
+// }
+
+// /************************************************************************* */
+// /* UTILITY SECTION -- Utility methods */
+
+// /**
+// * Get the swerve drive kinematics object.
+// *
+// * @return {@link SwerveDriveKinematics} of the swerve drive.
+// */
+// @AutoLogOutput(key = "Odometry/Kinematics")
+// public SwerveDriveKinematics getKinematics() {
+// return swerveDrive.kinematics;
+// }
+
+// /**
+// * Gets the current pose (position and rotation) of the robot, as reported by odometry.
+// *
+// * @return The robot's pose
+// */
+// @AutoLogOutput(key = "Odometry/RobotPose")
+// public Pose2d getPose() {
+// return swerveDrive.getPose();
+// }
+
+// /**
+// * Gets the current yaw angle of the robot, as reported by the swerve pose estimator in the
+// * underlying drivebase. Note, this is not the raw gyro reading, this may be corrected from
+// calls
+// * to resetOdometry().
+// *
+// * @return The yaw angle
+// */
+// @AutoLogOutput(key = "Odometry/Heading")
+// public Rotation2d getHeading() {
+// return getPose().getRotation();
+// }
+
+// /**
+// * Gets the current field-relative velocity (x, y and omega) of the robot
+// *
+// * @return A ChassisSpeeds object of the current field-relative velocity
+// */
+// @AutoLogOutput(key = "Odometry/FieldVelocity")
+// public ChassisSpeeds getFieldVelocity() {
+// return swerveDrive.getFieldVelocity();
+// }
+
+// /**
+// * Gets the current velocity (x, y and omega) of the robot
+// *
+// * @return A {@link ChassisSpeeds} object of the current velocity
+// */
+// @AutoLogOutput(key = "Odometry/RobotVelocity")
+// public ChassisSpeeds getRobotVelocity() {
+// return swerveDrive.getRobotVelocity();
+// }
+
+// /**
+// * Gets the current pitch angle of the robot, as reported by the imu.
+// *
+// * @return The heading as a {@link Rotation2d} angle
+// */
+// @AutoLogOutput(key = "Odometry/Pitch")
+// public Rotation2d getPitch() {
+// return swerveDrive.getPitch();
+// }
+
+// /**
+// * Set chassis speeds with closed-loop velocity control.
+// *
+// * @param chassisSpeeds Chassis Speeds to set.
+// */
+// public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) {
+// swerveDrive.setChassisSpeeds(chassisSpeeds);
+// }
+
+// /**
+// * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset
+// when
+// * calling this method. However, if either gyro angle or module position is reset, this must be
+// * called in order for odometry to keep working.
+// *
+// * @param initialHolonomicPose The pose to set the odometry to
+// */
+// public void resetOdometry(Pose2d initialHolonomicPose) {
+// swerveDrive.resetOdometry(initialHolonomicPose);
+// }
+
+// /**
+// * Post the trajectory to the field.
+// *
+// * @param trajectory The trajectory to post.
+// */
+// public void postTrajectory(Trajectory trajectory) {
+// swerveDrive.postTrajectory(trajectory);
+// }
+
+// /**
+// * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0.
+// */
+// public void zeroGyro() {
+// swerveDrive.zeroGyro();
+// }
+
+// /**
+// * Checks if the alliance is red, defaults to false if alliance isn't available.
+// *
+// * @return true if the red alliance, false if blue. Defaults to false if none is available.
+// */
+// private boolean isRedAlliance() {
+// var alliance = DriverStation.getAlliance();
+// return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false;
+// }
+
+// /**
+// * This will zero (calibrate) the robot to assume the current position is facing forward
+// *
+// * If red alliance rotate the robot 180 after the drviebase zero command
+// */
+// public void zeroGyroWithAlliance() {
+// if (isRedAlliance()) {
+// zeroGyro();
+// // Set the pose 180 degrees
+// resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(180)));
+// } else {
+// zeroGyro();
+// }
+// }
+
+// /**
+// * Sets the drive motors to brake/coast mode.
+// *
+// * @param brake True to set motors to brake mode, false for coast.
+// */
+// public void setMotorBrake(boolean brake) {
+// swerveDrive.setMotorIdleMode(brake);
+// }
+
+// /**
+// * Get the chassis speeds based on controller input of 2 joysticks. One for speeds in which
+// * direction. The other for the angle of the robot.
+// *
+// * @param xInput X joystick input for the robot to move in the X direction.
+// * @param yInput Y joystick input for the robot to move in the Y direction.
+// * @param headingX X joystick which controls the angle of the robot.
+// * @param headingY Y joystick which controls the angle of the robot.
+// * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive.
+// */
+// public ChassisSpeeds getTargetSpeeds(
+// double xInput, double yInput, double headingX, double headingY) {
+// Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput));
+// return swerveDrive.swerveController.getTargetSpeeds(
+// scaledInputs.getX(),
+// scaledInputs.getY(),
+// headingX,
+// headingY,
+// getHeading().getRadians(),
+// swerveDrive.getMaximumVelocity());
+// }
+
+// /**
+// * Get the chassis speeds based on controller input of 1 joystick and one angle. Control the
+// robot
+// * at an offset of 90deg.
+// *
+// * @param xInput X joystick input for the robot to move in the X direction.
+// * @param yInput Y joystick input for the robot to move in the Y direction.
+// * @param angle The angle in as a {@link Rotation2d}.
+// * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive.
+// */
+// public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) {
+// Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput));
+
+// return swerveDrive.swerveController.getTargetSpeeds(
+// scaledInputs.getX(),
+// scaledInputs.getY(),
+// angle.getRadians(),
+// getHeading().getRadians(),
+// swerveDrive.getMaximumVelocity());
+// }
+
+// /**
+// * Get the {@link SwerveController} in the swerve drive.
+// *
+// * @return {@link SwerveController} from the {@link SwerveDrive}.
+// */
+// public SwerveController getSwerveController() {
+// return swerveDrive.swerveController;
+// }
+
+// /**
+// * Get the {@link SwerveDriveConfiguration} object.
+// *
+// * @return The {@link SwerveDriveConfiguration} for the current drive.
+// */
+// @AutoLogOutput(key = "SwerveDrive/Config")
+// public SwerveDriveConfiguration getSwerveDriveConfiguration() {
+// return swerveDrive.swerveDriveConfiguration;
+// }
+
+// /** Lock the swerve drive to prevent it from moving. */
+// public void lock() {
+// swerveDrive.lockPose();
+// }
+
+// /** Add a fake vision reading for testing purposes. */
+// public void addFakeVisionReading() {
+// swerveDrive.addVisionMeasurement(
+// new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp());
+// }
+
+// /**
+// * Sets the maximum speed of the swerve drive.
+// *
+// * @param maximumSpeedInMetersPerSecond the maximum speed to set for the swerve drive in meters
+// * per second
+// */
+// public void setMaximumSpeed(double maximumSpeedInMetersPerSecond) {
+// swerveDrive.setMaximumSpeed(
+// maximumSpeedInMetersPerSecond,
+// false,
+// swerveDrive.swerveDriveConfiguration.physicalCharacteristics.optimalVoltage);
+// }
+
+// /**
+// * Replaces the swerve module feedforward with a new SimpleMotorFeedforward object.
+// *
+// * @param kS the static gain of the feedforward
+// * @param kV the velocity gain of the feedforward
+// * @param kA the acceleration gain of the feedforward
+// */
+// public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) {
+// swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA));
+// }
+
+// /** Returns the module states (turn angles and drive velocities) for all of the modules. */
+// @AutoLogOutput(key = "SwerveDive/States/Measured")
+// private SwerveModuleState[] getModuleStates() {
+// SwerveModuleState[] states = new SwerveModuleState[4];
+// for (int i = 0; i < 4; i++) {
+// states[i] =
+// new SwerveModuleState(
+// SwerveDriveTelemetry.measuredStates[(i * 2) + 1],
+// Rotation2d.fromDegrees(SwerveDriveTelemetry.measuredStates[i * 2]));
+// }
+// return states;
+// }
+
+// /** Returns the desired states (turn angles and drive velocities) for all of the modules. */
+// @AutoLogOutput(key = "SwerveDive/States/Desired")
+// private SwerveModuleState[] getDesiredStates() {
+// SwerveModuleState[] states = new SwerveModuleState[4];
+// for (int i = 0; i < 4; i++) {
+// states[i] =
+// new SwerveModuleState(
+// SwerveDriveTelemetry.desiredStates[(i * 2) + 1],
+// Rotation2d.fromDegrees(SwerveDriveTelemetry.desiredStates[i * 2]));
+// }
+// return states;
+// }
+
+// /************************************************************************* */
+// /** 2024 SEASON-SPECIFIC FUNCTIONS, INCLUDED AS EXAMPLES */
+
+// /**
+// * Aim the robot at the speaker.
+// *
+// * @param tolerance Tolerance in degrees.
+// * @return Command to turn the robot to the speaker.
+// */
+// public Command aimAtSpeaker(double tolerance) {
+// SwerveController controller = swerveDrive.getSwerveController();
+// return run(() -> {
+// drive(
+// ChassisSpeeds.fromFieldRelativeSpeeds(
+// 0,
+// 0,
+// controller.headingCalculate(
+// getHeading().getRadians(),
+// vision.getSpeakerYaw(swerveDrive.getOdometryHeading()).getRadians()),
+// getHeading()));
+// })
+// .until(
+// () ->
+// Math.abs(
+// vision
+// .getSpeakerYaw(swerveDrive.getOdometryHeading())
+// .minus(getHeading())
+// .getDegrees())
+// < tolerance);
+// }
+
+// /**
+// * Aim the robot at the target returned by PhotonVision.
+// *
+// * @param camera {@link PhotonCamera} to communicate with.
+// * @return A {@link Command} which will run the alignment.
+// */
+// public Command aimAtTarget(PhotonCamera camera) {
+
+// return run(
+// () -> {
+// PhotonPipelineResult result = camera.getLatestResult();
+// if (result.hasTargets()) {
+// drive(
+// getTargetSpeeds(
+// 0,
+// 0,
+// Rotation2d.fromDegrees(
+// result
+// .getBestTarget()
+// .getYaw()))); // Not sure if this will work, more math may be
+// required.
+// }
+// });
+// }
+// }
diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java
new file mode 100644
index 00000000..996600ab
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/vision/Vision.java
@@ -0,0 +1,172 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright (c) 2024 FRC 2486
+// http://github.com/Coconuts2486-FRC
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.vision;
+
+import static frc.robot.Constants.AprilTagConstants.*;
+import static frc.robot.Constants.VisionConstants.*;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc.robot.RobotContainer.Cameras;
+import frc.robot.subsystems.vision.VisionIO.VisionIOInputs;
+import frc.robot.util.GeomUtil;
+import frc.robot.util.LoggedTunableNumber;
+import frc.robot.util.VirtualSubsystem;
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.function.Supplier;
+import lombok.experimental.ExtensionMethod;
+import org.littletonrobotics.junction.Logger;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+/** Vision subsystem for AprilTag vision (built upon a virtual subsystem) */
+@ExtensionMethod({GeomUtil.class})
+public class Vision extends VirtualSubsystem {
+
+ private static final LoggedTunableNumber timestampOffset =
+ new LoggedTunableNumber("AprilTagVision/TimestampOffset", -(1.0 / 50.0));
+ private static final double demoTagPosePersistenceSecs = 0.5;
+
+ // Tag sources and layout
+ private final Supplier aprilTagTypeSupplier;
+ private final VisionIO[] io;
+ private final VisionIOInputs[] inputs;
+
+ // Bookkeeping for last frame exposure and last tag detection
+ private final Map lastFrameTimes = new HashMap<>();
+ private final Map lastTagDetectionTimes = new HashMap<>();
+
+ // Something to do with demo?
+ private Pose3d demoTagPose = null;
+ private double lastDemoTagPoseTimestamp = 0.0;
+ public static Pose2d robotPose = null;
+ public static Pose3d speakerPose = null;
+ private static Translation3d speakerTranslation = null;
+ private static Rotation3d robotRotation = null;
+ private static Pose3d thisSpeakerPose = null;
+ private static Pose3d thisRobotPose = null;
+
+ // Class method definition, including inputs
+ public Vision(Supplier aprilTagTypeSupplier, VisionIO... io) {
+ this.aprilTagTypeSupplier = aprilTagTypeSupplier;
+ this.io = io;
+ inputs = new VisionIOInputs[io.length];
+ for (int i = 0; i < io.length; i++) {
+ inputs[i] = new VisionIOInputs();
+ }
+
+ // Create map of last frame times for instances
+ for (int i = 0; i < io.length; i++) {
+ lastFrameTimes.put(i, 0.0);
+ }
+ }
+
+ public void periodic() {
+ for (int i = 0; i < io.length; i++) {
+ io[i].updateInputs(inputs[i]);
+ // NOTE: This line causes the RIO to overrun RAM
+ // Logger.processInputs("AprilTagVision/Inst" + i, inputs[i]);
+ }
+
+ // Loop over cameras
+ List allSpeakerPoses = new ArrayList<>();
+ List allRobotPoses = new ArrayList<>();
+ List allSpeakerPoses3d = new ArrayList<>();
+ for (int instanceIndex = 0; instanceIndex < io.length; instanceIndex++) {
+ var timestamp = inputs[instanceIndex].timestamp;
+ var latency = inputs[instanceIndex].latency;
+ var targets = inputs[instanceIndex].targets;
+
+ // Exit if no targets
+ if (targets == null) {
+ continue;
+ }
+
+ // Loop over found targets
+ for (PhotonTrackedTarget target : targets) {
+
+ // Get the speaker of interest for this alliance
+ if ((target.getFiducialId() == 4 && DriverStation.getAlliance().get() == Alliance.Red)
+ || (target.getFiducialId() == 7
+ && DriverStation.getAlliance().get() == Alliance.Blue)) {
+
+ // Camera pose is "WHERE IS THE CAMERA AS SEEN FROM TAG"; invert to get tag from camera
+ Pose3d cameraPose = GeomUtil.toPose3d(target.getBestCameraToTarget().inverse());
+ thisRobotPose =
+ cameraPose.transformBy(Cameras.cameraPoses[instanceIndex].toTransform3d().inverse());
+
+ // Get the speaker pose from the point of view of the robot
+ robotRotation =
+ thisRobotPose.getRotation(); // This is rotation of bot w.r.t. tag coordinate system
+ speakerTranslation =
+ thisRobotPose.getTranslation().unaryMinus().rotateBy(robotRotation.unaryMinus());
+ thisSpeakerPose = new Pose3d(speakerTranslation, new Rotation3d());
+
+ allSpeakerPoses3d.add(thisSpeakerPose);
+ allSpeakerPoses.add(thisSpeakerPose.toPose2d());
+ allRobotPoses.add(thisRobotPose.toPose2d());
+
+ // Log the relevant information to AdvantageKit
+ Logger.recordOutput("PhotonVision/Speaker_" + inputs[instanceIndex].camname, speakerPose);
+ Logger.recordOutput(
+ "PhotonVision/Robot2d_" + inputs[instanceIndex].camname,
+ robotRotation.toRotation2d());
+ }
+ }
+ }
+ // Set output value based on number of tags seen
+ switch ((int) allSpeakerPoses3d.size()) {
+ case 0:
+ // If no speaker tags, return a null Pose3d
+ speakerPose = null;
+ robotPose = null;
+ break;
+ case 1:
+ // One tag seen, return it
+ speakerPose = allSpeakerPoses3d.get(0);
+ robotPose = allRobotPoses.get(0);
+ break;
+ default:
+ // Otherwise, compute the average speakerPose
+ Translation3d strans0 = allSpeakerPoses3d.get(0).getTranslation();
+ Translation3d strans1 = allSpeakerPoses3d.get(1).getTranslation();
+ speakerPose =
+ new Pose3d(
+ (strans0.getX() + strans1.getX()) / 2.,
+ (strans0.getY() + strans1.getY()) / 2.,
+ (strans0.getZ() + strans1.getZ()) / 2.,
+ new Rotation3d());
+ // And then compute the average robotPose
+ Translation2d rtrans0 = allRobotPoses.get(0).getTranslation();
+ Translation2d rtrans1 = allRobotPoses.get(1).getTranslation();
+ Rotation2d rrot0 = allRobotPoses.get(0).getRotation();
+ Rotation2d rrot1 = allRobotPoses.get(1).getRotation();
+ robotPose =
+ new Pose2d(
+ (rtrans0.getX() + rtrans1.getX()) / 2.0,
+ (rtrans0.getY() + rtrans1.getY()) / 2.0,
+ rrot0.plus(rrot1).div(2.0));
+ }
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java
new file mode 100644
index 00000000..4c0246c8
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java
@@ -0,0 +1,54 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.vision;
+
+import java.util.ArrayList;
+import java.util.List;
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+public interface VisionIO {
+
+ class VisionIOInputs implements LoggableInputs {
+ public String camname = "";
+ public double latency = 0.0;
+ public double timestamp = 0.0;
+ public List targets = new ArrayList() {};
+
+ public long fps = 0;
+
+ @Override
+ public void toLog(LogTable table) {
+ table.put("Latency", latency);
+ table.put("Timestamp", timestamp);
+ table.put("TargetCount", targets.size());
+ for (int i = 0; i < targets.size(); i++) {
+ table.put("Target/" + i, targets.get(i));
+ }
+ table.put("Fps", fps);
+ }
+
+ @Override
+ public void fromLog(LogTable table) {
+ latency = table.get("Latency", 0.0);
+ timestamp = table.get("Timestamp", 0.0);
+ int targetCount = table.get("TargetCount", 0);
+ targets = new ArrayList(targetCount);
+ fps = table.get("Fps", 0);
+ }
+ }
+
+ default void updateInputs(VisionIOInputs inputs) {}
+}
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java
new file mode 100644
index 00000000..caa72314
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java
@@ -0,0 +1,77 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.vision;
+
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType;
+import frc.robot.util.Alert;
+import java.util.List;
+import java.util.function.Supplier;
+import org.littletonrobotics.junction.Logger;
+import org.photonvision.PhotonCamera;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+public class VisionIOPhoton implements VisionIO {
+
+ private final Supplier aprilTagTypeSupplier;
+ private AprilTagLayoutType lastAprilTagType = null;
+
+ private static final double disconnectedTimeout = 0.5;
+ private final Alert disconnectedAlert;
+ private final Timer disconnectedTimer = new Timer();
+ public final PhotonCamera camera;
+ public final String camname;
+
+ public VisionIOPhoton(Supplier aprilTagTypeSupplier, String camname) {
+ this.aprilTagTypeSupplier = aprilTagTypeSupplier;
+ this.camname = camname;
+ this.camera = new PhotonCamera(camname);
+
+ disconnectedAlert = new Alert("No data from \"" + camname + "\"", Alert.AlertType.ERROR);
+ disconnectedTimer.start();
+ }
+
+ public void updateInputs(VisionIOInputs inputs) {
+ // Get observations
+ var result = camera.getLatestResult();
+
+ // Log the entire result for each camera to AdvantageKit
+ Logger.recordOutput("PhotonVision/" + camname, result); // result.getLatencyMillis());
+
+ // Put the relevant information into the `inputs`
+ inputs.camname = camname;
+ inputs.latency = result.getLatencyMillis();
+ inputs.timestamp = result.getTimestampSeconds();
+
+ if (result.hasTargets()) {
+ List targets = result.getTargets();
+ inputs.targets = targets;
+ for (PhotonTrackedTarget target : targets) {
+ // Get information from target.
+ int targetID = target.getFiducialId();
+ double poseAmbiguity = target.getPoseAmbiguity();
+ Transform3d bestCameraToTarget = target.getBestCameraToTarget();
+ Transform3d alternateCameraToTarget = target.getAlternateCameraToTarget();
+
+ String logTag = "PhotonVision/Tag" + Integer.toString(targetID);
+ Logger.recordOutput(logTag + "/PoseAmbiguity", poseAmbiguity);
+ Logger.recordOutput(logTag + "/BestCameraToTarget", bestCameraToTarget);
+ Logger.recordOutput(logTag + "/AlternateCameraToTarget", alternateCameraToTarget);
+ }
+ } else {
+ inputs.targets = null;
+ }
+ }
+}
diff --git a/src/main/java/frc/robot/util/Alert.java b/src/main/java/frc/robot/util/Alert.java
new file mode 100644
index 00000000..5608c3b7
--- /dev/null
+++ b/src/main/java/frc/robot/util/Alert.java
@@ -0,0 +1,155 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.util;
+
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.function.Predicate;
+
+/** Class for managing persistent alerts to be sent over NetworkTables. */
+public class Alert {
+ private static Map groups = new HashMap();
+
+ private final AlertType type;
+ private boolean active = false;
+ private double activeStartTime = 0.0;
+ private String text;
+
+ /**
+ * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated,
+ * the appropriate entries will be added to NetworkTables.
+ *
+ * @param text Text to be displayed when the alert is active.
+ * @param type Alert level specifying urgency.
+ */
+ public Alert(String text, AlertType type) {
+ this("Alerts", text, type);
+ }
+
+ /**
+ * Creates a new Alert. If this is the first to be instantiated in its group, the appropriate
+ * entries will be added to NetworkTables.
+ *
+ * @param group Group identifier, also used as NetworkTables title
+ * @param text Text to be displayed when the alert is active.
+ * @param type Alert level specifying urgency.
+ */
+ public Alert(String group, String text, AlertType type) {
+ if (!groups.containsKey(group)) {
+ groups.put(group, new SendableAlerts());
+ SmartDashboard.putData(group, groups.get(group));
+ }
+
+ this.text = text;
+ this.type = type;
+ groups.get(group).alerts.add(this);
+ }
+
+ /**
+ * Sets whether the alert should currently be displayed. When activated, the alert text will also
+ * be sent to the console.
+ */
+ public void set(boolean active) {
+ if (active && !this.active) {
+ activeStartTime = Timer.getFPGATimestamp();
+ switch (type) {
+ case ERROR:
+ DriverStation.reportError(text, false);
+ break;
+ case WARNING:
+ DriverStation.reportWarning(text, false);
+ break;
+ case INFO:
+ System.out.println(text);
+ break;
+ }
+ }
+ this.active = active;
+ }
+
+ /** Updates current alert text. */
+ public void setText(String text) {
+ if (active && !text.equals(this.text)) {
+ switch (type) {
+ case ERROR:
+ DriverStation.reportError(text, false);
+ break;
+ case WARNING:
+ DriverStation.reportWarning(text, false);
+ break;
+ case INFO:
+ System.out.println(text);
+ break;
+ }
+ }
+ this.text = text;
+ }
+
+ private static class SendableAlerts implements Sendable {
+ public final List alerts = new ArrayList<>();
+
+ public String[] getStrings(AlertType type) {
+ Predicate activeFilter = (Alert x) -> x.type == type && x.active;
+ Comparator timeSorter =
+ (Alert a1, Alert a2) -> (int) (a2.activeStartTime - a1.activeStartTime);
+ return alerts.stream()
+ .filter(activeFilter)
+ .sorted(timeSorter)
+ .map((Alert a) -> a.text)
+ .toArray(String[]::new);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Alerts");
+ builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null);
+ builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null);
+ builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null);
+ }
+ }
+
+ /** Represents an alert's level of urgency. */
+ public static enum AlertType {
+ /**
+ * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type
+ * for problems which will seriously affect the robot's functionality and thus require immediate
+ * attention.
+ */
+ ERROR,
+
+ /**
+ * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this
+ * type for problems which could affect the robot's functionality but do not necessarily require
+ * immediate attention.
+ */
+ WARNING,
+
+ /**
+ * Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type
+ * for problems which are unlikely to affect the robot's functionality, or any other alerts
+ * which do not fall under "ERROR" or "WARNING".
+ */
+ INFO
+ }
+}
diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java
new file mode 100644
index 00000000..3a59a7fd
--- /dev/null
+++ b/src/main/java/frc/robot/util/GeomUtil.java
@@ -0,0 +1,171 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.util;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+
+/** Geometry utilities for working with translations, rotations, transforms, and poses. */
+public class GeomUtil {
+ /**
+ * Creates a pure translating transform
+ *
+ * @param translation The translation to create the transform with
+ * @return The resulting transform
+ */
+ public static Transform2d toTransform2d(Translation2d translation) {
+ return new Transform2d(translation, new Rotation2d());
+ }
+
+ /**
+ * Creates a pure translating transform
+ *
+ * @param x The x coordinate of the translation
+ * @param y The y coordinate of the translation
+ * @return The resulting transform
+ */
+ public static Transform2d toTransform2d(double x, double y) {
+ return new Transform2d(x, y, new Rotation2d());
+ }
+
+ /**
+ * Creates a pure rotating transform
+ *
+ * @param rotation The rotation to create the transform with
+ * @return The resulting transform
+ */
+ public static Transform2d toTransform2d(Rotation2d rotation) {
+ return new Transform2d(new Translation2d(), rotation);
+ }
+
+ /**
+ * Converts a Pose2d to a Transform2d to be used in a kinematic chain
+ *
+ * @param pose The pose that will represent the transform
+ * @return The resulting transform
+ */
+ public static Transform2d toTransform2d(Pose2d pose) {
+ return new Transform2d(pose.getTranslation(), pose.getRotation());
+ }
+
+ public static Pose2d inverse(Pose2d pose) {
+ Rotation2d rotationInverse = pose.getRotation().unaryMinus();
+ return new Pose2d(
+ pose.getTranslation().unaryMinus().rotateBy(rotationInverse), rotationInverse);
+ }
+
+ /**
+ * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic
+ * chain
+ *
+ * @param transform The transform that will represent the pose
+ * @return The resulting pose
+ */
+ public static Pose2d toPose2d(Transform2d transform) {
+ return new Pose2d(transform.getTranslation(), transform.getRotation());
+ }
+
+ /**
+ * Creates a pure translated pose
+ *
+ * @param translation The translation to create the pose with
+ * @return The resulting pose
+ */
+ public static Pose2d toPose2d(Translation2d translation) {
+ return new Pose2d(translation, new Rotation2d());
+ }
+
+ /**
+ * Creates a pure rotated pose
+ *
+ * @param rotation The rotation to create the pose with
+ * @return The resulting pose
+ */
+ public static Pose2d toPose2d(Rotation2d rotation) {
+ return new Pose2d(new Translation2d(), rotation);
+ }
+
+ /**
+ * Multiplies a twist by a scaling factor
+ *
+ * @param twist The twist to multiply
+ * @param factor The scaling factor for the twist components
+ * @return The new twist
+ */
+ public static Twist2d multiply(Twist2d twist, double factor) {
+ return new Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor);
+ }
+
+ /**
+ * Converts a Pose3d to a Transform3d to be used in a kinematic chain
+ *
+ * @param pose The pose that will represent the transform
+ * @return The resulting transform
+ */
+ public static Transform3d toTransform3d(Pose3d pose) {
+ return new Transform3d(pose.getTranslation(), pose.getRotation());
+ }
+
+ /**
+ * Converts a Transform3d to a Pose3d to be used as a position or as the start of a kinematic
+ * chain
+ *
+ * @param transform The transform that will represent the pose
+ * @return The resulting pose
+ */
+ public static Pose3d toPose3d(Transform3d transform) {
+ return new Pose3d(transform.getTranslation(), transform.getRotation());
+ }
+
+ /**
+ * Converts a ChassisSpeeds to a Twist2d by extracting two dimensions (Y and Z). chain
+ *
+ * @param speeds The original translation
+ * @return The resulting translation
+ */
+ public static Twist2d toTwist2d(ChassisSpeeds speeds) {
+ return new Twist2d(
+ speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond);
+ }
+
+ /**
+ * Creates a new pose from an existing one using a different translation value.
+ *
+ * @param pose The original pose
+ * @param translation The new translation to use
+ * @return The new pose with the new translation and original rotation
+ */
+ public static Pose2d withTranslation(Pose2d pose, Translation2d translation) {
+ return new Pose2d(translation, pose.getRotation());
+ }
+
+ /**
+ * Creates a new pose from an existing one using a different rotation value.
+ *
+ * @param pose The original pose
+ * @param rotation The new rotation to use
+ * @return The new pose with the original translation and new rotation
+ */
+ public static Pose2d withRotation(Pose2d pose, Rotation2d rotation) {
+ return new Pose2d(pose.getTranslation(), rotation);
+ }
+}
diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java
new file mode 100644
index 00000000..08b61727
--- /dev/null
+++ b/src/main/java/frc/robot/util/LocalADStarAK.java
@@ -0,0 +1,169 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright (c) 2024 Michael Jansen
+// http://gist.github.com/mjansen4857
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.util;
+
+import com.pathplanner.lib.path.GoalEndState;
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.path.PathPoint;
+import com.pathplanner.lib.pathfinding.LocalADStar;
+import com.pathplanner.lib.pathfinding.Pathfinder;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Translation2d;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+
+// PathPlannerLib pathfinder compatible with AdvantageKit log replay
+// NOTE: This file is available at
+// https://gist.github.com/mjansen4857/a8024b55eb427184dbd10ae8923bd57d
+
+public class LocalADStarAK implements Pathfinder {
+ private final ADStarIO io = new ADStarIO();
+
+ /**
+ * Get if a new path has been calculated since the last time a path was retrieved
+ *
+ * @return True if a new path is available
+ */
+ @Override
+ public boolean isNewPathAvailable() {
+ if (!Logger.hasReplaySource()) {
+ io.updateIsNewPathAvailable();
+ }
+
+ Logger.processInputs("LocalADStarAK", io);
+
+ return io.isNewPathAvailable;
+ }
+
+ /**
+ * Get the most recently calculated path
+ *
+ * @param constraints The path constraints to use when creating the path
+ * @param goalEndState The goal end state to use when creating the path
+ * @return The PathPlannerPath created from the points calculated by the pathfinder
+ */
+ @Override
+ public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) {
+ if (!Logger.hasReplaySource()) {
+ io.updateCurrentPathPoints(constraints, goalEndState);
+ }
+
+ Logger.processInputs("LocalADStarAK", io);
+
+ if (io.currentPathPoints.isEmpty()) {
+ return null;
+ }
+
+ return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState);
+ }
+
+ /**
+ * Set the start position to pathfind from
+ *
+ * @param startPosition Start position on the field. If this is within an obstacle it will be
+ * moved to the nearest non-obstacle node.
+ */
+ @Override
+ public void setStartPosition(Translation2d startPosition) {
+ if (!Logger.hasReplaySource()) {
+ io.adStar.setStartPosition(startPosition);
+ }
+ }
+
+ /**
+ * Set the goal position to pathfind to
+ *
+ * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved
+ * to the nearest non-obstacle node.
+ */
+ @Override
+ public void setGoalPosition(Translation2d goalPosition) {
+ if (!Logger.hasReplaySource()) {
+ io.adStar.setGoalPosition(goalPosition);
+ }
+ }
+
+ /**
+ * Set the dynamic obstacles that should be avoided while pathfinding.
+ *
+ * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents
+ * opposite corners of a bounding box.
+ * @param currentRobotPos The current position of the robot. This is needed to change the start
+ * position of the path to properly avoid obstacles
+ */
+ @Override
+ public void setDynamicObstacles(
+ List> obs, Translation2d currentRobotPos) {
+ if (!Logger.hasReplaySource()) {
+ io.adStar.setDynamicObstacles(obs, currentRobotPos);
+ }
+ }
+
+ private static class ADStarIO implements LoggableInputs {
+ public LocalADStar adStar = new LocalADStar();
+ public boolean isNewPathAvailable = false;
+ public List currentPathPoints = Collections.emptyList();
+
+ @Override
+ public void toLog(LogTable table) {
+ table.put("IsNewPathAvailable", isNewPathAvailable);
+
+ double[] pointsLogged = new double[currentPathPoints.size() * 2];
+ int idx = 0;
+ for (PathPoint point : currentPathPoints) {
+ pointsLogged[idx] = point.position.getX();
+ pointsLogged[idx + 1] = point.position.getY();
+ idx += 2;
+ }
+
+ table.put("CurrentPathPoints", pointsLogged);
+ }
+
+ @Override
+ public void fromLog(LogTable table) {
+ isNewPathAvailable = table.get("IsNewPathAvailable", false);
+
+ double[] pointsLogged = table.get("CurrentPathPoints", new double[0]);
+
+ List pathPoints = new ArrayList<>();
+ for (int i = 0; i < pointsLogged.length; i += 2) {
+ pathPoints.add(
+ new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null));
+ }
+
+ currentPathPoints = pathPoints;
+ }
+
+ public void updateIsNewPathAvailable() {
+ isNewPathAvailable = adStar.isNewPathAvailable();
+ }
+
+ public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) {
+ PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState);
+
+ if (currentPath != null) {
+ currentPathPoints = currentPath.getAllPathPoints();
+ } else {
+ currentPathPoints = Collections.emptyList();
+ }
+ }
+ }
+}
diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java
new file mode 100644
index 00000000..3ab4f3d9
--- /dev/null
+++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java
@@ -0,0 +1,131 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.util;
+
+import frc.robot.Constants;
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.function.Consumer;
+import java.util.function.DoubleSupplier;
+import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
+
+/**
+ * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or
+ * value not in dashboard.
+ */
+public class LoggedTunableNumber implements DoubleSupplier {
+ private static final String tableKey = "TunableNumbers";
+
+ private final String key;
+ private boolean hasDefault = false;
+ private double defaultValue;
+ private LoggedDashboardNumber dashboardNumber;
+ private Map lastHasChangedValues = new HashMap<>();
+
+ /**
+ * Create a new LoggedTunableNumber
+ *
+ * @param dashboardKey Key on dashboard
+ */
+ public LoggedTunableNumber(String dashboardKey) {
+ this.key = tableKey + "/" + dashboardKey;
+ }
+
+ /**
+ * Create a new LoggedTunableNumber with the default value
+ *
+ * @param dashboardKey Key on dashboard
+ * @param defaultValue Default value
+ */
+ public LoggedTunableNumber(String dashboardKey, double defaultValue) {
+ this(dashboardKey);
+ initDefault(defaultValue);
+ }
+
+ /**
+ * Set the default value of the number. The default value can only be set once.
+ *
+ * @param defaultValue The default value
+ */
+ public void initDefault(double defaultValue) {
+ if (!hasDefault) {
+ hasDefault = true;
+ this.defaultValue = defaultValue;
+ if (Constants.tuningMode) {
+ dashboardNumber = new LoggedDashboardNumber(key, defaultValue);
+ }
+ }
+ }
+
+ /**
+ * Get the current value, from dashboard if available and in tuning mode.
+ *
+ * @return The current value
+ */
+ public double get() {
+ if (!hasDefault) {
+ return 0.0;
+ } else {
+ return Constants.tuningMode ? dashboardNumber.get() : defaultValue;
+ }
+ }
+
+ /**
+ * Checks whether the number has changed since our last check
+ *
+ * @param id Unique identifier for the caller to avoid conflicts when shared between multiple
+ * objects. Recommended approach is to pass the result of "hashCode()"
+ * @return True if the number has changed since the last time this method was called, false
+ * otherwise.
+ */
+ public boolean hasChanged(int id) {
+ double currentValue = get();
+ Double lastValue = lastHasChangedValues.get(id);
+ if (lastValue == null || currentValue != lastValue) {
+ lastHasChangedValues.put(id, currentValue);
+ return true;
+ }
+
+ return false;
+ }
+
+ /**
+ * Runs action if any of the tunableNumbers have changed
+ *
+ * @param id Unique identifier for the caller to avoid conflicts when shared between multiple *
+ * objects. Recommended approach is to pass the result of "hashCode()"
+ * @param action Callback to run when any of the tunable numbers have changed. Access tunable
+ * numbers in order inputted in method
+ * @param tunableNumbers All tunable numbers to check
+ */
+ public static void ifChanged(
+ int id, Consumer action, LoggedTunableNumber... tunableNumbers) {
+ if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) {
+ action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray());
+ }
+ }
+
+ /** Runs action if any of the tunableNumbers have changed */
+ public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) {
+ ifChanged(id, values -> action.run(), tunableNumbers);
+ }
+
+ @Override
+ public double getAsDouble() {
+ return get();
+ }
+}
diff --git a/src/main/java/frc/robot/util/OverrideSwitches.java b/src/main/java/frc/robot/util/OverrideSwitches.java
new file mode 100644
index 00000000..a04b00e3
--- /dev/null
+++ b/src/main/java/frc/robot/util/OverrideSwitches.java
@@ -0,0 +1,101 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+// NOTE: This module assumes the design described in FRC6328's 2024 Chief
+// Delphi build thread:
+// https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2024-build-thread/442736/72
+//
+// The "Arcade Controller" used by 6328 (https://www.amazon.com/gp/product/B00UUROWWK)
+// enumerates as a “Generic USB Controller”, mapped in this modules as a GenericHID.
+//
+// If another type of controller is used, swap in the proper class.
+
+package frc.robot.util;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
+/** Interface for physical override switches on operator console. */
+public class OverrideSwitches {
+ private final GenericHID consoleSwitches;
+
+ public OverrideSwitches(int port) {
+ consoleSwitches = new GenericHID(port);
+ }
+
+ /** Returns whether the controller is connected. */
+ public boolean isConnected() {
+ return consoleSwitches.isConnected()
+ && !DriverStation.getJoystickIsXbox(consoleSwitches.getPort())
+ && consoleSwitches.getName().equals("Generic USB Joystick");
+ }
+
+ /** Gets the state of a driver-side switch (0-2 from left to right). */
+ public boolean getDriverSwitch(int index) {
+ if (index < 0 || index > 2) {
+ throw new RuntimeException(
+ "Invalid driver override index " + Integer.toString(index) + ". Must be 0-2.");
+ }
+ return consoleSwitches.getRawButton(index + 1);
+ }
+
+ /** Gets the state of an operator-side switch (0-4 from left to right). */
+ public boolean getOperatorSwitch(int index) {
+ if (index < 0 || index > 4) {
+ throw new RuntimeException(
+ "Invalid operator override index " + Integer.toString(index) + ". Must be 0-4.");
+ }
+ return consoleSwitches.getRawButton(index + 8);
+ }
+
+ /** Gets the state of the multi-directional switch. */
+ public MultiDirectionSwitchState getMultiDirectionSwitch() {
+ if (consoleSwitches.getRawButton(4)) {
+ return MultiDirectionSwitchState.LEFT;
+ } else if (consoleSwitches.getRawButton(5)) {
+ return MultiDirectionSwitchState.RIGHT;
+ } else {
+ return MultiDirectionSwitchState.NEUTRAL;
+ }
+ }
+
+ /** Returns a trigger for a driver-side switch (0-2 from left to right). */
+ public Trigger driverSwitch(int index) {
+ return new Trigger(() -> getDriverSwitch(index));
+ }
+
+ /** Returns a trigger for an operator-side switch (0-4 from left to right). */
+ public Trigger operatorSwitch(int index) {
+ return new Trigger(() -> getOperatorSwitch(index));
+ }
+
+ /** Returns a trigger for when the multi-directional switch is pushed to the left. */
+ public Trigger multiDirectionSwitchLeft() {
+ return new Trigger(() -> getMultiDirectionSwitch() == MultiDirectionSwitchState.LEFT);
+ }
+
+ /** Returns a trigger for when the multi-directional switch is pushed to the right. */
+ public Trigger multiDirectionSwitchRight() {
+ return new Trigger(() -> getMultiDirectionSwitch() == MultiDirectionSwitchState.RIGHT);
+ }
+
+ /** The state of the multi-directional switch. */
+ public static enum MultiDirectionSwitchState {
+ LEFT,
+ NEUTRAL,
+ RIGHT
+ }
+}
diff --git a/src/main/java/frc/robot/util/PowerMonitoring.java b/src/main/java/frc/robot/util/PowerMonitoring.java
new file mode 100644
index 00000000..735054e2
--- /dev/null
+++ b/src/main/java/frc/robot/util/PowerMonitoring.java
@@ -0,0 +1,105 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.util;
+
+import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.Constants.PowerConstants;
+import frc.robot.RobotContainer.Ports;
+import frc.robot.util.Alert.AlertType;
+import org.littletonrobotics.junction.Logger;
+
+/**
+ * Power monitoring virtual subsystem that periodically polls the Power Distribution Module. Each
+ * port and the sum total currents are compared with limits defined in the ``Constants.java`` file,
+ * and subsystem total currents are also computed based on the power ports listed in
+ * ``RobotContainer.java``.
+ */
+public class PowerMonitoring extends VirtualSubsystem {
+
+ private final RBSISubsystem[] subsystems;
+
+ /** Define the Power Distribution Hardware */
+ private PowerDistribution m_powerModule =
+ new PowerDistribution(
+ Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), PowerConstants.kPowerModule);
+
+ private int NUM_PDH_CHANNELS = m_powerModule.getNumChannels();
+ private double[] channelCurrents = new double[NUM_PDH_CHANNELS];
+
+ // DRIVE and STEER motor power ports
+ private final int[] m_drivePowerPorts = {
+ Ports.FL_DRIVE.getPowerPort(),
+ Ports.FR_DRIVE.getPowerPort(),
+ Ports.BL_DRIVE.getPowerPort(),
+ Ports.BR_DRIVE.getPowerPort()
+ };
+ private final int[] m_steerPowerPorts = {
+ Ports.FL_ROTATION.getPowerPort(),
+ Ports.FR_ROTATION.getPowerPort(),
+ Ports.BL_ROTATION.getPowerPort(),
+ Ports.BR_ROTATION.getPowerPort()
+ };
+
+ // Class method definition, including inputs of optional subsystems
+ public PowerMonitoring(RBSISubsystem... subsystems) {
+ this.subsystems = subsystems;
+ }
+
+ /** Periodic Method */
+ public void periodic() {
+
+ // Check the total robot current and individual port currents against Constants
+ double totalCurrent = m_powerModule.getTotalCurrent();
+ if (totalCurrent > PowerConstants.kTotalMaxCurrent) {
+ new Alert("Total current draw exceeds limit!", AlertType.WARNING).set(true);
+ }
+ for (int i = 0; i < NUM_PDH_CHANNELS; i++) {
+ channelCurrents[i] = m_powerModule.getCurrent(i);
+ if (channelCurrents[i] > PowerConstants.kMotorPortMaxCurrent) {
+ new Alert("Port " + i + " current draw exceeds limit!", AlertType.WARNING).set(true);
+ }
+ }
+
+ // Compute DRIVE and STEER summed currents
+ double driveCurrent = 0.0;
+ double steerCurrent = 0.0;
+ for (int port : m_drivePowerPorts) {
+ driveCurrent += channelCurrents[port];
+ }
+ for (int port : m_steerPowerPorts) {
+ steerCurrent += channelCurrents[port];
+ }
+ // Log values to AdvantageKit and to SmartDashboard
+ Logger.recordOutput("PowerMonitor/TotalCurrent", totalCurrent);
+ Logger.recordOutput("PowerMonitor/DriveCurrent", driveCurrent);
+ Logger.recordOutput("PowerMonitor/SteerCurrent", steerCurrent);
+ SmartDashboard.putNumber("TotalCurrent", totalCurrent);
+ SmartDashboard.putNumber("DriveCurrent", driveCurrent);
+ SmartDashboard.putNumber("SteerCurrent", steerCurrent);
+
+ // Compute and log any passed-in subsystems
+ for (RBSISubsystem subsystem : subsystems) {
+ double subsystemCurrent = 0.0;
+ for (int port : subsystem.getPowerPorts()) {
+ subsystemCurrent += channelCurrents[port];
+ }
+ Logger.recordOutput("PowerMonitor/" + subsystem.getName() + "Current", subsystemCurrent);
+ SmartDashboard.putNumber(subsystem.getName() + "Current", subsystemCurrent);
+ }
+
+ // Do something about setting priorities if drawing too much current
+
+ }
+}
diff --git a/src/main/java/frc/robot/util/RBSISubsystem.java b/src/main/java/frc/robot/util/RBSISubsystem.java
new file mode 100644
index 00000000..7a5c8e46
--- /dev/null
+++ b/src/main/java/frc/robot/util/RBSISubsystem.java
@@ -0,0 +1,35 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.util;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/**
+ * This class is designed to include Az-RBSI specific methods on top of the standard WPILib
+ * command-based subsystem classes. All non-drivebase subsystems (e.g., flywheels, arms, elevators,
+ * etc.) should subclass ``RBSISubsystem`` rather than ``SubsystemBase`` in order to gain access to
+ * added functionality.
+ */
+public class RBSISubsystem extends SubsystemBase {
+
+ /**
+ * Gets the power ports associated with this Subsystem.
+ *
+ * @return Array of power distribution module ports
+ */
+ public int[] getPowerPorts() {
+ int[] retval = {};
+ return retval;
+ }
+}
diff --git a/src/main/java/frc/robot/util/RobotDeviceId.java b/src/main/java/frc/robot/util/RobotDeviceId.java
new file mode 100644
index 00000000..055491bc
--- /dev/null
+++ b/src/main/java/frc/robot/util/RobotDeviceId.java
@@ -0,0 +1,58 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright (c) 2024 FRC 254
+// https://github.com/team254
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.util;
+
+/**
+ * Class for wrapping Robot / CAN devices with a name and functionality. Included here are both the
+ * CAN ID for devices and the port on the Power Distribution Module for power monitoring and
+ * management.
+ */
+public class RobotDeviceId {
+ private final int m_CANDeviceNumber;
+ private final String m_CANBus;
+ private final Integer m_PowerPort;
+
+ public RobotDeviceId(int CANdeviceNumber, String CANbus, Integer powerPort) {
+ m_CANDeviceNumber = CANdeviceNumber;
+ m_CANBus = CANbus;
+ m_PowerPort = powerPort;
+ }
+
+ /** Use the default bus name (empty string) */
+ public RobotDeviceId(int CANdeviceNumber, Integer powerPort) {
+ this(CANdeviceNumber, "", powerPort);
+ }
+
+ /** Get the CAN ID value for a named device */
+ public int getDeviceNumber() {
+ return m_CANDeviceNumber;
+ }
+
+ /** Get the CAN bus name for a named device */
+ public String getBus() {
+ return m_CANBus;
+ }
+
+ /** Get the Power Port for a named device */
+ public int getPowerPort() {
+ return m_PowerPort;
+ }
+
+ /** Check whether two named devices are, in fact, the same */
+ public boolean equals(RobotDeviceId other) {
+ return other.m_CANDeviceNumber == m_CANDeviceNumber && other.m_CANBus == m_CANBus;
+ }
+}
diff --git a/src/main/java/frc/robot/util/VirtualSubsystem.java b/src/main/java/frc/robot/util/VirtualSubsystem.java
new file mode 100644
index 00000000..2943eb06
--- /dev/null
+++ b/src/main/java/frc/robot/util/VirtualSubsystem.java
@@ -0,0 +1,41 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.util;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Base class for virtual subsystems -- not robot hardware -- that should be treated as subsystems
+ */
+public abstract class VirtualSubsystem {
+ private static List subsystems = new ArrayList<>();
+
+ // Load all defined virtual subsystems into a list
+ public VirtualSubsystem() {
+ subsystems.add(this);
+ }
+
+ public static void periodicAll() {
+ // Call each virtual subsystem during robotPeriodic()
+ for (VirtualSubsystem subsystem : subsystems) {
+ subsystem.periodic();
+ }
+ }
+
+ // Each virtual subsystem must implement its own periodic() method
+ public abstract void periodic();
+}
diff --git a/src/main/java/frc/robot/util/YagslConstants.java b/src/main/java/frc/robot/util/YagslConstants.java
new file mode 100644
index 00000000..9630b932
--- /dev/null
+++ b/src/main/java/frc/robot/util/YagslConstants.java
@@ -0,0 +1,204 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.util;
+
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.first.wpilibj.Filesystem;
+import frc.robot.Constants.DeployConstants;
+import java.io.File;
+import java.util.Arrays;
+import java.util.List;
+import swervelib.parser.json.ModuleJson;
+import swervelib.parser.json.PIDFPropertiesJson;
+import swervelib.parser.json.PhysicalPropertiesJson;
+import swervelib.parser.json.SwerveDriveJson;
+
+/**
+ * The YAGSL swerve drive constants class
+ *
+ * The purpose of this class is to convert the YAGSL-based JSON configuration file data into the
+ * same constants format as produced by the Phoenix Tuner X's ``TunerConstants.java`` file. The goal
+ * here is to make implenentation in the drive class easier.
+ */
+public class YagslConstants {
+
+ // Define the internal YAGSL objects we will read into
+ private static final File yagslDir =
+ new File(Filesystem.getDeployDirectory(), DeployConstants.yagslDir);
+ public static final SwerveDriveJson swerveDriveJson; // Needed by the Accelerometer subsystem
+ private static final PIDFPropertiesJson pidfPropertiesJson;
+ private static final PhysicalPropertiesJson physicalPropertiesJson;
+ private static final ModuleJson[] moduleJsons;
+
+ // Use a static to read in the YAGSL JSON files with error catching
+ static {
+ SwerveDriveJson tempSwerveJson = null;
+ PIDFPropertiesJson tempPdifJson = null;
+ PhysicalPropertiesJson tempPhysicalJson = null;
+ ModuleJson[] tempModuleJsons = null;
+
+ try {
+ tempSwerveJson =
+ new ObjectMapper()
+ .readValue(new File(yagslDir, "swervedrive.json"), SwerveDriveJson.class);
+ tempPdifJson =
+ new ObjectMapper()
+ .readValue(
+ new File(yagslDir, "modules/pidfproperties.json"), PIDFPropertiesJson.class);
+ tempPhysicalJson =
+ new ObjectMapper()
+ .readValue(
+ new File(yagslDir, "modules/physicalproperties.json"),
+ PhysicalPropertiesJson.class);
+
+ } catch (Exception e) {
+ throw new RuntimeException(e);
+ }
+ swerveDriveJson = tempSwerveJson;
+ pidfPropertiesJson = tempPdifJson;
+ physicalPropertiesJson = tempPhysicalJson;
+
+ tempModuleJsons = new ModuleJson[swerveDriveJson.modules.length];
+ try {
+ for (int i = 0; i < tempModuleJsons.length; i++) {
+ // moduleConfigs.put(swerveDriveJson.modules[i], i);
+ File moduleFile = new File(yagslDir, "modules/" + swerveDriveJson.modules[i]);
+ assert moduleFile.exists();
+ tempModuleJsons[i] = new ObjectMapper().readValue(moduleFile, ModuleJson.class);
+ }
+ } catch (Exception e) {
+ throw new RuntimeException(e);
+ }
+ moduleJsons = tempModuleJsons;
+ }
+
+ // Define the Tuner X - like constants from the YAGSL objects
+
+ // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
+ // This may need to be tuned to your individual robot
+ public static final double kCoupleRatio = 3.5;
+
+ public static final double kDriveGearRatio =
+ physicalPropertiesJson.conversionFactors.drive.gearRatio;
+ public static final double kSteerGearRatio =
+ physicalPropertiesJson.conversionFactors.angle.gearRatio;
+ public static final double kWheelRadiusInches =
+ physicalPropertiesJson.conversionFactors.drive.diameter / 2.0;
+
+ public static final String kCANbusName = swerveDriveJson.imu.canbus;
+ public static final int kPigeonId = swerveDriveJson.imu.id;
+
+ // These are only used for simulation
+ public static final double kSteerInertia = 0.004;
+ public static final double kDriveInertia = 0.025;
+ // Simulated voltage necessary to overcome friction
+ public static final double kSteerFrictionVoltage = 0.25;
+ public static final double kDriveFrictionVoltage = 0.25;
+
+ // Current / Voltage Limits
+ public static final double kSteerCurrentLimit = physicalPropertiesJson.currentLimit.angle;
+ public static final double kDriveCurrentLimit = physicalPropertiesJson.currentLimit.drive;
+ public static final double kOptimalVoltage = physicalPropertiesJson.optimalVoltage;
+
+ // The modules themselves
+ static List moduleList = Arrays.asList(swerveDriveJson.modules);
+
+ // Front Left
+ static ModuleJson flModule = moduleJsons[moduleList.indexOf("frontleft.json")];
+ public static final int kFrontLeftDriveMotorId = flModule.drive.id;
+ public static final int kFrontLeftSteerMotorId = flModule.angle.id;
+ public static final int kFrontLeftEncoderId = flModule.encoder.id;
+ public static final String kFrontLeftDriveCanbus = flModule.drive.canbus;
+ public static final String kFrontLeftSteerCanbus = flModule.angle.canbus;
+ public static final String kFrontLeftEncoderCanbus = flModule.encoder.canbus;
+ public static final String kFrontLeftDriveType = flModule.drive.type;
+ public static final String kFrontLeftSteerType = flModule.angle.type;
+ public static final String kFrontLeftEncoderType = flModule.encoder.type;
+ public static final double kFrontLeftEncoderOffset = flModule.absoluteEncoderOffset;
+ public static final boolean kFrontLeftDriveInvert = flModule.inverted.drive;
+ public static final boolean kFrontLeftSteerInvert = flModule.inverted.angle;
+ public static final boolean kFrontLeftEncoderInvert = flModule.absoluteEncoderInverted;
+ public static final double kFrontLeftXPosInches = flModule.location.left;
+ public static final double kFrontLeftYPosInches = flModule.location.front;
+
+ // Front Right
+ static ModuleJson frModule = moduleJsons[moduleList.indexOf("frontright.json")];
+ public static final int kFrontRightDriveMotorId = frModule.drive.id;
+ public static final int kFrontRightSteerMotorId = frModule.angle.id;
+ public static final int kFrontRightEncoderId = frModule.encoder.id;
+ public static final String kFrontRightDriveCanbus = frModule.drive.canbus;
+ public static final String kFrontRightSteerCanbus = frModule.angle.canbus;
+ public static final String kFrontRightEncoderCanbus = frModule.encoder.canbus;
+ public static final String kFrontRightDriveType = frModule.drive.type;
+ public static final String kFrontRightSteerType = frModule.angle.type;
+ public static final String kFrontRightEncoderType = frModule.encoder.type;
+ public static final double kFrontRightEncoderOffset = frModule.absoluteEncoderOffset;
+ public static final boolean kFrontRightDriveInvert = frModule.inverted.drive;
+ public static final boolean kFrontRightSteerInvert = frModule.inverted.angle;
+ public static final boolean kFrontRightEncoderInvert = frModule.absoluteEncoderInverted;
+ public static final double kFrontRightXPosInches = frModule.location.left;
+ public static final double kFrontRightYPosInches = frModule.location.front;
+
+ // Back Left
+ static ModuleJson blModule = moduleJsons[moduleList.indexOf("backleft.json")];
+ public static final int kBackLeftDriveMotorId = blModule.drive.id;
+ public static final int kBackLeftSteerMotorId = blModule.angle.id;
+ public static final int kBackLeftEncoderId = blModule.encoder.id;
+ public static final String kBackLeftDriveCanbus = blModule.drive.canbus;
+ public static final String kBackLeftSteerCanbus = blModule.angle.canbus;
+ public static final String kBackLeftEncoderCanbus = blModule.encoder.canbus;
+ public static final String kBackLeftDriveType = blModule.drive.type;
+ public static final String kBackLeftSteerType = blModule.angle.type;
+ public static final String kBackLeftEncoderType = blModule.encoder.type;
+ public static final double kBackLeftEncoderOffset = blModule.absoluteEncoderOffset;
+ public static final boolean kBackLeftDriveInvert = blModule.inverted.drive;
+ public static final boolean kBackLeftSteerInvert = blModule.inverted.angle;
+ public static final boolean kBackLeftEncoderInvert = blModule.absoluteEncoderInverted;
+ public static final double kBackLeftXPosInches = blModule.location.left;
+ public static final double kBackLeftYPosInches = blModule.location.front;
+
+ // Back Right
+ static ModuleJson brModule = moduleJsons[moduleList.indexOf("backright.json")];
+ public static final int kBackRightDriveMotorId = brModule.drive.id;
+ public static final int kBackRightSteerMotorId = brModule.angle.id;
+ public static final int kBackRightEncoderId = brModule.encoder.id;
+ public static final String kBackRightDriveCanbus = brModule.drive.canbus;
+ public static final String kBackRightSteerCanbus = brModule.angle.canbus;
+ public static final String kBackRightEncoderCanbus = brModule.encoder.canbus;
+ public static final String kBackRightDriveType = brModule.drive.type;
+ public static final String kBackRightSteerType = brModule.angle.type;
+ public static final String kBackRightEncoderType = brModule.encoder.type;
+ public static final double kBackRightEncoderOffset = brModule.absoluteEncoderOffset;
+ public static final boolean kBackRightDriveInvert = brModule.inverted.drive;
+ public static final boolean kBackRightSteerInvert = brModule.inverted.angle;
+ public static final boolean kBackRightEncoderInvert = brModule.absoluteEncoderInverted;
+ public static final double kBackRightXPosInches = brModule.location.left;
+ public static final double kBackRightYPosInches = brModule.location.front;
+
+ // YAGSL PIDF Values
+ public static final double kDriveP = pidfPropertiesJson.drive.p;
+ public static final double kDriveI = pidfPropertiesJson.drive.i;
+ public static final double kDriveD = pidfPropertiesJson.drive.d;
+ public static final double kDriveF = pidfPropertiesJson.drive.f;
+ public static final double kDriveIZ = pidfPropertiesJson.drive.iz;
+ public static final double kSteerP = pidfPropertiesJson.angle.p;
+ public static final double kSteerI = pidfPropertiesJson.angle.i;
+ public static final double kSteerD = pidfPropertiesJson.angle.d;
+ public static final double kSteerF = pidfPropertiesJson.angle.f;
+ public static final double kSteerIZ = pidfPropertiesJson.angle.iz;
+}
diff --git a/src/test/CurrentLimitTests.java b/src/test/CurrentLimitTests.java
new file mode 100644
index 00000000..dcf25f9c
--- /dev/null
+++ b/src/test/CurrentLimitTests.java
@@ -0,0 +1,167 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright (c) 2024 Cross The Road Electronics
+// https://github.com/CrossTheRoadElec/Phoenix6-Examples
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// 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.
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import java.util.function.Supplier;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+public class CurrentLimitTests implements AutoCloseable {
+ final int CONFIG_RETRY_COUNT = 5;
+
+ TalonFX talon;
+
+ @Override
+ public void close() {
+ /* destroy our TalonFX object */
+ talon.close();
+ }
+
+ @BeforeEach
+ public void constructDevices() {
+ assert HAL.initialize(500, 0);
+
+ talon = new TalonFX(0);
+
+ /* enable the robot */
+ DriverStationSim.setEnabled(true);
+ DriverStationSim.notifyNewData();
+
+ /* delay ~100ms so the devices can start up and enable */
+ Timer.delay(0.100);
+ }
+
+ @AfterEach
+ void shutdown() {
+ close();
+ }
+
+ @Test
+ public void robotIsEnabled() {
+ /* verify that the robot is enabled */
+ assertTrue(DriverStation.isEnabled());
+ }
+
+ @Test
+ public void testStatorLimit() {
+ var statorCurrent = talon.getStatorCurrent();
+
+ /* Configure a stator limit of 20 amps */
+ TalonFXConfiguration toConfigure = new TalonFXConfiguration();
+ CurrentLimitsConfigs currentLimitConfigs = toConfigure.CurrentLimits;
+ currentLimitConfigs.StatorCurrentLimit = 20;
+ currentLimitConfigs.StatorCurrentLimitEnable = false; // Start with stator limits off
+
+ retryConfigApply(() -> talon.getConfigurator().apply(toConfigure));
+
+ /* Put the talon in a stall, which should produce a lot of current */
+ talon.setControl(new DutyCycleOut(1));
+ /* wait for the control to apply */
+ Timer.delay(0.020);
+
+ /* Get the next update for stator current */
+ statorCurrent.waitForUpdate(1);
+
+ System.out.println("Stator current is " + statorCurrent);
+ assertTrue(statorCurrent.getValue() > 100); // Stator current should be in excess of 100 amps
+
+ /* Now apply the stator current limit */
+ currentLimitConfigs.StatorCurrentLimitEnable = true;
+ retryConfigApply(() -> talon.getConfigurator().apply(currentLimitConfigs));
+
+ /* wait for the current to drop */
+ Timer.delay(0.500);
+
+ /* Get the next update for stator current */
+ statorCurrent.waitForUpdate(1);
+
+ System.out.println("Stator current is " + statorCurrent);
+ assertTrue(statorCurrent.getValue() < 25); // Give some wiggle room
+ }
+
+ @Test
+ public void testSupplyLimit() {
+ var supplyCurrent = talon.getSupplyCurrent();
+
+ /* Configure a supply limit of 20 amps */
+ TalonFXConfiguration toConfigure = new TalonFXConfiguration();
+ CurrentLimitsConfigs currentLimitConfigs = toConfigure.CurrentLimits;
+ currentLimitConfigs.SupplyCurrentLimit = 5;
+ currentLimitConfigs.SupplyCurrentThreshold = 10;
+ currentLimitConfigs.SupplyTimeThreshold = 1.0;
+ currentLimitConfigs.StatorCurrentLimitEnable = false; // Start with supply limits off
+
+ retryConfigApply(() -> talon.getConfigurator().apply(toConfigure));
+
+ /* Put the talon in a stall, which should produce a lot of current */
+ talon.setControl(new DutyCycleOut(1));
+ /* wait for the control to apply */
+ Timer.delay(0.100);
+
+ /* Get the next update for supply current */
+ supplyCurrent.waitForUpdate(1);
+
+ System.out.println("Supply current is " + supplyCurrent);
+ assertTrue(supplyCurrent.getValue() > 80); // Supply current should be in excess of 80 amps
+
+ /* Now apply the supply current limit */
+ currentLimitConfigs.SupplyCurrentLimitEnable = true;
+ retryConfigApply(() -> talon.getConfigurator().apply(currentLimitConfigs));
+
+ /* wait for the current to adjust */
+ Timer.delay(0.500);
+
+ /* Get the next update for supply current */
+ supplyCurrent.waitForUpdate(1);
+
+ System.out.println("Supply current is " + supplyCurrent);
+ assertTrue(
+ supplyCurrent.getValue()
+ > 80); // Make sure it's still over 80 amps (time hasn't exceeded 1 second in total)
+
+ /* Wait a full extra couple seconds so the limit kicks in and starts limiting us */
+ Timer.delay(2);
+
+ /* Get the next update for supply current */
+ supplyCurrent.waitForUpdate(1);
+
+ System.out.println("Supply current is " + supplyCurrent);
+ assertTrue(supplyCurrent.getValue() < 10); // Give some wiggle room
+ }
+
+ private void retryConfigApply(Supplier toApply) {
+ StatusCode finalCode = StatusCode.StatusCodeNotInitialized;
+ int triesLeftOver = CONFIG_RETRY_COUNT;
+ do {
+ finalCode = toApply.get();
+ } while (!finalCode.isOK() && --triesLeftOver > 0);
+ assert (finalCode.isOK());
+ }
+}
diff --git a/src/test/FusedCANcoderTests.java b/src/test/FusedCANcoderTests.java
new file mode 100644
index 00000000..077eb269
--- /dev/null
+++ b/src/test/FusedCANcoderTests.java
@@ -0,0 +1,185 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright (c) 2024 Cross The Road Electronics
+// https://github.com/CrossTheRoadElec/Phoenix6-Examples
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// 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.
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
+import edu.wpi.first.hal.HAL;
+import java.util.function.Supplier;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+public class FusedCANcoderTests {
+ final double SET_DELTA = 0.1;
+ final int CONFIG_RETRY_COUNT = 5;
+
+ TalonFX talon;
+ CANcoder cancoder;
+
+ @BeforeEach
+ public void constructDevices() {
+ assert HAL.initialize(500, 0);
+
+ talon = new TalonFX(0);
+ cancoder = new CANcoder(0);
+ }
+
+ @Test
+ public void testIndividualPos() {
+ final double TALON_POSITION = 0.4;
+ final double CANCODER_POSITION = -3.1;
+
+ /* Factory-default Talon and CANcoder */
+ retryConfigApply(() -> talon.getConfigurator().apply(new TalonFXConfiguration()));
+ retryConfigApply(() -> cancoder.getConfigurator().apply(new CANcoderConfiguration()));
+
+ /* Get sim states */
+ var talonSimState = talon.getSimState();
+ var cancoderSimState = cancoder.getSimState();
+
+ /* Wait for signal to update and assert they match the set positions */
+ var talonPos = talon.getPosition();
+ var cancoderPos = cancoder.getPosition();
+
+ /* Make sure both are initially set to 0 before messing with sim state */
+ retryConfigApply(() -> talonSimState.setRawRotorPosition(0));
+ retryConfigApply(() -> cancoderSimState.setRawPosition(0));
+ retryConfigApply(() -> talon.setPosition(0));
+ retryConfigApply(() -> cancoder.setPosition(0));
+ /* Wait for sets to take affect */
+ BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos);
+
+ /* Set them to different values */
+ retryConfigApply(() -> talonSimState.setRawRotorPosition(TALON_POSITION));
+ retryConfigApply(() -> cancoderSimState.setRawPosition(CANCODER_POSITION));
+
+ BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos);
+ BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos);
+
+ System.out.println("Talon Pos vs expected: " + talonPos + " vs " + TALON_POSITION);
+ System.out.println("CANcoder Pos vs expected: " + cancoderPos + " vs " + CANCODER_POSITION);
+ assertEquals(talonPos.getValue(), TALON_POSITION, SET_DELTA);
+ assertEquals(cancoderPos.getValue(), CANCODER_POSITION, SET_DELTA);
+ }
+
+ @Test
+ public void testFusedCANcoderPos() {
+ final double TALON_POSITION = 0.4;
+ final double CANCODER_POSITION = -3.1;
+
+ /* Configure Talon to use Fused CANcoder, factory default CANcoder */
+ var configs = new TalonFXConfiguration();
+ configs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
+ configs.Feedback.FeedbackRemoteSensorID = cancoder.getDeviceID();
+ retryConfigApply(() -> talon.getConfigurator().apply(configs));
+ retryConfigApply(() -> cancoder.getConfigurator().apply(new CANcoderConfiguration()));
+
+ /* Get sim states */
+ var talonSimState = talon.getSimState();
+ var cancoderSimState = cancoder.getSimState();
+
+ /* Wait for signal to update and assert they match the set positions */
+ var talonPos = talon.getPosition();
+ var cancoderPos = cancoder.getPosition();
+
+ /* Make sure both are initially set to 0 before messing with sim state */
+ retryConfigApply(() -> talonSimState.setRawRotorPosition(0));
+ retryConfigApply(() -> cancoderSimState.setRawPosition(0));
+ retryConfigApply(() -> talon.setPosition(0));
+ retryConfigApply(() -> cancoder.setPosition(0));
+ /* Wait for sets to take affect */
+ BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos);
+
+ /* Set them to different values */
+ retryConfigApply(() -> talonSimState.setRawRotorPosition(TALON_POSITION));
+ retryConfigApply(() -> cancoderSimState.setRawPosition(CANCODER_POSITION));
+
+ BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos);
+ BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos);
+
+ /* Further wait for Talon, since it probably just received the new CANcoder position frame */
+ talonPos.waitForUpdate(1.0);
+
+ /* Make sure Talon matches CANcoder, since it should be using CANcoder's position */
+ System.out.println("Talon Pos vs expected: " + talonPos + " vs " + CANCODER_POSITION);
+ System.out.println("CANcoder Pos vs expected: " + cancoderPos + " vs " + CANCODER_POSITION);
+ assertEquals(talonPos.getValue(), CANCODER_POSITION, SET_DELTA);
+ assertEquals(cancoderPos.getValue(), CANCODER_POSITION, SET_DELTA);
+ }
+
+ @Test
+ public void testRemoteCANcoderPos() {
+ final double TALON_POSITION = 0.4;
+ final double CANCODER_POSITION = -3.1;
+
+ /* Configure Talon to use Fused CANcoder, factory default CANcoder */
+ var configs = new TalonFXConfiguration();
+ configs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
+ configs.Feedback.FeedbackRemoteSensorID = cancoder.getDeviceID();
+ retryConfigApply(() -> talon.getConfigurator().apply(configs));
+ retryConfigApply(() -> cancoder.getConfigurator().apply(new CANcoderConfiguration()));
+
+ /* Get sim states */
+ var talonSimState = talon.getSimState();
+ var cancoderSimState = cancoder.getSimState();
+
+ /* Wait for signal to update and assert they match the set positions */
+ var talonPos = talon.getPosition();
+ var cancoderPos = cancoder.getPosition();
+
+ /* Make sure both are initially set to 0 before messing with sim state */
+ retryConfigApply(() -> talonSimState.setRawRotorPosition(0));
+ retryConfigApply(() -> cancoderSimState.setRawPosition(0));
+ retryConfigApply(() -> talon.setPosition(0));
+ retryConfigApply(() -> cancoder.setPosition(0));
+ /* Wait for sets to take affect */
+ BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos);
+
+ /* Set them to different values */
+ retryConfigApply(() -> talonSimState.setRawRotorPosition(TALON_POSITION));
+ retryConfigApply(() -> cancoderSimState.setRawPosition(CANCODER_POSITION));
+
+ BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos);
+ BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos);
+
+ /* Further wait for Talon, since it probably just received the new CANcoder position frame */
+ talonPos.waitForUpdate(1.0);
+
+ /* Make sure Talon matches CANcoder, since it should be using CANcoder's position */
+ System.out.println("Talon Pos vs expected: " + talonPos + " vs " + CANCODER_POSITION);
+ System.out.println("CANcoder Pos vs expected: " + cancoderPos + " vs " + CANCODER_POSITION);
+ assertEquals(talonPos.getValue(), CANCODER_POSITION, SET_DELTA);
+ assertEquals(cancoderPos.getValue(), CANCODER_POSITION, SET_DELTA);
+ }
+
+ private void retryConfigApply(Supplier toApply) {
+ StatusCode finalCode = StatusCode.StatusCodeNotInitialized;
+ int triesLeftOver = CONFIG_RETRY_COUNT;
+ do {
+ finalCode = toApply.get();
+ } while (!finalCode.isOK() && --triesLeftOver > 0);
+ assert (finalCode.isOK());
+ }
+}
diff --git a/src/test/LatencyCompensationTests.java b/src/test/LatencyCompensationTests.java
new file mode 100644
index 00000000..27c696a2
--- /dev/null
+++ b/src/test/LatencyCompensationTests.java
@@ -0,0 +1,99 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+// Copyright (c) 2024 Cross The Road Electronics
+// https://github.com/CrossTheRoadElec/Phoenix6-Examples
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// 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.
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.hal.HAL;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+public class LatencyCompensationTests {
+ final double DOUBLE_DELTA = 0.01;
+
+ TalonFX talonfx;
+ CANcoder cancoder;
+
+ @BeforeEach
+ public void constructDevices() {
+ assert HAL.initialize(500, 0);
+
+ talonfx = new TalonFX(0);
+ cancoder = new CANcoder(0);
+ }
+
+ @Test
+ public void testLatencyCompensator() {
+ final double position = 35;
+ final double velocity = 24;
+ /* Initialize by making all the positions 0 */
+ talonfx.setPosition(0);
+ cancoder.setPosition(0);
+
+ /* Set the simulated state of device positions */
+ talonfx.getSimState().setRawRotorPosition(position);
+ talonfx.getSimState().setRotorVelocity(velocity);
+ cancoder.getSimState().setRawPosition(position);
+ cancoder.getSimState().setVelocity(velocity);
+
+ /* Perform latency compensation */
+ /* Start by getting signals */
+ var talonPos = talonfx.getPosition();
+ var talonVel = talonfx.getVelocity();
+ var cancoderPos = cancoder.getPosition();
+ var cancoderVel = cancoder.getVelocity();
+
+ /* Wait for an update on all of them so they're synchronized */
+ StatusCode status = StatusCode.OK;
+ for (int i = 0; i < 5; ++i) {
+ System.out.println("Waiting on signals");
+ status = BaseStatusSignal.waitForAll(1, talonPos, talonVel, cancoderPos, cancoderVel);
+ if (status.isOK()) break;
+ }
+ assertTrue(status.isOK());
+
+ /* Wait a bit longer for the latency to actually do some work */
+ try {
+ Thread.sleep(10);
+ } catch (Exception ex) {
+ }
+ /* Calculate how much latency we'd expect */
+ double talonLatency = talonPos.getTimestamp().getLatency();
+ double compensatedTalonPos = position + (velocity * talonLatency);
+ double cancoderLatency = cancoderPos.getTimestamp().getLatency();
+ double compensatedCANcoderPos = position + (velocity * cancoderLatency);
+
+ /* Calculate compensated values before the assert to avoid timing issue related to it */
+ double functionCompensatedTalon =
+ BaseStatusSignal.getLatencyCompensatedValue(talonPos, talonVel);
+ double functionCompensatedCANcoder =
+ BaseStatusSignal.getLatencyCompensatedValue(cancoderPos, cancoderVel);
+
+ /* Assert the two methods match */
+ System.out.println("Talon Pos: " + compensatedTalonPos + " - " + functionCompensatedTalon);
+ System.out.println(
+ "CANcoder Pos: " + compensatedCANcoderPos + " - " + functionCompensatedCANcoder);
+ assertEquals(compensatedTalonPos, functionCompensatedTalon, DOUBLE_DELTA);
+ assertEquals(compensatedCANcoderPos, functionCompensatedCANcoder, DOUBLE_DELTA);
+ }
+}
diff --git a/src/test/README b/src/test/README
new file mode 100644
index 00000000..9de38271
--- /dev/null
+++ b/src/test/README
@@ -0,0 +1 @@
+Directory containing unit tests.
\ No newline at end of file
diff --git a/src/test/RobotContainerTest.java b/src/test/RobotContainerTest.java
new file mode 100644
index 00000000..a29ba475
--- /dev/null
+++ b/src/test/RobotContainerTest.java
@@ -0,0 +1,26 @@
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package org.littletonrobotics.frc2024;
+
+import static org.junit.jupiter.api.Assertions.fail;
+
+import org.junit.jupiter.api.Test;
+
+public class RobotContainerTest {
+
+ @Test
+ public void createRobotContainer() {
+ // Instantiate RobotContainer
+ try {
+ new RobotContainer();
+ } catch (Exception e) {
+ e.printStackTrace();
+ fail("Failed to instantiate RobotContainer, see stack trace above.");
+ }
+ }
+}
diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
new file mode 100644
index 00000000..1f57c3b6
--- /dev/null
+++ b/vendordeps/AdvantageKit.json
@@ -0,0 +1,42 @@
+{
+ "fileName": "AdvantageKit.json",
+ "name": "AdvantageKit",
+ "version": "3.2.1",
+ "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
+ "frcYear": "2024",
+ "mavenUrls": [],
+ "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
+ "javaDependencies": [
+ {
+ "groupId": "org.littletonrobotics.akit.junction",
+ "artifactId": "wpilib-shim",
+ "version": "3.2.1"
+ },
+ {
+ "groupId": "org.littletonrobotics.akit.junction",
+ "artifactId": "junction-core",
+ "version": "3.2.1"
+ },
+ {
+ "groupId": "org.littletonrobotics.akit.conduit",
+ "artifactId": "conduit-api",
+ "version": "3.2.1"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "org.littletonrobotics.akit.conduit",
+ "artifactId": "conduit-wpilibio",
+ "version": "3.2.1",
+ "skipInvalidPlatforms": false,
+ "isJar": false,
+ "validPlatforms": [
+ "linuxathena",
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ],
+ "cppDependencies": []
+}
diff --git a/vendordeps/ChoreoLib.json b/vendordeps/ChoreoLib.json
new file mode 100644
index 00000000..7d6e9571
--- /dev/null
+++ b/vendordeps/ChoreoLib.json
@@ -0,0 +1,43 @@
+{
+ "fileName": "ChoreoLib.json",
+ "name": "ChoreoLib",
+ "version": "2024.2.3",
+ "uuid": "287cff6e-1b60-4412-8059-f6834fb30e30",
+ "frcYear": "2024",
+ "mavenUrls": [
+ "https://SleipnirGroup.github.io/ChoreoLib/dep"
+ ],
+ "jsonUrl": "https://SleipnirGroup.github.io/ChoreoLib/dep/ChoreoLib.json",
+ "javaDependencies": [
+ {
+ "groupId": "com.choreo.lib",
+ "artifactId": "ChoreoLib-java",
+ "version": "2024.2.3"
+ },
+ {
+ "groupId": "com.google.code.gson",
+ "artifactId": "gson",
+ "version": "2.10.1"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "com.choreo.lib",
+ "artifactId": "ChoreoLib-cpp",
+ "version": "2024.2.3",
+ "libName": "ChoreoLib",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal",
+ "linuxathena",
+ "linuxarm32",
+ "linuxarm64"
+ ]
+ }
+ ]
+}
diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json
new file mode 100644
index 00000000..a142879f
--- /dev/null
+++ b/vendordeps/NavX.json
@@ -0,0 +1,40 @@
+{
+ "fileName": "NavX.json",
+ "name": "NavX",
+ "version": "2024.1.0",
+ "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+ "frcYear": "2024",
+ "mavenUrls": [
+ "https://dev.studica.com/maven/release/2024/"
+ ],
+ "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json",
+ "javaDependencies": [
+ {
+ "groupId": "com.kauailabs.navx.frc",
+ "artifactId": "navx-frc-java",
+ "version": "2024.1.0"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "com.kauailabs.navx.frc",
+ "artifactId": "navx-frc-cpp",
+ "version": "2024.1.0",
+ "headerClassifier": "headers",
+ "sourcesClassifier": "sources",
+ "sharedLibrary": false,
+ "libName": "navx_frc",
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "linuxathena",
+ "linuxraspbian",
+ "linuxarm32",
+ "linuxarm64",
+ "linuxx86-64",
+ "osxuniversal",
+ "windowsx86-64"
+ ]
+ }
+ ]
+}
diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
new file mode 100644
index 00000000..f528a51a
--- /dev/null
+++ b/vendordeps/PathplannerLib.json
@@ -0,0 +1,38 @@
+{
+ "fileName": "PathplannerLib.json",
+ "name": "PathplannerLib",
+ "version": "2024.2.8",
+ "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
+ "frcYear": "2024",
+ "mavenUrls": [
+ "https://3015rangerrobotics.github.io/pathplannerlib/repo"
+ ],
+ "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
+ "javaDependencies": [
+ {
+ "groupId": "com.pathplanner.lib",
+ "artifactId": "PathplannerLib-java",
+ "version": "2024.2.8"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "com.pathplanner.lib",
+ "artifactId": "PathplannerLib-cpp",
+ "version": "2024.2.8",
+ "libName": "PathplannerLib",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal",
+ "linuxathena",
+ "linuxarm32",
+ "linuxarm64"
+ ]
+ }
+ ]
+}
diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json
new file mode 100644
index 00000000..adf6e02a
--- /dev/null
+++ b/vendordeps/Phoenix5.json
@@ -0,0 +1,151 @@
+{
+ "fileName": "Phoenix5.json",
+ "name": "CTRE-Phoenix (v5)",
+ "version": "5.33.1",
+ "frcYear": 2024,
+ "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
+ "mavenUrls": [
+ "https://maven.ctr-electronics.com/release/"
+ ],
+ "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json",
+ "requires": [
+ {
+ "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
+ "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.",
+ "offlineFileName": "Phoenix6.json",
+ "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json"
+ }
+ ],
+ "javaDependencies": [
+ {
+ "groupId": "com.ctre.phoenix",
+ "artifactId": "api-java",
+ "version": "5.33.1"
+ },
+ {
+ "groupId": "com.ctre.phoenix",
+ "artifactId": "wpiapi-java",
+ "version": "5.33.1"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "com.ctre.phoenix",
+ "artifactId": "cci",
+ "version": "5.33.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix.sim",
+ "artifactId": "cci-sim",
+ "version": "5.33.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "com.ctre.phoenix",
+ "artifactId": "wpiapi-cpp",
+ "version": "5.33.1",
+ "libName": "CTRE_Phoenix_WPI",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix",
+ "artifactId": "api-cpp",
+ "version": "5.33.1",
+ "libName": "CTRE_Phoenix",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix",
+ "artifactId": "cci",
+ "version": "5.33.1",
+ "libName": "CTRE_PhoenixCCI",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix.sim",
+ "artifactId": "wpiapi-cpp-sim",
+ "version": "5.33.1",
+ "libName": "CTRE_Phoenix_WPISim",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix.sim",
+ "artifactId": "api-cpp-sim",
+ "version": "5.33.1",
+ "libName": "CTRE_PhoenixSim",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix.sim",
+ "artifactId": "cci-sim",
+ "version": "5.33.1",
+ "libName": "CTRE_PhoenixCCISim",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ }
+ ]
+}
diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json
new file mode 100644
index 00000000..1f0cf3ab
--- /dev/null
+++ b/vendordeps/Phoenix6.json
@@ -0,0 +1,339 @@
+{
+ "fileName": "Phoenix6.json",
+ "name": "CTRE-Phoenix (v6)",
+ "version": "24.3.0",
+ "frcYear": 2024,
+ "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-frc2024-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": "24.3.0"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "tools",
+ "version": "24.3.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "tools-sim",
+ "version": "24.3.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simTalonSRX",
+ "version": "24.3.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simTalonFX",
+ "version": "24.3.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simVictorSPX",
+ "version": "24.3.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simPigeonIMU",
+ "version": "24.3.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simCANCoder",
+ "version": "24.3.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProTalonFX",
+ "version": "24.3.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANcoder",
+ "version": "24.3.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProPigeon2",
+ "version": "24.3.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "wpiapi-cpp",
+ "version": "24.3.0",
+ "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": "24.3.0",
+ "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": "24.3.0",
+ "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": "24.3.0",
+ "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": "24.3.0",
+ "libName": "CTRE_SimTalonSRX",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simTalonFX",
+ "version": "24.3.0",
+ "libName": "CTRE_SimTalonFX",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simVictorSPX",
+ "version": "24.3.0",
+ "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": "24.3.0",
+ "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": "24.3.0",
+ "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": "24.3.0",
+ "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": "24.3.0",
+ "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": "24.3.0",
+ "libName": "CTRE_SimProPigeon2",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ }
+ ]
+}
diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json
new file mode 100644
index 00000000..9eda5acb
--- /dev/null
+++ b/vendordeps/REVLib.json
@@ -0,0 +1,74 @@
+{
+ "fileName": "REVLib.json",
+ "name": "REVLib",
+ "version": "2024.2.4",
+ "frcYear": "2024",
+ "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
+ "mavenUrls": [
+ "https://maven.revrobotics.com/"
+ ],
+ "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json",
+ "javaDependencies": [
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-java",
+ "version": "2024.2.4"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-driver",
+ "version": "2024.2.4",
+ "skipInvalidPlatforms": true,
+ "isJar": false,
+ "validPlatforms": [
+ "windowsx86-64",
+ "windowsx86",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-cpp",
+ "version": "2024.2.4",
+ "libName": "REVLib",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "windowsx86",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ },
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-driver",
+ "version": "2024.2.4",
+ "libName": "REVLibDriver",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "windowsx86",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ }
+ ]
+}
diff --git a/vendordeps/ReduxLib_2024.json b/vendordeps/ReduxLib_2024.json
new file mode 100644
index 00000000..a97c269e
--- /dev/null
+++ b/vendordeps/ReduxLib_2024.json
@@ -0,0 +1,55 @@
+{
+ "fileName": "ReduxLib_2024.json",
+ "name": "ReduxLib",
+ "version": "2024.3.2",
+ "frcYear": 2024,
+ "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd",
+ "mavenUrls": [
+ "https://maven.reduxrobotics.com/"
+ ],
+ "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2024.json",
+ "javaDependencies": [
+ {
+ "groupId": "com.reduxrobotics.frc",
+ "artifactId": "ReduxLib-java",
+ "version": "2024.3.2"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "com.reduxrobotics.frc",
+ "artifactId": "ReduxLib-driver",
+ "version": "2024.3.2",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "linuxathena",
+ "linuxx86-64",
+ "linuxarm32",
+ "linuxarm64",
+ "osxuniversal",
+ "windowsx86-64"
+ ]
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "com.reduxrobotics.frc",
+ "artifactId": "ReduxLib-cpp",
+ "version": "2024.3.2",
+ "libName": "ReduxLib-cpp",
+ "headerClassifier": "headers",
+ "sourcesClassifier": "sources",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "linuxathena",
+ "linuxx86-64",
+ "linuxarm32",
+ "linuxarm64",
+ "osxuniversal",
+ "windowsx86-64"
+ ]
+ }
+ ]
+}
diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json
new file mode 100644
index 00000000..67bf3898
--- /dev/null
+++ b/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": "2024",
+ "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/vendordeps/photonlib.json b/vendordeps/photonlib.json
new file mode 100644
index 00000000..bef469ee
--- /dev/null
+++ b/vendordeps/photonlib.json
@@ -0,0 +1,57 @@
+{
+ "fileName": "photonlib.json",
+ "name": "photonlib",
+ "version": "v2024.3.1",
+ "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+ "frcYear": "2024",
+ "mavenUrls": [
+ "https://maven.photonvision.org/repository/internal",
+ "https://maven.photonvision.org/repository/snapshots"
+ ],
+ "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photonlib-cpp",
+ "version": "v2024.3.1",
+ "libName": "photonlib",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxathena",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ },
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photontargeting-cpp",
+ "version": "v2024.3.1",
+ "libName": "photontargeting",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxathena",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ],
+ "javaDependencies": [
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photonlib-java",
+ "version": "v2024.3.1"
+ },
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photontargeting-java",
+ "version": "v2024.3.1"
+ }
+ ]
+}
diff --git a/vendordeps/yagsl.json b/vendordeps/yagsl.json
new file mode 100644
index 00000000..9b441ee9
--- /dev/null
+++ b/vendordeps/yagsl.json
@@ -0,0 +1,52 @@
+{
+ "fileName": "yagsl.json",
+ "name": "YAGSL",
+ "version": "2024.7.0",
+ "frcYear": "2024",
+ "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400",
+ "mavenUrls": [
+ "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/repos"
+ ],
+ "jsonUrl": "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/yagsl.json",
+ "javaDependencies": [
+ {
+ "groupId": "swervelib",
+ "artifactId": "YAGSL-java",
+ "version": "2024.7.0"
+ }
+ ],
+ "requires": [
+ {
+ "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
+ "errorMessage": "REVLib is required!",
+ "offlineFileName": "REVLib.json",
+ "onlineUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json"
+ },
+ {
+ "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd",
+ "errorMessage": "ReduxLib is required!",
+ "offlineFileName": "ReduxLib_2024.json",
+ "onlineUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2024.json"
+ },
+ {
+ "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
+ "errorMessage": "Phoenix6 is required!",
+ "offlineFileName": "Phoenix6.json",
+ "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json"
+ },
+ {
+ "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
+ "errorMessage": "Phoenix5 is required!",
+ "offlineFileName": "Phoenix5.json",
+ "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json"
+ },
+ {
+ "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+ "errorMessage": "NavX is required!",
+ "offlineFileName": "NavX.json",
+ "onlineUrl": "https://dev.studica.com/releases/2024/NavX.json"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": []
+}