diff --git a/photonlib-java-examples/apriltagExample/.gitignore b/photonlib-java-examples/apriltagExample/.gitignore new file mode 100644 index 000000000..9535c8303 --- /dev/null +++ b/photonlib-java-examples/apriltagExample/.gitignore @@ -0,0 +1,162 @@ +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +# DO NOT REMOVE .classpath and .project +.classpath +.project +.settings/ +bin/ + +# Simulation GUI and other tools window save file +*-window.json diff --git a/photonlib-java-examples/apriltagExample/.wpilib/wpilib_preferences.json b/photonlib-java-examples/apriltagExample/.wpilib/wpilib_preferences.json new file mode 100644 index 000000000..66deed1a9 --- /dev/null +++ b/photonlib-java-examples/apriltagExample/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2023Beta", + "teamNumber": 1736 +} diff --git a/photonlib-java-examples/apriltagExample/WPILib-License.md b/photonlib-java-examples/apriltagExample/WPILib-License.md new file mode 100644 index 000000000..3d5a824ca --- /dev/null +++ b/photonlib-java-examples/apriltagExample/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-java-examples/apriltagExample/build.gradle b/photonlib-java-examples/apriltagExample/build.gradle new file mode 100644 index 000000000..aed88180f --- /dev/null +++ b/photonlib-java-examples/apriltagExample/build.gradle @@ -0,0 +1,92 @@ +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7" +} + +sourceCompatibility = JavaVersion.VERSION_11 +targetCompatibility = JavaVersion.VERSION_11 + +def ROBOT_MAIN_CLASS = "frc.robot.Main" + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project DeployUtils. +deploy { + targets { + roborio(getTargetTypeClass('RoboRIO')) { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = project.frc.getTeamNumber() + debug = project.frc.getDebugOrDefault(false) + + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string + + frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + } + } + } + } +} + +def deployArtifact = deploy.targets.roborio.artifacts.frcJava + +// Set to true to use debug for JNI. +wpi.java.debugJni = false + +// Set this to true to enable desktop support. +def includeDesktopSupport = true + +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 4. +dependencies { + implementation wpi.java.deps.wpilib() + implementation wpi.java.vendor.java() + + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) + roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + + roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + + nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) + nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) + simulationDebug wpi.sim.enableDebug() + + nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) + nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) + simulationRelease wpi.sim.enableRelease() + + testImplementation 'junit:junit:4.13.1' +} + +// Simulation configuration (e.g. environment variables). +wpi.sim.addGui().defaultEnabled = true +wpi.sim.addDriverstation() + +// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') +// in order to make them all available at runtime. Also adding the manifest so WPILib +// knows where to look for our Robot Class. +jar { + from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) + duplicatesStrategy = DuplicatesStrategy.INCLUDE +} + +// Configure jar and deploy tasks +deployArtifact.jarTask = jar +wpi.java.configureExecutableTasks(jar) +wpi.java.configureTestTasks(test) + +// Configure string concat to always inline compile +tasks.withType(JavaCompile) { + options.compilerArgs.add '-XDstringConcat=inline' +} diff --git a/photonlib-java-examples/apriltagExample/gradle/wrapper/gradle-wrapper.jar b/photonlib-java-examples/apriltagExample/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 000000000..249e5832f Binary files /dev/null and b/photonlib-java-examples/apriltagExample/gradle/wrapper/gradle-wrapper.jar differ diff --git a/photonlib-java-examples/apriltagExample/gradle/wrapper/gradle-wrapper.properties b/photonlib-java-examples/apriltagExample/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 000000000..c23a1b317 --- /dev/null +++ b/photonlib-java-examples/apriltagExample/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,5 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-7.5.1-bin.zip +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/photonlib-java-examples/apriltagExample/gradlew b/photonlib-java-examples/apriltagExample/gradlew new file mode 100644 index 000000000..a69d9cb6c --- /dev/null +++ b/photonlib-java-examples/apriltagExample/gradlew @@ -0,0 +1,240 @@ +#!/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 + +APP_HOME=$( cd "${APP_HOME:-./}" && 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-java-examples/apriltagExample/gradlew.bat b/photonlib-java-examples/apriltagExample/gradlew.bat new file mode 100644 index 000000000..f127cfd49 --- /dev/null +++ b/photonlib-java-examples/apriltagExample/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-java-examples/apriltagExample/settings.gradle b/photonlib-java-examples/apriltagExample/settings.gradle new file mode 100644 index 000000000..48c039ed8 --- /dev/null +++ b/photonlib-java-examples/apriltagExample/settings.gradle @@ -0,0 +1,27 @@ +import org.gradle.internal.os.OperatingSystem + +pluginManagement { + repositories { + mavenLocal() + gradlePluginPortal() + String frcYear = '2023' + 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-java-examples/apriltagExample/simgui-ds.json b/photonlib-java-examples/apriltagExample/simgui-ds.json new file mode 100644 index 000000000..69b1a3cbc --- /dev/null +++ b/photonlib-java-examples/apriltagExample/simgui-ds.json @@ -0,0 +1,97 @@ +{ + "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": "Keyboard0" + } + ] +} diff --git a/photonlib-java-examples/apriltagExample/simgui.json b/photonlib-java-examples/apriltagExample/simgui.json new file mode 100644 index 000000000..ec51dae2b --- /dev/null +++ b/photonlib-java-examples/apriltagExample/simgui.json @@ -0,0 +1,34 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Field": "Field2d" + }, + "windows": { + "/SmartDashboard/Field": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "Shuffleboard": { + "open": true + }, + "photonvision": { + "OV9281": { + "open": true + }, + "USB_Camera": { + "open": true + }, + "open": true, + "testCamera": { + "open": true + } + } + } + } +} diff --git a/photonlib-java-examples/apriltagExample/src/main/deploy/example.txt b/photonlib-java-examples/apriltagExample/src/main/deploy/example.txt new file mode 100644 index 000000000..d6ec5cf83 --- /dev/null +++ b/photonlib-java-examples/apriltagExample/src/main/deploy/example.txt @@ -0,0 +1,3 @@ +Files placed in this directory will be deployed to the RoboRIO into the +'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function +to get a proper path relative to the deploy directory. diff --git a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Constants.java new file mode 100644 index 000000000..7f2e7975b --- /dev/null +++ b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Constants.java @@ -0,0 +1,57 @@ +/* + * MIT License + * + * Copyright (c) 2022 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. + */ + +package frc.robot; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; + +public class Constants { + static class DriveTrainConstants { + static final double kMaxSpeed = 3.0; // meters per second + static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second + static final double kTrackWidth = 0.381 * 2; // meters + static final double kWheelRadius = 0.0508; // meters + static final int kEncoderResolution = 4096; + static final double distancePerPulse = 2 * Math.PI * kWheelRadius / (double) kEncoderResolution; + } + + static class FieldConstants { + static final double length = Units.feetToMeters(54); + static final double width = Units.feetToMeters(27); + } + + static class VisionConstants { + static final Transform3d robotToCam = + new Transform3d( + new Translation3d(0.5, 0.0, 0.5), + new Rotation3d( + 0, 0, + 0)); // Cam mounted facing forward, half a meter forward of center, half a meter up + // from center. + static final String cameraName = "YOUR CAMERA NAME"; + } +} diff --git a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java new file mode 100644 index 000000000..d22f95ac5 --- /dev/null +++ b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java @@ -0,0 +1,201 @@ +/* + * MIT License + * + * Copyright (c) 2022 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. + */ + +package frc.robot; + +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; +import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj.simulation.AnalogGyroSim; +import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; +import edu.wpi.first.wpilibj.simulation.EncoderSim; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants.DriveTrainConstants; + +/** Represents a differential drive style drivetrain. */ +public class Drivetrain { + private final MotorController m_leftLeader = new PWMSparkMax(1); + private final MotorController m_leftFollower = new PWMSparkMax(2); + private final MotorController m_rightLeader = new PWMSparkMax(3); + private final MotorController m_rightFollower = new PWMSparkMax(4); + + private final Encoder m_leftEncoder = new Encoder(0, 1); + private final Encoder m_rightEncoder = new Encoder(2, 3); + + private final MotorControllerGroup m_leftGroup = + new MotorControllerGroup(m_leftLeader, m_leftFollower); + private final MotorControllerGroup m_rightGroup = + new MotorControllerGroup(m_rightLeader, m_rightFollower); + + private final AnalogGyro m_gyro = new AnalogGyro(0); + + private final PIDController m_leftPIDController = new PIDController(1, 0, 0); + private final PIDController m_rightPIDController = new PIDController(1, 0, 0); + + private final DifferentialDriveKinematics m_kinematics = + new DifferentialDriveKinematics(Constants.DriveTrainConstants.kTrackWidth); + + public PhotonCameraWrapper pcw; + + /* + * Here we use DifferentialDrivePoseEstimator so that we can fuse odometry + * readings. The + * numbers used below are robot specific, and should be tuned. + */ + private final DifferentialDrivePoseEstimator m_poseEstimator = + new DifferentialDrivePoseEstimator( + m_kinematics, m_gyro.getRotation2d(), 0.0, 0.0, new Pose2d()); + + // Gains are for example purposes only - must be determined for your own robot! + private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3); + + // Simulation classes help us simulate our robot + private final AnalogGyroSim m_gyroSim = new AnalogGyroSim(m_gyro); + private final EncoderSim m_leftEncoderSim = new EncoderSim(m_leftEncoder); + private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder); + private final Field2d m_fieldSim = new Field2d(); + private final LinearSystem m_drivetrainSystem = + LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); + private final DifferentialDrivetrainSim m_drivetrainSimulator = + new DifferentialDrivetrainSim( + m_drivetrainSystem, + DCMotor.getCIM(2), + 8, + DriveTrainConstants.kTrackWidth, + DriveTrainConstants.kWheelRadius, + null); + + /** + * Constructs a differential drive object. Sets the encoder distance per pulse and resets the + * gyro. + */ + public Drivetrain() { + pcw = new PhotonCameraWrapper(); + + m_gyro.reset(); + + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_rightGroup.setInverted(true); + + // Set the distance per pulse for the drive encoders. We can simply use the + // distance traveled for one rotation of the wheel divided by the encoder + // resolution. + m_leftEncoder.setDistancePerPulse(DriveTrainConstants.distancePerPulse); + m_rightEncoder.setDistancePerPulse(DriveTrainConstants.distancePerPulse); + + m_leftEncoder.reset(); + m_rightEncoder.reset(); + + SmartDashboard.putData("Field", m_fieldSim); + } + + /** + * Sets the desired wheel speeds. + * + * @param speeds The desired wheel speeds. + */ + public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { + final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond); + final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond); + + final double leftOutput = + m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond); + final double rightOutput = + m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond); + m_leftGroup.setVoltage(leftOutput + leftFeedforward); + m_rightGroup.setVoltage(rightOutput + rightFeedforward); + } + + /** + * Drives the robot with the given linear velocity and angular velocity. + * + * @param xSpeed Linear velocity in m/s. + * @param rot Angular velocity in rad/s. + */ + public void drive(double xSpeed, double rot) { + var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0.0, rot)); + setSpeeds(wheelSpeeds); + } + + /** Update our simulation. This should be run every robot loop in simulation. */ + public void simulationPeriodic() { + // To update our simulation, we set motor voltage inputs, update the + // simulation, and write the simulated positions and velocities to our + // simulated encoder and gyro. We negate the right side so that positive + // voltages make the right side move forward. + m_drivetrainSimulator.setInputs( + m_leftGroup.get() * RobotController.getInputVoltage(), + m_rightGroup.get() * RobotController.getInputVoltage()); + m_drivetrainSimulator.update(0.02); + + m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters()); + m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond()); + m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters()); + m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond()); + m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees()); + } + + /** Updates the field-relative position. */ + public void updateOdometry() { + m_poseEstimator.update( + m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); + + // Also apply vision measurements. We use 0.3 seconds in the past as an example + // -- on + // a real robot, this must be calculated based either on latency or timestamps. + Pair result = + pcw.getEstimatedGlobalPose(m_poseEstimator.getEstimatedPosition()); + var camPose = result.getFirst(); + var camPoseObsTime = result.getSecond(); + if (camPose != null) { + m_poseEstimator.addVisionMeasurement(camPose, camPoseObsTime); + m_fieldSim.getObject("Cam Est Pos").setPose(camPose); + } else { + // move it way off the screen to make it disappear + m_fieldSim.getObject("Cam Est Pos").setPose(new Pose2d(-100, -100, new Rotation2d())); + } + + m_fieldSim.getObject("Actual Pos").setPose(m_drivetrainSimulator.getPose()); + m_fieldSim.setRobotPose(m_poseEstimator.getEstimatedPosition()); + } +} diff --git a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Main.java b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Main.java new file mode 100644 index 000000000..d3f421e9d --- /dev/null +++ b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Main.java @@ -0,0 +1,45 @@ +/* + * MIT License + * + * Copyright (c) 2022 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. + */ + +package frc.robot; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java new file mode 100644 index 000000000..8cca92504 --- /dev/null +++ b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java @@ -0,0 +1,103 @@ +/* + * MIT License + * + * Copyright (c) 2022 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. + */ + +package frc.robot; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants.FieldConstants; +import frc.robot.Constants.VisionConstants; +import java.util.ArrayList; +import java.util.Optional; +import org.photonvision.PhotonCamera; +import org.photonvision.RobotPoseEstimator; +import org.photonvision.RobotPoseEstimator.PoseStrategy; + +public class PhotonCameraWrapper { + public PhotonCamera photonCamera; + public RobotPoseEstimator robotPoseEstimator; + + public PhotonCameraWrapper() { + // Set up a test arena of two apriltags at the center of each driver station set + final AprilTag tag18 = + new AprilTag( + 18, + new Pose3d( + new Pose2d( + FieldConstants.length, + FieldConstants.width / 2.0, + Rotation2d.fromDegrees(180)))); + final AprilTag tag01 = + new AprilTag( + 01, + new Pose3d(new Pose2d(0.0, FieldConstants.width / 2.0, Rotation2d.fromDegrees(0.0)))); + ArrayList atList = new ArrayList(); + atList.add(tag18); + atList.add(tag01); + + // TODO - once 2023 happens, replace this with just loading the 2023 field arrangement + AprilTagFieldLayout atfl = + new AprilTagFieldLayout(atList, FieldConstants.length, FieldConstants.width); + + // Forward Camera + photonCamera = + new PhotonCamera( + VisionConstants + .cameraName); // Change the name of your camera here to whatever it is in the + // PhotonVision UI. + + // ... Add other cameras here + + // Assemble the list of cameras & mount locations + var camList = new ArrayList>(); + camList.add(new Pair(photonCamera, VisionConstants.robotToCam)); + + robotPoseEstimator = + new RobotPoseEstimator(atfl, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, camList); + } + + /** + * @param estimatedRobotPose The current best guess at robot pose + * @return A pair of the fused camera observations to a single Pose2d on the field, and the time + * of the observation. Assumes a planar field and the robot is always firmly on the ground + */ + public Pair getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) { + robotPoseEstimator.setReferencePose(prevEstimatedRobotPose); + + double currentTime = Timer.getFPGATimestamp(); + Optional> result = robotPoseEstimator.update(); + if (result.isPresent()) { + return new Pair( + result.get().getFirst().toPose2d(), currentTime - result.get().getSecond()); + } else { + return new Pair(null, 0.0); + } + } +} diff --git a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Robot.java new file mode 100644 index 000000000..3a258bc42 --- /dev/null +++ b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Robot.java @@ -0,0 +1,94 @@ +/* + * MIT License + * + * Copyright (c) 2022 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. + */ + +package frc.robot; + +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.XboxController; +import frc.robot.Constants.DriveTrainConstants; + +public class Robot extends TimedRobot { + private XboxController m_controller; + private Drivetrain m_drive; + + // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1. + private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3); + private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3); + + @Override + public void robotInit() { + if (Robot.isSimulation()) { + NetworkTableInstance instance = NetworkTableInstance.getDefault(); + instance.stopServer(); + // set the NT server if simulating this code. + // "localhost" for photon on desktop, or "photonvision.local" / "[ip-address]" for coprocessor + instance.setServer("localhost"); + instance.startClient4("myRobot"); + } + + m_controller = new XboxController(0); + m_drive = new Drivetrain(); + } + + @Override + public void robotPeriodic() { + m_drive.updateOdometry(); + } + + @Override + public void autonomousInit() {} + + @Override + public void autonomousPeriodic() {} + + @Override + public void teleopPeriodic() { + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + var joyY = m_controller.getLeftY(); + if (Math.abs(joyY) < 0.075) { + joyY = 0; + } + final var xSpeed = -m_speedLimiter.calculate(joyY) * DriveTrainConstants.kMaxSpeed; + + // Get the rate of angular rotation. We are inverting this because we want a + // positive value when we pull to the left (remember, CCW is positive in + // mathematics). Xbox controllers return positive values when you pull to + // the right by default. + var joyX = m_controller.getRightX(); + if (Math.abs(joyX) < 0.075) { + joyX = 0; + } + final var rot = -m_rotLimiter.calculate(joyX) * DriveTrainConstants.kMaxAngularSpeed; + + m_drive.drive(xSpeed, rot); + } + + @Override + public void simulationPeriodic() { + m_drive.simulationPeriodic(); + } +}