mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-08 16:28:07 -06:00
Merge branch 'full-robot-test' into Intake
This commit is contained in:
66945
+3
-2
@@ -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
|
||||
|
||||
Vendored
+2
-2
@@ -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"
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
Vendored
+4
-1
@@ -26,5 +26,8 @@
|
||||
},
|
||||
],
|
||||
"java.test.defaultConfig": "WPIlibUnitTests",
|
||||
"java.dependency.packagePresentation": "flat"
|
||||
"java.dependency.packagePresentation": "hierarchical",
|
||||
"files.associations": {
|
||||
"*.path": "json"
|
||||
}
|
||||
}
|
||||
|
||||
Vendored
+34
@@ -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
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -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"
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
build/
|
||||
bin/
|
||||
+12
@@ -0,0 +1,12 @@
|
||||
{
|
||||
"version": "0.2.0",
|
||||
"configurations": [
|
||||
{
|
||||
"type": "java",
|
||||
"name": "Launch NetworkTablesDesktopClient",
|
||||
"request": "launch",
|
||||
"mainClass": "NetworkTablesDesktopClient",
|
||||
"projectName": "NetworkTablesDesktopClient"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -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"
|
||||
}
|
||||
Binary file not shown.
@@ -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
|
||||
+234
@@ -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" "$@"
|
||||
+89
@@ -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
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
+9
-3
@@ -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)
|
||||
|
||||
@@ -0,0 +1,3 @@
|
||||
Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds)
|
||||
0 ,16 ,12000
|
||||
999 ,28.8 ,12000
|
||||
|
@@ -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.
|
||||
@@ -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}
|
||||
@@ -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}]}
|
||||
@@ -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}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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}
|
||||
@@ -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}
|
||||
@@ -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}]}
|
||||
@@ -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}
|
||||
@@ -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}
|
||||
@@ -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}]}
|
||||
@@ -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}]}
|
||||
@@ -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}
|
||||
@@ -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}]}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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 {
|
||||
* <p>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
|
||||
@@ -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.
|
||||
*
|
||||
* <p>This runs after the mode specific periodic functions, but before
|
||||
* <p>
|
||||
* 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();
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -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<File> autoChooser = new ListeningSendableChooser<>(this::loadPath);
|
||||
private final List<Waypoint> 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<CharSequence, String> PATH_EXTENSION_REMOVER = ((Function<CharSequence, Matcher>) 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<WatchEvent<?>> watchEvents = watchKey.pollEvents();
|
||||
if (!watchEvents.isEmpty()) {
|
||||
List<WatchEvent<?>> 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<Translation2d, Translation2d> 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));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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<ShooterTableEntry> 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 <E> Number linearInterpolate(final E[] table, final Number lookupValue, final Function<E, Number> lookupGetter, final Function<E, Number> targetGetter) {
|
||||
final Map.Entry<Integer, E> 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 <E> Optional<Map.Entry<Integer, E>> lookup(final E[] table, final Number value, final Function<E, Number> valueGetter, final boolean exactMatch) {
|
||||
final Optional<Map.Entry<Integer, E>> 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));
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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());
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 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)
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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<Integer, String> 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);
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -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<R> {
|
||||
private static final Pattern SANITIZER = Pattern.compile("[^$\\w,]");
|
||||
|
||||
private final Supplier<R> generator;
|
||||
private final IntFunction<R[]> arrayGenerator;
|
||||
private final Map<String, BiConsumer<R, String>> 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<R> generator) {
|
||||
final Class<?> clazz = generator.get().getClass();
|
||||
final Map<Class<?>, Function<String, ?>> 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<String, ?> 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<R, String>[] fieldSetters = Stream.of(headerSanitizer(reader.readLine()).split(",")).map(this::nameProcessor).map(setters::get).toArray(BiConsumer[]::new);
|
||||
final Stream<String> 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<String, ?> 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> R deserializeRecordString(final String recordString, final BiConsumer<R, String>[] 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<R, String> 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 <T> 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<List<ReflectionTable>> 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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<DummySensor> instances = new ArrayList<DummySensor>();
|
||||
|
||||
/**
|
||||
* 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;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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}.
|
||||
*
|
||||
* <p>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 <V> The type of the values to be stored
|
||||
*/
|
||||
public class ListeningSendableChooser<V> 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<String, V> m_map = new LinkedHashMap<>();
|
||||
|
||||
private String m_defaultChoice = "";
|
||||
private final int m_instance;
|
||||
private static final AtomicInteger s_instances = new AtomicInteger();
|
||||
|
||||
private Consumer<String> m_consumer;
|
||||
|
||||
/** Instantiates a {@link ListeningSendableChooser}. */
|
||||
public ListeningSendableChooser(Consumer<String> 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<NetworkTableEntry> 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();
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -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<Waypoint[]> waypoints;
|
||||
public Optional<Double> maxVelocity;
|
||||
public Optional<Double> maxAcceleration;
|
||||
public Optional<Boolean> 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<Translation2d> anchorPoint;
|
||||
public Optional<Translation2d> prevControl;
|
||||
public Optional<Translation2d> nextControl;
|
||||
public Optional<Double> holonomicAngle;
|
||||
public Optional<Boolean> isReversal;
|
||||
public Optional<Double> velOverride;
|
||||
public Optional<Boolean> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
/**
|
||||
* <p>
|
||||
* NavX:
|
||||
* <p>
|
||||
* 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.
|
||||
*
|
||||
* <p>
|
||||
* Pigeon:
|
||||
* <p>
|
||||
* 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);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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();
|
||||
}
|
||||
@@ -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");
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,5 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
public class XboxControllerTest {
|
||||
|
||||
}
|
||||
@@ -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"
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -70,4 +70,4 @@
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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"
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
Reference in New Issue
Block a user