diff --git a/photonlib-java-examples/aimattarget/.gitignore b/photonlib-java-examples/aimattarget/.gitignore index 9535c83037..9f76c3a543 100644 --- a/photonlib-java-examples/aimattarget/.gitignore +++ b/photonlib-java-examples/aimattarget/.gitignore @@ -158,5 +158,16 @@ gradle-app.setting .settings/ bin/ +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + # Simulation GUI and other tools window save file *-window.json +networktables.json diff --git a/photonlib-java-examples/aimattarget/.wpilib/wpilib_preferences.json b/photonlib-java-examples/aimattarget/.wpilib/wpilib_preferences.json index c72767ad48..6180392668 100644 --- a/photonlib-java-examples/aimattarget/.wpilib/wpilib_preferences.json +++ b/photonlib-java-examples/aimattarget/.wpilib/wpilib_preferences.json @@ -2,5 +2,5 @@ "enableCppIntellisense": false, "currentLanguage": "java", "projectYear": "2024", - "teamNumber": 6995 + "teamNumber": 4512 } diff --git a/photonlib-java-examples/aimattarget/2023-field.json b/photonlib-java-examples/aimattarget/2023-field.json new file mode 100644 index 0000000000..ff4c5ca238 --- /dev/null +++ b/photonlib-java-examples/aimattarget/2023-field.json @@ -0,0 +1,19 @@ +{ + "game": "Charged Up", + "field-image": "2023-field.png", + "field-corners": { + "top-left": [ + 46, + 36 + ], + "bottom-right": [ + 1088, + 544 + ] + }, + "field-size": [ + 54.27083, + 26.2916 + ], + "field-unit": "foot" + } diff --git a/photonlib-java-examples/aimattarget/2023-field.png b/photonlib-java-examples/aimattarget/2023-field.png new file mode 100644 index 0000000000..ab3f0ff333 Binary files /dev/null and b/photonlib-java-examples/aimattarget/2023-field.png differ diff --git a/photonlib-java-examples/aimattarget/WPILib-License.md b/photonlib-java-examples/aimattarget/WPILib-License.md index 3d5a824cad..645e54253a 100644 --- a/photonlib-java-examples/aimattarget/WPILib-License.md +++ b/photonlib-java-examples/aimattarget/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2021 FIRST and other WPILib contributors +Copyright (c) 2009-2024 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/photonlib-java-examples/aimattarget/build.gradle b/photonlib-java-examples/aimattarget/build.gradle index fe6e93ab8d..28dc34bcab 100644 --- a/photonlib-java-examples/aimattarget/build.gradle +++ b/photonlib-java-examples/aimattarget/build.gradle @@ -6,19 +6,14 @@ plugins { sourceCompatibility = JavaVersion.VERSION_11 targetCompatibility = JavaVersion.VERSION_11 -def ROBOT_MAIN_CLASS = "frc.robot.Main" - apply from: "${rootDir}/../shared/examples_common.gradle" -ext { - wpilibVersion = "2025.0.0-alpha-1" - wpimathVersion = wpilibVersion - openCVversion = "4.8.0-2" -} +def ROBOT_MAIN_CLASS = "frc.robot.Main" + +wpi.maven.useDevelopment = true +wpi.versions.wpilibVersion = "2024.3.2" +wpi.versions.wpimathVersion = "2024.3.2" -wpi.getVersions().getOpencvVersion().convention(openCVversion); -wpi.getVersions().getWpilibVersion().convention(wpilibVersion); -wpi.getVersions().getWpimathVersion().convention(wpimathVersion); // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. @@ -29,7 +24,7 @@ deploy { // 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) + team = project.frc.getTeamOrDefault(4512) debug = project.frc.getDebugOrDefault(false) artifacts { @@ -58,7 +53,7 @@ wpi.java.debugJni = false def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. -// Also defines JUnit 4. +// Also defines JUnit 5. dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() diff --git a/photonlib-java-examples/aimattarget/gradle/wrapper/gradle-wrapper.jar b/photonlib-java-examples/aimattarget/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000000..249e5832f0 Binary files /dev/null and b/photonlib-java-examples/aimattarget/gradle/wrapper/gradle-wrapper.jar differ diff --git a/photonlib-java-examples/aimattarget/gradle/wrapper/gradle-wrapper.properties b/photonlib-java-examples/aimattarget/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000000..ca8f28abd3 --- /dev/null +++ b/photonlib-java-examples/aimattarget/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,5 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.4-bin\.zip +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/photonlib-java-examples/aimattarget/gradlew b/photonlib-java-examples/aimattarget/gradlew new file mode 100644 index 0000000000..0ef4c1e860 --- /dev/null +++ b/photonlib-java-examples/aimattarget/gradlew @@ -0,0 +1,241 @@ +#!/bin/sh + +# +# Copyright © 2015-2021 the original authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# + +############################################################################## +# +# Gradle start up script for POSIX generated by Gradle. +# +# Important for running: +# +# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is +# noncompliant, but you have some other compliant shell such as ksh or +# bash, then to run this script, type that shell name before the whole +# command line, like: +# +# ksh Gradle +# +# Busybox and similar reduced shells will NOT work, because this script +# requires all of these POSIX shell features: +# * functions; +# * expansions «$var», «${var}», «${var:-default}», «${var+SET}», +# «${var#prefix}», «${var%suffix}», and «$( cmd )»; +# * compound commands having a testable exit status, especially «case»; +# * various built-in commands including «command», «set», and «ulimit». +# +# Important for patching: +# +# (2) This script targets any POSIX shell, so it avoids extensions provided +# by Bash, Ksh, etc; in particular arrays are avoided. +# +# The "traditional" practice of packing multiple parameters into a +# space-separated string is a well documented source of bugs and security +# problems, so this is (mostly) avoided, by progressively accumulating +# options in "$@", and eventually passing that to Java. +# +# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS, +# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly; +# see the in-line comments for details. +# +# There are tweaks for specific operating systems such as AIX, CygWin, +# Darwin, MinGW, and NonStop. +# +# (3) This script is generated from the Groovy template +# https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# within the Gradle project. +# +# You can find Gradle at https://github.com/gradle/gradle/. +# +############################################################################## + +# Attempt to set APP_HOME + +# Resolve links: $0 may be a link +app_path=$0 + +# Need this for daisy-chained symlinks. +while + APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path + [ -h "$app_path" ] +do + ls=$( ls -ld "$app_path" ) + link=${ls#*' -> '} + case $link in #( + /*) app_path=$link ;; #( + *) app_path=$APP_HOME$link ;; + esac +done + +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit + +APP_NAME="Gradle" +APP_BASE_NAME=${0##*/} + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD=maximum + +warn () { + echo "$*" +} >&2 + +die () { + echo + echo "$*" + echo + exit 1 +} >&2 + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "$( uname )" in #( + CYGWIN* ) cygwin=true ;; #( + Darwin* ) darwin=true ;; #( + MSYS* | MINGW* ) msys=true ;; #( + NONSTOP* ) nonstop=true ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD=$JAVA_HOME/jre/sh/java + else + JAVACMD=$JAVA_HOME/bin/java + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD=java + which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." +fi + +# Increase the maximum file descriptors if we can. +if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + ulimit -n "$MAX_FD" || + warn "Could not set maximum file descriptor limit to $MAX_FD" + esac +fi + +# Collect all arguments for the java command, stacking in reverse order: +# * args from the command line +# * the main class name +# * -classpath +# * -D...appname settings +# * --module-path (only if needed) +# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables. + +# For Cygwin or MSYS, switch paths to Windows format before running java +if "$cygwin" || "$msys" ; then + APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) + CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) + + JAVACMD=$( cygpath --unix "$JAVACMD" ) + + # Now convert the arguments - kludge to limit ourselves to /bin/sh + for arg do + if + case $arg in #( + -*) false ;; # don't mess with options #( + /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath + [ -e "$t" ] ;; #( + *) false ;; + esac + then + arg=$( cygpath --path --ignore --mixed "$arg" ) + fi + # Roll the args list around exactly as many times as the number of + # args, so each arg winds up back in the position where it started, but + # possibly modified. + # + # NB: a `for` loop captures its iteration list before it begins, so + # changing the positional parameters here affects neither the number of + # iterations, nor the values presented in `arg`. + shift # remove old arg + set -- "$@" "$arg" # push replacement arg + done +fi + +# Collect all arguments for the java command; +# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of +# shell script including quotes and variable substitutions, so put them in +# double quotes to make sure that they get re-expanded; and +# * put everything else in single quotes, so that it's not re-expanded. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Stop when "xargs" is not available. +if ! command -v xargs >/dev/null 2>&1 +then + die "xargs is not available" +fi + +# Use "xargs" to parse quoted args. +# +# With -n1 it outputs one arg per line, with the quotes and backslashes removed. +# +# In Bash we could simply go: +# +# readarray ARGS < <( xargs -n1 <<<"$var" ) && +# set -- "${ARGS[@]}" "$@" +# +# but POSIX shell has neither arrays nor command substitution, so instead we +# post-process each arg (as a line of input to sed) to backslash-escape any +# character that might be a shell metacharacter, then use eval to reverse +# that process (while maintaining the separation between arguments), and wrap +# the whole thing up as a single "set" statement. +# +# This will of course break if any of these variables contains a newline or +# an unmatched quote. +# + +eval "set -- $( + printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | + xargs -n1 | + sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | + tr '\n' ' ' + )" '"$@"' + +exec "$JAVACMD" "$@" diff --git a/photonlib-java-examples/aimattarget/gradlew.bat b/photonlib-java-examples/aimattarget/gradlew.bat new file mode 100644 index 0000000000..f127cfd49d --- /dev/null +++ b/photonlib-java-examples/aimattarget/gradlew.bat @@ -0,0 +1,91 @@ +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem + +@if "%DEBUG%"=="" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%"=="" set DIRNAME=. +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Resolve any "." and ".." in APP_HOME to make it shorter. +for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if %ERRORLEVEL% equ 0 goto execute + +echo. +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto execute + +echo. +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* + +:end +@rem End local scope for the variables with windows NT shell +if %ERRORLEVEL% equ 0 goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/photonlib-java-examples/aimattarget/settings.gradle b/photonlib-java-examples/aimattarget/settings.gradle index df10542440..091a37a027 100644 --- a/photonlib-java-examples/aimattarget/settings.gradle +++ b/photonlib-java-examples/aimattarget/settings.gradle @@ -1,7 +1,5 @@ import org.gradle.internal.os.OperatingSystem -rootProject.name = 'aimattarget' - pluginManagement { repositories { mavenLocal() diff --git a/photonlib-java-examples/aimattarget/simgui-ds.json b/photonlib-java-examples/aimattarget/simgui-ds.json index 69b1a3cbcd..addd5860ce 100644 --- a/photonlib-java-examples/aimattarget/simgui-ds.json +++ b/photonlib-java-examples/aimattarget/simgui-ds.json @@ -11,13 +11,16 @@ "incKey": 83 }, { - "decKey": 69, "decayRate": 0.0, - "incKey": 82, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 81, + "incKey": 69 } ], - "axisCount": 3, + "axisCount": 6, "buttonCount": 4, "buttonKeys": [ 90, diff --git a/photonlib-java-examples/aimattarget/simgui.json b/photonlib-java-examples/aimattarget/simgui.json index cc05c0f4c0..a1406290b3 100644 --- a/photonlib-java-examples/aimattarget/simgui.json +++ b/photonlib-java-examples/aimattarget/simgui.json @@ -2,17 +2,152 @@ "NTProvider": { "types": { "/FMSInfo": "FMSInfo", - "/SmartDashboard/Field": "Field2d" + "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" + }, + "windows": { + "/SmartDashboard/VisionSystemSim-main/Sim Field": { + "EstimatedRobot": { + "arrowWeight": 3.0, + "length": 0.800000011920929, + "selectable": false, + "weight": 3.0, + "width": 0.800000011920929 + }, + "EstimatedRobotModules": { + "arrows": false, + "image": "swerve_module.png", + "length": 0.30000001192092896, + "selectable": false, + "width": 0.30000001192092896 + }, + "Robot": { + "arrowColor": [ + 1.0, + 1.0, + 1.0, + 255.0 + ], + "arrowWeight": 2.0, + "color": [ + 1.0, + 1.0, + 1.0, + 255.0 + ], + "length": 0.800000011920929, + "selectable": false, + "weight": 2.0, + "width": 0.800000011920929 + }, + "VisionEstimation": { + "arrowColor": [ + 0.0, + 0.6075949668884277, + 1.0, + 255.0 + ], + "arrowWeight": 2.0, + "color": [ + 0.0, + 0.6075949668884277, + 1.0, + 255.0 + ], + "selectable": false, + "weight": 2.0 + }, + "apriltag": { + "arrows": false, + "image": "tag-green.png", + "length": 0.6000000238418579, + "width": 0.5 + }, + "bottom": 1476, + "cameras": { + "arrowColor": [ + 0.29535865783691406, + 1.0, + 0.9910804033279419, + 255.0 + ], + "arrowSize": 19, + "arrowWeight": 3.0, + "length": 1.0, + "style": "Hidden", + "weight": 1.0, + "width": 1.0 + }, + "height": 8.210550308227539, + "image": "2023-field.png", + "left": 150, + "right": 2961, + "top": 79, + "visibleTargetPoses": { + "arrows": false, + "image": "tag-blue.png", + "length": 0.5, + "selectable": false, + "width": 0.4000000059604645 + }, + "width": 16.541748046875, + "window": { + "visible": true + } + } } }, "NetworkTables": { "transitory": { + "CameraPublisher": { + "YOUR CAMERA NAME-processed": { + "open": true, + "string[]##v_/CameraPublisher/YOUR CAMERA NAME-processed/streams": { + "open": true + } + }, + "open": true + }, "SmartDashboard": { - "Field": { + "VisionSystemSim-main": { "open": true }, "open": true } } + }, + "NetworkTables Info": { + "visible": true + }, + "NetworkTables View": { + "visible": false + }, + "Plot": { + "Plot <0>": { + "plots": [ + { + "backgroundColor": [ + 0.0, + 0.0, + 0.0, + 0.8500000238418579 + ], + "height": 332, + "series": [ + { + "color": [ + 0.2980392277240753, + 0.44705885648727417, + 0.6901960968971252, + 1.0 + ], + "id": "NT:/SmartDashboard/Vision Target Visible" + } + ] + } + ], + "window": { + "visible": false + } + } } } diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000000..93a901d676 --- /dev/null +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Constants.java @@ -0,0 +1,143 @@ +/* + * 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. + */ + +package frc.robot; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; + +public class Constants { + public static class Vision { + public static final String kCameraName = "YOUR CAMERA NAME"; + // Cam mounted facing forward, half a meter forward of center, half a meter up from center, pitched upward. + private static final double camPitch = Units.degreesToRadians(30.0); + public static final Transform3d kRobotToCam = + new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, -camPitch, 0)); + + // The layout of the AprilTags on the field + public static final AprilTagFieldLayout kTagLayout = + AprilTagFields.kDefaultField.loadAprilTagLayoutField(); + + // The standard deviations of our vision estimated poses, which affect correction rate + // (Fake values. Experiment and determine estimation noise on an actual robot.) + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); + } + + public static class Swerve { + // Physical properties + public static final double kTrackWidth = Units.inchesToMeters(18.5); + public static final double kTrackLength = Units.inchesToMeters(18.5); + public static final double kRobotWidth = Units.inchesToMeters(25 + 3.25 * 2); + public static final double kRobotLength = Units.inchesToMeters(25 + 3.25 * 2); + public static final double kMaxLinearSpeed = Units.feetToMeters(15.5); + public static final double kMaxAngularSpeed = Units.rotationsToRadians(2); + public static final double kWheelDiameter = Units.inchesToMeters(4); + public static final double kWheelCircumference = kWheelDiameter * Math.PI; + + public static final double kDriveGearRatio = 6.75; // 6.75:1 SDS MK4 L2 ratio + public static final double kSteerGearRatio = 12.8; // 12.8:1 + + public static final double kDriveDistPerPulse = kWheelCircumference / 1024 / kDriveGearRatio; + public static final double kSteerRadPerPulse = 2 * Math.PI / 1024; + + public enum ModuleConstants { + FL( // Front left + 1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2), + FR( // Front Right + 2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2), + BL( // Back Left + 3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2), + BR( // Back Right + 4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2); + + public final int moduleNum; + public final int driveMotorID; + public final int driveEncoderA; + public final int driveEncoderB; + public final int steerMotorID; + public final int steerEncoderA; + public final int steerEncoderB; + public final double angleOffset; + public final Translation2d centerOffset; + + private ModuleConstants( + int moduleNum, + int driveMotorID, + int driveEncoderA, + int driveEncoderB, + int steerMotorID, + int steerEncoderA, + int steerEncoderB, + double angleOffset, + double xOffset, + double yOffset) { + this.moduleNum = moduleNum; + this.driveMotorID = driveMotorID; + this.driveEncoderA = driveEncoderA; + this.driveEncoderB = driveEncoderB; + this.steerMotorID = steerMotorID; + this.steerEncoderA = steerEncoderA; + this.steerEncoderB = steerEncoderB; + this.angleOffset = angleOffset; + centerOffset = new Translation2d(xOffset, yOffset); + } + } + + // Feedforward + // Linear drive feed forward + public static final SimpleMotorFeedforward kDriveFF = + new SimpleMotorFeedforward( // real + 0.25, // Voltage to break static friction + 2.5, // Volts per meter per second + 0.3 // Volts per meter per second squared + ); + // Steer feed forward + public static final SimpleMotorFeedforward kSteerFF = + new SimpleMotorFeedforward( // real + 0.5, // Voltage to break static friction + 0.25, // Volts per radian per second + 0.01 // Volts per radian per second squared + ); + + // PID + public static final double kDriveKP = 1; + public static final double kDriveKI = 0; + public static final double kDriveKD = 0; + + public static final double kSteerKP = 20; + public static final double kSteerKI = 0; + public static final double kSteerKD = 0.25; + } +} diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Main.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Main.java index 077f979f97..f227c28da0 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Main.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Main.java @@ -26,19 +26,9 @@ 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/aimattarget/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java index d4ff022670..07d24711ea 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java @@ -24,87 +24,128 @@ package frc.robot; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.util.Units; +import static frc.robot.Constants.Vision.*; + + +import org.photonvision.PhotonCamera; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import org.photonvision.PhotonCamera; +import frc.robot.subsystems.drivetrain.SwerveDrive; -/** - * The VM is configured to automatically run this class, and to call the functions corresponding to - * each mode, as described in the TimedRobot documentation. If you change the name of this class or - * the package after creating this project, you must also update the build.gradle file in the - * project. - */ public class Robot extends TimedRobot { - // Constants such as camera and target height stored. Change per robot and goal! - final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24); - final double TARGET_HEIGHT_METERS = Units.feetToMeters(5); - // Angle between horizontal and the camera. - final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0); + private SwerveDrive drivetrain; + private VisionSim visionSim; + private PhotonCamera camera; + + private final double VISION_TURN_kP = 0.01; + - // How far from the target we want to be - final double GOAL_RANGE_METERS = Units.feetToMeters(3); + private XboxController controller; - // Change this to match the name of your camera as shown in the web UI - PhotonCamera camera = new PhotonCamera("Microsoft_LifeCam_HD-3000"); - // PID constants should be tuned per robot - final double LINEAR_P = 0.1; - final double LINEAR_D = 0.0; - PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D); + @Override + public void robotInit() { + drivetrain = new SwerveDrive(); + camera = new PhotonCamera(kCameraName); - final double ANGULAR_P = 0.1; - final double ANGULAR_D = 0.0; - PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D); + visionSim = new VisionSim(camera); - XboxController xboxController = new XboxController(0); + controller = new XboxController(0); - // Drive motors - PWMVictorSPX leftMotor = new PWMVictorSPX(0); - PWMVictorSPX rightMotor = new PWMVictorSPX(1); - DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor); + } @Override public void robotPeriodic() { - var start = Timer.getFPGATimestamp(); - var res = camera.getLatestResult(); - var end = Timer.getFPGATimestamp(); - System.out.println( - "dt: " + (int) ((end - start) * 1e6) + "uS for targets: " + res.getTargets().size()); - SmartDashboard.putNumber("decodeTime", (int) ((end - start) * 1e6)); + + // Update drivetrain subsystem + drivetrain.periodic(); + + // Log values to the dashboard + drivetrain.log(); + } + + @Override + public void disabledPeriodic() { + drivetrain.stop(); + } + + @Override + public void teleopInit() { + resetPose(); } @Override public void teleopPeriodic() { - double forwardSpeed; - double rotationSpeed; - - forwardSpeed = -xboxController.getRightY(); - - if (xboxController.getAButton()) { - // Vision-alignment mode - // Query the latest result from PhotonVision - var result = camera.getLatestResult(); - - if (result.hasTargets()) { - // Calculate angular turn power - // -1.0 required to ensure positive PID controller effort _increases_ yaw - rotationSpeed = -turnController.calculate(result.getBestTarget().getYaw(), 0); - } else { - // If we have no targets, stay still. - rotationSpeed = 0; + + // Calculate drivetrain commands from Joystick values + double forward = -controller.getLeftY() * Constants.Swerve.kMaxLinearSpeed; + double strafe = -controller.getLeftX() * Constants.Swerve.kMaxLinearSpeed; + double turn = -controller.getRightX() * Constants.Swerve.kMaxAngularSpeed; + + // Read in relevant data from the Camera + boolean targetVisible = false; + double targetYaw = 0.0; + var results = camera.getAllUnreadResults(); + if(!results.isEmpty()){ + //Camera processed a new frame since last + //Get the last one in the list. + var result = results.get(results.size()-1); + if(result.hasTargets()){ + //At least one AprilTag was seen by the camera + for(var target:result.getTargets()){ + if(target.getFiducialId() == 7){ + //Found Tag 7, record its information + targetYaw = target.getYaw(); + targetVisible = true; + } + } } - } else { - // Manual Driver Mode - rotationSpeed = xboxController.getLeftX(); } - // Use our forward/turn speeds to control the drivetrain - drive.arcadeDrive(forwardSpeed, rotationSpeed); + // Auto-align when requested + if(controller.getAButton() && targetVisible){ + // Driver wants auto-alignment to tag 7 + // And, tag 7 is in sight, so we can turn toward it. + // Override the driver's turn command with an automatic one that turns toward the tag. + turn = -1.0 * targetYaw * VISION_TURN_kP * Constants.Swerve.kMaxAngularSpeed; + } + + // Command drivetrain motors based on target speeds + drivetrain.drive(forward, strafe, turn); + + // Put debug information to the dashboard + SmartDashboard.putBoolean("Vision Target Visible", targetVisible); + } + + @Override + public void simulationPeriodic() { + // Update drivetrain simulation + drivetrain.simulationPeriodic(); + + // Update camera simulation + visionSim.simulationPeriodic(drivetrain.getSimPose()); + + var debugField = visionSim.getSimDebugField(); + debugField.getObject("EstimatedRobot").setPose(drivetrain.getPose()); + debugField.getObject("EstimatedRobotModules").setPoses(drivetrain.getModulePoses()); + + // Calculate battery voltage sag due to current draw + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage(drivetrain.getCurrentDraw())); + } + + public void resetPose(){ + // Example Only - startPose should be derived from some assumption + // of where your robot was placed on the field. + // The first pose in an autonomous path is often a good choice. + var startPose = new Pose2d(1, 1, new Rotation2d()); + drivetrain.resetPose(startPose, true); + visionSim.resetSimPose(startPose); } } diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/VisionSim.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/VisionSim.java new file mode 100644 index 0000000000..c0335d267e --- /dev/null +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/VisionSim.java @@ -0,0 +1,86 @@ +/* + * 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. + */ + +package frc.robot; + +import static frc.robot.Constants.Vision.*; + + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; + +import org.photonvision.PhotonCamera; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; + +public class VisionSim { + + // Simulation + private PhotonCameraSim cameraSim; + private VisionSystemSim visionSim; + + public VisionSim(PhotonCamera cam_in) { + + // ----- Simulation + if (Robot.isSimulation()) { + // Create the vision system simulation which handles cameras and targets on the field. + visionSim = new VisionSystemSim("main"); + // Add all the AprilTags inside the tag layout as visible targets to this simulated field. + visionSim.addAprilTags(kTagLayout); + // Create simulated camera properties. These can be set to mimic your actual camera. + var cameraProp = new SimCameraProperties(); + cameraProp.setCalibration(320, 240, Rotation2d.fromDegrees(90)); + cameraProp.setCalibError(0.35, 0.10); + cameraProp.setFPS(70); + cameraProp.setAvgLatencyMs(30); + cameraProp.setLatencyStdDevMs(10); + // Create a PhotonCameraSim which will update the linked PhotonCamera's values with visible + // targets. + cameraSim = new PhotonCameraSim(cam_in, cameraProp); + // Add the simulated camera to view the targets on this simulated field. + visionSim.addCamera(cameraSim, kRobotToCam); + + cameraSim.enableDrawWireframe(true); + } + } + + // ----- Simulation + + public void simulationPeriodic(Pose2d robotSimPose) { + visionSim.update(robotSimPose); + } + + /** Reset pose history of the robot in the vision system simulation. */ + public void resetSimPose(Pose2d pose) { + if (Robot.isSimulation()) visionSim.resetRobotPose(pose); + } + + /** A Field2d for visualizing our robot and objects on the field. */ + public Field2d getSimDebugField() { + if (!Robot.isSimulation()) return null; + return visionSim.getDebugField(); + } +} diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java new file mode 100644 index 0000000000..b41017a3e1 --- /dev/null +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -0,0 +1,332 @@ +/* + * 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. + */ + +package frc.robot.subsystems.drivetrain; + +import static frc.robot.Constants.Swerve.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.*; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.ADXRS450_Gyro; +import edu.wpi.first.wpilibj.SPI.Port; +import edu.wpi.first.wpilibj.simulation.ADXRS450_GyroSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Robot; + +public class SwerveDrive { + // Construct the swerve modules with their respective constants. + // The SwerveModule class will handle all the details of controlling the modules. + private final SwerveModule[] swerveMods = { + new SwerveModule(ModuleConstants.FL), + new SwerveModule(ModuleConstants.FR), + new SwerveModule(ModuleConstants.BL), + new SwerveModule(ModuleConstants.BR) + }; + + // The kinematics for translating between robot chassis speeds and module states. + private final SwerveDriveKinematics kinematics = + new SwerveDriveKinematics( + swerveMods[0].getModuleConstants().centerOffset, + swerveMods[1].getModuleConstants().centerOffset, + swerveMods[2].getModuleConstants().centerOffset, + swerveMods[3].getModuleConstants().centerOffset); + + private final ADXRS450_Gyro gyro = new ADXRS450_Gyro(Port.kOnboardCS0); + + // The robot pose estimator for tracking swerve odometry and applying vision corrections. + private final SwerveDrivePoseEstimator poseEstimator; + + private ChassisSpeeds targetChassisSpeeds = new ChassisSpeeds(); + + // ----- Simulation + private final ADXRS450_GyroSim gyroSim; + private final SwerveDriveSim swerveDriveSim; + private double totalCurrentDraw = 0; + + public SwerveDrive() { + // Define the standard deviations for the pose estimator, which determine how fast the pose + // estimate converges to the vision measurement. This should depend on the vision measurement + // noise + // and how many or how frequently vision measurements are applied to the pose estimator. + var stateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); + var visionStdDevs = VecBuilder.fill(1, 1, 1); + poseEstimator = + new SwerveDrivePoseEstimator( + kinematics, + getGyroYaw(), + getModulePositions(), + new Pose2d(), + stateStdDevs, + visionStdDevs); + + // ----- Simulation + gyroSim = new ADXRS450_GyroSim(gyro); + swerveDriveSim = + new SwerveDriveSim( + kDriveFF, + DCMotor.getFalcon500(1), + kDriveGearRatio, + kWheelDiameter / 2.0, + kSteerFF, + DCMotor.getFalcon500(1), + kSteerGearRatio, + kinematics); + } + + public void periodic() { + for (SwerveModule module : swerveMods) { + module.periodic(); + } + + // Update the odometry of the swerve drive using the wheel encoders and gyro. + poseEstimator.update(getGyroYaw(), getModulePositions()); + } + + /** + * Basic drive control. A target field-relative ChassisSpeeds (vx, vy, omega) is converted to + * specific swerve module states. + * + * @param vxMeters X velocity (forwards/backwards) + * @param vyMeters Y velocity (strafe left/right) + * @param omegaRadians Angular velocity (rotation CCW+) + */ + public void drive(double vxMeters, double vyMeters, double omegaRadians) { + var targetChassisSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds(vxMeters, vyMeters, omegaRadians, getHeading()); + setChassisSpeeds(targetChassisSpeeds, true, false); + } + + /** + * Command the swerve drive to the desired chassis speeds by converting them to swerve module + * states and using {@link #setModuleStates(SwerveModuleState[], boolean)}. + * + * @param targetChassisSpeeds Target robot-relative chassis speeds (vx, vy, omega). + * @param openLoop If swerve modules should use feedforward only and ignore velocity feedback + * control. + * @param steerInPlace If modules should steer to the target angle when target velocity is 0. + */ + public void setChassisSpeeds( + ChassisSpeeds targetChassisSpeeds, boolean openLoop, boolean steerInPlace) { + setModuleStates(kinematics.toSwerveModuleStates(targetChassisSpeeds), openLoop, steerInPlace); + this.targetChassisSpeeds = targetChassisSpeeds; + } + + /** + * Command the swerve modules to the desired states. Velocites exceeding the maximum speed will be + * desaturated (while preserving the ratios between modules). + * + * @param openLoop If swerve modules should use feedforward only and ignore velocity feedback + * control. + * @param steerInPlace If modules should steer to the target angle when target velocity is 0. + */ + public void setModuleStates( + SwerveModuleState[] desiredStates, boolean openLoop, boolean steerInPlace) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, kMaxLinearSpeed); + for (int i = 0; i < swerveMods.length; i++) { + swerveMods[i].setDesiredState(desiredStates[i], openLoop, steerInPlace); + } + } + + /** Stop the swerve drive. */ + public void stop() { + drive(0, 0, 0); + } + + /** See {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double)}. */ + public void addVisionMeasurement(Pose2d visionMeasurement, double timestampSeconds) { + poseEstimator.addVisionMeasurement(visionMeasurement, timestampSeconds); + } + + /** See {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double, Matrix)}. */ + public void addVisionMeasurement( + Pose2d visionMeasurement, double timestampSeconds, Matrix stdDevs) { + poseEstimator.addVisionMeasurement(visionMeasurement, timestampSeconds, stdDevs); + } + + /** + * Reset the estimated pose of the swerve drive on the field. + * + * @param pose New robot pose. + * @param resetSimPose If the simulated robot pose should also be reset. This effectively + * teleports the robot and should only be used during the setup of the simulation world. + */ + public void resetPose(Pose2d pose, boolean resetSimPose) { + if (resetSimPose) { + swerveDriveSim.reset(pose, false); + // we shouldnt realistically be resetting pose after startup, but we will handle it anyway for + // testing + for (int i = 0; i < swerveMods.length; i++) { + swerveMods[i].simulationUpdate(0, 0, 0, 0, 0, 0); + } + gyroSim.setAngle(-pose.getRotation().getDegrees()); + gyroSim.setRate(0); + } + + poseEstimator.resetPosition(getGyroYaw(), getModulePositions(), pose); + } + + /** Get the estimated pose of the swerve drive on the field. */ + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** The heading of the swerve drive's estimated pose on the field. */ + public Rotation2d getHeading() { + return getPose().getRotation(); + } + + /** Raw gyro yaw (this may not match the field heading!). */ + public Rotation2d getGyroYaw() { + return gyro.getRotation2d(); + } + + /** Get the chassis speeds of the robot (vx, vy, omega) from the swerve module states. */ + public ChassisSpeeds getChassisSpeeds() { + return kinematics.toChassisSpeeds(getModuleStates()); + } + + /** + * Get the SwerveModuleState of each swerve module (speed, angle). The returned array order + * matches the kinematics module order. + */ + public SwerveModuleState[] getModuleStates() { + return new SwerveModuleState[] { + swerveMods[0].getState(), + swerveMods[1].getState(), + swerveMods[2].getState(), + swerveMods[3].getState() + }; + } + + /** + * Get the SwerveModulePosition of each swerve module (position, angle). The returned array order + * matches the kinematics module order. + */ + public SwerveModulePosition[] getModulePositions() { + return new SwerveModulePosition[] { + swerveMods[0].getPosition(), + swerveMods[1].getPosition(), + swerveMods[2].getPosition(), + swerveMods[3].getPosition() + }; + } + + /** + * Get the Pose2d of each swerve module based on kinematics and current robot pose. The returned + * array order matches the kinematics module order. + */ + public Pose2d[] getModulePoses() { + Pose2d[] modulePoses = new Pose2d[swerveMods.length]; + for (int i = 0; i < swerveMods.length; i++) { + var module = swerveMods[i]; + modulePoses[i] = + getPose() + .transformBy( + new Transform2d( + module.getModuleConstants().centerOffset, module.getAbsoluteHeading())); + } + return modulePoses; + } + + /** Log various drivetrain values to the dashboard. */ + public void log() { + String table = "Drive/"; + Pose2d pose = getPose(); + SmartDashboard.putNumber(table + "X", pose.getX()); + SmartDashboard.putNumber(table + "Y", pose.getY()); + SmartDashboard.putNumber(table + "Heading", pose.getRotation().getDegrees()); + ChassisSpeeds chassisSpeeds = getChassisSpeeds(); + SmartDashboard.putNumber(table + "VX", chassisSpeeds.vxMetersPerSecond); + SmartDashboard.putNumber(table + "VY", chassisSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber( + table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond)); + SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vxMetersPerSecond); + SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber( + table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omegaRadiansPerSecond)); + + for (SwerveModule module : swerveMods) { + module.log(); + } + } + + // ----- Simulation + + public void simulationPeriodic() { + // Pass commanded motor voltages into swerve drive simulation + double[] driveInputs = new double[swerveMods.length]; + double[] steerInputs = new double[swerveMods.length]; + for (int i = 0; i < swerveMods.length; i++) { + driveInputs[i] = swerveMods[i].getDriveVoltage(); + steerInputs[i] = swerveMods[i].getSteerVoltage(); + } + swerveDriveSim.setDriveInputs(driveInputs); + swerveDriveSim.setSteerInputs(steerInputs); + + // Simulate one timestep + swerveDriveSim.update(Robot.kDefaultPeriod); + + // Update module and gyro values with simulated values + var driveStates = swerveDriveSim.getDriveStates(); + var steerStates = swerveDriveSim.getSteerStates(); + totalCurrentDraw = 0; + double[] driveCurrents = swerveDriveSim.getDriveCurrentDraw(); + for (double current : driveCurrents) totalCurrentDraw += current; + double[] steerCurrents = swerveDriveSim.getSteerCurrentDraw(); + for (double current : steerCurrents) totalCurrentDraw += current; + for (int i = 0; i < swerveMods.length; i++) { + double drivePos = driveStates.get(i).get(0, 0); + double driveRate = driveStates.get(i).get(1, 0); + double steerPos = steerStates.get(i).get(0, 0); + double steerRate = steerStates.get(i).get(1, 0); + swerveMods[i].simulationUpdate( + drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]); + } + + gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec()); + gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees()); + } + + /** + * The "actual" pose of the robot on the field used in simulation. This is different from the + * swerve drive's estimated pose. + */ + public Pose2d getSimPose() { + return swerveDriveSim.getPose(); + } + + public double getCurrentDraw() { + return totalCurrentDraw; + } +} diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java new file mode 100644 index 0000000000..85849141d4 --- /dev/null +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java @@ -0,0 +1,495 @@ +/* + * 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. + */ + +package frc.robot.subsystems.drivetrain; + +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +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.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.Discretization; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.RobotController; +import java.util.ArrayList; +import java.util.List; +import java.util.Random; + +/** + * This class attempts to simulate the dynamics of a swerve drive. In simulationPeriodic, users + * should first set inputs from motors with {@link #setDriveInputs(double...)} and {@link + * #setSteerInputs(double...)}, call {@link #update(double)} to update the simulation, and then set + * swerve module's encoder values and the drivetrain's gyro values with the results from this class. + * + *

