From bb499594bf91bc49ec948bf9c1b2b37e465dd62e Mon Sep 17 00:00:00 2001 From: Drew Williams Date: Sun, 26 Nov 2023 14:58:13 -0500 Subject: [PATCH] Swerve Vision Sim C++ Project --- .../swervedriveposeestsim/.gitignore | 1 + .../.wpilib/wpilib_preferences.json | 6 + .../swervedriveposeestsim/WPILib-License.md | 24 ++ .../swervedriveposeestsim/build.gradle | 121 ++++++++ .../swervedriveposeestsim/gradlew | 241 +++++++++++++++ .../swervedriveposeestsim/gradlew.bat | 91 ++++++ .../swervedriveposeestsim/networktables.json | 1 + .../swervedriveposeestsim/settings.gradle | 30 ++ .../swervedriveposeestsim/simgui-ds.json | 98 ++++++ .../swervedriveposeestsim/simgui-window.json | 64 ++++ .../swervedriveposeestsim/simgui.json | 57 ++++ .../src/main/cpp/Robot.cpp | 123 ++++++++ .../src/main/cpp/subsystems/SwerveDrive.cpp | 211 +++++++++++++ .../main/cpp/subsystems/SwerveDriveSim.cpp | 285 ++++++++++++++++++ .../src/main/cpp/subsystems/SwerveModule.cpp | 146 +++++++++ .../src/main/deploy/example.txt | 4 + .../src/main/include/Constants.h | 118 ++++++++ .../src/main/include/Robot.h | 62 ++++ .../src/main/include/Vision.h | 152 ++++++++++ .../src/main/include/subsystems/SwerveDrive.h | 85 ++++++ .../main/include/subsystems/SwerveDriveSim.h | 102 +++++++ .../main/include/subsystems/SwerveModule.h | 81 +++++ .../src/test/cpp/main.cpp | 34 +++ .../swervedriveposeestsim/swerve_module.png | Bin 0 -> 2954 bytes .../swervedriveposeestsim/tag-blue.png | Bin 0 -> 4711 bytes .../swervedriveposeestsim/tag-green.png | Bin 0 -> 4698 bytes 26 files changed, 2137 insertions(+) create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/.gitignore create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/build.gradle create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/gradlew create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/networktables.json create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/settings.gradle create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/simgui.json create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/tag-green.png diff --git a/photonlib-cpp-examples/swervedriveposeestsim/.gitignore b/photonlib-cpp-examples/swervedriveposeestsim/.gitignore new file mode 100644 index 0000000000..34878ab18c --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/.gitignore @@ -0,0 +1 @@ +vendordeps diff --git a/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json b/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000000..a6e62683de --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": true, + "currentLanguage": "cpp", + "projectYear": "2023", + "teamNumber": 5 +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md b/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md new file mode 100644 index 0000000000..3d5a824cad --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/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. diff --git a/photonlib-cpp-examples/swervedriveposeestsim/build.gradle b/photonlib-cpp-examples/swervedriveposeestsim/build.gradle new file mode 100644 index 0000000000..8eeef20126 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/build.gradle @@ -0,0 +1,121 @@ +plugins { + id "cpp" + id "google-test-test-suite" + id "edu.wpi.first.GradleRIO" + + id "com.dorongold.task-tree" version "2.1.0" +} + +wpi.maven.useLocal = false +wpi.maven.useDevelopment = true +wpi.versions.wpilibVersion = '2024.1.1-beta-3-53-g31cd015' +wpi.versions.wpimathVersion = '2024.1.1-beta-3-53-g31cd015' + +repositories { + mavenLocal() + jcenter() +} + +apply from: "${rootDir}/../shared/examples_common.gradle" + +// 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) + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/gradlew b/photonlib-cpp-examples/swervedriveposeestsim/gradlew new file mode 100644 index 0000000000..0ef4c1e860 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/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" "$@" diff --git a/photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat b/photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat new file mode 100644 index 0000000000..f127cfd49d --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/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 diff --git a/photonlib-cpp-examples/swervedriveposeestsim/networktables.json b/photonlib-cpp-examples/swervedriveposeestsim/networktables.json new file mode 100644 index 0000000000..fe51488c70 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/networktables.json @@ -0,0 +1 @@ +[] diff --git a/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle b/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle new file mode 100644 index 0000000000..44fbca7512 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle @@ -0,0 +1,30 @@ +import org.gradle.internal.os.OperatingSystem + +rootProject.name = 'aimattarget' + +pluginManagement { + repositories { + mavenLocal() + jcenter() + gradlePluginPortal() + String frcYear = '2024' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name 'frcHome' + url frcHomeMaven + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json b/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json new file mode 100644 index 0000000000..c4b7efd3d8 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json @@ -0,0 +1,98 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "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": "78696e70757401000000000000000000", + "useGamepad": true + } + ] +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json b/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json new file mode 100644 index 0000000000..1605887709 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json @@ -0,0 +1,64 @@ +{ + "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": "199,200", + "Size": "1342,628" + }, + "###FMS": { + "Collapsed": "0", + "Pos": "5,540", + "Size": "283,146" + }, + "###Joysticks": { + "Collapsed": "0", + "Pos": "359,95", + "Size": "796,240" + }, + "###NetworkTables": { + "Collapsed": "0", + "Pos": "865,52", + "Size": "830,620" + }, + "###Other Devices": { + "Collapsed": "0", + "Pos": "1025,20", + "Size": "250,695" + }, + "###System Joysticks": { + "Collapsed": "0", + "Pos": "5,350", + "Size": "192,218" + }, + "###Timing": { + "Collapsed": "0", + "Pos": "5,150", + "Size": "135,127" + }, + "Debug##Default": { + "Collapsed": "0", + "Pos": "60,60", + "Size": "400,400" + }, + "Robot State": { + "Collapsed": "0", + "Pos": "5,20", + "Size": "92,99" + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui.json b/photonlib-cpp-examples/swervedriveposeestsim/simgui.json new file mode 100644 index 0000000000..e1ba9acce8 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/simgui.json @@ -0,0 +1,57 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" + }, + "windows": { + "/SmartDashboard/VisionSystemSim-main/Sim Field": { + "EstimatedRobotModules": { + "arrows": false, + "image": "swerve_module.png", + "length": 0.30000001192092896, + "width": 0.30000001192092896 + }, + "apriltag": { + "image": "tag-green.png", + "length": 0.6000000238418579, + "width": 0.6000000238418579 + }, + "bottom": 544, + "builtin": "2023 Charged Up", + "cameras": { + "arrowSize": 19, + "arrowWeight": 1.0, + "style": "Hidden" + }, + "height": 8.013679504394531, + "left": 46, + "right": 1088, + "top": 36, + "visibleTargetPoses": { + "image": "tag-blue.png" + }, + "width": 16.541748046875, + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "Drive": { + "open": true + }, + "open": true + }, + "photonvision": { + "open": true, + "photonvision": { + "open": true + } + } + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp new file mode 100644 index 0000000000..4505740955 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp @@ -0,0 +1,123 @@ +/* + * 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 + +#include +#include + +void Robot::RobotInit() {} + +void Robot::RobotPeriodic() { + 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() { + autoTimer.Restart(); + frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}}; + drivetrain.ResetPose(pose, true); +} + +void Robot::AutonomousPeriodic() { + if (autoTimer.Get() < 10_s) { + drivetrain.Drive(0.5_mps, 0.5_mps, 0.5_rad_per_s, false); + } else { + autoTimer.Stop(); + drivetrain.Stop(); + } +} + +void Robot::AutonomousExit() {} + +void Robot::TeleopInit() {} + +void Robot::TeleopPeriodic() { + double forward = -controller.GetLeftY() * kDriveSpeed; + if (std::abs(forward) < 0.1) { + forward = 0; + } + forward = forwardLimiter.Calculate(forward); + + double strafe = -controller.GetLeftX() * kDriveSpeed; + if (std::abs(strafe) < 0.1) { + strafe = 0; + } + strafe = strafeLimiter.Calculate(strafe); + + double turn = -controller.GetRightX() * kDriveSpeed; + if (std::abs(turn) < 0.1) { + turn = 0; + } + turn = turnLimiter.Calculate(turn); + + drivetrain.Drive(forward * constants::Swerve::kMaxLinearSpeed, + strafe * constants::Swerve::kMaxLinearSpeed, + turn * constants::Swerve::kMaxAngularSpeed, true); +} + +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}); + frc::sim::RoboRioSim::SetVInVoltage(loadedBattVolts); +} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp new file mode 100644 index 0000000000..bf33d7189d --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp @@ -0,0 +1,211 @@ +/* + * 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 + +#include +#include + +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, bool openLoop) { + frc::ChassisSpeeds newChassisSpeeds = + frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading()); + SetChassisSpeeds(newChassisSpeeds, openLoop, false); +} + +void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds, + bool openLoop, bool steerInPlace) { + SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), openLoop, + steerInPlace); + this->targetChassisSpeeds = newChassisSpeeds; +} + +void SwerveDrive::SetModuleStates( + const std::array& desiredStates, bool openLoop, + bool steerInPlace) { + std::array desaturatedStates = desiredStates; + frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds( + static_cast*>(&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, true); } + +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 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 SwerveDrive::GetModuleStates() const { + std::array 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 SwerveDrive::GetModulePositions() + const { + std::array 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 SwerveDrive::GetModulePoses() const { + std::array 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()); + frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to()); + frc::SmartDashboard::PutNumber(table + "Heading", + pose.Rotation().Degrees().to()); + frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds(); + frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to()); + frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to()); + frc::SmartDashboard::PutNumber( + table + "Omega Degrees", + chassisSpeeds.omega.convert().to()); + frc::SmartDashboard::PutNumber(table + "Target VX", + targetChassisSpeeds.vx.to()); + frc::SmartDashboard::PutNumber(table + "Target VY", + targetChassisSpeeds.vy.to()); + frc::SmartDashboard::PutNumber( + table + "Target Omega Degrees", + targetChassisSpeeds.omega.convert() + .to()); + + for (auto& module : swerveMods) { + module.Log(); + } +} + +void SwerveDrive::SimulationPeriodic() { + std::array driveInputs; + std::array 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 driveCurrents = + swerveDriveSim.GetDriveCurrentDraw(); + for (const auto& current : driveCurrents) { + totalCurrentDraw += current; + } + std::array 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; } diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp new file mode 100644 index 0000000000..31bbe09e61 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp @@ -0,0 +1,285 @@ +/* + * 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 + +#include +#include + +template +int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} + +SwerveDriveSim::SwerveDriveSim( + const frc::SimpleMotorFeedforward& driveFF, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::SimpleMotorFeedforward& steerFF, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics) + : SwerveDriveSim( + frc::LinearSystem<2, 1, 2>{ + (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, + -driveFF.kV.to() / driveFF.kA.to()) + .finished(), + Eigen::Matrix{0.0, 1.0 / driveFF.kA.to()}, + (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(), + Eigen::Matrix{0.0, 0.0}}, + driveFF.kS, driveMotor, driveGearing, driveWheelRadius, + frc::LinearSystem<2, 1, 2>{ + (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, + -steerFF.kV.to() / steerFF.kA.to()) + .finished(), + Eigen::Matrix{0.0, 1.0 / steerFF.kA.to()}, + (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(), + Eigen::Matrix{0.0, 0.0}}, + steerFF.kS, 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& 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& 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& 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 SwerveDriveSim::CalculateX( + const Eigen::Matrix& discA, + const Eigen::Matrix& discB, + const Eigen::Matrix& 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(), kS.to()); + + nextStateVel += discB(1, 0) * ksSystemEffect; + inputToStop = nextStateVel / -discB(1, 0); + double signToStop = sgn(inputToStop); + double inputSign = sgn(input.to()); + double ksInputEffect = 0; + + if (std::abs(ksSystemEffect) < kS.to()) { + double absInput = std::abs(input.to()); + ksInputEffect = + -std::clamp(kS.to() * inputSign, -absInput, absInput); + } else if ((input.to() * signToStop) > (inputToStop * signToStop)) { + double absInput = std::abs(input.to() - inputToStop); + ksInputEffect = + -std::clamp(kS.to() * inputSign, -absInput, absInput); + } + + auto sF = Eigen::Matrix{input.to() + ksSystemEffect + + ksInputEffect}; + auto Bu = discB * sF; + auto retVal = Ax + Bu; + return retVal; +} + +void SwerveDriveSim::Update(units::second_t dt) { + Eigen::Matrix driveDiscA; + Eigen::Matrix driveDiscB; + frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA, + &driveDiscB); + + Eigen::Matrix steerDiscA; + Eigen::Matrix steerDiscB; + frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA, + &steerDiscB); + + std::array 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{0, 0}; + steerStates[i] = Eigen::Matrix{0, 0}; + } + omega = 0_rad_per_s; + } +} + +void SwerveDriveSim::Reset(const frc::Pose2d& pose, + const std::array, + numModules>& moduleDriveStates, + const std::array, + numModules>& moduleSteerStates) { + this->pose = pose; + driveStates = moduleDriveStates; + steerStates = moduleSteerStates; + omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega; +} + +frc::Pose2d SwerveDriveSim::GetPose() const { return pose; } + +std::array +SwerveDriveSim::GetModulePositions() const { + std::array 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 +SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev, + units::radian_t steerStdDev) { + std::array 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 +SwerveDriveSim::GetModuleStates() { + std::array 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, numModules> +SwerveDriveSim::GetDriveStates() const { + return driveStates; +} + +std::array, 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 SwerveDriveSim::GetDriveCurrentDraw() + const { + std::array 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(); + currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i], + frc::RobotController::GetBatteryVoltage()); + } + return currents; +} + +std::array SwerveDriveSim::GetSteerCurrentDraw() + const { + std::array 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; +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp new file mode 100644 index 0000000000..781c28a338 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/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 + +#include +#include +#include + +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()); + steerEncoder.SetDistancePerPulse( + constants::Swerve::kSteerRadPerPulse.to()); + steerPIDController.EnableContinuousInput(-std::numbers::pi, std::numbers::pi); +} + +void SwerveModule::Periodic() { + units::volt_t steerPID = units::volt_t{ + steerPIDController.Calculate(GetAbsoluteHeading().Radians().to(), + desiredState.angle.Radians().to())}; + 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())}; + } + driveMotor.SetVoltage(driveFF + drivePID); +} + +void SwerveModule::SetDesiredState(const frc::SwerveModuleState& newState, + bool shouldBeOpenLoop, bool steerInPlace) { + frc::Rotation2d currentRotation = GetAbsoluteHeading(); + frc::SwerveModuleState optimizedState = + frc::SwerveModuleState::Optimize(newState, currentRotation); + desiredState = optimizedState; +} + +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() + .to()); + frc::SmartDashboard::PutNumber( + table + "Steer Target Degrees", + units::radian_t{steerPIDController.GetSetpoint()} + .convert() + .to()); + frc::SmartDashboard::PutNumber( + table + "Drive Velocity Feet", + state.speed.convert().to()); + frc::SmartDashboard::PutNumber( + table + "Drive Velocity Target Feet", + desiredState.speed.convert().to()); + frc::SmartDashboard::PutNumber(table + "Drive Current", + driveCurrentSim.to()); + frc::SmartDashboard::PutNumber(table + "Steer Current", + steerCurrentSim.to()); +} + +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()); + driveEncoderSim.SetRate(driveEncoderRate.to()); + driveCurrentSim = driveCurrent; + steerEncoderSim.SetDistance(steerEncoderDist.to()); + steerEncoderSim.SetRate(steerEncoderRate.to()); + steerCurrentSim = steerCurrent; +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt b/photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt new file mode 100644 index 0000000000..15bc5c1ebe --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/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. diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h new file mode 100644 index 0000000000..70be37d406 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h @@ -0,0 +1,118 @@ +/* + * 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 + +#include +#include +#include +#include +#include +#include + +namespace constants { +namespace Vision { +static constexpr std::string_view kCameraName{"YOUR CAMERA NAME"}; +static const frc::Transform3d kRobotToCam{ + frc::Translation3d{0.5_m, 0.0_m, 0.5_m}, + frc::Rotation3d{0_rad, 0_rad, 0_rad}}; +static const frc::AprilTagFieldLayout kTagLayout{ + frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp)}; + +static const Eigen::Matrix kSingleTagStdDevs{4, 4, 8}; +static const Eigen::Matrix kMultiTagStdDevs{0.5, 0.5, 1}; +} // namespace Vision +namespace Swerve { + +static constexpr units::meter_t kTrackWidth{18.5_in}; +static constexpr units::meter_t kTrackLength{18.5_in}; +static constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2}; +static constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2}; +static constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps}; +static constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s}; +static constexpr units::meter_t kWheelDiameter{4_in}; +static constexpr units::meter_t kWheelCircumference{kWheelDiameter * + std::numbers::pi}; + +static constexpr double kDriveGearRatio = 6.75; +static constexpr double kSteerGearRatio = 12.8; + +static constexpr units::meter_t kDriveDistPerPulse = + kWheelCircumference / 1024.0 / kDriveGearRatio; +static constexpr units::radian_t kSteerRadPerPulse = + units::radian_t{2 * std::numbers::pi} / 1024.0; + +static constexpr double kDriveKP = 1.0; +static constexpr double kDriveKI = 0.0; +static constexpr double kDriveKD = 0.0; + +static constexpr double kSteerKP = 20.0; +static constexpr double kSteerKI = 0.0; +static constexpr double kSteerKD = 0.25; + +static const frc::SimpleMotorFeedforward kDriveFF{ + 0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq}; + +static const frc::SimpleMotorFeedforward 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}) {} +}; + +static const ModuleConstants FL_CONSTANTS{ + 1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2}; +static const ModuleConstants FR_CONSTANTS{ + 2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2}; +static const ModuleConstants BL_CONSTANTS{ + 3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2}; +static const ModuleConstants BR_CONSTANTS{ + 4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2}; +} // namespace Swerve +} // namespace constants diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h new file mode 100644 index 0000000000..7a3697494a --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h @@ -0,0 +1,62 @@ +/* + * 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 +#include +#include +#include + +#include "Vision.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{}; + frc::XboxController controller{0}; + frc::SlewRateLimiter forwardLimiter{1.0 / 0.6_s}; + frc::SlewRateLimiter strafeLimiter{1.0 / 0.6_s}; + frc::SlewRateLimiter turnLimiter{1.0 / 0.33_s}; + frc::Timer autoTimer{}; + double kDriveSpeed{0.6}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h new file mode 100644 index 0000000000..619b34ade7 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/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 +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "Constants.h" + +class Vision { + public: + Vision() { + photonEstimator.SetMultiTagFallbackStrategy( + photon::PoseStrategy::LOWEST_AMBIGUITY); + + if (frc::RobotBase::IsSimulation()) { + visionSim = std::make_unique("main"); + + visionSim->AddAprilTags(constants::Vision::kTagLayout); + + cameraProp = std::make_unique(); + + 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(camera.get(), + *cameraProp.get()); + + visionSim->AddCamera(cameraSim.get(), robotToCam); + cameraSim->EnableDrawWireframe(true); + } + } + + photon::PhotonPipelineResult GetLatestResult() { + return camera->GetLatestResult(); + } + + std::optional GetEstimatedGlobalPose() { + auto visionEst = photonEstimator.Update(); + units::second_t latestTimestamp = camera->GetLatestResult().GetTimestamp(); + bool newResult = + units::math::abs(latestTimestamp - lastEstTimestamp) > 1e-5_s; + if (frc::RobotBase::IsSimulation()) { + if (visionEst.has_value()) { + GetSimDebugField() + .GetObject("VisionEstimation") + ->SetPose(visionEst.value().estimatedPose.ToPose2d()); + } else { + if (newResult) { + GetSimDebugField().GetObject("VisionEstimation")->SetPoses({}); + } + } + } + if (newResult) { + lastEstTimestamp = latestTimestamp; + } + return visionEst; + } + + Eigen::Matrix GetEstimationStdDevs(frc::Pose2d estimatedPose) { + Eigen::Matrix 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.has_value()) { + numTags++; + avgDist += tagPose.value().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::max(), + std::numeric_limits::max(), + std::numeric_limits::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: + frc::Transform3d robotToCam{frc::Translation3d{0.5_m, 0.5_m, 0.5_m}, + frc::Rotation3d{}}; + photon::PhotonPoseEstimator photonEstimator{ + LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp), + photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR, + photon::PhotonCamera{"photonvision"}, robotToCam}; + std::shared_ptr camera{photonEstimator.GetCamera()}; + std::unique_ptr visionSim; + std::unique_ptr cameraProp; + std::shared_ptr cameraSim; + units::second_t lastEstTimestamp{0_s}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h new file mode 100644 index 0000000000..1e3d26b32f --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/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 +#include +#include +#include +#include + +#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, bool openLoop); + void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds, + bool openLoop, bool steerInPlace); + void SetModuleStates( + const std::array& 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 GetModuleStates() const; + std::array GetModulePositions() const; + std::array GetModulePoses() const; + void Log(); + void SimulationPeriodic(); + frc::Pose2d GetSimPose() const; + units::ampere_t GetCurrentDraw() const; + + private: + std::array 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}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h new file mode 100644 index 0000000000..c1cee3e6f1 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/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 + +#include +#include +#include +#include +#include + +static constexpr int numModules{4}; + +class SwerveDriveSim { + public: + SwerveDriveSim(const frc::SimpleMotorFeedforward& driveFF, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::SimpleMotorFeedforward& steerFF, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& 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& kinematics); + void SetDriveInputs(const std::array& inputs); + void SetSteerInputs(const std::array& inputs); + static Eigen::Matrix CalculateX( + const Eigen::Matrix& discA, + const Eigen::Matrix& discB, + const Eigen::Matrix& 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, numModules>& + moduleDriveStates, + const std::array, numModules>& + moduleSteerStates); + frc::Pose2d GetPose() const; + std::array GetModulePositions() const; + std::array GetNoisyModulePositions( + units::meter_t driveStdDev, units::radian_t steerStdDev); + std::array GetModuleStates(); + std::array, numModules> GetDriveStates() const; + std::array, 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 GetDriveCurrentDraw() const; + std::array GetSteerCurrentDraw() const; + units::ampere_t GetTotalCurrentDraw() const; + + private: + std::random_device rd{}; + std::mt19937 generator{rd()}; + std::normal_distribution 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 kinematics; + std::array driveInputs{}; + std::array, numModules> driveStates{}; + std::array steerInputs{}; + std::array, numModules> steerStates{}; + frc::Pose2d pose{frc::Pose2d{}}; + units::radians_per_second_t omega{0}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h new file mode 100644 index 0000000000..444e4bdf44 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/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 +#include +#include +#include +#include +#include +#include + +#include "Constants.h" + +class SwerveModule { + public: + explicit SwerveModule(const constants::Swerve::ModuleConstants& consts); + void Periodic(); + void SetDesiredState(const 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}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp new file mode 100644 index 0000000000..031d1ce96b --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/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 + +#include "gtest/gtest.h" + +int main(int argc, char** argv) { + HAL_Initialize(500, 0); + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png b/photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png new file mode 100644 index 0000000000000000000000000000000000000000..25990c8399b37db4290551cf5b746d1b63108f8a GIT binary patch literal 2954 zcmd6p{Xf&|AIGoHHn!aB)S}Y1bA&p%>4uTmMoM);s1TuY8W_zn6QXs42#)=`~;1rCWD8q*!PxsLKU0@H?Y5@EYiwelObj0E2NW4!F4B+0)|)KJ|06ZmNpI-z#n;xRvR zqWzDSmskxwJ)p-%`D}!74j)?#?IN6*=WGAo(oW;-g>hgGD|^P3 zs`SBIPOW2J6=xTWgHR%1XqvVQoWI+xq`d!<_vG95z4yK0&!Obq&hwj`1wXdjKP3B z4_0z8tohEnou*{?3ncZ~n5U@%sY^CBw$U_0!A_CtQvC4xkm|Y(z#*~RDH!kM3Jj9U zUC*G_X#$U=QRhDU>_lL`;BK>RkoXZ|EV?_d!Dah_z@1xz1e09A#ig*m~F$k)s4&IKFaLNj?W7@?27S zY_L~=lc-1cwub#zG!~tdR_yUS-b(GM`C>yJx+WGpKkgl}!U`)<(u6VbUdpGU5SI}d zEN6Dt!OROMI@H;slBFp}!h@#q(>DgsKt5c00NY_;3pH# ztRoEcetiBj9$uqJ{Yn?-qLM$$BD7wNq&J6Hf*5hQXV1&R-jtdsaB-Q%P3e9Ux7#ER z*~yfoVH|dkbu^dhQ0p;9_;zcb_iD`p>UjlcF(&Rv*@;s}8`bm6;3mRW^YS5N#t@Xh z;+xZ+hEwMQeVl2=nGOd&{cEJM0zSd{2 z!fr)@Kkd^nqndSQCpLsfBRTW*rOXcso*y7LQOmB<(Ka{K>Gc3jsV>}!rftXq9u&8s zx?9aJbf_;dJB)+pZWwV&)tV>NC!Ulmo3r56iz&7H4No^{JUxeqJ;y@^O$7hws+o0& zIRT<7q~q*A5uV1u`~f4*fPB-HhB9UIU%`bfdoa;iMB3Z?UDv{(5D6}EYzS3206h`X z=j}3!`1RCldqD-^`?o?PPMLh)l6i%)c^_mh#t|yW&_PT2R+I_oLq9`OuxHzYV`oteF zLT(ZguEArdUS&XAw!kSj(KIm((J8Tw6J$cHy_){XF>*ocG*TdMwteSjIClUBqU%)wpU`vr!O~w#n91&0s1$A@ zwe?%JqUr$GkWdMIuieZ8CSj9G4=$LpFqvLp?7dhQR}r6?Ci3Htaj$sX7sE7q&d(@< z$q_Wzt_K)rVEe?(HPfNN>7^xKjdn1&cX4~j5Z?+$`^P}y4jk!}gjf^`=@ zN$6vchGDX|3hj|yrf`O$MFz}bRqmE#SZMF!AgD%COJ#5q1b&7SOCFC&!}WnD(xyeR zm(n<+)~p3m`;PYuVGKVN>a*!!X=@`eQ6wglYo)#?kv?*FZ{jA@xl56V6Vyq41ArEs zfsOBzR!=q!@vA>SeXrM!pDGxw<8H<7zPQXZ6}2^iPk7ikjDbKpNUQOH=7;@0stLsP0al&%!f2eK#O@>I~TnpB|zuxrH2~b-Ua-w5waCdfgm6>|dTtNRT zOX`s9p7dWCnF<(&*2LWll}fvhrE1tk zPq1TlAYK}pM+Vj#z&3OK5T*YGr18W(`93AA2gp!s_>&wp85?d9R|uks;O?irjR;SJH1_@5>TTBIR$WMA8qT@o@oWa)RY$qJlj*Cgy7jqC?^xAOEJh_I?y@k6 zU>e^$K-hC0AaFovcueFg04N)fRO8B{+rh9XC_m;K`*p~X=1?B;YEL?bC|v70;{QQ3 z_jDbs)Zhyc-oo3lpYw8{` z!8E)g>rUNR<|@MdEvKB@Cq%eAa$0-aO@ymob!8qRU*0LI*H`3vLUjSMgYdPgHeUPD z+S$Qa^$HIW?v$Ku=rk=2<%LLa7{A^D$^h78oyFtO*(t7NkzQwB8}O9tHCm3e)e`Cy9P- zbPV1maqh&HEO>oO z(7|)lxPIu%us(P^PwKp?h|yO4fzRg=g4o9wPa&EiGGgg&VtveZC>%!0R!^mDfx;C@ z*+!`pZ73Xyr|rp~eV`Aj%6`o+?oQe#d*e0Gf-Gh#a6k{M(pEBFp%3)TR8j=0V)BZA zk{5OwtuQPpYzn1Vdv9BgLs9>pP~%4YYkUOks8mmht=g*bSjpO8Sz!(J_YkB_h6=Cn o+ow(@pSLe?B8BF7pj(6~s>?;ov@y`RA$JBR&MHa@if05Kai!2kdN literal 0 HcmV?d00001 diff --git a/photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png b/photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png new file mode 100644 index 0000000000000000000000000000000000000000..04b9e4f7b7241c94a87249c24fb09c2577b2cf44 GIT binary patch literal 4711 zcmeHL`BPI@6uy8KL`Weqf;O_r;;<=N1zgHf6rvI=C?--RfQSo_M3w-8rcOr_vEWE? z83-NQ0%HxENFvITK^Bq4b{w#p7?~hg1VIc1WRaemK>H80{V6|W&U^2E_nhzC`_6aX zjbIPW-`H@aAwtNQ8sHaxE-pV>SB!(F;Vy4iC{BwiC+C(?cnn+J4 z=~|}*Iqum`u{c(=U9f^de>ycFnNIy>ugiMoaC09SnGruQoMENu zA`=@WLTCLb$omps4;c`Fr;iN*Sp_uephW@?#IrKhM<%vfkah{QZ-KTDv|8~0%c0wZ zmm)UYQXJGh7@ww_-{02TJC;?Qdgiqx<3`cZ>9Kcz$}G=V#>(qYO{c0RC*Ms?d+CeL zQ7I^KhR6<+J+L0(_1!!-dyXoaC<%~x%bK5$jZ*Cs1|x|2$k?*z^;f5I zst3L;gceZl9V%*bP$K4y@uHKF@!vpuaL&ILW)HbLRhT6_SrQE`BD8sL{0bNmT_T_O zfv?9SlM)WXV|KVJ?j2s&A0_cUP>)%t$QYh_JiL&k05JReoARyDR(d2N`fGo{J+|-f z{}QZ&EE9sCAaRg%T}ZXe6l^BFLU9h7Qm1eG!K>IFU)x_qa`geUS%f2(0dDV_J9W2V zY?Gi`h39q+HOUhj)&SA3g*WAGIFh}H9=-$SxJNf_#Xb9I0-u{j*^pxO?YB-)<$| zOld<$22kL+*%0W0>gIgDUNhf=8Qq^8-YJCc9dk{V;hE#Xyhx>#M<&pL0rMhX@9r^R zi!UYzmQVD_1$rpuMwtBS27zAlwMHFEVhQ<{+7It1*V@P`+G2Ejb^2n>|zmaC#*!d)S zjuT4gB_TTK$ndWhx@-yNQAB+<>Hb2*yxYL$KmAI%HqB*uy$l@Dp{e>tIcfUR+}PeY zO<6gqHv#Rhv}A3uA(*dLqS8nbn>#%!?xj(X)F`f)6|`7>g-Ti1Y)j~|A(YTQg*{WJ z|M)w}D$VSegxI*LWq7ILUvVcCS#Fv|H`(c%YPS-dzBl$}%tRB@T`(VA`sm<^d4Du^$0=5e z2F%N??ppv$cecAMM0Z+uzG@&u&7`9{f9|)4_StOrGK;#?D&hSft7$LC)o)Q=*#!5r z%Ps<|y>M6Zpj2@DzXlg8oV=sG?shBf2ud!APp=-`5jBUh;o zi$u*tWgM=&%>Cg)3-~1EH63`Q8FKEt+^ZS7J(i-@1)HJ3mqj_zQ10H(S3<(L9*>o1 zeCKa$k!3-}t{*RAJ!wtDVaL1cHgEI8Rc)szum{%+BC7=tgksDRJe|2*L<_>y@<^;= zEF&FsVU}7>7{WRzDO9?nMwpGQZ;cR3k?V2R)FvUDfF;P_){{2SOesSk88iX=j4v5f z%hy|=NpcN@$PYv`7S7Ir#FBul@^bi+R1B@enULbCFyzk=cIJTR0$WA@F1VkINZNb` z+)uW?xr6ml=>n=MeI$^mMYzg6BagNQqLi->EpD>{rM7F7-G)r)I-}yPFl~7(CRn}< zUp{=3_yN@G^BK==*+kYL$9>!UyWJwMsaMJB_CSA0Pn(>2n?2CGRy-k58 zhlpkmzZW0+cmV4{$5pCa`C=}mG9&%>nvcuk2m3M!ph+q_C6g?Ig4G@(v`e5pN@@?v k+Jo}Hd{91lsCH^;si4Yp)^TGd{5gcE+i8BKzRa_K0jU-?_5c6? literal 0 HcmV?d00001 diff --git a/photonlib-cpp-examples/swervedriveposeestsim/tag-green.png b/photonlib-cpp-examples/swervedriveposeestsim/tag-green.png new file mode 100644 index 0000000000000000000000000000000000000000..63029fcf205d2a1b52eb450aa1e26fbdfd310586 GIT binary patch literal 4698 zcmeHLYg3a~5I*5jKx+ge)e0&gii(5a6%bGmxikcdU<|@&07sk_2r+;_t4$FlTolv+ zLJ^(TkqjkV6#{Ze#3)q>+DXCEM4%CpMhuogNfFWR2{8QwZ9nBhX5Vx6d3T@Pb9T>p zlk<77|Dpve79fNc1qS%;N63JQkm1LsAHj@=-}q$^1lE3kA5_z0t%iY7oHxZAq56V_ z@1l)iY?c%d!9s}GiGKvk^`Eg3vUnBf>m8nUSR?biL^`;P9jzprnN`{yT`j)3bji&n zq+iLl$5s<7?d+31ioFEuqqoH*`|qAf-5pZ&g1Y~Lf+`R3G$R_q-!ONX&=hZlhZum`}dj;CRSR;k<)Syudg#^ zMg|S3ckPMD{j_OwwMHcwjV7fD%et%czNsd}1}T(}>=Q3GecN=tQT_aSkg^X$R}6$a zzCO8*B{d_jRUrDed|F9}m>}}eP)5XZ12`6Ft}cQgI47tvo0S!$hj29T^DE_BIH0k0 z?DK*%!*QMRHcSt7MBy`|V{6f%C;HG8Z{OLLcVn3zc7j zv3Vqx$^m3me_o3^D-?(}yN9)?NZ^S=tM|g55+CNKp&jj7fBlSOuC*eV2GAzy$5qjT+iMYgD&@99f?WkOReAidxIABHq-y7ttAA-E!fn zbglYLiJC;NJAXbQEoimdb%}Q5xpMsbzsfC`qU9DQn*v;vzShouV#31k?~b;TOpx=2 zXC+&64DWMcYNyt+ieq@6<4RwvT*0yab6tK4qIGzJ?ZZp&Ur>Sb4YexS98@^ao|;zMBbpWans#s)jyrR{7tah0eJ zE|%^>HAk%&WnI}JwZb-YU93slr^lNh$vBZ4cMwhrRx@^u!R@L0N$T54nkbQ0vS=A4lJO@&t4C_q4k_yyE z`*B4!o$FQG;z~XArOviZoCL28V&}K3WyuYPLB(xv)FlE6JV(%ltF%}wV`4=S#&qJ> zxyDseDNq|yie+ltSGH1l&#_FQ@*eB@iUmW3%~qK+RL~&VdB2r)P#mtDff$@RU%b}! zBC0~;j^Ej#&}4x8Gal2-BIDNLTqV^-_dqPu;4WydNLYm-_Bu!;Zit~&-UQsb+5S0# zDLfA02dkh3qrr&sRA9EJ%@|&!xsCS$h<*cS&}=O*r`~^&{3)2?XQf4c0)5BPW}q0h zjwn+@8LS```@o|?M7h&7jv@QpZk{Ssk>CkvqxduNR>!4X*EoVVko70yzYA?02Kq)5 zGYqaMC#tg5D_Ij`WrLGvTR*%#Hk~F+pV=Vqd$epN^cE#qd}kw#XQ2OB(0?z%_SAne l>QkWK1^;Rx47a6?FD6iOngjQS4#WRJC~#k}Z;cN<^KVFxHsJsO literal 0 HcmV?d00001