Initial commit

photonlib-cpp-examples/.styleguide-license

@@ -0,0 +1,23 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */

photonlib-cpp-examples/README.md

@@ -0,0 +1,7 @@
+## PhotonLib C++ Examples
+
+### Running examples
+
+For instructions on how to run these examples locally, see [Running Examples](https://docs.photonvision.org/en/latest/docs/contributing/photonvision/build-instructions.html#running-examples).
+
+---

photonlib-cpp-examples/aimandrange/.gitignore

@@ -0,0 +1 @@
+vendordeps

photonlib-cpp-examples/aimandrange/.wpilib/wpilib_preferences.json

@@ -0,0 +1,6 @@
+{
+    "enableCppIntellisense": true,
+    "currentLanguage": "cpp",
+    "projectYear": "2025",
+    "teamNumber": 5
+}

photonlib-cpp-examples/aimandrange/WPILib-License.md

@@ -0,0 +1,24 @@
+Copyright (c) 2009-2021 FIRST and other WPILib contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+   * Neither the name of FIRST, WPILib, nor the names of other WPILib
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

photonlib-cpp-examples/aimandrange/build.gradle

@@ -0,0 +1,117 @@
+plugins {
+    id "cpp"
+    id "google-test-test-suite"
+    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2"
+}
+
+repositories {
+    mavenLocal()
+    jcenter()
+}
+
+wpi.maven.useLocal = false
+wpi.maven.useDevelopment = false
+wpi.versions.wpilibVersion = "2025.1.1-beta-2"
+wpi.versions.wpimathVersion = "2025.1.1-beta-2"
+
+// 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.getTeamOrDefault(5940)
+            debug = project.frc.getDebugOrDefault(false)
+
+            artifacts {
+                // First part is artifact name, 2nd is artifact type
+                // getTargetTypeClass is a shortcut to get the class type using a string
+
+                frcCpp(getArtifactTypeClass('FRCNativeArtifact')) {
+                }
+
+                // Static files artifact
+                frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+                    files = project.fileTree('src/main/deploy')
+                    directory = '/home/lvuser/deploy'
+                }
+            }
+        }
+    }
+}
+
+def deployArtifact = deploy.targets.roborio.artifacts.frcCpp
+
+// Set this to true to enable desktop support.
+def includeDesktopSupport = true
+
+// Set to true to run simulation in debug mode
+wpi.cpp.debugSimulation = false
+
+// Default enable simgui
+wpi.sim.addGui().defaultEnabled = true
+// Enable DS but not by default
+wpi.sim.addDriverstation()
+
+model {
+    components {
+        frcUserProgram(NativeExecutableSpec) {
+            // We don't need to build for roborio -- if we do, we need to install
+            // a roborio toolchain every time we build in CI
+            // Ideally, we'd be able to set the roborio toolchain as optional, but
+            // I can't figure out how to set that environment variable from build.gradle
+            // (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71)
+            // for now, commented out
+
+            // targetPlatform wpi.platforms.roborio
+
+            if (includeDesktopSupport) {
+                targetPlatform wpi.platforms.desktop
+            }
+
+            sources.cpp {
+                source {
+                    srcDir 'src/main/cpp'
+                    include '**/*.cpp', '**/*.cc'
+                }
+                exportedHeaders {
+                    srcDir 'src/main/include'
+                }
+            }
+
+            // Set deploy task to deploy this component
+            deployArtifact.component = it
+
+            // Enable run tasks for this component
+            wpi.cpp.enableExternalTasks(it)
+
+            // Enable simulation for this component
+            wpi.sim.enable(it)
+            // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+            wpi.cpp.vendor.cpp(it)
+            wpi.cpp.deps.wpilib(it)
+        }
+    }
+    testSuites {
+        frcUserProgramTest(GoogleTestTestSuiteSpec) {
+            testing $.components.frcUserProgram
+
+            sources.cpp {
+                source {
+                    srcDir 'src/test/cpp'
+                    include '**/*.cpp'
+                }
+            }
+
+            // Enable run tasks for this component
+            wpi.cpp.enableExternalTasks(it)
+
+            wpi.cpp.vendor.cpp(it)
+            wpi.cpp.deps.wpilib(it)
+            wpi.cpp.deps.googleTest(it)
+        }
+    }
+}

photonlib-cpp-examples/aimandrange/gradlew

@@ -0,0 +1,241 @@
+#!/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/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+#       within the Gradle project.
+#
+#       You can find Gradle at https://github.com/gradle/gradle/.
+#
+##############################################################################
+
+# Attempt to set APP_HOME
+
+# Resolve links: $0 may be a link
+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
+
+# 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
+
+APP_NAME="Gradle"
+APP_BASE_NAME=${0##*/}
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD=maximum
+
+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
+    which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+fi
+
+# Increase the maximum file descriptors if we can.
+if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
+    case $MAX_FD in #(
+      max*)
+        MAX_FD=$( ulimit -H -n ) ||
+            warn "Could not query maximum file descriptor limit"
+    esac
+    case $MAX_FD in  #(
+      '' | soft) :;; #(
+      *)
+        ulimit -n "$MAX_FD" ||
+            warn "Could not set maximum file descriptor limit to $MAX_FD"
+    esac
+fi
+
+# 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
+
+# Collect all arguments for the java command;
+#   * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of
+#     shell script including quotes and variable substitutions, so put them in
+#     double quotes to make sure that they get re-expanded; and
+#   * put everything else in single quotes, so that it's not re-expanded.
+
+set -- \
+        "-Dorg.gradle.appname=$APP_BASE_NAME" \
+        -classpath "$CLASSPATH" \
+        org.gradle.wrapper.GradleWrapperMain \
+        "$@"
+
+# Stop when "xargs" is not available.
+if ! command -v xargs >/dev/null 2>&1
+then
+    die "xargs is not available"
+fi
+
+# Use "xargs" to parse quoted args.
+#
+# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
+#
+# In Bash we could simply go:
+#
+#   readarray ARGS < <( xargs -n1 <<<"$var" ) &&
+#   set -- "${ARGS[@]}" "$@"
+#
+# but POSIX shell has neither arrays nor command substitution, so instead we
+# post-process each arg (as a line of input to sed) to backslash-escape any
+# character that might be a shell metacharacter, then use eval to reverse
+# that process (while maintaining the separation between arguments), and wrap
+# the whole thing up as a single "set" statement.
+#
+# This will of course break if any of these variables contains a newline or
+# an unmatched quote.
+#
+
+eval "set -- $(
+        printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
+        xargs -n1 |
+        sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
+        tr '\n' ' '
+    )" '"$@"'
+
+exec "$JAVACMD" "$@"

photonlib-cpp-examples/aimandrange/gradlew.bat

@@ -0,0 +1,91 @@
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem      https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+
+@if "%DEBUG%"=="" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%"=="" set DIRNAME=.
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem 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

photonlib-cpp-examples/aimandrange/networktables.json

@@ -0,0 +1 @@
+[]

photonlib-cpp-examples/aimandrange/settings.gradle

@@ -0,0 +1,30 @@
+import org.gradle.internal.os.OperatingSystem
+
+rootProject.name = 'aimandrange'
+
+pluginManagement {
+    repositories {
+        mavenLocal()
+        jcenter()
+        gradlePluginPortal()
+        String frcYear = '2025'
+        File frcHome
+        if (OperatingSystem.current().isWindows()) {
+            String publicFolder = System.getenv('PUBLIC')
+            if (publicFolder == null) {
+                publicFolder = "C:\\Users\\Public"
+            }
+            def homeRoot = new File(publicFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        } else {
+            def userFolder = System.getProperty("user.home")
+            def homeRoot = new File(userFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        }
+        def frcHomeMaven = new File(frcHome, 'maven')
+        maven {
+            name 'frcHome'
+            url frcHomeMaven
+        }
+    }
+}

photonlib-cpp-examples/aimandrange/simgui-ds.json

@@ -0,0 +1,100 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decayRate": 0.0,
+          "keyRate": 0.009999999776482582
+        },
+        {},
+        {
+          "decKey": 81,
+          "incKey": 69
+        }
+      ],
+      "axisCount": 6,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
+  ]
+}

photonlib-cpp-examples/aimandrange/simgui-window.json

@@ -0,0 +1,74 @@
+{
+  "Docking": {
+    "Data": []
+  },
+  "MainWindow": {
+    "GLOBAL": {
+      "fps": "120",
+      "height": "910",
+      "maximized": "0",
+      "style": "0",
+      "userScale": "2",
+      "width": "1550",
+      "xpos": "-1602",
+      "ypos": "79"
+    }
+  },
+  "Window": {
+    "###/SmartDashboard/VisionSystemSim-main/Sim Field": {
+      "Collapsed": "0",
+      "Pos": "554,10",
+      "Size": "991,527"
+    },
+    "###FMS": {
+      "Collapsed": "0",
+      "Pos": "12,604",
+      "Size": "202,214"
+    },
+    "###Joysticks": {
+      "Collapsed": "0",
+      "Pos": "278,685",
+      "Size": "976,219"
+    },
+    "###NetworkTables": {
+      "Collapsed": "0",
+      "Pos": "353,479",
+      "Size": "830,620"
+    },
+    "###NetworkTables Info": {
+      "Collapsed": "0",
+      "Pos": "315,527",
+      "Size": "750,145"
+    },
+    "###Other Devices": {
+      "Collapsed": "0",
+      "Pos": "1025,20",
+      "Size": "250,695"
+    },
+    "###Plot <0>": {
+      "Collapsed": "0",
+      "Pos": "113,22",
+      "Size": "448,400"
+    },
+    "###System Joysticks": {
+      "Collapsed": "0",
+      "Pos": "5,350",
+      "Size": "232,254"
+    },
+    "###Timing": {
+      "Collapsed": "0",
+      "Pos": "5,150",
+      "Size": "162,142"
+    },
+    "Debug##Default": {
+      "Collapsed": "0",
+      "Pos": "60,60",
+      "Size": "400,400"
+    },
+    "Robot State": {
+      "Collapsed": "0",
+      "Pos": "5,20",
+      "Size": "109,134"
+    }
+  }
+}

photonlib-cpp-examples/aimandrange/simgui.json

@@ -0,0 +1,164 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d"
+    },
+    "windows": {
+      "/SmartDashboard/VisionSystemSim-main/Sim Field": {
+        "EstimatedRobot": {
+          "arrowWeight": 3.0,
+          "length": 0.800000011920929,
+          "selectable": false,
+          "weight": 3.0,
+          "width": 0.800000011920929
+        },
+        "EstimatedRobotModules": {
+          "arrows": false,
+          "image": "swerve_module.png",
+          "length": 0.30000001192092896,
+          "selectable": false,
+          "width": 0.30000001192092896
+        },
+        "Robot": {
+          "arrowColor": [
+            1.0,
+            1.0,
+            1.0,
+            255.0
+          ],
+          "arrowWeight": 2.0,
+          "color": [
+            1.0,
+            1.0,
+            1.0,
+            255.0
+          ],
+          "length": 0.800000011920929,
+          "selectable": false,
+          "weight": 2.0,
+          "width": 0.800000011920929
+        },
+        "VisionEstimation": {
+          "arrowColor": [
+            0.0,
+            0.6075949668884277,
+            1.0,
+            255.0
+          ],
+          "arrowWeight": 2.0,
+          "color": [
+            0.0,
+            0.6075949668884277,
+            1.0,
+            255.0
+          ],
+          "selectable": false,
+          "weight": 2.0
+        },
+        "apriltag": {
+          "arrows": false,
+          "image": "tag-green.png",
+          "length": 0.6000000238418579,
+          "width": 0.5
+        },
+        "bottom": 1476,
+        "cameras": {
+          "arrowColor": [
+            0.29535865783691406,
+            1.0,
+            0.9910804033279419,
+            255.0
+          ],
+          "arrowSize": 19,
+          "arrowWeight": 3.0,
+          "length": 1.0,
+          "style": "Hidden",
+          "weight": 1.0,
+          "width": 1.0
+        },
+        "height": 8.210550308227539,
+        "image": "2023-field.png",
+        "left": 150,
+        "right": 2961,
+        "top": 79,
+        "visibleTargetPoses": {
+          "arrows": false,
+          "image": "tag-blue.png",
+          "length": 0.5,
+          "selectable": false,
+          "width": 0.4000000059604645
+        },
+        "width": 16.541748046875,
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "CameraPublisher": {
+        "YOUR CAMERA NAME-processed": {
+          "open": true,
+          "string[]##v_/CameraPublisher/YOUR CAMERA NAME-processed/streams": {
+            "open": true
+          }
+        },
+        "open": true
+      },
+      "SmartDashboard": {
+        "VisionSystemSim-main": {
+          "open": true
+        },
+        "open": true
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  },
+  "Plot": {
+    "Plot <0>": {
+      "plots": [
+        {
+          "axis": [
+            {
+              "autoFit": true
+            }
+          ],
+          "backgroundColor": [
+            0.0,
+            0.0,
+            0.0,
+            0.8500000238418579
+          ],
+          "height": 332,
+          "series": [
+            {
+              "color": [
+                0.2980392277240753,
+                0.44705885648727417,
+                0.6901960968971252,
+                1.0
+              ],
+              "id": "NT:/SmartDashboard/GPLauncher Act Spd (RPM)"
+            },
+            {
+              "color": [
+                0.8666667342185974,
+                0.5176470875740051,
+                0.32156863808631897,
+                1.0
+              ],
+              "id": "NT:/SmartDashboard/GPLauncher Des Spd (RPM)"
+            }
+          ]
+        }
+      ],
+      "window": {
+        "visible": false
+      }
+    }
+  }
+}

photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp

@@ -0,0 +1,135 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "Robot.h"
+
+#include <photon/PhotonUtils.h>
+
+#include <iostream>
+
+#include <frc/simulation/BatterySim.h>
+#include <frc/simulation/RoboRioSim.h>
+
+void Robot::RobotInit() {}
+
+void Robot::RobotPeriodic() {
+  drivetrain.Periodic();
+  drivetrain.Log();
+}
+
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() { drivetrain.Stop(); }
+
+void Robot::DisabledExit() {}
+
+void Robot::AutonomousInit() {}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::AutonomousExit() {}
+
+void Robot::TeleopInit() {
+  frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}};
+  drivetrain.ResetPose(pose, true);
+}
+
+void Robot::TeleopPeriodic() {
+  // Calculate drivetrain commands from Joystick values
+  auto forward =
+      -1.0 * controller.GetLeftY() * constants::Swerve::kMaxLinearSpeed;
+  auto strafe =
+      -1.0 * controller.GetLeftX() * constants::Swerve::kMaxLinearSpeed;
+  auto turn =
+      -1.0 * controller.GetRightX() * constants::Swerve::kMaxAngularSpeed;
+
+  bool targetVisible = false;
+  units::degree_t targetYaw = 0.0_deg;
+  units::meter_t targetRange = 0.0_m;
+  auto results = camera.GetAllUnreadResults();
+  if (results.size() > 0) {
+    // Camera processed a new frame since last
+    // Get the last one in the list.
+    auto result = results[results.size() - 1];
+    if (result.HasTargets()) {
+      // At least one AprilTag was seen by the camera
+      for (auto& target : result.GetTargets()) {
+        if (target.GetFiducialId() == 7) {
+          // Found Tag 7, record its information
+          targetYaw = units::degree_t{target.GetYaw()};
+          targetRange = photon::PhotonUtils::CalculateDistanceToTarget(
+              0.5_m,      // Measured with a tape measure, or in CAD
+              1.435_m,    // From 2024 game manual for ID 7
+              -30.0_deg,  // Measured with a protractor, or in CAD
+              units::degree_t{target.GetPitch()});
+          targetVisible = true;
+        }
+      }
+    }
+  }
+
+  // Auto-align when requested
+  if (controller.GetAButton() && targetVisible) {
+    // Driver wants auto-alignment to tag 7
+    // And, tag 7 is in sight, so we can turn toward it.
+    // Override the driver's turn command with an automatic one that turns
+    // toward the tag and gets the range right.
+    turn = (VISION_DES_ANGLE - targetYaw).value() * VISION_TURN_kP *
+           constants::Swerve::kMaxAngularSpeed;
+    forward = (VISION_DES_RANGE - targetRange).value() * VISION_STRAFE_kP *
+              constants::Swerve::kMaxLinearSpeed;
+  }
+
+  // Command drivetrain motors based on target speeds
+  drivetrain.Drive(forward, strafe, turn);
+}
+
+void Robot::TeleopExit() {}
+
+void Robot::TestInit() {}
+
+void Robot::TestPeriodic() {}
+
+void Robot::TestExit() {}
+
+void Robot::SimulationPeriodic() {
+  drivetrain.SimulationPeriodic();
+  vision.SimPeriodic(drivetrain.GetSimPose());
+
+  frc::Field2d& debugField = vision.GetSimDebugField();
+  debugField.GetObject("EstimatedRobot")->SetPose(drivetrain.GetPose());
+  debugField.GetObject("EstimatedRobotModules")
+      ->SetPoses(drivetrain.GetModulePoses());
+
+  units::ampere_t totalCurrent = drivetrain.GetCurrentDraw();
+  units::volt_t loadedBattVolts =
+      frc::sim::BatterySim::Calculate({totalCurrent});
+  // Using max(0.1, voltage) here isn't a *physically correct* solution,
+  // but it avoids problems with battery voltage measuring 0.
+  frc::sim::RoboRioSim::SetVInVoltage(units::math::max(0.1_V, loadedBattVolts));
+}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp

@@ -0,0 +1,200 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "subsystems/SwerveDrive.h"
+
+#include <iostream>
+#include <string>
+
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+SwerveDrive::SwerveDrive()
+    : poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(),
+                    frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}),
+      gyroSim(gyro),
+      swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1),
+                     constants::Swerve::kDriveGearRatio,
+                     constants::Swerve::kWheelDiameter / 2,
+                     constants::Swerve::kSteerFF, frc::DCMotor::Falcon500(1),
+                     constants::Swerve::kSteerGearRatio, kinematics) {}
+
+void SwerveDrive::Periodic() {
+  for (auto& currentModule : swerveMods) {
+    currentModule.Periodic();
+  }
+
+  poseEstimator.Update(GetGyroYaw(), GetModulePositions());
+}
+
+void SwerveDrive::Drive(units::meters_per_second_t vx,
+                        units::meters_per_second_t vy,
+                        units::radians_per_second_t omega) {
+  frc::ChassisSpeeds newChassisSpeeds =
+      frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading());
+  SetChassisSpeeds(newChassisSpeeds, true, false);
+}
+
+void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds,
+                                   bool openLoop, bool steerInPlace) {
+  SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), true,
+                  steerInPlace);
+  this->targetChassisSpeeds = newChassisSpeeds;
+}
+
+void SwerveDrive::SetModuleStates(
+    const std::array<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
+    bool steerInPlace) {
+  std::array<frc::SwerveModuleState, 4> desaturatedStates = desiredStates;
+  frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
+      static_cast<wpi::array<frc::SwerveModuleState, 4>*>(&desaturatedStates),
+      constants::Swerve::kMaxLinearSpeed);
+  for (int i = 0; i < swerveMods.size(); i++) {
+    swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace);
+  }
+}
+
+void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s); }
+
+void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) {
+  if (resetSimPose) {
+    swerveDriveSim.Reset(pose, false);
+    for (int i = 0; i < swerveMods.size(); i++) {
+      swerveMods[i].SimulationUpdate(0_m, 0_mps, 0_A, 0_rad, 0_rad_per_s, 0_A);
+    }
+    gyroSim.SetAngle(-pose.Rotation().Degrees());
+    gyroSim.SetRate(0_rad_per_s);
+  }
+
+  poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose);
+}
+
+frc::Pose2d SwerveDrive::GetPose() const {
+  return poseEstimator.GetEstimatedPosition();
+}
+
+frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); }
+
+frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); }
+
+frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const {
+  return kinematics.ToChassisSpeeds(GetModuleStates());
+}
+
+std::array<frc::SwerveModuleState, 4> SwerveDrive::GetModuleStates() const {
+  std::array<frc::SwerveModuleState, 4> moduleStates;
+  moduleStates[0] = swerveMods[0].GetState();
+  moduleStates[1] = swerveMods[1].GetState();
+  moduleStates[2] = swerveMods[2].GetState();
+  moduleStates[3] = swerveMods[3].GetState();
+  return moduleStates;
+}
+
+std::array<frc::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
+    const {
+  std::array<frc::SwerveModulePosition, 4> modulePositions;
+  modulePositions[0] = swerveMods[0].GetPosition();
+  modulePositions[1] = swerveMods[1].GetPosition();
+  modulePositions[2] = swerveMods[2].GetPosition();
+  modulePositions[3] = swerveMods[3].GetPosition();
+  return modulePositions;
+}
+
+std::array<frc::Pose2d, 4> SwerveDrive::GetModulePoses() const {
+  std::array<frc::Pose2d, 4> modulePoses;
+  for (int i = 0; i < swerveMods.size(); i++) {
+    const SwerveModule& module = swerveMods[i];
+    modulePoses[i] = GetPose().TransformBy(frc::Transform2d{
+        module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()});
+  }
+  return modulePoses;
+}
+
+void SwerveDrive::Log() {
+  std::string table = "Drive/";
+  frc::Pose2d pose = GetPose();
+  frc::SmartDashboard::PutNumber(table + "X", pose.X().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Heading",
+                                 pose.Rotation().Degrees().to<double>());
+  frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds();
+  frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to<double>());
+  frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Omega Degrees",
+      chassisSpeeds.omega.convert<units::degrees_per_second>().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Target VX",
+                                 targetChassisSpeeds.vx.to<double>());
+  frc::SmartDashboard::PutNumber(table + "Target VY",
+                                 targetChassisSpeeds.vy.to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Target Omega Degrees",
+      targetChassisSpeeds.omega.convert<units::degrees_per_second>()
+          .to<double>());
+
+  for (auto& module : swerveMods) {
+    module.Log();
+  }
+}
+
+void SwerveDrive::SimulationPeriodic() {
+  std::array<units::volt_t, 4> driveInputs;
+  std::array<units::volt_t, 4> steerInputs;
+  for (int i = 0; i < swerveMods.size(); i++) {
+    driveInputs[i] = swerveMods[i].GetDriveVoltage();
+    steerInputs[i] = swerveMods[i].GetSteerVoltage();
+  }
+  swerveDriveSim.SetDriveInputs(driveInputs);
+  swerveDriveSim.SetSteerInputs(steerInputs);
+
+  swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod);
+
+  auto driveStates = swerveDriveSim.GetDriveStates();
+  auto steerStates = swerveDriveSim.GetSteerStates();
+  totalCurrentDraw = 0_A;
+  std::array<units::ampere_t, 4> driveCurrents =
+      swerveDriveSim.GetDriveCurrentDraw();
+  for (const auto& current : driveCurrents) {
+    totalCurrentDraw += current;
+  }
+  std::array<units::ampere_t, 4> steerCurrents =
+      swerveDriveSim.GetSteerCurrentDraw();
+  for (const auto& current : steerCurrents) {
+    totalCurrentDraw += current;
+  }
+  for (int i = 0; i < swerveMods.size(); i++) {
+    units::meter_t drivePos{driveStates[i](0, 0)};
+    units::meters_per_second_t driveRate{driveStates[i](1, 0)};
+    units::radian_t steerPos{steerStates[i](0, 0)};
+    units::radians_per_second_t steerRate{steerStates[i](1, 0)};
+    swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i],
+                                   steerPos, steerRate, steerCurrents[i]);
+  }
+  gyroSim.SetRate(-swerveDriveSim.GetOmega());
+  gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees());
+}
+
+frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); }
+
+units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; }

photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDriveSim.cpp

@@ -0,0 +1,287 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "subsystems/SwerveDriveSim.h"
+
+#include <iostream>
+
+#include <frc/RobotController.h>
+#include <frc/system/Discretization.h>
+
+template <typename T>
+int sgn(T val) {
+  return (T(0) < val) - (val < T(0));
+}
+
+SwerveDriveSim::SwerveDriveSim(
+    const frc::SimpleMotorFeedforward<units::meters>& driveFF,
+    const frc::DCMotor& driveMotor, double driveGearing,
+    units::meter_t driveWheelRadius,
+    const frc::SimpleMotorFeedforward<units::radians>& steerFF,
+    const frc::DCMotor& steerMotor, double steerGearing,
+    const frc::SwerveDriveKinematics<numModules>& kinematics)
+    : SwerveDriveSim(
+          frc::LinearSystem<2, 1, 2>{
+              (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
+               -driveFF.GetKv().to<double>() / driveFF.GetKa().to<double>())
+                  .finished(),
+              Eigen::Matrix<double, 2, 1>{0.0,
+                                          1.0 / driveFF.GetKa().to<double>()},
+              (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
+              Eigen::Matrix<double, 2, 1>{0.0, 0.0}},
+          driveFF.GetKs(), driveMotor, driveGearing, driveWheelRadius,
+          frc::LinearSystem<2, 1, 2>{
+              (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
+               -steerFF.GetKv().to<double>() / steerFF.GetKa().to<double>())
+                  .finished(),
+              Eigen::Matrix<double, 2, 1>{0.0,
+                                          1.0 / steerFF.GetKa().to<double>()},
+              (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
+              Eigen::Matrix<double, 2, 1>{0.0, 0.0}},
+          steerFF.GetKs(), steerMotor, steerGearing, kinematics) {}
+
+SwerveDriveSim::SwerveDriveSim(
+    const frc::LinearSystem<2, 1, 2>& drivePlant, units::volt_t driveKs,
+    const frc::DCMotor& driveMotor, double driveGearing,
+    units::meter_t driveWheelRadius,
+    const frc::LinearSystem<2, 1, 2>& steerPlant, units::volt_t steerKs,
+    const frc::DCMotor& steerMotor, double steerGearing,
+    const frc::SwerveDriveKinematics<numModules>& kinematics)
+    : drivePlant(drivePlant),
+      driveKs(driveKs),
+      driveMotor(driveMotor),
+      driveGearing(driveGearing),
+      driveWheelRadius(driveWheelRadius),
+      steerPlant(steerPlant),
+      steerKs(steerKs),
+      steerMotor(steerMotor),
+      steerGearing(steerGearing),
+      kinematics(kinematics) {}
+
+void SwerveDriveSim::SetDriveInputs(
+    const std::array<units::volt_t, numModules>& inputs) {
+  units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage();
+  for (int i = 0; i < driveInputs.size(); i++) {
+    units::volt_t input = inputs[i];
+    driveInputs[i] = std::clamp(input, -battVoltage, battVoltage);
+  }
+}
+
+void SwerveDriveSim::SetSteerInputs(
+    const std::array<units::volt_t, numModules>& inputs) {
+  units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage();
+  for (int i = 0; i < steerInputs.size(); i++) {
+    units::volt_t input = inputs[i];
+    steerInputs[i] = std::clamp(input, -battVoltage, battVoltage);
+  }
+}
+
+Eigen::Matrix<double, 2, 1> SwerveDriveSim::CalculateX(
+    const Eigen::Matrix<double, 2, 2>& discA,
+    const Eigen::Matrix<double, 2, 1>& discB,
+    const Eigen::Matrix<double, 2, 1>& x, units::volt_t input,
+    units::volt_t kS) {
+  auto Ax = discA * x;
+  double nextStateVel = Ax(1, 0);
+  double inputToStop = nextStateVel / -discB(1, 0);
+  double ksSystemEffect =
+      std::clamp(inputToStop, -kS.to<double>(), kS.to<double>());
+
+  nextStateVel += discB(1, 0) * ksSystemEffect;
+  inputToStop = nextStateVel / -discB(1, 0);
+  double signToStop = sgn(inputToStop);
+  double inputSign = sgn(input.to<double>());
+  double ksInputEffect = 0;
+
+  if (std::abs(ksSystemEffect) < kS.to<double>()) {
+    double absInput = std::abs(input.to<double>());
+    ksInputEffect =
+        -std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
+  } else if ((input.to<double>() * signToStop) > (inputToStop * signToStop)) {
+    double absInput = std::abs(input.to<double>() - inputToStop);
+    ksInputEffect =
+        -std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
+  }
+
+  auto sF = Eigen::Matrix<double, 1, 1>{input.to<double>() + ksSystemEffect +
+                                        ksInputEffect};
+  auto Bu = discB * sF;
+  auto retVal = Ax + Bu;
+  return retVal;
+}
+
+void SwerveDriveSim::Update(units::second_t dt) {
+  Eigen::Matrix<double, 2, 2> driveDiscA;
+  Eigen::Matrix<double, 2, 1> driveDiscB;
+  frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA,
+                          &driveDiscB);
+
+  Eigen::Matrix<double, 2, 2> steerDiscA;
+  Eigen::Matrix<double, 2, 1> steerDiscB;
+  frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA,
+                          &steerDiscB);
+
+  std::array<frc::SwerveModulePosition, 4> moduleDeltas;
+
+  for (int i = 0; i < numModules; i++) {
+    double prevDriveStatePos = driveStates[i](0, 0);
+    driveStates[i] = CalculateX(driveDiscA, driveDiscB, driveStates[i],
+                                driveInputs[i], driveKs);
+    double currentDriveStatePos = driveStates[i](0, 0);
+    steerStates[i] = CalculateX(steerDiscA, steerDiscB, steerStates[i],
+                                steerInputs[i], steerKs);
+    double currentSteerStatePos = steerStates[i](0, 0);
+    moduleDeltas[i] = frc::SwerveModulePosition{
+        units::meter_t{currentDriveStatePos - prevDriveStatePos},
+        frc::Rotation2d{units::radian_t{currentSteerStatePos}}};
+  }
+
+  frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas);
+  pose = pose.Exp(twist);
+  omega = twist.dtheta / dt;
+}
+
+void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) {
+  this->pose = pose;
+  if (!preserveMotion) {
+    for (int i = 0; i < numModules; i++) {
+      driveStates[i] = Eigen::Matrix<double, 2, 1>{0, 0};
+      steerStates[i] = Eigen::Matrix<double, 2, 1>{0, 0};
+    }
+    omega = 0_rad_per_s;
+  }
+}
+
+void SwerveDriveSim::Reset(const frc::Pose2d& pose,
+                           const std::array<Eigen::Matrix<double, 2, 1>,
+                                            numModules>& moduleDriveStates,
+                           const std::array<Eigen::Matrix<double, 2, 1>,
+                                            numModules>& moduleSteerStates) {
+  this->pose = pose;
+  driveStates = moduleDriveStates;
+  steerStates = moduleSteerStates;
+  omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega;
+}
+
+frc::Pose2d SwerveDriveSim::GetPose() const { return pose; }
+
+std::array<frc::SwerveModulePosition, numModules>
+SwerveDriveSim::GetModulePositions() const {
+  std::array<frc::SwerveModulePosition, numModules> positions;
+  for (int i = 0; i < numModules; i++) {
+    positions[i] = frc::SwerveModulePosition{
+        units::meter_t{driveStates[i](0, 0)},
+        frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}};
+  }
+  return positions;
+}
+
+std::array<frc::SwerveModulePosition, numModules>
+SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev,
+                                        units::radian_t steerStdDev) {
+  std::array<frc::SwerveModulePosition, numModules> positions;
+  for (int i = 0; i < numModules; i++) {
+    positions[i] = frc::SwerveModulePosition{
+        units::meter_t{driveStates[i](0, 0)} +
+            randDist(generator) * driveStdDev,
+        frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} +
+                        randDist(generator) * steerStdDev}};
+  }
+  return positions;
+}
+
+std::array<frc::SwerveModuleState, numModules>
+SwerveDriveSim::GetModuleStates() {
+  std::array<frc::SwerveModuleState, numModules> states;
+  for (int i = 0; i < numModules; i++) {
+    states[i] = frc::SwerveModuleState{
+        units::meters_per_second_t{driveStates[i](1, 0)},
+        frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}};
+  }
+  return states;
+}
+
+std::array<Eigen::Matrix<double, 2, 1>, numModules>
+SwerveDriveSim::GetDriveStates() const {
+  return driveStates;
+}
+
+std::array<Eigen::Matrix<double, 2, 1>, numModules>
+SwerveDriveSim::GetSteerStates() const {
+  return steerStates;
+}
+
+units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; }
+
+units::ampere_t SwerveDriveSim::GetCurrentDraw(
+    const frc::DCMotor& motor, units::radians_per_second_t velocity,
+    units::volt_t inputVolts, units::volt_t batteryVolts) const {
+  units::volt_t effVolts = inputVolts - velocity / motor.Kv;
+  if (inputVolts >= 0_V) {
+    effVolts = std::clamp(effVolts, 0_V, inputVolts);
+  } else {
+    effVolts = std::clamp(effVolts, inputVolts, 0_V);
+  }
+  auto retVal = (inputVolts / batteryVolts) * (effVolts / motor.R);
+  return retVal;
+}
+
+std::array<units::ampere_t, numModules> SwerveDriveSim::GetDriveCurrentDraw()
+    const {
+  std::array<units::ampere_t, numModules> currents;
+  for (int i = 0; i < numModules; i++) {
+    units::radians_per_second_t speed =
+        units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing /
+        driveWheelRadius.to<double>();
+    currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i],
+                                 frc::RobotController::GetBatteryVoltage());
+  }
+  return currents;
+}
+
+std::array<units::ampere_t, numModules> SwerveDriveSim::GetSteerCurrentDraw()
+    const {
+  std::array<units::ampere_t, numModules> currents;
+  for (int i = 0; i < numModules; i++) {
+    units::radians_per_second_t speed =
+        units::radians_per_second_t{steerStates[i](1, 0) * steerGearing};
+    // TODO: If uncommented we get huge current values.. Not sure how to fix
+    // atm. :(
+    currents[i] = 20_A;
+    // currents[i] = GetCurrentDraw(steerMotor, speed, steerInputs[i],
+    // frc::RobotController::GetBatteryVoltage());
+  }
+  return currents;
+}
+
+units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const {
+  units::ampere_t total{0};
+  for (const auto& val : GetDriveCurrentDraw()) {
+    total += val;
+  }
+  for (const auto& val : GetSteerCurrentDraw()) {
+    total += val;
+  }
+  return total;
+}

photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveModule.cpp

@@ -0,0 +1,146 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "subsystems/SwerveModule.h"
+
+#include <iostream>
+#include <string>
+
+#include <frc/MathUtil.h>
+#include <frc/RobotController.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts)
+    : moduleConstants(consts),
+      driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}),
+      driveEncoder(frc::Encoder{moduleConstants.driveEncoderA,
+                                moduleConstants.driveEncoderB}),
+      steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}),
+      steerEncoder(frc::Encoder{moduleConstants.steerEncoderA,
+                                moduleConstants.steerEncoderB}),
+      driveEncoderSim(driveEncoder),
+      steerEncoderSim(steerEncoder) {
+  driveEncoder.SetDistancePerPulse(
+      constants::Swerve::kDriveDistPerPulse.to<double>());
+  steerEncoder.SetDistancePerPulse(
+      constants::Swerve::kSteerRadPerPulse.to<double>());
+  steerPIDController.EnableContinuousInput(-std::numbers::pi, std::numbers::pi);
+}
+
+void SwerveModule::Periodic() {
+  units::volt_t steerPID = units::volt_t{
+      steerPIDController.Calculate(GetAbsoluteHeading().Radians().to<double>(),
+                                   desiredState.angle.Radians().to<double>())};
+  steerMotor.SetVoltage(steerPID);
+
+  units::volt_t driveFF =
+      constants::Swerve::kDriveFF.Calculate(desiredState.speed);
+  units::volt_t drivePID{0};
+  if (!openLoop) {
+    drivePID = units::volt_t{drivePIDController.Calculate(
+        driveEncoder.GetRate(), desiredState.speed.to<double>())};
+  }
+  driveMotor.SetVoltage(driveFF + drivePID);
+}
+
+void SwerveModule::SetDesiredState(frc::SwerveModuleState newState,
+                                   bool shouldBeOpenLoop, bool steerInPlace) {
+  frc::Rotation2d currentRotation = GetAbsoluteHeading();
+  newState.Optimize(currentRotation);
+  desiredState = newState;
+}
+
+frc::Rotation2d SwerveModule::GetAbsoluteHeading() const {
+  return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}};
+}
+
+frc::SwerveModuleState SwerveModule::GetState() const {
+  return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps,
+                                GetAbsoluteHeading()};
+}
+
+frc::SwerveModulePosition SwerveModule::GetPosition() const {
+  return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m,
+                                   GetAbsoluteHeading()};
+}
+
+units::volt_t SwerveModule::GetDriveVoltage() const {
+  return driveMotor.Get() * frc::RobotController::GetBatteryVoltage();
+}
+
+units::volt_t SwerveModule::GetSteerVoltage() const {
+  return steerMotor.Get() * frc::RobotController::GetBatteryVoltage();
+}
+
+units::ampere_t SwerveModule::GetDriveCurrentSim() const {
+  return driveCurrentSim;
+}
+
+units::ampere_t SwerveModule::GetSteerCurrentSim() const {
+  return steerCurrentSim;
+}
+
+constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const {
+  return moduleConstants;
+}
+
+void SwerveModule::Log() {
+  frc::SwerveModuleState state = GetState();
+
+  std::string table =
+      "Module " + std::to_string(moduleConstants.moduleNum) + "/";
+  frc::SmartDashboard::PutNumber(table + "Steer Degrees",
+                                 frc::AngleModulus(state.angle.Radians())
+                                     .convert<units::degrees>()
+                                     .to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Steer Target Degrees",
+      units::radian_t{steerPIDController.GetSetpoint()}
+          .convert<units::degrees>()
+          .to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Drive Velocity Feet",
+      state.speed.convert<units::feet_per_second>().to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Drive Velocity Target Feet",
+      desiredState.speed.convert<units::feet_per_second>().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Drive Current",
+                                 driveCurrentSim.to<double>());
+  frc::SmartDashboard::PutNumber(table + "Steer Current",
+                                 steerCurrentSim.to<double>());
+}
+
+void SwerveModule::SimulationUpdate(
+    units::meter_t driveEncoderDist,
+    units::meters_per_second_t driveEncoderRate, units::ampere_t driveCurrent,
+    units::radian_t steerEncoderDist,
+    units::radians_per_second_t steerEncoderRate,
+    units::ampere_t steerCurrent) {
+  driveEncoderSim.SetDistance(driveEncoderDist.to<double>());
+  driveEncoderSim.SetRate(driveEncoderRate.to<double>());
+  driveCurrentSim = driveCurrent;
+  steerEncoderSim.SetDistance(steerEncoderDist.to<double>());
+  steerEncoderSim.SetRate(steerEncoderRate.to<double>());
+  steerCurrentSim = steerCurrent;
+}

photonlib-cpp-examples/aimandrange/src/main/deploy/example.txt

@@ -0,0 +1,4 @@
+Files placed in this directory will be deployed to the RoboRIO into the
+  'deploy' directory in the home folder. Use the 'frc::filesystem::GetDeployDirectory'
+  function from the 'frc/Filesystem.h' header to get a proper path relative to the deploy
+  directory.

photonlib-cpp-examples/aimandrange/src/main/include/Constants.h

@@ -0,0 +1,125 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <numbers>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/apriltag/AprilTagFields.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/geometry/Transform3d.h>
+#include <frc/geometry/Translation2d.h>
+#include <units/acceleration.h>
+#include <units/angular_acceleration.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <units/velocity.h>
+
+namespace constants {
+namespace Vision {
+inline constexpr std::string_view kCameraName{"YOUR CAMERA NAME"};
+inline const frc::Transform3d kRobotToCam{
+    frc::Translation3d{0.5_m, 0.0_m, 0.5_m},
+    frc::Rotation3d{0_rad, -30_deg, 0_rad}};
+inline const frc::AprilTagFieldLayout kTagLayout{
+    frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo)};
+
+inline const Eigen::Matrix<double, 3, 1> kSingleTagStdDevs{4, 4, 8};
+inline const Eigen::Matrix<double, 3, 1> kMultiTagStdDevs{0.5, 0.5, 1};
+}  // namespace Vision
+namespace Swerve {
+using namespace units;
+
+inline constexpr units::meter_t kTrackWidth{18.5_in};
+inline constexpr units::meter_t kTrackLength{18.5_in};
+inline constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2};
+inline constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2};
+inline constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps};
+inline constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s};
+inline constexpr units::meter_t kWheelDiameter{4_in};
+inline constexpr units::meter_t kWheelCircumference{kWheelDiameter *
+                                                    std::numbers::pi};
+
+inline constexpr double kDriveGearRatio = 6.75;
+inline constexpr double kSteerGearRatio = 12.8;
+
+inline constexpr units::meter_t kDriveDistPerPulse =
+    kWheelCircumference / 1024.0 / kDriveGearRatio;
+inline constexpr units::radian_t kSteerRadPerPulse =
+    units::radian_t{2 * std::numbers::pi} / 1024.0;
+
+inline constexpr double kDriveKP = 1.0;
+inline constexpr double kDriveKI = 0.0;
+inline constexpr double kDriveKD = 0.0;
+
+inline constexpr double kSteerKP = 20.0;
+inline constexpr double kSteerKI = 0.0;
+inline constexpr double kSteerKD = 0.25;
+
+using namespace units;
+
+inline const frc::SimpleMotorFeedforward<units::meters> kDriveFF{
+    0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq};
+
+inline const frc::SimpleMotorFeedforward<units::radians> kSteerFF{
+    0.5_V, 0.25_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq};
+
+struct ModuleConstants {
+ public:
+  const int moduleNum;
+  const int driveMotorId;
+  const int driveEncoderA;
+  const int driveEncoderB;
+  const int steerMotorId;
+  const int steerEncoderA;
+  const int steerEncoderB;
+  const double angleOffset;
+  const frc::Translation2d centerOffset;
+
+  ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA,
+                  int driveEncoderB, int steerMotorId, int steerEncoderA,
+                  int steerEncoderB, double angleOffset, units::meter_t xOffset,
+                  units::meter_t yOffset)
+      : moduleNum(moduleNum),
+        driveMotorId(driveMotorId),
+        driveEncoderA(driveEncoderA),
+        driveEncoderB(driveEncoderB),
+        steerMotorId(steerMotorId),
+        steerEncoderA(steerEncoderA),
+        steerEncoderB(steerEncoderB),
+        angleOffset(angleOffset),
+        centerOffset(frc::Translation2d{xOffset, yOffset}) {}
+};
+
+inline const ModuleConstants FL_CONSTANTS{
+    1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2};
+inline const ModuleConstants FR_CONSTANTS{
+    2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2};
+inline const ModuleConstants BL_CONSTANTS{
+    3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2};
+inline const ModuleConstants BR_CONSTANTS{
+    4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2};
+}  // namespace Swerve
+}  // namespace constants

photonlib-cpp-examples/aimandrange/src/main/include/Robot.h