In this class, distances are expressed with meters, angles with radians, and inputs with + * voltages. + * + *

Teams can use {@link edu.wpi.first.wpilibj.smartdashboard.Field2d} to visualize their robot on + * the Sim GUI's field. + */ +public class SwerveDriveSim { + private final LinearSystem drivePlant; + private final double driveKs; + private final DCMotor driveMotor; + private final double driveGearing; + private final double driveWheelRadius; + private final LinearSystem steerPlant; + private final double steerKs; + private final DCMotor steerMotor; + private final double steerGearing; + + private final SwerveDriveKinematics kinematics; + private final int numModules; + + private final double[] driveInputs; + private final List> driveStates; + private final double[] steerInputs; + private final List> steerStates; + + private final Random rand = new Random(); + + // noiseless "actual" pose of the robot on the field + private Pose2d pose = new Pose2d(); + private double omegaRadsPerSec = 0; + + /** + * Creates a swerve drive simulation. + * + * @param driveFF The feedforward for the drive motors of this swerve drive. This should be in + * units of meters. + * @param driveMotor The DCMotor model for the drive motor(s) of this swerve drive's modules. This + * should not have any gearing applied. + * @param driveGearing The gear ratio of the drive system. Positive values indicate a reduction + * where one rotation of the drive wheel equals driveGearing rotations of the drive motor. + * @param driveWheelRadius The radius of the module's driving wheel. + * @param steerFF The feedforward for the steer motors of this swerve drive. This should be in + * units of radians. + * @param steerMotor The DCMotor model for the steer motor(s) of this swerve drive's modules. This + * should not have any gearing applied. + * @param steerGearing The gear ratio of the steer system. Positive values indicate a reduction + * where one rotation of the module heading/azimuth equals steerGearing rotations of the steer + * motor. + * @param kinematics The kinematics for this swerve drive. All swerve module information used in + * this class should match the order of the modules this kinematics object was constructed + * with. + */ + public SwerveDriveSim( + SimpleMotorFeedforward driveFF, + DCMotor driveMotor, + double driveGearing, + double driveWheelRadius, + SimpleMotorFeedforward steerFF, + DCMotor steerMotor, + double steerGearing, + SwerveDriveKinematics kinematics) { + this( + new LinearSystem( + MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -driveFF.kv / driveFF.ka), + VecBuilder.fill(0.0, 1.0 / driveFF.ka), + MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0), + VecBuilder.fill(0.0, 0.0)), + driveFF.ks, + driveMotor, + driveGearing, + driveWheelRadius, + new LinearSystem( + MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -steerFF.kv / steerFF.ka), + VecBuilder.fill(0.0, 1.0 / steerFF.ka), + MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0), + VecBuilder.fill(0.0, 0.0)), + steerFF.ks, + steerMotor, + steerGearing, + kinematics); + } + + /** + * Creates a swerve drive simulation. + * + * @param drivePlant The {@link LinearSystem} representing a swerve module's drive system. The + * state should be in units of meters and input in volts. + * @param driveKs The static gain in volts of the drive system's feedforward, or the minimum + * voltage to cause motion. Set this to 0 to ignore static friction. + * @param driveMotor The DCMotor model for the drive motor(s) of this swerve drive's modules. This + * should not have any gearing applied. + * @param driveGearing The gear ratio of the drive system. Positive values indicate a reduction + * where one rotation of the drive wheel equals driveGearing rotations of the drive motor. + * @param driveWheelRadius The radius of the module's driving wheel. + * @param steerPlant The {@link LinearSystem} representing a swerve module's steer system. The + * state should be in units of radians and input in volts. + * @param steerKs The static gain in volts of the steer system's feedforward, or the minimum + * voltage to cause motion. Set this to 0 to ignore static friction. + * @param steerMotor The DCMotor model for the steer motor(s) of this swerve drive's modules. This + * should not have any gearing applied. + * @param steerGearing The gear ratio of the steer system. Positive values indicate a reduction + * where one rotation of the module heading/azimuth equals steerGearing rotations of the steer + * motor. + * @param kinematics The kinematics for this swerve drive. All swerve module information used in + * this class should match the order of the modules this kinematics object was constructed + * with. + */ + public SwerveDriveSim( + LinearSystem drivePlant, + double driveKs, + DCMotor driveMotor, + double driveGearing, + double driveWheelRadius, + LinearSystem steerPlant, + double steerKs, + DCMotor steerMotor, + double steerGearing, + SwerveDriveKinematics kinematics) { + this.drivePlant = drivePlant; + this.driveKs = driveKs; + this.driveMotor = driveMotor; + this.driveGearing = driveGearing; + this.driveWheelRadius = driveWheelRadius; + this.steerPlant = steerPlant; + this.steerKs = steerKs; + this.steerMotor = steerMotor; + this.steerGearing = steerGearing; + + this.kinematics = kinematics; + numModules = kinematics.toSwerveModuleStates(new ChassisSpeeds()).length; + driveInputs = new double[numModules]; + driveStates = new ArrayList<>(numModules); + steerInputs = new double[numModules]; + steerStates = new ArrayList<>(numModules); + for (int i = 0; i < numModules; i++) { + driveStates.add(VecBuilder.fill(0, 0)); + steerStates.add(VecBuilder.fill(0, 0)); + } + } + + /** + * Sets the input voltages of the drive motors. + * + * @param inputs Input voltages. These should match the module order used in the kinematics. + */ + public void setDriveInputs(double... inputs) { + final double battVoltage = RobotController.getBatteryVoltage(); + for (int i = 0; i < driveInputs.length; i++) { + double input = inputs[i]; + driveInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage); + } + } + + /** + * Sets the input voltages of the steer motors. + * + * @param inputs Input voltages. These should match the module order used in the kinematics. + */ + public void setSteerInputs(double... inputs) { + final double battVoltage = RobotController.getBatteryVoltage(); + for (int i = 0; i < steerInputs.length; i++) { + double input = inputs[i]; + steerInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage); + } + } + + /** + * Computes the new x given the old x and the control input. Includes the effect of static + * friction. + * + * @param discA The discretized system matrix. + * @param discB The discretized input matrix. + * @param x The position/velocity state of the drive/steer system. + * @param input The input voltage. + * @param ks The kS value of the feedforward model. + * @return The updated x, including the effect of static friction. + */ + protected static Matrix calculateX( + Matrix discA, Matrix discB, Matrix x, double input, double ks) { + var Ax = discA.times(x); + double nextStateVel = Ax.get(1, 0); + // input required to make next state vel == 0 + double inputToStop = nextStateVel / -discB.get(1, 0); + // ks effect on system velocity + double ksSystemEffect = MathUtil.clamp(inputToStop, -ks, ks); + + // after ks system effect: + nextStateVel += discB.get(1, 0) * ksSystemEffect; + inputToStop = nextStateVel / -discB.get(1, 0); + double signToStop = Math.signum(inputToStop); + double inputSign = Math.signum(input); + double ksInputEffect = 0; + + // system velocity was reduced to 0, resist any input + if (Math.abs(ksSystemEffect) < ks) { + double absInput = Math.abs(input); + ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput); + } + // non-zero system velocity, but input causes velocity sign change. Resist input after sign + // change + else if ((input * signToStop) > (inputToStop * signToStop)) { + double absInput = Math.abs(input - inputToStop); + ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput); + } + + // calculate next x including static friction + var Bu = discB.times(VecBuilder.fill(input + ksSystemEffect + ksInputEffect)); + return Ax.plus(Bu); + } + + /** + * Update the drivetrain states with the given timestep. + * + * @param dtSeconds The timestep in seconds. This should be the robot loop period. + */ + public void update(double dtSeconds) { + var driveDiscAB = Discretization.discretizeAB(drivePlant.getA(), drivePlant.getB(), dtSeconds); + var steerDiscAB = Discretization.discretizeAB(steerPlant.getA(), steerPlant.getB(), dtSeconds); + + var moduleDeltas = new SwerveModulePosition[numModules]; + for (int i = 0; i < numModules; i++) { + double prevDriveStatePos = driveStates.get(i).get(0, 0); + driveStates.set( + i, + calculateX( + driveDiscAB.getFirst(), + driveDiscAB.getSecond(), + driveStates.get(i), + driveInputs[i], + driveKs)); + double currDriveStatePos = driveStates.get(i).get(0, 0); + steerStates.set( + i, + calculateX( + steerDiscAB.getFirst(), + steerDiscAB.getSecond(), + steerStates.get(i), + steerInputs[i], + steerKs)); + double currSteerStatePos = steerStates.get(i).get(0, 0); + moduleDeltas[i] = + new SwerveModulePosition( + currDriveStatePos - prevDriveStatePos, new Rotation2d(currSteerStatePos)); + } + + var twist = kinematics.toTwist2d(moduleDeltas); + pose = pose.exp(twist); + omegaRadsPerSec = twist.dtheta / dtSeconds; + } + + /** + * Reset the simulated swerve drive state. This effectively teleports the robot and should only be + * used during the setup of the simulation world. + * + * @param pose The new pose of the simulated swerve drive. + * @param preserveMotion If true, the current module states will be preserved. Otherwise, they + * will be reset to zeros. + */ + public void reset(Pose2d pose, boolean preserveMotion) { + this.pose = pose; + if (!preserveMotion) { + for (int i = 0; i < numModules; i++) { + driveStates.set(i, VecBuilder.fill(0, 0)); + steerStates.set(i, VecBuilder.fill(0, 0)); + } + omegaRadsPerSec = 0; + } + } + + /** + * Reset the simulated swerve drive state. This effectively teleports the robot and should only be + * used during the setup of the simulation world. + * + * @param pose The new pose of the simulated swerve drive. + * @param moduleDriveStates The new states of the modules' drive systems in [meters, meters/sec]. + * These should match the module order used in the kinematics. + * @param moduleSteerStates The new states of the modules' steer systems in [radians, + * radians/sec]. These should match the module order used in the kinematics. + */ + public void reset( + Pose2d pose, List> moduleDriveStates, List> moduleSteerStates) { + if (moduleDriveStates.size() != driveStates.size() + || moduleSteerStates.size() != steerStates.size()) + throw new IllegalArgumentException("Provided module states do not match number of modules!"); + this.pose = pose; + for (int i = 0; i < numModules; i++) { + driveStates.set(i, moduleDriveStates.get(i).copy()); + steerStates.set(i, moduleSteerStates.get(i).copy()); + } + omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond; + } + + /** + * Get the pose of the simulated swerve drive. Note that this is the "actual" pose of the robot in + * the simulation world, without any noise. If you are simulating a pose estimator, this pose + * should only be used for visualization or camera simulation. This should be called after {@link + * #update(double)}. + */ + public Pose2d getPose() { + return pose; + } + + /** + * Get the {@link SwerveModulePosition} of each module. The returned array order matches the + * kinematics module order. This should be called after {@link #update(double)}. + */ + public SwerveModulePosition[] getModulePositions() { + var positions = new SwerveModulePosition[numModules]; + for (int i = 0; i < numModules; i++) { + positions[i] = + new SwerveModulePosition( + driveStates.get(i).get(0, 0), new Rotation2d(steerStates.get(i).get(0, 0))); + } + return positions; + } + + /** + * Get the {@link SwerveModulePosition} of each module with rudimentary noise simulation. The + * returned array order matches the kinematics module order. This should be called after {@link + * #update(double)}. + * + * @param driveStdDev The standard deviation in meters of the drive wheel position. + * @param steerStdDev The standard deviation in radians of the steer angle. + */ + public SwerveModulePosition[] getNoisyModulePositions(double driveStdDev, double steerStdDev) { + var positions = new SwerveModulePosition[numModules]; + for (int i = 0; i < numModules; i++) { + positions[i] = + new SwerveModulePosition( + driveStates.get(i).get(0, 0) + rand.nextGaussian() * driveStdDev, + new Rotation2d(steerStates.get(i).get(0, 0) + rand.nextGaussian() * steerStdDev)); + } + return positions; + } + + /** + * Get the {@link SwerveModuleState} of each module. The returned array order matches the + * kinematics module order. This should be called after {@link #update(double)}. + */ + public SwerveModuleState[] getModuleStates() { + var positions = new SwerveModuleState[numModules]; + for (int i = 0; i < numModules; i++) { + positions[i] = + new SwerveModuleState( + driveStates.get(i).get(1, 0), new Rotation2d(steerStates.get(i).get(0, 0))); + } + return positions; + } + + /** + * Get the state of each module's drive system in [meters, meters/sec]. The returned list order + * matches the kinematics module order. This should be called after {@link #update(double)}. + */ + public List> getDriveStates() { + List> states = new ArrayList<>(); + for (int i = 0; i < driveStates.size(); i++) { + states.add(driveStates.get(i).copy()); + } + return states; + } + + /** + * Get the state of each module's steer system in [radians, radians/sec]. The returned list order + * matches the kinematics module order. This should be called after {@link #update(double)}. + */ + public List> getSteerStates() { + List> states = new ArrayList<>(); + for (int i = 0; i < steerStates.size(); i++) { + states.add(steerStates.get(i).copy()); + } + return states; + } + + /** + * Get the angular velocity of the robot, which can be useful for gyro simulation. CCW positive. + * This should be called after {@link #update(double)}. + */ + public double getOmegaRadsPerSec() { + return omegaRadsPerSec; + } + + /** + * Calculates the current drawn from the battery by the motor(s). Ignores regenerative current + * from back-emf. + * + * @param motor The motor(s) used. + * @param radiansPerSec The velocity of the motor in radians per second. + * @param inputVolts The voltage commanded by the motor controller (battery voltage * duty cycle). + * @param battVolts The voltage of the battery. + */ + protected static double getCurrentDraw( + DCMotor motor, double radiansPerSec, double inputVolts, double battVolts) { + double effVolts = inputVolts - radiansPerSec / motor.KvRadPerSecPerVolt; + // ignore regeneration + if (inputVolts >= 0) effVolts = MathUtil.clamp(effVolts, 0, inputVolts); + else effVolts = MathUtil.clamp(effVolts, inputVolts, 0); + // calculate battery current + return (inputVolts / battVolts) * (effVolts / motor.rOhms); + } + + /** + * Get the current draw in amps for each module's drive motor(s). This should be called after + * {@link #update(double)}. The returned array order matches the kinematics module order. + */ + public double[] getDriveCurrentDraw() { + double[] currents = new double[numModules]; + for (int i = 0; i < numModules; i++) { + double radiansPerSec = driveStates.get(i).get(1, 0) * driveGearing / driveWheelRadius; + currents[i] = + getCurrentDraw( + driveMotor, radiansPerSec, driveInputs[i], RobotController.getBatteryVoltage()); + } + return currents; + } + + /** + * Get the current draw in amps for each module's steer motor(s). This should be called after + * {@link #update(double)}. The returned array order matches the kinematics module order. + */ + public double[] getSteerCurrentDraw() { + double[] currents = new double[numModules]; + for (int i = 0; i < numModules; i++) { + double radiansPerSec = steerStates.get(i).get(1, 0) * steerGearing; + currents[i] = + getCurrentDraw( + steerMotor, radiansPerSec, steerInputs[i], RobotController.getBatteryVoltage()); + } + return currents; + } + + /** + * Get the total current draw in amps of all swerve motors. This should be called after {@link + * #update(double)}. + */ + public double getTotalCurrentDraw() { + double sum = 0; + for (double val : getDriveCurrentDraw()) sum += val; + for (double val : getSteerCurrentDraw()) sum += val; + return sum; + } +} diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java new file mode 100644 index 0000000000..4615d29217 --- /dev/null +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -0,0 +1,192 @@ +/* + * 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. + */ + +package frc.robot.subsystems.drivetrain; + +import static frc.robot.Constants.Swerve.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj.simulation.EncoderSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class SwerveModule { + // --- Module Constants + private final ModuleConstants moduleConstants; + + // --- Hardware + private final PWMSparkMax driveMotor; + private final Encoder driveEncoder; + private final PWMSparkMax steerMotor; + private final Encoder steerEncoder; + + // --- Control + private SwerveModuleState desiredState = new SwerveModuleState(); + private boolean openLoop = false; + + // Simple PID feedback controllers run on the roborio + private PIDController drivePidController = new PIDController(kDriveKP, kDriveKI, kDriveKD); + // (A profiled steering PID controller may give better results by utilizing feedforward.) + private PIDController steerPidController = new PIDController(kSteerKP, kSteerKI, kSteerKD); + + // --- Simulation + private final EncoderSim driveEncoderSim; + private double driveCurrentSim = 0; + private final EncoderSim steerEncoderSim; + private double steerCurrentSim = 0; + + public SwerveModule(ModuleConstants moduleConstants) { + this.moduleConstants = moduleConstants; + + driveMotor = new PWMSparkMax(moduleConstants.driveMotorID); + driveEncoder = new Encoder(moduleConstants.driveEncoderA, moduleConstants.driveEncoderB); + driveEncoder.setDistancePerPulse(kDriveDistPerPulse); + steerMotor = new PWMSparkMax(moduleConstants.steerMotorID); + steerEncoder = new Encoder(moduleConstants.steerEncoderA, moduleConstants.steerEncoderB); + steerEncoder.setDistancePerPulse(kSteerRadPerPulse); + + steerPidController.enableContinuousInput(-Math.PI, Math.PI); + + // --- Simulation + driveEncoderSim = new EncoderSim(driveEncoder); + steerEncoderSim = new EncoderSim(steerEncoder); + } + + public void periodic() { + // Perform PID feedback control to steer the module to the target angle + double steerPid = + steerPidController.calculate( + getAbsoluteHeading().getRadians(), desiredState.angle.getRadians()); + steerMotor.setVoltage(steerPid); + + // Use feedforward model to translate target drive velocities into voltages + double driveFF = kDriveFF.calculate(desiredState.speedMetersPerSecond); + double drivePid = 0; + if (!openLoop) { + // Perform PID feedback control to compensate for disturbances + drivePid = + drivePidController.calculate(driveEncoder.getRate(), desiredState.speedMetersPerSecond); + } + + driveMotor.setVoltage(driveFF + drivePid); + } + + /** + * Command this swerve module to the desired angle and velocity. + * + * @param steerInPlace If modules should steer to target angle when target velocity is 0 + */ + public void setDesiredState( + SwerveModuleState desiredState, boolean openLoop, boolean steerInPlace) { + Rotation2d currentRotation = getAbsoluteHeading(); + // Avoid turning more than 90 degrees by inverting speed on large angle changes + desiredState = SwerveModuleState.optimize(desiredState, currentRotation); + + this.desiredState = desiredState; + } + + /** Module heading reported by steering encoder. */ + public Rotation2d getAbsoluteHeading() { + return Rotation2d.fromRadians(steerEncoder.getDistance()); + } + + /** + * {@link SwerveModuleState} describing absolute module rotation and velocity in meters per + * second. + */ + public SwerveModuleState getState() { + return new SwerveModuleState(driveEncoder.getRate(), getAbsoluteHeading()); + } + + /** {@link SwerveModulePosition} describing absolute module rotation and position in meters. */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(driveEncoder.getDistance(), getAbsoluteHeading()); + } + + /** Voltage of the drive motor */ + public double getDriveVoltage() { + return driveMotor.get() * RobotController.getBatteryVoltage(); + } + + /** Voltage of the steer motor */ + public double getSteerVoltage() { + return steerMotor.get() * RobotController.getBatteryVoltage(); + } + + /** + * Constants about this module, like motor IDs, encoder angle offset, and translation from center. + */ + public ModuleConstants getModuleConstants() { + return moduleConstants; + } + + public void log() { + var state = getState(); + + String table = "Module " + moduleConstants.moduleNum + "/"; + SmartDashboard.putNumber( + table + "Steer Degrees", Math.toDegrees(MathUtil.angleModulus(state.angle.getRadians()))); + SmartDashboard.putNumber( + table + "Steer Target Degrees", Math.toDegrees(steerPidController.getSetpoint())); + SmartDashboard.putNumber( + table + "Drive Velocity Feet", Units.metersToFeet(state.speedMetersPerSecond)); + SmartDashboard.putNumber( + table + "Drive Velocity Target Feet", + Units.metersToFeet(desiredState.speedMetersPerSecond)); + SmartDashboard.putNumber(table + "Drive Current", driveCurrentSim); + SmartDashboard.putNumber(table + "Steer Current", steerCurrentSim); + } + + // ----- Simulation + + public void simulationUpdate( + double driveEncoderDist, + double driveEncoderRate, + double driveCurrent, + double steerEncoderDist, + double steerEncoderRate, + double steerCurrent) { + driveEncoderSim.setDistance(driveEncoderDist); + driveEncoderSim.setRate(driveEncoderRate); + this.driveCurrentSim = driveCurrent; + steerEncoderSim.setDistance(steerEncoderDist); + steerEncoderSim.setRate(steerEncoderRate); + this.steerCurrentSim = steerCurrent; + } + + public double getDriveCurrentSim() { + return driveCurrentSim; + } + + public double getSteerCurrentSim() { + return steerCurrentSim; + } +} diff --git a/photonlib-java-examples/aimattarget/swerve_module.png b/photonlib-java-examples/aimattarget/swerve_module.png new file mode 100644 index 0000000000..25990c8399 Binary files /dev/null and b/photonlib-java-examples/aimattarget/swerve_module.png differ diff --git a/photonlib-java-examples/aimattarget/tag-blue.png b/photonlib-java-examples/aimattarget/tag-blue.png new file mode 100644 index 0000000000..04b9e4f7b7 Binary files /dev/null and b/photonlib-java-examples/aimattarget/tag-blue.png differ diff --git a/photonlib-java-examples/aimattarget/tag-green.png b/photonlib-java-examples/aimattarget/tag-green.png new file mode 100644 index 0000000000..63029fcf20 Binary files /dev/null and b/photonlib-java-examples/aimattarget/tag-green.png differ