Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Examples Clean-Up (#1408)

Browse Source
This commit is contained in:
Chris Gerth
2024-09-14 23:10:02 -05:00
committed by GitHub
parent 596c87519c
commit 9e6a066561
269 changed files with 9346 additions and 3734 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
photonlib-java-examples/aimandrange/.gitignore vendored
View File

@@ -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

2
photonlib-java-examples/aimandrange/.wpilib/wpilib_preferences.json
View File

@@ -2,5 +2,5 @@
"enableCppIntellisense": false,
"currentLanguage": "java",
"projectYear": "2024",
"teamNumber": 6995
"teamNumber": 4512
}

4
photonlib-java-examples/aimandrange/README.md
View File

@@ -1,3 +1,3 @@
## **`aimandrange`**
## **`aimattarget`**
### See [PhotonLib Java Examples](../README.md#aimandrange)
### See [PhotonLib Java Examples](../README.md#aimattarget)

2
photonlib-java-examples/aimandrange/WPILib-License.md
View File

@@ -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

9
photonlib-java-examples/aimandrange/build.gradle
View File

@@ -10,6 +10,11 @@ apply from: "${rootDir}/../shared/examples_common.gradle"
def ROBOT_MAIN_CLASS = "frc.robot.Main"
wpi.maven.useDevelopment = true
wpi.versions.wpilibVersion = "2024.3.2"
wpi.versions.wpimathVersion = "2024.3.2"
// Define my targets (RoboRIO) and artifacts (deployable files)
// This is added by GradleRIO's backing project DeployUtils.
deploy {
@@ -19,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 {
@@ -48,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()

BIN
photonlib-java-examples/aimandrange/gradle/wrapper/gradle-wrapper.jar vendored Normal file
View File

Binary file not shown.

5
photonlib-java-examples/aimandrange/gradle/wrapper/gradle-wrapper.properties vendored Normal file
View File

@@ -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

241
photonlib-java-examples/aimandrange/gradlew vendored Normal file
View File

@@ -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" "$@"

91
photonlib-java-examples/aimandrange/gradlew.bat vendored Normal file
View File

@@ -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

2
photonlib-java-examples/aimandrange/settings.gradle
View File

@@ -1,7 +1,5 @@
import org.gradle.internal.os.OperatingSystem
rootProject.name = 'aimandrange'
pluginManagement {
repositories {
mavenLocal()

9
photonlib-java-examples/aimandrange/simgui-ds.json
View File

@@ -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,

141
photonlib-java-examples/aimandrange/simgui.json
View File

@@ -2,17 +2,154 @@
"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": [
{
"axis": [
{
"autoFit": true
}
],
"backgroundColor": [
0.0,
0.0,
0.0,
0.8500000238418579
],
"height": 338,
"series": [
{
"color": [
0.2980392277240753,
0.44705885648727417,
0.6901960968971252,
1.0
],
"id": "NT:/SmartDashboard/Vision Target Range (m)"
}
]
}
]
}
}
}

144
photonlib-java-examples/aimandrange/src/main/java/frc/robot/Constants.java Normal file
View File

@@ -0,0 +1,144 @@
/*
* 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<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
public static final Matrix<N3, N1> 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;
}
}

10
photonlib-java-examples/aimandrange/src/main/java/frc/robot/Main.java
View File

@@ -26,19 +26,9 @@ package frc.robot;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}

173
photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java
View File

@@ -24,88 +24,139 @@
package frc.robot;
import edu.wpi.first.math.controller.PIDController;
import static frc.robot.Constants.Vision.*;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.TimedRobot;
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 frc.robot.subsystems.drivetrain.SwerveDrive;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonUtils;
/**
* 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;
// How far from the target we want to be
final double GOAL_RANGE_METERS = Units.feetToMeters(3);
private final double VISION_TURN_kP = 0.01;
private final double VISION_DES_ANGLE_deg = 0.0;
private final double VISION_STRAFE_kP = 0.5;
private final double VISION_DES_RANGE_m = 1.25;
// Change this to match the name of your camera
PhotonCamera camera = new PhotonCamera("photonvision");
private XboxController controller;
// 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() {
// 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;
if (xboxController.getAButton()) {
// Vision-alignment mode
// Query the latest result from PhotonVision
var result = camera.getLatestResult();
// 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;
double targetRange = 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()) {
// First calculate range
double range =
PhotonUtils.calculateDistanceToTargetMeters(
CAMERA_HEIGHT_METERS,
TARGET_HEIGHT_METERS,
CAMERA_PITCH_RADIANS,
Units.degreesToRadians(result.getBestTarget().getPitch()));
// 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();
targetRange =
PhotonUtils.calculateDistanceToTargetMeters(
0.5, // Measured with a tape measure, or in CAD.
1.435, // From 2024 game manual for ID 7
Units.degreesToRadians(-30.0), // Measured with a protractor, or in CAD.
Units.degreesToRadians(target.getPitch()));
// Use this range as the measurement we give to the PID controller.
// -1.0 required to ensure positive PID controller effort _increases_ range
forwardSpeed = -forwardController.calculate(range, GOAL_RANGE_METERS);
// Also calculate angular 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.
forwardSpeed = 0;
rotationSpeed = 0;
targetVisible = true;
}
}
}
} else {
// Manual Driver Mode
forwardSpeed = -xboxController.getRightY();
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 and fwd/rev command with an automatic one
// That turns toward the tag, and gets the range right.
turn =
(VISION_DES_ANGLE_deg - targetYaw) * VISION_TURN_kP * Constants.Swerve.kMaxAngularSpeed;
forward =
(VISION_DES_RANGE_m - targetRange) * VISION_STRAFE_kP * Constants.Swerve.kMaxLinearSpeed;
}
// Command drivetrain motors based on target speeds
drivetrain.drive(forward, strafe, turn);
// Put debug information to the dashboard
SmartDashboard.putBoolean("Vision Target Visible", targetVisible);
SmartDashboard.putNumber("Vision Target Range (m)", targetRange);
}
@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);
}
}

82
photonlib-java-examples/aimandrange/src/main/java/frc/robot/VisionSim.java Normal file
View File

@@ -0,0 +1,82 @@
/*
* 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();
}
}

332
photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java Normal file
View File

@@ -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<N3, N1> 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;
}
}

495
photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java Normal file
View File

@@ -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.
*
* <p>In this class, distances are expressed with meters, angles with radians, and inputs with
* voltages.
*
* <p>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<N2, N1, N2> drivePlant;
private final double driveKs;
private final DCMotor driveMotor;
private final double driveGearing;
private final double driveWheelRadius;
private final LinearSystem<N2, N1, N2> 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<Matrix<N2, N1>> driveStates;
private final double[] steerInputs;
private final List<Matrix<N2, N1>> 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<N2, N1, N2>(
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<N2, N1, N2>(
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<N2, N1, N2> drivePlant,
double driveKs,
DCMotor driveMotor,
double driveGearing,
double driveWheelRadius,
LinearSystem<N2, N1, N2> 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<N2, N1> calculateX(
Matrix<N2, N2> discA, Matrix<N2, N1> discB, Matrix<N2, N1> 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<Matrix<N2, N1>> moduleDriveStates, List<Matrix<N2, N1>> 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<Matrix<N2, N1>> getDriveStates() {
List<Matrix<N2, N1>> 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<Matrix<N2, N1>> getSteerStates() {
List<Matrix<N2, N1>> 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;
}
}

192
photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java Normal file
View File

@@ -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;
}
}

BIN
photonlib-java-examples/aimandrange/swerve_module.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 2.9 KiB

BIN
photonlib-java-examples/aimandrange/tag-blue.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 4.6 KiB

BIN
photonlib-java-examples/aimandrange/tag-green.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 4.6 KiB