.
diff --git a/WPILib-License.md b/WPILib-License.md
new file mode 100644
index 0000000..645e542
--- /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 0000000..51a55b2
--- /dev/null
+++ b/build.gradle
@@ -0,0 +1,186 @@
+plugins {
+ id "java"
+ id "edu.wpi.first.GradleRIO" version "2024.3.2"
+ id "com.peterabeles.gversion" version "1.10"
+ id "com.diffplug.spotless" version "6.12.0"
+}
+
+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.add '-XDstringConcat=inline'
+}
+
+// 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/New_York"
+ indent = " "
+}
+
+// Spotless formatting
+project.compileJava.dependsOn(spotlessApply)
+spotless {
+ java {
+ target fileTree(".") {
+ include "**/*.java"
+ exclude "**/build/**", "**/build-*/**"
+ }
+ 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 "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 0000000..7f93135
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 0000000..1058752
--- /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.4-bin.zip
+networkTimeout=10000
+validateDistributionUrl=true
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=permwrapper/dists
diff --git a/gradlew b/gradlew
new file mode 100644
index 0000000..1aa94a4
--- /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 0000000..93e3f59
--- /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 0000000..d94f73c
--- /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/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto
new file mode 100644
index 0000000..77a8433
--- /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/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
new file mode 100644
index 0000000..690f5db
--- /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 0000000..8f3609b
--- /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/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
new file mode 100644
index 0000000..84e3801
--- /dev/null
+++ b/src/main/java/frc/robot/Constants.java
@@ -0,0 +1,37 @@
+// 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;
+
+/**
+ * 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 {
+ public static final Mode currentMode = Mode.REAL;
+ public static final boolean tuningMode = true;
+ public static enum Mode {
+ /** Running on a real robot. */
+ REAL,
+
+ /** Running a physics simulator. */
+ SIM,
+
+ /** Replaying from a log file. */
+ REPLAY
+ }
+}
diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java
new file mode 100644
index 0000000..e8af1c1
--- /dev/null
+++ b/src/main/java/frc/robot/Main.java
@@ -0,0 +1,34 @@
+// 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;
+
+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/Robot.java b/src/main/java/frc/robot/Robot.java
new file mode 100644
index 0000000..ea47d35
--- /dev/null
+++ b/src/main/java/frc/robot/Robot.java
@@ -0,0 +1,160 @@
+// 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;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import org.littletonrobotics.junction.LogFileUtil;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.Logger;
+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 autonomousCommand;
+ private RobotContainer robotContainer;
+
+ /**
+ * 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("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.currentMode) {
+ case REAL:
+ // Running on a real robot, log to a USB stick ("/U/logs")
+ Logger.addDataReceiver(new WPILOGWriter());
+ Logger.addDataReceiver(new NT4Publisher());
+ 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
+ Logger.start();
+
+ // Instantiate our RobotContainer. This will perform all our button bindings,
+ // and put our autonomous chooser on the dashboard.
+ robotContainer = new RobotContainer();
+ }
+
+ /** This function is called periodically during all modes. */
+ @Override
+ public void robotPeriodic() {
+ // 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() {}
+
+ /** This function is called periodically when disabled. */
+ @Override
+ public void disabledPeriodic() {}
+
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+ @Override
+ public void autonomousInit() {
+ autonomousCommand = robotContainer.getAutonomousCommand();
+
+ // schedule the autonomous command (example)
+ if (autonomousCommand != null) {
+ 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 (autonomousCommand != null) {
+ autonomousCommand.cancel();
+ }
+ }
+
+ /** 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();
+ }
+
+ /** 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 0000000..9b0dedc
--- /dev/null
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -0,0 +1,172 @@
+// 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;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+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.commands.DriveCommands;
+import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.drive.GyroIO;
+import frc.robot.subsystems.drive.GyroIOPigeon2;
+import frc.robot.subsystems.drive.ModuleIO;
+import frc.robot.subsystems.drive.ModuleIOSim;
+import frc.robot.subsystems.drive.ModuleIOSparkMax;
+import frc.robot.subsystems.flywheel.Flywheel;
+import frc.robot.subsystems.flywheel.FlywheelIO;
+import frc.robot.subsystems.flywheel.FlywheelIOSparkMax;
+import frc.robot.subsystems.wrist.FlywheelIOSim;
+
+import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
+import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // Subsystems
+ private final Drive drive;
+ private final Flywheel flywheel;
+
+ // Controller
+ private final CommandXboxController controller = new CommandXboxController(0);
+
+ // Dashboard inputs
+ private final LoggedDashboardChooser autoChooser;
+ private final LoggedDashboardNumber flywheelSpeedInput =
+ new LoggedDashboardNumber("Flywheel Speed", 1500.0);
+
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
+ public RobotContainer() {
+ switch (Constants.currentMode) {
+ case REAL:
+ // Real robot, instantiate hardware IO implementations
+ drive =
+ new Drive(
+ new GyroIOPigeon2(false),
+ new ModuleIOSparkMax(0),
+ new ModuleIOSparkMax(1),
+ new ModuleIOSparkMax(2),
+ new ModuleIOSparkMax(3));
+ flywheel = new Flywheel(new FlywheelIOSparkMax());
+ break;
+
+ case SIM:
+ // Sim robot, instantiate physics sim IO implementations
+ drive =
+ new Drive(
+ new GyroIO() {},
+ new ModuleIOSim(),
+ new ModuleIOSim(),
+ new ModuleIOSim(),
+ new ModuleIOSim());
+ flywheel = new Flywheel(new FlywheelIOSim());
+ break;
+
+ default:
+ // Replayed robot, disable IO implementations
+ drive =
+ new Drive(
+ new GyroIO() {},
+ new ModuleIO() {},
+ new ModuleIO() {},
+ new ModuleIO() {},
+ new ModuleIO() {});
+ flywheel = new Flywheel(new FlywheelIO() {});
+ break;
+ }
+
+ // Set up auto routines
+ NamedCommands.registerCommand(
+ "Run Flywheel",
+ Commands.startEnd(
+ () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel)
+ .withTimeout(5.0));
+ autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
+
+ // Set up SysId routines
+ autoChooser.addOption(
+ "Drive SysId (Quasistatic Forward)",
+ drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+ autoChooser.addOption(
+ "Drive SysId (Quasistatic Reverse)",
+ drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+ autoChooser.addOption(
+ "Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
+ autoChooser.addOption(
+ "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+ autoChooser.addOption(
+ "Flywheel SysId (Quasistatic Forward)",
+ flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+ autoChooser.addOption(
+ "Flywheel SysId (Quasistatic Reverse)",
+ flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+ autoChooser.addOption(
+ "Flywheel SysId (Dynamic Forward)", flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward));
+ autoChooser.addOption(
+ "Flywheel SysId (Dynamic Reverse)", flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+
+ // Configure the button bindings
+ configureButtonBindings();
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+ * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ */
+ private void configureButtonBindings() {
+ drive.setDefaultCommand(
+ DriveCommands.joystickDrive(
+ drive,
+ () -> -controller.getLeftY(),
+ () -> -controller.getLeftX(),
+ () -> -controller.getRightX()));
+ controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
+ controller
+ .b()
+ .onTrue(
+ Commands.runOnce(
+ () ->
+ drive.setPose(
+ new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
+ drive)
+ .ignoringDisable(true));
+ controller
+ .a()
+ .whileTrue(
+ Commands.startEnd(
+ () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel));
+ }
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ return autoChooser.get();
+ }
+}
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 0000000..fc8f596
--- /dev/null
+++ b/src/main/java/frc/robot/commands/DriveCommands.java
@@ -0,0 +1,77 @@
+// 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.subsystems.drive.Drive;
+import java.util.function.DoubleSupplier;
+
+public class DriveCommands {
+ private static final double DEADBAND = 0.1;
+
+ private DriveCommands() {}
+
+ /**
+ * Field relative drive command using two joysticks (controlling linear and angular velocities).
+ */
+ public static Command joystickDrive(
+ Drive drive,
+ DoubleSupplier xSupplier,
+ DoubleSupplier ySupplier,
+ DoubleSupplier omegaSupplier) {
+ return Commands.run(
+ () -> {
+ // Apply deadband
+ double linearMagnitude =
+ MathUtil.applyDeadband(
+ Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND);
+ Rotation2d linearDirection =
+ new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble());
+ double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND);
+
+ // Square values
+ linearMagnitude = linearMagnitude * linearMagnitude;
+ omega = Math.copySign(omega * omega, omega);
+
+ // Calcaulate new linear velocity
+ Translation2d linearVelocity =
+ new Pose2d(new Translation2d(), linearDirection)
+ .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d()))
+ .getTranslation();
+
+ // 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);
+ }
+}
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 0000000..c7b2048
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -0,0 +1,298 @@
+// 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 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.util.LocalADStarAK;
+import java.util.concurrent.locks.Lock;
+import java.util.concurrent.locks.ReentrantLock;
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+public class Drive extends SubsystemBase {
+ private static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5);
+ private static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0);
+ private static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0);
+ private static final double DRIVE_BASE_RADIUS =
+ Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0);
+ private static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
+
+ static final Lock odometryLock = new ReentrantLock();
+ 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 poseEstimator =
+ new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d());
+
+ public Drive(
+ GyroIO gyroIO,
+ ModuleIO flModuleIO,
+ ModuleIO frModuleIO,
+ ModuleIO blModuleIO,
+ ModuleIO brModuleIO) {
+ this.gyroIO = gyroIO;
+ modules[0] = new Module(flModuleIO, 0);
+ modules[1] = new Module(frModuleIO, 1);
+ modules[2] = new Module(blModuleIO, 2);
+ modules[3] = new Module(brModuleIO, 3);
+
+ // Start threads (no-op for each if no signals have been created)
+ SparkMaxOdometryThread.getInstance().start();
+
+ // Configure AutoBuilder for PathPlanner
+ AutoBuilder.configureHolonomic(
+ this::getPose,
+ this::setPose,
+ () -> kinematics.toChassisSpeeds(getModuleStates()),
+ this::runVelocity,
+ new HolonomicPathFollowerConfig(
+ MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, 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);
+ });
+
+ // 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() {
+ odometryLock.lock(); // Prevents odometry updates while reading data
+ gyroIO.updateInputs(gyroInputs);
+ for (var module : modules) {
+ module.updateInputs();
+ }
+ odometryLock.unlock();
+ 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[] {});
+ }
+
+ // Update odometry
+ double[] sampleTimestamps =
+ modules[0].getOdometryTimestamps(); // All signals are sampled together
+ int sampleCount = sampleTimestamps.length;
+ for (int i = 0; i < sampleCount; i++) {
+ // Read wheel positions and deltas from each module
+ SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];
+ SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4];
+ for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) {
+ modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i];
+ 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.odometryYawPositions[i];
+ } else {
+ // Use the angle delta from the kinematics and module deltas
+ Twist2d twist = kinematics.toTwist2d(moduleDeltas);
+ rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta));
+ }
+
+ // Apply update
+ poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions);
+ }
+ }
+
+ /**
+ * 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, MAX_LINEAR_SPEED);
+
+ // 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);
+ }
+
+ /** 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 poseEstimator.getEstimatedPosition();
+ }
+
+ /** Returns the current odometry rotation. */
+ public Rotation2d getRotation() {
+ return getPose().getRotation();
+ }
+
+ /** Resets the current odometry pose. */
+ public void setPose(Pose2d pose) {
+ 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) {
+ poseEstimator.addVisionMeasurement(visionPose, timestamp);
+ }
+
+ /** Returns the maximum linear speed in meters per sec. */
+ public double getMaxLinearSpeedMetersPerSec() {
+ return MAX_LINEAR_SPEED;
+ }
+
+ /** Returns the maximum angular speed in radians per sec. */
+ public double getMaxAngularSpeedRadPerSec() {
+ return MAX_ANGULAR_SPEED;
+ }
+
+ /** Returns an array of module translations. */
+ public static Translation2d[] getModuleTranslations() {
+ return new Translation2d[] {
+ new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0),
+ new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0),
+ new Translation2d(-TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0),
+ new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0)
+ };
+ }
+}
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 0000000..18ce6fd
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java
@@ -0,0 +1,30 @@
+// 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[] odometryYawTimestamps = new double[] {};
+ public Rotation2d[] odometryYawPositions = new Rotation2d[] {};
+ public double yawVelocityRadPerSec = 0.0;
+ }
+
+ public default void updateInputs(GyroIOInputs inputs) {}
+}
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 0000000..68a2417
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
@@ -0,0 +1,69 @@
+// 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.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 java.util.OptionalDouble;
+import java.util.Queue;
+
+/** IO implementation for Pigeon2 */
+public class GyroIOPigeon2 implements GyroIO {
+ private final Pigeon2 pigeon = new Pigeon2(20);
+ private final StatusSignal yaw = pigeon.getYaw();
+ private final Queue yawPositionQueue;
+ private final Queue yawTimestampQueue;
+ private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld();
+
+ public GyroIOPigeon2(boolean phoenixDrive) {
+ pigeon.getConfigurator().apply(new Pigeon2Configuration());
+ pigeon.getConfigurator().setYaw(0.0);
+ yaw.setUpdateFrequency(Module.ODOMETRY_FREQUENCY);
+ yawVelocity.setUpdateFrequency(100.0);
+ pigeon.optimizeBusUtilization();
+ yawTimestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue();
+ yawPositionQueue =
+ SparkMaxOdometryThread.getInstance()
+ .registerSignal(
+ () -> {
+ boolean valid = yaw.refresh().getStatus().isOK();
+ if (valid) {
+ return OptionalDouble.of(yaw.getValueAsDouble());
+ } else {
+ return OptionalDouble.empty();
+ }
+ });
+ }
+
+ @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());
+
+ inputs.odometryYawTimestamps =
+ yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray();
+ inputs.odometryYawPositions =
+ yawPositionQueue.stream()
+ .map((Double value) -> Rotation2d.fromDegrees(value))
+ .toArray(Rotation2d[]::new);
+ yawTimestampQueue.clear();
+ yawPositionQueue.clear();
+ }
+}
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 0000000..65016c4
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/Module.java
@@ -0,0 +1,204 @@
+// 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.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(2.0);
+ static final double ODOMETRY_FREQUENCY = 250.0;
+
+ 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
+ private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
+
+ 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.currentMode) {
+ case REAL:
+ case REPLAY:
+ driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13);
+ driveFeedback = new PIDController(0.05, 0.0, 0.0);
+ turnFeedback = new PIDController(7.0, 0.0, 0.0);
+ 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);
+ }
+
+ /**
+ * Update inputs without running the rest of the periodic logic. This is useful since these
+ * updates need to be properly thread-locked.
+ */
+ public void updateInputs() {
+ io.updateInputs(inputs);
+ }
+
+ public void periodic() {
+ 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));
+ }
+ }
+
+ // Calculate positions for odometry
+ int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together
+ odometryPositions = new SwerveModulePosition[sampleCount];
+ for (int i = 0; i < sampleCount; i++) {
+ double positionMeters = inputs.odometryDrivePositionsRad[i] * WHEEL_RADIUS;
+ Rotation2d angle =
+ inputs.odometryTurnPositions[i].plus(
+ turnRelativeOffset != null ? turnRelativeOffset : new Rotation2d());
+ odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
+ }
+ }
+
+ /** 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 module positions received this cycle. */
+ public SwerveModulePosition[] getOdometryPositions() {
+ return odometryPositions;
+ }
+
+ /** Returns the timestamps of the samples received this cycle. */
+ public double[] getOdometryTimestamps() {
+ return inputs.odometryTimestamps;
+ }
+
+ /** 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 0000000..200afa3
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java
@@ -0,0 +1,52 @@
+// 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[] {};
+
+ public double[] odometryTimestamps = new double[] {};
+ public double[] odometryDrivePositionsRad = new double[] {};
+ public Rotation2d[] odometryTurnPositions = new Rotation2d[] {};
+ }
+
+ /** 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/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java
new file mode 100644
index 0000000..f2ffe48
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java
@@ -0,0 +1,72 @@
+// 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.Timer;
+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())};
+
+ inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()};
+ inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad};
+ inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition};
+ }
+
+ @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/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java
new file mode 100644
index 0000000..781ec70
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java
@@ -0,0 +1,202 @@
+// 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.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.REVLibError;
+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;
+import java.util.OptionalDouble;
+import java.util.Queue;
+
+/**
+ * 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 {
+ // Gear ratios for SDS MK4i L2, adjust as necessary
+ private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
+ private static final double TURN_GEAR_RATIO = 150.0 / 7.0;
+
+ private final CANSparkMax driveSparkMax;
+ private final CANSparkMax turnSparkMax;
+
+ private final RelativeEncoder driveEncoder;
+ private final RelativeEncoder turnRelativeEncoder;
+ private final AnalogInput turnAbsoluteEncoder;
+ private final Queue timestampQueue;
+ private final Queue drivePositionQueue;
+ private final Queue turnPositionQueue;
+
+ private final boolean isTurnMotorInverted = true;
+ private final Rotation2d absoluteEncoderOffset;
+
+ public ModuleIOSparkMax(int index) {
+ switch (index) {
+ case 0:
+ driveSparkMax = new CANSparkMax(1, MotorType.kBrushless);
+ turnSparkMax = new CANSparkMax(2, MotorType.kBrushless);
+ turnAbsoluteEncoder = new AnalogInput(0);
+ absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
+ break;
+ case 1:
+ driveSparkMax = new CANSparkMax(3, MotorType.kBrushless);
+ turnSparkMax = new CANSparkMax(4, MotorType.kBrushless);
+ turnAbsoluteEncoder = new AnalogInput(1);
+ absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
+ break;
+ case 2:
+ driveSparkMax = new CANSparkMax(5, MotorType.kBrushless);
+ turnSparkMax = new CANSparkMax(6, MotorType.kBrushless);
+ turnAbsoluteEncoder = new AnalogInput(2);
+ absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
+ break;
+ case 3:
+ driveSparkMax = new CANSparkMax(7, MotorType.kBrushless);
+ turnSparkMax = new CANSparkMax(8, MotorType.kBrushless);
+ turnAbsoluteEncoder = new AnalogInput(3);
+ absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
+ 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(40);
+ turnSparkMax.setSmartCurrentLimit(30);
+ driveSparkMax.enableVoltageCompensation(12.0);
+ 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.setPeriodicFramePeriod(
+ PeriodicFrame.kStatus2, (int) (1000.0 / Module.ODOMETRY_FREQUENCY));
+ turnSparkMax.setPeriodicFramePeriod(
+ PeriodicFrame.kStatus2, (int) (1000.0 / Module.ODOMETRY_FREQUENCY));
+ timestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue();
+ drivePositionQueue =
+ SparkMaxOdometryThread.getInstance()
+ .registerSignal(
+ () -> {
+ double value = driveEncoder.getPosition();
+ if (driveSparkMax.getLastError() == REVLibError.kOk) {
+ return OptionalDouble.of(value);
+ } else {
+ return OptionalDouble.empty();
+ }
+ });
+ turnPositionQueue =
+ SparkMaxOdometryThread.getInstance()
+ .registerSignal(
+ () -> {
+ double value = turnRelativeEncoder.getPosition();
+ if (turnSparkMax.getLastError() == REVLibError.kOk) {
+ return OptionalDouble.of(value);
+ } else {
+ return OptionalDouble.empty();
+ }
+ });
+
+ driveSparkMax.burnFlash();
+ turnSparkMax.burnFlash();
+ }
+
+ @Override
+ public void updateInputs(ModuleIOInputs inputs) {
+ inputs.drivePositionRad =
+ Units.rotationsToRadians(driveEncoder.getPosition()) / DRIVE_GEAR_RATIO;
+ inputs.driveVelocityRadPerSec =
+ Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO;
+ 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() / TURN_GEAR_RATIO);
+ inputs.turnVelocityRadPerSec =
+ Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity())
+ / TURN_GEAR_RATIO;
+ inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage();
+ inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()};
+
+ inputs.odometryTimestamps =
+ timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
+ inputs.odometryDrivePositionsRad =
+ drivePositionQueue.stream()
+ .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO)
+ .toArray();
+ inputs.odometryTurnPositions =
+ turnPositionQueue.stream()
+ .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO))
+ .toArray(Rotation2d[]::new);
+ timestampQueue.clear();
+ drivePositionQueue.clear();
+ turnPositionQueue.clear();
+ }
+
+ @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/SparkMaxOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java
new file mode 100644
index 0000000..15db266
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java
@@ -0,0 +1,107 @@
+// 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.wpilibj.Notifier;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.OptionalDouble;
+import java.util.Queue;
+import java.util.concurrent.ArrayBlockingQueue;
+import java.util.function.Supplier;
+import org.littletonrobotics.junction.Logger;
+
+/**
+ * Provides an interface for asynchronously reading high-frequency measurements to a set of queues.
+ *
+ * This version is intended for devices like the SparkMax that require polling rather than a
+ * blocking thread. A Notifier thread is used to gather samples with consistent timing.
+ */
+public class SparkMaxOdometryThread {
+ private List> signals = new ArrayList<>();
+ private List> queues = new ArrayList<>();
+ private List> timestampQueues = new ArrayList<>();
+
+ private final Notifier notifier;
+ private static SparkMaxOdometryThread instance = null;
+
+ public static SparkMaxOdometryThread getInstance() {
+ if (instance == null) {
+ instance = new SparkMaxOdometryThread();
+ }
+ return instance;
+ }
+
+ private SparkMaxOdometryThread() {
+ notifier = new Notifier(this::periodic);
+ notifier.setName("SparkMaxOdometryThread");
+ }
+
+ public void start() {
+ if (timestampQueues.size() > 0) {
+ notifier.startPeriodic(1.0 / Module.ODOMETRY_FREQUENCY);
+ }
+ }
+
+ public Queue registerSignal(Supplier signal) {
+ Queue queue = new ArrayBlockingQueue<>(20);
+ Drive.odometryLock.lock();
+ try {
+ signals.add(signal);
+ queues.add(queue);
+ } finally {
+ Drive.odometryLock.unlock();
+ }
+ return queue;
+ }
+
+ public Queue makeTimestampQueue() {
+ Queue queue = new ArrayBlockingQueue<>(20);
+ Drive.odometryLock.lock();
+ try {
+ timestampQueues.add(queue);
+ } finally {
+ Drive.odometryLock.unlock();
+ }
+ return queue;
+ }
+
+ private void periodic() {
+ Drive.odometryLock.lock();
+ double timestamp = Logger.getRealTimestamp() / 1e6;
+ try {
+ double[] values = new double[signals.size()];
+ boolean isValid = true;
+ for (int i = 0; i < signals.size(); i++) {
+ OptionalDouble value = signals.get(i).get();
+ if (value.isPresent()) {
+ values[i] = value.getAsDouble();
+ } else {
+ isValid = false;
+ break;
+ }
+ }
+ if (isValid) {
+ for (int i = 0; i < queues.size(); i++) {
+ queues.get(i).offer(values[i]);
+ }
+ for (int i = 0; i < timestampQueues.size(); i++) {
+ timestampQueues.get(i).offer(timestamp);
+ }
+ }
+ } finally {
+ Drive.odometryLock.unlock();
+ }
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java
new file mode 100644
index 0000000..6056b24
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java
@@ -0,0 +1,110 @@
+// 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;
+
+import static edu.wpi.first.units.Units.*;
+
+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.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.Constants;
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+public class Flywheel extends SubsystemBase {
+ 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.currentMode) {
+ case REAL:
+ case REPLAY:
+ ffModel = new SimpleMotorFeedforward(0.1, 0.05);
+ io.configurePID(1.0, 0.0, 0.0);
+ break;
+ case SIM:
+ ffModel = new SimpleMotorFeedforward(0.0, 0.03);
+ io.configurePID(0.5, 0.0, 0.0);
+ 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
+ public double getVelocityRPM() {
+ return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec);
+ }
+
+ /** Returns the current velocity in radians per second. */
+ public double getCharacterizationVelocity() {
+ return inputs.velocityRadPerSec;
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java
new file mode 100644
index 0000000..98f7624
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java
@@ -0,0 +1,41 @@
+// 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;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface FlywheelIO {
+ @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/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java
new file mode 100644
index 0000000..32ffa6f
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/flywheel/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;
+
+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/FlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java
new file mode 100644
index 0000000..787f369
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java
@@ -0,0 +1,89 @@
+// 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;
+
+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;
+
+/**
+ * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with
+ * "CANSparkFlex".
+ */
+public class FlywheelIOSparkMax implements FlywheelIO {
+ private static final double GEAR_RATIO = 1.5;
+
+ private final CANSparkMax leader = new CANSparkMax(0, MotorType.kBrushless);
+ private final CANSparkMax follower = new CANSparkMax(1, MotorType.kBrushless);
+ private final RelativeEncoder encoder = leader.getEncoder();
+ private final SparkPIDController pid = leader.getPIDController();
+
+ 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() / GEAR_RATIO);
+ inputs.velocityRadPerSec =
+ Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / GEAR_RATIO);
+ 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) * GEAR_RATIO,
+ 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/wrist/WristIO.java b/src/main/java/frc/robot/subsystems/wrist/WristIO.java
new file mode 100644
index 0000000..85117f9
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/wrist/WristIO.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.wrist;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface WristIO {
+ @AutoLog
+ public static class WristIOInputs {
+ public double wristAngleRad = 0.0;
+ public double wristVelocityRadPerSec = 0.0;
+ public double wristAppliedVolts = 0.0;
+ public double wristCurrentAmps = 0.0;
+ public double wristTemperature = 0.0;
+
+ public double rollerVelocityRadPerSec = 0.0;
+ public double rollerAppliedVolts = 0.0;
+ public double rollerCurrentAmps = 0.0;
+ public double rollerTemperature = 0.0;
+ }
+
+ /** Updates the set of loggable inputs. */
+ public default void updateInputs(WristIOInputs inputs) {}
+
+ public default void setWristPosition(double positionRad, double ffVolts) {}
+
+ public default void setWristVolts(double volts){}
+
+ /** Run closed loop at the specified velocity. */
+ public default void setRollerVelocity(double velocityRadPerSec, double ffVolts) {}
+
+ /** Set velocity PID constants. */
+ public default void configureRollerPID(double kP, double kI, double kD) {}
+}
diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java b/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java
new file mode 100644
index 0000000..4beede8
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java
@@ -0,0 +1,76 @@
+// 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.wrist;
+
+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.math.util.Units;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+
+public class WristIOSim implements WristIO {
+ private static final double LOOP_PERIOD_SECS = 0.02;
+
+ private SingleJointedArmSim wristSim = new SingleJointedArmSim(DCMotor.getNEO(1), 4.0, 0.1/*fix*/, Units.inchesToMeters(13), Units.degreesToRadians(-50), Units.degreesToRadians(250), true, Units.degreesToRadians(-50));
+ private FlywheelSim rollerSim = new FlywheelSim(DCMotor.getNEO(1), 1, 0.004);
+ private PIDController rollerPID = new PIDController(1.0, 0.0, 0.0);
+ private double rollerAppliedVolts = 0.0;
+ private double rollerFFVolts = 0.0;
+
+ private double wristAppliedVolts = 0.0;
+
+ @Override
+ public void updateInputs(WristIOInputs inputs) {
+ //because we're doing rio pid, we don't needa do da math
+ wristSim.update(LOOP_PERIOD_SECS);
+
+ inputs.wristAngleRad = wristSim.getAngleRads();
+ inputs.wristVelocityRadPerSec = wristSim.getVelocityRadPerSec();
+ inputs.wristAppliedVolts = wristAppliedVolts;
+ inputs.wristCurrentAmps = wristSim.getCurrentDrawAmps();
+ inputs.wristTemperature = -1.0;
+
+ //because we use motor pid, we do da math
+ rollerAppliedVolts =
+ MathUtil.clamp(rollerPID.calculate(rollerSim.getAngularVelocityRadPerSec())
+ + rollerFFVolts, -12.0, 12.0);
+ rollerSim.setInputVoltage(rollerAppliedVolts);
+ rollerSim.update(LOOP_PERIOD_SECS);
+
+ inputs.rollerVelocityRadPerSec = rollerSim.getAngularVelocityRadPerSec();
+ inputs.rollerAppliedVolts = rollerAppliedVolts;
+ inputs.rollerCurrentAmps = rollerSim.getCurrentDrawAmps();
+ inputs.rollerTemperature = -1.0;
+ }
+
+ @Override
+ public void setWristVolts(double volts){
+ wristAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0);
+ wristSim.setInputVoltage(wristAppliedVolts);
+ }
+
+ /** Run closed loop at the specified velocity. */
+ @Override
+ public void setRollerVelocity(double velocityRadPerSec, double ffVolts) {
+ rollerPID.setSetpoint(velocityRadPerSec);
+ rollerFFVolts = ffVolts;
+ }
+
+ /** Set velocity PID constants. */
+ @Override
+ public void configureRollerPID(double kP, double kI, double kD) {
+ rollerPID.setPID(kP, kI, kD);
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOSparkMax.java b/src/main/java/frc/robot/subsystems/wrist/WristIOSparkMax.java
new file mode 100644
index 0000000..86305bf
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/wrist/WristIOSparkMax.java
@@ -0,0 +1,95 @@
+// 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.wrist;
+
+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;
+
+public class WristIOSparkMax implements WristIO {
+ private static final double GEAR_RATIO = 1;
+
+ private final CANSparkMax wrist = new CANSparkMax(0, MotorType.kBrushless);
+ private final RelativeEncoder wristEncoder = wrist.getEncoder();
+
+ private final CANSparkMax rollers = new CANSparkMax(1, MotorType.kBrushless);
+ private final RelativeEncoder rollerEncoder = rollers.getEncoder();
+ private final SparkPIDController rollerPID = rollers.getPIDController();
+
+ public WristIOSparkMax() {
+ wrist.restoreFactoryDefaults();
+ rollers.restoreFactoryDefaults();
+
+ wrist.setCANTimeout(250);
+ rollers.setCANTimeout(250);
+
+ wrist.setInverted(false);
+ rollers.setInverted(false);
+
+ wrist.enableVoltageCompensation(12.0);
+ rollers.enableVoltageCompensation(12.0);
+
+ wrist.setSmartCurrentLimit(25);
+ rollers.setSmartCurrentLimit(30);
+
+ wrist.burnFlash();
+ rollers.burnFlash();
+ }
+
+ @Override
+ public void updateInputs(WristIOInputs inputs) {
+ inputs.wristAngleRad = Units.rotationsToRadians(wristEncoder.getPosition() / GEAR_RATIO);
+ inputs.wristVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(wristEncoder.getVelocity() / GEAR_RATIO);
+ inputs.wristAppliedVolts = wrist.getAppliedOutput() * wrist.getBusVoltage();
+ inputs.wristCurrentAmps = wrist.getOutputCurrent();
+ inputs.wristTemperature = wrist.getMotorTemperature();
+
+ inputs.rollerVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(rollerEncoder.getVelocity() / GEAR_RATIO);
+ inputs.rollerAppliedVolts = rollers.getAppliedOutput() * rollers.getBusVoltage();
+ inputs.rollerCurrentAmps = rollers.getOutputCurrent();
+ inputs.rollerTemperature = rollers.getMotorTemperature();
+ }
+
+ @Override
+ public void setVoltage(double volts) {
+ leader.setVoltage(volts);
+ }
+
+ @Override
+ public void setVelocity(double velocityRadPerSec, double ffVolts) {
+ pid.setReference(
+ Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO,
+ 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/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java
new file mode 100644
index 0000000..191b54f
--- /dev/null
+++ b/src/main/java/frc/robot/util/LocalADStarAK.java
@@ -0,0 +1,153 @@
+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;
+
+// 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 0000000..27bc464
--- /dev/null
+++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java
@@ -0,0 +1,124 @@
+// 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 frc.robot.util;
+
+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;
+
+import frc.robot.Constants;
+
+/**
+ * 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();
+ }
+}
\ No newline at end of file
diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
new file mode 100644
index 0000000..55a634d
--- /dev/null
+++ b/vendordeps/AdvantageKit.json
@@ -0,0 +1,42 @@
+{
+ "fileName": "AdvantageKit.json",
+ "name": "AdvantageKit",
+ "version": "3.2.0",
+ "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.0"
+ },
+ {
+ "groupId": "org.littletonrobotics.akit.junction",
+ "artifactId": "junction-core",
+ "version": "3.2.0"
+ },
+ {
+ "groupId": "org.littletonrobotics.akit.conduit",
+ "artifactId": "conduit-api",
+ "version": "3.2.0"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "org.littletonrobotics.akit.conduit",
+ "artifactId": "conduit-wpilibio",
+ "version": "3.2.0",
+ "skipInvalidPlatforms": false,
+ "isJar": false,
+ "validPlatforms": [
+ "linuxathena",
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ],
+ "cppDependencies": []
+}
diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
new file mode 100644
index 0000000..544956a
--- /dev/null
+++ b/vendordeps/PathplannerLib.json
@@ -0,0 +1,38 @@
+{
+ "fileName": "PathplannerLib.json",
+ "name": "PathplannerLib",
+ "version": "2024.2.3",
+ "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.3"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "com.pathplanner.lib",
+ "artifactId": "PathplannerLib-cpp",
+ "version": "2024.2.3",
+ "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 0000000..f572dcc
--- /dev/null
+++ b/vendordeps/Phoenix5.json
@@ -0,0 +1,151 @@
+{
+ "fileName": "Phoenix5.json",
+ "name": "CTRE-Phoenix (v5)",
+ "version": "5.33.0",
+ "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.0"
+ },
+ {
+ "groupId": "com.ctre.phoenix",
+ "artifactId": "wpiapi-java",
+ "version": "5.33.0"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "com.ctre.phoenix",
+ "artifactId": "cci",
+ "version": "5.33.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix.sim",
+ "artifactId": "cci-sim",
+ "version": "5.33.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "com.ctre.phoenix",
+ "artifactId": "wpiapi-cpp",
+ "version": "5.33.0",
+ "libName": "CTRE_Phoenix_WPI",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix",
+ "artifactId": "api-cpp",
+ "version": "5.33.0",
+ "libName": "CTRE_Phoenix",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix",
+ "artifactId": "cci",
+ "version": "5.33.0",
+ "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.0",
+ "libName": "CTRE_Phoenix_WPISim",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix.sim",
+ "artifactId": "api-cpp-sim",
+ "version": "5.33.0",
+ "libName": "CTRE_PhoenixSim",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix.sim",
+ "artifactId": "cci-sim",
+ "version": "5.33.0",
+ "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 0000000..e9113c8
--- /dev/null
+++ b/vendordeps/Phoenix6.json
@@ -0,0 +1,339 @@
+{
+ "fileName": "Phoenix6.json",
+ "name": "CTRE-Phoenix (v6)",
+ "version": "24.2.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.2.0"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "tools",
+ "version": "24.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "tools-sim",
+ "version": "24.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simTalonSRX",
+ "version": "24.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simTalonFX",
+ "version": "24.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simVictorSPX",
+ "version": "24.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simPigeonIMU",
+ "version": "24.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simCANCoder",
+ "version": "24.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProTalonFX",
+ "version": "24.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProCANcoder",
+ "version": "24.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix6.sim",
+ "artifactId": "simProPigeon2",
+ "version": "24.2.0",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "wpiapi-cpp",
+ "version": "24.2.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.2.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.2.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.2.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.2.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.2.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.2.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.2.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.2.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.2.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.2.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.2.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 0000000..6bb009c
--- /dev/null
+++ b/vendordeps/REVLib.json
@@ -0,0 +1,74 @@
+{
+ "fileName": "REVLib.json",
+ "name": "REVLib",
+ "version": "2024.2.1",
+ "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.1"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-driver",
+ "version": "2024.2.1",
+ "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.1",
+ "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.1",
+ "libName": "REVLibDriver",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "windowsx86",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ }
+ ]
+}
diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json
new file mode 100644
index 0000000..67bf389
--- /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"
+ ]
+ }
+ ]
+}