@@ -0,0 +1,63 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <photon/PhotonCamera.h>
+
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+
+#include "Constants.h"
+#include "VisionSim.h"
+#include "subsystems/SwerveDrive.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override;
+  void RobotPeriodic() override;
+  void DisabledInit() override;
+  void DisabledPeriodic() override;
+  void DisabledExit() override;
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+  void AutonomousExit() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void TeleopExit() override;
+  void TestInit() override;
+  void TestPeriodic() override;
+  void TestExit() override;
+  void SimulationPeriodic() override;
+
+ private:
+  photon::PhotonCamera camera{constants::Vision::kCameraName};
+  SwerveDrive drivetrain{};
+  VisionSim vision{&camera};
+  frc::XboxController controller{0};
+  static constexpr auto VISION_TURN_kP = 0.01;
+  static constexpr auto VISION_DES_ANGLE = 0.0_deg;
+  static constexpr auto VISION_STRAFE_kP = 0.5;
+  static constexpr auto VISION_DES_RANGE = 1.25_m;
+};

photonlib-cpp-examples/aimandrange/src/main/include/VisionSim.h

@@ -0,0 +1,87 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <photon/PhotonCamera.h>
+#include <photon/PhotonPoseEstimator.h>
+#include <photon/estimation/VisionEstimation.h>
+#include <photon/simulation/VisionSystemSim.h>
+#include <photon/simulation/VisionTargetSim.h>
+#include <photon/targeting/PhotonPipelineResult.h>
+
+#include <limits>
+#include <memory>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/apriltag/AprilTagFields.h>
+
+#include "Constants.h"
+
+class VisionSim {
+ public:
+  explicit VisionSim(photon::PhotonCamera* camera) {
+    if (frc::RobotBase::IsSimulation()) {
+      visionSim = std::make_unique<photon::VisionSystemSim>("main");
+
+      visionSim->AddAprilTags(constants::Vision::kTagLayout);
+
+      cameraProp = std::make_unique<photon::SimCameraProperties>();
+
+      cameraProp->SetCalibration(320, 240, frc::Rotation2d{90_deg});
+      cameraProp->SetCalibError(.35, .10);
+      cameraProp->SetFPS(70_Hz);
+      cameraProp->SetAvgLatency(30_ms);
+      cameraProp->SetLatencyStdDev(15_ms);
+
+      cameraSim =
+          std::make_shared<photon::PhotonCameraSim>(camera, *cameraProp.get());
+
+      visionSim->AddCamera(cameraSim.get(), constants::Vision::kRobotToCam);
+      cameraSim->EnableDrawWireframe(true);
+    }
+  }
+
+  photon::PhotonPipelineResult GetLatestResult() { return m_latestResult; }
+
+  void SimPeriodic(frc::Pose2d robotSimPose) {
+    visionSim->Update(robotSimPose);
+  }
+
+  void ResetSimPose(frc::Pose2d pose) {
+    if (frc::RobotBase::IsSimulation()) {
+      visionSim->ResetRobotPose(pose);
+    }
+  }
+
+  frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); }
+
+ private:
+  std::unique_ptr<photon::VisionSystemSim> visionSim;
+  std::unique_ptr<photon::SimCameraProperties> cameraProp;
+  std::shared_ptr<photon::PhotonCameraSim> cameraSim;
+
+  // The most recent result, cached for calculating std devs
+  photon::PhotonPipelineResult m_latestResult;
+};

photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h

@@ -0,0 +1,80 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <frc/ADXRS450_Gyro.h>
+#include <frc/SPI.h>
+#include <frc/estimator/SwerveDrivePoseEstimator.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/simulation/ADXRS450_GyroSim.h>
+
+#include "SwerveDriveSim.h"
+#include "SwerveModule.h"
+
+class SwerveDrive {
+ public:
+  SwerveDrive();
+  void Periodic();
+  void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy,
+             units::radians_per_second_t omega);
+  void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds,
+                        bool openLoop, bool steerInPlace);
+  void SetModuleStates(
+      const std::array<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
+      bool steerInPlace);
+  void Stop();
+  void ResetPose(const frc::Pose2d& pose, bool resetSimPose);
+  frc::Pose2d GetPose() const;
+  frc::Rotation2d GetHeading() const;
+  frc::Rotation2d GetGyroYaw() const;
+  frc::ChassisSpeeds GetChassisSpeeds() const;
+  std::array<frc::SwerveModuleState, 4> GetModuleStates() const;
+  std::array<frc::SwerveModulePosition, 4> GetModulePositions() const;
+  std::array<frc::Pose2d, 4> GetModulePoses() const;
+  void Log();
+  void SimulationPeriodic();
+  frc::Pose2d GetSimPose() const;
+  units::ampere_t GetCurrentDraw() const;
+
+ private:
+  std::array<SwerveModule, 4> swerveMods{
+      SwerveModule{constants::Swerve::FL_CONSTANTS},
+      SwerveModule{constants::Swerve::FR_CONSTANTS},
+      SwerveModule{constants::Swerve::BL_CONSTANTS},
+      SwerveModule{constants::Swerve::BR_CONSTANTS}};
+  frc::SwerveDriveKinematics<4> kinematics{
+      swerveMods[0].GetModuleConstants().centerOffset,
+      swerveMods[1].GetModuleConstants().centerOffset,
+      swerveMods[2].GetModuleConstants().centerOffset,
+      swerveMods[3].GetModuleConstants().centerOffset,
+  };
+  frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0};
+  frc::SwerveDrivePoseEstimator<4> poseEstimator;
+  frc::ChassisSpeeds targetChassisSpeeds{};
+
+  frc::sim::ADXRS450_GyroSim gyroSim;
+  SwerveDriveSim swerveDriveSim;
+  units::ampere_t totalCurrentDraw{0};
+};

photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDriveSim.h

@@ -0,0 +1,102 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <random>
+
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/system/LinearSystem.h>
+#include <frc/system/plant/DCMotor.h>
+#include <units/voltage.h>
+
+static constexpr int numModules{4};
+
+class SwerveDriveSim {
+ public:
+  SwerveDriveSim(const frc::SimpleMotorFeedforward<units::meters>& driveFF,
+                 const frc::DCMotor& driveMotor, double driveGearing,
+                 units::meter_t driveWheelRadius,
+                 const frc::SimpleMotorFeedforward<units::radians>& steerFF,
+                 const frc::DCMotor& steerMotor, double steerGearing,
+                 const frc::SwerveDriveKinematics<numModules>& kinematics);
+  SwerveDriveSim(const frc::LinearSystem<2, 1, 2>& drivePlant,
+                 units::volt_t driveKs, const frc::DCMotor& driveMotor,
+                 double driveGearing, units::meter_t driveWheelRadius,
+                 const frc::LinearSystem<2, 1, 2>& steerPlant,
+                 units::volt_t steerKs, const frc::DCMotor& steerMotor,
+                 double steerGearing,
+                 const frc::SwerveDriveKinematics<numModules>& kinematics);
+  void SetDriveInputs(const std::array<units::volt_t, numModules>& inputs);
+  void SetSteerInputs(const std::array<units::volt_t, numModules>& inputs);
+  static Eigen::Matrix<double, 2, 1> CalculateX(
+      const Eigen::Matrix<double, 2, 2>& discA,
+      const Eigen::Matrix<double, 2, 1>& discB,
+      const Eigen::Matrix<double, 2, 1>& x, units::volt_t input,
+      units::volt_t kS);
+  void Update(units::second_t dt);
+  void Reset(const frc::Pose2d& pose, bool preserveMotion);
+  void Reset(const frc::Pose2d& pose,
+             const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
+                 moduleDriveStates,
+             const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
+                 moduleSteerStates);
+  frc::Pose2d GetPose() const;
+  std::array<frc::SwerveModulePosition, numModules> GetModulePositions() const;
+  std::array<frc::SwerveModulePosition, numModules> GetNoisyModulePositions(
+      units::meter_t driveStdDev, units::radian_t steerStdDev);
+  std::array<frc::SwerveModuleState, numModules> GetModuleStates();
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> GetDriveStates() const;
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> GetSteerStates() const;
+  units::radians_per_second_t GetOmega() const;
+  units::ampere_t GetCurrentDraw(const frc::DCMotor& motor,
+                                 units::radians_per_second_t velocity,
+                                 units::volt_t inputVolts,
+                                 units::volt_t batteryVolts) const;
+  std::array<units::ampere_t, numModules> GetDriveCurrentDraw() const;
+  std::array<units::ampere_t, numModules> GetSteerCurrentDraw() const;
+  units::ampere_t GetTotalCurrentDraw() const;
+
+ private:
+  std::random_device rd{};
+  std::mt19937 generator{rd()};
+  std::normal_distribution<double> randDist{0.0, 1.0};
+  const frc::LinearSystem<2, 1, 2> drivePlant;
+  const units::volt_t driveKs;
+  const frc::DCMotor driveMotor;
+  const double driveGearing;
+  const units::meter_t driveWheelRadius;
+  const frc::LinearSystem<2, 1, 2> steerPlant;
+  const units::volt_t steerKs;
+  const frc::DCMotor steerMotor;
+  const double steerGearing;
+  const frc::SwerveDriveKinematics<numModules> kinematics;
+  std::array<units::volt_t, numModules> driveInputs{};
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> driveStates{};
+  std::array<units::volt_t, numModules> steerInputs{};
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> steerStates{};
+  frc::Pose2d pose{frc::Pose2d{}};
+  units::radians_per_second_t omega{0};
+};

photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveModule.h

@@ -0,0 +1,81 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/controller/PIDController.h>
+#include <frc/kinematics/SwerveModulePosition.h>
+#include <frc/kinematics/SwerveModuleState.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc/simulation/EncoderSim.h>
+#include <units/current.h>
+
+#include "Constants.h"
+
+class SwerveModule {
+ public:
+  explicit SwerveModule(const constants::Swerve::ModuleConstants& consts);
+  void Periodic();
+  void SetDesiredState(frc::SwerveModuleState newState, bool shouldBeOpenLoop,
+                       bool steerInPlace);
+  frc::Rotation2d GetAbsoluteHeading() const;
+  frc::SwerveModuleState GetState() const;
+  frc::SwerveModulePosition GetPosition() const;
+  units::volt_t GetDriveVoltage() const;
+  units::volt_t GetSteerVoltage() const;
+  units::ampere_t GetDriveCurrentSim() const;
+  units::ampere_t GetSteerCurrentSim() const;
+  constants::Swerve::ModuleConstants GetModuleConstants() const;
+  void Log();
+  void SimulationUpdate(units::meter_t driveEncoderDist,
+                        units::meters_per_second_t driveEncoderRate,
+                        units::ampere_t driveCurrent,
+                        units::radian_t steerEncoderDist,
+                        units::radians_per_second_t steerEncoderRate,
+                        units::ampere_t steerCurrent);
+
+ private:
+  const constants::Swerve::ModuleConstants moduleConstants;
+
+  frc::PWMSparkMax driveMotor;
+  frc::Encoder driveEncoder;
+  frc::PWMSparkMax steerMotor;
+  frc::Encoder steerEncoder;
+
+  frc::SwerveModuleState desiredState{};
+  bool openLoop{false};
+
+  frc::PIDController drivePIDController{constants::Swerve::kDriveKP,
+                                        constants::Swerve::kDriveKI,
+                                        constants::Swerve::kDriveKD};
+  frc::PIDController steerPIDController{constants::Swerve::kSteerKP,
+                                        constants::Swerve::kSteerKI,
+                                        constants::Swerve::kSteerKD};
+
+  frc::sim::EncoderSim driveEncoderSim;
+  units::ampere_t driveCurrentSim{0};
+  frc::sim::EncoderSim steerEncoderSim;
+  units::ampere_t steerCurrentSim{0};
+};

photonlib-cpp-examples/aimandrange/src/test/cpp/main.cpp

@@ -0,0 +1,34 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <hal/HAL.h>
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}

photonlib-cpp-examples/aimattarget/.gitignore

@@ -0,0 +1 @@
+vendordeps

photonlib-cpp-examples/aimattarget/.wpilib/wpilib_preferences.json

@@ -0,0 +1,6 @@
+{
+    "enableCppIntellisense": true,
+    "currentLanguage": "cpp",
+    "projectYear": "2025",
+    "teamNumber": 5
+}

photonlib-cpp-examples/aimattarget/WPILib-License.md

@@ -0,0 +1,24 @@
+Copyright (c) 2009-2021 FIRST and other WPILib contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+   * Neither the name of FIRST, WPILib, nor the names of other WPILib
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

photonlib-cpp-examples/aimattarget/build.gradle

@@ -0,0 +1,117 @@
+plugins {
+    id "cpp"
+    id "google-test-test-suite"
+    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2"
+}
+
+repositories {
+    mavenLocal()
+    jcenter()
+}
+
+wpi.maven.useLocal = false
+wpi.maven.useDevelopment = false
+wpi.versions.wpilibVersion = "2025.1.1-beta-2"
+wpi.versions.wpimathVersion = "2025.1.1-beta-2"
+
+// 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.getTeamOrDefault(5940)
+            debug = project.frc.getDebugOrDefault(false)
+
+            artifacts {
+                // First part is artifact name, 2nd is artifact type
+                // getTargetTypeClass is a shortcut to get the class type using a string
+
+                frcCpp(getArtifactTypeClass('FRCNativeArtifact')) {
+                }
+
+                // Static files artifact
+                frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+                    files = project.fileTree('src/main/deploy')
+                    directory = '/home/lvuser/deploy'
+                }
+            }
+        }
+    }
+}
+
+def deployArtifact = deploy.targets.roborio.artifacts.frcCpp
+
+// Set this to true to enable desktop support.
+def includeDesktopSupport = true
+
+// Set to true to run simulation in debug mode
+wpi.cpp.debugSimulation = false
+
+// Default enable simgui
+wpi.sim.addGui().defaultEnabled = true
+// Enable DS but not by default
+wpi.sim.addDriverstation()
+
+model {
+    components {
+        frcUserProgram(NativeExecutableSpec) {
+            // We don't need to build for roborio -- if we do, we need to install
+            // a roborio toolchain every time we build in CI
+            // Ideally, we'd be able to set the roborio toolchain as optional, but
+            // I can't figure out how to set that environment variable from build.gradle
+            // (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71)
+            // for now, commented out
+
+            // targetPlatform wpi.platforms.roborio
+
+            if (includeDesktopSupport) {
+                targetPlatform wpi.platforms.desktop
+            }
+
+            sources.cpp {
+                source {
+                    srcDir 'src/main/cpp'
+                    include '**/*.cpp', '**/*.cc'
+                }
+                exportedHeaders {
+                    srcDir 'src/main/include'
+                }
+            }
+
+            // Set deploy task to deploy this component
+            deployArtifact.component = it
+
+            // Enable run tasks for this component
+            wpi.cpp.enableExternalTasks(it)
+
+            // Enable simulation for this component
+            wpi.sim.enable(it)
+            // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+            wpi.cpp.vendor.cpp(it)
+            wpi.cpp.deps.wpilib(it)
+        }
+    }
+    testSuites {
+        frcUserProgramTest(GoogleTestTestSuiteSpec) {
+            testing $.components.frcUserProgram
+
+            sources.cpp {
+                source {
+                    srcDir 'src/test/cpp'
+                    include '**/*.cpp'
+                }
+            }
+
+            // Enable run tasks for this component
+            wpi.cpp.enableExternalTasks(it)
+
+            wpi.cpp.vendor.cpp(it)
+            wpi.cpp.deps.wpilib(it)
+            wpi.cpp.deps.googleTest(it)
+        }
+    }
+}

photonlib-cpp-examples/aimattarget/gradlew

@@ -0,0 +1,241 @@
+#!/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/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+#       within the Gradle project.
+#
+#       You can find Gradle at https://github.com/gradle/gradle/.
+#
+##############################################################################
+
+# Attempt to set APP_HOME
+
+# Resolve links: $0 may be a link
+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
+
+# 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
+
+APP_NAME="Gradle"
+APP_BASE_NAME=${0##*/}
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD=maximum
+
+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
+    which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+fi
+
+# Increase the maximum file descriptors if we can.
+if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
+    case $MAX_FD in #(
+      max*)
+        MAX_FD=$( ulimit -H -n ) ||
+            warn "Could not query maximum file descriptor limit"
+    esac
+    case $MAX_FD in  #(
+      '' | soft) :;; #(
+      *)
+        ulimit -n "$MAX_FD" ||
+            warn "Could not set maximum file descriptor limit to $MAX_FD"
+    esac
+fi
+
+# 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
+
+# Collect all arguments for the java command;
+#   * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of
+#     shell script including quotes and variable substitutions, so put them in
+#     double quotes to make sure that they get re-expanded; and
+#   * put everything else in single quotes, so that it's not re-expanded.
+
+set -- \
+        "-Dorg.gradle.appname=$APP_BASE_NAME" \
+        -classpath "$CLASSPATH" \
+        org.gradle.wrapper.GradleWrapperMain \
+        "$@"
+
+# Stop when "xargs" is not available.
+if ! command -v xargs >/dev/null 2>&1
+then
+    die "xargs is not available"
+fi
+
+# Use "xargs" to parse quoted args.
+#
+# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
+#
+# In Bash we could simply go:
+#
+#   readarray ARGS < <( xargs -n1 <<<"$var" ) &&
+#   set -- "${ARGS[@]}" "$@"
+#
+# but POSIX shell has neither arrays nor command substitution, so instead we
+# post-process each arg (as a line of input to sed) to backslash-escape any
+# character that might be a shell metacharacter, then use eval to reverse
+# that process (while maintaining the separation between arguments), and wrap
+# the whole thing up as a single "set" statement.
+#
+# This will of course break if any of these variables contains a newline or
+# an unmatched quote.
+#
+
+eval "set -- $(
+        printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
+        xargs -n1 |
+        sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
+        tr '\n' ' '
+    )" '"$@"'
+
+exec "$JAVACMD" "$@"

photonlib-cpp-examples/aimattarget/gradlew.bat

@@ -0,0 +1,91 @@
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem      https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+
+@if "%DEBUG%"=="" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%"=="" set DIRNAME=.
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem 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

photonlib-cpp-examples/aimattarget/networktables.json

@@ -0,0 +1 @@
+[]

photonlib-cpp-examples/aimattarget/settings.gradle

@@ -0,0 +1,30 @@
+import org.gradle.internal.os.OperatingSystem
+
+rootProject.name = 'aimattarget'
+
+pluginManagement {
+    repositories {
+        mavenLocal()
+        jcenter()
+        gradlePluginPortal()
+        String frcYear = '2025'
+        File frcHome
+        if (OperatingSystem.current().isWindows()) {
+            String publicFolder = System.getenv('PUBLIC')
+            if (publicFolder == null) {
+                publicFolder = "C:\\Users\\Public"
+            }
+            def homeRoot = new File(publicFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        } else {
+            def userFolder = System.getProperty("user.home")
+            def homeRoot = new File(userFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        }
+        def frcHomeMaven = new File(frcHome, 'maven')
+        maven {
+            name 'frcHome'
+            url frcHomeMaven
+        }
+    }
+}

photonlib-cpp-examples/aimattarget/simgui-ds.json

@@ -0,0 +1,100 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decayRate": 0.0,
+          "keyRate": 0.009999999776482582
+        },
+        {},
+        {
+          "decKey": 81,
+          "incKey": 69
+        }
+      ],
+      "axisCount": 6,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
+  ]
+}

photonlib-cpp-examples/aimattarget/simgui-window.json

@@ -0,0 +1,74 @@
+{
+  "Docking": {
+    "Data": []
+  },
+  "MainWindow": {
+    "GLOBAL": {
+      "fps": "120",
+      "height": "910",
+      "maximized": "0",
+      "style": "0",
+      "userScale": "2",
+      "width": "1550",
+      "xpos": "-1602",
+      "ypos": "79"
+    }
+  },
+  "Window": {
+    "###/SmartDashboard/VisionSystemSim-main/Sim Field": {
+      "Collapsed": "0",
+      "Pos": "554,10",
+      "Size": "991,527"
+    },
+    "###FMS": {
+      "Collapsed": "0",
+      "Pos": "12,604",
+      "Size": "336,158"
+    },
+    "###Joysticks": {
+      "Collapsed": "0",
+      "Pos": "278,685",
+      "Size": "976,219"
+    },
+    "###NetworkTables": {
+      "Collapsed": "0",
+      "Pos": "353,479",
+      "Size": "830,620"
+    },
+    "###NetworkTables Info": {
+      "Collapsed": "0",
+      "Pos": "315,527",
+      "Size": "750,145"
+    },
+    "###Other Devices": {
+      "Collapsed": "0",
+      "Pos": "1025,20",
+      "Size": "250,695"
+    },
+    "###Plot <0>": {
+      "Collapsed": "0",
+      "Pos": "113,22",
+      "Size": "448,400"
+    },
+    "###System Joysticks": {
+      "Collapsed": "0",
+      "Pos": "5,350",
+      "Size": "232,254"
+    },
+    "###Timing": {
+      "Collapsed": "0",
+      "Pos": "5,150",
+      "Size": "162,142"
+    },
+    "Debug##Default": {
+      "Collapsed": "0",
+      "Pos": "60,60",
+      "Size": "400,400"
+    },
+    "Robot State": {
+      "Collapsed": "0",
+      "Pos": "5,20",
+      "Size": "109,134"
+    }
+  }
+}

photonlib-cpp-examples/aimattarget/simgui.json

