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 @@
|
||||
vendordeps
|
||||
@@ -0,0 +1,6 @@
|
||||
{
|
||||
"enableCppIntellisense": true,
|
||||
"currentLanguage": "cpp",
|
||||
"projectYear": "2025",
|
||||
"teamNumber": 5
|
||||
}
|
||||
@@ -0,0 +1,24 @@
|
||||
Copyright (c) 2009-2021 FIRST and other WPILib contributors
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of FIRST, WPILib, nor the names of other WPILib
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
|
||||
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
|
||||
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
@@ -0,0 +1,117 @@
|
||||
plugins {
|
||||
id "cpp"
|
||||
id "google-test-test-suite"
|
||||
id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2"
|
||||
}
|
||||
|
||||
repositories {
|
||||
mavenLocal()
|
||||
jcenter()
|
||||
}
|
||||
|
||||
wpi.maven.useLocal = false
|
||||
wpi.maven.useDevelopment = false
|
||||
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(5940)
|
||||
debug = project.frc.getDebugOrDefault(false)
|
||||
|
||||
artifacts {
|
||||
// First part is artifact name, 2nd is artifact type
|
||||
// getTargetTypeClass is a shortcut to get the class type using a string
|
||||
|
||||
frcCpp(getArtifactTypeClass('FRCNativeArtifact')) {
|
||||
}
|
||||
|
||||
// Static files artifact
|
||||
frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
|
||||
files = project.fileTree('src/main/deploy')
|
||||
directory = '/home/lvuser/deploy'
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
def deployArtifact = deploy.targets.roborio.artifacts.frcCpp
|
||||
|
||||
// Set this to true to enable desktop support.
|
||||
def includeDesktopSupport = true
|
||||
|
||||
// Set to true to run simulation in debug mode
|
||||
wpi.cpp.debugSimulation = false
|
||||
|
||||
// Default enable simgui
|
||||
wpi.sim.addGui().defaultEnabled = true
|
||||
// Enable DS but not by default
|
||||
wpi.sim.addDriverstation()
|
||||
|
||||
model {
|
||||
components {
|
||||
frcUserProgram(NativeExecutableSpec) {
|
||||
// We don't need to build for roborio -- if we do, we need to install
|
||||
// a roborio toolchain every time we build in CI
|
||||
// Ideally, we'd be able to set the roborio toolchain as optional, but
|
||||
// I can't figure out how to set that environment variable from build.gradle
|
||||
// (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71)
|
||||
// for now, commented out
|
||||
|
||||
// targetPlatform wpi.platforms.roborio
|
||||
|
||||
if (includeDesktopSupport) {
|
||||
targetPlatform wpi.platforms.desktop
|
||||
}
|
||||
|
||||
sources.cpp {
|
||||
source {
|
||||
srcDir 'src/main/cpp'
|
||||
include '**/*.cpp', '**/*.cc'
|
||||
}
|
||||
exportedHeaders {
|
||||
srcDir 'src/main/include'
|
||||
}
|
||||
}
|
||||
|
||||
// Set deploy task to deploy this component
|
||||
deployArtifact.component = it
|
||||
|
||||
// Enable run tasks for this component
|
||||
wpi.cpp.enableExternalTasks(it)
|
||||
|
||||
// Enable simulation for this component
|
||||
wpi.sim.enable(it)
|
||||
// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
|
||||
wpi.cpp.vendor.cpp(it)
|
||||
wpi.cpp.deps.wpilib(it)
|
||||
}
|
||||
}
|
||||
testSuites {
|
||||
frcUserProgramTest(GoogleTestTestSuiteSpec) {
|
||||
testing $.components.frcUserProgram
|
||||
|
||||
sources.cpp {
|
||||
source {
|
||||
srcDir 'src/test/cpp'
|
||||
include '**/*.cpp'
|
||||
}
|
||||
}
|
||||
|
||||
// Enable run tasks for this component
|
||||
wpi.cpp.enableExternalTasks(it)
|
||||
|
||||
wpi.cpp.vendor.cpp(it)
|
||||
wpi.cpp.deps.wpilib(it)
|
||||
wpi.cpp.deps.googleTest(it)
|
||||
}
|
||||
}
|
||||
}
|
||||
+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 @@
|
||||
[]
|
||||
@@ -0,0 +1,30 @@
|
||||
import org.gradle.internal.os.OperatingSystem
|
||||
|
||||
rootProject.name = 'aimandrange'
|
||||
|
||||
pluginManagement {
|
||||
repositories {
|
||||
mavenLocal()
|
||||
jcenter()
|
||||
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,74 @@
|
||||
{
|
||||
"Docking": {
|
||||
"Data": []
|
||||
},
|
||||
"MainWindow": {
|
||||
"GLOBAL": {
|
||||
"fps": "120",
|
||||
"height": "910",
|
||||
"maximized": "0",
|
||||
"style": "0",
|
||||
"userScale": "2",
|
||||
"width": "1550",
|
||||
"xpos": "-1602",
|
||||
"ypos": "79"
|
||||
}
|
||||
},
|
||||
"Window": {
|
||||
"###/SmartDashboard/VisionSystemSim-main/Sim Field": {
|
||||
"Collapsed": "0",
|
||||
"Pos": "554,10",
|
||||
"Size": "991,527"
|
||||
},
|
||||
"###FMS": {
|
||||
"Collapsed": "0",
|
||||
"Pos": "12,604",
|
||||
"Size": "202,214"
|
||||
},
|
||||
"###Joysticks": {
|
||||
"Collapsed": "0",
|
||||
"Pos": "278,685",
|
||||
"Size": "976,219"
|
||||
},
|
||||
"###NetworkTables": {
|
||||
"Collapsed": "0",
|
||||
"Pos": "353,479",
|
||||
"Size": "830,620"
|
||||
},
|
||||
"###NetworkTables Info": {
|
||||
"Collapsed": "0",
|
||||
"Pos": "315,527",
|
||||
"Size": "750,145"
|
||||
},
|
||||
"###Other Devices": {
|
||||
"Collapsed": "0",
|
||||
"Pos": "1025,20",
|
||||
"Size": "250,695"
|
||||
},
|
||||
"###Plot <0>": {
|
||||
"Collapsed": "0",
|
||||
"Pos": "113,22",
|
||||
"Size": "448,400"
|
||||
},
|
||||
"###System Joysticks": {
|
||||
"Collapsed": "0",
|
||||
"Pos": "5,350",
|
||||
"Size": "232,254"
|
||||
},
|
||||
"###Timing": {
|
||||
"Collapsed": "0",
|
||||
"Pos": "5,150",
|
||||
"Size": "162,142"
|
||||
},
|
||||
"Debug##Default": {
|
||||
"Collapsed": "0",
|
||||
"Pos": "60,60",
|
||||
"Size": "400,400"
|
||||
},
|
||||
"Robot State": {
|
||||
"Collapsed": "0",
|
||||
"Pos": "5,20",
|
||||
"Size": "109,134"
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,164 @@
|
||||
{
|
||||
"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)"
|
||||
}
|
||||
]
|
||||
}
|
||||
],
|
||||
"window": {
|
||||
"visible": false
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,135 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include "Robot.h"
|
||||
|
||||
#include <photon/PhotonUtils.h>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <frc/simulation/BatterySim.h>
|
||||
#include <frc/simulation/RoboRioSim.h>
|
||||
|
||||
void Robot::RobotInit() {}
|
||||
|
||||
void Robot::RobotPeriodic() {
|
||||
drivetrain.Periodic();
|
||||
drivetrain.Log();
|
||||
}
|
||||
|
||||
void Robot::DisabledInit() {}
|
||||
|
||||
void Robot::DisabledPeriodic() { drivetrain.Stop(); }
|
||||
|
||||
void Robot::DisabledExit() {}
|
||||
|
||||
void Robot::AutonomousInit() {}
|
||||
|
||||
void Robot::AutonomousPeriodic() {}
|
||||
|
||||
void Robot::AutonomousExit() {}
|
||||
|
||||
void Robot::TeleopInit() {
|
||||
frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}};
|
||||
drivetrain.ResetPose(pose, true);
|
||||
}
|
||||
|
||||
void Robot::TeleopPeriodic() {
|
||||
// Calculate drivetrain commands from Joystick values
|
||||
auto forward =
|
||||
-1.0 * controller.GetLeftY() * constants::Swerve::kMaxLinearSpeed;
|
||||
auto strafe =
|
||||
-1.0 * controller.GetLeftX() * constants::Swerve::kMaxLinearSpeed;
|
||||
auto turn =
|
||||
-1.0 * controller.GetRightX() * constants::Swerve::kMaxAngularSpeed;
|
||||
|
||||
bool targetVisible = false;
|
||||
units::degree_t targetYaw = 0.0_deg;
|
||||
units::meter_t targetRange = 0.0_m;
|
||||
auto results = camera.GetAllUnreadResults();
|
||||
if (results.size() > 0) {
|
||||
// Camera processed a new frame since last
|
||||
// Get the last one in the list.
|
||||
auto result = results[results.size() - 1];
|
||||
if (result.HasTargets()) {
|
||||
// At least one AprilTag was seen by the camera
|
||||
for (auto& target : result.GetTargets()) {
|
||||
if (target.GetFiducialId() == 7) {
|
||||
// Found Tag 7, record its information
|
||||
targetYaw = units::degree_t{target.GetYaw()};
|
||||
targetRange = photon::PhotonUtils::CalculateDistanceToTarget(
|
||||
0.5_m, // Measured with a tape measure, or in CAD
|
||||
1.435_m, // From 2024 game manual for ID 7
|
||||
-30.0_deg, // Measured with a protractor, or in CAD
|
||||
units::degree_t{target.GetPitch()});
|
||||
targetVisible = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Auto-align when requested
|
||||
if (controller.GetAButton() && targetVisible) {
|
||||
// Driver wants auto-alignment to tag 7
|
||||
// And, tag 7 is in sight, so we can turn toward it.
|
||||
// Override the driver's turn command with an automatic one that turns
|
||||
// toward the tag and gets the range right.
|
||||
turn = (VISION_DES_ANGLE - targetYaw).value() * VISION_TURN_kP *
|
||||
constants::Swerve::kMaxAngularSpeed;
|
||||
forward = (VISION_DES_RANGE - targetRange).value() * VISION_STRAFE_kP *
|
||||
constants::Swerve::kMaxLinearSpeed;
|
||||
}
|
||||
|
||||
// Command drivetrain motors based on target speeds
|
||||
drivetrain.Drive(forward, strafe, turn);
|
||||
}
|
||||
|
||||
void Robot::TeleopExit() {}
|
||||
|
||||
void Robot::TestInit() {}
|
||||
|
||||
void Robot::TestPeriodic() {}
|
||||
|
||||
void Robot::TestExit() {}
|
||||
|
||||
void Robot::SimulationPeriodic() {
|
||||
drivetrain.SimulationPeriodic();
|
||||
vision.SimPeriodic(drivetrain.GetSimPose());
|
||||
|
||||
frc::Field2d& debugField = vision.GetSimDebugField();
|
||||
debugField.GetObject("EstimatedRobot")->SetPose(drivetrain.GetPose());
|
||||
debugField.GetObject("EstimatedRobotModules")
|
||||
->SetPoses(drivetrain.GetModulePoses());
|
||||
|
||||
units::ampere_t totalCurrent = drivetrain.GetCurrentDraw();
|
||||
units::volt_t loadedBattVolts =
|
||||
frc::sim::BatterySim::Calculate({totalCurrent});
|
||||
// Using max(0.1, voltage) here isn't a *physically correct* solution,
|
||||
// but it avoids problems with battery voltage measuring 0.
|
||||
frc::sim::RoboRioSim::SetVInVoltage(units::math::max(0.1_V, loadedBattVolts));
|
||||
}
|
||||
|
||||
#ifndef RUNNING_FRC_TESTS
|
||||
int main() { return frc::StartRobot<Robot>(); }
|
||||
#endif
|
||||
@@ -0,0 +1,200 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include "subsystems/SwerveDrive.h"
|
||||
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
|
||||
SwerveDrive::SwerveDrive()
|
||||
: poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(),
|
||||
frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}),
|
||||
gyroSim(gyro),
|
||||
swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1),
|
||||
constants::Swerve::kDriveGearRatio,
|
||||
constants::Swerve::kWheelDiameter / 2,
|
||||
constants::Swerve::kSteerFF, frc::DCMotor::Falcon500(1),
|
||||
constants::Swerve::kSteerGearRatio, kinematics) {}
|
||||
|
||||
void SwerveDrive::Periodic() {
|
||||
for (auto& currentModule : swerveMods) {
|
||||
currentModule.Periodic();
|
||||
}
|
||||
|
||||
poseEstimator.Update(GetGyroYaw(), GetModulePositions());
|
||||
}
|
||||
|
||||
void SwerveDrive::Drive(units::meters_per_second_t vx,
|
||||
units::meters_per_second_t vy,
|
||||
units::radians_per_second_t omega) {
|
||||
frc::ChassisSpeeds newChassisSpeeds =
|
||||
frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading());
|
||||
SetChassisSpeeds(newChassisSpeeds, true, false);
|
||||
}
|
||||
|
||||
void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds,
|
||||
bool openLoop, bool steerInPlace) {
|
||||
SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), true,
|
||||
steerInPlace);
|
||||
this->targetChassisSpeeds = newChassisSpeeds;
|
||||
}
|
||||
|
||||
void SwerveDrive::SetModuleStates(
|
||||
const std::array<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
|
||||
bool steerInPlace) {
|
||||
std::array<frc::SwerveModuleState, 4> desaturatedStates = desiredStates;
|
||||
frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
|
||||
static_cast<wpi::array<frc::SwerveModuleState, 4>*>(&desaturatedStates),
|
||||
constants::Swerve::kMaxLinearSpeed);
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace);
|
||||
}
|
||||
}
|
||||
|
||||
void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s); }
|
||||
|
||||
void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) {
|
||||
if (resetSimPose) {
|
||||
swerveDriveSim.Reset(pose, false);
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
swerveMods[i].SimulationUpdate(0_m, 0_mps, 0_A, 0_rad, 0_rad_per_s, 0_A);
|
||||
}
|
||||
gyroSim.SetAngle(-pose.Rotation().Degrees());
|
||||
gyroSim.SetRate(0_rad_per_s);
|
||||
}
|
||||
|
||||
poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose);
|
||||
}
|
||||
|
||||
frc::Pose2d SwerveDrive::GetPose() const {
|
||||
return poseEstimator.GetEstimatedPosition();
|
||||
}
|
||||
|
||||
frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); }
|
||||
|
||||
frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); }
|
||||
|
||||
frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const {
|
||||
return kinematics.ToChassisSpeeds(GetModuleStates());
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModuleState, 4> SwerveDrive::GetModuleStates() const {
|
||||
std::array<frc::SwerveModuleState, 4> moduleStates;
|
||||
moduleStates[0] = swerveMods[0].GetState();
|
||||
moduleStates[1] = swerveMods[1].GetState();
|
||||
moduleStates[2] = swerveMods[2].GetState();
|
||||
moduleStates[3] = swerveMods[3].GetState();
|
||||
return moduleStates;
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
|
||||
const {
|
||||
std::array<frc::SwerveModulePosition, 4> modulePositions;
|
||||
modulePositions[0] = swerveMods[0].GetPosition();
|
||||
modulePositions[1] = swerveMods[1].GetPosition();
|
||||
modulePositions[2] = swerveMods[2].GetPosition();
|
||||
modulePositions[3] = swerveMods[3].GetPosition();
|
||||
return modulePositions;
|
||||
}
|
||||
|
||||
std::array<frc::Pose2d, 4> SwerveDrive::GetModulePoses() const {
|
||||
std::array<frc::Pose2d, 4> modulePoses;
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
const SwerveModule& module = swerveMods[i];
|
||||
modulePoses[i] = GetPose().TransformBy(frc::Transform2d{
|
||||
module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()});
|
||||
}
|
||||
return modulePoses;
|
||||
}
|
||||
|
||||
void SwerveDrive::Log() {
|
||||
std::string table = "Drive/";
|
||||
frc::Pose2d pose = GetPose();
|
||||
frc::SmartDashboard::PutNumber(table + "X", pose.X().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Heading",
|
||||
pose.Rotation().Degrees().to<double>());
|
||||
frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds();
|
||||
frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
table + "Omega Degrees",
|
||||
chassisSpeeds.omega.convert<units::degrees_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Target VX",
|
||||
targetChassisSpeeds.vx.to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Target VY",
|
||||
targetChassisSpeeds.vy.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
table + "Target Omega Degrees",
|
||||
targetChassisSpeeds.omega.convert<units::degrees_per_second>()
|
||||
.to<double>());
|
||||
|
||||
for (auto& module : swerveMods) {
|
||||
module.Log();
|
||||
}
|
||||
}
|
||||
|
||||
void SwerveDrive::SimulationPeriodic() {
|
||||
std::array<units::volt_t, 4> driveInputs;
|
||||
std::array<units::volt_t, 4> steerInputs;
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
driveInputs[i] = swerveMods[i].GetDriveVoltage();
|
||||
steerInputs[i] = swerveMods[i].GetSteerVoltage();
|
||||
}
|
||||
swerveDriveSim.SetDriveInputs(driveInputs);
|
||||
swerveDriveSim.SetSteerInputs(steerInputs);
|
||||
|
||||
swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod);
|
||||
|
||||
auto driveStates = swerveDriveSim.GetDriveStates();
|
||||
auto steerStates = swerveDriveSim.GetSteerStates();
|
||||
totalCurrentDraw = 0_A;
|
||||
std::array<units::ampere_t, 4> driveCurrents =
|
||||
swerveDriveSim.GetDriveCurrentDraw();
|
||||
for (const auto& current : driveCurrents) {
|
||||
totalCurrentDraw += current;
|
||||
}
|
||||
std::array<units::ampere_t, 4> steerCurrents =
|
||||
swerveDriveSim.GetSteerCurrentDraw();
|
||||
for (const auto& current : steerCurrents) {
|
||||
totalCurrentDraw += current;
|
||||
}
|
||||
for (int i = 0; i < swerveMods.size(); i++) {
|
||||
units::meter_t drivePos{driveStates[i](0, 0)};
|
||||
units::meters_per_second_t driveRate{driveStates[i](1, 0)};
|
||||
units::radian_t steerPos{steerStates[i](0, 0)};
|
||||
units::radians_per_second_t steerRate{steerStates[i](1, 0)};
|
||||
swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i],
|
||||
steerPos, steerRate, steerCurrents[i]);
|
||||
}
|
||||
gyroSim.SetRate(-swerveDriveSim.GetOmega());
|
||||
gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees());
|
||||
}
|
||||
|
||||
frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); }
|
||||
|
||||
units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; }
|
||||
@@ -0,0 +1,287 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include "subsystems/SwerveDriveSim.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <frc/RobotController.h>
|
||||
#include <frc/system/Discretization.h>
|
||||
|
||||
template <typename T>
|
||||
int sgn(T val) {
|
||||
return (T(0) < val) - (val < T(0));
|
||||
}
|
||||
|
||||
SwerveDriveSim::SwerveDriveSim(
|
||||
const frc::SimpleMotorFeedforward<units::meters>& driveFF,
|
||||
const frc::DCMotor& driveMotor, double driveGearing,
|
||||
units::meter_t driveWheelRadius,
|
||||
const frc::SimpleMotorFeedforward<units::radians>& steerFF,
|
||||
const frc::DCMotor& steerMotor, double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& kinematics)
|
||||
: SwerveDriveSim(
|
||||
frc::LinearSystem<2, 1, 2>{
|
||||
(Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
|
||||
-driveFF.GetKv().to<double>() / driveFF.GetKa().to<double>())
|
||||
.finished(),
|
||||
Eigen::Matrix<double, 2, 1>{0.0,
|
||||
1.0 / driveFF.GetKa().to<double>()},
|
||||
(Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
|
||||
Eigen::Matrix<double, 2, 1>{0.0, 0.0}},
|
||||
driveFF.GetKs(), driveMotor, driveGearing, driveWheelRadius,
|
||||
frc::LinearSystem<2, 1, 2>{
|
||||
(Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
|
||||
-steerFF.GetKv().to<double>() / steerFF.GetKa().to<double>())
|
||||
.finished(),
|
||||
Eigen::Matrix<double, 2, 1>{0.0,
|
||||
1.0 / steerFF.GetKa().to<double>()},
|
||||
(Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
|
||||
Eigen::Matrix<double, 2, 1>{0.0, 0.0}},
|
||||
steerFF.GetKs(), steerMotor, steerGearing, kinematics) {}
|
||||
|
||||
SwerveDriveSim::SwerveDriveSim(
|
||||
const frc::LinearSystem<2, 1, 2>& drivePlant, units::volt_t driveKs,
|
||||
const frc::DCMotor& driveMotor, double driveGearing,
|
||||
units::meter_t driveWheelRadius,
|
||||
const frc::LinearSystem<2, 1, 2>& steerPlant, units::volt_t steerKs,
|
||||
const frc::DCMotor& steerMotor, double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& kinematics)
|
||||
: drivePlant(drivePlant),
|
||||
driveKs(driveKs),
|
||||
driveMotor(driveMotor),
|
||||
driveGearing(driveGearing),
|
||||
driveWheelRadius(driveWheelRadius),
|
||||
steerPlant(steerPlant),
|
||||
steerKs(steerKs),
|
||||
steerMotor(steerMotor),
|
||||
steerGearing(steerGearing),
|
||||
kinematics(kinematics) {}
|
||||
|
||||
void SwerveDriveSim::SetDriveInputs(
|
||||
const std::array<units::volt_t, numModules>& inputs) {
|
||||
units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage();
|
||||
for (int i = 0; i < driveInputs.size(); i++) {
|
||||
units::volt_t input = inputs[i];
|
||||
driveInputs[i] = std::clamp(input, -battVoltage, battVoltage);
|
||||
}
|
||||
}
|
||||
|
||||
void SwerveDriveSim::SetSteerInputs(
|
||||
const std::array<units::volt_t, numModules>& inputs) {
|
||||
units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage();
|
||||
for (int i = 0; i < steerInputs.size(); i++) {
|
||||
units::volt_t input = inputs[i];
|
||||
steerInputs[i] = std::clamp(input, -battVoltage, battVoltage);
|
||||
}
|
||||
}
|
||||
|
||||
Eigen::Matrix<double, 2, 1> SwerveDriveSim::CalculateX(
|
||||
const Eigen::Matrix<double, 2, 2>& discA,
|
||||
const Eigen::Matrix<double, 2, 1>& discB,
|
||||
const Eigen::Matrix<double, 2, 1>& x, units::volt_t input,
|
||||
units::volt_t kS) {
|
||||
auto Ax = discA * x;
|
||||
double nextStateVel = Ax(1, 0);
|
||||
double inputToStop = nextStateVel / -discB(1, 0);
|
||||
double ksSystemEffect =
|
||||
std::clamp(inputToStop, -kS.to<double>(), kS.to<double>());
|
||||
|
||||
nextStateVel += discB(1, 0) * ksSystemEffect;
|
||||
inputToStop = nextStateVel / -discB(1, 0);
|
||||
double signToStop = sgn(inputToStop);
|
||||
double inputSign = sgn(input.to<double>());
|
||||
double ksInputEffect = 0;
|
||||
|
||||
if (std::abs(ksSystemEffect) < kS.to<double>()) {
|
||||
double absInput = std::abs(input.to<double>());
|
||||
ksInputEffect =
|
||||
-std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
|
||||
} else if ((input.to<double>() * signToStop) > (inputToStop * signToStop)) {
|
||||
double absInput = std::abs(input.to<double>() - inputToStop);
|
||||
ksInputEffect =
|
||||
-std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
|
||||
}
|
||||
|
||||
auto sF = Eigen::Matrix<double, 1, 1>{input.to<double>() + ksSystemEffect +
|
||||
ksInputEffect};
|
||||
auto Bu = discB * sF;
|
||||
auto retVal = Ax + Bu;
|
||||
return retVal;
|
||||
}
|
||||
|
||||
void SwerveDriveSim::Update(units::second_t dt) {
|
||||
Eigen::Matrix<double, 2, 2> driveDiscA;
|
||||
Eigen::Matrix<double, 2, 1> driveDiscB;
|
||||
frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA,
|
||||
&driveDiscB);
|
||||
|
||||
Eigen::Matrix<double, 2, 2> steerDiscA;
|
||||
Eigen::Matrix<double, 2, 1> steerDiscB;
|
||||
frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA,
|
||||
&steerDiscB);
|
||||
|
||||
std::array<frc::SwerveModulePosition, 4> moduleDeltas;
|
||||
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
double prevDriveStatePos = driveStates[i](0, 0);
|
||||
driveStates[i] = CalculateX(driveDiscA, driveDiscB, driveStates[i],
|
||||
driveInputs[i], driveKs);
|
||||
double currentDriveStatePos = driveStates[i](0, 0);
|
||||
steerStates[i] = CalculateX(steerDiscA, steerDiscB, steerStates[i],
|
||||
steerInputs[i], steerKs);
|
||||
double currentSteerStatePos = steerStates[i](0, 0);
|
||||
moduleDeltas[i] = frc::SwerveModulePosition{
|
||||
units::meter_t{currentDriveStatePos - prevDriveStatePos},
|
||||
frc::Rotation2d{units::radian_t{currentSteerStatePos}}};
|
||||
}
|
||||
|
||||
frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas);
|
||||
pose = pose.Exp(twist);
|
||||
omega = twist.dtheta / dt;
|
||||
}
|
||||
|
||||
void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) {
|
||||
this->pose = pose;
|
||||
if (!preserveMotion) {
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
driveStates[i] = Eigen::Matrix<double, 2, 1>{0, 0};
|
||||
steerStates[i] = Eigen::Matrix<double, 2, 1>{0, 0};
|
||||
}
|
||||
omega = 0_rad_per_s;
|
||||
}
|
||||
}
|
||||
|
||||
void SwerveDriveSim::Reset(const frc::Pose2d& pose,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>,
|
||||
numModules>& moduleDriveStates,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>,
|
||||
numModules>& moduleSteerStates) {
|
||||
this->pose = pose;
|
||||
driveStates = moduleDriveStates;
|
||||
steerStates = moduleSteerStates;
|
||||
omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega;
|
||||
}
|
||||
|
||||
frc::Pose2d SwerveDriveSim::GetPose() const { return pose; }
|
||||
|
||||
std::array<frc::SwerveModulePosition, numModules>
|
||||
SwerveDriveSim::GetModulePositions() const {
|
||||
std::array<frc::SwerveModulePosition, numModules> positions;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
positions[i] = frc::SwerveModulePosition{
|
||||
units::meter_t{driveStates[i](0, 0)},
|
||||
frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}};
|
||||
}
|
||||
return positions;
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModulePosition, numModules>
|
||||
SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev,
|
||||
units::radian_t steerStdDev) {
|
||||
std::array<frc::SwerveModulePosition, numModules> positions;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
positions[i] = frc::SwerveModulePosition{
|
||||
units::meter_t{driveStates[i](0, 0)} +
|
||||
randDist(generator) * driveStdDev,
|
||||
frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} +
|
||||
randDist(generator) * steerStdDev}};
|
||||
}
|
||||
return positions;
|
||||
}
|
||||
|
||||
std::array<frc::SwerveModuleState, numModules>
|
||||
SwerveDriveSim::GetModuleStates() {
|
||||
std::array<frc::SwerveModuleState, numModules> states;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
states[i] = frc::SwerveModuleState{
|
||||
units::meters_per_second_t{driveStates[i](1, 0)},
|
||||
frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}};
|
||||
}
|
||||
return states;
|
||||
}
|
||||
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules>
|
||||
SwerveDriveSim::GetDriveStates() const {
|
||||
return driveStates;
|
||||
}
|
||||
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules>
|
||||
SwerveDriveSim::GetSteerStates() const {
|
||||
return steerStates;
|
||||
}
|
||||
|
||||
units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; }
|
||||
|
||||
units::ampere_t SwerveDriveSim::GetCurrentDraw(
|
||||
const frc::DCMotor& motor, units::radians_per_second_t velocity,
|
||||
units::volt_t inputVolts, units::volt_t batteryVolts) const {
|
||||
units::volt_t effVolts = inputVolts - velocity / motor.Kv;
|
||||
if (inputVolts >= 0_V) {
|
||||
effVolts = std::clamp(effVolts, 0_V, inputVolts);
|
||||
} else {
|
||||
effVolts = std::clamp(effVolts, inputVolts, 0_V);
|
||||
}
|
||||
auto retVal = (inputVolts / batteryVolts) * (effVolts / motor.R);
|
||||
return retVal;
|
||||
}
|
||||
|
||||
std::array<units::ampere_t, numModules> SwerveDriveSim::GetDriveCurrentDraw()
|
||||
const {
|
||||
std::array<units::ampere_t, numModules> currents;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
units::radians_per_second_t speed =
|
||||
units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing /
|
||||
driveWheelRadius.to<double>();
|
||||
currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i],
|
||||
frc::RobotController::GetBatteryVoltage());
|
||||
}
|
||||
return currents;
|
||||
}
|
||||
|
||||
std::array<units::ampere_t, numModules> SwerveDriveSim::GetSteerCurrentDraw()
|
||||
const {
|
||||
std::array<units::ampere_t, numModules> currents;
|
||||
for (int i = 0; i < numModules; i++) {
|
||||
units::radians_per_second_t speed =
|
||||
units::radians_per_second_t{steerStates[i](1, 0) * steerGearing};
|
||||
// TODO: If uncommented we get huge current values.. Not sure how to fix
|
||||
// atm. :(
|
||||
currents[i] = 20_A;
|
||||
// currents[i] = GetCurrentDraw(steerMotor, speed, steerInputs[i],
|
||||
// frc::RobotController::GetBatteryVoltage());
|
||||
}
|
||||
return currents;
|
||||
}
|
||||
|
||||
units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const {
|
||||
units::ampere_t total{0};
|
||||
for (const auto& val : GetDriveCurrentDraw()) {
|
||||
total += val;
|
||||
}
|
||||
for (const auto& val : GetSteerCurrentDraw()) {
|
||||
total += val;
|
||||
}
|
||||
return total;
|
||||
}
|
||||
@@ -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.
|
||||
*/
|
||||
|
||||
#include "subsystems/SwerveModule.h"
|
||||
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
|
||||
#include <frc/MathUtil.h>
|
||||
#include <frc/RobotController.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
|
||||
SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts)
|
||||
: moduleConstants(consts),
|
||||
driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}),
|
||||
driveEncoder(frc::Encoder{moduleConstants.driveEncoderA,
|
||||
moduleConstants.driveEncoderB}),
|
||||
steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}),
|
||||
steerEncoder(frc::Encoder{moduleConstants.steerEncoderA,
|
||||
moduleConstants.steerEncoderB}),
|
||||
driveEncoderSim(driveEncoder),
|
||||
steerEncoderSim(steerEncoder) {
|
||||
driveEncoder.SetDistancePerPulse(
|
||||
constants::Swerve::kDriveDistPerPulse.to<double>());
|
||||
steerEncoder.SetDistancePerPulse(
|
||||
constants::Swerve::kSteerRadPerPulse.to<double>());
|
||||
steerPIDController.EnableContinuousInput(-std::numbers::pi, std::numbers::pi);
|
||||
}
|
||||
|
||||
void SwerveModule::Periodic() {
|
||||
units::volt_t steerPID = units::volt_t{
|
||||
steerPIDController.Calculate(GetAbsoluteHeading().Radians().to<double>(),
|
||||
desiredState.angle.Radians().to<double>())};
|
||||
steerMotor.SetVoltage(steerPID);
|
||||
|
||||
units::volt_t driveFF =
|
||||
constants::Swerve::kDriveFF.Calculate(desiredState.speed);
|
||||
units::volt_t drivePID{0};
|
||||
if (!openLoop) {
|
||||
drivePID = units::volt_t{drivePIDController.Calculate(
|
||||
driveEncoder.GetRate(), desiredState.speed.to<double>())};
|
||||
}
|
||||
driveMotor.SetVoltage(driveFF + drivePID);
|
||||
}
|
||||
|
||||
void SwerveModule::SetDesiredState(frc::SwerveModuleState newState,
|
||||
bool shouldBeOpenLoop, bool steerInPlace) {
|
||||
frc::Rotation2d currentRotation = GetAbsoluteHeading();
|
||||
newState.Optimize(currentRotation);
|
||||
desiredState = newState;
|
||||
}
|
||||
|
||||
frc::Rotation2d SwerveModule::GetAbsoluteHeading() const {
|
||||
return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}};
|
||||
}
|
||||
|
||||
frc::SwerveModuleState SwerveModule::GetState() const {
|
||||
return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps,
|
||||
GetAbsoluteHeading()};
|
||||
}
|
||||
|
||||
frc::SwerveModulePosition SwerveModule::GetPosition() const {
|
||||
return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m,
|
||||
GetAbsoluteHeading()};
|
||||
}
|
||||
|
||||
units::volt_t SwerveModule::GetDriveVoltage() const {
|
||||
return driveMotor.Get() * frc::RobotController::GetBatteryVoltage();
|
||||
}
|
||||
|
||||
units::volt_t SwerveModule::GetSteerVoltage() const {
|
||||
return steerMotor.Get() * frc::RobotController::GetBatteryVoltage();
|
||||
}
|
||||
|
||||
units::ampere_t SwerveModule::GetDriveCurrentSim() const {
|
||||
return driveCurrentSim;
|
||||
}
|
||||
|
||||
units::ampere_t SwerveModule::GetSteerCurrentSim() const {
|
||||
return steerCurrentSim;
|
||||
}
|
||||
|
||||
constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const {
|
||||
return moduleConstants;
|
||||
}
|
||||
|
||||
void SwerveModule::Log() {
|
||||
frc::SwerveModuleState state = GetState();
|
||||
|
||||
std::string table =
|
||||
"Module " + std::to_string(moduleConstants.moduleNum) + "/";
|
||||
frc::SmartDashboard::PutNumber(table + "Steer Degrees",
|
||||
frc::AngleModulus(state.angle.Radians())
|
||||
.convert<units::degrees>()
|
||||
.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
table + "Steer Target Degrees",
|
||||
units::radian_t{steerPIDController.GetSetpoint()}
|
||||
.convert<units::degrees>()
|
||||
.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
table + "Drive Velocity Feet",
|
||||
state.speed.convert<units::feet_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
table + "Drive Velocity Target Feet",
|
||||
desiredState.speed.convert<units::feet_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Drive Current",
|
||||
driveCurrentSim.to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Steer Current",
|
||||
steerCurrentSim.to<double>());
|
||||
}
|
||||
|
||||
void SwerveModule::SimulationUpdate(
|
||||
units::meter_t driveEncoderDist,
|
||||
units::meters_per_second_t driveEncoderRate, units::ampere_t driveCurrent,
|
||||
units::radian_t steerEncoderDist,
|
||||
units::radians_per_second_t steerEncoderRate,
|
||||
units::ampere_t steerCurrent) {
|
||||
driveEncoderSim.SetDistance(driveEncoderDist.to<double>());
|
||||
driveEncoderSim.SetRate(driveEncoderRate.to<double>());
|
||||
driveCurrentSim = driveCurrent;
|
||||
steerEncoderSim.SetDistance(steerEncoderDist.to<double>());
|
||||
steerEncoderSim.SetRate(steerEncoderRate.to<double>());
|
||||
steerCurrentSim = steerCurrent;
|
||||
}
|
||||
@@ -0,0 +1,4 @@
|
||||
Files placed in this directory will be deployed to the RoboRIO into the
|
||||
'deploy' directory in the home folder. Use the 'frc::filesystem::GetDeployDirectory'
|
||||
function from the 'frc/Filesystem.h' header to get a proper path relative to the deploy
|
||||
directory.
|
||||
@@ -0,0 +1,125 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <frc/apriltag/AprilTagFieldLayout.h>
|
||||
#include <frc/apriltag/AprilTagFields.h>
|
||||
#include <frc/controller/SimpleMotorFeedforward.h>
|
||||
#include <frc/geometry/Transform3d.h>
|
||||
#include <frc/geometry/Translation2d.h>
|
||||
#include <units/acceleration.h>
|
||||
#include <units/angular_acceleration.h>
|
||||
#include <units/angular_velocity.h>
|
||||
#include <units/length.h>
|
||||
#include <units/velocity.h>
|
||||
|
||||
namespace constants {
|
||||
namespace Vision {
|
||||
inline constexpr std::string_view kCameraName{"YOUR CAMERA NAME"};
|
||||
inline const frc::Transform3d kRobotToCam{
|
||||
frc::Translation3d{0.5_m, 0.0_m, 0.5_m},
|
||||
frc::Rotation3d{0_rad, -30_deg, 0_rad}};
|
||||
inline const frc::AprilTagFieldLayout kTagLayout{
|
||||
frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo)};
|
||||
|
||||
inline const Eigen::Matrix<double, 3, 1> kSingleTagStdDevs{4, 4, 8};
|
||||
inline const Eigen::Matrix<double, 3, 1> kMultiTagStdDevs{0.5, 0.5, 1};
|
||||
} // namespace Vision
|
||||
namespace Swerve {
|
||||
using namespace units;
|
||||
|
||||
inline constexpr units::meter_t kTrackWidth{18.5_in};
|
||||
inline constexpr units::meter_t kTrackLength{18.5_in};
|
||||
inline constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2};
|
||||
inline constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2};
|
||||
inline constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps};
|
||||
inline constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s};
|
||||
inline constexpr units::meter_t kWheelDiameter{4_in};
|
||||
inline constexpr units::meter_t kWheelCircumference{kWheelDiameter *
|
||||
std::numbers::pi};
|
||||
|
||||
inline constexpr double kDriveGearRatio = 6.75;
|
||||
inline constexpr double kSteerGearRatio = 12.8;
|
||||
|
||||
inline constexpr units::meter_t kDriveDistPerPulse =
|
||||
kWheelCircumference / 1024.0 / kDriveGearRatio;
|
||||
inline constexpr units::radian_t kSteerRadPerPulse =
|
||||
units::radian_t{2 * std::numbers::pi} / 1024.0;
|
||||
|
||||
inline constexpr double kDriveKP = 1.0;
|
||||
inline constexpr double kDriveKI = 0.0;
|
||||
inline constexpr double kDriveKD = 0.0;
|
||||
|
||||
inline constexpr double kSteerKP = 20.0;
|
||||
inline constexpr double kSteerKI = 0.0;
|
||||
inline constexpr double kSteerKD = 0.25;
|
||||
|
||||
using namespace units;
|
||||
|
||||
inline const frc::SimpleMotorFeedforward<units::meters> kDriveFF{
|
||||
0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq};
|
||||
|
||||
inline const frc::SimpleMotorFeedforward<units::radians> kSteerFF{
|
||||
0.5_V, 0.25_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq};
|
||||
|
||||
struct ModuleConstants {
|
||||
public:
|
||||
const int moduleNum;
|
||||
const int driveMotorId;
|
||||
const int driveEncoderA;
|
||||
const int driveEncoderB;
|
||||
const int steerMotorId;
|
||||
const int steerEncoderA;
|
||||
const int steerEncoderB;
|
||||
const double angleOffset;
|
||||
const frc::Translation2d centerOffset;
|
||||
|
||||
ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA,
|
||||
int driveEncoderB, int steerMotorId, int steerEncoderA,
|
||||
int steerEncoderB, double angleOffset, units::meter_t xOffset,
|
||||
units::meter_t yOffset)
|
||||
: moduleNum(moduleNum),
|
||||
driveMotorId(driveMotorId),
|
||||
driveEncoderA(driveEncoderA),
|
||||
driveEncoderB(driveEncoderB),
|
||||
steerMotorId(steerMotorId),
|
||||
steerEncoderA(steerEncoderA),
|
||||
steerEncoderB(steerEncoderB),
|
||||
angleOffset(angleOffset),
|
||||
centerOffset(frc::Translation2d{xOffset, yOffset}) {}
|
||||
};
|
||||
|
||||
inline const ModuleConstants FL_CONSTANTS{
|
||||
1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2};
|
||||
inline const ModuleConstants FR_CONSTANTS{
|
||||
2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2};
|
||||
inline const ModuleConstants BL_CONSTANTS{
|
||||
3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2};
|
||||
inline const ModuleConstants BR_CONSTANTS{
|
||||
4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2};
|
||||
} // namespace Swerve
|
||||
} // namespace constants
|
||||
@@ -0,0 +1,63 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <photon/PhotonCamera.h>
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/XboxController.h>
|
||||
|
||||
#include "Constants.h"
|
||||
#include "VisionSim.h"
|
||||
#include "subsystems/SwerveDrive.h"
|
||||
|
||||
class Robot : public frc::TimedRobot {
|
||||
public:
|
||||
void RobotInit() override;
|
||||
void RobotPeriodic() override;
|
||||
void DisabledInit() override;
|
||||
void DisabledPeriodic() override;
|
||||
void DisabledExit() override;
|
||||
void AutonomousInit() override;
|
||||
void AutonomousPeriodic() override;
|
||||
void AutonomousExit() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TeleopExit() override;
|
||||
void TestInit() override;
|
||||
void TestPeriodic() override;
|
||||
void TestExit() override;
|
||||
void SimulationPeriodic() override;
|
||||
|
||||
private:
|
||||
photon::PhotonCamera camera{constants::Vision::kCameraName};
|
||||
SwerveDrive drivetrain{};
|
||||
VisionSim vision{&camera};
|
||||
frc::XboxController controller{0};
|
||||
static constexpr auto VISION_TURN_kP = 0.01;
|
||||
static constexpr auto VISION_DES_ANGLE = 0.0_deg;
|
||||
static constexpr auto VISION_STRAFE_kP = 0.5;
|
||||
static constexpr auto VISION_DES_RANGE = 1.25_m;
|
||||
};
|
||||
@@ -0,0 +1,87 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <photon/PhotonCamera.h>
|
||||
#include <photon/PhotonPoseEstimator.h>
|
||||
#include <photon/estimation/VisionEstimation.h>
|
||||
#include <photon/simulation/VisionSystemSim.h>
|
||||
#include <photon/simulation/VisionTargetSim.h>
|
||||
#include <photon/targeting/PhotonPipelineResult.h>
|
||||
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
|
||||
#include <frc/apriltag/AprilTagFieldLayout.h>
|
||||
#include <frc/apriltag/AprilTagFields.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
class VisionSim {
|
||||
public:
|
||||
explicit VisionSim(photon::PhotonCamera* camera) {
|
||||
if (frc::RobotBase::IsSimulation()) {
|
||||
visionSim = std::make_unique<photon::VisionSystemSim>("main");
|
||||
|
||||
visionSim->AddAprilTags(constants::Vision::kTagLayout);
|
||||
|
||||
cameraProp = std::make_unique<photon::SimCameraProperties>();
|
||||
|
||||
cameraProp->SetCalibration(320, 240, frc::Rotation2d{90_deg});
|
||||
cameraProp->SetCalibError(.35, .10);
|
||||
cameraProp->SetFPS(70_Hz);
|
||||
cameraProp->SetAvgLatency(30_ms);
|
||||
cameraProp->SetLatencyStdDev(15_ms);
|
||||
|
||||
cameraSim =
|
||||
std::make_shared<photon::PhotonCameraSim>(camera, *cameraProp.get());
|
||||
|
||||
visionSim->AddCamera(cameraSim.get(), constants::Vision::kRobotToCam);
|
||||
cameraSim->EnableDrawWireframe(true);
|
||||
}
|
||||
}
|
||||
|
||||
photon::PhotonPipelineResult GetLatestResult() { return m_latestResult; }
|
||||
|
||||
void SimPeriodic(frc::Pose2d robotSimPose) {
|
||||
visionSim->Update(robotSimPose);
|
||||
}
|
||||
|
||||
void ResetSimPose(frc::Pose2d pose) {
|
||||
if (frc::RobotBase::IsSimulation()) {
|
||||
visionSim->ResetRobotPose(pose);
|
||||
}
|
||||
}
|
||||
|
||||
frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); }
|
||||
|
||||
private:
|
||||
std::unique_ptr<photon::VisionSystemSim> visionSim;
|
||||
std::unique_ptr<photon::SimCameraProperties> cameraProp;
|
||||
std::shared_ptr<photon::PhotonCameraSim> cameraSim;
|
||||
|
||||
// The most recent result, cached for calculating std devs
|
||||
photon::PhotonPipelineResult m_latestResult;
|
||||
};
|
||||
@@ -0,0 +1,80 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/ADXRS450_Gyro.h>
|
||||
#include <frc/SPI.h>
|
||||
#include <frc/estimator/SwerveDrivePoseEstimator.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/simulation/ADXRS450_GyroSim.h>
|
||||
|
||||
#include "SwerveDriveSim.h"
|
||||
#include "SwerveModule.h"
|
||||
|
||||
class SwerveDrive {
|
||||
public:
|
||||
SwerveDrive();
|
||||
void Periodic();
|
||||
void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy,
|
||||
units::radians_per_second_t omega);
|
||||
void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds,
|
||||
bool openLoop, bool steerInPlace);
|
||||
void SetModuleStates(
|
||||
const std::array<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
|
||||
bool steerInPlace);
|
||||
void Stop();
|
||||
void ResetPose(const frc::Pose2d& pose, bool resetSimPose);
|
||||
frc::Pose2d GetPose() const;
|
||||
frc::Rotation2d GetHeading() const;
|
||||
frc::Rotation2d GetGyroYaw() const;
|
||||
frc::ChassisSpeeds GetChassisSpeeds() const;
|
||||
std::array<frc::SwerveModuleState, 4> GetModuleStates() const;
|
||||
std::array<frc::SwerveModulePosition, 4> GetModulePositions() const;
|
||||
std::array<frc::Pose2d, 4> GetModulePoses() const;
|
||||
void Log();
|
||||
void SimulationPeriodic();
|
||||
frc::Pose2d GetSimPose() const;
|
||||
units::ampere_t GetCurrentDraw() const;
|
||||
|
||||
private:
|
||||
std::array<SwerveModule, 4> swerveMods{
|
||||
SwerveModule{constants::Swerve::FL_CONSTANTS},
|
||||
SwerveModule{constants::Swerve::FR_CONSTANTS},
|
||||
SwerveModule{constants::Swerve::BL_CONSTANTS},
|
||||
SwerveModule{constants::Swerve::BR_CONSTANTS}};
|
||||
frc::SwerveDriveKinematics<4> kinematics{
|
||||
swerveMods[0].GetModuleConstants().centerOffset,
|
||||
swerveMods[1].GetModuleConstants().centerOffset,
|
||||
swerveMods[2].GetModuleConstants().centerOffset,
|
||||
swerveMods[3].GetModuleConstants().centerOffset,
|
||||
};
|
||||
frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0};
|
||||
frc::SwerveDrivePoseEstimator<4> poseEstimator;
|
||||
frc::ChassisSpeeds targetChassisSpeeds{};
|
||||
|
||||
frc::sim::ADXRS450_GyroSim gyroSim;
|
||||
SwerveDriveSim swerveDriveSim;
|
||||
units::ampere_t totalCurrentDraw{0};
|
||||
};
|
||||
@@ -0,0 +1,102 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <random>
|
||||
|
||||
#include <frc/controller/SimpleMotorFeedforward.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/system/LinearSystem.h>
|
||||
#include <frc/system/plant/DCMotor.h>
|
||||
#include <units/voltage.h>
|
||||
|
||||
static constexpr int numModules{4};
|
||||
|
||||
class SwerveDriveSim {
|
||||
public:
|
||||
SwerveDriveSim(const frc::SimpleMotorFeedforward<units::meters>& driveFF,
|
||||
const frc::DCMotor& driveMotor, double driveGearing,
|
||||
units::meter_t driveWheelRadius,
|
||||
const frc::SimpleMotorFeedforward<units::radians>& steerFF,
|
||||
const frc::DCMotor& steerMotor, double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& kinematics);
|
||||
SwerveDriveSim(const frc::LinearSystem<2, 1, 2>& drivePlant,
|
||||
units::volt_t driveKs, const frc::DCMotor& driveMotor,
|
||||
double driveGearing, units::meter_t driveWheelRadius,
|
||||
const frc::LinearSystem<2, 1, 2>& steerPlant,
|
||||
units::volt_t steerKs, const frc::DCMotor& steerMotor,
|
||||
double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& kinematics);
|
||||
void SetDriveInputs(const std::array<units::volt_t, numModules>& inputs);
|
||||
void SetSteerInputs(const std::array<units::volt_t, numModules>& inputs);
|
||||
static Eigen::Matrix<double, 2, 1> CalculateX(
|
||||
const Eigen::Matrix<double, 2, 2>& discA,
|
||||
const Eigen::Matrix<double, 2, 1>& discB,
|
||||
const Eigen::Matrix<double, 2, 1>& x, units::volt_t input,
|
||||
units::volt_t kS);
|
||||
void Update(units::second_t dt);
|
||||
void Reset(const frc::Pose2d& pose, bool preserveMotion);
|
||||
void Reset(const frc::Pose2d& pose,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
|
||||
moduleDriveStates,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
|
||||
moduleSteerStates);
|
||||
frc::Pose2d GetPose() const;
|
||||
std::array<frc::SwerveModulePosition, numModules> GetModulePositions() const;
|
||||
std::array<frc::SwerveModulePosition, numModules> GetNoisyModulePositions(
|
||||
units::meter_t driveStdDev, units::radian_t steerStdDev);
|
||||
std::array<frc::SwerveModuleState, numModules> GetModuleStates();
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> GetDriveStates() const;
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> GetSteerStates() const;
|
||||
units::radians_per_second_t GetOmega() const;
|
||||
units::ampere_t GetCurrentDraw(const frc::DCMotor& motor,
|
||||
units::radians_per_second_t velocity,
|
||||
units::volt_t inputVolts,
|
||||
units::volt_t batteryVolts) const;
|
||||
std::array<units::ampere_t, numModules> GetDriveCurrentDraw() const;
|
||||
std::array<units::ampere_t, numModules> GetSteerCurrentDraw() const;
|
||||
units::ampere_t GetTotalCurrentDraw() const;
|
||||
|
||||
private:
|
||||
std::random_device rd{};
|
||||
std::mt19937 generator{rd()};
|
||||
std::normal_distribution<double> randDist{0.0, 1.0};
|
||||
const frc::LinearSystem<2, 1, 2> drivePlant;
|
||||
const units::volt_t driveKs;
|
||||
const frc::DCMotor driveMotor;
|
||||
const double driveGearing;
|
||||
const units::meter_t driveWheelRadius;
|
||||
const frc::LinearSystem<2, 1, 2> steerPlant;
|
||||
const units::volt_t steerKs;
|
||||
const frc::DCMotor steerMotor;
|
||||
const double steerGearing;
|
||||
const frc::SwerveDriveKinematics<numModules> kinematics;
|
||||
std::array<units::volt_t, numModules> driveInputs{};
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> driveStates{};
|
||||
std::array<units::volt_t, numModules> steerInputs{};
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> steerStates{};
|
||||
frc::Pose2d pose{frc::Pose2d{}};
|
||||
units::radians_per_second_t omega{0};
|
||||
};
|
||||
@@ -0,0 +1,81 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/kinematics/SwerveModulePosition.h>
|
||||
#include <frc/kinematics/SwerveModuleState.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc/simulation/EncoderSim.h>
|
||||
#include <units/current.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
class SwerveModule {
|
||||
public:
|
||||
explicit SwerveModule(const constants::Swerve::ModuleConstants& consts);
|
||||
void Periodic();
|
||||
void SetDesiredState(frc::SwerveModuleState newState, bool shouldBeOpenLoop,
|
||||
bool steerInPlace);
|
||||
frc::Rotation2d GetAbsoluteHeading() const;
|
||||
frc::SwerveModuleState GetState() const;
|
||||
frc::SwerveModulePosition GetPosition() const;
|
||||
units::volt_t GetDriveVoltage() const;
|
||||
units::volt_t GetSteerVoltage() const;
|
||||
units::ampere_t GetDriveCurrentSim() const;
|
||||
units::ampere_t GetSteerCurrentSim() const;
|
||||
constants::Swerve::ModuleConstants GetModuleConstants() const;
|
||||
void Log();
|
||||
void SimulationUpdate(units::meter_t driveEncoderDist,
|
||||
units::meters_per_second_t driveEncoderRate,
|
||||
units::ampere_t driveCurrent,
|
||||
units::radian_t steerEncoderDist,
|
||||
units::radians_per_second_t steerEncoderRate,
|
||||
units::ampere_t steerCurrent);
|
||||
|
||||
private:
|
||||
const constants::Swerve::ModuleConstants moduleConstants;
|
||||
|
||||
frc::PWMSparkMax driveMotor;
|
||||
frc::Encoder driveEncoder;
|
||||
frc::PWMSparkMax steerMotor;
|
||||
frc::Encoder steerEncoder;
|
||||
|
||||
frc::SwerveModuleState desiredState{};
|
||||
bool openLoop{false};
|
||||
|
||||
frc::PIDController drivePIDController{constants::Swerve::kDriveKP,
|
||||
constants::Swerve::kDriveKI,
|
||||
constants::Swerve::kDriveKD};
|
||||
frc::PIDController steerPIDController{constants::Swerve::kSteerKP,
|
||||
constants::Swerve::kSteerKI,
|
||||
constants::Swerve::kSteerKD};
|
||||
|
||||
frc::sim::EncoderSim driveEncoderSim;
|
||||
units::ampere_t driveCurrentSim{0};
|
||||
frc::sim::EncoderSim steerEncoderSim;
|
||||
units::ampere_t steerCurrentSim{0};
|
||||
};
|
||||
@@ -0,0 +1,34 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include <hal/HAL.h>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
HAL_Initialize(500, 0);
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
int ret = RUN_ALL_TESTS();
|
||||
return ret;
|
||||
}
|
||||
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