diff --git a/.gitignore b/.gitignore index 6d862765b..721d1619a 100644 --- a/.gitignore +++ b/.gitignore @@ -153,3 +153,5 @@ photon-server/src/main/resources/nativelibraries/apriltag/* photonlib-java-examples/*/vendordeps/* photonlib-cpp-examples/*/vendordeps/* + +networktables.json diff --git a/photon-server/lib/libphotonlibcamera.so b/photon-server/lib/libphotonlibcamera.so deleted file mode 100644 index f11ed7338..000000000 Binary files a/photon-server/lib/libphotonlibcamera.so and /dev/null differ diff --git a/photonlib-cpp-examples/aimandrange/build.gradle b/photonlib-cpp-examples/aimandrange/build.gradle index b30042967..10e9a0f76 100644 --- a/photonlib-cpp-examples/aimandrange/build.gradle +++ b/photonlib-cpp-examples/aimandrange/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2023.2.1" + id "edu.wpi.first.GradleRIO" version "2023.3.2" id "com.dorongold.task-tree" version "2.1.0" } diff --git a/photonlib-cpp-examples/aimattarget/build.gradle b/photonlib-cpp-examples/aimattarget/build.gradle index b30042967..10e9a0f76 100644 --- a/photonlib-cpp-examples/aimattarget/build.gradle +++ b/photonlib-cpp-examples/aimattarget/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2023.2.1" + id "edu.wpi.first.GradleRIO" version "2023.3.2" id "com.dorongold.task-tree" version "2.1.0" } diff --git a/photonlib-cpp-examples/apriltagExample/.gitignore b/photonlib-cpp-examples/apriltagExample/.gitignore new file mode 100644 index 000000000..34878ab18 --- /dev/null +++ b/photonlib-cpp-examples/apriltagExample/.gitignore @@ -0,0 +1 @@ +vendordeps diff --git a/photonlib-cpp-examples/apriltagExample/WPILib-License.md b/photonlib-cpp-examples/apriltagExample/WPILib-License.md new file mode 100644 index 000000000..ba35a02c5 --- /dev/null +++ b/photonlib-cpp-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-cpp-examples/apriltagExample/build.gradle b/photonlib-cpp-examples/apriltagExample/build.gradle new file mode 100644 index 000000000..10e9a0f76 --- /dev/null +++ b/photonlib-cpp-examples/apriltagExample/build.gradle @@ -0,0 +1,116 @@ +plugins { + id "cpp" + id "google-test-test-suite" + id "edu.wpi.first.GradleRIO" version "2023.3.2" + + id "com.dorongold.task-tree" version "2.1.0" +} + +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/apriltagExample/gradlew b/photonlib-cpp-examples/apriltagExample/gradlew new file mode 100755 index 000000000..a69d9cb6c --- /dev/null +++ b/photonlib-cpp-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-cpp-examples/apriltagExample/gradlew.bat b/photonlib-cpp-examples/apriltagExample/gradlew.bat new file mode 100644 index 000000000..53a6b238d --- /dev/null +++ b/photonlib-cpp-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-cpp-examples/apriltagExample/settings.gradle b/photonlib-cpp-examples/apriltagExample/settings.gradle new file mode 100644 index 000000000..adeb0f45c --- /dev/null +++ b/photonlib-cpp-examples/apriltagExample/settings.gradle @@ -0,0 +1,30 @@ +import org.gradle.internal.os.OperatingSystem + +rootProject.name = 'aimattarget' + +pluginManagement { + repositories { + mavenLocal() + jcenter() + 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-cpp-examples/apriltagExample/simgui-ds.json b/photonlib-cpp-examples/apriltagExample/simgui-ds.json new file mode 100644 index 000000000..7b5100502 --- /dev/null +++ b/photonlib-cpp-examples/apriltagExample/simgui-ds.json @@ -0,0 +1,102 @@ +{ + "Keyboard 0 Settings": { + "window": { + "visible": true + } + }, + "keyboardJoysticks": [ + { + "axisConfig": [ + {}, + { + "decKey": 87, + "incKey": 83 + }, + { + "decayRate": 0.0, + "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 65, + "incKey": 68 + } + ], + "axisCount": 6, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } + ] +} diff --git a/photonlib-cpp-examples/apriltagExample/src/main/cpp/Drivetrain.cpp b/photonlib-cpp-examples/apriltagExample/src/main/cpp/Drivetrain.cpp new file mode 100644 index 000000000..ad778e254 --- /dev/null +++ b/photonlib-cpp-examples/apriltagExample/src/main/cpp/Drivetrain.cpp @@ -0,0 +1,92 @@ +/* + * 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. + */ + +#include "Drivetrain.h" + +#include +#include + +void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) { + auto leftFeedforward = m_feedforward.Calculate(speeds.left); + auto rightFeedforward = m_feedforward.Calculate(speeds.right); + double leftOutput = m_leftPIDController.Calculate(m_leftEncoder.GetRate(), + speeds.left.value()); + double rightOutput = m_rightPIDController.Calculate(m_rightEncoder.GetRate(), + speeds.right.value()); + + m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward); + m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward); +} + +void Drivetrain::Drive(units::meters_per_second_t xSpeed, + units::radians_per_second_t rot) { + SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot})); +} + +void Drivetrain::UpdateOdometry() { + m_poseEstimator.Update(m_gyro.GetRotation2d(), + units::meter_t{m_leftEncoder.GetDistance()}, + units::meter_t{m_rightEncoder.GetDistance()}); +} + +void Drivetrain::ResetOdometry(const frc::Pose2d& pose) { + m_leftEncoder.Reset(); + m_rightEncoder.Reset(); + m_drivetrainSimulator.SetPose(pose); + m_poseEstimator.ResetPosition( + m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()}, + units::meter_t{m_rightEncoder.GetDistance()}, pose); +} + +void Drivetrain::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(units::volt_t{m_leftGroup.Get()} * + frc::RobotController::GetInputVoltage(), + units::volt_t{m_rightGroup.Get()} * + frc::RobotController::GetInputVoltage()); + m_drivetrainSimulator.Update(20_ms); + + m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value()); + m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetLeftVelocity().value()); + m_rightEncoderSim.SetDistance( + m_drivetrainSimulator.GetRightPosition().value()); + m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetRightVelocity().value()); + m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees().value()); +} + +void Drivetrain::Periodic() { + UpdateOdometry(); + + auto result = m_pcw.Update(m_poseEstimator.GetEstimatedPosition()); + if (result) { + m_poseEstimator.AddVisionMeasurement( + result.value().estimatedPose.ToPose2d(), result.value().timestamp); + } + + m_fieldSim.SetRobotPose(m_poseEstimator.GetEstimatedPosition()); +} diff --git a/photonlib-cpp-examples/apriltagExample/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/apriltagExample/src/main/cpp/Robot.cpp new file mode 100644 index 000000000..4d3742aeb --- /dev/null +++ b/photonlib-cpp-examples/apriltagExample/src/main/cpp/Robot.cpp @@ -0,0 +1,61 @@ +/* + * 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. + */ + +#include "Robot.h" + +#include +#include + +void Robot::RobotInit() { + if constexpr (frc::RobotBase::IsSimulation()) { + auto inst = nt::NetworkTableInstance::GetDefault(); + inst.StopServer(); + // set the NT server if simulating this code. + // "localhost" for photon on desktop, or "photonvision.local" or + // "[ip-address]" for coprocessor + inst.SetServer("localhost"); + inst.StartClient4("Robot Simulation"); + } +} + +void Robot::TeleopPeriodic() { + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + const auto xSpeed = -m_controller.GetLeftY() * Drivetrain::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. + auto rot = -m_controller.GetRightX() * Drivetrain::kMaxAngularSpeed; + + m_drive.Drive(xSpeed, rot); +} + +void Robot::RobotPeriodic() { m_drive.Periodic(); } +void Robot::SimulationPeriodic() { m_drive.SimulationPeriodic(); } + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/photonlib-cpp-examples/apriltagExample/src/main/include/Drivetrain.h b/photonlib-cpp-examples/apriltagExample/src/main/include/Drivetrain.h new file mode 100644 index 000000000..7995030ba --- /dev/null +++ b/photonlib-cpp-examples/apriltagExample/src/main/include/Drivetrain.h @@ -0,0 +1,140 @@ +/* + * 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. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "PhotonCameraWrapper.h" + +/** + * Represents a differential drive style drivetrain. + */ +class Drivetrain { + public: + Drivetrain() { + 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(2 * std::numbers::pi * kWheelRadius / + kEncoderResolution); + m_rightEncoder.SetDistancePerPulse(2 * std::numbers::pi * kWheelRadius / + kEncoderResolution); + + m_leftEncoder.Reset(); + m_rightEncoder.Reset(); + + m_rightGroup.SetInverted(true); + + frc::SmartDashboard::PutData("Field", &m_fieldSim); + } + + static constexpr units::meters_per_second_t kMaxSpeed = + 3.0_mps; // 3 meters per second + static constexpr units::radians_per_second_t kMaxAngularSpeed{ + std::numbers::pi}; // 1/2 rotation per second + + void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds); + void Drive(units::meters_per_second_t xSpeed, + units::radians_per_second_t rot); + void UpdateOdometry(); + void ResetOdometry(const frc::Pose2d& pose); + + frc::Pose2d GetPose() const { return m_poseEstimator.GetEstimatedPosition(); } + + void SimulationPeriodic(); + void Periodic(); + + private: + static constexpr units::meter_t kTrackWidth = 0.381_m * 2; + static constexpr double kWheelRadius = 0.0508; // meters + static constexpr int kEncoderResolution = 4096; + + frc::PWMSparkMax m_leftLeader{1}; + frc::PWMSparkMax m_leftFollower{2}; + frc::PWMSparkMax m_rightLeader{3}; + frc::PWMSparkMax m_rightFollower{4}; + + frc::MotorControllerGroup m_leftGroup{m_leftLeader, m_leftFollower}; + frc::MotorControllerGroup m_rightGroup{m_rightLeader, m_rightFollower}; + + frc::Encoder m_leftEncoder{0, 1}; + frc::Encoder m_rightEncoder{2, 3}; + + frc2::PIDController m_leftPIDController{8.5, 0.0, 0.0}; + frc2::PIDController m_rightPIDController{8.5, 0.0, 0.0}; + + frc::AnalogGyro m_gyro{0}; + + frc::DifferentialDriveKinematics m_kinematics{kTrackWidth}; + + frc::DifferentialDrivePoseEstimator m_poseEstimator{ + m_kinematics, m_gyro.GetRotation2d(), + units::meter_t{m_leftEncoder.GetDistance()}, + units::meter_t{m_rightEncoder.GetDistance()}, frc::Pose2d{}}; + + PhotonCameraWrapper m_pcw; + + // Gains are for example purposes only - must be determined for your own + // robot! + frc::SimpleMotorFeedforward m_feedforward{1_V, 3_V / 1_mps}; + + // Simulation classes help us simulate our robot + frc::sim::AnalogGyroSim m_gyroSim{m_gyro}; + frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder}; + frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder}; + frc::Field2d m_fieldSim; + frc::LinearSystem<2, 2, 2> m_drivetrainSystem = + frc::LinearSystemId::IdentifyDrivetrainSystem( + 1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq); + frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{ + m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in}; +}; diff --git a/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h b/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h new file mode 100644 index 000000000..e45517b8f --- /dev/null +++ b/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h @@ -0,0 +1,46 @@ +/* + * 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. + */ + +#pragma once +#include +#include + +#include + +#include +#include + +class PhotonCameraWrapper { + public: + photonlib::PhotonPoseEstimator m_poseEstimator{ + frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp), + photonlib::LOWEST_AMBIGUITY, + std::move(photonlib::PhotonCamera{"WPI2023"}), frc::Transform3d{}}; + + inline std::optional Update( + frc::Pose2d estimatedPose) { + m_poseEstimator.SetReferencePose(frc::Pose3d(estimatedPose)); + return m_poseEstimator.Update(); + } +}; diff --git a/photonlib-cpp-examples/apriltagExample/src/main/include/Robot.h b/photonlib-cpp-examples/apriltagExample/src/main/include/Robot.h new file mode 100644 index 000000000..9570d66f8 --- /dev/null +++ b/photonlib-cpp-examples/apriltagExample/src/main/include/Robot.h @@ -0,0 +1,50 @@ +/* + * 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. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "Drivetrain.h" + +class Robot : public frc::TimedRobot { + public: + void RobotInit() override; + void RobotPeriodic() override; + void TeleopPeriodic() override; + void SimulationPeriodic() override; + + private: + frc::XboxController m_controller{0}; + + Drivetrain m_drive; +}; diff --git a/photonlib-cpp-examples/apriltagExample/src/test/cpp/main.cpp b/photonlib-cpp-examples/apriltagExample/src/test/cpp/main.cpp new file mode 100644 index 000000000..e9ee94e8b --- /dev/null +++ b/photonlib-cpp-examples/apriltagExample/src/test/cpp/main.cpp @@ -0,0 +1,34 @@ +/* + * 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. + */ + +#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/examples.txt b/photonlib-cpp-examples/examples.txt index 54fc7fa00..25f764852 100644 --- a/photonlib-cpp-examples/examples.txt +++ b/photonlib-cpp-examples/examples.txt @@ -1,3 +1,4 @@ aimandrange getinrange aimattarget +apriltagExample diff --git a/photonlib-cpp-examples/getinrange/build.gradle b/photonlib-cpp-examples/getinrange/build.gradle index b30042967..10e9a0f76 100644 --- a/photonlib-cpp-examples/getinrange/build.gradle +++ b/photonlib-cpp-examples/getinrange/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2023.2.1" + id "edu.wpi.first.GradleRIO" version "2023.3.2" id "com.dorongold.task-tree" version "2.1.0" } diff --git a/photonlib-java-examples/aimandrange/build.gradle b/photonlib-java-examples/aimandrange/build.gradle index a5629b00c..a7b059a15 100644 --- a/photonlib-java-examples/aimandrange/build.gradle +++ b/photonlib-java-examples/aimandrange/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.2.1" + id "edu.wpi.first.GradleRIO" version "2023.3.2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/photonlib-java-examples/aimattarget/build.gradle b/photonlib-java-examples/aimattarget/build.gradle index 0c26b0633..1536aafcc 100644 --- a/photonlib-java-examples/aimattarget/build.gradle +++ b/photonlib-java-examples/aimattarget/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.2.1" + id "edu.wpi.first.GradleRIO" version "2023.3.2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/photonlib-java-examples/apriltagExample/build.gradle b/photonlib-java-examples/apriltagExample/build.gradle index a5629b00c..a7b059a15 100644 --- a/photonlib-java-examples/apriltagExample/build.gradle +++ b/photonlib-java-examples/apriltagExample/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.2.1" + id "edu.wpi.first.GradleRIO" version "2023.3.2" } sourceCompatibility = JavaVersion.VERSION_11 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 index 3a258bc42..184446f2c 100644 --- a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Robot.java +++ b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Robot.java @@ -46,7 +46,7 @@ public class Robot extends TimedRobot { // 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"); + instance.startClient4("Robot Simulation"); } m_controller = new XboxController(0); diff --git a/photonlib-java-examples/getinrange/build.gradle b/photonlib-java-examples/getinrange/build.gradle index a5629b00c..a7b059a15 100644 --- a/photonlib-java-examples/getinrange/build.gradle +++ b/photonlib-java-examples/getinrange/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.2.1" + id "edu.wpi.first.GradleRIO" version "2023.3.2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/photonlib-java-examples/simaimandrange/build.gradle b/photonlib-java-examples/simaimandrange/build.gradle index a5629b00c..a7b059a15 100644 --- a/photonlib-java-examples/simaimandrange/build.gradle +++ b/photonlib-java-examples/simaimandrange/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.2.1" + id "edu.wpi.first.GradleRIO" version "2023.3.2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/photonlib-java-examples/simposeest/build.gradle b/photonlib-java-examples/simposeest/build.gradle index a5629b00c..a7b059a15 100644 --- a/photonlib-java-examples/simposeest/build.gradle +++ b/photonlib-java-examples/simposeest/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.2.1" + id "edu.wpi.first.GradleRIO" version "2023.3.2" } sourceCompatibility = JavaVersion.VERSION_11