@@ -0,0 +1,164 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d"
+    },
+    "windows": {
+      "/SmartDashboard/VisionSystemSim-main/Sim Field": {
+        "EstimatedRobot": {
+          "arrowWeight": 3.0,
+          "length": 0.800000011920929,
+          "selectable": false,
+          "weight": 3.0,
+          "width": 0.800000011920929
+        },
+        "EstimatedRobotModules": {
+          "arrows": false,
+          "image": "swerve_module.png",
+          "length": 0.30000001192092896,
+          "selectable": false,
+          "width": 0.30000001192092896
+        },
+        "Robot": {
+          "arrowColor": [
+            1.0,
+            1.0,
+            1.0,
+            255.0
+          ],
+          "arrowWeight": 2.0,
+          "color": [
+            1.0,
+            1.0,
+            1.0,
+            255.0
+          ],
+          "length": 0.800000011920929,
+          "selectable": false,
+          "weight": 2.0,
+          "width": 0.800000011920929
+        },
+        "VisionEstimation": {
+          "arrowColor": [
+            0.0,
+            0.6075949668884277,
+            1.0,
+            255.0
+          ],
+          "arrowWeight": 2.0,
+          "color": [
+            0.0,
+            0.6075949668884277,
+            1.0,
+            255.0
+          ],
+          "selectable": false,
+          "weight": 2.0
+        },
+        "apriltag": {
+          "arrows": false,
+          "image": "tag-green.png",
+          "length": 0.6000000238418579,
+          "width": 0.5
+        },
+        "bottom": 1476,
+        "cameras": {
+          "arrowColor": [
+            0.29535865783691406,
+            1.0,
+            0.9910804033279419,
+            255.0
+          ],
+          "arrowSize": 19,
+          "arrowWeight": 3.0,
+          "length": 1.0,
+          "style": "Hidden",
+          "weight": 1.0,
+          "width": 1.0
+        },
+        "height": 8.210550308227539,
+        "image": "2023-field.png",
+        "left": 150,
+        "right": 2961,
+        "top": 79,
+        "visibleTargetPoses": {
+          "arrows": false,
+          "image": "tag-blue.png",
+          "length": 0.5,
+          "selectable": false,
+          "width": 0.4000000059604645
+        },
+        "width": 16.541748046875,
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "CameraPublisher": {
+        "YOUR CAMERA NAME-processed": {
+          "open": true,
+          "string[]##v_/CameraPublisher/YOUR CAMERA NAME-processed/streams": {
+            "open": true
+          }
+        },
+        "open": true
+      },
+      "SmartDashboard": {
+        "VisionSystemSim-main": {
+          "open": true
+        },
+        "open": true
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  },
+  "Plot": {
+    "Plot <0>": {
+      "plots": [
+        {
+          "axis": [
+            {
+              "autoFit": true
+            }
+          ],
+          "backgroundColor": [
+            0.0,
+            0.0,
+            0.0,
+            0.8500000238418579
+          ],
+          "height": 332,
+          "series": [
+            {
+              "color": [
+                0.2980392277240753,
+                0.44705885648727417,
+                0.6901960968971252,
+                1.0
+              ],
+              "id": "NT:/SmartDashboard/GPLauncher Act Spd (RPM)"
+            },
+            {
+              "color": [
+                0.8666667342185974,
+                0.5176470875740051,
+                0.32156863808631897,
+                1.0
+              ],
+              "id": "NT:/SmartDashboard/GPLauncher Des Spd (RPM)"
+            }
+          ]
+        }
+      ],
+      "window": {
+        "visible": false
+      }
+    }
+  }
+}

photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp

@@ -0,0 +1,125 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "Robot.h"
+
+#include <iostream>
+
+#include <frc/simulation/BatterySim.h>
+#include <frc/simulation/RoboRioSim.h>
+
+void Robot::RobotInit() {}
+
+void Robot::RobotPeriodic() {
+  drivetrain.Periodic();
+  drivetrain.Log();
+}
+
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() { drivetrain.Stop(); }
+
+void Robot::DisabledExit() {}
+
+void Robot::AutonomousInit() {}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::AutonomousExit() {}
+
+void Robot::TeleopInit() {
+  frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}};
+  drivetrain.ResetPose(pose, true);
+}
+
+void Robot::TeleopPeriodic() {
+  // Calculate drivetrain commands from Joystick values
+  auto forward =
+      -1.0 * controller.GetLeftY() * constants::Swerve::kMaxLinearSpeed;
+  auto strafe =
+      -1.0 * controller.GetLeftX() * constants::Swerve::kMaxLinearSpeed;
+  auto turn =
+      -1.0 * controller.GetRightX() * constants::Swerve::kMaxAngularSpeed;
+
+  bool targetVisible = false;
+  double targetYaw = 0.0;
+  auto results = camera.GetAllUnreadResults();
+  if (results.size() > 0) {
+    // Camera processed a new frame since last
+    // Get the last one in the list.
+    auto result = results[results.size() - 1];
+    if (result.HasTargets()) {
+      // At least one AprilTag was seen by the camera
+      for (auto& target : result.GetTargets()) {
+        if (target.GetFiducialId() == 7) {
+          // Found Tag 7, record its information
+          targetYaw = target.GetYaw();
+          targetVisible = true;
+        }
+      }
+    }
+  }
+
+  // Auto-align when requested
+  if (controller.GetAButton() && targetVisible) {
+    // Driver wants auto-alignment to tag 7
+    // And, tag 7 is in sight, so we can turn toward it.
+    // Override the driver's turn command with an automatic one that turns
+    // toward the tag.
+    turn =
+        -1.0 * targetYaw * VISION_TURN_kP * constants::Swerve::kMaxAngularSpeed;
+  }
+
+  // Command drivetrain motors based on target speeds
+  drivetrain.Drive(forward, strafe, turn);
+}
+
+void Robot::TeleopExit() {}
+
+void Robot::TestInit() {}
+
+void Robot::TestPeriodic() {}
+
+void Robot::TestExit() {}
+
+void Robot::SimulationPeriodic() {
+  drivetrain.SimulationPeriodic();
+  vision.SimPeriodic(drivetrain.GetSimPose());
+
+  frc::Field2d& debugField = vision.GetSimDebugField();
+  debugField.GetObject("EstimatedRobot")->SetPose(drivetrain.GetPose());
+  debugField.GetObject("EstimatedRobotModules")
+      ->SetPoses(drivetrain.GetModulePoses());
+
+  units::ampere_t totalCurrent = drivetrain.GetCurrentDraw();
+  units::volt_t loadedBattVolts =
+      frc::sim::BatterySim::Calculate({totalCurrent});
+  // Using max(0.1, voltage) here isn't a *physically correct* solution,
+  // but it avoids problems with battery voltage measuring 0.
+  frc::sim::RoboRioSim::SetVInVoltage(units::math::max(0.1_V, loadedBattVolts));
+}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDrive.cpp

@@ -0,0 +1,200 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "subsystems/SwerveDrive.h"
+
+#include <iostream>
+#include <string>
+
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+SwerveDrive::SwerveDrive()
+    : poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(),
+                    frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}),
+      gyroSim(gyro),
+      swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1),
+                     constants::Swerve::kDriveGearRatio,
+                     constants::Swerve::kWheelDiameter / 2,
+                     constants::Swerve::kSteerFF, frc::DCMotor::Falcon500(1),
+                     constants::Swerve::kSteerGearRatio, kinematics) {}
+
+void SwerveDrive::Periodic() {
+  for (auto& currentModule : swerveMods) {
+    currentModule.Periodic();
+  }
+
+  poseEstimator.Update(GetGyroYaw(), GetModulePositions());
+}
+
+void SwerveDrive::Drive(units::meters_per_second_t vx,
+                        units::meters_per_second_t vy,
+                        units::radians_per_second_t omega) {
+  frc::ChassisSpeeds newChassisSpeeds =
+      frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading());
+  SetChassisSpeeds(newChassisSpeeds, true, false);
+}
+
+void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds,
+                                   bool openLoop, bool steerInPlace) {
+  SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), true,
+                  steerInPlace);
+  this->targetChassisSpeeds = newChassisSpeeds;
+}
+
+void SwerveDrive::SetModuleStates(
+    const std::array<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
+    bool steerInPlace) {
+  std::array<frc::SwerveModuleState, 4> desaturatedStates = desiredStates;
+  frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
+      static_cast<wpi::array<frc::SwerveModuleState, 4>*>(&desaturatedStates),
+      constants::Swerve::kMaxLinearSpeed);
+  for (int i = 0; i < swerveMods.size(); i++) {
+    swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace);
+  }
+}
+
+void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s); }
+
+void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) {
+  if (resetSimPose) {
+    swerveDriveSim.Reset(pose, false);
+    for (int i = 0; i < swerveMods.size(); i++) {
+      swerveMods[i].SimulationUpdate(0_m, 0_mps, 0_A, 0_rad, 0_rad_per_s, 0_A);
+    }
+    gyroSim.SetAngle(-pose.Rotation().Degrees());
+    gyroSim.SetRate(0_rad_per_s);
+  }
+
+  poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose);
+}
+
+frc::Pose2d SwerveDrive::GetPose() const {
+  return poseEstimator.GetEstimatedPosition();
+}
+
+frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); }
+
+frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); }
+
+frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const {
+  return kinematics.ToChassisSpeeds(GetModuleStates());
+}
+
+std::array<frc::SwerveModuleState, 4> SwerveDrive::GetModuleStates() const {
+  std::array<frc::SwerveModuleState, 4> moduleStates;
+  moduleStates[0] = swerveMods[0].GetState();
+  moduleStates[1] = swerveMods[1].GetState();
+  moduleStates[2] = swerveMods[2].GetState();
+  moduleStates[3] = swerveMods[3].GetState();
+  return moduleStates;
+}
+
+std::array<frc::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
+    const {
+  std::array<frc::SwerveModulePosition, 4> modulePositions;
+  modulePositions[0] = swerveMods[0].GetPosition();
+  modulePositions[1] = swerveMods[1].GetPosition();
+  modulePositions[2] = swerveMods[2].GetPosition();
+  modulePositions[3] = swerveMods[3].GetPosition();
+  return modulePositions;
+}
+
+std::array<frc::Pose2d, 4> SwerveDrive::GetModulePoses() const {
+  std::array<frc::Pose2d, 4> modulePoses;
+  for (int i = 0; i < swerveMods.size(); i++) {
+    const SwerveModule& module = swerveMods[i];
+    modulePoses[i] = GetPose().TransformBy(frc::Transform2d{
+        module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()});
+  }
+  return modulePoses;
+}
+
+void SwerveDrive::Log() {
+  std::string table = "Drive/";
+  frc::Pose2d pose = GetPose();
+  frc::SmartDashboard::PutNumber(table + "X", pose.X().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Heading",
+                                 pose.Rotation().Degrees().to<double>());
+  frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds();
+  frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to<double>());
+  frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Omega Degrees",
+      chassisSpeeds.omega.convert<units::degrees_per_second>().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Target VX",
+                                 targetChassisSpeeds.vx.to<double>());
+  frc::SmartDashboard::PutNumber(table + "Target VY",
+                                 targetChassisSpeeds.vy.to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Target Omega Degrees",
+      targetChassisSpeeds.omega.convert<units::degrees_per_second>()
+          .to<double>());
+
+  for (auto& module : swerveMods) {
+    module.Log();
+  }
+}
+
+void SwerveDrive::SimulationPeriodic() {
+  std::array<units::volt_t, 4> driveInputs;
+  std::array<units::volt_t, 4> steerInputs;
+  for (int i = 0; i < swerveMods.size(); i++) {
+    driveInputs[i] = swerveMods[i].GetDriveVoltage();
+    steerInputs[i] = swerveMods[i].GetSteerVoltage();
+  }
+  swerveDriveSim.SetDriveInputs(driveInputs);
+  swerveDriveSim.SetSteerInputs(steerInputs);
+
+  swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod);
+
+  auto driveStates = swerveDriveSim.GetDriveStates();
+  auto steerStates = swerveDriveSim.GetSteerStates();
+  totalCurrentDraw = 0_A;
+  std::array<units::ampere_t, 4> driveCurrents =
+      swerveDriveSim.GetDriveCurrentDraw();
+  for (const auto& current : driveCurrents) {
+    totalCurrentDraw += current;
+  }
+  std::array<units::ampere_t, 4> steerCurrents =
+      swerveDriveSim.GetSteerCurrentDraw();
+  for (const auto& current : steerCurrents) {
+    totalCurrentDraw += current;
+  }
+  for (int i = 0; i < swerveMods.size(); i++) {
+    units::meter_t drivePos{driveStates[i](0, 0)};
+    units::meters_per_second_t driveRate{driveStates[i](1, 0)};
+    units::radian_t steerPos{steerStates[i](0, 0)};
+    units::radians_per_second_t steerRate{steerStates[i](1, 0)};
+    swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i],
+                                   steerPos, steerRate, steerCurrents[i]);
+  }
+  gyroSim.SetRate(-swerveDriveSim.GetOmega());
+  gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees());
+}
+
+frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); }
+
+units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; }

photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDriveSim.cpp

@@ -0,0 +1,287 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "subsystems/SwerveDriveSim.h"
+
+#include <iostream>
+
+#include <frc/RobotController.h>
+#include <frc/system/Discretization.h>
+
+template <typename T>
+int sgn(T val) {
+  return (T(0) < val) - (val < T(0));
+}
+
+SwerveDriveSim::SwerveDriveSim(
+    const frc::SimpleMotorFeedforward<units::meters>& driveFF,
+    const frc::DCMotor& driveMotor, double driveGearing,
+    units::meter_t driveWheelRadius,
+    const frc::SimpleMotorFeedforward<units::radians>& steerFF,
+    const frc::DCMotor& steerMotor, double steerGearing,
+    const frc::SwerveDriveKinematics<numModules>& kinematics)
+    : SwerveDriveSim(
+          frc::LinearSystem<2, 1, 2>{
+              (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
+               -driveFF.GetKv().to<double>() / driveFF.GetKa().to<double>())
+                  .finished(),
+              Eigen::Matrix<double, 2, 1>{0.0,
+                                          1.0 / driveFF.GetKa().to<double>()},
+              (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
+              Eigen::Matrix<double, 2, 1>{0.0, 0.0}},
+          driveFF.GetKs(), driveMotor, driveGearing, driveWheelRadius,
+          frc::LinearSystem<2, 1, 2>{
+              (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
+               -steerFF.GetKv().to<double>() / steerFF.GetKa().to<double>())
+                  .finished(),
+              Eigen::Matrix<double, 2, 1>{0.0,
+                                          1.0 / steerFF.GetKa().to<double>()},
+              (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
+              Eigen::Matrix<double, 2, 1>{0.0, 0.0}},
+          steerFF.GetKs(), steerMotor, steerGearing, kinematics) {}
+
+SwerveDriveSim::SwerveDriveSim(
+    const frc::LinearSystem<2, 1, 2>& drivePlant, units::volt_t driveKs,
+    const frc::DCMotor& driveMotor, double driveGearing,
+    units::meter_t driveWheelRadius,
+    const frc::LinearSystem<2, 1, 2>& steerPlant, units::volt_t steerKs,
+    const frc::DCMotor& steerMotor, double steerGearing,
+    const frc::SwerveDriveKinematics<numModules>& kinematics)
+    : drivePlant(drivePlant),
+      driveKs(driveKs),
+      driveMotor(driveMotor),
+      driveGearing(driveGearing),
+      driveWheelRadius(driveWheelRadius),
+      steerPlant(steerPlant),
+      steerKs(steerKs),
+      steerMotor(steerMotor),
+      steerGearing(steerGearing),
+      kinematics(kinematics) {}
+
+void SwerveDriveSim::SetDriveInputs(
+    const std::array<units::volt_t, numModules>& inputs) {
+  units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage();
+  for (int i = 0; i < driveInputs.size(); i++) {
+    units::volt_t input = inputs[i];
+    driveInputs[i] = std::clamp(input, -battVoltage, battVoltage);
+  }
+}
+
+void SwerveDriveSim::SetSteerInputs(
+    const std::array<units::volt_t, numModules>& inputs) {
+  units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage();
+  for (int i = 0; i < steerInputs.size(); i++) {
+    units::volt_t input = inputs[i];
+    steerInputs[i] = std::clamp(input, -battVoltage, battVoltage);
+  }
+}
+
+Eigen::Matrix<double, 2, 1> SwerveDriveSim::CalculateX(
+    const Eigen::Matrix<double, 2, 2>& discA,
+    const Eigen::Matrix<double, 2, 1>& discB,
+    const Eigen::Matrix<double, 2, 1>& x, units::volt_t input,
+    units::volt_t kS) {
+  auto Ax = discA * x;
+  double nextStateVel = Ax(1, 0);
+  double inputToStop = nextStateVel / -discB(1, 0);
+  double ksSystemEffect =
+      std::clamp(inputToStop, -kS.to<double>(), kS.to<double>());
+
+  nextStateVel += discB(1, 0) * ksSystemEffect;
+  inputToStop = nextStateVel / -discB(1, 0);
+  double signToStop = sgn(inputToStop);
+  double inputSign = sgn(input.to<double>());
+  double ksInputEffect = 0;
+
+  if (std::abs(ksSystemEffect) < kS.to<double>()) {
+    double absInput = std::abs(input.to<double>());
+    ksInputEffect =
+        -std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
+  } else if ((input.to<double>() * signToStop) > (inputToStop * signToStop)) {
+    double absInput = std::abs(input.to<double>() - inputToStop);
+    ksInputEffect =
+        -std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
+  }
+
+  auto sF = Eigen::Matrix<double, 1, 1>{input.to<double>() + ksSystemEffect +
+                                        ksInputEffect};
+  auto Bu = discB * sF;
+  auto retVal = Ax + Bu;
+  return retVal;
+}
+
+void SwerveDriveSim::Update(units::second_t dt) {
+  Eigen::Matrix<double, 2, 2> driveDiscA;
+  Eigen::Matrix<double, 2, 1> driveDiscB;
+  frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA,
+                          &driveDiscB);
+
+  Eigen::Matrix<double, 2, 2> steerDiscA;
+  Eigen::Matrix<double, 2, 1> steerDiscB;
+  frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA,
+                          &steerDiscB);
+
+  std::array<frc::SwerveModulePosition, 4> moduleDeltas;
+
+  for (int i = 0; i < numModules; i++) {
+    double prevDriveStatePos = driveStates[i](0, 0);
+    driveStates[i] = CalculateX(driveDiscA, driveDiscB, driveStates[i],
+                                driveInputs[i], driveKs);
+    double currentDriveStatePos = driveStates[i](0, 0);
+    steerStates[i] = CalculateX(steerDiscA, steerDiscB, steerStates[i],
+                                steerInputs[i], steerKs);
+    double currentSteerStatePos = steerStates[i](0, 0);
+    moduleDeltas[i] = frc::SwerveModulePosition{
+        units::meter_t{currentDriveStatePos - prevDriveStatePos},
+        frc::Rotation2d{units::radian_t{currentSteerStatePos}}};
+  }
+
+  frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas);
+  pose = pose.Exp(twist);
+  omega = twist.dtheta / dt;
+}
+
+void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) {
+  this->pose = pose;
+  if (!preserveMotion) {
+    for (int i = 0; i < numModules; i++) {
+      driveStates[i] = Eigen::Matrix<double, 2, 1>{0, 0};
+      steerStates[i] = Eigen::Matrix<double, 2, 1>{0, 0};
+    }
+    omega = 0_rad_per_s;
+  }
+}
+
+void SwerveDriveSim::Reset(const frc::Pose2d& pose,
+                           const std::array<Eigen::Matrix<double, 2, 1>,
+                                            numModules>& moduleDriveStates,
+                           const std::array<Eigen::Matrix<double, 2, 1>,
+                                            numModules>& moduleSteerStates) {
+  this->pose = pose;
+  driveStates = moduleDriveStates;
+  steerStates = moduleSteerStates;
+  omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega;
+}
+
+frc::Pose2d SwerveDriveSim::GetPose() const { return pose; }
+
+std::array<frc::SwerveModulePosition, numModules>
+SwerveDriveSim::GetModulePositions() const {
+  std::array<frc::SwerveModulePosition, numModules> positions;
+  for (int i = 0; i < numModules; i++) {
+    positions[i] = frc::SwerveModulePosition{
+        units::meter_t{driveStates[i](0, 0)},
+        frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}};
+  }
+  return positions;
+}
+
+std::array<frc::SwerveModulePosition, numModules>
+SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev,
+                                        units::radian_t steerStdDev) {
+  std::array<frc::SwerveModulePosition, numModules> positions;
+  for (int i = 0; i < numModules; i++) {
+    positions[i] = frc::SwerveModulePosition{
+        units::meter_t{driveStates[i](0, 0)} +
+            randDist(generator) * driveStdDev,
+        frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} +
+                        randDist(generator) * steerStdDev}};
+  }
+  return positions;
+}
+
+std::array<frc::SwerveModuleState, numModules>
+SwerveDriveSim::GetModuleStates() {
+  std::array<frc::SwerveModuleState, numModules> states;
+  for (int i = 0; i < numModules; i++) {
+    states[i] = frc::SwerveModuleState{
+        units::meters_per_second_t{driveStates[i](1, 0)},
+        frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}};
+  }
+  return states;
+}
+
+std::array<Eigen::Matrix<double, 2, 1>, numModules>
+SwerveDriveSim::GetDriveStates() const {
+  return driveStates;
+}
+
+std::array<Eigen::Matrix<double, 2, 1>, numModules>
+SwerveDriveSim::GetSteerStates() const {
+  return steerStates;
+}
+
+units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; }
+
+units::ampere_t SwerveDriveSim::GetCurrentDraw(
+    const frc::DCMotor& motor, units::radians_per_second_t velocity,
+    units::volt_t inputVolts, units::volt_t batteryVolts) const {
+  units::volt_t effVolts = inputVolts - velocity / motor.Kv;
+  if (inputVolts >= 0_V) {
+    effVolts = std::clamp(effVolts, 0_V, inputVolts);
+  } else {
+    effVolts = std::clamp(effVolts, inputVolts, 0_V);
+  }
+  auto retVal = (inputVolts / batteryVolts) * (effVolts / motor.R);
+  return retVal;
+}
+
+std::array<units::ampere_t, numModules> SwerveDriveSim::GetDriveCurrentDraw()
+    const {
+  std::array<units::ampere_t, numModules> currents;
+  for (int i = 0; i < numModules; i++) {
+    units::radians_per_second_t speed =
+        units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing /
+        driveWheelRadius.to<double>();
+    currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i],
+                                 frc::RobotController::GetBatteryVoltage());
+  }
+  return currents;
+}
+
+std::array<units::ampere_t, numModules> SwerveDriveSim::GetSteerCurrentDraw()
+    const {
+  std::array<units::ampere_t, numModules> currents;
+  for (int i = 0; i < numModules; i++) {
+    units::radians_per_second_t speed =
+        units::radians_per_second_t{steerStates[i](1, 0) * steerGearing};
+    // TODO: If uncommented we get huge current values.. Not sure how to fix
+    // atm. :(
+    currents[i] = 20_A;
+    // currents[i] = GetCurrentDraw(steerMotor, speed, steerInputs[i],
+    // frc::RobotController::GetBatteryVoltage());
+  }
+  return currents;
+}
+
+units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const {
+  units::ampere_t total{0};
+  for (const auto& val : GetDriveCurrentDraw()) {
+    total += val;
+  }
+  for (const auto& val : GetSteerCurrentDraw()) {
+    total += val;
+  }
+  return total;
+}

photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveModule.cpp

@@ -0,0 +1,146 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "subsystems/SwerveModule.h"
+
+#include <iostream>
+#include <string>
+
+#include <frc/MathUtil.h>
+#include <frc/RobotController.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts)
+    : moduleConstants(consts),
+      driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}),
+      driveEncoder(frc::Encoder{moduleConstants.driveEncoderA,
+                                moduleConstants.driveEncoderB}),
+      steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}),
+      steerEncoder(frc::Encoder{moduleConstants.steerEncoderA,
+                                moduleConstants.steerEncoderB}),
+      driveEncoderSim(driveEncoder),
+      steerEncoderSim(steerEncoder) {
+  driveEncoder.SetDistancePerPulse(
+      constants::Swerve::kDriveDistPerPulse.to<double>());
+  steerEncoder.SetDistancePerPulse(
+      constants::Swerve::kSteerRadPerPulse.to<double>());
+  steerPIDController.EnableContinuousInput(-std::numbers::pi, std::numbers::pi);
+}
+
+void SwerveModule::Periodic() {
+  units::volt_t steerPID = units::volt_t{
+      steerPIDController.Calculate(GetAbsoluteHeading().Radians().to<double>(),
+                                   desiredState.angle.Radians().to<double>())};
+  steerMotor.SetVoltage(steerPID);
+
+  units::volt_t driveFF =
+      constants::Swerve::kDriveFF.Calculate(desiredState.speed);
+  units::volt_t drivePID{0};
+  if (!openLoop) {
+    drivePID = units::volt_t{drivePIDController.Calculate(
+        driveEncoder.GetRate(), desiredState.speed.to<double>())};
+  }
+  driveMotor.SetVoltage(driveFF + drivePID);
+}
+
+void SwerveModule::SetDesiredState(frc::SwerveModuleState newState,
+                                   bool shouldBeOpenLoop, bool steerInPlace) {
+  frc::Rotation2d currentRotation = GetAbsoluteHeading();
+  newState.Optimize(currentRotation);
+  desiredState = newState;
+}
+
+frc::Rotation2d SwerveModule::GetAbsoluteHeading() const {
+  return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}};
+}
+
+frc::SwerveModuleState SwerveModule::GetState() const {
+  return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps,
+                                GetAbsoluteHeading()};
+}
+
+frc::SwerveModulePosition SwerveModule::GetPosition() const {
+  return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m,
+                                   GetAbsoluteHeading()};
+}
+
+units::volt_t SwerveModule::GetDriveVoltage() const {
+  return driveMotor.Get() * frc::RobotController::GetBatteryVoltage();
+}
+
+units::volt_t SwerveModule::GetSteerVoltage() const {
+  return steerMotor.Get() * frc::RobotController::GetBatteryVoltage();
+}
+
+units::ampere_t SwerveModule::GetDriveCurrentSim() const {
+  return driveCurrentSim;
+}
+
+units::ampere_t SwerveModule::GetSteerCurrentSim() const {
+  return steerCurrentSim;
+}
+
+constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const {
+  return moduleConstants;
+}
+
+void SwerveModule::Log() {
+  frc::SwerveModuleState state = GetState();
+
+  std::string table =
+      "Module " + std::to_string(moduleConstants.moduleNum) + "/";
+  frc::SmartDashboard::PutNumber(table + "Steer Degrees",
+                                 frc::AngleModulus(state.angle.Radians())
+                                     .convert<units::degrees>()
+                                     .to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Steer Target Degrees",
+      units::radian_t{steerPIDController.GetSetpoint()}
+          .convert<units::degrees>()
+          .to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Drive Velocity Feet",
+      state.speed.convert<units::feet_per_second>().to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Drive Velocity Target Feet",
+      desiredState.speed.convert<units::feet_per_second>().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Drive Current",
+                                 driveCurrentSim.to<double>());
+  frc::SmartDashboard::PutNumber(table + "Steer Current",
+                                 steerCurrentSim.to<double>());
+}
+
+void SwerveModule::SimulationUpdate(
+    units::meter_t driveEncoderDist,
+    units::meters_per_second_t driveEncoderRate, units::ampere_t driveCurrent,
+    units::radian_t steerEncoderDist,
+    units::radians_per_second_t steerEncoderRate,
+    units::ampere_t steerCurrent) {
+  driveEncoderSim.SetDistance(driveEncoderDist.to<double>());
+  driveEncoderSim.SetRate(driveEncoderRate.to<double>());
+  driveCurrentSim = driveCurrent;
+  steerEncoderSim.SetDistance(steerEncoderDist.to<double>());
+  steerEncoderSim.SetRate(steerEncoderRate.to<double>());
+  steerCurrentSim = steerCurrent;
+}

photonlib-cpp-examples/aimattarget/src/main/deploy/example.txt

@@ -0,0 +1,4 @@
+Files placed in this directory will be deployed to the RoboRIO into the
+  'deploy' directory in the home folder. Use the 'frc::filesystem::GetDeployDirectory'
+  function from the 'frc/Filesystem.h' header to get a proper path relative to the deploy
+  directory.

photonlib-cpp-examples/aimattarget/src/main/include/Constants.h

@@ -0,0 +1,125 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <numbers>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/apriltag/AprilTagFields.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/geometry/Transform3d.h>
+#include <frc/geometry/Translation2d.h>
+#include <units/acceleration.h>
+#include <units/angular_acceleration.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <units/velocity.h>
+
+namespace constants {
+namespace Vision {
+inline constexpr std::string_view kCameraName{"YOUR CAMERA NAME"};
+inline const frc::Transform3d kRobotToCam{
+    frc::Translation3d{0.5_m, 0.0_m, 0.5_m},
+    frc::Rotation3d{0_rad, -30_deg, 0_rad}};
+inline const frc::AprilTagFieldLayout kTagLayout{
+    frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo)};
+
+inline const Eigen::Matrix<double, 3, 1> kSingleTagStdDevs{4, 4, 8};
+inline const Eigen::Matrix<double, 3, 1> kMultiTagStdDevs{0.5, 0.5, 1};
+}  // namespace Vision
+namespace Swerve {
+using namespace units;
+
+inline constexpr units::meter_t kTrackWidth{18.5_in};
+inline constexpr units::meter_t kTrackLength{18.5_in};
+inline constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2};
+inline constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2};
+inline constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps};
+inline constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s};
+inline constexpr units::meter_t kWheelDiameter{4_in};
+inline constexpr units::meter_t kWheelCircumference{kWheelDiameter *
+                                                    std::numbers::pi};
+
+inline constexpr double kDriveGearRatio = 6.75;
+inline constexpr double kSteerGearRatio = 12.8;
+
+inline constexpr units::meter_t kDriveDistPerPulse =
+    kWheelCircumference / 1024.0 / kDriveGearRatio;
+inline constexpr units::radian_t kSteerRadPerPulse =
+    units::radian_t{2 * std::numbers::pi} / 1024.0;
+
+inline constexpr double kDriveKP = 1.0;
+inline constexpr double kDriveKI = 0.0;
+inline constexpr double kDriveKD = 0.0;
+
+inline constexpr double kSteerKP = 20.0;
+inline constexpr double kSteerKI = 0.0;
+inline constexpr double kSteerKD = 0.25;
+
+using namespace units;
+
+inline const frc::SimpleMotorFeedforward<units::meters> kDriveFF{
+    0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq};
+
+inline const frc::SimpleMotorFeedforward<units::radians> kSteerFF{
+    0.5_V, 0.25_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq};
+
+struct ModuleConstants {
+ public:
+  const int moduleNum;
+  const int driveMotorId;
+  const int driveEncoderA;
+  const int driveEncoderB;
+  const int steerMotorId;
+  const int steerEncoderA;
+  const int steerEncoderB;
+  const double angleOffset;
+  const frc::Translation2d centerOffset;
+
+  ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA,
+                  int driveEncoderB, int steerMotorId, int steerEncoderA,
+                  int steerEncoderB, double angleOffset, units::meter_t xOffset,
+                  units::meter_t yOffset)
+      : moduleNum(moduleNum),
+        driveMotorId(driveMotorId),
+        driveEncoderA(driveEncoderA),
+        driveEncoderB(driveEncoderB),
+        steerMotorId(steerMotorId),
+        steerEncoderA(steerEncoderA),
+        steerEncoderB(steerEncoderB),
+        angleOffset(angleOffset),
+        centerOffset(frc::Translation2d{xOffset, yOffset}) {}
+};
+
+inline const ModuleConstants FL_CONSTANTS{
+    1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2};
+inline const ModuleConstants FR_CONSTANTS{
+    2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2};
+inline const ModuleConstants BL_CONSTANTS{
+    3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2};
+inline const ModuleConstants BR_CONSTANTS{
+    4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2};
+}  // namespace Swerve
+}  // namespace constants

photonlib-cpp-examples/aimattarget/src/main/include/Robot.h

@@ -0,0 +1,60 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <photon/PhotonCamera.h>
+
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+
+#include "Constants.h"
+#include "VisionSim.h"
+#include "subsystems/SwerveDrive.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override;
+  void RobotPeriodic() override;
+  void DisabledInit() override;
+  void DisabledPeriodic() override;
+  void DisabledExit() override;
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+  void AutonomousExit() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void TeleopExit() override;
+  void TestInit() override;
+  void TestPeriodic() override;
+  void TestExit() override;
+  void SimulationPeriodic() override;
+
+ private:
+  photon::PhotonCamera camera{constants::Vision::kCameraName};
+  SwerveDrive drivetrain{};
+  VisionSim vision{&camera};
+  frc::XboxController controller{0};
+  static constexpr double VISION_TURN_kP = 0.01;
+};

photonlib-cpp-examples/aimattarget/src/main/include/VisionSim.h

@@ -0,0 +1,87 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <photon/PhotonCamera.h>
+#include <photon/PhotonPoseEstimator.h>
+#include <photon/estimation/VisionEstimation.h>
+#include <photon/simulation/VisionSystemSim.h>
+#include <photon/simulation/VisionTargetSim.h>
+#include <photon/targeting/PhotonPipelineResult.h>
+
+#include <limits>
+#include <memory>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/apriltag/AprilTagFields.h>
+
+#include "Constants.h"
+
+class VisionSim {
+ public:
+  explicit VisionSim(photon::PhotonCamera* camera) {
+    if (frc::RobotBase::IsSimulation()) {
+      visionSim = std::make_unique<photon::VisionSystemSim>("main");
+
+      visionSim->AddAprilTags(constants::Vision::kTagLayout);
+
+      cameraProp = std::make_unique<photon::SimCameraProperties>();
+
+      cameraProp->SetCalibration(320, 240, frc::Rotation2d{90_deg});
+      cameraProp->SetCalibError(.35, .10);
+      cameraProp->SetFPS(70_Hz);
+      cameraProp->SetAvgLatency(30_ms);
+      cameraProp->SetLatencyStdDev(15_ms);
+
+      cameraSim =
+          std::make_shared<photon::PhotonCameraSim>(camera, *cameraProp.get());
+
+      visionSim->AddCamera(cameraSim.get(), constants::Vision::kRobotToCam);
+      cameraSim->EnableDrawWireframe(true);
+    }
+  }
+
+  photon::PhotonPipelineResult GetLatestResult() { return m_latestResult; }
+
+  void SimPeriodic(frc::Pose2d robotSimPose) {
+    visionSim->Update(robotSimPose);
+  }
+
+  void ResetSimPose(frc::Pose2d pose) {
+    if (frc::RobotBase::IsSimulation()) {
+      visionSim->ResetRobotPose(pose);
+    }
+  }
+
+  frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); }
+
+ private:
+  std::unique_ptr<photon::VisionSystemSim> visionSim;
+  std::unique_ptr<photon::SimCameraProperties> cameraProp;
+  std::shared_ptr<photon::PhotonCameraSim> cameraSim;
+
+  // The most recent result, cached for calculating std devs
+  photon::PhotonPipelineResult m_latestResult;
+};

photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDrive.h

@@ -0,0 +1,80 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <frc/ADXRS450_Gyro.h>
+#include <frc/SPI.h>
+#include <frc/estimator/SwerveDrivePoseEstimator.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/simulation/ADXRS450_GyroSim.h>
+
+#include "SwerveDriveSim.h"
+#include "SwerveModule.h"
+
+class SwerveDrive {
+ public:
+  SwerveDrive();
+  void Periodic();
+  void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy,
+             units::radians_per_second_t omega);
+  void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds,
+                        bool openLoop, bool steerInPlace);
+  void SetModuleStates(
+      const std::array<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
+      bool steerInPlace);
+  void Stop();
+  void ResetPose(const frc::Pose2d& pose, bool resetSimPose);
+  frc::Pose2d GetPose() const;
+  frc::Rotation2d GetHeading() const;
+  frc::Rotation2d GetGyroYaw() const;
+  frc::ChassisSpeeds GetChassisSpeeds() const;
+  std::array<frc::SwerveModuleState, 4> GetModuleStates() const;
+  std::array<frc::SwerveModulePosition, 4> GetModulePositions() const;
+  std::array<frc::Pose2d, 4> GetModulePoses() const;
+  void Log();
+  void SimulationPeriodic();
+  frc::Pose2d GetSimPose() const;
+  units::ampere_t GetCurrentDraw() const;
+
+ private:
+  std::array<SwerveModule, 4> swerveMods{
+      SwerveModule{constants::Swerve::FL_CONSTANTS},
+      SwerveModule{constants::Swerve::FR_CONSTANTS},
+      SwerveModule{constants::Swerve::BL_CONSTANTS},
+      SwerveModule{constants::Swerve::BR_CONSTANTS}};
+  frc::SwerveDriveKinematics<4> kinematics{
+      swerveMods[0].GetModuleConstants().centerOffset,
+      swerveMods[1].GetModuleConstants().centerOffset,
+      swerveMods[2].GetModuleConstants().centerOffset,
+      swerveMods[3].GetModuleConstants().centerOffset,
+  };
+  frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0};
+  frc::SwerveDrivePoseEstimator<4> poseEstimator;
+  frc::ChassisSpeeds targetChassisSpeeds{};
+
+  frc::sim::ADXRS450_GyroSim gyroSim;
+  SwerveDriveSim swerveDriveSim;
+  units::ampere_t totalCurrentDraw{0};
+};

photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDriveSim.h

@@ -0,0 +1,102 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <random>
+
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/system/LinearSystem.h>
+#include <frc/system/plant/DCMotor.h>
+#include <units/voltage.h>
+
+static constexpr int numModules{4};
+
+class SwerveDriveSim {
+ public:
+  SwerveDriveSim(const frc::SimpleMotorFeedforward<units::meters>& driveFF,
+                 const frc::DCMotor& driveMotor, double driveGearing,
+                 units::meter_t driveWheelRadius,
+                 const frc::SimpleMotorFeedforward<units::radians>& steerFF,
+                 const frc::DCMotor& steerMotor, double steerGearing,
+                 const frc::SwerveDriveKinematics<numModules>& kinematics);
+  SwerveDriveSim(const frc::LinearSystem<2, 1, 2>& drivePlant,
+                 units::volt_t driveKs, const frc::DCMotor& driveMotor,
+                 double driveGearing, units::meter_t driveWheelRadius,
+                 const frc::LinearSystem<2, 1, 2>& steerPlant,
+                 units::volt_t steerKs, const frc::DCMotor& steerMotor,
+                 double steerGearing,
+                 const frc::SwerveDriveKinematics<numModules>& kinematics);
+  void SetDriveInputs(const std::array<units::volt_t, numModules>& inputs);
+  void SetSteerInputs(const std::array<units::volt_t, numModules>& inputs);
+  static Eigen::Matrix<double, 2, 1> CalculateX(
+      const Eigen::Matrix<double, 2, 2>& discA,
+      const Eigen::Matrix<double, 2, 1>& discB,
+      const Eigen::Matrix<double, 2, 1>& x, units::volt_t input,
+      units::volt_t kS);
+  void Update(units::second_t dt);
+  void Reset(const frc::Pose2d& pose, bool preserveMotion);
+  void Reset(const frc::Pose2d& pose,
+             const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
+                 moduleDriveStates,
+             const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
+                 moduleSteerStates);
+  frc::Pose2d GetPose() const;
+  std::array<frc::SwerveModulePosition, numModules> GetModulePositions() const;
+  std::array<frc::SwerveModulePosition, numModules> GetNoisyModulePositions(
+      units::meter_t driveStdDev, units::radian_t steerStdDev);
+  std::array<frc::SwerveModuleState, numModules> GetModuleStates();
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> GetDriveStates() const;
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> GetSteerStates() const;
+  units::radians_per_second_t GetOmega() const;
+  units::ampere_t GetCurrentDraw(const frc::DCMotor& motor,
+                                 units::radians_per_second_t velocity,
+                                 units::volt_t inputVolts,
+                                 units::volt_t batteryVolts) const;
+  std::array<units::ampere_t, numModules> GetDriveCurrentDraw() const;
+  std::array<units::ampere_t, numModules> GetSteerCurrentDraw() const;
+  units::ampere_t GetTotalCurrentDraw() const;
+
+ private:
+  std::random_device rd{};
+  std::mt19937 generator{rd()};
+  std::normal_distribution<double> randDist{0.0, 1.0};
+  const frc::LinearSystem<2, 1, 2> drivePlant;
+  const units::volt_t driveKs;
+  const frc::DCMotor driveMotor;
+  const double driveGearing;
+  const units::meter_t driveWheelRadius;
+  const frc::LinearSystem<2, 1, 2> steerPlant;
+  const units::volt_t steerKs;
+  const frc::DCMotor steerMotor;
+  const double steerGearing;
+  const frc::SwerveDriveKinematics<numModules> kinematics;
+  std::array<units::volt_t, numModules> driveInputs{};
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> driveStates{};
+  std::array<units::volt_t, numModules> steerInputs{};
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> steerStates{};
+  frc::Pose2d pose{frc::Pose2d{}};
+  units::radians_per_second_t omega{0};
+};

photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveModule.h

@@ -0,0 +1,81 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/controller/PIDController.h>
+#include <frc/kinematics/SwerveModulePosition.h>
+#include <frc/kinematics/SwerveModuleState.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc/simulation/EncoderSim.h>
+#include <units/current.h>
+
+#include "Constants.h"
+
+class SwerveModule {
+ public:
+  explicit SwerveModule(const constants::Swerve::ModuleConstants& consts);
+  void Periodic();
+  void SetDesiredState(frc::SwerveModuleState newState, bool shouldBeOpenLoop,
+                       bool steerInPlace);
+  frc::Rotation2d GetAbsoluteHeading() const;
+  frc::SwerveModuleState GetState() const;
+  frc::SwerveModulePosition GetPosition() const;
+  units::volt_t GetDriveVoltage() const;
+  units::volt_t GetSteerVoltage() const;
+  units::ampere_t GetDriveCurrentSim() const;
+  units::ampere_t GetSteerCurrentSim() const;
+  constants::Swerve::ModuleConstants GetModuleConstants() const;
+  void Log();
+  void SimulationUpdate(units::meter_t driveEncoderDist,
+                        units::meters_per_second_t driveEncoderRate,
+                        units::ampere_t driveCurrent,
+                        units::radian_t steerEncoderDist,
+                        units::radians_per_second_t steerEncoderRate,
+                        units::ampere_t steerCurrent);
+
+ private:
+  const constants::Swerve::ModuleConstants moduleConstants;
+
+  frc::PWMSparkMax driveMotor;
+  frc::Encoder driveEncoder;
+  frc::PWMSparkMax steerMotor;
+  frc::Encoder steerEncoder;
+
+  frc::SwerveModuleState desiredState{};
+  bool openLoop{false};
+
+  frc::PIDController drivePIDController{constants::Swerve::kDriveKP,
+                                        constants::Swerve::kDriveKI,
+                                        constants::Swerve::kDriveKD};
+  frc::PIDController steerPIDController{constants::Swerve::kSteerKP,
+                                        constants::Swerve::kSteerKI,
+                                        constants::Swerve::kSteerKD};
+
+  frc::sim::EncoderSim driveEncoderSim;
+  units::ampere_t driveCurrentSim{0};
+  frc::sim::EncoderSim steerEncoderSim;
+  units::ampere_t steerCurrentSim{0};
+};

