mirror of
https://github.com/Astatin3/photonvision-2025.0.0-beta-6.git
synced 2026-06-09 00:28:06 -06:00
Initial commit
This commit is contained in:
astatin3
@@ -0,0 +1,173 @@
|
||||
# This gitignore has been specially created by the WPILib team.
|
||||
# If you remove items from this file, intellisense might break.
|
||||
|
||||
### C++ ###
|
||||
# Prerequisites
|
||||
*.d
|
||||
|
||||
# Compiled Object files
|
||||
*.slo
|
||||
*.lo
|
||||
*.o
|
||||
*.obj
|
||||
|
||||
# Precompiled Headers
|
||||
*.gch
|
||||
*.pch
|
||||
|
||||
# Compiled Dynamic libraries
|
||||
*.so
|
||||
*.dylib
|
||||
*.dll
|
||||
|
||||
# Fortran module files
|
||||
*.mod
|
||||
*.smod
|
||||
|
||||
# Compiled Static libraries
|
||||
*.lai
|
||||
*.la
|
||||
*.a
|
||||
*.lib
|
||||
|
||||
# Executables
|
||||
*.exe
|
||||
*.out
|
||||
*.app
|
||||
|
||||
### Java ###
|
||||
# Compiled class file
|
||||
*.class
|
||||
|
||||
# Log file
|
||||
*.log
|
||||
|
||||
# BlueJ files
|
||||
*.ctxt
|
||||
|
||||
# Mobile Tools for Java (J2ME)
|
||||
.mtj.tmp/
|
||||
|
||||
# Package Files #
|
||||
*.jar
|
||||
*.war
|
||||
*.nar
|
||||
*.ear
|
||||
*.zip
|
||||
*.tar.gz
|
||||
*.rar
|
||||
|
||||
# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml
|
||||
hs_err_pid*
|
||||
|
||||
### Linux ###
|
||||
*~
|
||||
|
||||
# temporary files which can be created if a process still has a handle open of a deleted file
|
||||
.fuse_hidden*
|
||||
|
||||
# KDE directory preferences
|
||||
.directory
|
||||
|
||||
# Linux trash folder which might appear on any partition or disk
|
||||
.Trash-*
|
||||
|
||||
# .nfs files are created when an open file is removed but is still being accessed
|
||||
.nfs*
|
||||
|
||||
### macOS ###
|
||||
# General
|
||||
.DS_Store
|
||||
.AppleDouble
|
||||
.LSOverride
|
||||
|
||||
# Icon must end with two \r
|
||||
Icon
|
||||
|
||||
# Thumbnails
|
||||
._*
|
||||
|
||||
# Files that might appear in the root of a volume
|
||||
.DocumentRevisions-V100
|
||||
.fseventsd
|
||||
.Spotlight-V100
|
||||
.TemporaryItems
|
||||
.Trashes
|
||||
.VolumeIcon.icns
|
||||
.com.apple.timemachine.donotpresent
|
||||
|
||||
# Directories potentially created on remote AFP share
|
||||
.AppleDB
|
||||
.AppleDesktop
|
||||
Network Trash Folder
|
||||
Temporary Items
|
||||
.apdisk
|
||||
|
||||
### VisualStudioCode ###
|
||||
.vscode/*
|
||||
!.vscode/settings.json
|
||||
!.vscode/tasks.json
|
||||
!.vscode/launch.json
|
||||
!.vscode/extensions.json
|
||||
|
||||
### Windows ###
|
||||
# Windows thumbnail cache files
|
||||
Thumbs.db
|
||||
ehthumbs.db
|
||||
ehthumbs_vista.db
|
||||
|
||||
# Dump file
|
||||
*.stackdump
|
||||
|
||||
# Folder config file
|
||||
[Dd]esktop.ini
|
||||
|
||||
# Recycle Bin used on file shares
|
||||
$RECYCLE.BIN/
|
||||
|
||||
# Windows Installer files
|
||||
*.cab
|
||||
*.msi
|
||||
*.msix
|
||||
*.msm
|
||||
*.msp
|
||||
|
||||
# Windows shortcuts
|
||||
*.lnk
|
||||
|
||||
### Gradle ###
|
||||
.gradle
|
||||
/build/
|
||||
|
||||
# Ignore Gradle GUI config
|
||||
gradle-app.setting
|
||||
|
||||
# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored)
|
||||
!gradle-wrapper.jar
|
||||
|
||||
# Cache of project
|
||||
.gradletasknamecache
|
||||
|
||||
# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898
|
||||
# gradle/wrapper/gradle-wrapper.properties
|
||||
|
||||
# # VS Code Specific Java Settings
|
||||
# DO NOT REMOVE .classpath and .project
|
||||
.classpath
|
||||
.project
|
||||
.settings/
|
||||
bin/
|
||||
|
||||
# IntelliJ
|
||||
*.iml
|
||||
*.ipr
|
||||
*.iws
|
||||
.idea/
|
||||
out/
|
||||
|
||||
# Fleet
|
||||
.fleet
|
||||
|
||||
# Simulation GUI and other tools window save file
|
||||
*-window.json
|
||||
networktables.json
|
||||
@@ -0,0 +1,6 @@
|
||||
{
|
||||
"enableCppIntellisense": false,
|
||||
"currentLanguage": "java",
|
||||
"projectYear": "2025",
|
||||
"teamNumber": 4512
|
||||
}
|
||||
@@ -0,0 +1,3 @@
|
||||
## **`poseest`**
|
||||
|
||||
### See [PhotonLib Java Examples](../README.md#poseest)
|
||||
@@ -0,0 +1,24 @@
|
||||
Copyright (c) 2009-2024 FIRST and other WPILib contributors
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of FIRST, WPILib, nor the names of other WPILib
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
|
||||
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
|
||||
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
@@ -0,0 +1,101 @@
|
||||
plugins {
|
||||
id "java"
|
||||
id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2"
|
||||
}
|
||||
|
||||
sourceCompatibility = JavaVersion.VERSION_11
|
||||
targetCompatibility = JavaVersion.VERSION_11
|
||||
|
||||
def ROBOT_MAIN_CLASS = "frc.robot.Main"
|
||||
|
||||
wpi.maven.useDevelopment = true
|
||||
wpi.versions.wpilibVersion = "2025.1.1-beta-2"
|
||||
wpi.versions.wpimathVersion = "2025.1.1-beta-2"
|
||||
|
||||
|
||||
// Define my targets (RoboRIO) and artifacts (deployable files)
|
||||
// This is added by GradleRIO's backing project DeployUtils.
|
||||
deploy {
|
||||
targets {
|
||||
roborio(getTargetTypeClass('RoboRIO')) {
|
||||
// Team number is loaded either from the .wpilib/wpilib_preferences.json
|
||||
// or from command line. If not found an exception will be thrown.
|
||||
// You can use getTeamOrDefault(team) instead of getTeamNumber if you
|
||||
// want to store a team number in this file.
|
||||
team = project.frc.getTeamOrDefault(4512)
|
||||
debug = project.frc.getDebugOrDefault(false)
|
||||
|
||||
artifacts {
|
||||
// First part is artifact name, 2nd is artifact type
|
||||
// getTargetTypeClass is a shortcut to get the class type using a string
|
||||
|
||||
frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
|
||||
}
|
||||
|
||||
// Static files artifact
|
||||
frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
|
||||
files = project.fileTree('src/main/deploy')
|
||||
directory = '/home/lvuser/deploy'
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
def deployArtifact = deploy.targets.roborio.artifacts.frcJava
|
||||
|
||||
// Set to true to use debug for JNI.
|
||||
wpi.java.debugJni = false
|
||||
|
||||
// Set this to true to enable desktop support.
|
||||
def includeDesktopSupport = true
|
||||
|
||||
// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
|
||||
// Also defines JUnit 5.
|
||||
dependencies {
|
||||
implementation wpi.java.deps.wpilib()
|
||||
implementation wpi.java.vendor.java()
|
||||
|
||||
roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
|
||||
roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
|
||||
|
||||
roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
|
||||
roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)
|
||||
|
||||
nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
|
||||
nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
|
||||
simulationDebug wpi.sim.enableDebug()
|
||||
|
||||
nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop)
|
||||
nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
|
||||
simulationRelease wpi.sim.enableRelease()
|
||||
|
||||
testImplementation 'junit:junit:4.13.1'
|
||||
}
|
||||
|
||||
// Simulation configuration (e.g. environment variables).
|
||||
wpi.sim.addGui().defaultEnabled = true
|
||||
wpi.sim.addDriverstation()
|
||||
|
||||
// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
|
||||
// in order to make them all available at runtime. Also adding the manifest so WPILib
|
||||
// knows where to look for our Robot Class.
|
||||
jar {
|
||||
from {
|
||||
configurations.runtimeClasspath.collect {
|
||||
it.isDirectory() ? it : zipTree(it)
|
||||
}
|
||||
}
|
||||
manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
|
||||
duplicatesStrategy = DuplicatesStrategy.INCLUDE
|
||||
}
|
||||
|
||||
// Configure jar and deploy tasks
|
||||
deployArtifact.jarTask = jar
|
||||
wpi.java.configureExecutableTasks(jar)
|
||||
wpi.java.configureTestTasks(test)
|
||||
|
||||
// Configure string concat to always inline compile
|
||||
tasks.withType(JavaCompile) {
|
||||
options.compilerArgs.add '-XDstringConcat=inline'
|
||||
}
|
||||
Binary file not shown.
@@ -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
@@ -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
@@ -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
|
||||
@@ -0,0 +1,27 @@
|
||||
import org.gradle.internal.os.OperatingSystem
|
||||
|
||||
pluginManagement {
|
||||
repositories {
|
||||
mavenLocal()
|
||||
gradlePluginPortal()
|
||||
String frcYear = '2025'
|
||||
File frcHome
|
||||
if (OperatingSystem.current().isWindows()) {
|
||||
String publicFolder = System.getenv('PUBLIC')
|
||||
if (publicFolder == null) {
|
||||
publicFolder = "C:\\Users\\Public"
|
||||
}
|
||||
def homeRoot = new File(publicFolder, "wpilib")
|
||||
frcHome = new File(homeRoot, frcYear)
|
||||
} else {
|
||||
def userFolder = System.getProperty("user.home")
|
||||
def homeRoot = new File(userFolder, "wpilib")
|
||||
frcHome = new File(homeRoot, frcYear)
|
||||
}
|
||||
def frcHomeMaven = new File(frcHome, 'maven')
|
||||
maven {
|
||||
name 'frcHome'
|
||||
url frcHomeMaven
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,100 @@
|
||||
{
|
||||
"keyboardJoysticks": [
|
||||
{
|
||||
"axisConfig": [
|
||||
{
|
||||
"decKey": 65,
|
||||
"incKey": 68
|
||||
},
|
||||
{
|
||||
"decKey": 87,
|
||||
"incKey": 83
|
||||
},
|
||||
{
|
||||
"decayRate": 0.0,
|
||||
"keyRate": 0.009999999776482582
|
||||
},
|
||||
{},
|
||||
{
|
||||
"decKey": 81,
|
||||
"incKey": 69
|
||||
}
|
||||
],
|
||||
"axisCount": 6,
|
||||
"buttonCount": 4,
|
||||
"buttonKeys": [
|
||||
90,
|
||||
88,
|
||||
67,
|
||||
86
|
||||
],
|
||||
"povConfig": [
|
||||
{
|
||||
"key0": 328,
|
||||
"key135": 323,
|
||||
"key180": 322,
|
||||
"key225": 321,
|
||||
"key270": 324,
|
||||
"key315": 327,
|
||||
"key45": 329,
|
||||
"key90": 326
|
||||
}
|
||||
],
|
||||
"povCount": 1
|
||||
},
|
||||
{
|
||||
"axisConfig": [
|
||||
{
|
||||
"decKey": 74,
|
||||
"incKey": 76
|
||||
},
|
||||
{
|
||||
"decKey": 73,
|
||||
"incKey": 75
|
||||
}
|
||||
],
|
||||
"axisCount": 2,
|
||||
"buttonCount": 4,
|
||||
"buttonKeys": [
|
||||
77,
|
||||
44,
|
||||
46,
|
||||
47
|
||||
],
|
||||
"povCount": 0
|
||||
},
|
||||
{
|
||||
"axisConfig": [
|
||||
{
|
||||
"decKey": 263,
|
||||
"incKey": 262
|
||||
},
|
||||
{
|
||||
"decKey": 265,
|
||||
"incKey": 264
|
||||
}
|
||||
],
|
||||
"axisCount": 2,
|
||||
"buttonCount": 6,
|
||||
"buttonKeys": [
|
||||
260,
|
||||
268,
|
||||
266,
|
||||
261,
|
||||
269,
|
||||
267
|
||||
],
|
||||
"povCount": 0
|
||||
},
|
||||
{
|
||||
"axisCount": 0,
|
||||
"buttonCount": 0,
|
||||
"povCount": 0
|
||||
}
|
||||
],
|
||||
"robotJoysticks": [
|
||||
{
|
||||
"guid": "Keyboard0"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,161 @@
|
||||
{
|
||||
"NTProvider": {
|
||||
"types": {
|
||||
"/FMSInfo": "FMSInfo",
|
||||
"/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": {
|
||||
"VisionSystemSim-main": {
|
||||
"open": true
|
||||
},
|
||||
"open": true
|
||||
}
|
||||
}
|
||||
},
|
||||
"NetworkTables Info": {
|
||||
"visible": true
|
||||
},
|
||||
"Plot": {
|
||||
"Plot <0>": {
|
||||
"plots": [
|
||||
{
|
||||
"axis": [
|
||||
{
|
||||
"autoFit": true
|
||||
}
|
||||
],
|
||||
"backgroundColor": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.8500000238418579
|
||||
],
|
||||
"height": 332,
|
||||
"series": [
|
||||
{
|
||||
"color": [
|
||||
0.2980392277240753,
|
||||
0.44705885648727417,
|
||||
0.6901960968971252,
|
||||
1.0
|
||||
],
|
||||
"id": "NT:/SmartDashboard/GPLauncher Act Spd (RPM)"
|
||||
},
|
||||
{
|
||||
"color": [
|
||||
0.8666667342185974,
|
||||
0.5176470875740051,
|
||||
0.32156863808631897,
|
||||
1.0
|
||||
],
|
||||
"id": "NT:/SmartDashboard/GPLauncher Des Spd (RPM)"
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,3 @@
|
||||
Files placed in this directory will be deployed to the RoboRIO into the
|
||||
'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function
|
||||
to get a proper path relative to the deploy directory.
|
||||
@@ -0,0 +1,142 @@
|
||||
/*
|
||||
* 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.
|
||||
public static final Transform3d kRobotToCam =
|
||||
new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, 0, 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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
/*
|
||||
* 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.wpilibj.RobotBase;
|
||||
|
||||
public final class Main {
|
||||
private Main() {}
|
||||
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,146 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) PhotonVision
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
package frc.robot;
|
||||
|
||||
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.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
import edu.wpi.first.wpilibj.simulation.BatterySim;
|
||||
import edu.wpi.first.wpilibj.simulation.RoboRioSim;
|
||||
import frc.robot.subsystems.GamepieceLauncher;
|
||||
import frc.robot.subsystems.drivetrain.SwerveDrive;
|
||||
|
||||
public class Robot extends TimedRobot {
|
||||
private SwerveDrive drivetrain;
|
||||
private Vision vision;
|
||||
|
||||
private GamepieceLauncher gpLauncher;
|
||||
|
||||
private XboxController controller;
|
||||
|
||||
@Override
|
||||
public void robotInit() {
|
||||
drivetrain = new SwerveDrive();
|
||||
vision = new Vision();
|
||||
|
||||
controller = new XboxController(0);
|
||||
|
||||
gpLauncher = new GamepieceLauncher();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
// Update Gamepiece Launcher Subsystem
|
||||
gpLauncher.periodic();
|
||||
|
||||
// Update drivetrain subsystem
|
||||
drivetrain.periodic();
|
||||
|
||||
// Correct pose estimate with vision measurements
|
||||
var visionEst = vision.getEstimatedGlobalPose();
|
||||
visionEst.ifPresent(
|
||||
est -> {
|
||||
// Change our trust in the measurement based on the tags we can see
|
||||
var estStdDevs = vision.getEstimationStdDevs();
|
||||
|
||||
drivetrain.addVisionMeasurement(
|
||||
est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs);
|
||||
});
|
||||
|
||||
// Test/Example only!
|
||||
// Apply an offset to pose estimator to test vision correction
|
||||
// You probably don't want this on a real robot, just delete it.
|
||||
if (controller.getBButtonPressed()) {
|
||||
var disturbance =
|
||||
new Transform2d(new Translation2d(1.0, 1.0), new Rotation2d(0.17 * 2 * Math.PI));
|
||||
drivetrain.resetPose(drivetrain.getPose().plus(disturbance), false);
|
||||
}
|
||||
|
||||
// Log values to the dashboard
|
||||
drivetrain.log();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void disabledPeriodic() {
|
||||
drivetrain.stop();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
resetPose();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopPeriodic() {
|
||||
// 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;
|
||||
|
||||
// Command drivetrain motors based on target speeds
|
||||
drivetrain.drive(forward, strafe, turn);
|
||||
|
||||
// Calculate whether the gamepiece launcher runs based on our global pose estimate.
|
||||
var curPose = drivetrain.getPose();
|
||||
var shouldRun = (curPose.getY() > 2.0 && curPose.getX() < 4.0); // Close enough to blue speaker
|
||||
gpLauncher.setRunning(shouldRun);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void simulationPeriodic() {
|
||||
// Update drivetrain simulation
|
||||
drivetrain.simulationPeriodic();
|
||||
|
||||
// Update camera simulation
|
||||
vision.simulationPeriodic(drivetrain.getSimPose());
|
||||
|
||||
var debugField = vision.getSimDebugField();
|
||||
debugField.getObject("EstimatedRobot").setPose(drivetrain.getPose());
|
||||
debugField.getObject("EstimatedRobotModules").setPoses(drivetrain.getModulePoses());
|
||||
|
||||
// Update gamepiece launcher simulation
|
||||
gpLauncher.simulationPeriodic();
|
||||
|
||||
// Calculate battery voltage sag due to current draw
|
||||
var batteryVoltage =
|
||||
BatterySim.calculateDefaultBatteryLoadedVoltage(drivetrain.getCurrentDraw());
|
||||
|
||||
// Using max(0.1, voltage) here isn't a *physically correct* solution,
|
||||
// but it avoids problems with battery voltage measuring 0.
|
||||
RoboRioSim.setVInVoltage(Math.max(0.1, batteryVoltage));
|
||||
}
|
||||
|
||||
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);
|
||||
vision.resetSimPose(startPose);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,191 @@
|
||||
/*
|
||||
* 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.Matrix;
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N3;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
|
||||
import java.util.List;
|
||||
import java.util.Optional;
|
||||
import org.photonvision.EstimatedRobotPose;
|
||||
import org.photonvision.PhotonCamera;
|
||||
import org.photonvision.PhotonPoseEstimator;
|
||||
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
|
||||
import org.photonvision.simulation.PhotonCameraSim;
|
||||
import org.photonvision.simulation.SimCameraProperties;
|
||||
import org.photonvision.simulation.VisionSystemSim;
|
||||
import org.photonvision.targeting.PhotonTrackedTarget;
|
||||
|
||||
public class Vision {
|
||||
private final PhotonCamera camera;
|
||||
private final PhotonPoseEstimator photonEstimator;
|
||||
private Matrix<N3, N1> curStdDevs;
|
||||
|
||||
// Simulation
|
||||
private PhotonCameraSim cameraSim;
|
||||
private VisionSystemSim visionSim;
|
||||
|
||||
public Vision() {
|
||||
camera = new PhotonCamera(kCameraName);
|
||||
|
||||
photonEstimator =
|
||||
new PhotonPoseEstimator(kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, kRobotToCam);
|
||||
photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
|
||||
|
||||
// ----- 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(960, 720, Rotation2d.fromDegrees(90));
|
||||
cameraProp.setCalibError(0.35, 0.10);
|
||||
cameraProp.setFPS(15);
|
||||
cameraProp.setAvgLatencyMs(50);
|
||||
cameraProp.setLatencyStdDevMs(15);
|
||||
// Create a PhotonCameraSim which will update the linked PhotonCamera's values with visible
|
||||
// targets.
|
||||
cameraSim = new PhotonCameraSim(camera, cameraProp);
|
||||
// Add the simulated camera to view the targets on this simulated field.
|
||||
visionSim.addCamera(cameraSim, kRobotToCam);
|
||||
|
||||
cameraSim.enableDrawWireframe(true);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* The latest estimated robot pose on the field from vision data. This may be empty. This should
|
||||
* only be called once per loop.
|
||||
*
|
||||
* <p>Also includes updates for the standard deviations, which can (optionally) be retrieved with
|
||||
* {@link getEstimationStdDevs}
|
||||
*
|
||||
* @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
|
||||
* used for estimation.
|
||||
*/
|
||||
public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
|
||||
Optional<EstimatedRobotPose> visionEst = Optional.empty();
|
||||
for (var change : camera.getAllUnreadResults()) {
|
||||
visionEst = photonEstimator.update(change);
|
||||
updateEstimationStdDevs(visionEst, change.getTargets());
|
||||
|
||||
if (Robot.isSimulation()) {
|
||||
visionEst.ifPresentOrElse(
|
||||
est ->
|
||||
getSimDebugField()
|
||||
.getObject("VisionEstimation")
|
||||
.setPose(est.estimatedPose.toPose2d()),
|
||||
() -> {
|
||||
getSimDebugField().getObject("VisionEstimation").setPoses();
|
||||
});
|
||||
}
|
||||
}
|
||||
return visionEst;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard
|
||||
* deviations based on number of tags, estimation strategy, and distance from the tags.
|
||||
*
|
||||
* @param estimatedPose The estimated pose to guess standard deviations for.
|
||||
* @param targets All targets in this camera frame
|
||||
*/
|
||||
private void updateEstimationStdDevs(
|
||||
Optional<EstimatedRobotPose> estimatedPose, List<PhotonTrackedTarget> targets) {
|
||||
if (estimatedPose.isEmpty()) {
|
||||
// No pose input. Default to single-tag std devs
|
||||
curStdDevs = kSingleTagStdDevs;
|
||||
|
||||
} else {
|
||||
// Pose present. Start running Heuristic
|
||||
var estStdDevs = kSingleTagStdDevs;
|
||||
int numTags = 0;
|
||||
double avgDist = 0;
|
||||
|
||||
// Precalculation - see how many tags we found, and calculate an average-distance metric
|
||||
for (var tgt : targets) {
|
||||
var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
|
||||
if (tagPose.isEmpty()) continue;
|
||||
numTags++;
|
||||
avgDist +=
|
||||
tagPose
|
||||
.get()
|
||||
.toPose2d()
|
||||
.getTranslation()
|
||||
.getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
|
||||
}
|
||||
|
||||
if (numTags == 0) {
|
||||
// No tags visible. Default to single-tag std devs
|
||||
curStdDevs = kSingleTagStdDevs;
|
||||
} else {
|
||||
// One or more tags visible, run the full heuristic.
|
||||
avgDist /= numTags;
|
||||
// Decrease std devs if multiple targets are visible
|
||||
if (numTags > 1) estStdDevs = kMultiTagStdDevs;
|
||||
// Increase std devs based on (average) distance
|
||||
if (numTags == 1 && avgDist > 4)
|
||||
estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
|
||||
else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
|
||||
curStdDevs = estStdDevs;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the latest standard deviations of the estimated pose from {@link
|
||||
* #getEstimatedGlobalPose()}, for use with {@link
|
||||
* edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should
|
||||
* only be used when there are targets visible.
|
||||
*/
|
||||
public Matrix<N3, N1> getEstimationStdDevs() {
|
||||
return curStdDevs;
|
||||
}
|
||||
|
||||
// ----- 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();
|
||||
}
|
||||
}
|
||||
+82
@@ -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.subsystems;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.system.plant.DCMotor;
|
||||
import edu.wpi.first.math.system.plant.LinearSystemId;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.RobotController;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
|
||||
import edu.wpi.first.wpilibj.simulation.FlywheelSim;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
|
||||
public class GamepieceLauncher {
|
||||
private final PWMSparkMax motor;
|
||||
|
||||
private final double LAUNCH_SPEED_RPM = 2500;
|
||||
private double curDesSpd;
|
||||
private double curMotorCmd = 0.0;
|
||||
|
||||
public GamepieceLauncher() {
|
||||
motor = new PWMSparkMax(8);
|
||||
simulationInit();
|
||||
}
|
||||
|
||||
public void setRunning(boolean shouldRun) {
|
||||
curDesSpd = shouldRun ? LAUNCH_SPEED_RPM : 0.0;
|
||||
}
|
||||
|
||||
public void periodic() {
|
||||
double maxRPM =
|
||||
Units.radiansPerSecondToRotationsPerMinute(DCMotor.getFalcon500(1).freeSpeedRadPerSec);
|
||||
curMotorCmd = curDesSpd / maxRPM;
|
||||
curMotorCmd = MathUtil.clamp(curMotorCmd, 0.0, 1.0);
|
||||
motor.set(curMotorCmd);
|
||||
|
||||
SmartDashboard.putNumber("GPLauncher Des Spd (RPM)", curDesSpd);
|
||||
}
|
||||
|
||||
// -- SIMULATION SUPPORT
|
||||
private DCMotor motorSim;
|
||||
private FlywheelSim launcherSim;
|
||||
private final double flywheelMoiKgM2 = 0.002;
|
||||
private final double flywheelGearRatio = 1.0;
|
||||
|
||||
private void simulationInit() {
|
||||
motorSim = DCMotor.getFalcon500(1);
|
||||
launcherSim =
|
||||
new FlywheelSim(
|
||||
LinearSystemId.createFlywheelSystem(motorSim, flywheelMoiKgM2, flywheelGearRatio),
|
||||
motorSim);
|
||||
}
|
||||
|
||||
public void simulationPeriodic() {
|
||||
launcherSim.setInputVoltage(curMotorCmd * RobotController.getBatteryVoltage());
|
||||
launcherSim.update(0.02);
|
||||
var spd = launcherSim.getAngularVelocityRPM();
|
||||
SmartDashboard.putNumber("GPLauncher Act Spd (RPM)", spd);
|
||||
}
|
||||
}
|
||||
+332
@@ -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
@@ -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.getKv() / driveFF.getKa()),
|
||||
VecBuilder.fill(0.0, 1.0 / driveFF.getKa()),
|
||||
MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0),
|
||||
VecBuilder.fill(0.0, 0.0)),
|
||||
driveFF.getKs(),
|
||||
driveMotor,
|
||||
driveGearing,
|
||||
driveWheelRadius,
|
||||
new LinearSystem<N2, N1, N2>(
|
||||
MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -steerFF.getKv() / steerFF.getKa()),
|
||||
VecBuilder.fill(0.0, 1.0 / steerFF.getKa()),
|
||||
MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0),
|
||||
VecBuilder.fill(0.0, 0.0)),
|
||||
steerFF.getKs(),
|
||||
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
@@ -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;
|
||||
}
|
||||
}
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 2.9 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 4.6 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 4.6 KiB |
Reference in New Issue
Block a user