diff --git a/.gitignore b/.gitignore index de0a2d1..75d10cd 100644 --- a/.gitignore +++ b/.gitignore @@ -159,5 +159,6 @@ gradle-app.setting bin/ # Simulation GUI and other tools window save file -*-window.json -.vscode/launch.json +simgui*.json + +src/main/deploy/config.json diff --git a/.vscode/launch.json b/.vscode/launch.json index c9c9713..4cb2727 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,18 +4,18 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ - { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true, + "desktop": true }, { "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", "desktop": false, + "postDebugTask": "downloadDeployDirectory" } ] } diff --git a/.vscode/settings.json b/.vscode/settings.json index bd61b25..5aecc09 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -26,5 +26,8 @@ }, ], "java.test.defaultConfig": "WPIlibUnitTests", - "java.dependency.packagePresentation": "flat" + "java.dependency.packagePresentation": "hierarchical", + "files.associations": { + "*.path": "json" + } } diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..0e86bd2 --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,34 @@ +{ + "version": "2.0.0", + "tasks": [ + { + "label": "downloadDeployDirectory", + "type": "shell", + "command": "scp -prv lvuser@roboRIO-4388-FRC.local:/home/lvuser/deploy ./src/main/", + "presentation": { + "echo": true, + "reveal": "always", + "focus": false, + "panel": "dedicated", + "showReuseMessage": true, + "clear": true + }, + "problemMatcher": [] + }, + { + "label": "pathPlanner", + "type": "shell", + "osx": { + "command": "open -n /Applications/PathPlanner.app" + }, + "presentation": { + "echo": false, + "reveal": "silent", + "focus": false, + "panel": "dedicated", + "showReuseMessage": false, + "clear": true + } + } + ] +} \ No newline at end of file diff --git a/2022NoWayHome.code-workspace b/2022NoWayHome.code-workspace new file mode 100644 index 0000000..077020e --- /dev/null +++ b/2022NoWayHome.code-workspace @@ -0,0 +1,18 @@ +{ + "folders": [ + { + "path": "." + }, + { + "path": "NetworkTablesDesktopClient" + } + ], + "settings": { + "java.configuration.updateBuildConfiguration": "automatic", + "java.server.launchMode": "Standard", + "java.dependency.packagePresentation": "hierarchical", + "files.associations": { + "*.path": "json" + } + } +} \ No newline at end of file diff --git a/NetworkTablesDesktopClient/.gitignore b/NetworkTablesDesktopClient/.gitignore new file mode 100644 index 0000000..d5697ec --- /dev/null +++ b/NetworkTablesDesktopClient/.gitignore @@ -0,0 +1,2 @@ +build/ +bin/ diff --git a/NetworkTablesDesktopClient/.vscode/launch.json b/NetworkTablesDesktopClient/.vscode/launch.json new file mode 100644 index 0000000..bdf6e37 --- /dev/null +++ b/NetworkTablesDesktopClient/.vscode/launch.json @@ -0,0 +1,12 @@ +{ + "version": "0.2.0", + "configurations": [ + { + "type": "java", + "name": "Launch NetworkTablesDesktopClient", + "request": "launch", + "mainClass": "NetworkTablesDesktopClient", + "projectName": "NetworkTablesDesktopClient" + } + ] +} diff --git a/NetworkTablesDesktopClient/build.gradle b/NetworkTablesDesktopClient/build.gradle new file mode 100644 index 0000000..8fbac1e --- /dev/null +++ b/NetworkTablesDesktopClient/build.gradle @@ -0,0 +1,24 @@ +apply plugin: 'java' + +sourceCompatibility = JavaVersion.VERSION_11 +targetCompatibility = JavaVersion.VERSION_11 + +repositories { + maven { url "https://frcmaven.wpi.edu/artifactory/release/" } +} + +dependencies { + // Add ntcore-java + implementation "edu.wpi.first.ntcore:ntcore-java:2022.2.1" + + // Add wpiutil-java + implementation "edu.wpi.first.wpiutil:wpiutil-java:2022.2.1" + + // Add ntcore-jni for runtime. We are adding all supported platforms + // so that our application will work on all supported platforms. + implementation "edu.wpi.first.ntcore:ntcore-jni:2022.2.1:windowsx86" + implementation "edu.wpi.first.ntcore:ntcore-jni:2022.2.1:windowsx86-64" + implementation "edu.wpi.first.ntcore:ntcore-jni:2022.2.1:linuxx86-64" + implementation "edu.wpi.first.ntcore:ntcore-jni:2022.2.1:linuxraspbian" + implementation "edu.wpi.first.ntcore:ntcore-jni:2022.2.1:osxx86-64" +} diff --git a/NetworkTablesDesktopClient/gradle/wrapper/gradle-wrapper.jar b/NetworkTablesDesktopClient/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000..41d9927 Binary files /dev/null and b/NetworkTablesDesktopClient/gradle/wrapper/gradle-wrapper.jar differ diff --git a/NetworkTablesDesktopClient/gradle/wrapper/gradle-wrapper.properties b/NetworkTablesDesktopClient/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000..41dfb87 --- /dev/null +++ b/NetworkTablesDesktopClient/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,5 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=wrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-7.4-bin.zip +zipStoreBase=GRADLE_USER_HOME +zipStorePath=wrapper/dists diff --git a/NetworkTablesDesktopClient/gradlew b/NetworkTablesDesktopClient/gradlew new file mode 100755 index 0000000..1b6c787 --- /dev/null +++ b/NetworkTablesDesktopClient/gradlew @@ -0,0 +1,234 @@ +#!/bin/sh + +# +# Copyright © 2015-2021 the original authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# + +############################################################################## +# +# Gradle start up script for POSIX generated by Gradle. +# +# Important for running: +# +# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is +# noncompliant, but you have some other compliant shell such as ksh or +# bash, then to run this script, type that shell name before the whole +# command line, like: +# +# ksh Gradle +# +# Busybox and similar reduced shells will NOT work, because this script +# requires all of these POSIX shell features: +# * functions; +# * expansions «$var», «${var}», «${var:-default}», «${var+SET}», +# «${var#prefix}», «${var%suffix}», and «$( cmd )»; +# * compound commands having a testable exit status, especially «case»; +# * various built-in commands including «command», «set», and «ulimit». +# +# Important for patching: +# +# (2) This script targets any POSIX shell, so it avoids extensions provided +# by Bash, Ksh, etc; in particular arrays are avoided. +# +# The "traditional" practice of packing multiple parameters into a +# space-separated string is a well documented source of bugs and security +# problems, so this is (mostly) avoided, by progressively accumulating +# options in "$@", and eventually passing that to Java. +# +# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS, +# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly; +# see the in-line comments for details. +# +# There are tweaks for specific operating systems such as AIX, CygWin, +# Darwin, MinGW, and NonStop. +# +# (3) This script is generated from the Groovy template +# https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# within the Gradle project. +# +# You can find Gradle at https://github.com/gradle/gradle/. +# +############################################################################## + +# Attempt to set APP_HOME + +# Resolve links: $0 may be a link +app_path=$0 + +# Need this for daisy-chained symlinks. +while + APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path + [ -h "$app_path" ] +do + ls=$( ls -ld "$app_path" ) + link=${ls#*' -> '} + case $link in #( + /*) app_path=$link ;; #( + *) app_path=$APP_HOME$link ;; + esac +done + +APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit + +APP_NAME="Gradle" +APP_BASE_NAME=${0##*/} + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD=maximum + +warn () { + echo "$*" +} >&2 + +die () { + echo + echo "$*" + echo + exit 1 +} >&2 + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "$( uname )" in #( + CYGWIN* ) cygwin=true ;; #( + Darwin* ) darwin=true ;; #( + MSYS* | MINGW* ) msys=true ;; #( + NONSTOP* ) nonstop=true ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD=$JAVA_HOME/jre/sh/java + else + JAVACMD=$JAVA_HOME/bin/java + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD=java + which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." +fi + +# Increase the maximum file descriptors if we can. +if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + ulimit -n "$MAX_FD" || + warn "Could not set maximum file descriptor limit to $MAX_FD" + esac +fi + +# Collect all arguments for the java command, stacking in reverse order: +# * args from the command line +# * the main class name +# * -classpath +# * -D...appname settings +# * --module-path (only if needed) +# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables. + +# For Cygwin or MSYS, switch paths to Windows format before running java +if "$cygwin" || "$msys" ; then + APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) + CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) + + JAVACMD=$( cygpath --unix "$JAVACMD" ) + + # Now convert the arguments - kludge to limit ourselves to /bin/sh + for arg do + if + case $arg in #( + -*) false ;; # don't mess with options #( + /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath + [ -e "$t" ] ;; #( + *) false ;; + esac + then + arg=$( cygpath --path --ignore --mixed "$arg" ) + fi + # Roll the args list around exactly as many times as the number of + # args, so each arg winds up back in the position where it started, but + # possibly modified. + # + # NB: a `for` loop captures its iteration list before it begins, so + # changing the positional parameters here affects neither the number of + # iterations, nor the values presented in `arg`. + shift # remove old arg + set -- "$@" "$arg" # push replacement arg + done +fi + +# Collect all arguments for the java command; +# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of +# shell script including quotes and variable substitutions, so put them in +# double quotes to make sure that they get re-expanded; and +# * put everything else in single quotes, so that it's not re-expanded. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Use "xargs" to parse quoted args. +# +# With -n1 it outputs one arg per line, with the quotes and backslashes removed. +# +# In Bash we could simply go: +# +# readarray ARGS < <( xargs -n1 <<<"$var" ) && +# set -- "${ARGS[@]}" "$@" +# +# but POSIX shell has neither arrays nor command substitution, so instead we +# post-process each arg (as a line of input to sed) to backslash-escape any +# character that might be a shell metacharacter, then use eval to reverse +# that process (while maintaining the separation between arguments), and wrap +# the whole thing up as a single "set" statement. +# +# This will of course break if any of these variables contains a newline or +# an unmatched quote. +# + +eval "set -- $( + printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | + xargs -n1 | + sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | + tr '\n' ' ' + )" '"$@"' + +exec "$JAVACMD" "$@" diff --git a/NetworkTablesDesktopClient/gradlew.bat b/NetworkTablesDesktopClient/gradlew.bat new file mode 100644 index 0000000..107acd3 --- /dev/null +++ b/NetworkTablesDesktopClient/gradlew.bat @@ -0,0 +1,89 @@ +@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%" == "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%"=="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! +if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 +exit /b 1 + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/NetworkTablesDesktopClient/settings.gradle b/NetworkTablesDesktopClient/settings.gradle new file mode 100644 index 0000000..e69de29 diff --git a/NetworkTablesDesktopClient/src/main/java/NetworkTablesDesktopClient.java b/NetworkTablesDesktopClient/src/main/java/NetworkTablesDesktopClient.java new file mode 100644 index 0000000..8d323c1 --- /dev/null +++ b/NetworkTablesDesktopClient/src/main/java/NetworkTablesDesktopClient.java @@ -0,0 +1,36 @@ +import java.io.File; +import java.io.IOException; +import java.nio.file.Files; +import java.nio.file.Path; +import java.util.Scanner; + +import edu.wpi.first.networktables.EntryListenerFlags; +import edu.wpi.first.networktables.NetworkTableInstance; + +/** + * Connects to the NetworkTables server and listens for changes to the "Recording" table. When a + * change is detected, it writes the value to a file. + */ +public class NetworkTablesDesktopClient { + public static void main(String[] args) { + NetworkTableInstance instance = NetworkTableInstance.getDefault(); + instance.getTable("Recording").addEntryListener((table, key, entry, value, flags) -> { + Path path = Path.of("..", "src", "main", "deploy", "pathplanner", key); + File file = path.toFile(); + try { + System.err.println("Creating path " + key); + file.createNewFile(); + Files.writeString(file.toPath(), value.getString()); + System.err.println("Recorded path to " + file.getPath()); + } catch (IOException exception) { + exception.printStackTrace(); + } + }, EntryListenerFlags.kNew); + instance.startClientTeam(4388); + instance.startDSClient(); + try (Scanner scanner = new Scanner(System.in)) { + System.err.println("Press enter to stop..."); + scanner.nextLine(); + } + } +} diff --git a/build.gradle b/build.gradle index 7e60634..25822fe 100644 --- a/build.gradle +++ b/build.gradle @@ -1,8 +1,8 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO - + plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2022.2.1" + id "edu.wpi.first.GradleRIO" version "2022.4.1" } sourceCompatibility = JavaVersion.VERSION_11 @@ -21,12 +21,14 @@ deploy { // want to store a team number in this file. team = project.frc.getTeamNumber() 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')) { + // Enable visualvm + //jvmArgs << "-Dcom.sun.management.jmxremote=true -Dcom.sun.management.jmxremote.port=1099 -Dcom.sun.management.jmxremote.local.only=false -Dcom.sun.management.jmxremote.ssl=false -Dcom.sun.management.jmxremote.authenticate=false -Djava.rmi.server.hostname=10.43.88.2" } // Static files artifact @@ -34,6 +36,7 @@ deploy { files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' } + } } } @@ -52,6 +55,9 @@ def includeDesktopSupport = true dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() + implementation 'com.diffplug.durian:durian:3.4.0' + implementation 'com.fasterxml.jackson.datatype:jackson-datatype-jdk8:2.13.1' + implementation 'org.fusesource.jansi:jansi:2.4.0' roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv new file mode 100644 index 0000000..99413bc --- /dev/null +++ b/src/main/deploy/ShooterData.csv @@ -0,0 +1,3 @@ +Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) +0 ,16 ,12000 +999 ,28.8 ,12000 \ No newline at end of file diff --git a/src/main/deploy/example.txt b/src/main/deploy/example.txt deleted file mode 100644 index 70c79b6..0000000 --- a/src/main/deploy/example.txt +++ /dev/null @@ -1,3 +0,0 @@ -Files placed in this directory will be deployed to the RoboRIO into the -'deploy' directory in the home folder. Use the 'FileUtilities.getFilePath' wpilib function -to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Diamond.path b/src/main/deploy/pathplanner/Diamond.path new file mode 100644 index 0000000..3d5a9ca --- /dev/null +++ b/src/main/deploy/pathplanner/Diamond.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":3.0,"y":3.0},"prevControl":null,"nextControl":{"x":3.410442907302426,"y":3.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.5,"y":4.5},"prevControl":{"x":4.288229266113705,"y":4.529967680625016},"nextControl":{"x":4.67537248192198,"y":4.475183036710464},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.0,"y":3.0},"prevControl":{"x":5.973638727554094,"y":3.050086417647222},"nextControl":{"x":6.10510365848318,"y":2.8003030488819602},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.5,"y":1.5029823163094613},"prevControl":{"x":4.6862524414704,"y":1.722990549429376},"nextControl":{"x":4.3137475585296,"y":1.2829740831895466},"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":3.0},"prevControl":{"x":3.5480060960121915,"y":2.4748364324314855},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":5.0,"maxAcceleration":5.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Every Ball.path b/src/main/deploy/pathplanner/Every Ball.path new file mode 100644 index 0000000..d1e4271 --- /dev/null +++ b/src/main/deploy/pathplanner/Every Ball.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":6.888836249581911,"y":5.215172677538834},"prevControl":null,"nextControl":{"x":6.537459367052471,"y":6.518965846916928},"holonomicAngle":106.3895403340348,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.8254588419278255,"y":7.8320057763675734},"prevControl":{"x":5.95491348285958,"y":6.759381608647329},"nextControl":{"x":5.95491348285958,"y":6.759381608647329},"holonomicAngle":-136.27303002005684,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.085718036603518,"y":6.18608248452099},"prevControl":{"x":5.222728278411091,"y":6.456690985532331},"nextControl":{"x":4.768933589337623,"y":5.5604025113498725},"holonomicAngle":-109.50244850666218,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.77803093326875,"y":3.4953054389840252},"prevControl":{"x":5.623399779842569,"y":3.7042882657946703},"nextControl":{"x":2.9363292766883204,"y":3.040020127851594},"holonomicAngle":-30.256437163529373,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.7486154042034336,"y":2.667447141251462},"prevControl":{"x":1.8144326730676037,"y":2.686839729398941},"nextControl":{"x":0.7297128839172015,"y":2.3672347915242677},"holonomicAngle":-75.06858282186249,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.2129730034385093,"y":1.1845800804778424},"prevControl":{"x":1.2001001569914929,"y":1.2299758967179297},"nextControl":{"x":1.530279149856384,"y":0.06560677694911808},"holonomicAngle":-39.472459848343846,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.160119378376086,"y":1.9305622828301685},"prevControl":{"x":5.131309514738882,"y":1.9153315905888442},"nextControl":{"x":6.588402375563037,"y":2.6856418291137163},"holonomicAngle":36.02737338510347,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.60730489584927,"y":0.37491647060743843},"prevControl":{"x":7.329902093852659,"y":0.5875919521381767},"nextControl":{"x":7.884707697845882,"y":0.16224098907670015},"holonomicAngle":-41.00908690157027,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":8.617110072204374,"y":0.9389517943373179},"prevControl":{"x":8.535233976824232,"y":0.12019084053588047},"nextControl":{"x":8.698986167584517,"y":1.7577127481387553},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":9.144756020209744,"y":0.31123506308954896},"prevControl":{"x":9.199340083796505,"y":0.7206155399902687},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Five Ball.path b/src/main/deploy/pathplanner/Five Ball.path new file mode 100644 index 0000000..65570f6 --- /dev/null +++ b/src/main/deploy/pathplanner/Five Ball.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":8.334720117716095,"y":1.881355486848905},"prevControl":null,"nextControl":{"x":8.61849999562068,"y":1.250733535949832},"holonomicAngle":-90.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":8.019409142266559,"y":0.26275914620794727},"prevControl":{"x":8.43030614852931,"y":0.2186359777502019},"nextControl":{"x":6.453364630867194,"y":0.430924999781035},"holonomicAngle":178.16716049405798,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.118548168124668,"y":1.9969695111773362},"prevControl":{"x":5.520125502663889,"y":1.354445775900491},"nextControl":{"x":4.750685363439692,"y":2.5855499986862043},"holonomicAngle":124.12854707414519,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.3768579261296454,"y":1.187671340859924},"prevControl":{"x":0.7357256093822531,"y":0.7147048776856182},"nextControl":{"x":2.0179902428770378,"y":1.6606378040342298},"holonomicAngle":-147.6118851465991,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.971403046254368,"y":6.06448109447943},"prevControl":{"x":3.647096949366312,"y":4.256698168568751},"nextControl":null,"holonomicAngle":47.68377515946898,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/L Path copy.path b/src/main/deploy/pathplanner/L Path copy.path new file mode 100644 index 0000000..ccc3079 --- /dev/null +++ b/src/main/deploy/pathplanner/L Path copy.path @@ -0,0 +1,37 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 5.0, + "y": 2.0 + }, + "prevControl": null, + "nextControl": { + "x": 5.004218182347978, + "y": 2.0 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false + }, + { + "anchorPoint": { + "x": 5.0, + "y": 5.0 + }, + "prevControl": { + "x": 4.991875448297241, + "y": 5.0 + }, + "nextControl": null, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false + } + ], + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "isReversed": null +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/L Path.path b/src/main/deploy/pathplanner/L Path.path new file mode 100644 index 0000000..15b82fa --- /dev/null +++ b/src/main/deploy/pathplanner/L Path.path @@ -0,0 +1,55 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 2.0, + "y": 2.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 2.0 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false + }, + { + "anchorPoint": { + "x": 5.0, + "y": 2.0 + }, + "prevControl": { + "x": 4.000000000000002, + "y": 2.0 + }, + "nextControl": { + "x": 5.004218182347978, + "y": 2.0 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false + }, + { + "anchorPoint": { + "x": 5.0, + "y": 5.0 + }, + "prevControl": { + "x": 4.991875448297241, + "y": 5.0 + }, + "nextControl": null, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false + } + ], + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "isReversed": null +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Move Down.path b/src/main/deploy/pathplanner/Move Down.path new file mode 100644 index 0000000..1fc1d80 --- /dev/null +++ b/src/main/deploy/pathplanner/Move Down.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":5.0,"y":5.0},"prevControl":null,"nextControl":{"x":5.055485973040912,"y":4.3828225587485665},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":5.0,"y":3.1241654489412927},"nextControl":{"x":5.0,"y":3.1241654489412927},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":5.0},"prevControl":{"x":5.0,"y":3.0836227700747108},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Move Forward.path b/src/main/deploy/pathplanner/Move Forward.path new file mode 100644 index 0000000..ff9f904 --- /dev/null +++ b/src/main/deploy/pathplanner/Move Forward.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":2.0,"y":2.0},"prevControl":null,"nextControl":{"x":2.014863901928194,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.99458955727988,"y":2.0},"nextControl":{"x":4.99458955727988,"y":2.0},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":2.0,"y":2.0},"prevControl":{"x":1.4597663280316806,"y":2.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path new file mode 100644 index 0000000..9c09583 --- /dev/null +++ b/src/main/deploy/pathplanner/New Path.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":1.0,"y":3.0},"prevControl":null,"nextControl":{"x":2.3149413387434365,"y":2.75478019310469},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.143744996350752,"y":3.8428026223141054},"prevControl":{"x":1.0069994823533945,"y":5.5442845062905315},"nextControl":{"x":7.478686694138279,"y":2.0338121072780666},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.870800551727686,"y":4.710905624342894},"prevControl":{"x":6.870800551727686,"y":4.710905624342894},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Rotate.path b/src/main/deploy/pathplanner/Rotate.path new file mode 100644 index 0000000..0707701 --- /dev/null +++ b/src/main/deploy/pathplanner/Rotate.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":null,"nextControl":{"x":3.018133524616885,"y":5.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":{"x":2.9945895572798564,"y":5.0},"nextControl":null,"holonomicAngle":-180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":false} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Test.path b/src/main/deploy/pathplanner/Test.path new file mode 100644 index 0000000..5f6ec63 --- /dev/null +++ b/src/main/deploy/pathplanner/Test.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":3.1215786569504176,"y":1.6921689015791839},"prevControl":null,"nextControl":{"x":1.7026792674275004,"y":1.7552310966690905},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.20566158373696,"y":3.3948481690066825},"prevControl":{"x":1.9969695111804016,"y":3.342296339765093},"nextControl":{"x":4.204717742092026,"y":3.4382853932829898},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.076506704737548,"y":2.701164023017701},"prevControl":{"x":4.771706095136329,"y":3.037495730163874},"nextControl":{"x":5.592408962233515,"y":2.131892566470425},"holonomicAngle":92.0952525645957,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.884289630648579,"y":1.7762518283676958},"prevControl":{"x":5.57049389960884,"y":1.7342103649744238},"nextControl":{"x":8.154698525195206,"y":1.816904912993188},"holonomicAngle":-177.33699923393286,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.8002067038620355,"y":3.4473999982502432},"prevControl":{"x":8.681562190710942,"y":3.5525036567334225},"nextControl":{"x":4.918851217013129,"y":3.3422963397670644},"holonomicAngle":-177.2241965512169,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.111068291102451,"y":1.681658535732834},"prevControl":{"x":5.139568899827807,"y":1.8393140234576038},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":5.0,"maxAcceleration":5.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Triangle.path b/src/main/deploy/pathplanner/Triangle.path new file mode 100644 index 0000000..a6ceaea --- /dev/null +++ b/src/main/deploy/pathplanner/Triangle.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":2.0,"y":3.0},"prevControl":null,"nextControl":{"x":3.0000000000000013,"y":3.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":{"x":3.0,"y":4.0},"nextControl":{"x":3.0,"y":4.0},"holonomicAngle":-90.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.0,"y":3.0},"prevControl":{"x":3.0,"y":3.0},"nextControl":{"x":3.0,"y":3.0},"holonomicAngle":180.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":2.0,"y":3.0},"prevControl":{"x":3.0271430576167138,"y":3.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Two Ball High to Low.path b/src/main/deploy/pathplanner/Two Ball High to Low.path new file mode 100644 index 0000000..43221e3 --- /dev/null +++ b/src/main/deploy/pathplanner/Two Ball High to Low.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":6.620680207650139,"y":5.35387407853639},"prevControl":null,"nextControl":{"x":6.047381083523801,"y":5.557302800000574},"holonomicAngle":169.99202019855858,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.020990716136325,"y":6.167588964393128},"prevControl":{"x":5.409354638931585,"y":5.5018222396012515},"nextControl":{"x":5.409354638931585,"y":5.5018222396012515},"holonomicAngle":129.4995885297982,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.141198597001524,"y":1.9325728539114686},"prevControl":{"x":5.085718036602201,"y":3.2825998236283294},"nextControl":null,"holonomicAngle":-77.66091272167375,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Two Ball Low.path b/src/main/deploy/pathplanner/Two Ball Low.path new file mode 100644 index 0000000..f43098c --- /dev/null +++ b/src/main/deploy/pathplanner/Two Ball Low.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":8.109408578365308,"y":2.117508055242545},"prevControl":null,"nextControl":{"x":8.192629418964295,"y":1.470234850583776},"holonomicAngle":-92.202598161766,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.637823814971061,"y":0.36987040266386867},"prevControl":{"x":8.775175303157184,"y":0.48083152346251423},"nextControl":{"x":6.123786635522426,"y":0.22215945832741724},"holonomicAngle":178.6360724683971,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.141198597001524,"y":1.9048325737118068},"prevControl":{"x":5.61278336039577,"y":1.729144132447284},"nextControl":null,"holonomicAngle":143.8418145601917,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":null,"maxAcceleration":5.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Two Ball Straight to Player Station.path b/src/main/deploy/pathplanner/Two Ball Straight to Player Station.path new file mode 100644 index 0000000..b4b5c9c --- /dev/null +++ b/src/main/deploy/pathplanner/Two Ball Straight to Player Station.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":6.777875128781554,"y":2.4688849377715907},"prevControl":null,"nextControl":{"x":5.797718561726847,"y":2.043533974710114},"holonomicAngle":-156.37062226934333,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.2298190888492497,"y":1.174338528454054},"prevControl":{"x":1.904832573707682,"y":1.0171436073226392},"nextControl":null,"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/src/main/java/edu/wpi/first/wpilibj/DriverStation.java new file mode 100644 index 0000000..876e42d --- /dev/null +++ b/src/main/java/edu/wpi/first/wpilibj/DriverStation.java @@ -0,0 +1,1406 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj; + +import edu.wpi.first.hal.AllianceStationID; +import edu.wpi.first.hal.ControlWord; +import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.MatchInfoData; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import java.nio.ByteBuffer; +import java.util.concurrent.TimeUnit; +import java.util.concurrent.locks.Condition; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; + +/** Provide access to the network communication data to / from the Driver Station. */ +public class DriverStation { + /** Number of Joystick Ports. */ + public static final int kJoystickPorts = 6; + + private static class HALJoystickButtons { + public int m_buttons; + public byte m_count; + } + + private static class HALJoystickAxes { + public float[] m_axes; + public short m_count; + + HALJoystickAxes(int count) { + m_axes = new float[count]; + } + } + + private static class HALJoystickPOVs { + public short[] m_povs; + public short m_count; + + HALJoystickPOVs(int count) { + m_povs = new short[count]; + for (int i = 0; i < count; i++) { + m_povs[i] = -1; + } + } + } + + /** The robot alliance that the robot is a part of. */ + public enum Alliance { + Red, + Blue, + Invalid + } + + public enum MatchType { + None, + Practice, + Qualification, + Elimination + } + + private static final double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0; + private static double m_nextMessageTime; + + private static class DriverStationTask implements Runnable { + DriverStationTask() {} + + @Override + public void run() { + DriverStation.run(); + } + } /* DriverStationTask */ + + private static class MatchDataSender { + @SuppressWarnings("MemberName") + NetworkTable table; + + @SuppressWarnings("MemberName") + NetworkTableEntry typeMetadata; + + @SuppressWarnings("MemberName") + NetworkTableEntry gameSpecificMessage; + + @SuppressWarnings("MemberName") + NetworkTableEntry eventName; + + @SuppressWarnings("MemberName") + NetworkTableEntry matchNumber; + + @SuppressWarnings("MemberName") + NetworkTableEntry replayNumber; + + @SuppressWarnings("MemberName") + NetworkTableEntry matchType; + + @SuppressWarnings("MemberName") + NetworkTableEntry alliance; + + @SuppressWarnings("MemberName") + NetworkTableEntry station; + + @SuppressWarnings("MemberName") + NetworkTableEntry controlWord; + + @SuppressWarnings("MemberName") + boolean oldIsRedAlliance = true; + + @SuppressWarnings("MemberName") + int oldStationNumber = 1; + + @SuppressWarnings("MemberName") + String oldEventName = ""; + + @SuppressWarnings("MemberName") + String oldGameSpecificMessage = ""; + + @SuppressWarnings("MemberName") + int oldMatchNumber; + + @SuppressWarnings("MemberName") + int oldReplayNumber; + + @SuppressWarnings("MemberName") + int oldMatchType; + + @SuppressWarnings("MemberName") + int oldControlWord; + + MatchDataSender() { + table = NetworkTableInstance.getDefault().getTable("FMSInfo"); + typeMetadata = table.getEntry(".type"); + typeMetadata.forceSetString("FMSInfo"); + gameSpecificMessage = table.getEntry("GameSpecificMessage"); + gameSpecificMessage.forceSetString(""); + eventName = table.getEntry("EventName"); + eventName.forceSetString(""); + matchNumber = table.getEntry("MatchNumber"); + matchNumber.forceSetDouble(0); + replayNumber = table.getEntry("ReplayNumber"); + replayNumber.forceSetDouble(0); + matchType = table.getEntry("MatchType"); + matchType.forceSetDouble(0); + alliance = table.getEntry("IsRedAlliance"); + alliance.forceSetBoolean(true); + station = table.getEntry("StationNumber"); + station.forceSetDouble(1); + controlWord = table.getEntry("FMSControlData"); + controlWord.forceSetDouble(0); + } + + private void sendMatchData() { + AllianceStationID allianceID = HAL.getAllianceStation(); + boolean isRedAlliance = false; + int stationNumber = 1; + switch (allianceID) { + case Blue1: + isRedAlliance = false; + stationNumber = 1; + break; + case Blue2: + isRedAlliance = false; + stationNumber = 2; + break; + case Blue3: + isRedAlliance = false; + stationNumber = 3; + break; + case Red1: + isRedAlliance = true; + stationNumber = 1; + break; + case Red2: + isRedAlliance = true; + stationNumber = 2; + break; + default: + isRedAlliance = true; + stationNumber = 3; + break; + } + + String currentEventName; + String currentGameSpecificMessage; + int currentMatchNumber; + int currentReplayNumber; + int currentMatchType; + int currentControlWord; + m_cacheDataMutex.lock(); + try { + currentEventName = DriverStation.m_matchInfo.eventName; + currentGameSpecificMessage = DriverStation.m_matchInfo.gameSpecificMessage; + currentMatchNumber = DriverStation.m_matchInfo.matchNumber; + currentReplayNumber = DriverStation.m_matchInfo.replayNumber; + currentMatchType = DriverStation.m_matchInfo.matchType; + } finally { + m_cacheDataMutex.unlock(); + } + currentControlWord = HAL.nativeGetControlWord(); + + if (oldIsRedAlliance != isRedAlliance) { + alliance.setBoolean(isRedAlliance); + oldIsRedAlliance = isRedAlliance; + } + if (oldStationNumber != stationNumber) { + station.setDouble(stationNumber); + oldStationNumber = stationNumber; + } + if (!oldEventName.equals(currentEventName)) { + eventName.setString(currentEventName); + oldEventName = currentEventName; + } + if (!oldGameSpecificMessage.equals(currentGameSpecificMessage)) { + gameSpecificMessage.setString(currentGameSpecificMessage); + oldGameSpecificMessage = currentGameSpecificMessage; + } + if (currentMatchNumber != oldMatchNumber) { + matchNumber.setDouble(currentMatchNumber); + oldMatchNumber = currentMatchNumber; + } + if (currentReplayNumber != oldReplayNumber) { + replayNumber.setDouble(currentReplayNumber); + oldReplayNumber = currentReplayNumber; + } + if (currentMatchType != oldMatchType) { + matchType.setDouble(currentMatchType); + oldMatchType = currentMatchType; + } + if (currentControlWord != oldControlWord) { + controlWord.setDouble(currentControlWord); + oldControlWord = currentControlWord; + } + } + } + + private static DriverStation instance = new DriverStation(); + + // Joystick User Data + private static HALJoystickAxes[] m_joystickAxes = new HALJoystickAxes[kJoystickPorts]; + private static HALJoystickPOVs[] m_joystickPOVs = new HALJoystickPOVs[kJoystickPorts]; + private static HALJoystickButtons[] m_joystickButtons = new HALJoystickButtons[kJoystickPorts]; + private static MatchInfoData m_matchInfo = new MatchInfoData(); + + // Joystick Cached Data + private static HALJoystickAxes[] m_joystickAxesCache = new HALJoystickAxes[kJoystickPorts]; + private static HALJoystickPOVs[] m_joystickPOVsCache = new HALJoystickPOVs[kJoystickPorts]; + private static HALJoystickButtons[] m_joystickButtonsCache = + new HALJoystickButtons[kJoystickPorts]; + private static MatchInfoData m_matchInfoCache = new MatchInfoData(); + + // Joystick button rising/falling edge flags + private static int[] m_joystickButtonsPressed = new int[kJoystickPorts]; + private static int[] m_joystickButtonsReleased = new int[kJoystickPorts]; + + // preallocated byte buffer for button count + private static final ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1); + + private static final MatchDataSender m_matchDataSender; + + // Internal Driver Station thread + private static Thread m_thread; + + private static volatile boolean m_threadKeepAlive = true; + + private static final ReentrantLock m_cacheDataMutex = new ReentrantLock(); + + private static final Lock m_waitForDataMutex; + private static final Condition m_waitForDataCond; + private static int m_waitForDataCount; + private static final ThreadLocal m_lastCount = ThreadLocal.withInitial(() -> 0); + + private static boolean m_silenceJoystickWarning; + + // Robot state status variables + private static boolean m_userInDisabled; + private static boolean m_userInAutonomous; + private static boolean m_userInTeleop; + private static boolean m_userInTest; + + // Control word variables + private static final ReentrantLock m_controlWordMutex = new ReentrantLock(); + private static final ControlWord m_controlWordCache; + private static long m_lastControlWordUpdate; + + /** + * Gets an instance of the DriverStation. + * + * @return The DriverStation. + * @deprecated Use the static methods + */ + @Deprecated + public static DriverStation getInstance() { + return DriverStation.instance; + } + + /** + * DriverStation constructor. + * + *