photonlib-cpp-examples/aimattarget/src/test/cpp/main.cpp

@@ -0,0 +1,34 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <hal/HAL.h>
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}

photonlib-cpp-examples/build.gradle

@@ -0,0 +1,26 @@
+plugins {
+    id "com.diffplug.spotless" version "6.1.2"
+}
+
+allprojects {
+    repositories {
+        mavenCentral()
+        mavenLocal()
+        maven { url = "https://maven.photonvision.org/releases" }
+    }
+}
+
+spotless {
+    java {
+        toggleOffOn()
+        googleJavaFormat()
+        indentWithTabs(2)
+        indentWithSpaces(4)
+        removeUnusedImports()
+        trimTrailingWhitespace()
+        endWithNewline()
+    }
+    java {
+        target "**/*.java"
+    }
+}

photonlib-cpp-examples/examples.gradle

@@ -0,0 +1,14 @@
+// These should be the only 2 non-project subdirectories in the examples folder
+// I could check for (it)/build.gradle to exist, but w/e
+def EXCLUDED_DIRS = ["bin", "build"]
+
+// List all non-hidden directories not in EXCUDED_DIRS
+ext.exampleFolderNames = file("${rootDir}")
+        .listFiles()
+        .findAll {
+            return (it.isDirectory()
+                    && !it.isHidden()
+                    && !(it.name in EXCLUDED_DIRS) && !it.name.startsWith(".")
+                    && it.toPath().resolve("build.gradle").toFile().exists())
+        }
+        .collect { it.name }

photonlib-cpp-examples/gradle.properties

@@ -0,0 +1,8 @@
+# The --add-exports flags work around a bug with spotless and JDK 17
+# https://github.com/diffplug/spotless/issues/834
+org.gradle.jvmargs= \
+  --add-exports jdk.compiler/com.sun.tools.javac.api=ALL-UNNAMED \
+  --add-exports jdk.compiler/com.sun.tools.javac.file=ALL-UNNAMED \
+  --add-exports jdk.compiler/com.sun.tools.javac.parser=ALL-UNNAMED \
+  --add-exports jdk.compiler/com.sun.tools.javac.tree=ALL-UNNAMED \
+  --add-exports jdk.compiler/com.sun.tools.javac.util=ALL-UNNAMED

photonlib-cpp-examples/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

photonlib-cpp-examples/gradlew

@@ -0,0 +1,191 @@
+#!/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.
+#
+
+##############################################################################
+
+# 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
+
+# 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
+
+APP_NAME="Gradle"
+APP_BASE_NAME=${0##*/}
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD=maximum
+
+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
+    which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+fi
+
+# Increase the maximum file descriptors if we can.
+if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
+    case $MAX_FD in #(
+      max*)
+        MAX_FD=$( ulimit -H -n ) ||
+            warn "Could not query maximum file descriptor limit"
+    esac
+    case $MAX_FD in  #(
+      '' | soft) :;; #(
+      *)
+        ulimit -n "$MAX_FD" ||
+            warn "Could not set maximum file descriptor limit to $MAX_FD"
+    esac
+fi
+
+# 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
+
+# Collect all arguments for the java command;
+#   * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of
+#     shell script including quotes and variable substitutions, so put them in
+#     double quotes to make sure that they get re-expanded; and
+#   * put everything else in single quotes, so that it's not re-expanded.
+
+set -- \
+        "-Dorg.gradle.appname=$APP_BASE_NAME" \
+        -classpath "$CLASSPATH" \
+        org.gradle.wrapper.GradleWrapperMain \
+        "$@"
+
+# 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" "$@"

photonlib-cpp-examples/gradlew.bat

@@ -0,0 +1,89 @@
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem      https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+
+@if "%DEBUG%" == "" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%" == "" set DIRNAME=.
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem 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%" == "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%"=="0" goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+if  not "" == "%GRADLE_EXIT_CONSOLE%" exit 1
+exit /b 1
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega

photonlib-cpp-examples/poseest/.gitignore

@@ -0,0 +1 @@
+vendordeps

photonlib-cpp-examples/poseest/.wpilib/wpilib_preferences.json

@@ -0,0 +1,6 @@
+{
+    "enableCppIntellisense": true,
+    "currentLanguage": "cpp",
+    "projectYear": "2025",
+    "teamNumber": 5
+}

photonlib-cpp-examples/poseest/WPILib-License.md

@@ -0,0 +1,24 @@
+Copyright (c) 2009-2021 FIRST and other WPILib contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+   * Neither the name of FIRST, WPILib, nor the names of other WPILib
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

photonlib-cpp-examples/poseest/build.gradle

@@ -0,0 +1,117 @@
+plugins {
+    id "cpp"
+    id "google-test-test-suite"
+    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2"
+}
+
+repositories {
+    mavenLocal()
+    jcenter()
+}
+
+wpi.maven.useLocal = false
+wpi.maven.useDevelopment = false
+wpi.versions.wpilibVersion = "2025.1.1-beta-2"
+wpi.versions.wpimathVersion = "2025.1.1-beta-2"
+
+// 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.getTeamOrDefault(5940)
+            debug = project.frc.getDebugOrDefault(false)
+
+            artifacts {
+                // First part is artifact name, 2nd is artifact type
+                // getTargetTypeClass is a shortcut to get the class type using a string
+
+                frcCpp(getArtifactTypeClass('FRCNativeArtifact')) {
+                }
+
+                // Static files artifact
+                frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+                    files = project.fileTree('src/main/deploy')
+                    directory = '/home/lvuser/deploy'
+                }
+            }
+        }
+    }
+}
+
+def deployArtifact = deploy.targets.roborio.artifacts.frcCpp
+
+// Set this to true to enable desktop support.
+def includeDesktopSupport = true
+
+// Set to true to run simulation in debug mode
+wpi.cpp.debugSimulation = false
+
+// Default enable simgui
+wpi.sim.addGui().defaultEnabled = true
+// Enable DS but not by default
+wpi.sim.addDriverstation()
+
+model {
+    components {
+        frcUserProgram(NativeExecutableSpec) {
+            // We don't need to build for roborio -- if we do, we need to install
+            // a roborio toolchain every time we build in CI
+            // Ideally, we'd be able to set the roborio toolchain as optional, but
+            // I can't figure out how to set that environment variable from build.gradle
+            // (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71)
+            // for now, commented out
+
+            // targetPlatform wpi.platforms.roborio
+
+            if (includeDesktopSupport) {
+                targetPlatform wpi.platforms.desktop
+            }
+
+            sources.cpp {
+                source {
+                    srcDir 'src/main/cpp'
+                    include '**/*.cpp', '**/*.cc'
+                }
+                exportedHeaders {
+                    srcDir 'src/main/include'
+                }
+            }
+
+            // Set deploy task to deploy this component
+            deployArtifact.component = it
+
+            // Enable run tasks for this component
+            wpi.cpp.enableExternalTasks(it)
+
+            // Enable simulation for this component
+            wpi.sim.enable(it)
+            // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+            wpi.cpp.vendor.cpp(it)
+            wpi.cpp.deps.wpilib(it)
+        }
+    }
+    testSuites {
+        frcUserProgramTest(GoogleTestTestSuiteSpec) {
+            testing $.components.frcUserProgram
+
+            sources.cpp {
+                source {
+                    srcDir 'src/test/cpp'
+                    include '**/*.cpp'
+                }
+            }
+
+            // Enable run tasks for this component
+            wpi.cpp.enableExternalTasks(it)
+
+            wpi.cpp.vendor.cpp(it)
+            wpi.cpp.deps.wpilib(it)
+            wpi.cpp.deps.googleTest(it)
+        }
+    }
+}

photonlib-cpp-examples/poseest/gradlew

@@ -0,0 +1,241 @@
+#!/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/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+#       within the Gradle project.
+#
+#       You can find Gradle at https://github.com/gradle/gradle/.
+#
+##############################################################################
+
+# Attempt to set APP_HOME
+
+# Resolve links: $0 may be a link
+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
+
+# 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
+
+APP_NAME="Gradle"
+APP_BASE_NAME=${0##*/}
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD=maximum
+
+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
+    which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+fi
+
+# Increase the maximum file descriptors if we can.
+if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
+    case $MAX_FD in #(
+      max*)
+        MAX_FD=$( ulimit -H -n ) ||
+            warn "Could not query maximum file descriptor limit"
+    esac
+    case $MAX_FD in  #(
+      '' | soft) :;; #(
+      *)
+        ulimit -n "$MAX_FD" ||
+            warn "Could not set maximum file descriptor limit to $MAX_FD"
+    esac
+fi
+
+# 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
+
+# Collect all arguments for the java command;
+#   * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of
+#     shell script including quotes and variable substitutions, so put them in
+#     double quotes to make sure that they get re-expanded; and
+#   * put everything else in single quotes, so that it's not re-expanded.
+
+set -- \
+        "-Dorg.gradle.appname=$APP_BASE_NAME" \
+        -classpath "$CLASSPATH" \
+        org.gradle.wrapper.GradleWrapperMain \
+        "$@"
+
+# Stop when "xargs" is not available.
+if ! command -v xargs >/dev/null 2>&1
+then
+    die "xargs is not available"
+fi
+
+# Use "xargs" to parse quoted args.
+#
+# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
+#
+# In Bash we could simply go:
+#
+#   readarray ARGS < <( xargs -n1 <<<"$var" ) &&
+#   set -- "${ARGS[@]}" "$@"
+#
+# but POSIX shell has neither arrays nor command substitution, so instead we
+# post-process each arg (as a line of input to sed) to backslash-escape any
+# character that might be a shell metacharacter, then use eval to reverse
+# that process (while maintaining the separation between arguments), and wrap
+# the whole thing up as a single "set" statement.
+#
+# This will of course break if any of these variables contains a newline or
+# an unmatched quote.
+#
+
+eval "set -- $(
+        printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
+        xargs -n1 |
+        sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
+        tr '\n' ' '
+    )" '"$@"'
+
+exec "$JAVACMD" "$@"

photonlib-cpp-examples/poseest/gradlew.bat

@@ -0,0 +1,91 @@
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem      https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+
+@if "%DEBUG%"=="" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%"=="" set DIRNAME=.
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem 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

photonlib-cpp-examples/poseest/networktables.json

@@ -0,0 +1 @@
+[]

photonlib-cpp-examples/poseest/settings.gradle

@@ -0,0 +1,30 @@
+import org.gradle.internal.os.OperatingSystem
+
+rootProject.name = 'poseest'
+
+pluginManagement {
+    repositories {
+        mavenLocal()
+        jcenter()
+        gradlePluginPortal()
+        String frcYear = '2025'
+        File frcHome
+        if (OperatingSystem.current().isWindows()) {
+            String publicFolder = System.getenv('PUBLIC')
+            if (publicFolder == null) {
+                publicFolder = "C:\\Users\\Public"
+            }
+            def homeRoot = new File(publicFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        } else {
+            def userFolder = System.getProperty("user.home")
+            def homeRoot = new File(userFolder, "wpilib")
+            frcHome = new File(homeRoot, frcYear)
+        }
+        def frcHomeMaven = new File(frcHome, 'maven')
+        maven {
+            name 'frcHome'
+            url frcHomeMaven
+        }
+    }
+}

photonlib-cpp-examples/poseest/simgui-ds.json

@@ -0,0 +1,100 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decayRate": 0.0,
+          "keyRate": 0.009999999776482582
+        },
+        {},
+        {
+          "decKey": 81,
+          "incKey": 69
+        }
+      ],
+      "axisCount": 6,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
+  ]
+}

photonlib-cpp-examples/poseest/simgui-window.json

@@ -0,0 +1,74 @@
+{
+  "Docking": {
+    "Data": []
+  },
+  "MainWindow": {
+    "GLOBAL": {
+      "fps": "120",
+      "height": "910",
+      "maximized": "0",
+      "style": "0",
+      "userScale": "2",
+      "width": "1550",
+      "xpos": "-1602",
+      "ypos": "79"
+    }
+  },
+  "Window": {
+    "###/SmartDashboard/VisionSystemSim-main/Sim Field": {
+      "Collapsed": "0",
+      "Pos": "554,10",
+      "Size": "991,527"
+    },
+    "###FMS": {
+      "Collapsed": "0",
+      "Pos": "12,604",
+      "Size": "202,214"
+    },
+    "###Joysticks": {
+      "Collapsed": "0",
+      "Pos": "278,685",
+      "Size": "976,219"
+    },
+    "###NetworkTables": {
+      "Collapsed": "0",
+      "Pos": "353,479",
+      "Size": "830,620"
+    },
+    "###NetworkTables Info": {
+      "Collapsed": "0",
+      "Pos": "315,527",
+      "Size": "750,145"
+    },
+    "###Other Devices": {
+      "Collapsed": "0",
+      "Pos": "1025,20",
+      "Size": "250,695"
+    },
+    "###Plot <0>": {
+      "Collapsed": "0",
+      "Pos": "113,22",
+      "Size": "448,400"
+    },
+    "###System Joysticks": {
+      "Collapsed": "0",
+      "Pos": "5,350",
+      "Size": "232,254"
+    },
+    "###Timing": {
+      "Collapsed": "0",
+      "Pos": "5,150",
+      "Size": "162,142"
+    },
+    "Debug##Default": {
+      "Collapsed": "0",
+      "Pos": "60,60",
+      "Size": "400,400"
+    },
+    "Robot State": {
+      "Collapsed": "0",
+      "Pos": "5,20",
+      "Size": "109,134"
+    }
+  }
+}

photonlib-cpp-examples/poseest/simgui.json

@@ -0,0 +1,161 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d"
+    },
+    "windows": {
+      "/SmartDashboard/VisionSystemSim-main/Sim Field": {
+        "EstimatedRobot": {
+          "arrowWeight": 3.0,
+          "length": 0.800000011920929,
+          "selectable": false,
+          "weight": 3.0,
+          "width": 0.800000011920929
+        },
+        "EstimatedRobotModules": {
+          "arrows": false,
+          "image": "swerve_module.png",
+          "length": 0.30000001192092896,
+          "selectable": false,
+          "width": 0.30000001192092896
+        },
+        "Robot": {
+          "arrowColor": [
+            1.0,
+            1.0,
+            1.0,
+            255.0
+          ],
+          "arrowWeight": 2.0,
+          "color": [
+            1.0,
+            1.0,
+            1.0,
+            255.0
+          ],
+          "length": 0.800000011920929,
+          "selectable": false,
+          "weight": 2.0,
+          "width": 0.800000011920929
+        },
+        "VisionEstimation": {
+          "arrowColor": [
+            0.0,
+            0.6075949668884277,
+            1.0,
+            255.0
+          ],
+          "arrowWeight": 2.0,
+          "color": [
+            0.0,
+            0.6075949668884277,
+            1.0,
+            255.0
+          ],
+          "selectable": false,
+          "weight": 2.0
+        },
+        "apriltag": {
+          "arrows": false,
+          "image": "tag-green.png",
+          "length": 0.6000000238418579,
+          "width": 0.5
+        },
+        "bottom": 1476,
+        "cameras": {
+          "arrowColor": [
+            0.29535865783691406,
+            1.0,
+            0.9910804033279419,
+            255.0
+          ],
+          "arrowSize": 19,
+          "arrowWeight": 3.0,
+          "length": 1.0,
+          "style": "Hidden",
+          "weight": 1.0,
+          "width": 1.0
+        },
+        "height": 8.210550308227539,
+        "image": "2023-field.png",
+        "left": 150,
+        "right": 2961,
+        "top": 79,
+        "visibleTargetPoses": {
+          "arrows": false,
+          "image": "tag-blue.png",
+          "length": 0.5,
+          "selectable": false,
+          "width": 0.4000000059604645
+        },
+        "width": 16.541748046875,
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "CameraPublisher": {
+        "YOUR CAMERA NAME-processed": {
+          "open": true,
+          "string[]##v_/CameraPublisher/YOUR CAMERA NAME-processed/streams": {
+            "open": true
+          }
+        },
+        "open": true
+      },
+      "SmartDashboard": {
+        "VisionSystemSim-main": {
+          "open": true
+        },
+        "open": true
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  },
+  "Plot": {
+    "Plot <0>": {
+      "plots": [
+        {
+          "axis": [
+            {
+              "autoFit": true
+            }
+          ],
+          "backgroundColor": [
+            0.0,
+            0.0,
+            0.0,
+            0.8500000238418579
+          ],
+          "height": 332,
+          "series": [
+            {
+              "color": [
+                0.2980392277240753,
+                0.44705885648727417,
+                0.6901960968971252,
+                1.0
+              ],
+              "id": "NT:/SmartDashboard/GPLauncher Act Spd (RPM)"
+            },
+            {
+              "color": [
+                0.8666667342185974,
+                0.5176470875740051,
+                0.32156863808631897,
+                1.0
+              ],
+              "id": "NT:/SmartDashboard/GPLauncher Des Spd (RPM)"
+            }
+          ]
+        }
+      ]
+    }
+  }
+}

photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp

@@ -0,0 +1,116 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "Robot.h"
+
+#include <iostream>
+
+#include <frc/simulation/BatterySim.h>
+#include <frc/simulation/RoboRioSim.h>
+
+void Robot::RobotInit() {}
+
+void Robot::RobotPeriodic() {
+  launcher.periodic();
+  drivetrain.Periodic();
+
+  auto visionEst = vision.GetEstimatedGlobalPose();
+  if (visionEst.has_value()) {
+    auto est = visionEst.value();
+    auto estPose = est.estimatedPose.ToPose2d();
+    auto estStdDevs = vision.GetEstimationStdDevs(estPose);
+    drivetrain.AddVisionMeasurement(est.estimatedPose.ToPose2d(), est.timestamp,
+                                    estStdDevs);
+  }
+
+  drivetrain.Log();
+}
+
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() { drivetrain.Stop(); }
+
+void Robot::DisabledExit() {}
+
+void Robot::AutonomousInit() {}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::AutonomousExit() {}
+
+void Robot::TeleopInit() {
+  frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}};
+  drivetrain.ResetPose(pose, true);
+}
+
+void Robot::TeleopPeriodic() {
+  // Calculate drivetrain commands from Joystick values
+  auto forward =
+      -1.0 * controller.GetLeftY() * constants::Swerve::kMaxLinearSpeed;
+  auto strafe =
+      -1.0 * controller.GetLeftX() * constants::Swerve::kMaxLinearSpeed;
+  auto turn =
+      -1.0 * controller.GetRightX() * constants::Swerve::kMaxAngularSpeed;
+
+  // Command drivetrain motors based on target speeds
+  drivetrain.Drive(forward, strafe, turn);
+
+  // Calculate whether the gamepiece launcher runs based on our global pose
+  // estimate.
+  frc::Pose2d curPose = drivetrain.GetPose();
+  bool shouldRun = (curPose.Y() > 2.0_m &&
+                    curPose.X() < 4.0_m);  // Close enough to blue speaker
+  launcher.setRunning(shouldRun);
+}
+
+void Robot::TeleopExit() {}
+
+void Robot::TestInit() {}
+
+void Robot::TestPeriodic() {}
+
+void Robot::TestExit() {}
+
+void Robot::SimulationPeriodic() {
+  launcher.simulationPeriodic();
+  drivetrain.SimulationPeriodic();
+  vision.SimPeriodic(drivetrain.GetSimPose());
+
+  frc::Field2d& debugField = vision.GetSimDebugField();
+  debugField.GetObject("EstimatedRobot")->SetPose(drivetrain.GetPose());
+  debugField.GetObject("EstimatedRobotModules")
+      ->SetPoses(drivetrain.GetModulePoses());
+
+  units::ampere_t totalCurrent = drivetrain.GetCurrentDraw();
+  units::volt_t loadedBattVolts =
+      frc::sim::BatterySim::Calculate({totalCurrent});
+
+  // Using max(0.1, voltage) here isn't a *physically correct* solution,
+  // but it avoids problems with battery voltage measuring 0.
+  frc::sim::RoboRioSim::SetVInVoltage(units::math::max(0.1_V, loadedBattVolts));
+}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

photonlib-cpp-examples/poseest/src/main/cpp/subsystems/GamepieceLauncher.cpp

@@ -0,0 +1,58 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "subsystems/GamepieceLauncher.h"  // Include the header file
+
+GamepieceLauncher::GamepieceLauncher() {
+  motor = new frc::PWMSparkMax(8);  // Create the motor object for PWM port 8
+  simulationInit();
+}
+
+void GamepieceLauncher::setRunning(bool shouldRun) {
+  curDesSpd = shouldRun ? LAUNCH_SPEED_RPM : 0.0;
+}
+
+void GamepieceLauncher::periodic() {
+  // Calculate the maximum RPM
+  double maxRPM =
+      units::radians_per_second_t(frc::DCMotor::Falcon500(1).freeSpeed)
+          .to<double>() *
+      60 / (2 * std::numbers::pi);
+  curMotorCmd = curDesSpd / maxRPM;
+  curMotorCmd = std::clamp(curMotorCmd, 0.0, 1.0);
+  motor->Set(curMotorCmd);
+
+  frc::SmartDashboard::PutNumber("GPLauncher Des Spd (RPM)", curDesSpd);
+}
+
+void GamepieceLauncher::simulationPeriodic() {
+  launcherSim.SetInputVoltage(curMotorCmd *
+                              frc::RobotController::GetBatteryVoltage());
+  launcherSim.Update(0.02_s);
+  auto spd = launcherSim.GetAngularVelocity().to<double>() * 60 /
+             (2 * std::numbers::pi);
+  frc::SmartDashboard::PutNumber("GPLauncher Act Spd (RPM)", spd);
+}
+
+void GamepieceLauncher::simulationInit() {}

photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDrive.cpp

@@ -0,0 +1,212 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "subsystems/SwerveDrive.h"
+
+#include <iostream>
+#include <string>
+
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+SwerveDrive::SwerveDrive()
+    : poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(),
+                    frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}),
+      gyroSim(gyro),
+      swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1),
+                     constants::Swerve::kDriveGearRatio,
+                     constants::Swerve::kWheelDiameter / 2,
+                     constants::Swerve::kSteerFF, frc::DCMotor::Falcon500(1),
+                     constants::Swerve::kSteerGearRatio, kinematics) {}
+
+void SwerveDrive::Periodic() {
+  for (auto& currentModule : swerveMods) {
+    currentModule.Periodic();
+  }
+
+  poseEstimator.Update(GetGyroYaw(), GetModulePositions());
+}
+
+void SwerveDrive::Drive(units::meters_per_second_t vx,
+                        units::meters_per_second_t vy,
+                        units::radians_per_second_t omega) {
+  frc::ChassisSpeeds newChassisSpeeds =
+      frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading());
+  SetChassisSpeeds(newChassisSpeeds, true, false);
+}
+
+void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds,
+                                   bool openLoop, bool steerInPlace) {
+  SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), true,
+                  steerInPlace);
+  this->targetChassisSpeeds = newChassisSpeeds;
+}
+
+void SwerveDrive::SetModuleStates(
+    const std::array<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
+    bool steerInPlace) {
+  std::array<frc::SwerveModuleState, 4> desaturatedStates = desiredStates;
+  frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
+      static_cast<wpi::array<frc::SwerveModuleState, 4>*>(&desaturatedStates),
+      constants::Swerve::kMaxLinearSpeed);
+  for (int i = 0; i < swerveMods.size(); i++) {
+    swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace);
+  }
+}
+
+void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s); }
+
+void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement,
+                                       units::second_t timestamp) {
+  poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp);
+}
+
+void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement,
+                                       units::second_t timestamp,
+                                       const Eigen::Vector3d& stdDevs) {
+  wpi::array<double, 3> newStdDevs{stdDevs(0), stdDevs(1), stdDevs(2)};
+  poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp, newStdDevs);
+}
+
+void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) {
+  if (resetSimPose) {
+    swerveDriveSim.Reset(pose, false);
+    for (int i = 0; i < swerveMods.size(); i++) {
+      swerveMods[i].SimulationUpdate(0_m, 0_mps, 0_A, 0_rad, 0_rad_per_s, 0_A);
+    }
+    gyroSim.SetAngle(-pose.Rotation().Degrees());
+    gyroSim.SetRate(0_rad_per_s);
+  }
+
+  poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose);
+}
+
+frc::Pose2d SwerveDrive::GetPose() const {
+  return poseEstimator.GetEstimatedPosition();
+}
+
+frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); }
+
+frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); }
+
+frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const {
+  return kinematics.ToChassisSpeeds(GetModuleStates());
+}
+
+std::array<frc::SwerveModuleState, 4> SwerveDrive::GetModuleStates() const {
+  std::array<frc::SwerveModuleState, 4> moduleStates;
+  moduleStates[0] = swerveMods[0].GetState();
+  moduleStates[1] = swerveMods[1].GetState();
+  moduleStates[2] = swerveMods[2].GetState();
+  moduleStates[3] = swerveMods[3].GetState();
+  return moduleStates;
+}
+
+std::array<frc::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
+    const {
+  std::array<frc::SwerveModulePosition, 4> modulePositions;
+  modulePositions[0] = swerveMods[0].GetPosition();
+  modulePositions[1] = swerveMods[1].GetPosition();
+  modulePositions[2] = swerveMods[2].GetPosition();
+  modulePositions[3] = swerveMods[3].GetPosition();
+  return modulePositions;
+}
+
+std::array<frc::Pose2d, 4> SwerveDrive::GetModulePoses() const {
+  std::array<frc::Pose2d, 4> modulePoses;
+  for (int i = 0; i < swerveMods.size(); i++) {
+    const SwerveModule& module = swerveMods[i];
+    modulePoses[i] = GetPose().TransformBy(frc::Transform2d{
+        module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()});
+  }
+  return modulePoses;
+}
+
+void SwerveDrive::Log() {
+  std::string table = "Drive/";
+  frc::Pose2d pose = GetPose();
+  frc::SmartDashboard::PutNumber(table + "X", pose.X().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Heading",
+                                 pose.Rotation().Degrees().to<double>());
+  frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds();
+  frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to<double>());
+  frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Omega Degrees",
+      chassisSpeeds.omega.convert<units::degrees_per_second>().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Target VX",
+                                 targetChassisSpeeds.vx.to<double>());
+  frc::SmartDashboard::PutNumber(table + "Target VY",
+                                 targetChassisSpeeds.vy.to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Target Omega Degrees",
+      targetChassisSpeeds.omega.convert<units::degrees_per_second>()
+          .to<double>());
+
+  for (auto& module : swerveMods) {
+    module.Log();
+  }
+}
+
+void SwerveDrive::SimulationPeriodic() {
+  std::array<units::volt_t, 4> driveInputs;
+  std::array<units::volt_t, 4> steerInputs;
+  for (int i = 0; i < swerveMods.size(); i++) {
+    driveInputs[i] = swerveMods[i].GetDriveVoltage();
+    steerInputs[i] = swerveMods[i].GetSteerVoltage();
+  }
+  swerveDriveSim.SetDriveInputs(driveInputs);
+  swerveDriveSim.SetSteerInputs(steerInputs);
+
+  swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod);
+
+  auto driveStates = swerveDriveSim.GetDriveStates();
+  auto steerStates = swerveDriveSim.GetSteerStates();
+  totalCurrentDraw = 0_A;
+  std::array<units::ampere_t, 4> driveCurrents =
+      swerveDriveSim.GetDriveCurrentDraw();
+  for (const auto& current : driveCurrents) {
+    totalCurrentDraw += current;
+  }
+  std::array<units::ampere_t, 4> steerCurrents =
+      swerveDriveSim.GetSteerCurrentDraw();
+  for (const auto& current : steerCurrents) {
+    totalCurrentDraw += current;
+  }
+  for (int i = 0; i < swerveMods.size(); i++) {
+    units::meter_t drivePos{driveStates[i](0, 0)};
+    units::meters_per_second_t driveRate{driveStates[i](1, 0)};
+    units::radian_t steerPos{steerStates[i](0, 0)};
+    units::radians_per_second_t steerRate{steerStates[i](1, 0)};
+    swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i],
+                                   steerPos, steerRate, steerCurrents[i]);
+  }
+  gyroSim.SetRate(-swerveDriveSim.GetOmega());
+  gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees());
+}
+
+frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); }
+
+units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; }

photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDriveSim.cpp

@@ -0,0 +1,287 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "subsystems/SwerveDriveSim.h"
+
+#include <iostream>
+
+#include <frc/RobotController.h>
+#include <frc/system/Discretization.h>
+
+template <typename T>
+int sgn(T val) {
+  return (T(0) < val) - (val < T(0));
+}
+
+SwerveDriveSim::SwerveDriveSim(
+    const frc::SimpleMotorFeedforward<units::meters>& driveFF,
+    const frc::DCMotor& driveMotor, double driveGearing,
+    units::meter_t driveWheelRadius,
+    const frc::SimpleMotorFeedforward<units::radians>& steerFF,
+    const frc::DCMotor& steerMotor, double steerGearing,
+    const frc::SwerveDriveKinematics<numModules>& kinematics)
+    : SwerveDriveSim(
+          frc::LinearSystem<2, 1, 2>{
+              (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
+               -driveFF.GetKv().to<double>() / driveFF.GetKa().to<double>())
+                  .finished(),
+              Eigen::Matrix<double, 2, 1>{0.0,
+                                          1.0 / driveFF.GetKa().to<double>()},
+              (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
+              Eigen::Matrix<double, 2, 1>{0.0, 0.0}},
+          driveFF.GetKs(), driveMotor, driveGearing, driveWheelRadius,
+          frc::LinearSystem<2, 1, 2>{
+              (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
+               -steerFF.GetKv().to<double>() / steerFF.GetKa().to<double>())
+                  .finished(),
+              Eigen::Matrix<double, 2, 1>{0.0,
+                                          1.0 / steerFF.GetKa().to<double>()},
+              (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
+              Eigen::Matrix<double, 2, 1>{0.0, 0.0}},
+          steerFF.GetKs(), steerMotor, steerGearing, kinematics) {}
+
+SwerveDriveSim::SwerveDriveSim(
+    const frc::LinearSystem<2, 1, 2>& drivePlant, units::volt_t driveKs,
+    const frc::DCMotor& driveMotor, double driveGearing,
+    units::meter_t driveWheelRadius,
+    const frc::LinearSystem<2, 1, 2>& steerPlant, units::volt_t steerKs,
+    const frc::DCMotor& steerMotor, double steerGearing,
+    const frc::SwerveDriveKinematics<numModules>& kinematics)
+    : drivePlant(drivePlant),
+      driveKs(driveKs),
+      driveMotor(driveMotor),
+      driveGearing(driveGearing),
+      driveWheelRadius(driveWheelRadius),
+      steerPlant(steerPlant),
+      steerKs(steerKs),
+      steerMotor(steerMotor),
+      steerGearing(steerGearing),
+      kinematics(kinematics) {}
+
+void SwerveDriveSim::SetDriveInputs(
+    const std::array<units::volt_t, numModules>& inputs) {
+  units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage();
+  for (int i = 0; i < driveInputs.size(); i++) {
+    units::volt_t input = inputs[i];
+    driveInputs[i] = std::clamp(input, -battVoltage, battVoltage);
+  }
+}
+
+void SwerveDriveSim::SetSteerInputs(
+    const std::array<units::volt_t, numModules>& inputs) {
+  units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage();
+  for (int i = 0; i < steerInputs.size(); i++) {
+    units::volt_t input = inputs[i];
+    steerInputs[i] = std::clamp(input, -battVoltage, battVoltage);
+  }
+}
+
+Eigen::Matrix<double, 2, 1> SwerveDriveSim::CalculateX(
+    const Eigen::Matrix<double, 2, 2>& discA,
+    const Eigen::Matrix<double, 2, 1>& discB,
+    const Eigen::Matrix<double, 2, 1>& x, units::volt_t input,
+    units::volt_t kS) {
+  auto Ax = discA * x;
+  double nextStateVel = Ax(1, 0);
+  double inputToStop = nextStateVel / -discB(1, 0);
+  double ksSystemEffect =
+      std::clamp(inputToStop, -kS.to<double>(), kS.to<double>());
+
+  nextStateVel += discB(1, 0) * ksSystemEffect;
+  inputToStop = nextStateVel / -discB(1, 0);
+  double signToStop = sgn(inputToStop);
+  double inputSign = sgn(input.to<double>());
+  double ksInputEffect = 0;
+
+  if (std::abs(ksSystemEffect) < kS.to<double>()) {
+    double absInput = std::abs(input.to<double>());
+    ksInputEffect =
+        -std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
+  } else if ((input.to<double>() * signToStop) > (inputToStop * signToStop)) {
+    double absInput = std::abs(input.to<double>() - inputToStop);
+    ksInputEffect =
+        -std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
+  }
+
+  auto sF = Eigen::Matrix<double, 1, 1>{input.to<double>() + ksSystemEffect +
+                                        ksInputEffect};
+  auto Bu = discB * sF;
+  auto retVal = Ax + Bu;
+  return retVal;
+}
+
+void SwerveDriveSim::Update(units::second_t dt) {
+  Eigen::Matrix<double, 2, 2> driveDiscA;
+  Eigen::Matrix<double, 2, 1> driveDiscB;
+  frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA,
+                          &driveDiscB);
+
+  Eigen::Matrix<double, 2, 2> steerDiscA;
+  Eigen::Matrix<double, 2, 1> steerDiscB;
+  frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA,
+                          &steerDiscB);
+
+  std::array<frc::SwerveModulePosition, 4> moduleDeltas;
+
+  for (int i = 0; i < numModules; i++) {
+    double prevDriveStatePos = driveStates[i](0, 0);
+    driveStates[i] = CalculateX(driveDiscA, driveDiscB, driveStates[i],
+                                driveInputs[i], driveKs);
+    double currentDriveStatePos = driveStates[i](0, 0);
+    steerStates[i] = CalculateX(steerDiscA, steerDiscB, steerStates[i],
+                                steerInputs[i], steerKs);
+    double currentSteerStatePos = steerStates[i](0, 0);
+    moduleDeltas[i] = frc::SwerveModulePosition{
+        units::meter_t{currentDriveStatePos - prevDriveStatePos},
+        frc::Rotation2d{units::radian_t{currentSteerStatePos}}};
+  }
+
+  frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas);
+  pose = pose.Exp(twist);
+  omega = twist.dtheta / dt;
+}
+
+void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) {
+  this->pose = pose;
+  if (!preserveMotion) {
+    for (int i = 0; i < numModules; i++) {
+      driveStates[i] = Eigen::Matrix<double, 2, 1>{0, 0};
+      steerStates[i] = Eigen::Matrix<double, 2, 1>{0, 0};
+    }
+    omega = 0_rad_per_s;
+  }
+}
+
+void SwerveDriveSim::Reset(const frc::Pose2d& pose,
+                           const std::array<Eigen::Matrix<double, 2, 1>,
+                                            numModules>& moduleDriveStates,
+                           const std::array<Eigen::Matrix<double, 2, 1>,
+                                            numModules>& moduleSteerStates) {
+  this->pose = pose;
+  driveStates = moduleDriveStates;
+  steerStates = moduleSteerStates;
+  omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega;
+}
+
+frc::Pose2d SwerveDriveSim::GetPose() const { return pose; }
+
+std::array<frc::SwerveModulePosition, numModules>
+SwerveDriveSim::GetModulePositions() const {
+  std::array<frc::SwerveModulePosition, numModules> positions;
+  for (int i = 0; i < numModules; i++) {
+    positions[i] = frc::SwerveModulePosition{
+        units::meter_t{driveStates[i](0, 0)},
+        frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}};
+  }
+  return positions;
+}
+
+std::array<frc::SwerveModulePosition, numModules>
+SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev,
+                                        units::radian_t steerStdDev) {
+  std::array<frc::SwerveModulePosition, numModules> positions;
+  for (int i = 0; i < numModules; i++) {
+    positions[i] = frc::SwerveModulePosition{
+        units::meter_t{driveStates[i](0, 0)} +
+            randDist(generator) * driveStdDev,
+        frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} +
+                        randDist(generator) * steerStdDev}};
+  }
+  return positions;
+}
+
+std::array<frc::SwerveModuleState, numModules>
+SwerveDriveSim::GetModuleStates() {
+  std::array<frc::SwerveModuleState, numModules> states;
+  for (int i = 0; i < numModules; i++) {
+    states[i] = frc::SwerveModuleState{
+        units::meters_per_second_t{driveStates[i](1, 0)},
+        frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}};
+  }
+  return states;
+}
+
+std::array<Eigen::Matrix<double, 2, 1>, numModules>
+SwerveDriveSim::GetDriveStates() const {
+  return driveStates;
+}
+
+std::array<Eigen::Matrix<double, 2, 1>, numModules>
+SwerveDriveSim::GetSteerStates() const {
+  return steerStates;
+}
+
+units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; }
+
+units::ampere_t SwerveDriveSim::GetCurrentDraw(
+    const frc::DCMotor& motor, units::radians_per_second_t velocity,
+    units::volt_t inputVolts, units::volt_t batteryVolts) const {
+  units::volt_t effVolts = inputVolts - velocity / motor.Kv;
+  if (inputVolts >= 0_V) {
+    effVolts = std::clamp(effVolts, 0_V, inputVolts);
+  } else {
+    effVolts = std::clamp(effVolts, inputVolts, 0_V);
+  }
+  auto retVal = (inputVolts / batteryVolts) * (effVolts / motor.R);
+  return retVal;
+}
+
+std::array<units::ampere_t, numModules> SwerveDriveSim::GetDriveCurrentDraw()
+    const {
+  std::array<units::ampere_t, numModules> currents;
+  for (int i = 0; i < numModules; i++) {
+    units::radians_per_second_t speed =
+        units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing /
+        driveWheelRadius.to<double>();
+    currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i],
+                                 frc::RobotController::GetBatteryVoltage());
+  }
+  return currents;
+}
+
+std::array<units::ampere_t, numModules> SwerveDriveSim::GetSteerCurrentDraw()
+    const {
+  std::array<units::ampere_t, numModules> currents;
+  for (int i = 0; i < numModules; i++) {
+    units::radians_per_second_t speed =
+        units::radians_per_second_t{steerStates[i](1, 0) * steerGearing};
+    // TODO: If uncommented we get huge current values.. Not sure how to fix
+    // atm. :(
+    currents[i] = 20_A;
+    // currents[i] = GetCurrentDraw(steerMotor, speed, steerInputs[i],
+    // frc::RobotController::GetBatteryVoltage());
+  }
+  return currents;
+}
+
+units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const {
+  units::ampere_t total{0};
+  for (const auto& val : GetDriveCurrentDraw()) {
+    total += val;
+  }
+  for (const auto& val : GetSteerCurrentDraw()) {
+    total += val;
+  }
+  return total;
+}

photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveModule.cpp

@@ -0,0 +1,146 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "subsystems/SwerveModule.h"
+
+#include <iostream>
+#include <string>
+
+#include <frc/MathUtil.h>
+#include <frc/RobotController.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts)
+    : moduleConstants(consts),
+      driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}),
+      driveEncoder(frc::Encoder{moduleConstants.driveEncoderA,
+                                moduleConstants.driveEncoderB}),
+      steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}),
+      steerEncoder(frc::Encoder{moduleConstants.steerEncoderA,
+                                moduleConstants.steerEncoderB}),
+      driveEncoderSim(driveEncoder),
+      steerEncoderSim(steerEncoder) {
+  driveEncoder.SetDistancePerPulse(
+      constants::Swerve::kDriveDistPerPulse.to<double>());
+  steerEncoder.SetDistancePerPulse(
+      constants::Swerve::kSteerRadPerPulse.to<double>());
+  steerPIDController.EnableContinuousInput(-std::numbers::pi, std::numbers::pi);
+}
+
+void SwerveModule::Periodic() {
+  units::volt_t steerPID = units::volt_t{
+      steerPIDController.Calculate(GetAbsoluteHeading().Radians().to<double>(),
+                                   desiredState.angle.Radians().to<double>())};
+  steerMotor.SetVoltage(steerPID);
+
+  units::volt_t driveFF =
+      constants::Swerve::kDriveFF.Calculate(desiredState.speed);
+  units::volt_t drivePID{0};
+  if (!openLoop) {
+    drivePID = units::volt_t{drivePIDController.Calculate(
+        driveEncoder.GetRate(), desiredState.speed.to<double>())};
+  }
+  driveMotor.SetVoltage(driveFF + drivePID);
+}
+
+void SwerveModule::SetDesiredState(frc::SwerveModuleState newState,
+                                   bool shouldBeOpenLoop, bool steerInPlace) {
+  frc::Rotation2d currentRotation = GetAbsoluteHeading();
+  newState.Optimize(currentRotation);
+  desiredState = newState;
+}
+
+frc::Rotation2d SwerveModule::GetAbsoluteHeading() const {
+  return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}};
+}
+
+frc::SwerveModuleState SwerveModule::GetState() const {
+  return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps,
+                                GetAbsoluteHeading()};
+}
+
+frc::SwerveModulePosition SwerveModule::GetPosition() const {
+  return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m,
+                                   GetAbsoluteHeading()};
+}
+
+units::volt_t SwerveModule::GetDriveVoltage() const {
+  return driveMotor.Get() * frc::RobotController::GetBatteryVoltage();
+}
+
+units::volt_t SwerveModule::GetSteerVoltage() const {
+  return steerMotor.Get() * frc::RobotController::GetBatteryVoltage();
+}
+
+units::ampere_t SwerveModule::GetDriveCurrentSim() const {
+  return driveCurrentSim;
+}
+
+units::ampere_t SwerveModule::GetSteerCurrentSim() const {
+  return steerCurrentSim;
+}
+
+constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const {
+  return moduleConstants;
+}
+
+void SwerveModule::Log() {
+  frc::SwerveModuleState state = GetState();
+
+  std::string table =
+      "Module " + std::to_string(moduleConstants.moduleNum) + "/";
+  frc::SmartDashboard::PutNumber(table + "Steer Degrees",
+                                 frc::AngleModulus(state.angle.Radians())
+                                     .convert<units::degrees>()
+                                     .to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Steer Target Degrees",
+      units::radian_t{steerPIDController.GetSetpoint()}
+          .convert<units::degrees>()
+          .to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Drive Velocity Feet",
+      state.speed.convert<units::feet_per_second>().to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Drive Velocity Target Feet",
+      desiredState.speed.convert<units::feet_per_second>().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Drive Current",
+                                 driveCurrentSim.to<double>());
+  frc::SmartDashboard::PutNumber(table + "Steer Current",
+                                 steerCurrentSim.to<double>());
+}
+
+void SwerveModule::SimulationUpdate(
+    units::meter_t driveEncoderDist,
+    units::meters_per_second_t driveEncoderRate, units::ampere_t driveCurrent,
+    units::radian_t steerEncoderDist,
+    units::radians_per_second_t steerEncoderRate,
+    units::ampere_t steerCurrent) {
+  driveEncoderSim.SetDistance(driveEncoderDist.to<double>());
+  driveEncoderSim.SetRate(driveEncoderRate.to<double>());
+  driveCurrentSim = driveCurrent;
+  steerEncoderSim.SetDistance(steerEncoderDist.to<double>());
+  steerEncoderSim.SetRate(steerEncoderRate.to<double>());
+  steerCurrentSim = steerCurrent;
+}