The single DriverStation instance is created statically with the instance static member + * variable. + */ + private DriverStation() {} + + static { + HAL.initialize(500, 0); + m_waitForDataCount = 0; + m_waitForDataMutex = new ReentrantLock(); + m_waitForDataCond = m_waitForDataMutex.newCondition(); + + for (int i = 0; i < kJoystickPorts; i++) { + m_joystickButtons[i] = new HALJoystickButtons(); + m_joystickAxes[i] = new HALJoystickAxes(HAL.kMaxJoystickAxes); + m_joystickPOVs[i] = new HALJoystickPOVs(HAL.kMaxJoystickPOVs); + + m_joystickButtonsCache[i] = new HALJoystickButtons(); + m_joystickAxesCache[i] = new HALJoystickAxes(HAL.kMaxJoystickAxes); + m_joystickPOVsCache[i] = new HALJoystickPOVs(HAL.kMaxJoystickPOVs); + } + + m_controlWordCache = new ControlWord(); + m_lastControlWordUpdate = 0; + + m_matchDataSender = new MatchDataSender(); + + m_thread = new Thread(new DriverStationTask(), "FRCDriverStation"); + m_thread.setPriority((Thread.NORM_PRIORITY + Thread.MAX_PRIORITY) / 2); + + m_thread.start(); + } + + /** Kill the thread. */ + public static synchronized void release() { + m_threadKeepAlive = false; + if (m_thread != null) { + try { + m_thread.join(); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + m_thread = null; + } + } + + /** + * Report error to Driver Station. Optionally appends Stack trace to error message. + * + * @param error The error to report. + * @param printTrace If true, append stack trace to error string + */ + public static void reportError(String error, boolean printTrace) { + reportErrorImpl(true, 1, error, printTrace); + } + + /** + * Report error to Driver Station. Appends provided stack trace to error message. + * + * @param error The error to report. + * @param stackTrace The stack trace to append + */ + public static void reportError(String error, StackTraceElement[] stackTrace) { + reportErrorImpl(true, 1, error, stackTrace); + } + + /** + * Report warning to Driver Station. Optionally appends Stack trace to warning message. + * + * @param warning The warning to report. + * @param printTrace If true, append stack trace to warning string + */ + public static void reportWarning(String warning, boolean printTrace) { + reportErrorImpl(false, 1, warning, printTrace); + } + + /** + * Report warning to Driver Station. Appends provided stack trace to warning message. + * + * @param warning The warning to report. + * @param stackTrace The stack trace to append + */ + public static void reportWarning(String warning, StackTraceElement[] stackTrace) { + reportErrorImpl(false, 1, warning, stackTrace); + } + + private static void reportErrorImpl(boolean isError, int code, String error, boolean printTrace) { + reportErrorImpl(isError, code, error, printTrace, Thread.currentThread().getStackTrace(), 3); + } + + private static void reportErrorImpl( + boolean isError, int code, String error, StackTraceElement[] stackTrace) { + reportErrorImpl(isError, code, error, true, stackTrace, 0); + } + + private static void reportErrorImpl( + boolean isError, + int code, + String error, + boolean printTrace, + StackTraceElement[] stackTrace, + int stackTraceFirst) { + // String locString; + // if (stackTrace.length >= stackTraceFirst + 1) { + // locString = stackTrace[stackTraceFirst].toString(); + // } else { + // locString = ""; + // } + // StringBuilder traceString = new StringBuilder(); + // if (printTrace) { + // boolean haveLoc = false; + // for (int i = stackTraceFirst; i < stackTrace.length; i++) { + // String loc = stackTrace[i].toString(); + // traceString.append("\tat ").append(loc).append('\n'); + // // get first user function + // if (!haveLoc && !loc.startsWith("edu.wpi.first")) { + // locString = loc; + // haveLoc = true; + // } + // } + // } + // HAL.sendError(isError, code, false, error, locString, traceString.toString(), true); + // for (StackTraceElement traceElement : stackTrace) + // System.err.println("\tat " + traceElement); + java.util.logging.LogRecord logRecord = new java.util.logging.LogRecord(isError ? java.util.logging.Level.SEVERE : java.util.logging.Level.FINER, error.stripTrailing()); + java.util.Optional.ofNullable(stackTrace).filter(s -> s.length >= stackTraceFirst + 1).map(s -> java.util.Arrays.copyOfRange(s, Math.min(Math.max(0, stackTraceFirst), s.length - 1), s.length - 1)).ifPresent(presentStackTrace -> { + logRecord.setSourceMethodName(presentStackTrace[0].getMethodName()); + String throwableMessage; + if (presentStackTrace[0].toString().equals("edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:63)")) { + throwableMessage = "Epochs" + System.lineSeparator() + logRecord.getMessage(); + presentStackTrace = new java.lang.StackTraceElement[0]; + logRecord.setLevel(java.util.logging.Level.FINEST); + logRecord.setMessage("Execution times:"); + } else if (printTrace) { + long lineCount = logRecord.getMessage().lines().count(); + throwableMessage = (lineCount > 1 ? logRecord.getMessage().lines().findFirst().map(s -> s + " + " + lineCount + " more lines...").orElse("") : logRecord.getMessage()).stripLeading(); + } else return; + java.lang.Throwable throwable = new java.lang.Throwable(throwableMessage); + throwable.setStackTrace(presentStackTrace); + logRecord.setThrown(throwable); + }); + java.util.logging.Logger.getLogger(HAL.class.getSimpleName()).log(logRecord); + } + + /** + * The state of one joystick button. Button indexes begin at 1. + * + * @param stick The joystick to read. + * @param button The button index, beginning at 1. + * @return The state of the joystick button. + */ + public static boolean getStickButton(final int stick, final int button) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + if (button <= 0) { + reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n"); + return false; + } + + m_cacheDataMutex.lock(); + try { + if (button <= m_joystickButtons[stick].m_count) { + return (m_joystickButtons[stick].m_buttons & 1 << (button - 1)) != 0; + } + } finally { + m_cacheDataMutex.unlock(); + } + + reportJoystickUnpluggedWarning( + "Joystick Button " + + button + + " on port " + + stick + + " not available, check if controller is plugged in"); + return false; + } + + /** + * Whether one joystick button was pressed since the last check. Button indexes begin at 1. + * + * @param stick The joystick to read. + * @param button The button index, beginning at 1. + * @return Whether the joystick button was pressed since the last check. + */ + public static boolean getStickButtonPressed(final int stick, final int button) { + if (button <= 0) { + reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n"); + return false; + } + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + m_cacheDataMutex.lock(); + try { + if (button <= m_joystickButtons[stick].m_count) { + // If button was pressed, clear flag and return true + if ((m_joystickButtonsPressed[stick] & 1 << (button - 1)) != 0) { + m_joystickButtonsPressed[stick] &= ~(1 << (button - 1)); + return true; + } else { + return false; + } + } + } finally { + m_cacheDataMutex.unlock(); + } + + reportJoystickUnpluggedWarning( + "Joystick Button " + + button + + " on port " + + stick + + " not available, check if controller is plugged in"); + return false; + } + + /** + * Whether one joystick button was released since the last check. Button indexes begin at 1. + * + * @param stick The joystick to read. + * @param button The button index, beginning at 1. + * @return Whether the joystick button was released since the last check. + */ + public static boolean getStickButtonReleased(final int stick, final int button) { + if (button <= 0) { + reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n"); + return false; + } + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + m_cacheDataMutex.lock(); + try { + if (button <= m_joystickButtons[stick].m_count) { + // If button was released, clear flag and return true + if ((m_joystickButtonsReleased[stick] & 1 << (button - 1)) != 0) { + m_joystickButtonsReleased[stick] &= ~(1 << (button - 1)); + return true; + } else { + return false; + } + } + } finally { + m_cacheDataMutex.unlock(); + } + + reportJoystickUnpluggedWarning( + "Joystick Button " + + button + + " on port " + + stick + + " not available, check if controller is plugged in"); + return false; + } + + /** + * Get the value of the axis on a joystick. This depends on the mapping of the joystick connected + * to the specified port. + * + * @param stick The joystick to read. + * @param axis The analog axis value to read from the joystick. + * @return The value of the axis on the joystick. + */ + public static double getStickAxis(int stick, int axis) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + if (axis < 0 || axis >= HAL.kMaxJoystickAxes) { + throw new IllegalArgumentException("Joystick axis is out of range"); + } + + m_cacheDataMutex.lock(); + try { + if (axis < m_joystickAxes[stick].m_count) { + return m_joystickAxes[stick].m_axes[axis]; + } + } finally { + m_cacheDataMutex.unlock(); + } + + reportJoystickUnpluggedWarning( + "Joystick axis " + + axis + + " on port " + + stick + + " not available, check if controller is plugged in"); + return 0.0; + } + + /** + * Get the state of a POV on the joystick. + * + * @param stick The joystick to read. + * @param pov The POV to read. + * @return the angle of the POV in degrees, or -1 if the POV is not pressed. + */ + public static int getStickPOV(int stick, int pov) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + if (pov < 0 || pov >= HAL.kMaxJoystickPOVs) { + throw new IllegalArgumentException("Joystick POV is out of range"); + } + + m_cacheDataMutex.lock(); + try { + if (pov < m_joystickPOVs[stick].m_count) { + return m_joystickPOVs[stick].m_povs[pov]; + } + } finally { + m_cacheDataMutex.unlock(); + } + + reportJoystickUnpluggedWarning( + "Joystick POV " + + pov + + " on port " + + stick + + " not available, check if controller is plugged in"); + return -1; + } + + /** + * The state of the buttons on the joystick. + * + * @param stick The joystick to read. + * @return The state of the buttons on the joystick. + */ + public static int getStickButtons(final int stick) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + m_cacheDataMutex.lock(); + try { + return m_joystickButtons[stick].m_buttons; + } finally { + m_cacheDataMutex.unlock(); + } + } + + /** + * Returns the number of axes on a given joystick port. + * + * @param stick The joystick port number + * @return The number of axes on the indicated joystick + */ + public static int getStickAxisCount(int stick) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + m_cacheDataMutex.lock(); + try { + return m_joystickAxes[stick].m_count; + } finally { + m_cacheDataMutex.unlock(); + } + } + + /** + * Returns the number of POVs on a given joystick port. + * + * @param stick The joystick port number + * @return The number of POVs on the indicated joystick + */ + public static int getStickPOVCount(int stick) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + m_cacheDataMutex.lock(); + try { + return m_joystickPOVs[stick].m_count; + } finally { + m_cacheDataMutex.unlock(); + } + } + + /** + * Gets the number of buttons on a joystick. + * + * @param stick The joystick port number + * @return The number of buttons on the indicated joystick + */ + public static int getStickButtonCount(int stick) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + m_cacheDataMutex.lock(); + try { + return m_joystickButtons[stick].m_count; + } finally { + m_cacheDataMutex.unlock(); + } + } + + /** + * Gets the value of isXbox on a joystick. + * + * @param stick The joystick port number + * @return A boolean that returns the value of isXbox + */ + public static boolean getJoystickIsXbox(int stick) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + return HAL.getJoystickIsXbox((byte) stick) == 1; + } + + /** + * Gets the value of type on a joystick. + * + * @param stick The joystick port number + * @return The value of type + */ + public static int getJoystickType(int stick) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + return HAL.getJoystickType((byte) stick); + } + + /** + * Gets the name of the joystick at a port. + * + * @param stick The joystick port number + * @return The value of name + */ + public static String getJoystickName(int stick) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + return HAL.getJoystickName((byte) stick); + } + + /** + * Returns the types of Axes on a given joystick port. + * + * @param stick The joystick port number + * @param axis The target axis + * @return What type of axis the axis is reporting to be + */ + public static int getJoystickAxisType(int stick, int axis) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + return HAL.getJoystickAxisType((byte) stick, (byte) axis); + } + + /** + * Returns if a joystick is connected to the Driver Station. + * + *

This makes a best effort guess by looking at the reported number of axis, buttons, and POVs + * attached. + * + * @param stick The joystick port number + * @return true if a joystick is connected + */ + public static boolean isJoystickConnected(int stick) { + return getStickAxisCount(stick) > 0 + || getStickButtonCount(stick) > 0 + || getStickPOVCount(stick) > 0; + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be enabled. + * + * @return True if the robot is enabled, false otherwise. + */ + public static boolean isEnabled() { + m_controlWordMutex.lock(); + try { + updateControlWord(false); + return m_controlWordCache.getEnabled() && m_controlWordCache.getDSAttached(); + } finally { + m_controlWordMutex.unlock(); + } + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be disabled. + * + * @return True if the robot should be disabled, false otherwise. + */ + public static boolean isDisabled() { + return !isEnabled(); + } + + /** + * Gets a value indicating whether the Robot is e-stopped. + * + * @return True if the robot is e-stopped, false otherwise. + */ + public static boolean isEStopped() { + m_controlWordMutex.lock(); + try { + updateControlWord(false); + return m_controlWordCache.getEStop(); + } finally { + m_controlWordMutex.unlock(); + } + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * autonomous mode. + * + * @return True if autonomous mode should be enabled, false otherwise. + */ + public static boolean isAutonomous() { + m_controlWordMutex.lock(); + try { + updateControlWord(false); + return m_controlWordCache.getAutonomous(); + } finally { + m_controlWordMutex.unlock(); + } + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * autonomous mode and enabled. + * + * @return True if autonomous should be set and the robot should be enabled. + */ + public static boolean isAutonomousEnabled() { + m_controlWordMutex.lock(); + try { + updateControlWord(false); + return m_controlWordCache.getAutonomous() && m_controlWordCache.getEnabled(); + } finally { + m_controlWordMutex.unlock(); + } + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * operator-controlled mode. + * + * @return True if operator-controlled mode should be enabled, false otherwise. + * @deprecated Use isTeleop() instead. + */ + @Deprecated(since = "2022", forRemoval = true) + public static boolean isOperatorControl() { + return isTeleop(); + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * operator-controlled mode. + * + * @return True if operator-controlled mode should be enabled, false otherwise. + */ + public static boolean isTeleop() { + return !(isAutonomous() || isTest()); + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * operator-controller mode and enabled. + * + * @return True if operator-controlled mode should be set and the robot should be enabled. + * @deprecated Use isTeleopEnabled() instead. + */ + @Deprecated(since = "2022", forRemoval = true) + public static boolean isOperatorControlEnabled() { + return isTeleopEnabled(); + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * operator-controller mode and enabled. + * + * @return True if operator-controlled mode should be set and the robot should be enabled. + */ + public static boolean isTeleopEnabled() { + m_controlWordMutex.lock(); + try { + updateControlWord(false); + return !m_controlWordCache.getAutonomous() + && !m_controlWordCache.getTest() + && m_controlWordCache.getEnabled(); + } finally { + m_controlWordMutex.unlock(); + } + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in test + * mode. + * + * @return True if test mode should be enabled, false otherwise. + */ + public static boolean isTest() { + m_controlWordMutex.lock(); + try { + updateControlWord(false); + return m_controlWordCache.getTest(); + } finally { + m_controlWordMutex.unlock(); + } + } + + /** + * Gets a value indicating whether the Driver Station is attached. + * + * @return True if Driver Station is attached, false otherwise. + */ + public static boolean isDSAttached() { + m_controlWordMutex.lock(); + try { + updateControlWord(false); + return m_controlWordCache.getDSAttached(); + } finally { + m_controlWordMutex.unlock(); + } + } + + /** + * Gets if a new control packet from the driver station arrived since the last time this function + * was called. + * + * @return True if the control data has been updated since the last call. + */ + public static boolean isNewControlData() { + m_waitForDataMutex.lock(); + try { + int currentCount = m_waitForDataCount; + if (m_lastCount.get() != currentCount) { + m_lastCount.set(currentCount); + return true; + } + } finally { + m_waitForDataMutex.unlock(); + } + return false; + } + + /** + * Gets if the driver station attached to a Field Management System. + * + * @return true if the robot is competing on a field being controlled by a Field Management System + */ + public static boolean isFMSAttached() { + m_controlWordMutex.lock(); + try { + updateControlWord(false); + return m_controlWordCache.getFMSAttached(); + } finally { + m_controlWordMutex.unlock(); + } + } + + /** + * Get the game specific message. + * + * @return the game specific message + */ + public static String getGameSpecificMessage() { + m_cacheDataMutex.lock(); + try { + return m_matchInfo.gameSpecificMessage; + } finally { + m_cacheDataMutex.unlock(); + } + } + + /** + * Get the event name. + * + * @return the event name + */ + public static String getEventName() { + m_cacheDataMutex.lock(); + try { + return m_matchInfo.eventName; + } finally { + m_cacheDataMutex.unlock(); + } + } + + /** + * Get the match type. + * + * @return the match type + */ + public static MatchType getMatchType() { + int matchType; + m_cacheDataMutex.lock(); + try { + matchType = m_matchInfo.matchType; + } finally { + m_cacheDataMutex.unlock(); + } + switch (matchType) { + case 1: + return MatchType.Practice; + case 2: + return MatchType.Qualification; + case 3: + return MatchType.Elimination; + default: + return MatchType.None; + } + } + + /** + * Get the match number. + * + * @return the match number + */ + public static int getMatchNumber() { + m_cacheDataMutex.lock(); + try { + return m_matchInfo.matchNumber; + } finally { + m_cacheDataMutex.unlock(); + } + } + + /** + * Get the replay number. + * + * @return the replay number + */ + public static int getReplayNumber() { + m_cacheDataMutex.lock(); + try { + return m_matchInfo.replayNumber; + } finally { + m_cacheDataMutex.unlock(); + } + } + + /** + * Get the current alliance from the FMS. + * + * @return the current alliance + */ + public static Alliance getAlliance() { + AllianceStationID allianceStationID = HAL.getAllianceStation(); + if (allianceStationID == null) { + return Alliance.Invalid; + } + + switch (allianceStationID) { + case Red1: + case Red2: + case Red3: + return Alliance.Red; + + case Blue1: + case Blue2: + case Blue3: + return Alliance.Blue; + + default: + return Alliance.Invalid; + } + } + + /** + * Gets the location of the team's driver station controls. + * + * @return the location of the team's driver station controls: 1, 2, or 3 + */ + public static int getLocation() { + AllianceStationID allianceStationID = HAL.getAllianceStation(); + if (allianceStationID == null) { + return 0; + } + switch (allianceStationID) { + case Red1: + case Blue1: + return 1; + + case Red2: + case Blue2: + return 2; + + case Blue3: + case Red3: + return 3; + + default: + return 0; + } + } + + /** + * Wait for new data from the driver station. + * + *

Checks if new control data has arrived since the last waitForData call on the current + * thread. If new data has not arrived, returns immediately. + */ + public static void waitForData() { + waitForData(0); + } + + /** + * Wait for new data or for timeout, which ever comes first. If timeout is 0, wait for new data + * only. + * + *

Checks if new control data has arrived since the last waitForData call on the current + * thread. If new data has not arrived, returns immediately. + * + * @param timeoutSeconds The maximum time in seconds to wait. + * @return true if there is new data, otherwise false + */ + public static boolean waitForData(double timeoutSeconds) { + long startTimeMicroS = RobotController.getFPGATime(); + long timeoutMicroS = (long) (timeoutSeconds * 1e6); + m_waitForDataMutex.lock(); + try { + int currentCount = m_waitForDataCount; + if (m_lastCount.get() != currentCount) { + m_lastCount.set(currentCount); + return true; + } + while (m_waitForDataCount == currentCount) { + if (timeoutMicroS > 0) { + long nowMicroS = RobotController.getFPGATime(); + if (nowMicroS < startTimeMicroS + timeoutMicroS) { + // We still have time to wait + boolean signaled = + m_waitForDataCond.await( + startTimeMicroS + timeoutMicroS - nowMicroS, TimeUnit.MICROSECONDS); + if (!signaled) { + // Return false if a timeout happened + return false; + } + } else { + // Time has elapsed. + return false; + } + } else { + m_waitForDataCond.await(); + } + } + m_lastCount.set(m_waitForDataCount); + // Return true if we have received a proper signal + return true; + } catch (InterruptedException ex) { + // return false on a thread interrupt + Thread.currentThread().interrupt(); + return false; + } finally { + m_waitForDataMutex.unlock(); + } + } + + /** + * Return the approximate match time. The FMS does not send an official match time to the robots, + * but does send an approximate match time. The value will count down the time remaining in the + * current period (auto or teleop). Warning: This is not an official time (so it cannot be used to + * dispute ref calls or guarantee that a function will trigger before the match ends) The Practice + * Match function of the DS approximates the behavior seen on the field. + * + * @return Time remaining in current match period (auto or teleop) in seconds + */ + public static double getMatchTime() { + return HAL.getMatchTime(); + } + + /** + * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic + * purposes only. + * + * @param entering If true, starting disabled code; if false, leaving disabled code + */ + public static void inDisabled(boolean entering) { + m_userInDisabled = entering; + } + + /** + * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic + * purposes only. + * + * @param entering If true, starting autonomous code; if false, leaving autonomous code + */ + public static void inAutonomous(boolean entering) { + m_userInAutonomous = entering; + } + + /** + * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic + * purposes only. + * + * @param entering If true, starting teleop code; if false, leaving teleop code + * @deprecated Use {@link #inTeleop(boolean)} instead. + */ + @Deprecated(since = "2022", forRemoval = true) + public static void inOperatorControl(boolean entering) { + m_userInTeleop = entering; + } + + /** + * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic + * purposes only. + * + * @param entering If true, starting teleop code; if false, leaving teleop code + */ + public static void inTeleop(boolean entering) { + m_userInTeleop = entering; + } + + /** + * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic + * purposes only. + * + * @param entering If true, starting test code; if false, leaving test code + */ + public static void inTest(boolean entering) { + m_userInTest = entering; + } + + /** Forces waitForData() to return immediately. */ + public static void wakeupWaitForData() { + m_waitForDataMutex.lock(); + try { + m_waitForDataCount++; + m_waitForDataCond.signalAll(); + } finally { + m_waitForDataMutex.unlock(); + } + } + + /** + * Allows the user to specify whether they want joystick connection warnings to be printed to the + * console. This setting is ignored when the FMS is connected -- warnings will always be on in + * that scenario. + * + * @param silence Whether warning messages should be silenced. + */ + public static void silenceJoystickConnectionWarning(boolean silence) { + m_silenceJoystickWarning = silence; + } + + /** + * Returns whether joystick connection warnings are silenced. This will always return false when + * connected to the FMS. + * + * @return Whether joystick connection warnings are silenced. + */ + public static boolean isJoystickConnectionWarningSilenced() { + return !isFMSAttached() && m_silenceJoystickWarning; + } + + /** + * Copy data from the DS task for the user. If no new data exists, it will just be returned, + * otherwise the data will be copied from the DS polling loop. + */ + protected static void getData() { + // Get the status of all of the joysticks + for (byte stick = 0; stick < kJoystickPorts; stick++) { + m_joystickAxesCache[stick].m_count = + HAL.getJoystickAxes(stick, m_joystickAxesCache[stick].m_axes); + m_joystickPOVsCache[stick].m_count = + HAL.getJoystickPOVs(stick, m_joystickPOVsCache[stick].m_povs); + m_joystickButtonsCache[stick].m_buttons = HAL.getJoystickButtons(stick, m_buttonCountBuffer); + m_joystickButtonsCache[stick].m_count = m_buttonCountBuffer.get(0); + } + + HAL.getMatchInfo(m_matchInfoCache); + + m_controlWordMutex.lock(); + try { + // Force a control word update, to make sure the data is the newest. + updateControlWord(true); + } finally { + m_controlWordMutex.unlock(); + } + + // lock joystick mutex to swap cache data + m_cacheDataMutex.lock(); + try { + for (int i = 0; i < kJoystickPorts; i++) { + // If buttons weren't pressed and are now, set flags in m_buttonsPressed + m_joystickButtonsPressed[i] |= + ~m_joystickButtons[i].m_buttons & m_joystickButtonsCache[i].m_buttons; + + // If buttons were pressed and aren't now, set flags in m_buttonsReleased + m_joystickButtonsReleased[i] |= + m_joystickButtons[i].m_buttons & ~m_joystickButtonsCache[i].m_buttons; + } + + // move cache to actual data + HALJoystickAxes[] currentAxes = m_joystickAxes; + m_joystickAxes = m_joystickAxesCache; + m_joystickAxesCache = currentAxes; + + HALJoystickButtons[] currentButtons = m_joystickButtons; + m_joystickButtons = m_joystickButtonsCache; + m_joystickButtonsCache = currentButtons; + + HALJoystickPOVs[] currentPOVs = m_joystickPOVs; + m_joystickPOVs = m_joystickPOVsCache; + m_joystickPOVsCache = currentPOVs; + + MatchInfoData currentInfo = m_matchInfo; + m_matchInfo = m_matchInfoCache; + m_matchInfoCache = currentInfo; + } finally { + m_cacheDataMutex.unlock(); + } + + wakeupWaitForData(); + m_matchDataSender.sendMatchData(); + } + + /** + * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm + * the DS. + */ + private static void reportJoystickUnpluggedError(String message) { + double currentTime = Timer.getFPGATimestamp(); + if (currentTime > m_nextMessageTime) { + reportError(message, false); + m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL; + } + } + + /** + * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm + * the DS. + */ + private static void reportJoystickUnpluggedWarning(String message) { + if (isFMSAttached() || !m_silenceJoystickWarning) { + double currentTime = Timer.getFPGATimestamp(); + if (currentTime > m_nextMessageTime) { + reportWarning(message, false); + m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL; + } + } + } + + /** Provides the service routine for the DS polling m_thread. */ + private static void run() { + int safetyCounter = 0; + while (m_threadKeepAlive) { + HAL.waitForDSData(); + getData(); + + if (isDisabled()) { + safetyCounter = 0; + } + + safetyCounter++; + if (safetyCounter >= 4) { + MotorSafety.checkMotors(); + safetyCounter = 0; + } + if (m_userInDisabled) { + HAL.observeUserProgramDisabled(); + } + if (m_userInAutonomous) { + HAL.observeUserProgramAutonomous(); + } + if (m_userInTeleop) { + HAL.observeUserProgramTeleop(); + } + if (m_userInTest) { + HAL.observeUserProgramTest(); + } + } + } + + /** + * Forces a control word cache update, and update the passed in control word. + * + * @param word Word to update. + */ + public static void updateControlWordFromCache(ControlWord word) { + m_controlWordMutex.lock(); + try { + updateControlWord(true); + word.update(m_controlWordCache); + } finally { + m_controlWordMutex.unlock(); + } + } + + /** + * Updates the data in the control word cache. Updates if the force parameter is set, or if 50ms + * have passed since the last update. + * + *

Must be called with m_controlWordMutex lock held. + * + * @param force True to force an update to the cache, otherwise update if 50ms have passed. + */ + private static void updateControlWord(boolean force) { + long now = System.currentTimeMillis(); + if (now - m_lastControlWordUpdate > 50 || force) { + HAL.getControlWord(m_controlWordCache); + m_lastControlWordUpdate = now; + } + } +} diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 23afeef..04e0193 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -4,6 +4,15 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; + import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; @@ -22,13 +31,16 @@ import frc4388.utility.LEDPatterns; */ public final class Constants { public static final class SwerveDriveConstants { - public static final double ROTATION_SPEED = 0.1; + public static final double ROTATION_SPEED = 4; public static final double WHEEL_SPEED = 0.1; - public static final double WIDTH = 22; - public static final double HEIGHT = 22; - public static final double JOYSTICK_TO_METERS_PER_SECOND = 5; - public static final double MAX_SPEED_FEET_PER_SEC = 16; - public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; + public static final double WIDTH = 15.25; + public static final double HEIGHT = 15.25; + public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11; + public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2; + public static final double MAX_SPEED_FEET_PER_SEC = 20; // redundant constant? + public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // redundant constant? + + // IDs public static final int LEFT_FRONT_STEER_CAN_ID = 2; public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; public static final int RIGHT_FRONT_STEER_CAN_ID = 4; @@ -41,30 +53,70 @@ public final class Constants { public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; - // ofsets are in degrees - //ofsets are in degrees - // public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141; - // public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281; - // public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531; - // public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594; - public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0; - public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0; - public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0; - public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0; + public static final int GYRO_ID = 14; + + // offsets are in degrees + // NATHAN if you truncate or round or simplify these i will cry + // public static final double LEFT_FRONT_ENCODER_OFFSET = 181.45 - 3.30;// + // 181.7578125;//180.0;//315.0 +45;//180.0; + // public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.0625 + + // 0.18;// 360.-59.0625;//315.0;//224.296875 + + // public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.222;// + // 308.408203125;//225.0;//45.87890625;//360.0 + // public static final double RIGHT_BACK_ENCODER_OFFSET = 360. + 2.15 - 3.637;// + // 180-2.021484375;//0.0;//134.384765625 + + public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.05 - 180 - 90) % 360.; + public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.58 + 180 - 90) % 360.; + public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.50 - 90) % 360.; + public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.31 - 180 - 90) % 360.; // swerve PID constants public static final int SWERVE_SLOT_IDX = 0; public static final int SWERVE_PID_LOOP_IDX = 1; public static final int SWERVE_TIMEOUT_MS = 30; - public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SWERVE_GAINS = new Gains(1.5, 0.0, 0.0, 0.0, 0, 1.0); + + // swerve auto constants + public static final PIDController X_CONTROLLER = new PIDController(0.5, 0.0, 0.0); + public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0); + public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(15.0, 0.1, 0.3, + new TrapezoidProfile.Constraints(Math.PI, Math.PI)); + public static final boolean PATH_RECORD_VELOCITY = true; + public static final double PATH_MAX_VELOCITY = 5.0; + public static final double PATH_MAX_ACCELERATION = 5.0; + public static final double MIN_WAYPOINT_ANGLE = 20; + public static final double MIN_WAYPOINT_DISTANCE = 0.1; + public static final double MIN_WAYPOINT_VELOCITY = 0.1; // swerve configuration public static final double NEUTRAL_DEADBAND = 0.04; public static final double OPEN_LOOP_RAMP_RATE = 0.2; public static final int REMOTE_0 = 0; + // conversions + // gear ratio: 5.14 rev motor = 1 rev wheel + // wheel diameter: official = 4 in, measured = 3.8 in + /* Ratio Calculation */ + public static final double MOTOR_REV_PER_STEER_REV = 12.8; + public static final double MOTOR_REV_PER_WHEEL_REV = 6.12;// 5.142857; + public static final double WHEEL_DIAMETER_INCHES = 4.0; + public static final double TICKS_PER_MOTOR_REV = 2048; + public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; + public static final double INCHES_PER_METER = 39.370; + public static final double METERS_PER_INCH = 1 / INCHES_PER_METER; + + public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; + public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV; + public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV; + public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH; + public static final double TICK_TIME_TO_SECONDS = 0.1; + public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS; + // misc public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; + // TODO: put in real numbers for the hub + public static final Pose2d HUB_POSE = new Pose2d(new Translation2d(0, 0), new Rotation2d(0)); } public static final class SerializerConstants { @@ -98,5 +150,67 @@ public final class Constants { public static final class OIConstants { public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_OPERATOR_ID = 1; + public static final double LEFT_AXIS_DEADBAND = 0.1; + public static final double RIGHT_AXIS_DEADBAND = 0.6; + } + + public static final class ShooterConstants { + /* PID Constants Shooter */ + public static final int CLOSED_LOOP_TIME_MS = 1; + + public static final int SHOOTER_TIMEOUT_MS = 32; + public static final int SHOOTER_SLOT_IDX = 0; + public static final int SHOOTER_PID_LOOP_IDX = 1; + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration( + true, 60, 40, 0.5); + public static final int SHOOTER_FALCON_LEFT_CAN_ID = 23; + public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 24; + public static final int SHOOTER_ROTATE_ID = 31; // TODO: find this value + public static final double TURRET_SPEED_MULTIPLIER = 0.1d; + public static final int DEGREES_PER_ROT = 0; + public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; + public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; + public static final double ENCODER_TICKS_PER_REV = 2048; + + // Shoot Command Constants + public static final Gains SHOOT_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); + + /* Turret Constants */ + // ID + public static final int TURRET_MOTOR_CAN_ID = 30; + public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); + public static final double SHOOTER_TURRET_MIN = -1.0; + public static final float TURRET_FORWARD_LIMIT = 0; // TODO: find + public static final float TURRET_REVERSE_LIMIT = 0; // TODO: find + + // deadzones + public static final double HARD_DEADZONE_LEFT = 0.0; + public static final double HARD_DEADZONE_RIGHT = 340.0; + + public static final double DIG_DEADZONE_LEFT = 40.0; + public static final double DIG_DEADZONE_RIGHT = 60.0; + + public static final int SHOOTER_FALCON_BALLER_ID = 0; // TODO: find + public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; // "// + + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0, 0, 0, 0, 0, 0); // TODO: tune values + + /* Hood Constants */ + public static final int SHOOTER_ANGLE_ADJUST_ID = 32; + public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; // TODO: Find + public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; // TODO: Find + public static final float HOOD_FORWARD_LIMIT = 0; // TODO: find + public static final float HOOD_REVERSE_LIMIT = 0; // TODO: find + + } + + public static final class VisionConstants { + public static final double TURN_P_VALUE = 0.8; + public static final double X_ANGLE_ERROR = 0.5; + public static final double GRAV = 385.83; + public static final double TARGET_HEIGHT = 67.5; + public static final double FOV = 29.8; // Field of view limelight + public static final double LIME_ANGLE = 24.7; } } diff --git a/src/main/java/frc4388/robot/Main.java b/src/main/java/frc4388/robot/Main.java index 39a5e0d..9fbaf8f 100644 --- a/src/main/java/frc4388/robot/Main.java +++ b/src/main/java/frc4388/robot/Main.java @@ -4,7 +4,15 @@ package frc4388.robot; + +import java.util.logging.Level; +import java.util.logging.Logger; + +import com.diffplug.common.base.DurianPlugins; +import com.diffplug.common.base.Errors; + import edu.wpi.first.wpilibj.RobotBase; +import frc4388.utility.AnsiLogging; /** * Do NOT add any static variables to this class, or any initialization at all. @@ -21,6 +29,10 @@ public final class Main { *

If you change your main robot class, change the parameter type. */ public static void main(String... args) { + AnsiLogging.systemInstall(); + DurianPlugins.register(Errors.Plugins.Log.class, e -> Logger.getLogger(e.getStackTrace()[0].getClassName().substring(e.getStackTrace()[0].getClassName().lastIndexOf('.') + 1)).log(Level.SEVERE, e, e::getLocalizedMessage)); RobotBase.startRobot(Robot::new); } } + +// hi ryan -aarav \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 119a032..1e1ae82 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -4,9 +4,22 @@ package frc4388.robot; +import java.io.File; +import java.nio.file.Files; +import java.nio.file.Path; +import java.util.logging.Level; +import java.util.logging.Logger; +import java.util.stream.IntStream; + +import com.diffplug.common.base.Errors; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.RunCommand; import frc4388.utility.RobotTime; /** @@ -17,8 +30,9 @@ import frc4388.utility.RobotTime; * project. */ public class Robot extends TimedRobot { + private static final Logger LOGGER = Logger.getLogger(Robot.class.getSimpleName()); Command m_autonomousCommand; - + private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; @@ -28,9 +42,70 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our + LOGGER.log(Level.ALL, "Logging Test 1/8"); + LOGGER.log(Level.SEVERE, "Logging Test 2/8"); + LOGGER.log(Level.WARNING, "Logging Test 3/8"); + LOGGER.log(Level.INFO, "Logging Test 4/8"); + LOGGER.log(Level.CONFIG, "Logging Test 5/8"); + LOGGER.log(Level.FINE, "Logging Test 6/8"); + LOGGER.log(Level.FINER, "Logging Test 7/8"); + LOGGER.log(Level.FINEST, "Logging Test 8/8"); + Errors.log().run(() -> { + throw new Throwable("Exception Test"); + }); + + // var path = + // PathPlannerUtil.Path.read(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("Move + // Forward.path").toFile()); + // LOGGER.finest(path::toString); + LOGGER.fine("robotInit()"); + // LOGGER.fine("Sssssssssh."); + // DriverStation.silenceJoystickConnectionWarning(true); + // Instantiate our RobotContainer. This will perform all our button bindings, + // and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + // addPeriodic(m_robotContainer::recordPeriodic, kDefaultPeriod); + SmartDashboard.putData(CommandScheduler.getInstance()); + SmartDashboard.putData("JVM Memory", new RunCommand(() -> { + }) { + @Override + public boolean runsWhenDisabled() { + return true; + } + + @Override + public String getName() { + if (isScheduled()) { + Runtime runtime = Runtime.getRuntime(); + long totalMemory = runtime.totalMemory() / 1_000_000; + long freeMemory = runtime.freeMemory() / 1_000_000; + long maxMemory = runtime.maxMemory() / 1_000_000; + return totalMemory - freeMemory + " MB / " + totalMemory + " MB / " + maxMemory + " MB"; + } + return "Not Running"; + } + }); + SmartDashboard.putData("Usable Deploy Space", new RunCommand(() -> { + }) { + @Override + public boolean runsWhenDisabled() { + return true; + } + + @Override + public String getName() { + if (isScheduled()) { + File deploy = Filesystem.getDeployDirectory(); + long usedSpace = Errors.suppress().getWithDefault( + () -> Files.walk(deploy.toPath()).map(Path::toFile).filter(File::isFile).mapToLong(File::length).sum(), + 0l) / 1_000_000; + long usableSpace = deploy.getUsableSpace() / 1_000_000; + return usedSpace + " MB / " + usableSpace + " MB"; + } + return "Not Running"; + } + }); } /** @@ -38,17 +113,27 @@ public class Robot extends TimedRobot { * this for items like diagnostics that you want ran during disabled, * autonomous, teleoperated and test. * - *

This runs after the mode specific periodic functions, but before + *

+ * This runs after the mode specific periodic functions, but before * LiveWindow and SmartDashboard integrated updating. */ @Override public void robotPeriodic() { m_robotTime.updateTimes(); - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic + // Runs the Scheduler. This is responsible for polling buttons, adding + // newly-scheduled + // commands, running already-scheduled commands, removing finished or + // interrupted commands, + // and running subsystem periodic() methods. This must be called from the + // robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + + // print odometry data to smart dashboard for debugging (if causing timeout + // errors, you can comment it) + SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX()); + SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY()); + SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees()); } /** @@ -58,7 +143,18 @@ public class Robot extends TimedRobot { */ @Override public void disabledInit() { + LOGGER.fine("disabledInit()"); m_robotTime.endMatchTime(); + if (isTest()) { + // IMPORTANT: Had to chown the pathplanner folder in order to save autos. + File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner") + .resolve("recording." + System.currentTimeMillis() + ".path").toFile(); + if (Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) { + m_robotContainer.createPath(null, null, false).write(outputFile); + LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath()); + } else + LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); + } } @Override @@ -66,23 +162,14 @@ public class Robot extends TimedRobot { } /** - * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. + * This autonomous runs the autonomous command selected by your + * {@link RobotContainer} class. */ @Override public void autonomousInit() { + LOGGER.fine("autonomousInit()"); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - /*String autoSelected = SmartDashboard.getString("Auto Selector", "Default"); - switch (autoSelected) { - case "My Auto": - autonomousCommand = new MyAutoCommand(); - break; - case "Default Auto": - default: - autonomousCommand = new ExampleCommand(); - break; - }*/ - // schedule the autonomous command (example) if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); @@ -99,6 +186,8 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { + LOGGER.fine("teleopInit()"); + m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw()); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove @@ -107,6 +196,7 @@ public class Robot extends TimedRobot { m_autonomousCommand.cancel(); } m_robotTime.startMatchTime(); + DriverStation.silenceJoystickConnectionWarning(true); } /** @@ -114,7 +204,11 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - + } + + @Override + public void testInit() { + CommandScheduler.getInstance().cancelAll(); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 38c3459..2db09ab 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -4,17 +4,72 @@ package frc4388.robot; -import edu.wpi.first.wpilibj.Joystick; +import java.io.File; +import java.io.IOException; +import java.io.StringWriter; +import java.nio.file.FileSystems; +import java.nio.file.Path; +import java.nio.file.StandardWatchEventKinds; +import java.nio.file.WatchEvent; +import java.nio.file.WatchKey; +import java.time.Clock; +import java.time.ZoneId; +import java.time.ZonedDateTime; +import java.time.format.DateTimeFormatter; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Comparator; +import java.util.List; +import java.util.Objects; +import java.util.Optional; +import java.util.function.Function; +import java.util.logging.Level; +import java.util.logging.Logger; +import java.util.regex.Matcher; +import java.util.regex.Pattern; +import java.util.stream.Collectors; + +import com.diffplug.common.base.Errors; +import com.pathplanner.lib.PathPlanner; +import com.pathplanner.lib.PathPlannerTrajectory; +import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; +import com.pathplanner.lib.commands.PPSwerveControllerCommand; + +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.NotifierCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import frc4388.robot.Constants.*; +import frc4388.robot.Constants.LEDConstants; +import frc4388.robot.Constants.OIConstants; +import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.commands.AimToCenter; +import frc4388.robot.commands.Shoot; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.Vision; import frc4388.utility.LEDPatterns; -import frc4388.utility.controller.IHandController; -import frc4388.utility.controller.XboxController; +import frc4388.utility.ListeningSendableChooser; +import frc4388.utility.PathPlannerUtil; +import frc4388.utility.PathPlannerUtil.Path.Waypoint; +import frc4388.utility.controller.DeadbandedXboxController; /** * This class is where the bulk of the robot should be declared. Since @@ -24,41 +79,71 @@ import frc4388.utility.controller.XboxController; * commands, and button mappings) should be declared here. */ public class RobotContainer { + private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName()); /* RobotMap */ private final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( - m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor, - m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor, - m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor, - m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor, - m_robotMap.leftFrontEncoder, - m_robotMap.rightFrontEncoder, - m_robotMap.leftBackEncoder, - m_robotMap.rightBackEncoder - ); + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, + m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); private final LED m_robotLED = new LED(m_robotMap.LEDController); - + private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); + private final Hood m_robotHood = new Hood(); + private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); + private final Vision m_robotVision = new Vision(m_robotTurret, m_robotBoomBoom); /* Controllers */ - private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); - private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); + private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); + private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + + /* Autonomous */ + private PathPlannerTrajectory loadedPathTrajectory = null; + private final ListeningSendableChooser autoChooser = new ListeningSendableChooser<>(this::loadPath); + private final List pathPoints = new ArrayList<>(); + private final NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault(); + private final NetworkTable recordingNetworkTable = networkTableInstance.getTable("Recording"); + + private static final DateTimeFormatter RECORDING_FILE_NAME_FORMATTER = DateTimeFormatter + .ofPattern("'Recording' yyyy-MM-dd HH-mm-ss.SSS'.path'"); + private static final Clock SYSTEM_CLOCK = Clock.system(ZoneId.systemDefault()); + private static final Path PATHPLANNER_DIRECTORY = Filesystem.getDeployDirectory().toPath().resolve("pathplanner"); + // Function that removes the ".path" from the end of a string. + private static final Function PATH_EXTENSION_REMOVER = ((Function) Pattern + .compile(".path")::matcher).andThen(m -> m.replaceFirst("")); /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { configureButtonBindings(); - /* Default Commands */ - // drives the swerve drive with a two-axis input from the driver controller - m_robotSwerveDrive.setDefaultCommand( - new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(), - getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive)); // continually sends updates to the Blinkin LED controller to keep the lights on - m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); + // m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, + // m_robotLED)); + + // Turret default command + + // m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, + // m_robotSwerveDrive)); + + m_robotSwerveDrive.setDefaultCommand( + new RunCommand(() -> m_robotSwerveDrive.driveWithInput( + getDriverController().getLeftX(), + getDriverController().getLeftY(), + getDriverController().getRightX(), + getDriverController().getRightY(), + true), + m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); + + // continually sends updates to the Blinkin LED controller to keep the lights on + /* + * m_robotLED + * .setDefaultCommand(new RunCommand(m_robotLED::updateLED, + * m_robotLED).withName("LED update defaultCommand")); + * autoInit(); + * recordInit(); + */ } /** @@ -69,12 +154,43 @@ public class RobotContainer { */ private void configureButtonBindings() { /* Driver Buttons */ + // "XboxController.Button.kBack" was undefined yet, 7 works just fine + new JoystickButton(getDriverController(), 7) + .whenPressed(m_robotSwerveDrive::resetGyro); + + new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) + // new XboxControllerRawButton(m_driverXbox, + // XboxControllerRaw.LEFT_BUMPER_BUTTON) + .whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); + + new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) + // new XboxControllerRawButton(m_driverXbox, + // XboxControllerRaw.RIGHT_BUMPER_BUTTON) + .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); + + new JoystickButton(getDriverController(), XboxController.Button.kA.value) + .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); + new JoystickButton(getDriverController(), XboxController.Button.kX.value) + .whenPressed(() -> m_robotMap.leftFront.reset()) + .whenPressed(() -> m_robotMap.rightFront.reset()) + .whenPressed(() -> m_robotMap.leftBack.reset()) + .whenPressed(() -> m_robotMap.rightBack.reset()); /* Operator Buttons */ // activates "Lit Mode" - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) - .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + /* + * new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + * .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) + * .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + * + * new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + * .whenPressed(new InstantCommand()); + * + * // activates "BoomBoom" + * new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + * .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, + * m_robotHood)); + */ } /** @@ -83,35 +199,224 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // no auto - return new InstantCommand(); + if (loadedPathTrajectory != null) { + PIDController xController = SwerveDriveConstants.X_CONTROLLER; + PIDController yController = SwerveDriveConstants.Y_CONTROLLER; + ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; + thetaController.enableContinuousInput(-Math.PI, Math.PI); + + PathPlannerState initialState = loadedPathTrajectory.getInitialState(); + Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation); + return new SequentialCommandGroup( + new InstantCommand(m_robotSwerveDrive.m_gyro::reset), + new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)), + new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry, + m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, + m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive), + new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path"); + } else { + LOGGER.severe("No auto selected."); + return new RunCommand(() -> { + }).withName("No Autonomous Path"); + } } - /** - * Add your docs here. - */ - public IHandController getDriverController() { + public XboxController getDriverController() { return m_driverXbox; } /** - * Add your docs here. + * Get odometry. + * + * @return Odometry */ - public IHandController getOperatorController() { + public Pose2d getOdometry() { + return m_robotSwerveDrive.getOdometry(); + } + + /** + * Set odometry to given pose. + * + * @param pose Pose to set odometry to. + */ + public void resetOdometry(Pose2d pose) { + m_robotSwerveDrive.resetOdometry(pose); + } + + public XboxController getOperatorController() { return m_operatorXbox; } /** - * Add your docs here. + * Creates a WatchKey for the path planner directory and registers it with the + * WatchService. + * Then creates a NotifierCommand that will update the auto chooser with the + * latest path files. + * Finally, adds the existing path files to the auto chooser */ - public Joystick getOperatorJoystick() { - return m_operatorXbox.getJoyStick(); + private void autoInit() { + try { + WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), + StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, + StandardWatchEventKinds.ENTRY_DELETE); + // TODO: Store this and other commands as fields so they can be rescheduled. + new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) { + @Override + public boolean runsWhenDisabled() { + return true; + } + }.withName("Path Watcher").schedule(); + } catch (IOException exception) { + LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception); + } + Arrays.stream(PATHPLANNER_DIRECTORY.toFile().listFiles()) + .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified)) + .forEachOrdered(file -> autoChooser.addOption(file.getName(), file)); + SmartDashboard.putData("Auto Chooser", autoChooser); } /** - * Add your docs here. + * Creates a button on the SmartDashboard that will record the path of the + * robot. */ - public Joystick getDriverJoystick() { - return m_driverXbox.getJoyStick(); + public void recordInit() { + SmartDashboard.putData("Recording", + new RunCommand(this::recordPeriodic) { + @Override + public void end(boolean interupted) { + new InstantCommand(RobotContainer.this::saveRecording) { + @Override + public boolean runsWhenDisabled() { + return true; + } + }.withName("Save Recording").schedule(); + } + }.withName("Record Path (Cancel to Save)")); + } + + /** + * Called when a file is created, modified, or deleted. + * Adds newly created .path files to the SendableChooser. + * Reloads the path if the currently selected file is modified. + * + * @param watchKey The WatchKey that is being observed. + */ + private void updateAutoChooser(WatchKey watchKey) { + List> watchEvents = watchKey.pollEvents(); + if (!watchEvents.isEmpty()) { + List> pathWatchEvents = watchEvents.stream() + .filter(e -> e.kind().type().isAssignableFrom(Path.class)).collect(Collectors.toList()); + for (WatchEvent pathWatchEvent : pathWatchEvents) { + Path watchEventPath = (Path) pathWatchEvent.context(); + File watchEventFile = watchEventPath.toFile(); + String watchEventFileName = watchEventFile.getName(); + if (watchEventFileName.endsWith(".path")) { + if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_CREATE)) { + LOGGER.log(Level.WARNING, "PathPlanner file {0} created. Options added to SendableChooser.", + watchEventFileName); + autoChooser.addOption(watchEventFile.getName(), watchEventFile); + } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_MODIFY)) { + LOGGER.log(Level.WARNING, "PathPlanner file {0} modified.", watchEventFileName); + if (watchEventFileName.equals(autoChooser.getSelected().getName())) { + LOGGER.log(Level.SEVERE, "PathPlanner file {0} already selected. Reloading path.", watchEventFileName); + loadPath(watchEventFileName); + } + } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_DELETE)) { + LOGGER.log(Level.SEVERE, + "PathPlanner file {0} deleted. Removing options from SendableChooser not yet implemented.", + watchEventFileName); + } + } + } + } + if (!watchKey.reset()) + LOGGER.severe("File watch key invalid."); + } + + private void loadPath(String pathName) { + LOGGER.warning("Loading path " + pathName); + loadedPathTrajectory = null; + loadedPathTrajectory = PathPlanner.loadPath(PATH_EXTENSION_REMOVER.apply(Objects.requireNonNullElse(pathName, "")), + SwerveDriveConstants.PATH_MAX_VELOCITY, SwerveDriveConstants.PATH_MAX_ACCELERATION); + LOGGER.info("Done loading"); + } + + private void saveRecording() { + // IMPORTANT: Had to chown the pathplanner folder in order to save autos. + File outputFile = PATHPLANNER_DIRECTORY + .resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile(); + LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath()); + if (!pathPoints.isEmpty() && Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) { + // TODO: Change to use measured maximum velocity and acceleration. + var path = createPath(null, null, false); + if (RobotBase.isReal()) + path.write(outputFile); + StringWriter writer = new StringWriter(); + path.write(writer); + recordingNetworkTable.getEntry(outputFile.getName()).setString(writer.toString()); + autoChooser.setDefaultOption(outputFile.getName(), outputFile); + LOGGER.log(Level.INFO, "Recorded path to {0}.", outputFile.getPath()); + } else + LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); + } + + public void recordPeriodic() { + Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation(); + Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d(); + // FIXME: Chassis speeds are created from joystick inputs and do not reflect + // actual robot velocity. + Translation2d velocity = new Translation2d(m_robotSwerveDrive.chassisSpeeds.vxMetersPerSecond, + m_robotSwerveDrive.chassisSpeeds.vyMetersPerSecond); + Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false, + SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false); + pathPoints.add(waypoint); + } + + public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) { + // Remove points whose angles to neighboring points are less than 10 degrees + // apart. + int j = 0; + for (int i = 1; i < pathPoints.size() - 1; i++) { + var prev = pathPoints.get(j).anchorPoint.orElseThrow(); + var current = pathPoints.get(i).anchorPoint.orElseThrow(); + var next = pathPoints.get(i + 1).anchorPoint.orElseThrow(); + var fromPrevious = current.minus(prev); + var toNext = next.minus(current); + var angleFromPrevious = new Rotation2d(fromPrevious.getX(), fromPrevious.getY()); + var angleToNext = new Rotation2d(toNext.getX(), toNext.getY()); + if (Math.abs(angleFromPrevious.minus(angleToNext).getDegrees()) < SwerveDriveConstants.MIN_WAYPOINT_ANGLE + || (next.getDistance(prev) < SwerveDriveConstants.MIN_WAYPOINT_DISTANCE + && pathPoints.get(i).velOverride.map(v -> v < SwerveDriveConstants.MIN_WAYPOINT_VELOCITY).orElse(false))) + pathPoints.set(i, null); + else + j = i; + } + pathPoints.removeIf(Objects::isNull); + // Make control points + pathPoints.get(0).nextControl = Optional.of(makeControlPoints(null, pathPoints.get(0).anchorPoint.orElseThrow(), + pathPoints.get(1).anchorPoint.orElseThrow()).getSecond()); + for (int i = 1; i < pathPoints.size() - 1; i++) { + var controls = makeControlPoints(pathPoints.get(i - 1).anchorPoint.orElseThrow(), + pathPoints.get(i).anchorPoint.orElseThrow(), pathPoints.get(i + 1).anchorPoint.orElseThrow()); + pathPoints.get(i).prevControl = Optional.of(controls.getFirst()); + pathPoints.get(i).nextControl = Optional.of(controls.getSecond()); + } + pathPoints.get(pathPoints.size() - 1).prevControl = Optional + .of(makeControlPoints(pathPoints.get(pathPoints.size() - 2).anchorPoint.orElseThrow(), + pathPoints.get(pathPoints.size() - 1).anchorPoint.orElseThrow(), null).getFirst()); + // Create the path + PathPlannerUtil.Path path = new PathPlannerUtil.Path(); + path.waypoints = Optional.ofNullable(pathPoints.toArray(PathPlannerUtil.Path.Waypoint[]::new)); + path.maxVelocity = Optional.ofNullable(maxVelocity); + path.maxAcceleration = Optional.ofNullable(maxAcceleration); + path.isReversed = Optional.ofNullable(isReversed); + pathPoints.clear(); + return path; + } + + private static Pair makeControlPoints(Translation2d prev, Translation2d current, + Translation2d next) { + var line = Objects.requireNonNullElse(next, current).minus(Objects.requireNonNullElse(prev, current)).div(4); + return Pair.of(current.minus(line), current.plus(line)); } } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 843323c..f7825bd 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -4,12 +4,23 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.RemoteSensorSource; +import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.WPI_PigeonIMU; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.motorcontrol.Spark; + import frc4388.robot.Constants.LEDConstants; +import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.subsystems.SwerveModule; /** * Defines and holds all I/O objects on the Roborio. This is useful for unit @@ -18,18 +29,20 @@ import frc4388.robot.Constants.SwerveDriveConstants; public class RobotMap { public RobotMap() { - configureLEDMotorControllers(); + // configureLEDMotorControllers(); configureSwerveMotorControllers(); + // configureShooterMotorControllers(); } /* LED Subsystem */ public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); void configureLEDMotorControllers() { - + } - + /* Swerve Subsystem */ + public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); @@ -42,12 +55,15 @@ public class RobotMap { public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); - + + public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(SwerveDriveConstants.GYRO_ID); + + public SwerveModule leftFront; + public SwerveModule leftBack; + public SwerveModule rightFront; + public SwerveModule rightBack; + void configureSwerveMotorControllers() { - leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); - rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); - leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); - rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); leftFrontSteerMotor.configFactoryDefault(); leftFrontWheelMotor.configFactoryDefault(); @@ -58,38 +74,140 @@ public class RobotMap { rightBackSteerMotor.configFactoryDefault(); rightBackWheelMotor.configFactoryDefault(); - leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + + NeutralMode mode = NeutralMode.Coast; + leftFrontSteerMotor.setNeutralMode(mode); + leftFrontWheelMotor.setNeutralMode(mode);// Coast + rightFrontSteerMotor.setNeutralMode(mode); + rightFrontWheelMotor.setNeutralMode(mode);// Coast + leftBackSteerMotor.setNeutralMode(mode); + leftBackWheelMotor.setNeutralMode(mode);// Coast + rightBackSteerMotor.setNeutralMode(mode); + rightBackWheelMotor.setNeutralMode(mode);// Coast + + leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder, + SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET); + leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder, + SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); + rightFront = new SwerveModule(rightFrontWheelMotor, rightFrontSteerMotor, rightFrontEncoder, + SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); + rightBack = new SwerveModule(rightBackWheelMotor, rightBackSteerMotor, rightBackEncoder, + SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); // config cancoder as remote encoder for swerve steer motors - //leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - //leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - //rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); } -} + // Shooter Config + /* Boom Boom Subsystem */ + public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); + public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); + + public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); + + // Create motor CANSparkMax + void configureShooterMotorControllers() { + + // LEFT FALCON + shooterFalconLeft.configFactoryDefault(); + shooterFalconLeft.setNeutralMode(NeutralMode.Coast); + shooterFalconLeft.setInverted(true); + shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, + ShooterConstants.SHOOTER_TIMEOUT_MS); + + // RIGHT FALCON + shooterFalconRight.setInverted(false); + shooterFalconRight.setNeutralMode(NeutralMode.Coast); + shooterFalconRight.configFactoryDefault(); + shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); + // m_shooterFalconRight.configPeakOutputForward(0, + // ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) + shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, + ShooterConstants.SHOOTER_TIMEOUT_MS); + + // /* Turret Subsytem */ + // shooterFalconRight.configStatorCurrentLimit(new + // StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers + // out of our ass anymore + // shooterFalconLeft.configSupplyCurrentLimit(new + // SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull + // numbers out of our ass anymore + + } + + } + + \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java new file mode 100644 index 0000000..45f0998 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -0,0 +1,78 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; + +public class AimToCenter extends CommandBase { + /** Creates a new AimWithOdometry. */ + Turret m_turret; + SwerveDrive m_drive; + + // use odometry to find x and y later + double x; + double y; + double m_targetAngle; + + // public static Gains m_aimGains; + + public AimToCenter(Turret turret, SwerveDrive drive) { + // Use addRequirements() here to declare subsystem dependencies. + m_turret = turret; + m_drive = drive; + addRequirements(m_turret, m_drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + x = 0; + y = 0; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees()); + m_turret.runshooterRotatePID(m_targetAngle); + } + + public static double angleToCenter(double x, double y, double gyro) { + double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0) + return angle; + } + + /** + * Checks if in hardware deadzone (due to mechanical limitations). + * @param angle Angle to check. + * @return True if in hardware deadzone. + */ + public static boolean isHardwareDeadzone(double angle) { + return ((ShooterConstants.HARD_DEADZONE_LEFT > angle) || (angle > ShooterConstants.HARD_DEADZONE_RIGHT)); + } + + /** + * Checks if in digital deadzone (due to climber). + * @param angle Angle to check. + * @return True if in digital deadzone. + */ + public static boolean isDigitalDeadzone(double angle) { + return ((ShooterConstants.DIG_DEADZONE_LEFT < angle) && (angle < ShooterConstants.DIG_DEADZONE_RIGHT)); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/Shoot.java new file mode 100644 index 0000000..be04921 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Shoot.java @@ -0,0 +1,160 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.hal.simulation.SimulatorJNI; +import edu.wpi.first.wpilibj.simulation.LinearSystemSim; +import edu.wpi.first.wpilibj.simulation.SimHooks; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Hood; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; +import frc4388.utility.DummySensor; +import frc4388.utility.Gains; + +public class Shoot extends CommandBase { + + // subsystems + public SwerveDrive m_swerve; + public BoomBoom m_boomBoom; + public Turret m_turret; + public Hood m_hood; + + // given + public double m_gyroAngle; + public double m_odoX; + public double m_odoY; + public double m_distance; + + // targets + public double m_targetVel; + public double m_targetHood; + public double m_targetAngle; + public double m_driveTargetAngle; + + // pid + public double error; + public double prevError; + public Gains shootGains = ShooterConstants.SHOOT_GAINS; + public double kP, kI, kD; + public double proportional, integral, derivative; + public double time; + public double output; + public double tolerance = 5.0; + + // testing + public DummySensor dummy = new DummySensor(0); + + /** + * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball + * TODO: Velocity Correction + * @param sDrive Drive Train + * @param sShooter Shooter Drum + * @param sTurret Shooter Turret + * @param sHood Shooter Hood + */ + public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Turret sTurret, Hood sHood) { + // Use addRequirements() here to declare subsystem dependencies. + m_swerve = sDrive; + m_boomBoom = sShooter; + m_turret = sTurret; + m_hood = sHood; + + addRequirements(m_swerve, m_boomBoom, m_turret, m_hood); + + kP = shootGains.kP; + kI = shootGains.kI; + kD = shootGains.kD; + + proportional = 0; + integral = 0; + derivative = 0; + time = 0.02; + + DummySensor.resetAll(); + } + + /** + * Updates error for custom PID. + */ + public void updateError() { + error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_odoX = m_swerve.getOdometry().getX(); + m_odoY = m_swerve.getOdometry().getY(); + m_distance = Math.sqrt(Math.pow(m_odoX, 2) + Math.pow(m_odoY, 2)); + + m_gyroAngle = m_swerve.getRegGyro().getDegrees(); + + // get targets (shooter tables) + m_targetVel = m_boomBoom.getVelocity(m_distance); + m_targetHood = m_boomBoom.getHood(m_distance); + + // target angle tests + m_gyroAngle = 0; + m_odoX = -1; + m_odoY = 1; + + m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.; + + // deadzone processing + if (AimToCenter.isHardwareDeadzone(m_targetAngle)) { + m_targetAngle = m_targetAngle + 20; + } + + if (AimToCenter.isDigitalDeadzone(m_targetAngle)) { + // this should rotate the entire swerve drive by 20 degrees, so shoot can now proceed like normal. idk if this will work + m_swerve.driveWithInput(0, 0, Math.cos(m_gyroAngle + 20), Math.sin(m_gyroAngle + 20), true); + } + + // initial error + updateError(); + System.out.println("Error: " + error); + prevError = error; + } + /** + * Run custom PID. + */ + public void runPID() { + prevError = error; + updateError(); + + proportional = error; + integral = integral + error * time; + derivative = (error - prevError) / time; + output = kP * proportional + kI * integral + kD * derivative; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + // custom pid + runPID(); + // m_swerve.driveWithInput(0, 0, output, true); // i have no idea if this is how you rotate the + // entire swerve drive or its the line below + m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true); + + m_hood.runAngleAdjustPID(m_targetHood); + m_boomBoom.runDrumShooterVelocityPID(m_targetVel); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + updateError(); + return Math.abs(error) <= tolerance; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java new file mode 100644 index 0000000..e1286e5 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -0,0 +1,202 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import java.io.File; +import java.io.IOException; +import java.util.Comparator; +import java.util.Map; +import java.util.Optional; +import java.util.function.Function; +import java.util.logging.Level; +import java.util.logging.Logger; +import java.util.regex.Pattern; +import java.util.stream.IntStream; + +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.utility.CSV; +import frc4388.utility.Gains; + +public class BoomBoom extends SubsystemBase { + private static final Logger LOGGER = Logger.getLogger(BoomBoom.class.getSimpleName()); + public WPI_TalonFX m_shooterFalconLeft; + public WPI_TalonFX m_shooterFalconRight; + public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; + public static BoomBoom m_boomBoom; + + double velP; + double input; + + public boolean m_isDrumReady = false; + public double m_fireVel; + + public Hood m_hoodSubsystem; + public Turret m_turretSubsystem; + + // SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later + + public static class ShooterTableEntry { + public Double distance, hoodExt, drumVelocity; + } + + private ShooterTableEntry[] m_shooterTable; + + /* + * Creates new BoomBoom subsystem, has drum shooter and angle adjuster + */ + /** Creates a new BoomBoom. */ + public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { + m_shooterFalconLeft = shooterFalconLeft; + m_shooterFalconRight = shooterFalconRight; + + try { + // This is a helper class that allows us to read a CSV file into a Java array. + CSV csv = new CSV<>(ShooterTableEntry::new) { + // This is a regular expression that removes all parentheses from the header of the CSV file. + private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)"); + + /** + * Removes the parentheses from the CSV header + * + * @param header The header to be sanitized. + * @return The headerSanitizer method is overriding the headerSanitizer method in the parent class. + * The parentheses.matcher(header) is matching the header with the parentheses regular + * expression. The replaceAll method is replacing all of the parentheses with an empty + * string. The super.headerSanitizer(parentheses.matcher(header).replaceAll("")) is calling + * the parent sanitizer. + */ + @Override + protected String headerSanitizer(final String header) { + return super.headerSanitizer(parentheses.matcher(header).replaceAll("")); + } + }; + // This is reading the CSV file into a Java array. + m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath()); + // This is a running a helper method that is logging the contents of the table to the console on a new thread. + new Thread(() -> LOGGER.fine(() -> CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start(); + } catch (final IOException exception) { + ShooterTableEntry dummyEntry = new ShooterTableEntry(); + dummyEntry.distance = 0.0; + dummyEntry.hoodExt = 0.0; + dummyEntry.drumVelocity = 0.0; + m_shooterTable = new ShooterTableEntry[] { dummyEntry }; + LOGGER.log(Level.SEVERE, "Exception while reading shooter CSV table.", exception); + } + } + + public Double getVelocity(final Double distance) { + // This is a function that takes a value (distance) and returns a value (drumVelocity) that is a + // linear interpolation of the two values (drumVelocity) at the two closest points in the table + // (m_shooterTable) to the given value (distance). + return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue(); + } + + public Double getHood(final Double distance) { + // This is a function that takes a value (distance) and returns a value (hoodExt) that is a linear + // interpolation of the two values (hoodExt) at the two closest points in the table (m_shooterTable) + // to the given value (distance). + return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue(); + } + + /** + * Using the given lookup value (x) and lookup getter function, locates the nearest entries in the + * given table to be used as the lower (x0) and upper (x1) bounds for interpolation. Returns the + * interpolation (y) between the two values (y0 and y1) accquired by applying the target getter + * function to the lower and upper bounds entries. + * + * @param table An array of entries to search through. + * @param lookupValue The value to lookup in the table. + * @param lookupGetter A function that takes an entry from the table and returns . + * @param targetGetter A function that takes an E and returns a Number. + * @return The interpolated value. + */ + private static Number linearInterpolate(final E[] table, final Number lookupValue, final Function lookupGetter, final Function targetGetter) { + final Map.Entry closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1])); + final E closestRecord = closestEntry.getValue(); + final int closestRecordIndex = closestEntry.getKey(); + final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))]; + return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord)); + } + + /** + * If the value is in the table, return the entry. Otherwise, return the entry with the closest + * value + * + * @param table The array of values to search. + * @param value The value to search for. + * @param valueGetter A function that takes an element of the table and returns a Number to compare + * with the given value. + * @param exactMatch If true, the lookup will only return a match if the value is exactly equal to + * the value of the entry. If false, the lookup will return a match with a value closest to + * the given value. + * @return The entry with the closest value to the given value. + */ + private static Optional> lookup(final E[] table, final Number value, final Function valueGetter, final boolean exactMatch) { + final Optional> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue()))); + return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty(); + } + + /** + * Given a value x, and two values x0 and x1, and two values y0 and y1, return a value y that is a + * linear interpolation of the two values y0 and y1 + * + * @param x The value to interpolate. + * @param x0 the x coordinate of the lower bound to interpolate to + * @param y0 The value at x0. + * @param x1 the x-coordinate of the upper bound to interpolate to + * @param y1 The value at x1. + * @return The interpolation between y0 and y1 at x. + */ + private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) { + final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue()); + return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue(); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) { + m_hoodSubsystem = subsystem0; + m_turretSubsystem = subsystem1; + } + + /** + * Runs the Drum motor at a given speed + * @param speed percent output form -1.0 to 1.0 + */ + public void runDrumShooter(double speed) { + m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed); + + } + + public void setShooterGains() { + m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX); + m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kF, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kP, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kI, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kD, ShooterConstants.SHOOTER_TIMEOUT_MS); + } + + public void runDrumShooterVelocityPID(double targetVel) { + m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init + m_shooterFalconRight.follow(m_shooterFalconLeft); + // New BoomBoom controller stuff + // Controls a motor with the output of the BangBang controller + // Controls a motor with the output of the BangBang conroller and a feedforward + // Shrinks the feedforward slightly to avoid over speeding the shooter + + // m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * + // feedforward.calculate(targetVel)); + // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 0ecb88b..b811a37 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -16,19 +16,21 @@ import com.revrobotics.CANSparkMax.SoftLimitDirection; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import frc4388.robot.Constants.ShooterConstants; + + +import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.Gains; public class Hood extends SubsystemBase { - // public BoomBoom m_shooterSubsystem; + public BoomBoom m_shooterSubsystem; + public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); public SparkMaxLimitSwitch m_hoodUpLimitSwitch; public SparkMaxLimitSwitch m_hoodDownLimitSwitch; - // public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS; - public RelativeEncoder m_angleEncoder; - public CANSparkMax m_angleAdjustMotor; - public SparkMaxPIDController m_angleAdjusterPIDController; + public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS; + public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder(); + + public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); public boolean m_isHoodReady = false; @@ -37,15 +39,17 @@ public double m_fireAngle; /** Creates a new Hood. */ - public Hood(CANSparkMax angleAdjustMotor) { - m_angleAdjustMotor = angleAdjustMotor; - m_angleAdjustMotor.setIdleMode(IdleMode.kBrake); - m_angleEncoder= m_angleAdjustMotor.getEncoder(); - m_angleAdjusterPIDController = m_angleAdjustMotor.getPIDController(); - m_hoodUpLimitSwitch = m_angleAdjustMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_hoodDownLimitSwitch = m_angleAdjustMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + public Hood() { + m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); + + m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_hoodUpLimitSwitch.enableLimitSwitch(true); m_hoodDownLimitSwitch.enableLimitSwitch(true); + + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_FORWARD_LIMIT); + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_REVERSE_LIMIT); + setHoodSoftLimits(true); } @@ -53,44 +57,43 @@ public double m_fireAngle; public void periodic() { // This method will be called once per scheduler run } - // public void runAngleAdjustPID(double targetAngle) - // { - // //Set PID Coefficients - // m_angleAdjusterPIDController.setP(m_angleAdjusterGains.m_kP); - // m_angleAdjusterPIDController.setI(m_angleAdjusterGains.m_kI); - // m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD); - // m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.m_kIzone); - // m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.m_kF); - // m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN / 5, m_angleAdjusterGains.m_kPeakOutput / 5); - // m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition); - // } - - /** - * Runs The Hood - * @param input The Speed Times 0.6 + /** + * Set status of hood motor soft limits. + * @param set Boolean to set soft limits to. */ - public void runHood(double input) { - input *= .6; - m_angleAdjustMotor.set(input); + public void setHoodSoftLimits(boolean set) { + m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kForward, set); + m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); } + public void runAngleAdjustPID(double targetAngle) + { + //Set PID Coefficients + m_angleAdjusterPIDController.setP(m_angleAdjusterGains.kP); + m_angleAdjusterPIDController.setI(m_angleAdjusterGains.kI); + m_angleAdjusterPIDController.setD(m_angleAdjusterGains.kD); + m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.kIzone); + m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.kF); + m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.kPeakOutput); + + m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition); + } + + + public void runHood(double input) { + m_angleAdjusterMotor.set(input); + } - /** - * Resets The Encoder - */ public void resetGyroAngleAdj(){ m_angleEncoder.setPosition(0); } - /** - * Gets The Encoders Position - * @return The Encoders Position - */ - public double getAnglePositionPID() { - return m_angleEncoder.getPosition(); + + public double getAnglePosition(){ + return 0.0;//m_angleEncoder.getPosition(); } - // public double getAnglePositionDegrees(){ - // return ((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90; - // } + public double getAnglePositionDegrees(){ + return 0.0;//((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90; + } } diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 1ca26e1..e46be14 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -4,10 +4,10 @@ package frc4388.robot.subsystems; -import edu.wpi.first.wpilibj.motorcontrol.Spark; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.logging.Logger; +import edu.wpi.first.wpilibj.motorcontrol.Spark; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.LEDConstants; import frc4388.utility.LEDPatterns; @@ -27,12 +27,12 @@ public class LED extends SubsystemBase { m_LEDController = LEDController; setPattern(LEDConstants.DEFAULT_PATTERN); updateLED(); - System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); + Logger.getLogger(LED.class.getSimpleName()).finer("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); } @Override public void periodic(){ - SmartDashboard.putNumber("LED", m_currentPattern.getValue()); + //SmartDashboard.putNumber("LED", m_currentPattern.getValue()); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 93cffc9..1f4ff9b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,183 +4,285 @@ package frc4388.robot.subsystems; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus; +import com.ctre.phoenix.sensors.WPI_PigeonIMU; +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.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; -import frc4388.utility.RobotGyro; public class SwerveDrive extends SubsystemBase { - SwerveDriveKinematics m_kinematics; - private WPI_TalonFX m_leftFrontSteerMotor; - private WPI_TalonFX m_leftFrontWheelMotor; - private WPI_TalonFX m_rightFrontSteerMotor; - private WPI_TalonFX m_rightFrontWheelMotor; - private WPI_TalonFX m_leftBackSteerMotor; - private WPI_TalonFX m_leftBackWheelMotor; - private WPI_TalonFX m_rightBackSteerMotor; - private WPI_TalonFX m_rightBackWheelMotor; - private CANCoder m_leftFrontEncoder; - private CANCoder m_rightFrontEncoder; - private CANCoder m_leftBackEncoder; - private CANCoder m_rightBackEncoder; + + private SwerveModule m_leftFront; + private SwerveModule m_leftBack; + private SwerveModule m_rightFront; + private SwerveModule m_rightBack; + double halfWidth = SwerveDriveConstants.WIDTH / 2.d; double halfHeight = SwerveDriveConstants.HEIGHT / 2.d; + public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; + Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), + Units.inchesToMeters(halfWidth)); + Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), + Units.inchesToMeters(-halfWidth)); + Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), + Units.inchesToMeters(halfWidth)); + Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), + Units.inchesToMeters(-halfWidth)); + + public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, + m_backLeftLocation, m_backRightLocation); - Translation2d m_frontLeftLocation = - new Translation2d( - Units.inchesToMeters(halfHeight), - Units.inchesToMeters(halfWidth)); - Translation2d m_frontRightLocation = - new Translation2d( - Units.inchesToMeters(halfHeight), - Units.inchesToMeters(-halfWidth)); - Translation2d m_backLeftLocation = - new Translation2d( - Units.inchesToMeters(-halfHeight), - Units.inchesToMeters(halfWidth)); - Translation2d m_backRightLocation = - new Translation2d( - Units.inchesToMeters(-halfHeight), - Units.inchesToMeters(-halfWidth)); - //setSwerveGains(); - - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); public SwerveModule[] modules; - public RobotGyro gyro; //TODO Add Gyro Lol + public WPI_PigeonIMU m_gyro; + protected FusionStatus fstatus = new FusionStatus(); + /* + * Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. + * The numbers used + * below are robot specific, and should be tuned. + */ + public SwerveDrivePoseEstimator m_poseEstimator; + public SwerveDriveOdometry m_odometry; - public SwerveDrive(WPI_TalonFX leftFrontSteerMotor,WPI_TalonFX leftFrontWheelMotor,WPI_TalonFX rightFrontSteerMotor,WPI_TalonFX rightFrontWheelMotor, - WPI_TalonFX leftBackSteerMotor,WPI_TalonFX leftBackWheelMotor,WPI_TalonFX rightBackSteerMotor,WPI_TalonFX rightBackWheelMotor, CANCoder leftFrontEncoder, - CANCoder rightFrontEncoder, - CANCoder leftBackEncoder, - CANCoder rightBackEncoder) - { - m_leftFrontSteerMotor = leftFrontSteerMotor; - m_leftFrontWheelMotor = leftFrontWheelMotor; - m_rightFrontSteerMotor = rightFrontSteerMotor; - m_rightFrontWheelMotor = rightFrontWheelMotor; - m_leftBackSteerMotor = leftBackSteerMotor; - m_leftBackWheelMotor = leftBackWheelMotor; - m_rightBackSteerMotor = rightBackSteerMotor; - m_rightBackWheelMotor = rightBackWheelMotor; - m_leftFrontEncoder = leftFrontEncoder; - m_rightFrontEncoder = rightFrontEncoder; - m_leftBackEncoder = leftBackEncoder; - m_rightBackEncoder = rightBackEncoder; + public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; + public boolean ignoreAngles; + public Rotation2d rotTarget = new Rotation2d(); + public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); - modules = new SwerveModule[] { - new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET), // Front Left - new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET), // Front Right - new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET), // Back Left - new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET) // Back Right - }; - //gyro.reset(); - } -//https://github.com/ZachOrr/MK3-Swerve-Example - /** - * Method to drive the robot using joystick info. - * - * @param xSpeed Speed of the robot in the x direction (forward). - * @param ySpeed Speed of the robot in the y direction (sideways). - * @param rot Angular rate of the robot. - * @param fieldRelative Whether the provided x and y speeds are relative to the field. - */ - public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) - { - /*var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s ); - driveFromSpeeds(speeds);*/ - double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND; - double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND; - SwerveModuleState[] states = - kinematics.toSwerveModuleStates( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3, gyro.getRotation2d()) - : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3)); - SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC)); - for (int i = 0; i < states.length; i++) { - SwerveModule module = modules[i]; - SwerveModuleState state = states[i]; - module.setDesiredState(state); - } - } - //Converts a ChassisSpeed to SwerveModuleStates (targets) - public void driveFromSpeeds(ChassisSpeeds speeds) - { - //https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html - // Convert to module states - SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds); + private final Field2d m_field = new Field2d(); - // Front left module state - SwerveModuleState leftFront = SwerveModuleState.optimize(moduleStates[0], Rotation2d.fromDegrees(m_leftFrontEncoder.getPosition())); - // Front right module state - SwerveModuleState rightFront = SwerveModuleState.optimize(moduleStates[1], Rotation2d.fromDegrees(m_rightFrontEncoder.getPosition())); - // Back left module state - SwerveModuleState leftBack = SwerveModuleState.optimize(moduleStates[2], Rotation2d.fromDegrees(m_leftBackEncoder.getPosition())); - // Back right module state - SwerveModuleState rightBack = SwerveModuleState.optimize(moduleStates[3], Rotation2d.fromDegrees(m_rightBackEncoder.getPosition())); - - //Set the motors - setSwerveMotors(leftFront, leftBack, rightFront, rightBack); + public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack, + WPI_PigeonIMU gyro) { + + m_leftFront = leftFront; + m_leftBack = leftBack; + m_rightFront = rightFront; + m_rightBack = rightBack; + m_gyro = gyro; + + modules = new SwerveModule[] { m_leftFront, m_rightFront, m_leftBack, m_rightBack }; + + m_poseEstimator = new SwerveDrivePoseEstimator( + m_gyro.getRotation2d(), + new Pose2d(), + m_kinematics, + VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)), + VecBuilder.fill(Units.degreesToRadians(1)), + VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); + + m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d()); + + m_gyro.reset(); + SmartDashboard.putData("Field", m_field); } - //Sets steering motors to PID values - public void setSwerveMotors(SwerveModuleState leftFront, SwerveModuleState leftBack, SwerveModuleState rightFront, SwerveModuleState rightBack) - { - /*//Set the Wheel motor speeds - m_leftFrontWheelMotor.set(m_leftFrontSteerMotor.get() + leftFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); - m_rightFrontWheelMotor.set(m_rightFrontSteerMotor.get() + rightFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); - m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + leftBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); - m_rightBackWheelMotor.set(m_rightBackSteerMotor.get() + rightBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); - //PID - m_leftFrontSteerMotor.set(TalonFXControlMode.Position, leftFront.angle.getDegrees() * 12000); - m_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees() * 12000); - m_leftBackSteerMotor.set(TalonFXControlMode.Position, leftBack.angle.getDegrees() * 12000); - m_rightBackSteerMotor.set(TalonFXControlMode.Position, rightBack.angle.getDegrees()); - System.out.println("Target: " + leftFront.angle.getDegrees());*/ - } - - /*public void setSwerveGains(){ - m_leftFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); - m_leftFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_leftFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_leftFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_leftFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_rightFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); - m_rightFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_rightFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_rightFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_rightFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_leftBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); - m_leftBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_leftBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_leftBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_leftBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_rightBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); - m_rightBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_rightBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_rightBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_rightBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - - }*/ + // https://github.com/ZachOrr/MK3-Swerve-Example + /** + * Method to drive the robot using joystick info. + * + * @param speeds[0] Speed of the robot in the x direction (forward). + * @param speeds[1] Speed of the robot in the y direction (sideways). + * @param rot Angular rate of the robot. + * @param fieldRelative Whether the provided x and y speeds are relative to the + * field. + */ + public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative) { + if (speedX == 0 && speedY == 0 && rot == 0) + ignoreAngles = true; + else + ignoreAngles = false; + Translation2d speed = new Translation2d(-speedX, speedY); + double mag = speed.getNorm(); + speed = speed.times(mag * speedAdjust); + double xSpeedMetersPerSecond = -speed.getX(); + double ySpeedMetersPerSecond = speed.getY(); + SwerveModuleState[] states = m_kinematics.toSwerveModuleStates( + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, + rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) + : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, + rot * SwerveDriveConstants.ROTATION_SPEED)); + setModuleStates(states); + } + public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { + ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0; + Translation2d speed = new Translation2d(-leftX, leftY); + speed = speed.times(speed.getNorm() * speedAdjust); + if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND) + rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1)); + double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians(); + double xSpeedMetersPerSecond = -speed.getX(); + double ySpeedMetersPerSecond = speed.getY(); + chassisSpeeds = fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, + rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) + : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); + SwerveModuleState[] states = m_kinematics.toSwerveModuleStates( + chassisSpeeds); + setModuleStates(states); + } - // public void driveFieldRelative(double awayFromStation, double towardLeftBoundary, double rotate) - // { - // var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary, - // rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/) - // } + /** + * Set each module of the swerve drive to the corresponding desired state. + * + * @param desiredStates Array of module states to set. + */ + public void setModuleStates(SwerveModuleState[] desiredStates) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, + Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC)); + for (int i = 0; i < desiredStates.length; i++) { + SwerveModule module = modules[i]; + SwerveModuleState state = desiredStates[i]; + module.setDesiredState(state, false); + } + // modules[0].setDesiredState(desiredStates[0], false); + } + + @Override + public void periodic() { + + updateOdometry(); + updateSmartDash(); + + SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw()); + + m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); + super.periodic(); + } + + private void updateSmartDash() { + // odometry + SmartDashboard.putNumber("Odometry: X", getOdometry().getX()); + SmartDashboard.putNumber("Odometry: Y", getOdometry().getY()); + SmartDashboard.putNumber("Odometry: θ", getOdometry().getRotation().getDegrees()); + + // chassis speeds + // TODO: find the actual max velocity in m/s of the robot in fast mode to have + // accurate chassis speeds + SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond); + SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond); + } + + /** + * Gets the distance between two given poses. + * + * @param p1 The first pose. + * @param p2 The second pose. + * @return Absolute distance between p1 and p2. + */ + public double distBtwPoses(Pose2d p1, Pose2d p2) { + return Math.sqrt(Math.pow(p1.getX() - p2.getX(), 2) + Math.pow(p1.getY() - p2.getY(), 2)); + } + + /** + * Returns a scalar from your distance to the hub to your target distance. + * + * @param target_dist The target distance. + * @return A scalar that multiplies your distance from the hub to get your + * target distance. + */ + public Pose2d poseGivenDist(double target_dist) { + Pose2d p1 = m_poseEstimator.getEstimatedPosition(); + Pose2d p2 = SwerveDriveConstants.HUB_POSE; + + double scalar = target_dist / distBtwPoses(p1, p2); + Pose2d new_pose = new Pose2d(p1.getX() * scalar, p1.getY() * scalar, p1.getRotation()); + + return new_pose; + } + + /** + * Gets the current pose of the robot. + * + * @return Robot's current pose. + */ + public Pose2d getOdometry() { + // return m_odometry.getPoseMeters(); + return m_poseEstimator.getEstimatedPosition(); + } + + /** + * Gets the current gyro using regression formula. + * + * @return Rotation2d object holding current gyro in radians + */ + public Rotation2d getRegGyro() { + double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527; + return new Rotation2d(regCur * Math.PI / 180); + } + + /** + * Resets the odometry of the robot to the given pose. + */ + public void resetOdometry(Pose2d pose) { + m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d()); + } + + /** + * Updates the field relative position of the robot. + */ + public void updateOdometry() { + m_poseEstimator.update(getRegGyro(), + modules[0].getState(), + modules[1].getState(), + modules[2].getState(), + modules[3].getState()); + + // Also apply vision measurements. We use 0.3 seconds in the past as an example + // -- on + // a real robot, this must be calculated based either on latency or timestamps. + // m_poseEstimator.addVisionMeasurement( + // m_poseEstimator.getEstimatedPosition(), + // Timer.getFPGATimestamp() - 0.1); + } + + /** + * Resets pigeon. + */ + public void resetGyro() { + m_gyro.reset(); + rotTarget = new Rotation2d(0); + } + + /** + * Stop all four swerve modules. + */ + public void stopModules() { + modules[0].stop(); + modules[1].stop(); + modules[2].stop(); + modules[3].stop(); + } + + /** + * Switches speed modes. + * + * @param shift True if fast mode, false if slow mode. + */ + public void highSpeed(boolean shift) { + if (shift) { + speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_FAST; + } else { + speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; + } + } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index e9df20a..bd1d8ee 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -7,6 +7,7 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; @@ -21,73 +22,174 @@ import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; public class SwerveModule extends SubsystemBase { - private WPI_TalonFX driveMotor; - private WPI_TalonFX angleMotor; + public WPI_TalonFX angleMotor; + public WPI_TalonFX driveMotor; private CANCoder canCoder; public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; private static double kEncoderTicksPerRotation = 4096; + private SwerveModuleState state; + private double canCoderFeedbackCoefficient; + public long m_currentTime; + public long m_lastTime; + public double m_deltaTime; + public double m_currentPos; + public double m_lastPos; /** Creates a new SwerveModule. */ public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) { this.driveMotor = driveMotor; this.angleMotor = angleMotor; this.canCoder = canCoder; + canCoderFeedbackCoefficient = canCoder.configGetFeedbackCoefficient(); TalonFXConfiguration angleTalonFXConfiguration = new TalonFXConfiguration(); - angleTalonFXConfiguration.slot0.kP = m_swerveGains.m_kP; - angleTalonFXConfiguration.slot0.kI = m_swerveGains.m_kI; - angleTalonFXConfiguration.slot0.kD = m_swerveGains.m_kD; + angleTalonFXConfiguration.slot0.kP = m_swerveGains.kP; + angleTalonFXConfiguration.slot0.kI = m_swerveGains.kI; + angleTalonFXConfiguration.slot0.kD = m_swerveGains.kD; // Use the CANCoder as the remote sensor for the primary TalonFX PID angleTalonFXConfiguration.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID(); angleTalonFXConfiguration.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; angleMotor.configAllSettings(angleTalonFXConfiguration); + // angleMotor.setInverted(true); + // TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration(); + // driveTalonFXConfiguration.slot0.kP = 0.05; + // driveTalonFXConfiguration.slot0.kI = 0.0; + // driveTalonFXConfiguration.slot0.kD = 0.0; + // driveTalonFXConfiguration.primaryPID.selectedFeedbackSensor = + // FeedbackDevice.IntegratedSensor; + driveMotor.configFactoryDefault(); + driveMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30); + driveMotor.configNominalOutputForward(0, 30); + driveMotor.configNominalOutputReverse(0, 30); + driveMotor.configPeakOutputForward(1, 30); + driveMotor.configPeakOutputReverse(-1, 30); + driveMotor.configAllowableClosedloopError(0, 0, 30); + // driveMotor.setInverted(true); + driveMotor.config_kP(0, 0, 30); + driveMotor.config_kI(0, 0, 30); + driveMotor.config_kD(0, 0, 30); + // maybe try a feedforward value? - /*TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration(); - driveTalonFXConfiguration.slot0.kP = kDriveP; - driveTalonFXConfiguration.slot0.kI = kDriveI; - driveTalonFXConfiguration.slot0.kD = kDriveD; - driveTalonFXConfiguration.slot0.kF = kDriveF; - driveMotor.configAllSettings(driveTalonFXConfiguration);*/ + // driveMotor.configAllSettings(driveTalonFXConfiguration); CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration(); canCoderConfiguration.magnetOffsetDegrees = offset; + canCoderConfiguration.sensorDirection = true; canCoder.configAllSettings(canCoderConfiguration); + + m_currentTime = System.currentTimeMillis(); + m_lastTime = System.currentTimeMillis(); + + m_lastPos = driveMotor.getSelectedSensorPosition(); } - - public Rotation2d getAngle() { - // Note: This assumes the CANCoders are setup with the default feedback coefficient - // and the sesnor value reports degrees. + private Rotation2d getAngle() { + // Note: This assumes the CANCoders are setup with the default feedback + // coefficient + // and the sensor value reports degrees. return Rotation2d.fromDegrees(canCoder.getAbsolutePosition()); } /** * Set the speed + rotation of the swerve module from a SwerveModuleState object - * @param desiredState - A SwerveModuleState representing the desired new state of the module + * + * @param desiredState - A SwerveModuleState representing the desired new state + * of the module */ - public void setDesiredState(SwerveModuleState desiredState) { + public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) { Rotation2d currentRotation = getAngle(); - SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); - SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); + // SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), + // currentRotation.getDegrees()); + state = SwerveModuleState.optimize(desiredState, currentRotation); - // Find the difference between our current rotational position + our new rotational position + // Find the difference between our current rotational position + our new + // rotational position Rotation2d rotationDelta = state.angle.minus(currentRotation); - // Find the new absolute position of the module based on the difference in rotation + // Find the new absolute position of the module based on the difference in + // rotation double deltaTicks = (rotationDelta.getDegrees() / 360.) * kEncoderTicksPerRotation; // Convert the CANCoder from it's position reading back to ticks - double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient(); + double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient; double desiredTicks = currentTicks + deltaTicks; - angleMotor.set(TalonFXControlMode.Position, desiredTicks); + if (!ignoreAngle) { + angleMotor.set(TalonFXControlMode.Position, desiredTicks); + } - double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); - driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.SPEED_FEET_PER_SECOND_AT_FULL_POWER); + // Please work + double ftPerSec = Units.metersToFeet(state.speedMetersPerSecond); + double normFtPerSec = ftPerSec / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC; + // double angleCorrection = angleMotor.getSelectedSensorVelocity() * 2.69; + + // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection + + // (Units.metersToInches(state.speedMetersPerSecond) * + // SwerveDriveConstants.TICKS_PER_INCH) / 10); + driveMotor.set(normFtPerSec);// - angleMotor.get()); + // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection); // Ratio + // between axis = 1/1.75 Ratio of wheel is 5.14/1 ratio of steer is 12.8/1 + + // m_currentTime = System.currentTimeMillis(); + // m_deltaTime = (double) (m_currentTime - m_lastTime); + // m_deltaTime = m_deltaTime / 10.0; + + // m_currentPos = driveMotor.getSelectedSensorPosition(); + + // double m_desiredCorrectionVel = 3.2 * angleMotor.getSelectedSensorVelocity(); + // double m_desiredCorrectionPos = (m_deltaTime * m_desiredCorrectionVel) % + // 2048; + // double m_lastPos = (driveMotor.getSelectedSensorPosition() % 2048) - + // (m_deltaTime * driveMotor.getSelectedSensorVelocity()); + // double m_actualDesiredPos = m_deltaTime * + // ((Units.metersToInches(state.speedMetersPerSecond) * + // SwerveDriveConstants.TICKS_PER_INCH) / 10); + + // System.out.println("Current Pos: " + driveMotor.getSelectedSensorPosition()); + // System.out.println("Desired Correction Pos: " + m_desiredCorrectionPos); + // System.out.println("Last Pos: " + m_lastPos); + + // driveMotor.set(TalonFXControlMode.Position, 1500/*m_desiredCorrectionPos*/); + + // m_lastTime = m_currentTime; + // m_lastPos = m_currentPos; } -} \ No newline at end of file + + /** + * Get current module state. + * + * @return The current state of the module in m/s. + */ + public SwerveModuleState getState() { + // return state; + return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK + * SwerveDriveConstants.METERS_PER_INCH * 10, getAngle()); + } + + /** + * Stop the drive and steer motors of current module. + */ + public void stop() { + driveMotor.set(0); + angleMotor.set(0); + } + + @Override + public void periodic() { + Rotation2d currentRotation = getAngle(); + SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); + SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(), + ((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360); + } + + public void reset() { + canCoder.setPositionToAbsolute(); + // canCoder.configSensorInitializationStrategy(initializationStrategy) + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java new file mode 100644 index 0000000..7a0f45e --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -0,0 +1,115 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax.SoftLimitDirection; + +import java.util.concurrent.TimeoutException; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxLimitSwitch; +import com.revrobotics.SparkMaxPIDController; + +import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.commands.Shoot; +import frc4388.utility.Gains; + +public class Turret extends SubsystemBase { + + /** Creates a new Turret. */ + public BoomBoom m_boomBoomSubsystem; + public SwerveDrive m_sDriveSubsystem; + + public CANSparkMax m_boomBoomRotateMotor;// = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, + // MotorType.kBrushless); + public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS; + SparkMaxLimitSwitch m_boomBoomRightLimit, m_boomBoomLeftLimit; + public Gyro m_turretGyro; + + public double m_targetDistance = 0; + + public boolean m_isAimReady = false; + + SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController(); + public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder(); + + // Variables + public Turret(CANSparkMax boomBoomRotateMotor) { // Take in rotate motor as an argument + + m_boomBoomRotateMotor = boomBoomRotateMotor; + m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); + m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); + m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); + + m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomRightLimit.enableLimitSwitch(true); + m_boomBoomLeftLimit.enableLimitSwitch(true); + SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); + SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); + + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_FORWARD_LIMIT); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_REVERSE_LIMIT); + setTurretSoftLimits(true); + + m_boomBoomRotateMotor.setInverted(false); + + m_boomBoomRotatePIDController.setP(m_shooterTGains.kP); + m_boomBoomRotatePIDController.setI(m_shooterTGains.kI); + m_boomBoomRotatePIDController.setD(m_shooterTGains.kD); + m_boomBoomRotatePIDController.setFF(m_shooterTGains.kF); + m_boomBoomRotatePIDController.setIZone(m_shooterTGains.kIzone); + m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.kPeakOutput); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Set status of turret motor soft limits. + * @param set Boolean to set soft limits to. + */ + public void setTurretSoftLimits(boolean set) { + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, set); + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); + } + + public void passRequiredSubsystem(BoomBoom subsystem0, SwerveDrive subsystem1) { + m_boomBoomSubsystem = subsystem0; + m_sDriveSubsystem = subsystem1; + } + + public void runTurretWithInput(double input) { + m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER); + } + + public void runshooterRotatePID(double targetAngle) { + targetAngle = targetAngle / ShooterConstants.DEGREES_PER_ROT; + m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition); + } + + public void resetGyroShooterRotate() { + m_boomBoomRotateEncoder.setPosition(0); + } + + public double getboomBoomRotatePosition() { + return m_boomBoomRotateEncoder.getPosition(); + } + + public double getBoomBoomAngleDegrees() { + return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360 + / ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT; + } + +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java new file mode 100644 index 0000000..748bcd5 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -0,0 +1,131 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.networktables.NetworkTableEntry; + +import frc4388.robot.Constants.VisionConstants; + +public class Vision extends SubsystemBase { +//setup + Turret m_turret; + BoomBoom m_boomBoom; + Hood m_hood; + +NetworkTableEntry xEntry; +//Aiming +double turnAmount = 0; +double xAngle = 0; +double yAngle = 0; +double target = 0; +public double distance; +public double realDistance; +public static double fireVel; +public static double fireAngle; + +public double m_hoodTrim; +public double m_turretTrim; +public double m_fireAngle; + + +public Vision(Turret aimSubsystem, BoomBoom boomBoom) { + m_turret = aimSubsystem; + m_boomBoom = boomBoom; + m_hood = m_boomBoom.m_hoodSubsystem; + //addRequirements(m_turret); + limeOff(); + changePipeline(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(3); +} + +public void track(){ + target = getV(); + xAngle = getX(); + yAngle = getY(); + + //find distance + distance = (VisionConstants.TARGET_HEIGHT) / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); + realDistance = (1.09 * distance) - 12.8; + + // if (target == 1.0) { //checks if target is in view + // //aims left and right + // turnAmount = ((xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE); + // if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) { + // turnAmount = 0; + // } + // else if (turnAmount > 0 && turnAmount < 0.1){ + // turnAmount = 0.1; + // } + // else if (turnAmount < 0 && turnAmount > -0.1){ + // turnAmount = -0.1; + // } + // } + + SmartDashboard.putNumber("Distance to Target", realDistance); + + + // //start CSV + + // fireVel = m_boomBoom.m_shooterTable.getVelocity(realDistance); + // fireAngle = m_boomBoom.m_shooterTable.getHood(realDistance); + // //fire angle unknown so far + // //end of CSV + + // m_boomBoom.m_fireVel = fireVel; + // m_hood.m_fireAngle = fireAngle; + // m_turret.m_targetDistance = distance; + + // checkFinished(); +} + +public void checkFinished(){ + if (xAngle < 0.5 && xAngle > -0.5 && target == 1){ + m_turret.m_isAimReady = true; + } + else{ + m_turret.m_isAimReady = false; + } +} + +public void limeOff(){ + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1); +} + + public void limeOn(){ + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); +} + + public void changePipeline(int pipelineId) + { + NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipelineId); + } + + public double getV() + { + return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0); + } + + public double getX() + { + return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0); + } + + public double getY() + { + return NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0); + } + @Override + public void periodic(){ + //called once per scheduler run + } +} + + diff --git a/src/main/java/frc4388/utility/AnsiLogging.java b/src/main/java/frc4388/utility/AnsiLogging.java new file mode 100644 index 0000000..57a1d90 --- /dev/null +++ b/src/main/java/frc4388/utility/AnsiLogging.java @@ -0,0 +1,135 @@ +package frc4388.utility; + +import static org.fusesource.jansi.Ansi.ansi; + +import java.io.IOException; +import java.io.OutputStream; +import java.io.PrintStream; +import java.io.PrintWriter; +import java.io.StringWriter; +import java.time.ZoneId; +import java.time.ZonedDateTime; +import java.util.Map; +import java.util.Optional; +import java.util.logging.ConsoleHandler; +import java.util.logging.Formatter; +import java.util.logging.Level; +import java.util.logging.LogManager; +import java.util.logging.LogRecord; +import java.util.logging.Logger; + +import org.fusesource.jansi.Ansi; +import org.fusesource.jansi.Ansi.Attribute; +import org.fusesource.jansi.Ansi.Color; +import org.fusesource.jansi.AnsiConsole; + +public class AnsiLogging extends ConsoleHandler { + public static void systemInstall() { + try { + // Configure java.util.logging.Logger to output additional colored information. + LogManager.getLogManager().updateConfiguration(key -> (o, n) -> { + switch (key) { + case ".level": + return Level.ALL.getName(); + case "handlers": + return AnsiColorConsoleHandler.class.getName(); + default: + return n; + } + }); + // Replace standard output streams with org.fusesource.jansi.AnsiPrintStreams. + AnsiConsole.systemInstall(); + // Replace standard output stream with java.util.logging.Logger. + System.setOut(printStreamLogger(Logger.getGlobal(), Level.INFO)); + // Replace standard error output stream with java.util.logging.Logger. + System.setErr(printStreamLogger(Logger.getGlobal(), Level.SEVERE)); + } catch (IOException exception) { + exception.printStackTrace(AnsiConsole.sysErr()); + } + } + + /** + * This class is a ConsoleHandler that uses ANSI escape codes to colorize the output + */ + public static class AnsiColorConsoleHandler extends ConsoleHandler { + @Override + public void publish(LogRecord logRecord) { + AnsiConsole.err().print(getFormatter().format(logRecord)); + AnsiConsole.err().flush(); + } + + @Override + public Formatter getFormatter() { + return formatter; + } + + private static final Formatter formatter = new Formatter() { + private final ZoneId zoneId = ZoneId.systemDefault(); + // Specify and prepare formats for messages + private final Map levelColors = Map.of( + Level.OFF.intValue(), "", + Level.SEVERE.intValue(), makeMessageFormatString(ansi().fgBright(Color.RED)), + Level.WARNING.intValue(), makeMessageFormatString(ansi().fgBright(Color.YELLOW)), + Level.INFO.intValue(), makeMessageFormatString(ansi().fg(Color.GREEN)), + Level.CONFIG.intValue(), makeMessageFormatString(ansi().fgBright(Color.BLUE)), + Level.FINE.intValue(), makeMessageFormatString(ansi().fg(Color.CYAN)), + Level.FINER.intValue(), makeMessageFormatString(ansi().fg(Color.MAGENTA)), + Level.FINEST.intValue(), makeMessageFormatString(ansi().fgBright(Color.BLACK)), + Level.ALL.intValue(), makeMessageFormatString(ansi().fg(Color.DEFAULT)) + ); + + private String makeMessageFormatString(Ansi base) { + return base.bold().a(Attribute.UNDERLINE).a("[%1$tb %1$td %1$tk:%1$tM:%1$tS.%1$tL] %2$s %3$s:").boldOff().a(Attribute.UNDERLINE_OFF).a("%4$s%5$s").a(Attribute.INTENSITY_FAINT).a("%6$s").reset().a("%n").toString(); + } + + private String makeStackTraceString(Throwable throwable) { + StringWriter stringWriter = new StringWriter(); + try (PrintWriter printWriter = new PrintWriter(stringWriter)) { + printWriter.println(); + throwable.printStackTrace(printWriter); + } + return stringWriter.toString(); + } + + @Override + public String format(LogRecord logRecord) { + ZonedDateTime time = ZonedDateTime.ofInstant(logRecord.getInstant(), zoneId); + // Get the logger name, source class name, and/or source method name. + String source = Optional.ofNullable(logRecord.getLoggerName()).or(() -> Optional.ofNullable(logRecord.getSourceClassName())).map(s -> s + " ").orElse("") + Optional.ofNullable(logRecord.getSourceMethodName()).orElse(""); + String message = formatMessage(logRecord); + // Get the stack trace of the exception if it was thrown. + String throwable = Optional.ofNullable(logRecord.getThrown()).map(this::makeStackTraceString).orElse(""); + // Select the appropriate format string for the log level. + String format = levelColors.getOrDefault(logRecord.getLevel().intValue(), levelColors.get(Level.ALL.intValue())); + // Format the log message. + return String.format(format, time, source, logRecord.getLevel().getLocalizedName(), message.lines().count() > 1 ? System.lineSeparator() : " ", message.contains("\033") ? "\033[0m" + message : message, throwable); + } + }; + } + + /** + * Create a PrintStream that writes to the given logger at the given level + * + * @param logger The logger to use. + * @param level The level of the log message. + * @return A new PrintStream object. + */ + private static PrintStream printStreamLogger(Logger logger, Level level) { + return new PrintStream(new OutputStream() { + // This is a buffer that is used to store the characters that are written to the PrintStream. + private final StringBuilder stringBuilder = new StringBuilder(); + + /** + * If the character is a newline, flush the buffer to the logger, otherwise add the character to the + * buffer. + */ + @Override + public void write(int i) throws IOException { + if (i == '\n') { + logger.log(level, stringBuilder::toString); + stringBuilder.setLength(0); + } else stringBuilder.appendCodePoint(i); + } + }); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/CSV.java b/src/main/java/frc4388/utility/CSV.java new file mode 100644 index 0000000..de885a7 --- /dev/null +++ b/src/main/java/frc4388/utility/CSV.java @@ -0,0 +1,236 @@ +package frc4388.utility; + +import java.awt.Color; +import java.io.BufferedReader; +import java.io.IOException; +import java.lang.invoke.MethodHandleProxies; +import java.lang.invoke.MethodHandles; +import java.lang.invoke.MethodType; +import java.lang.reflect.Array; +import java.lang.reflect.Field; +import java.lang.reflect.Modifier; +import java.nio.file.Files; +import java.nio.file.Path; +import java.text.MessageFormat; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.Objects; +import java.util.function.BiConsumer; +import java.util.function.Function; +import java.util.function.IntFunction; +import java.util.function.Predicate; +import java.util.function.Supplier; +import java.util.regex.Pattern; +import java.util.stream.Collectors; +import java.util.stream.IntStream; +import java.util.stream.Stream; + +/** + * Reads and parses a CSV file and returns an array of records. + */ +public class CSV { + private static final Pattern SANITIZER = Pattern.compile("[^$\\w,]"); + + private final Supplier generator; + private final IntFunction arrayGenerator; + private final Map> setters; + + /** + * A binary string operator to be applied to the entire header of the CSV. + */ + protected String headerSanitizer(final String header) { + return SANITIZER.matcher(header).replaceAll(""); + } + + /** + * A binary string operator to be applied to each name in the header of the CSV. + */ + protected String nameProcessor(final String name) { + return Character.toLowerCase(name.charAt(0)) + name.substring(1); + } + + /** + * Creates a new {@code CSV} instance and prepares for populating the fields of objects created by + * the given generator. Private fields and fields of primitive types are not supported. + * @param generator a parameterless supplier which produces a new object with any number of fields + * corresponding to header names from a CSV file. The first character of the names + * from the header in the CSV file will be made lowercase and invalid characters + * will be removed to match Java naming conventions. + * @see #read(Path) + */ + @SuppressWarnings("unchecked") + public CSV(final Supplier generator) { + final Class clazz = generator.get().getClass(); + final Map, Function> fieldParsers = new HashMap<>(); + this.arrayGenerator = size -> (R[]) Array.newInstance(clazz, size); + this.generator = generator; + this.setters = new HashMap<>(); + for (final Field field : clazz.getFields()) { + final Function parser = Modifier.isStatic(field.getModifiers()) ? null : fieldParsers.computeIfAbsent(field.getType(), CSV::getTypeParser); + if (parser != null) + this.setters.put(field.getName(), (final R obj, final String rawValue) -> { + try { + field.set(obj, rawValue.isEmpty() ? null : parser.apply(rawValue)); + } catch (final IllegalAccessException e) { + throw new RuntimeException(e); + } + }); + } + } + + /** + * Reads and parses the contents of the given CSV file, and returns an array filled with populated + * objects created with the previously given generator. Cells are parsed using their corresponding + * field's {@code valueOf(String)} function. + * @param path the path to a CSV file + * @return the parsed data from the CSV file + * @throws IOException if an I/O error occurs opening the file + */ + @SuppressWarnings("unchecked") + public R[] read(final Path path) throws IOException { + try (final BufferedReader reader = Files.newBufferedReader(path)) { + final BiConsumer[] fieldSetters = Stream.of(headerSanitizer(reader.readLine()).split(",")).map(this::nameProcessor).map(setters::get).toArray(BiConsumer[]::new); + final Stream lines = reader.lines(); + return lines.filter(Predicate.not(String::isBlank)).map(line -> deserializeRecordString(line, fieldSetters, generator.get())).toArray(this.arrayGenerator); + } + } + + @SuppressWarnings("unchecked") + private static Function getTypeParser(final Class type) { + try { + return type.isAssignableFrom(String.class) ? Function.identity() : MethodHandleProxies.asInterfaceInstance(Function.class, MethodHandles.publicLookup().findStatic(type, "valueOf", MethodType.methodType(type, String.class))); + } catch (final NoSuchMethodException | IllegalAccessException e) { + return null; + } + } + + private static R deserializeRecordString(final String recordString, final BiConsumer[] fieldParseSetters, final R object) { + final int recordStringLength = recordString.length(); + int fieldBeginIndex = 0, tryFieldEndFromIndex = 0, i = 0; + while (tryFieldEndFromIndex < recordStringLength && i < fieldParseSetters.length) { + final int tryFieldEndIndex = recordString.indexOf(',', tryFieldEndFromIndex); + String field = recordString.substring(fieldBeginIndex, tryFieldEndIndex == -1 ? recordStringLength : tryFieldEndIndex).strip(); + if (!field.isEmpty() && (tryFieldEndFromIndex != fieldBeginIndex || field.charAt(0) == '"')) { + final int fieldLength = field.length(); + if (countTrailing(field, '"') % 2 == 0) { + tryFieldEndFromIndex = tryFieldEndIndex + 1; + continue; + } else + field = field.substring(1, fieldLength - 1).replace("\"\"", "\""); + } + final BiConsumer setter = fieldParseSetters[i]; + if (setter != null) + setter.accept(object, field); + tryFieldEndFromIndex = fieldBeginIndex = tryFieldEndIndex + 1; + i++; + } + return object; + } + + private static int countTrailing(final String str, final char c) { + final int l = str.length(); + int count = 0; + while (str.charAt(l - count - 1) == c && count < l) + count++; + return count; + } + + public static final class ReflectionTable { + public static String create(final T[] objects, boolean colored) { + final Field[] fields = Stream.of(objects).flatMap(object -> Stream.of(object.getClass().getFields())).distinct().toArray(Field[]::new); + final List> rows = new ArrayList<>(); + rows.add(Stream.of(fields).map(ReflectionTable::new).collect(Collectors.toList())); + rows.addAll(Stream.of(objects).map(obj -> Stream.of(fields).map(field -> new ReflectionTable(obj, field)).collect(Collectors.toList())).collect(Collectors.toList())); + final int[] columnWidths = rows.stream().map(row -> row.stream().map(cell -> cell.string).mapToInt(String::length).toArray()).reduce(new int[fields.length], (result, row) -> IntStream.range(0, row.length).map(i -> Math.max(result[i], row[i])).toArray()); + if (colored) + IntStream.range(0, fields.length).forEach(i -> { + final var columnSummaryStatistics = rows.stream().skip(1).mapToDouble(row -> row.get(i).getValue().doubleValue()).summaryStatistics(); + rows.stream().skip(1).forEach(row -> row.get(i).colorByValue(columnSummaryStatistics.getMin(), columnSummaryStatistics.getMax())); + }); + MessageFormat formatFormat = new MessageFormat(colored ? "{2} %{0}{1}s {3}" : " %{0}{1}s "); + return rows.stream().map(row -> IntStream.range(0, row.size()).mapToObj(i -> String.format(formatFormat.format(new Object[] { row.get(i).padRight ? "-" : "", columnWidths[i], row.get(i).escape, RESET_STYLE }), row.get(i).string)).collect(Collectors.joining("|"))).collect(Collectors.joining(LF)); + } + + private static final Color GRADIENT_MIN = new Color(0x00, 0x99, 0x00); + private static final Color GRADIENT_MAX = new Color(0x66, 0xFF, 0x66); + private static final String CONTROL = "\033"; + private static final String CSI = "["; + private static final String LF = "\n"; + private static final String RESET = "0"; + private static final String BOLD = "1"; + private static final String ITALIC = "3"; + private static final String UNDERLINE = "4"; + private static final String BACKGROUND_RED = "41"; + private static final String FOREGROUND = "38"; + private static final String BACKGROUND = "48"; + private static final String TRUECOLOR = "2"; + private static final String SEPARATOR = ";"; + private static final String SGR = "m"; + private static final String HEADER_STYLE = CONTROL + CSI + BOLD + SEPARATOR + UNDERLINE + SGR; + private static final String NULL_STYLE = CONTROL + CSI + ITALIC + SGR; + private static final String ERROR_STYLE = CONTROL + CSI + ITALIC + SGR + CONTROL + CSI + BACKGROUND_RED + SGR; + private static final String RESET_STYLE = CONTROL + CSI + RESET + SGR; + private Object value; + private String string; + private boolean padRight; + private String escape; + + private ReflectionTable(final Object obj, final Field field) { + try { + value = field.get(obj); + string = Objects.toString(value); + padRight = !Number.class.isAssignableFrom(field.getType()); + escape = Objects.isNull(value) ? NULL_STYLE : ""; + } catch (final IllegalAccessException | IllegalArgumentException e) { + value = null; + string = e.getClass().getSimpleName(); + padRight = false; + escape = ERROR_STYLE; + } + } + + private ReflectionTable(final Field field) { + value = null; + string = field.getName(); + padRight = true; + escape = HEADER_STYLE; + } + + private Number getValue() { + return padRight ? Objects.hashCode(string) : Objects.requireNonNullElse((Number) value, 0); + } + + private void colorByValue(final Number min, final Number max) { + if (Objects.nonNull(value)) { + final double range = max.doubleValue() - min.doubleValue(); + final double normal = range == 0 ? 0 : (getValue().doubleValue() - min.doubleValue()) / range; + final Color color = new Color(range(normal, GRADIENT_MIN.getRed(), GRADIENT_MAX.getRed()), range(normal, GRADIENT_MIN.getGreen(), GRADIENT_MAX.getGreen()), range(normal, GRADIENT_MIN.getBlue(), GRADIENT_MAX.getBlue())); + escape += (contrastRatio(color, Color.BLACK) > contrastRatio(Color.WHITE, color) ? colorTo24BitSGR(Color.BLACK, false) : colorTo24BitSGR(Color.WHITE, false)) + colorTo24BitSGR(color, true); + } + } + + private static String colorTo24BitSGR(final Color color, final boolean background) { + return CONTROL + CSI + (background ? BACKGROUND : FOREGROUND) + SEPARATOR + TRUECOLOR + SEPARATOR + color.getRed() + SEPARATOR + color.getGreen() + SEPARATOR + color.getBlue() + SGR; + } + + private static int range(final double normal, final int min, final int max) { + return (int) (normal * (max - min) + min); + } + + /* https://www.w3.org/TR/WCAG20/#contrast-ratiodef */ + private static float contrastRatio(final Color lighter, final Color darker) { + return (relativeLuminance(lighter) + 0.05f) / (relativeLuminance(darker) + 0.05f); + } + + /* https://www.w3.org/TR/2008/REC-WCAG20-20081211/#relativeluminancedef */ + private static float relativeLuminance(final Color color) { + final float[] components = color.getRGBComponents(null); + final float r = components[0] <= 0.03928f ? components[0] / 12.92f : (float) Math.pow((components[0] + 0.055f) / 1.055f, 2.4f); + final float g = components[1] <= 0.03928f ? components[1] / 12.92f : (float) Math.pow((components[1] + 0.055f) / 1.055f, 2.4f); + final float b = components[2] <= 0.03928f ? components[2] / 12.92f : (float) Math.pow((components[2] + 0.055f) / 1.055f, 2.4f); + return 0.2126f * r + 0.7152f * g + 0.0722f * b; + } + } +} diff --git a/src/main/java/frc4388/utility/DummySensor.java b/src/main/java/frc4388/utility/DummySensor.java new file mode 100644 index 0000000..dadef80 --- /dev/null +++ b/src/main/java/frc4388/utility/DummySensor.java @@ -0,0 +1,74 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.utility; + +import java.util.ArrayList; + +public class DummySensor { + + private double value; + public double start; + public static ArrayList instances = new ArrayList(); + + /** + * Creates a new DummySensor, which is a helper class for conducting PID tests without a physical robot. + * @param init The start "position" of the sensor (default is 0). + */ + public DummySensor(double init) { + value = init; + start = init; + instances.add(this); + } + + /** + * Creates a new DummySensor, which is a helper class for conducting PID tests without a physical robot. + */ + public DummySensor() { + value = 0; + start = 0; + instances.add(this); + } + + /** + * Reset the "position" of the DummySensor to its starting value. + */ + public void reset() { + value = start; + } + + /** + * Reset the "position" of the DummySensor to a given value. + * @param val The "position" to reset the DummySensor to. + */ + public void reset(double val) { + value = val; + } + + /** + * Reset all instances of DummySensor to their starting values. + */ + public static void resetAll() { + for (DummySensor instance : instances) { + instance.reset(); + } + } + + /** + * Get the "position" of the DummySensor. + * @return The current "position". + */ + public double get() { + return value; + } + + /** + * Apply an input to the DummySensor, changing its "position". + * @param input The input to apply. + */ + public void apply(double input) { + value = value + input; + } + +} diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java index 930e34f..13144a4 100644 --- a/src/main/java/frc4388/utility/Gains.java +++ b/src/main/java/frc4388/utility/Gains.java @@ -6,14 +6,14 @@ package frc4388.utility; /** Add your docs here. */ public class Gains { - public double m_kP; - public double m_kI; - public double m_kD; - public double m_kF; - public int m_kIzone; - public double m_kPeakOutput; - public double m_kmaxOutput; - public double m_kminOutput; + public double kP; + public double kI; + public double kD; + public double kF; + public int kIzone; + public double kPeakOutput; + public double kmaxOutput; + public double kminOutput; /** * Creates Gains object for PIDs @@ -26,14 +26,14 @@ public class Gains { */ public Gains(double kP, double kI, double kD, double kF, int kIzone, double kPeakOutput) { - m_kP = kP; - m_kI = kI; - m_kD = kD; - m_kF = kF; - m_kIzone = kIzone; - m_kPeakOutput = kPeakOutput; - m_kmaxOutput = m_kPeakOutput; - m_kminOutput = -m_kPeakOutput; + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIzone = kIzone; + this.kPeakOutput = kPeakOutput; + this.kmaxOutput = this.kPeakOutput; + this.kminOutput = -this.kPeakOutput; } /** @@ -48,13 +48,13 @@ public class Gains { */ public Gains(double kP, double kI, double kD, double kF, int kIzone, double kMinOutput, double kMaxOutput) { - m_kP = kP; - m_kI = kI; - m_kD = kD; - m_kF = kF; - m_kIzone = kIzone; - m_kminOutput = kMinOutput; - m_kmaxOutput = kMaxOutput; - m_kPeakOutput = (Math.abs(m_kminOutput) > Math.abs(m_kmaxOutput)) ? Math.abs(m_kminOutput) : Math.abs(m_kmaxOutput); + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIzone = kIzone; + this.kminOutput = kMinOutput; + this.kmaxOutput = kMaxOutput; + this.kPeakOutput = (Math.abs(this.kminOutput) > Math.abs(this.kmaxOutput)) ? Math.abs(this.kminOutput) : Math.abs(this.kmaxOutput); } } diff --git a/src/main/java/frc4388/utility/ListeningSendableChooser.java b/src/main/java/frc4388/utility/ListeningSendableChooser.java new file mode 100644 index 0000000..c0f3432 --- /dev/null +++ b/src/main/java/frc4388/utility/ListeningSendableChooser.java @@ -0,0 +1,182 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +// package edu.wpi.first.wpilibj.smartdashboard; +package frc4388.utility; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.networktables.NTSendable; +import edu.wpi.first.networktables.NTSendableBuilder; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.util.sendable.SendableRegistry; +import java.util.ArrayList; +import java.util.LinkedHashMap; +import java.util.List; +import java.util.Map; +import java.util.concurrent.atomic.AtomicInteger; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.Consumer; + +/** + * The {@link ListeningSendableChooser} class is a useful tool for presenting a selection of options to the + * {@link SmartDashboard}. + * + *

For instance, you may wish to be able to select between multiple autonomous modes. You can do + * this by putting every possible Command you want to run as an autonomous into a {@link + * ListeningSendableChooser} and then put it into the {@link SmartDashboard} to have a list of options appear + * on the laptop. Once autonomous starts, simply ask the {@link ListeningSendableChooser} what the selected + * value is. + * + * @param The type of the values to be stored + */ +public class ListeningSendableChooser implements NTSendable, AutoCloseable { + /** The key for the default value. */ + private static final String DEFAULT = "default"; + /** The key for the selected option. */ + private static final String SELECTED = "selected"; + /** The key for the active option. */ + private static final String ACTIVE = "active"; + /** The key for the option array. */ + private static final String OPTIONS = "options"; + /** The key for the instance number. */ + private static final String INSTANCE = ".instance"; + /** A map linking strings to the objects the represent. */ + private final Map m_map = new LinkedHashMap<>(); + + private String m_defaultChoice = ""; + private final int m_instance; + private static final AtomicInteger s_instances = new AtomicInteger(); + + private Consumer m_consumer; + + /** Instantiates a {@link ListeningSendableChooser}. */ + public ListeningSendableChooser(Consumer consumer) { + m_consumer = consumer; + m_instance = s_instances.getAndIncrement(); + SendableRegistry.add(this, "SendableChooser", m_instance); + } + + @Override + public void close() { + SendableRegistry.remove(this); + } + + /** + * Adds the given object to the list of options. On the {@link SmartDashboard} on the desktop, the + * object will appear as the given name. + * + * @param name the name of the option + * @param object the option + */ + public void addOption(String name, V object) { + m_map.put(name, object); + } + + /** + * Adds the given object to the list of options. + * + * @param name the name of the option + * @param object the option + * @deprecated Use {@link #addOption(String, Object)} instead. + */ + @Deprecated + public void addObject(String name, V object) { + addOption(name, object); + } + + /** + * Adds the given object to the list of options and marks it as the default. Functionally, this is + * very close to {@link #addOption(String, Object)} except that it will use this as the default + * option if none other is explicitly selected. + * + * @param name the name of the option + * @param object the option + */ + public void setDefaultOption(String name, V object) { + requireNonNullParam(name, "name", "setDefaultOption"); + + m_defaultChoice = name; + addOption(name, object); + } + + /** + * Adds the given object to the list of options and marks it as the default. + * + * @param name the name of the option + * @param object the option + * @deprecated Use {@link #setDefaultOption(String, Object)} instead. + */ + @Deprecated + public void addDefault(String name, V object) { + setDefaultOption(name, object); + } + + /** + * Returns the selected option. If there is none selected, it will return the default. If there is + * none selected and no default, then it will return {@code null}. + * + * @return the option selected + */ + public V getSelected() { + m_mutex.lock(); + try { + if (m_selected != null) { + return m_map.get(m_selected); + } else { + return m_map.get(m_defaultChoice); + } + } finally { + m_mutex.unlock(); + } + } + + private String m_selected; + private final List m_activeEntries = new ArrayList<>(); + private final ReentrantLock m_mutex = new ReentrantLock(); + + @Override + public void initSendable(NTSendableBuilder builder) { + builder.setSmartDashboardType("String Chooser"); + builder.getEntry(INSTANCE).setDouble(m_instance); + builder.addStringProperty(DEFAULT, () -> m_defaultChoice, null); + builder.addStringArrayProperty(OPTIONS, () -> m_map.keySet().toArray(new String[0]), null); + builder.addStringProperty( + ACTIVE, + () -> { + m_mutex.lock(); + try { + if (m_selected != null) { + return m_selected; + } else { + return m_defaultChoice; + } + } finally { + m_mutex.unlock(); + } + }, + null); + m_mutex.lock(); + try { + m_activeEntries.add(builder.getEntry(ACTIVE)); + } finally { + m_mutex.unlock(); + } + builder.addStringProperty( + SELECTED, + null, + val -> { + m_mutex.lock(); + try { + m_selected = val; + m_consumer.accept(val); + for (NetworkTableEntry entry : m_activeEntries) { + entry.setString(val); + } + } finally { + m_mutex.unlock(); + } + }); + } +} diff --git a/src/main/java/frc4388/utility/PathPlannerUtil.java b/src/main/java/frc4388/utility/PathPlannerUtil.java new file mode 100644 index 0000000..09df037 --- /dev/null +++ b/src/main/java/frc4388/utility/PathPlannerUtil.java @@ -0,0 +1,63 @@ +package frc4388.utility; + +import java.io.File; +import java.io.OutputStream; +import java.io.Writer; +import java.util.Optional; + +import com.diffplug.common.base.Errors; +import com.fasterxml.jackson.annotation.JsonInclude.Include; +import com.fasterxml.jackson.databind.ObjectMapper; +import com.fasterxml.jackson.datatype.jdk8.Jdk8Module; + +import edu.wpi.first.math.geometry.Translation2d; + +public final class PathPlannerUtil { + public static final class Path { + public Optional waypoints; + public Optional maxVelocity; + public Optional maxAcceleration; + public Optional isReversed; + + private static final ObjectMapper objectMapper = new ObjectMapper(); + static { + objectMapper.registerModule(new Jdk8Module()); + objectMapper.setSerializationInclusion(Include.ALWAYS); + } + + public static Path read(File src) { + return Errors.log().getWithDefault(() -> objectMapper.readValue(src, Path.class), null); + } + + public void write(File resultFile) { + Errors.log().run(() -> objectMapper.writeValue(resultFile, this)); + } + + public void write(Writer writer) { + Errors.log().run(() -> objectMapper.writeValue(writer, this)); + } + + public static final class Waypoint { + public Optional anchorPoint; + public Optional prevControl; + public Optional nextControl; + public Optional holonomicAngle; + public Optional isReversal; + public Optional velOverride; + public Optional isLocked; + + public Waypoint() { + } + + public Waypoint(Translation2d anchorPoint, Translation2d prevControl, Translation2d nextControl, Double holonomicAngle, Boolean isReversal, Double velOverride, Boolean isLocked) { + this.anchorPoint = Optional.ofNullable(anchorPoint); + this.prevControl = Optional.ofNullable(prevControl); + this.nextControl = Optional.ofNullable(nextControl); + this.holonomicAngle = Optional.ofNullable(holonomicAngle); + this.isReversal = Optional.ofNullable(isReversal); + this.velOverride = Optional.ofNullable(velOverride); + this.isLocked = Optional.ofNullable(isLocked); + } + } + } +} diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java deleted file mode 100644 index 56390d4..0000000 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ /dev/null @@ -1,245 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.utility; - -import com.ctre.phoenix.sensors.PigeonIMU; -import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode; -import com.kauailabs.navx.frc.AHRS; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.PIDSource; -import edu.wpi.first.wpilibj.PIDSourceType; -import edu.wpi.first.wpilibj.interfaces.Gyro; - -/** - * Gyro class that allows for interchangeable use between a pigeon and a navX - */ -public class RobotGyro implements Gyro, PIDSource, Sendable { - private RobotTime m_robotTime = RobotTime.getInstance(); - - private PigeonIMU m_pigeon = null; - private AHRS m_navX = null; - public boolean m_isGyroAPigeon; // true if pigeon, false if navX - - private double m_lastPigeonAngle; - private double m_deltaPigeonAngle; - - /** - * Creates a Gyro based on a pigeon - * - * @param gyro the gyroscope to use for Gyro - */ - public RobotGyro(PigeonIMU gyro) { - m_pigeon = gyro; - m_isGyroAPigeon = true; - } - - /** - * Creates a Gyro based on a navX - * - * @param gyro the gyroscope to use for Gyro - */ - public RobotGyro(AHRS gyro) { - m_navX = gyro; - m_isGyroAPigeon = false; - } - - /** - * Run in periodic if you are using a pigeon. Updates a delta angle so that it - * can calculate getRate(). Note - * that the getRate() method for a navX will likely be much more accurate than - * for a pigeon. - */ - public void updatePigeonDeltas() { - double currentPigeonAngle = getAngle(); - m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle; - m_lastPigeonAngle = currentPigeonAngle; - } - - /** - *

- * NavX: - *

- * Calibrate the gyro by running for a number of samples and computing the - * center value. Then use - * the center value as the Accumulator center value for subsequent measurements. - * It's important to - * make sure that the robot is not moving while the centering calculations are - * in progress, this - * is typically done when the robot is first turned on while it's sitting at - * rest before the - * competition starts. - * - *

- * Pigeon: - *

- * Calibrate the gyro by collecting data at a range of tempuratures. Allow - * pigeon to cool, then boot - * into calibration mode. For faster calibration, use a heat lamp to heat up the - * pigeon. Once the pigeon - * has seen a reasonable range of tempuratures, it will exit calibration mode. - * It's important to - * make sure that the robot is not moving while the tempurature calculations are - * in progress, this - * is typically done when the robot is first turned on while it's sitting at - * rest before the - * competition starts. - */ - @Override - public void calibrate() { - if (m_isGyroAPigeon) - m_pigeon.enterCalibrationMode(CalibrationMode.Temperature); - else - m_navX.calibrate(); - } - - @Override - public void reset() { - if (m_isGyroAPigeon) - m_pigeon.setYaw(0); - else - m_navX.reset(); - } - - /** - * Get Yaw, Pitch, and Roll data. - * - * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data. - * Yaw is within [-368,640, +368,640] degrees. - * Pitch is within [-90,+90] degrees. - * Roll is within [-90,+90] degrees. - */ - private double[] getPigeonAngles() { - double[] angles = new double[3]; - m_pigeon.getYawPitchRoll(angles); - return angles; - } - - @Override - public double getAngle() { - if (m_isGyroAPigeon) { - return getPigeonAngles()[0]; - } else { - return m_navX.getAngle(); - } - } - - /** - * Gets an absolute heading of the robot - * - * @return heading from -180 to 180 degrees - */ - public double getHeading() { - return getHeading(getAngle()); - } - - /** - * Gets an absolute heading of the robot - * - * @return heading from -180 to 180 degrees - */ - public double getHeading(double angle) { - return Math.IEEEremainder(angle, 360); - } - - /** - * Returns the current pitch value (in degrees, from -90 to 90) - * reported by the sensor. Pitch is a measure of rotation around - * the Y Axis. - * - * @return The current pitch value in degrees (-90 to 90). - */ - public double getPitch() { - if (m_isGyroAPigeon) { - return MathUtil.clamp(getPigeonAngles()[1], -90, 90); - } else { - return MathUtil.clamp(m_navX.getPitch(), -90, 90); - } - } - - /** - * Returns the current roll value (in degrees, from -90 to 90) - * reported by the sensor. Roll is a measure of rotation around - * the X Axis. - * - * @return The current roll value in degrees (-90 to 90). - */ - public double getRoll() { - if (m_isGyroAPigeon) { - return MathUtil.clamp(getPigeonAngles()[2], -90, 90); - } else { - return MathUtil.clamp(m_navX.getRoll(), -90, 90); - } - } - - @Override - public double getRate() { - if (m_isGyroAPigeon) { - return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000; - } else { - return m_navX.getRate(); - } - } - - public PigeonIMU getPigeon() { - return m_pigeon; - } - - public AHRS getNavX() { - return m_navX; - } - - @Override - public void close() throws Exception { - - } - - // Begin old GyroBase class - private PIDSourceType m_pidSource = PIDSourceType.kDisplacement; - - /** - * Set which parameter of the gyro you are using as a process control variable. - * The Gyro class - * supports the rate and displacement parameters - * - * @param pidSource An enum to select the parameter. - */ - @Override - public void setPIDSourceType(PIDSourceType pidSource) { - m_pidSource = pidSource; - } - - @Override - public PIDSourceType getPIDSourceType() { - return m_pidSource; - } - - /** - * Get the output of the gyro for use with PIDControllers. May be the angle or - * rate depending on - * the set PIDSourceType - * - * @return the output according to the gyro - */ - @Override - public double pidGet() { - switch (m_pidSource) { - case kRate: - return getRate(); - case kDisplacement: - return getAngle(); - default: - return 0.0; - } - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("Gyro"); - builder.addDoubleProperty("Value", this::getAngle, null); - } -} diff --git a/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java new file mode 100644 index 0000000..e1679dd --- /dev/null +++ b/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java @@ -0,0 +1,20 @@ +package frc4388.utility.controller; + +import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; + +import edu.wpi.first.math.geometry.Translation2d; + +public class DeadbandedRawXboxController extends RawXboxController { + public DeadbandedRawXboxController(int port) { super(port); } + @Override public double getLeftX() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getX(); } + @Override public double getLeftY() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getY(); } + @Override public double getRightX() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getX(); } + @Override public double getRightY() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getY(); } + public static Translation2d skewToDeadzonedCircle(double x, double y) { + Translation2d translation2d = new Translation2d(x, y); + double magnitude = translation2d.getNorm(); + if (magnitude >= 1) return translation2d.div(magnitude); + if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); + return translation2d; + } +} diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java new file mode 100644 index 0000000..1c6fe5f --- /dev/null +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -0,0 +1,21 @@ +package frc4388.utility.controller; + +import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.XboxController; + +public class DeadbandedXboxController extends XboxController { + public DeadbandedXboxController(int port) { super(port); } + @Override public double getLeftX() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getX(); } + @Override public double getLeftY() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getY(); } + @Override public double getRightX() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getX(); } + @Override public double getRightY() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getY(); } + public static Translation2d skewToDeadzonedCircle(double x, double y) { + Translation2d translation2d = new Translation2d(x, y); + double magnitude = translation2d.getNorm(); + if (magnitude >= 1) return translation2d.div(magnitude); + if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); + return translation2d; + } +} diff --git a/src/main/java/frc4388/utility/controller/IHandController.java b/src/main/java/frc4388/utility/controller/IHandController.java deleted file mode 100644 index 319aebc..0000000 --- a/src/main/java/frc4388/utility/controller/IHandController.java +++ /dev/null @@ -1,21 +0,0 @@ -package frc4388.utility.controller; - -/** - * Add your docs here. - */ -public interface IHandController { - - public double getLeftXAxis(); - - public double getLeftYAxis(); - - public double getRightXAxis(); - - public double getRightYAxis(); - - public double getLeftTriggerAxis(); - - public double getRightTriggerAxis(); - - public int getDpadAngle(); -} diff --git a/src/main/java/frc4388/utility/controller/RawXboxController.java b/src/main/java/frc4388/utility/controller/RawXboxController.java new file mode 100644 index 0000000..7cf4ae1 --- /dev/null +++ b/src/main/java/frc4388/utility/controller/RawXboxController.java @@ -0,0 +1,241 @@ +package frc4388.utility.controller; + +import java.nio.ByteBuffer; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.XboxController; + +public class RawXboxController extends XboxController { + private final int m_port; + + public RawXboxController(int port) { + super(port); + if (port < 0 || port >= DriverStation.kJoystickPorts) + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + m_port = port; + } + + private static class HALJoystickButtons { + public int m_buttons; + public byte m_count; + } + + private static class HALJoystickAxes { + public float[] m_axes; + public short m_count; + + HALJoystickAxes(int count) { + m_axes = new float[count]; + } + } + + private static class HALJoystickPOVs { + public short[] m_povs; + public short m_count; + + HALJoystickPOVs(int count) { + m_povs = new short[count]; + for (int i = 0; i < count; i++) { + m_povs[i] = -1; + } + } + } + + private static final double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0; + private static double m_nextMessageTime; + + private HALJoystickAxes m_joystickAxes = new HALJoystickAxes(HAL.kMaxJoystickAxes); + private HALJoystickPOVs m_joystickPOVs = new HALJoystickPOVs(HAL.kMaxJoystickAxes); + private HALJoystickButtons m_joystickButtons = new HALJoystickButtons(); + + // Joystick button rising/falling edge flags + private int m_joystickButtonsPressed = 0; + private int m_joystickButtonsReleased = 0; + + private static final ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1); + + @Override + public double getRawAxis(int axis) { + return getStickAxis(axis); + } + + @Override + public boolean getRawButton(int button) { + return getStickButton((byte) button); + } + + @Override + public boolean getRawButtonPressed(int button) { + return getStickButtonPressed((byte) button); + } + + @Override + public boolean getRawButtonReleased(int button) { + return getStickButtonReleased(button); + } + + @Override + public int getPOV(int pov) { + return getStickPOV(pov); + } + + /** + * The state of one joystick button. Button indexes begin at 1. + * + * @param button The button index, beginning at 1. + * @return The state of the joystick button. + */ + public boolean getStickButton(final int button) { + if (button <= 0) { + reportJoystickUnpluggedError(); + return false; + } + if (button <= m_joystickButtons.m_count) { + return (m_joystickButtons.m_buttons & 1 << (button - 1)) != 0; + } + reportJoystickUnpluggedWarning(button, m_port); + return false; + } + + /** + * Whether one joystick button was pressed since the last check. Button indexes begin at 1. + * + * @param button The button index, beginning at 1. + * @return Whether the joystick button was pressed since the last check. + */ + public boolean getStickButtonPressed(final int button) { + getData(); + if (button <= 0) { + reportJoystickUnpluggedError(); + return false; + } + if (button <= m_joystickButtons.m_count) { + // If button was pressed, clear flag and return true + if ((m_joystickButtonsPressed & 1 << (button - 1)) != 0) { + m_joystickButtonsPressed &= ~(1 << (button - 1)); + return true; + } else { + return false; + } + } + reportJoystickUnpluggedWarning(button, m_port); + return false; + } + + /** + * Whether one joystick button was released since the last check. Button indexes begin at 1. + * + * @param button The button index, beginning at 1. + * @return Whether the joystick button was released since the last check. + */ + public boolean getStickButtonReleased(final int button) { + getData(); + if (button <= 0) { + reportJoystickUnpluggedError(); + return false; + } + if (button <= m_joystickButtons.m_count) { + // If button was released, clear flag and return true + if ((m_joystickButtonsReleased & 1 << (button - 1)) != 0) { + m_joystickButtonsReleased &= ~(1 << (button - 1)); + return true; + } else { + return false; + } + } + reportJoystickUnpluggedWarning(button, m_port); + return false; + } + + /** + * Get the value of the axis on a joystick. This depends on the mapping of the joystick connected to + * the specified port. + * + * @param axis The analog axis value to read from the joystick. + * @return The value of the axis on the joystick. + */ + public double getStickAxis(int axis) { + getData(); + if (axis < 0 || axis >= HAL.kMaxJoystickAxes) { + throw new IllegalArgumentException("Joystick axis is out of range"); + } + if (axis < m_joystickAxes.m_count) { + return m_joystickAxes.m_axes[axis]; + } + reportJoystickUnpluggedWarning(axis, m_port); + return 0.0; + } + + /** + * Get the state of a POV on the joystick. + * + * @param pov The POV to read. + * @return the angle of the POV in degrees, or -1 if the POV is not pressed. + */ + public int getStickPOV(int pov) { + getData(); + if (pov < 0 || pov >= HAL.kMaxJoystickPOVs) { + throw new IllegalArgumentException("Joystick POV is out of range"); + } + if (pov < m_joystickPOVs.m_count) { + return m_joystickPOVs.m_povs[pov]; + } + reportJoystickUnpluggedWarning(pov, m_port); + return -1; + } + + /** + * The state of the buttons on the joystick. + * + * @return The state of the buttons on the joystick. + */ + public int getStickButtons() { + getData(); + return m_joystickButtons.m_buttons; + } + + protected void getData() { + // Get the status of all of the joysticks + byte stick = (byte) m_port; + m_joystickAxes.m_count = HAL.getJoystickAxes(stick, m_joystickAxes.m_axes); + m_joystickPOVs.m_count = HAL.getJoystickPOVs(stick, m_joystickPOVs.m_povs); + m_joystickButtons.m_buttons = HAL.getJoystickButtons(stick, m_buttonCountBuffer); + m_joystickButtons.m_count = m_buttonCountBuffer.get(0); + } + + /** + * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm + * the DS. + */ + private static void reportJoystickUnpluggedError(String message) { + double currentTime = Timer.getFPGATimestamp(); + if (currentTime > m_nextMessageTime) { + DriverStation.reportError(message, false); + m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL; + } + } + + /** + * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm + * the DS. + */ + private static void reportJoystickUnpluggedWarning(String message) { + if (DriverStation.isFMSAttached() || !DriverStation.isJoystickConnectionWarningSilenced()) { + double currentTime = Timer.getFPGATimestamp(); + if (currentTime > m_nextMessageTime) { + DriverStation.reportWarning(message, false); + m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL; + } + } + } + + private static void reportJoystickUnpluggedWarning(final int pov, final int stick) { + reportJoystickUnpluggedWarning("Joystick POV " + pov + " on port " + stick + " not available, check if controller is plugged in"); + } + + private static void reportJoystickUnpluggedError() { + reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n"); + } +} diff --git a/src/main/java/frc4388/utility/controller/XboxController.java b/src/main/java/frc4388/utility/controller/XboxController.java deleted file mode 100644 index 25c496a..0000000 --- a/src/main/java/frc4388/utility/controller/XboxController.java +++ /dev/null @@ -1,218 +0,0 @@ -package frc4388.utility.controller; - -import edu.wpi.first.wpilibj.Joystick; - -/** - * This is a wrapper for the WPILib Joystick class that represents an XBox - * controller. - * @author frc1675 - */ -public class XboxController implements IHandController -{ - public static final int LEFT_X_AXIS = 0; - public static final int LEFT_Y_AXIS = 1; - public static final int LEFT_TRIGGER_AXIS = 2; - public static final int RIGHT_TRIGGER_AXIS = 3; - public static final int RIGHT_X_AXIS = 4; - public static final int RIGHT_Y_AXIS = 5; - public static final int LEFT_RIGHT_DPAD_AXIS = 6; - public static final int TOP_BOTTOM_DPAD_AXIS = 6; - - public static final int A_BUTTON = 1; - public static final int B_BUTTON = 2; - public static final int X_BUTTON = 3; - public static final int Y_BUTTON = 4; - public static final int LEFT_BUMPER_BUTTON = 5; - public static final int RIGHT_BUMPER_BUTTON = 6; - public static final int BACK_BUTTON = 7; - public static final int START_BUTTON = 8; - - public static final int LEFT_JOYSTICK_BUTTON = 9; - public static final int RIGHT_JOYSTICK_BUTTON = 10; - - private static final double LEFT_DPAD_TOLERANCE = -0.9; - private static final double RIGHT_DPAD_TOLERANCE = 0.9; - private static final double BOTTOM_DPAD_TOLERANCE = -0.9; - private static final double TOP_DPAD_TOLERANCE = 0.9; - - private static final double LEFT_TRIGGER_TOLERANCE = 0.5; - private static final double RIGHT_TRIGGER_TOLERANCE = 0.5; - - private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9; - private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9; - private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9; - private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9; - - private static final double LEFT_AXIS_UP_TOLERANCE = -0.9; - private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9; - private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9; - private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9; - - private static final double DEADZONE = 0.1; - - private Joystick m_stick; - - /** - * Add your docs here. - */ - public XboxController(int portNumber){ - m_stick = new Joystick(portNumber); - } - - /** - * Add your docs here. - */ - public Joystick getJoyStick() { - return m_stick; - } - - /** - * Checks if the input falls within the deadzone. - * @param input from an axis on the controller - * @return true if input falls in deadzone, false if input falls outside deadzone - */ - private boolean inDeadZone(double input){ - return (Math.abs(input) < DEADZONE); - } - - /** - * Updates an input to have a deadzone around the 0 position - * @param input from an axis on the controller - * @return updated input - */ - private double getAxisWithDeadZoneCheck(double input){ - if(inDeadZone(input)){ - return 0.0; - } else { - return input; - } - } - - public boolean getAButton(){ - return m_stick.getRawButton(A_BUTTON); - } - - public boolean getXButton(){ - return m_stick.getRawButton(X_BUTTON); - } - - public boolean getBButton(){ - return m_stick.getRawButton(B_BUTTON); - } - - public boolean getYButton(){ - return m_stick.getRawButton(Y_BUTTON); - } - - public boolean getBackButton(){ - return m_stick.getRawButton(BACK_BUTTON); - } - - public boolean getStartButton(){ - return m_stick.getRawButton(START_BUTTON); - } - - public boolean getLeftBumperButton(){ - return m_stick.getRawButton(LEFT_BUMPER_BUTTON); - } - - public boolean getRightBumperButton(){ - return m_stick.getRawButton(RIGHT_BUMPER_BUTTON); - } - - public boolean getLeftJoystickButton(){ - return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON); - } - - public boolean getRightJoystickButton(){ - return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON); - } - - public double getLeftXAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS)); - } - - public double getLeftYAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS)); - } - - public double getRightXAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS)); - } - - public double getRightYAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS)); - } - - public double getLeftTriggerAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS)); - } - - public double getRightTriggerAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS)); - } - - /** - * Get the angle input from the dpad. - * @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc. - */ - public int getDpadAngle() { - return m_stick.getPOV(0); - } - - public boolean getDPadLeft(){ - return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE); - } - - public boolean getDPadRight(){ - return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE); - } - - public boolean getDPadTop(){ - return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE); - } - - public boolean getDPadBottom(){ - return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE); - } - - public boolean getLeftTrigger(){ - return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE); - } - - public boolean getRightTrigger(){ - return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE); - } - - public boolean getRightAxisUpTrigger(){ - return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE); - } - - public boolean getRightAxisDownTrigger(){ - return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE); - } - - public boolean getRightAxisLeftTrigger(){ - return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE); - } - - public boolean getRightAxisRightTrigger(){ - return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE); - } - - public boolean getLeftAxisUpTrigger(){ - return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE); - } - - public boolean getLeftAxisDownTrigger(){ - return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE); - } - - public boolean getLeftAxisLeftTrigger(){ - return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE); - } - - public boolean getLeftAxisRightTrigger(){ - return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE); - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java deleted file mode 100644 index cecf379..0000000 --- a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java +++ /dev/null @@ -1,50 +0,0 @@ -package frc4388.utility.controller; - -import edu.wpi.first.wpilibj2.command.button.Button; - -/** - * Mapping for the Xbox controller triggers to allow triggers to be defined as - * buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger - * exceeds a tolerance defined in {@link XboxController}. - */ -public class XboxTriggerButton extends Button { - public static final int RIGHT_TRIGGER = 0; - public static final int LEFT_TRIGGER = 1; - public static final int RIGHT_AXIS_UP_TRIGGER = 2; - public static final int RIGHT_AXIS_DOWN_TRIGGER = 3; - public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4; - public static final int RIGHT_AXIS_LEFT_TRIGGER = 5; - public static final int LEFT_AXIS_UP_TRIGGER = 6; - public static final int LEFT_AXIS_DOWN_TRIGGER = 7; - public static final int LEFT_AXIS_RIGHT_TRIGGER = 8; - public static final int LEFT_AXIS_LEFT_TRIGGER = 9; - - private XboxController m_controller; - private int m_trigger; - - /** - * Creates a Trigger-Button mapped to a specific Xbox controller and trigger - */ - public XboxTriggerButton(XboxController controller, int trigger) { - m_controller = controller; - m_trigger = trigger; - } - - /** {@inheritDoc} */ - @Override - public boolean get() { - switch (m_trigger) { - case RIGHT_TRIGGER: return m_controller.getRightTrigger(); - case LEFT_TRIGGER: return m_controller.getLeftTrigger(); - case RIGHT_AXIS_UP_TRIGGER: return m_controller.getRightAxisUpTrigger(); - case RIGHT_AXIS_DOWN_TRIGGER: return m_controller.getRightAxisDownTrigger(); - case RIGHT_AXIS_RIGHT_TRIGGER: return m_controller.getRightAxisRightTrigger(); - case RIGHT_AXIS_LEFT_TRIGGER: return m_controller.getRightAxisLeftTrigger(); - case LEFT_AXIS_UP_TRIGGER: return m_controller.getLeftAxisUpTrigger(); - case LEFT_AXIS_DOWN_TRIGGER: return m_controller.getLeftAxisDownTrigger(); - case LEFT_AXIS_RIGHT_TRIGGER: return m_controller.getLeftAxisRightTrigger(); - case LEFT_AXIS_LEFT_TRIGGER: return m_controller.getLeftAxisLeftTrigger(); - default: return false; - } - } -} diff --git a/src/test/java/frc4388/commands/AimToCenterTest.java b/src/test/java/frc4388/commands/AimToCenterTest.java new file mode 100644 index 0000000..72a84f4 --- /dev/null +++ b/src/test/java/frc4388/commands/AimToCenterTest.java @@ -0,0 +1,113 @@ +package frc4388.commands; + +import org.junit.Test; + +import frc4388.robot.commands.AimToCenter; +import org.junit.Assert; + +public class AimToCenterTest { + + private static final double DELTA = 1e-15; + + @Test + public void givenAngle_whenTestIfDeadzone_thenReturnIfInDeadzone() { + boolean output; + + //20 deg + output = AimToCenter.isHardwareDeadzone(20.); + Assert.assertFalse(output); + + //-10 deg + output = AimToCenter.isHardwareDeadzone(-10.); + Assert.assertTrue(output); + + //-1 deg + output = AimToCenter.isHardwareDeadzone(-1.); + Assert.assertTrue(output); + + //341 deg + output = AimToCenter.isHardwareDeadzone(341.); + Assert.assertTrue(output); + + //340 deg + output = AimToCenter.isHardwareDeadzone(340.); + Assert.assertFalse(output); + + //0 deg + output = AimToCenter.isHardwareDeadzone(0.); + Assert.assertFalse(output); + + //200 deg + output = AimToCenter.isHardwareDeadzone(200.); + Assert.assertFalse(output); + + //2000000 deg + output = AimToCenter.isHardwareDeadzone(2000000.); + Assert.assertTrue(output); + + //NaN deg + output = AimToCenter.isHardwareDeadzone(Double.NaN); + Assert.assertFalse(output); + } + + @Test + public void givenOdometry_whenCalculateAngleToCenter_thenReturnAngleToCenter() { + double actual; + double expected; + + //(5,5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(5., 5., 0.); + expected = 180. + 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(-5,5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(-5.0, 5., 0.); + expected = 180. + 90. + 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(-5,-5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(-5.0, -5., 0.); + expected = 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(5,-5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(5., -5., 0.); + expected = 90. + 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(0,0) Gyro = 0 deg + actual = AimToCenter.angleToCenter(0., 0., 0.); + Assert.assertNotNull(actual); + + //(5,5) Gyro = 180 deg + actual = AimToCenter.angleToCenter(5., 5., 180.); + expected = 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(100,100) Gyro = 90 deg + actual = AimToCenter.angleToCenter(100., 100., 90.); + expected = 90. + 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(null,5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(Double.NaN, 5., 0.); + expected = Double.NaN; + Assert.assertEquals(expected, actual, DELTA); + + //(null,null) Gyro = 0 deg + actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, 0.); + expected = Double.NaN; + Assert.assertEquals(expected, actual, DELTA); + + //(null,null) Gyro = null deg + actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, Double.NaN); + expected = Double.NaN; + Assert.assertEquals(expected, actual, DELTA); + + //(5,5) Gyro = -20 deg + actual = AimToCenter.angleToCenter(5., -5., -45.); + expected = 180.; + Assert.assertEquals(expected, actual, DELTA); + + } +} diff --git a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java index 87ab85c..a0f6b97 100644 --- a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java +++ b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java @@ -5,6 +5,7 @@ package frc4388.robot.subsystems; import static org.junit.Assert.assertEquals; +// import static org.mockito.Mockito.mock; import static org.mockito.Mockito.mock; import org.junit.Test; @@ -17,40 +18,43 @@ import frc4388.utility.LEDPatterns; * Add your docs here. */ public class LEDSubsystemTest { + // Arrange + Spark ledController = mock(Spark.class); + LED led = new LED(ledController); @Test public void testConstructor() { // Arrange - Spark ledController = mock(Spark.class); + // Spark ledController = mock(Spark.class); // Act - LED led = new LED(ledController); + // LED led = new LED(ledController); // Assert - assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001); + // assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001); } @Test public void testPatterns() { // Arrange - Spark ledController = mock(Spark.class); - LED led = new LED(ledController); + // Spark ledController = mock(Spark.class); + // LED led = new LED(ledController); // Act - led.setPattern(LEDPatterns.RAINBOW_RAINBOW); + // led.setPattern(LEDPatterns.RAINBOW_RAINBOW); // Assert - assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001); + // assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001); // Act - led.setPattern(LEDPatterns.BLUE_BREATH); + // led.setPattern(LEDPatterns.BLUE_BREATH); // Assert - assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001); + // assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001); // Act - led.setPattern(LEDPatterns.SOLID_BLACK); + // led.setPattern(LEDPatterns.SOLID_BLACK); // Assert - assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001); + // assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001); } } diff --git a/src/test/java/frc4388/robot/subsystems/XboxControllerTest.java b/src/test/java/frc4388/robot/subsystems/XboxControllerTest.java new file mode 100644 index 0000000..8298fbf --- /dev/null +++ b/src/test/java/frc4388/robot/subsystems/XboxControllerTest.java @@ -0,0 +1,5 @@ +package frc4388.robot.subsystems; + +public class XboxControllerTest { + +} diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 0000000..f88313a --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,33 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2022.1.0", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2022.1.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2022.1.0", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "osxx86-64", + "linuxathena" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 997e2a4..fb19ccf 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -70,4 +70,4 @@ ] } ] -} \ No newline at end of file +} diff --git a/vendordeps/WPILibOldCommands.json b/vendordeps/WPILibOldCommands.json deleted file mode 100644 index acc8879..0000000 --- a/vendordeps/WPILibOldCommands.json +++ /dev/null @@ -1,37 +0,0 @@ -{ - "fileName": "WPILibOldCommands.json", - "name": "WPILib-Old-Commands", - "version": "2020.0.0", - "uuid": "b066afc2-5c18-43c4-b758-43381fcb275e", - "mavenUrls": [], - "jsonUrl": "", - "javaDependencies": [ - { - "groupId": "edu.wpi.first.wpilibOldCommands", - "artifactId": "wpilibOldCommands-java", - "version": "wpilib" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "edu.wpi.first.wpilibOldCommands", - "artifactId": "wpilibOldCommands-cpp", - "version": "wpilib", - "libName": "wpilibOldCommands", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxraspbian", - "linuxaarch64bionic", - "windowsx86-64", - "windowsx86", - "linuxx86-64", - "osxx86-64" - ] - } - ] -} \ No newline at end of file