photonlib-cpp-examples/poseest/src/main/deploy/example.txt

@@ -0,0 +1,4 @@
+Files placed in this directory will be deployed to the RoboRIO into the
+  'deploy' directory in the home folder. Use the 'frc::filesystem::GetDeployDirectory'
+  function from the 'frc/Filesystem.h' header to get a proper path relative to the deploy
+  directory.

photonlib-cpp-examples/poseest/src/main/include/Constants.h

@@ -0,0 +1,125 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <numbers>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/apriltag/AprilTagFields.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/geometry/Transform3d.h>
+#include <frc/geometry/Translation2d.h>
+#include <units/acceleration.h>
+#include <units/angular_acceleration.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <units/velocity.h>
+
+namespace constants {
+namespace Vision {
+inline constexpr std::string_view kCameraName{"YOUR CAMERA NAME"};
+inline const frc::Transform3d kRobotToCam{
+    frc::Translation3d{0.5_m, 0.0_m, 0.5_m},
+    frc::Rotation3d{0_rad, -30_deg, 0_rad}};
+inline const frc::AprilTagFieldLayout kTagLayout{
+    frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo)};
+
+inline const Eigen::Matrix<double, 3, 1> kSingleTagStdDevs{4, 4, 8};
+inline const Eigen::Matrix<double, 3, 1> kMultiTagStdDevs{0.5, 0.5, 1};
+}  // namespace Vision
+namespace Swerve {
+using namespace units;
+
+inline constexpr units::meter_t kTrackWidth{18.5_in};
+inline constexpr units::meter_t kTrackLength{18.5_in};
+inline constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2};
+inline constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2};
+inline constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps};
+inline constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s};
+inline constexpr units::meter_t kWheelDiameter{4_in};
+inline constexpr units::meter_t kWheelCircumference{kWheelDiameter *
+                                                    std::numbers::pi};
+
+inline constexpr double kDriveGearRatio = 6.75;
+inline constexpr double kSteerGearRatio = 12.8;
+
+inline constexpr units::meter_t kDriveDistPerPulse =
+    kWheelCircumference / 1024.0 / kDriveGearRatio;
+inline constexpr units::radian_t kSteerRadPerPulse =
+    units::radian_t{2 * std::numbers::pi} / 1024.0;
+
+inline constexpr double kDriveKP = 1.0;
+inline constexpr double kDriveKI = 0.0;
+inline constexpr double kDriveKD = 0.0;
+
+inline constexpr double kSteerKP = 20.0;
+inline constexpr double kSteerKI = 0.0;
+inline constexpr double kSteerKD = 0.25;
+
+using namespace units;
+
+inline const frc::SimpleMotorFeedforward<units::meters> kDriveFF{
+    0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq};
+
+inline const frc::SimpleMotorFeedforward<units::radians> kSteerFF{
+    0.5_V, 0.25_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq};
+
+struct ModuleConstants {
+ public:
+  const int moduleNum;
+  const int driveMotorId;
+  const int driveEncoderA;
+  const int driveEncoderB;
+  const int steerMotorId;
+  const int steerEncoderA;
+  const int steerEncoderB;
+  const double angleOffset;
+  const frc::Translation2d centerOffset;
+
+  ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA,
+                  int driveEncoderB, int steerMotorId, int steerEncoderA,
+                  int steerEncoderB, double angleOffset, units::meter_t xOffset,
+                  units::meter_t yOffset)
+      : moduleNum(moduleNum),
+        driveMotorId(driveMotorId),
+        driveEncoderA(driveEncoderA),
+        driveEncoderB(driveEncoderB),
+        steerMotorId(steerMotorId),
+        steerEncoderA(steerEncoderA),
+        steerEncoderB(steerEncoderB),
+        angleOffset(angleOffset),
+        centerOffset(frc::Translation2d{xOffset, yOffset}) {}
+};
+
+inline const ModuleConstants FL_CONSTANTS{
+    1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2};
+inline const ModuleConstants FR_CONSTANTS{
+    2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2};
+inline const ModuleConstants BL_CONSTANTS{
+    3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2};
+inline const ModuleConstants BR_CONSTANTS{
+    4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2};
+}  // namespace Swerve
+}  // namespace constants

photonlib-cpp-examples/poseest/src/main/include/Robot.h

@@ -0,0 +1,57 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+
+#include "Vision.h"
+#include "subsystems/GamepieceLauncher.h"
+#include "subsystems/SwerveDrive.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override;
+  void RobotPeriodic() override;
+  void DisabledInit() override;
+  void DisabledPeriodic() override;
+  void DisabledExit() override;
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+  void AutonomousExit() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void TeleopExit() override;
+  void TestInit() override;
+  void TestPeriodic() override;
+  void TestExit() override;
+  void SimulationPeriodic() override;
+
+ private:
+  SwerveDrive drivetrain{};
+  Vision vision{};
+  GamepieceLauncher launcher{};
+  frc::XboxController controller{0};
+};

photonlib-cpp-examples/poseest/src/main/include/Vision.h

@@ -0,0 +1,152 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <photon/PhotonCamera.h>
+#include <photon/PhotonPoseEstimator.h>
+#include <photon/estimation/VisionEstimation.h>
+#include <photon/simulation/VisionSystemSim.h>
+#include <photon/simulation/VisionTargetSim.h>
+#include <photon/targeting/PhotonPipelineResult.h>
+
+#include <limits>
+#include <memory>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/apriltag/AprilTagFields.h>
+
+#include "Constants.h"
+
+class Vision {
+ public:
+  Vision() {
+    photonEstimator.SetMultiTagFallbackStrategy(
+        photon::PoseStrategy::LOWEST_AMBIGUITY);
+
+    if (frc::RobotBase::IsSimulation()) {
+      visionSim = std::make_unique<photon::VisionSystemSim>("main");
+
+      visionSim->AddAprilTags(constants::Vision::kTagLayout);
+
+      cameraProp = std::make_unique<photon::SimCameraProperties>();
+
+      cameraProp->SetCalibration(960, 720, frc::Rotation2d{90_deg});
+      cameraProp->SetCalibError(.35, .10);
+      cameraProp->SetFPS(15_Hz);
+      cameraProp->SetAvgLatency(50_ms);
+      cameraProp->SetLatencyStdDev(15_ms);
+
+      cameraSim =
+          std::make_shared<photon::PhotonCameraSim>(&camera, *cameraProp.get());
+
+      visionSim->AddCamera(cameraSim.get(), constants::Vision::kRobotToCam);
+      cameraSim->EnableDrawWireframe(true);
+    }
+  }
+
+  photon::PhotonPipelineResult GetLatestResult() { return m_latestResult; }
+
+  std::optional<photon::EstimatedRobotPose> GetEstimatedGlobalPose() {
+    std::optional<photon::EstimatedRobotPose> visionEst;
+
+    // Run each new pipeline result through our pose estimator
+    for (const auto& result : camera.GetAllUnreadResults()) {
+      // cache result and update pose estimator
+      auto visionEst = photonEstimator.Update(result);
+      m_latestResult = result;
+
+      // In sim only, add our vision estimate to the sim debug field
+      if (frc::RobotBase::IsSimulation()) {
+        if (visionEst) {
+          GetSimDebugField()
+              .GetObject("VisionEstimation")
+              ->SetPose(visionEst->estimatedPose.ToPose2d());
+        } else {
+          GetSimDebugField().GetObject("VisionEstimation")->SetPoses({});
+        }
+      }
+    }
+
+    return visionEst;
+  }
+
+  Eigen::Matrix<double, 3, 1> GetEstimationStdDevs(frc::Pose2d estimatedPose) {
+    Eigen::Matrix<double, 3, 1> estStdDevs =
+        constants::Vision::kSingleTagStdDevs;
+    auto targets = GetLatestResult().GetTargets();
+    int numTags = 0;
+    units::meter_t avgDist = 0_m;
+    for (const auto& tgt : targets) {
+      auto tagPose =
+          photonEstimator.GetFieldLayout().GetTagPose(tgt.GetFiducialId());
+      if (tagPose) {
+        numTags++;
+        avgDist += tagPose->ToPose2d().Translation().Distance(
+            estimatedPose.Translation());
+      }
+    }
+    if (numTags == 0) {
+      return estStdDevs;
+    }
+    avgDist /= numTags;
+    if (numTags > 1) {
+      estStdDevs = constants::Vision::kMultiTagStdDevs;
+    }
+    if (numTags == 1 && avgDist > 4_m) {
+      estStdDevs = (Eigen::MatrixXd(3, 1) << std::numeric_limits<double>::max(),
+                    std::numeric_limits<double>::max(),
+                    std::numeric_limits<double>::max())
+                       .finished();
+    } else {
+      estStdDevs = estStdDevs * (1 + (avgDist.value() * avgDist.value() / 30));
+    }
+    return estStdDevs;
+  }
+
+  void SimPeriodic(frc::Pose2d robotSimPose) {
+    visionSim->Update(robotSimPose);
+  }
+
+  void ResetSimPose(frc::Pose2d pose) {
+    if (frc::RobotBase::IsSimulation()) {
+      visionSim->ResetRobotPose(pose);
+    }
+  }
+
+  frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); }
+
+ private:
+  photon::PhotonPoseEstimator photonEstimator{
+      constants::Vision::kTagLayout,
+      photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR,
+      constants::Vision::kRobotToCam};
+  photon::PhotonCamera camera{constants::Vision::kCameraName};
+  std::unique_ptr<photon::VisionSystemSim> visionSim;
+  std::unique_ptr<photon::SimCameraProperties> cameraProp;
+  std::shared_ptr<photon::PhotonCameraSim> cameraSim;
+
+  // The most recent result, cached for calculating std devs
+  photon::PhotonPipelineResult m_latestResult;
+};

photonlib-cpp-examples/poseest/src/main/include/subsystems/GamepieceLauncher.h

@@ -0,0 +1,66 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef PHOTONVISION_PHOTONLIB_CPP_EXAMPLES_POSEEST_SRC_MAIN_INCLUDE_SUBSYSTEMS_GAMEPIECELAUNCHER_H_
+#define PHOTONVISION_PHOTONLIB_CPP_EXAMPLES_POSEEST_SRC_MAIN_INCLUDE_SUBSYSTEMS_GAMEPIECELAUNCHER_H_
+
+#include <cmath>
+#include <numbers>
+
+#include <frc/RobotController.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc/simulation/FlywheelSim.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc/system/plant/DCMotor.h>
+#include <frc/system/plant/LinearSystemId.h>
+#include <units/angle.h>
+#include <units/moment_of_inertia.h>
+
+class GamepieceLauncher {
+ public:
+  GamepieceLauncher();              // Constructor
+  void setRunning(bool shouldRun);  // Method to start/stop the launcher
+  void periodic();                  // Method to handle periodic updates
+  void simulationPeriodic();        // Method to handle simulation updates
+
+ private:
+  frc::PWMSparkMax* motor;  // Motor controller
+
+  const double LAUNCH_SPEED_RPM = 2500;
+  double curDesSpd = 0.0;
+  double curMotorCmd = 0.0;
+
+  static constexpr units::kilogram_square_meter_t kFlywheelMomentOfInertia =
+      0.5 * 1.5_lb * 4_in * 4_in;
+
+  frc::DCMotor m_gearbox = frc::DCMotor::Falcon500(1);
+  frc::LinearSystem<1, 1, 1> m_plant{frc::LinearSystemId::FlywheelSystem(
+      m_gearbox, kFlywheelMomentOfInertia, 1.0)};
+
+  frc::sim::FlywheelSim launcherSim{m_plant, m_gearbox};
+
+  void simulationInit();  // Method to initialize simulation components
+};
+
+#endif  // PHOTONVISION_PHOTONLIB_CPP_EXAMPLES_POSEEST_SRC_MAIN_INCLUDE_SUBSYSTEMS_GAMEPIECELAUNCHER_H_

photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDrive.h

@@ -0,0 +1,85 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <frc/ADXRS450_Gyro.h>
+#include <frc/SPI.h>
+#include <frc/estimator/SwerveDrivePoseEstimator.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/simulation/ADXRS450_GyroSim.h>
+
+#include "SwerveDriveSim.h"
+#include "SwerveModule.h"
+
+class SwerveDrive {
+ public:
+  SwerveDrive();
+  void Periodic();
+  void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy,
+             units::radians_per_second_t omega);
+  void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds,
+                        bool openLoop, bool steerInPlace);
+  void SetModuleStates(
+      const std::array<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
+      bool steerInPlace);
+  void Stop();
+  void AddVisionMeasurement(const frc::Pose2d& visionMeasurement,
+                            units::second_t timestamp);
+  void AddVisionMeasurement(const frc::Pose2d& visionMeasurement,
+                            units::second_t timestamp,
+                            const Eigen::Vector3d& stdDevs);
+  void ResetPose(const frc::Pose2d& pose, bool resetSimPose);
+  frc::Pose2d GetPose() const;
+  frc::Rotation2d GetHeading() const;
+  frc::Rotation2d GetGyroYaw() const;
+  frc::ChassisSpeeds GetChassisSpeeds() const;
+  std::array<frc::SwerveModuleState, 4> GetModuleStates() const;
+  std::array<frc::SwerveModulePosition, 4> GetModulePositions() const;
+  std::array<frc::Pose2d, 4> GetModulePoses() const;
+  void Log();
+  void SimulationPeriodic();
+  frc::Pose2d GetSimPose() const;
+  units::ampere_t GetCurrentDraw() const;
+
+ private:
+  std::array<SwerveModule, 4> swerveMods{
+      SwerveModule{constants::Swerve::FL_CONSTANTS},
+      SwerveModule{constants::Swerve::FR_CONSTANTS},
+      SwerveModule{constants::Swerve::BL_CONSTANTS},
+      SwerveModule{constants::Swerve::BR_CONSTANTS}};
+  frc::SwerveDriveKinematics<4> kinematics{
+      swerveMods[0].GetModuleConstants().centerOffset,
+      swerveMods[1].GetModuleConstants().centerOffset,
+      swerveMods[2].GetModuleConstants().centerOffset,
+      swerveMods[3].GetModuleConstants().centerOffset,
+  };
+  frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0};
+  frc::SwerveDrivePoseEstimator<4> poseEstimator;
+  frc::ChassisSpeeds targetChassisSpeeds{};
+
+  frc::sim::ADXRS450_GyroSim gyroSim;
+  SwerveDriveSim swerveDriveSim;
+  units::ampere_t totalCurrentDraw{0};
+};

photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDriveSim.h

@@ -0,0 +1,102 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <random>
+
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/system/LinearSystem.h>
+#include <frc/system/plant/DCMotor.h>
+#include <units/voltage.h>
+
+static constexpr int numModules{4};
+
+class SwerveDriveSim {
+ public:
+  SwerveDriveSim(const frc::SimpleMotorFeedforward<units::meters>& driveFF,
+                 const frc::DCMotor& driveMotor, double driveGearing,
+                 units::meter_t driveWheelRadius,
+                 const frc::SimpleMotorFeedforward<units::radians>& steerFF,
+                 const frc::DCMotor& steerMotor, double steerGearing,
+                 const frc::SwerveDriveKinematics<numModules>& kinematics);
+  SwerveDriveSim(const frc::LinearSystem<2, 1, 2>& drivePlant,
+                 units::volt_t driveKs, const frc::DCMotor& driveMotor,
+                 double driveGearing, units::meter_t driveWheelRadius,
+                 const frc::LinearSystem<2, 1, 2>& steerPlant,
+                 units::volt_t steerKs, const frc::DCMotor& steerMotor,
+                 double steerGearing,
+                 const frc::SwerveDriveKinematics<numModules>& kinematics);
+  void SetDriveInputs(const std::array<units::volt_t, numModules>& inputs);
+  void SetSteerInputs(const std::array<units::volt_t, numModules>& inputs);
+  static Eigen::Matrix<double, 2, 1> CalculateX(
+      const Eigen::Matrix<double, 2, 2>& discA,
+      const Eigen::Matrix<double, 2, 1>& discB,
+      const Eigen::Matrix<double, 2, 1>& x, units::volt_t input,
+      units::volt_t kS);
+  void Update(units::second_t dt);
+  void Reset(const frc::Pose2d& pose, bool preserveMotion);
+  void Reset(const frc::Pose2d& pose,
+             const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
+                 moduleDriveStates,
+             const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
+                 moduleSteerStates);
+  frc::Pose2d GetPose() const;
+  std::array<frc::SwerveModulePosition, numModules> GetModulePositions() const;
+  std::array<frc::SwerveModulePosition, numModules> GetNoisyModulePositions(
+      units::meter_t driveStdDev, units::radian_t steerStdDev);
+  std::array<frc::SwerveModuleState, numModules> GetModuleStates();
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> GetDriveStates() const;
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> GetSteerStates() const;
+  units::radians_per_second_t GetOmega() const;
+  units::ampere_t GetCurrentDraw(const frc::DCMotor& motor,
+                                 units::radians_per_second_t velocity,
+                                 units::volt_t inputVolts,
+                                 units::volt_t batteryVolts) const;
+  std::array<units::ampere_t, numModules> GetDriveCurrentDraw() const;
+  std::array<units::ampere_t, numModules> GetSteerCurrentDraw() const;
+  units::ampere_t GetTotalCurrentDraw() const;
+
+ private:
+  std::random_device rd{};
+  std::mt19937 generator{rd()};
+  std::normal_distribution<double> randDist{0.0, 1.0};
+  const frc::LinearSystem<2, 1, 2> drivePlant;
+  const units::volt_t driveKs;
+  const frc::DCMotor driveMotor;
+  const double driveGearing;
+  const units::meter_t driveWheelRadius;
+  const frc::LinearSystem<2, 1, 2> steerPlant;
+  const units::volt_t steerKs;
+  const frc::DCMotor steerMotor;
+  const double steerGearing;
+  const frc::SwerveDriveKinematics<numModules> kinematics;
+  std::array<units::volt_t, numModules> driveInputs{};
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> driveStates{};
+  std::array<units::volt_t, numModules> steerInputs{};
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> steerStates{};
+  frc::Pose2d pose{frc::Pose2d{}};
+  units::radians_per_second_t omega{0};
+};

photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveModule.h

@@ -0,0 +1,81 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/controller/PIDController.h>
+#include <frc/kinematics/SwerveModulePosition.h>
+#include <frc/kinematics/SwerveModuleState.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc/simulation/EncoderSim.h>
+#include <units/current.h>
+
+#include "Constants.h"
+
+class SwerveModule {
+ public:
+  explicit SwerveModule(const constants::Swerve::ModuleConstants& consts);
+  void Periodic();
+  void SetDesiredState(frc::SwerveModuleState newState, bool shouldBeOpenLoop,
+                       bool steerInPlace);
+  frc::Rotation2d GetAbsoluteHeading() const;
+  frc::SwerveModuleState GetState() const;
+  frc::SwerveModulePosition GetPosition() const;
+  units::volt_t GetDriveVoltage() const;
+  units::volt_t GetSteerVoltage() const;
+  units::ampere_t GetDriveCurrentSim() const;
+  units::ampere_t GetSteerCurrentSim() const;
+  constants::Swerve::ModuleConstants GetModuleConstants() const;
+  void Log();
+  void SimulationUpdate(units::meter_t driveEncoderDist,
+                        units::meters_per_second_t driveEncoderRate,
+                        units::ampere_t driveCurrent,
+                        units::radian_t steerEncoderDist,
+                        units::radians_per_second_t steerEncoderRate,
+                        units::ampere_t steerCurrent);
+
+ private:
+  const constants::Swerve::ModuleConstants moduleConstants;
+
+  frc::PWMSparkMax driveMotor;
+  frc::Encoder driveEncoder;
+  frc::PWMSparkMax steerMotor;
+  frc::Encoder steerEncoder;
+
+  frc::SwerveModuleState desiredState{};
+  bool openLoop{false};
+
+  frc::PIDController drivePIDController{constants::Swerve::kDriveKP,
+                                        constants::Swerve::kDriveKI,
+                                        constants::Swerve::kDriveKD};
+  frc::PIDController steerPIDController{constants::Swerve::kSteerKP,
+                                        constants::Swerve::kSteerKI,
+                                        constants::Swerve::kSteerKD};
+
+  frc::sim::EncoderSim driveEncoderSim;
+  units::ampere_t driveCurrentSim{0};
+  frc::sim::EncoderSim steerEncoderSim;
+  units::ampere_t steerCurrentSim{0};
+};

photonlib-cpp-examples/poseest/src/test/cpp/main.cpp

@@ -0,0 +1,35 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <hal/HAL.h>
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  HAL_Shutdown();
+  return ret;
+}

photonlib-cpp-examples/settings.gradle

@@ -0,0 +1,3 @@
+apply from: "examples.gradle"
+
+exampleFolderNames.each { line -> include line }