diff --git a/.gitignore b/.gitignore index 983678a..9535c83 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ -# Created by https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. ### C++ ### # Prerequisites @@ -151,11 +152,11 @@ gradle-app.setting # gradle/wrapper/gradle-wrapper.properties # # VS Code Specific Java Settings +# DO NOT REMOVE .classpath and .project .classpath .project .settings/ bin/ -imgui.ini - -# End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode +# Simulation GUI and other tools window save file +*-window.json diff --git a/.templates/SubsystemTest.java b/.templates/SubsystemTest.java index 51f30d5..5755087 100644 --- a/.templates/SubsystemTest.java +++ b/.templates/SubsystemTest.java @@ -1,9 +1,6 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ +// 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; diff --git a/.templates/UtilityTest.java b/.templates/UtilityTest.java index 035e580..c95cf29 100644 --- a/.templates/UtilityTest.java +++ b/.templates/UtilityTest.java @@ -1,9 +1,6 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ +// 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; diff --git a/.vscode/settings.json b/.vscode/settings.json index 5200b5c..4ed293b 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,5 +1,6 @@ { "java.configuration.updateBuildConfiguration": "automatic", + "java.server.launchMode": "Standard", "files.exclude": { "**/.git": true, "**/.svn": true, @@ -10,6 +11,19 @@ "**/.classpath": true, "**/.project": true, "**/.settings": true, - "**/.factorypath": true - } + "**/.factorypath": true, + "**/*~": true + }, + "java.test.config": [ + { + "name": "WPIlibUnitTests", + "workingDirectory": "${workspaceFolder}/build/jni/release", + "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], + "env": { + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } + }, + ], + "java.test.defaultConfig": "WPIlibUnitTests" } diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 0b623f1..dea47be 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2021", + "projectYear": "2022", "teamNumber": 4388 } \ No newline at end of file diff --git a/LICENSE b/LICENSE deleted file mode 100644 index c714e13..0000000 --- a/LICENSE +++ /dev/null @@ -1,24 +0,0 @@ -Copyright (c) 2009-2018 FIRST -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of the FIRST nor the - names of its contributors may be used to endorse or promote products - derived from this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS``AS IS'' AND ANY -EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. \ No newline at end of file diff --git a/README.md b/README.md deleted file mode 100644 index 3e57656..0000000 --- a/README.md +++ /dev/null @@ -1,2 +0,0 @@ -# 2022 No Way Home -[![Java CI](https://github.com/Team4388/2022NoWayHome/actions/workflows/gradle.yml/badge.svg)](https://github.com/Team4388/2022NoWayHome/actions/workflows/gradle.yml) diff --git a/WPILib-License.md b/WPILib-License.md new file mode 100644 index 0000000..3d5a824 --- /dev/null +++ b/WPILib-License.md @@ -0,0 +1,24 @@ +Copyright (c) 2009-2021 FIRST and other WPILib contributors +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of FIRST, WPILib, nor the names of other WPILib + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/build.gradle b/build.gradle index b0a450c..c1f07ee 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,8 @@ +import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO + plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2021.3.1" + id "edu.wpi.first.GradleRIO" version "2022.1.1" } sourceCompatibility = JavaVersion.VERSION_11 @@ -9,67 +11,69 @@ targetCompatibility = JavaVersion.VERSION_11 def ROBOT_MAIN_CLASS = "frc4388.robot.Main" // Define my targets (RoboRIO) and artifacts (deployable files) -// This is added by GradleRIO's backing project EmbeddedTools. +// This is added by GradleRIO's backing project DeployUtils. deploy { targets { - roboRIO("roborio") { + roborio(getTargetTypeClass('RoboRIO')) { // Team number is loaded either from the .wpilib/wpilib_preferences.json // or from command line. If not found an exception will be thrown. // You can use getTeamOrDefault(team) instead of getTeamNumber if you // want to store a team number in this file. - team = frc.getTeamNumber() - } - } - artifacts { - frcJavaArtifact('frcJava') { - targets << "roborio" - // Debug can be overridden by command line, for use with VSCode - debug = frc.getDebugOrDefault(false) - } - // Built in artifact to deploy arbitrary files to the roboRIO. - fileTreeArtifact('frcStaticFileDeploy') { - // The directory below is the local directory to deploy - files = fileTree(dir: 'src/main/deploy') - // Deploy to RoboRIO target, into /home/lvuser/deploy - targets << "roborio" - directory = '/home/lvuser/deploy' + 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')) { + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + } + } } } } +def deployArtifact = deploy.targets.roborio.artifacts.frcJava + +// Set to true to use debug for JNI. +wpi.java.debugJni = false + // Set this to true to enable desktop support. def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 4. dependencies { - implementation wpi.deps.wpilib() - nativeZip wpi.deps.wpilibJni(wpi.platforms.roborio) - nativeDesktopZip wpi.deps.wpilibJni(wpi.platforms.desktop) + implementation wpi.java.deps.wpilib() + implementation wpi.java.vendor.java() + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) + roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) - implementation wpi.deps.vendor.java() - nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio) - nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop) + roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + + nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) + nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) + simulationDebug wpi.sim.enableDebug() + + nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) + nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) + simulationRelease wpi.sim.enableRelease() testImplementation 'junit:junit:4.12' - testCompile "org.mockito:mockito-core:2.+" - - // Enable simulation gui support. Must check the box in vscode to enable support - // upon debugging - simulation wpi.deps.sim.gui(wpi.platforms.desktop, false) - simulation wpi.deps.sim.driverstation(wpi.platforms.desktop, false) - - // Websocket extensions require additional configuration. - // simulation wpi.deps.sim.ws_server(wpi.platforms.desktop, false) - // simulation wpi.deps.sim.ws_client(wpi.platforms.desktop, false) + testImplementation "org.mockito:mockito-core:2.+" } // Simulation configuration (e.g. environment variables). -sim { - // Sets the websocket client remote host. - // envVar "HALSIMWS_HOST", "10.0.0.2" -} +wpi.sim.addGui().defaultEnabled = true +wpi.sim.addDriverstation() // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') // in order to make them all available at runtime. Also adding the manifest so WPILib @@ -77,4 +81,10 @@ sim { jar { from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) + duplicatesStrategy = DuplicatesStrategy.INCLUDE } + +// Configure jar and deploy tasks +deployArtifact.jarTask = jar +wpi.java.configureExecutableTasks(jar) +wpi.java.configureTestTasks(test) diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar index cc4fdc2..7454180 100644 Binary files a/gradle/wrapper/gradle-wrapper.jar and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index d050f17..f959987 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,5 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-6.0.1-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-7.3.3-bin.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=permwrapper/dists diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 index 2fe81a7..c53aefa --- a/gradlew +++ b/gradlew @@ -1,7 +1,7 @@ -#!/usr/bin/env sh +#!/bin/sh # -# Copyright 2015 the original author or authors. +# 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. @@ -17,78 +17,113 @@ # ############################################################################## -## -## Gradle start up script for UN*X -## +# +# 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 -PRG="$0" -# Need this for relative symlinks. -while [ -h "$PRG" ] ; do - ls=`ls -ld "$PRG"` - link=`expr "$ls" : '.*-> \(.*\)$'` - if expr "$link" : '/.*' > /dev/null; then - PRG="$link" - else - PRG=`dirname "$PRG"`"/$link" - fi +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 -SAVED="`pwd`" -cd "`dirname \"$PRG\"`/" >/dev/null -APP_HOME="`pwd -P`" -cd "$SAVED" >/dev/null + +APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit APP_NAME="Gradle" -APP_BASE_NAME=`basename "$0"` +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" +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 - ;; - MINGW* ) - msys=true - ;; - NONSTOP* ) - nonstop=true - ;; +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" + JAVACMD=$JAVA_HOME/jre/sh/java else - JAVACMD="$JAVA_HOME/bin/java" + JAVACMD=$JAVA_HOME/bin/java fi if [ ! -x "$JAVACMD" ] ; then die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME @@ -97,7 +132,7 @@ Please set the JAVA_HOME variable in your environment to match the location of your Java installation." fi else - JAVACMD="java" + 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 @@ -105,79 +140,95 @@ location of your Java installation." fi # Increase the maximum file descriptors if we can. -if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then - MAX_FD_LIMIT=`ulimit -H -n` - if [ $? -eq 0 ] ; then - if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then - MAX_FD="$MAX_FD_LIMIT" - fi - ulimit -n $MAX_FD - if [ $? -ne 0 ] ; then - warn "Could not set maximum file descriptor limit: $MAX_FD" - fi - else - warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT" - fi -fi - -# For Darwin, add options to specify how the application appears in the dock -if $darwin; then - GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\"" -fi - -# For Cygwin or MSYS, switch paths to Windows format before running java -if [ "$cygwin" = "true" -o "$msys" = "true" ] ; then - APP_HOME=`cygpath --path --mixed "$APP_HOME"` - CLASSPATH=`cygpath --path --mixed "$CLASSPATH"` - JAVACMD=`cygpath --unix "$JAVACMD"` - - # We build the pattern for arguments to be converted via cygpath - ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null` - SEP="" - for dir in $ROOTDIRSRAW ; do - ROOTDIRS="$ROOTDIRS$SEP$dir" - SEP="|" - done - OURCYGPATTERN="(^($ROOTDIRS))" - # Add a user-defined pattern to the cygpath arguments - if [ "$GRADLE_CYGPATTERN" != "" ] ; then - OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)" - fi - # Now convert the arguments - kludge to limit ourselves to /bin/sh - i=0 - for arg in "$@" ; do - CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -` - CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option - - if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition - eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"` - else - eval `echo args$i`="\"$arg\"" - fi - i=`expr $i + 1` - done - case $i in - 0) set -- ;; - 1) set -- "$args0" ;; - 2) set -- "$args0" "$args1" ;; - 3) set -- "$args0" "$args1" "$args2" ;; - 4) set -- "$args0" "$args1" "$args2" "$args3" ;; - 5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;; - 6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;; - 7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;; - 8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;; - 9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;; +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 -# Escape application args -save () { - for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done - echo " " -} -APP_ARGS=`save "$@"` +# 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. -# Collect all arguments for the java command, following the shell quoting and substitution rules -eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS" +# For Cygwin or MSYS, switch paths to Windows format before running java +if "$cygwin" || "$msys" ; then + APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) + CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) + + JAVACMD=$( cygpath --unix "$JAVACMD" ) + + # Now convert the arguments - kludge to limit ourselves to /bin/sh + for arg do + if + case $arg in #( + -*) false ;; # don't mess with options #( + /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath + [ -e "$t" ] ;; #( + *) false ;; + esac + then + arg=$( cygpath --path --ignore --mixed "$arg" ) + fi + # Roll the args list around exactly as many times as the number of + # args, so each arg winds up back in the position where it started, but + # possibly modified. + # + # NB: a `for` loop captures its iteration list before it begins, so + # changing the positional parameters here affects neither the number of + # iterations, nor the values presented in `arg`. + shift # remove old arg + set -- "$@" "$arg" # push replacement arg + done +fi + +# Collect all arguments for the java command; +# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of +# shell script including quotes and variable substitutions, so put them in +# double quotes to make sure that they get re-expanded; and +# * put everything else in single quotes, so that it's not re-expanded. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Use "xargs" to parse quoted args. +# +# With -n1 it outputs one arg per line, with the quotes and backslashes removed. +# +# In Bash we could simply go: +# +# readarray ARGS < <( xargs -n1 <<<"$var" ) && +# set -- "${ARGS[@]}" "$@" +# +# but POSIX shell has neither arrays nor command substitution, so instead we +# post-process each arg (as a line of input to sed) to backslash-escape any +# character that might be a shell metacharacter, then use eval to reverse +# that process (while maintaining the separation between arguments), and wrap +# the whole thing up as a single "set" statement. +# +# This will of course break if any of these variables contains a newline or +# an unmatched quote. +# + +eval "set -- $( + printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | + xargs -n1 | + sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | + tr '\n' ' ' + )" '"$@"' exec "$JAVACMD" "$@" diff --git a/gradlew.bat b/gradlew.bat index 9618d8d..107acd3 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -29,6 +29,9 @@ 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" @@ -37,7 +40,7 @@ if defined JAVA_HOME goto findJavaFromJavaHome set JAVA_EXE=java.exe %JAVA_EXE% -version >NUL 2>&1 -if "%ERRORLEVEL%" == "0" goto init +if "%ERRORLEVEL%" == "0" goto execute echo. echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. @@ -51,7 +54,7 @@ goto fail set JAVA_HOME=%JAVA_HOME:"=% set JAVA_EXE=%JAVA_HOME%/bin/java.exe -if exist "%JAVA_EXE%" goto init +if exist "%JAVA_EXE%" goto execute echo. echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% @@ -61,28 +64,14 @@ echo location of your Java installation. goto fail -:init -@rem Get command-line arguments, handling Windows variants - -if not "%OS%" == "Windows_NT" goto win9xME_args - -:win9xME_args -@rem Slurp the command line arguments. -set CMD_LINE_ARGS= -set _SKIP=2 - -:win9xME_args_slurp -if "x%~1" == "x" goto execute - -set CMD_LINE_ARGS=%* - :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 %CMD_LINE_ARGS% +"%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 diff --git a/settings.gradle b/settings.gradle index 0bc697a..c363694 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2021' + String frcYear = '2022' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 8258835..b6daa92 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -1,9 +1,6 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ +// 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; @@ -19,62 +16,51 @@ import frc4388.utility.LEDPatterns; * constants are needed, to reduce verbosity. */ public final class Constants { - public static final class ArcadeDriveConstants { - public static final int DRIVE_LEFT_FRONT_CAN_ID = 2; - public static final int DRIVE_RIGHT_FRONT_CAN_ID = 4; - public static final int DRIVE_LEFT_BACK_CAN_ID = 3; - public static final int DRIVE_RIGHT_BACK_CAN_ID = 5; - - public static final int DRIVE_PIGEON_ID = 6; + public static final class SwerveDriveConstants { + public static final double ROTATION_SPEED = 0.1; + 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 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; + public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; + public static final int LEFT_BACK_STEER_CAN_ID = 6; + public static final int LEFT_BACK_WHEEL_CAN_ID = 7; + public static final int RIGHT_BACK_STEER_CAN_ID = 8; + public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; + public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; + 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 + public static final float LEFT_FRONT_ENCODER_OFFSET = 0; + public static final float RIGHT_FRONT_ENCODER_OFFSET = 0; + public static final float LEFT_BACK_ENCODER_OFFSET = 0; + public static final float RIGHT_BACK_ENCODER_OFFSET = 0; - public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; - } + // 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 class SwerveDriveConstants { - public static final double ROTATION_SPEED = 0.1; - 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 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; - public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; - public static final int LEFT_BACK_STEER_CAN_ID = 6; - public static final int LEFT_BACK_WHEEL_CAN_ID = 7; - public static final int RIGHT_BACK_STEER_CAN_ID = 8; - public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; - public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; - 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 - public static final float LEFT_FRONT_ENCODER_OFFSET = 0; - public static final float RIGHT_FRONT_ENCODER_OFFSET = 0; - public static final float LEFT_BACK_ENCODER_OFFSET = 0; - public static final float RIGHT_BACK_ENCODER_OFFSET = 0; + // 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; + } + public static final class LEDConstants { + public static final int LED_SPARK_ID = 0; - // 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 LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; + } - // 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; - } - public static final class LEDConstants { - public static final int LED_SPARK_ID = 0; - - public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; - } - - public static final class OIConstants { - public static final int XBOX_DRIVER_ID = 0; - public static final int XBOX_OPERATOR_ID = 1; - } + public static final class OIConstants { + public static final int XBOX_DRIVER_ID = 0; + public static final int XBOX_OPERATOR_ID = 1; + } } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index e145b38..b8962ad 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -75,12 +75,16 @@ public class Robot extends TimedRobot { public void 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; } - */ + /*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) { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c697a7f..7b69652 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -1,9 +1,6 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ +// 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; @@ -13,7 +10,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; -import frc4388.robot.subsystems.ArcadeDrive; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.LEDPatterns; @@ -28,105 +24,94 @@ import frc4388.utility.controller.XboxController; * commands, and button mappings) should be declared here. */ public class RobotContainer { - /* RobotMap */ - private final RobotMap m_robotMap = new RobotMap(); + /* RobotMap */ + private final RobotMap m_robotMap = new RobotMap(); - /* Subsystems */ - //private final ArcadeDrive m_robotArcadeDrive = new ArcadeDrive(m_robotMap.leftFrontMotor, m_robotMap.rightFrontMotor, - // m_robotMap.leftBackMotor, m_robotMap.rightBackMotor, m_robotMap.driveTrain, m_robotMap.gyroDrive); + /* 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 + ); - 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 - ); + private final LED m_robotLED = new LED(m_robotMap.LEDController); - private final LED m_robotLED = new LED(m_robotMap.LEDController); + /* Controllers */ + private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); + private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); - /* Controllers */ - private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); - private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + configureButtonBindings(); - /** - * 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)); - /* Default Commands */ - // drives the arcade drive with a two-axis input from the driver controller - /*m_robotArcadeDrive.setDefaultCommand( - new RunCommand(() -> m_robotArcadeDrive.driveWithInput(getDriverController().getLeftYAxis(), - getDriverController().getRightXAxis()), m_robotArcadeDrive));*/ + // continually sends updates to the Blinkin LED controller to keep the lights on + m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); + } - // 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)); + /** + * Use this method to define your button->command mappings. Buttons can be + * created by instantiating a {@link GenericHID} or one of its subclasses + * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then + * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + /* Driver Buttons */ - // continually sends updates to the Blinkin LED controller to keep the lights on - m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - } + /* 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)); + } - /** - * Use this method to define your button->command mappings. Buttons can be - * created by instantiating a {@link GenericHID} or one of its subclasses - * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then - * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - /* Driver Buttons */ - // test command to spin the robot while pressing A on the driver controller - //new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - //.whileHeld(() -> m_robotArcadeDrive.driveWithInput(0, 1)); + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + // no auto + return new InstantCommand(); + } - /* 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)); - } + /** + * Add your docs here. + */ + public IHandController getDriverController() { + return m_driverXbox; + } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - // no auto - return new InstantCommand(); - } + /** + * Add your docs here. + */ + public IHandController getOperatorController() { + return m_operatorXbox; + } - /** - * Add your docs here. - */ - public IHandController getDriverController() { - return m_driverXbox; - } + /** + * Add your docs here. + */ + public Joystick getOperatorJoystick() { + return m_operatorXbox.getJoyStick(); + } - /** - * Add your docs here. - */ - public IHandController getOperatorController() { - return m_operatorXbox; - } - - /** - * Add your docs here. - */ - public Joystick getOperatorJoystick() { - return m_operatorXbox.getJoyStick(); - } - - /** - * Add your docs here. - */ - public Joystick getDriverJoystick() { - return m_driverXbox.getJoyStick(); - } + /** + * Add your docs here. + */ + public Joystick getDriverJoystick() { + return m_driverXbox.getJoyStick(); + } } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index d4e533f..843323c 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -1,26 +1,15 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ +// 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; -import com.ctre.phoenix.motorcontrol.InvertType; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.sensors.PigeonIMU; -import com.ctre.phoenix.sensors.AbsoluteSensorRange; import com.ctre.phoenix.sensors.CANCoder; -import edu.wpi.first.wpilibj.Spark; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import frc4388.robot.Constants.ArcadeDriveConstants; +import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.utility.RobotGyro; /** * Defines and holds all I/O objects on the Roborio. This is useful for unit @@ -28,112 +17,79 @@ import frc4388.utility.RobotGyro; */ public class RobotMap { - public RobotMap() { - configureLEDMotorControllers(); - //configureArcadeDriveMotorControllers(); - configureSwerveMotorControllers(); - } + public RobotMap() { + configureLEDMotorControllers(); + configureSwerveMotorControllers(); + } - /* LED Subsystem */ - public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + /* LED Subsystem */ + public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); - void configureLEDMotorControllers() { - - } + void configureLEDMotorControllers() { - /* ArcadeDrive Subsystem */ - //public final WPI_TalonFX leftFrontMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_LEFT_FRONT_CAN_ID); - //public final WPI_TalonFX rightFrontMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); - //public final WPI_TalonFX leftBackMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_LEFT_BACK_CAN_ID); - //public final WPI_TalonFX rightBackMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_RIGHT_BACK_CAN_ID); - //public final DifferentialDrive driveTrain = new DifferentialDrive(leftFrontMotor, rightFrontMotor); - //public final RobotGyro gyroDrive = new RobotGyro(new PigeonIMU(ArcadeDriveConstants.DRIVE_PIGEON_ID)); + } + + /* 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); + public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID); + public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID); + public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); + public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); + public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); + public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); + 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); + + 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); - /*void configureArcadeDriveMotorControllers() { + leftFrontSteerMotor.configFactoryDefault(); + leftFrontWheelMotor.configFactoryDefault(); + rightFrontSteerMotor.configFactoryDefault(); + rightFrontWheelMotor.configFactoryDefault(); + leftBackSteerMotor.configFactoryDefault(); + leftBackWheelMotor.configFactoryDefault(); + rightBackSteerMotor.configFactoryDefault(); + rightBackWheelMotor.configFactoryDefault(); - // factory default values - leftFrontMotor.configFactoryDefault(); - rightFrontMotor.configFactoryDefault(); - leftBackMotor.configFactoryDefault(); - rightBackMotor.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); - // set back motors as followers - leftBackMotor.follow(leftFrontMotor); - rightBackMotor.follow(rightFrontMotor); + 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); - // set neutral mode - leftFrontMotor.setNeutralMode(NeutralMode.Brake); - rightFrontMotor.setNeutralMode(NeutralMode.Brake); - leftFrontMotor.setNeutralMode(NeutralMode.Brake); - rightFrontMotor.setNeutralMode(NeutralMode.Brake); + 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); - // flip input so forward becomes back, etc - leftFrontMotor.setInverted(false); - rightFrontMotor.setInverted(false); - leftBackMotor.setInverted(InvertType.FollowMaster); - rightBackMotor.setInverted(InvertType.FollowMaster); - }*/ - /* 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); - public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID); - public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID); - public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); - public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); - public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); - public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); - 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); - - 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(); - rightFrontSteerMotor.configFactoryDefault(); - rightFrontWheelMotor.configFactoryDefault(); - leftBackSteerMotor.configFactoryDefault(); - leftBackWheelMotor.configFactoryDefault(); - 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); - - 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); - - // 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); - } + // 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); + } } diff --git a/src/main/java/frc4388/robot/subsystems/ArcadeDrive.java b/src/main/java/frc4388/robot/subsystems/ArcadeDrive.java deleted file mode 100644 index f06a547..0000000 --- a/src/main/java/frc4388/robot/subsystems/ArcadeDrive.java +++ /dev/null @@ -1,85 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; - -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import frc4388.robot.Constants.ArcadeDriveConstants; -import frc4388.utility.RobotGyro; -import frc4388.utility.RobotTime; - -/** - * Add your docs here. - */ -public class ArcadeDrive extends SubsystemBase { - // Put methods for controlling this subsystem - // here. Call these from Commands. - - private RobotTime m_robotTime = RobotTime.getInstance(); - - private WPI_TalonFX m_leftFrontMotor; - private WPI_TalonFX m_rightFrontMotor; - private WPI_TalonFX m_leftBackMotor; - private WPI_TalonFX m_rightBackMotor; - private DifferentialDrive m_driveTrain; - private RobotGyro m_gyro; - - /** - * Add your docs here. - */ - public ArcadeDrive(WPI_TalonFX leftFrontMotor, WPI_TalonFX rightFrontMotor, WPI_TalonFX leftBackMotor, - WPI_TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) { - - m_leftFrontMotor = leftFrontMotor; - m_rightFrontMotor = rightFrontMotor; - m_leftBackMotor = leftBackMotor; - m_rightBackMotor = rightBackMotor; - m_driveTrain = driveTrain; - m_gyro = gyro; - } - - @Override - public void periodic() { - m_gyro.updatePigeonDeltas(); - - if (m_robotTime.m_frameNumber % ArcadeDriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) { - updateSmartDashboard(); - } - } - - /** - * Add your docs here. - */ - public void driveWithInput(double move, double steer) { - m_driveTrain.arcadeDrive(move, steer); - } - - /** - * Add your docs here. - */ - public void tankDriveWithInput(double leftMove, double rightMove) { - m_leftFrontMotor.set(leftMove); - m_rightFrontMotor.set(rightMove); - } - - /** - * Add your docs here. - */ - private void updateSmartDashboard() { - - // Examples of the functionality of RobotGyro - SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon); - SmartDashboard.putNumber("Turn Rate", m_gyro.getRate()); - SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch()); - SmartDashboard.putData(m_gyro); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 57c7b58..0d4af80 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -7,7 +7,7 @@ package frc4388.robot.subsystems; -import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 6ba30b8..0ac4044 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -1,194 +1,177 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ +/* the project. */ /*----------------------------------------------------------------------------*/ package frc4388.robot.subsystems; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; -import edu.wpi.first.wpilibj.geometry.Rotation2d; -import edu.wpi.first.wpilibj.geometry.Translation2d; -import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics; -import edu.wpi.first.wpilibj.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.util.Units; +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.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.wpilibj2.command.SubsystemBase; 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; - double halfWidth = SwerveDriveConstants.WIDTH / 2.d; - double halfHeight = SwerveDriveConstants.HEIGHT / 2.d; - public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; +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; + 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)); + // setSwerveGains(); - 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 + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); + public SwerveModule[] modules; + public Gyro gyro; // TODO Add Gyro Lol + 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 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; + modules = new SwerveModule[] { + new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder), // Front Left + new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder), // Front Right + new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder), // Back Left + new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder) // Back Right + }; + // gyro.reset(); + } - modules = new SwerveModule[] { - new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder), // Front Left - new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder), // Front Right - new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder), // Back Left - new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder) // Back Right - }; - //gyro.reset(); - } -//https://github.com/ZachOrr/MK3-Swerve-Example - /** + // 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. + * @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, gyro.getRotation2d()) - : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot)); - SwerveDriveKinematics.normalizeWheelSpeeds(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); + 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, gyro.getRotation2d()) + : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot)); + 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); + } - // 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); - } + // 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); - //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); + // 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())); - //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()); + // Set the motors + setSwerveMotors(leftFront, leftBack, rightFront, rightBack); + } + //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); - System.out.println("Target: " + leftFront.angle.getDegrees());*/ - } + //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); - /*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); - - }*/ + }*/ - // public void driveFieldRelative(double awayFromStation, double towardLeftBoundary, double rotate) - // { - // var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary, - // rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/) - // } + // public void driveFieldRelative(double awayFromStation, double towardLeftBoundary, double rotate) + // { + // var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary, rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/) + // } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 4726964..7754ef3 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -12,22 +12,20 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.CANCoderConfiguration; -import edu.wpi.first.wpilibj.geometry.Rotation2d; -import edu.wpi.first.wpilibj.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.simulation.EncoderSim; -import edu.wpi.first.wpilibj.util.Units; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; public class SwerveModule extends SubsystemBase { - private WPI_TalonFX driveMotor; - private WPI_TalonFX angleMotor; - private CANCoder canCoder; - public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; - - private static double kEncoderTicksPerRotation = 4096; + private WPI_TalonFX driveMotor; + private WPI_TalonFX angleMotor; + private CANCoder canCoder; + public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; + private static double kEncoderTicksPerRotation = 4096; /** Creates a new SwerveModule. */ public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder) { @@ -47,30 +45,32 @@ public class SwerveModule extends SubsystemBase { angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; angleMotor.configAllSettings(angleTalonFXConfiguration); - /*TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration(); - - driveTalonFXConfiguration.slot0.kP = kDriveP; - driveTalonFXConfiguration.slot0.kI = kDriveI; - driveTalonFXConfiguration.slot0.kD = kDriveD; - driveTalonFXConfiguration.slot0.kF = kDriveF; - - driveMotor.configAllSettings(driveTalonFXConfiguration);*/ + /* + * TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration(); + * + * driveTalonFXConfiguration.slot0.kP = kDriveP; + * driveTalonFXConfiguration.slot0.kI = kDriveI; + * driveTalonFXConfiguration.slot0.kD = kDriveD; + * driveTalonFXConfiguration.slot0.kF = kDriveF; + * + * driveMotor.configAllSettings(driveTalonFXConfiguration); + */ CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration(); - //CANCODER CONFIG + // CANCODER CONFIG canCoder.configAllSettings(canCoderConfiguration); } - public Rotation2d getAngle() { - // Note: This assumes the CANCoders are setup with the default feedback coefficient - // and the sesnor value reports degrees. + // Note: This assumes the CANCoders are setup with the default feedback coefficient and the sesnor 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) { Rotation2d currentRotation = getAngle(); @@ -86,7 +86,6 @@ public class SwerveModule extends SubsystemBase { double desiredTicks = currentTicks + deltaTicks; angleMotor.set(TalonFXControlMode.Position, desiredTicks); - double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.SPEED_FEET_PER_SECOND_AT_FULL_POWER); } diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java index e3e136f..930e34f 100644 --- a/src/main/java/frc4388/utility/Gains.java +++ b/src/main/java/frc4388/utility/Gains.java @@ -6,55 +6,55 @@ 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 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; - /** - * Creates Gains object for PIDs - * @param kP The P value. - * @param kI The I value. - * @param kD The D value. - * @param kF The F value. - * @param kIzone The zone of the I value. - * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0. - */ - 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; - } + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIzone The zone of the I value. + * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0. + */ + 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; + } - /** - * Creates Gains object for PIDs - * @param kP The P value. - * @param kI The I value. - * @param kD The D value. - * @param kF The F value. - * @param kIzone The zone of the I value. - * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0. - * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0. - */ - 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); - } + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIzone The zone of the I value. + * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0. + * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0. + */ + 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); + } } diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java deleted file mode 100644 index a42e8c8..0000000 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ /dev/null @@ -1,180 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the 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.wpilibj.GyroBase; -import edu.wpi.first.wpiutil.math.MathUtil; - -/** - * Gyro class that allows for interchangeable use between a pigeon and a navX - */ -public class RobotGyro extends GyroBase { - private RobotTime m_robotTime = RobotTime.getInstance(); - - private PigeonIMU m_pigeon = null; - private AHRS m_navX = null; - public boolean m_isGyroAPigeon; //true if pigeon, false if navX - - private double m_lastPigeonAngle; - private double m_deltaPigeonAngle; - - /** - * Creates a Gyro based on a pigeon - * @param gyro the gyroscope to use for Gyro - */ - public RobotGyro(PigeonIMU gyro) { - m_pigeon = gyro; - m_isGyroAPigeon = true; - } - - /** - * Creates a Gyro based on a navX - * @param gyro the gyroscope to use for Gyro - */ - public RobotGyro(AHRS gyro){ - m_navX = gyro; - m_isGyroAPigeon = false; - } - - /** - * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note - * that the getRate() method for a navX will likely be much more accurate than for a pigeon. - */ - public void updatePigeonDeltas() { - double currentPigeonAngle = getAngle(); - m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle; - m_lastPigeonAngle = currentPigeonAngle; - } - - /** - *

NavX: - *

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

Pigeon: - *

Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot - * into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon - * has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to - * make sure that the robot is not moving while the tempurature calculations are in progress, this - * is typically done when the robot is first turned on while it's sitting at rest before the - * competition starts. - */ - @Override - public void calibrate() { - if (m_isGyroAPigeon) { - m_pigeon.enterCalibrationMode(CalibrationMode.Temperature); - } else { - m_navX.calibrate(); - } - } - - @Override - public void reset() { - if (m_isGyroAPigeon) { - m_pigeon.setYaw(0); - } else { - m_navX.reset(); - } - } - - /** - * Get Yaw, Pitch, and Roll data. - * - * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data. - * Yaw is within [-368,640, +368,640] degrees. - * Pitch is within [-90,+90] degrees. - * Roll is within [-90,+90] degrees. - */ - private double[] getPigeonAngles() { - double[] angles = new double[3]; - m_pigeon.getYawPitchRoll(angles); - return angles; - } - - @Override - public double getAngle() { - if (m_isGyroAPigeon) { - return getPigeonAngles()[0]; - } else { - return m_navX.getAngle(); - } - } - - /** - * Gets an absolute heading of the robot - * @return heading from -180 to 180 degrees - */ - public double getHeading() { - return getHeading(getAngle()); - } - - /** - * Gets an absolute heading of the robot - * @return heading from -180 to 180 degrees - */ - public double getHeading(double angle) { - return Math.IEEEremainder(angle, 360); - } - - /** - * Returns the current pitch value (in degrees, from -90 to 90) - * reported by the sensor. Pitch is a measure of rotation around - * the Y Axis. - * @return The current pitch value in degrees (-90 to 90). - */ - public double getPitch() { - if (m_isGyroAPigeon) { - return MathUtil.clamp(getPigeonAngles()[1], -90, 90); - } else { - return MathUtil.clamp(m_navX.getPitch(), -90, 90); - } - } - - /** - * Returns the current roll value (in degrees, from -90 to 90) - * reported by the sensor. Roll is a measure of rotation around - * the X Axis. - * @return The current roll value in degrees (-90 to 90). - */ - public double getRoll() { - if (m_isGyroAPigeon) { - return MathUtil.clamp(getPigeonAngles()[2], -90, 90); - } else { - return MathUtil.clamp(m_navX.getRoll(), -90, 90); - } - } - - @Override - public double getRate() { - if (m_isGyroAPigeon) { - return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000; - } else { - return m_navX.getRate(); - } - } - - public PigeonIMU getPigeon(){ - return m_pigeon; - } - - public AHRS getNavX(){ - return m_navX; - } - - @Override - public void close() throws Exception { - - } -} diff --git a/src/main/java/frc4388/utility/RobotGyro.java.old b/src/main/java/frc4388/utility/RobotGyro.java.old new file mode 100644 index 0000000..56390d4 --- /dev/null +++ b/src/main/java/frc4388/utility/RobotGyro.java.old @@ -0,0 +1,245 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.utility; + +import com.ctre.phoenix.sensors.PigeonIMU; +import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode; +import com.kauailabs.navx.frc.AHRS; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; +import edu.wpi.first.wpilibj.interfaces.Gyro; + +/** + * Gyro class that allows for interchangeable use between a pigeon and a navX + */ +public class RobotGyro implements Gyro, PIDSource, Sendable { + private RobotTime m_robotTime = RobotTime.getInstance(); + + private PigeonIMU m_pigeon = null; + private AHRS m_navX = null; + public boolean m_isGyroAPigeon; // true if pigeon, false if navX + + private double m_lastPigeonAngle; + private double m_deltaPigeonAngle; + + /** + * Creates a Gyro based on a pigeon + * + * @param gyro the gyroscope to use for Gyro + */ + public RobotGyro(PigeonIMU gyro) { + m_pigeon = gyro; + m_isGyroAPigeon = true; + } + + /** + * Creates a Gyro based on a navX + * + * @param gyro the gyroscope to use for Gyro + */ + public RobotGyro(AHRS gyro) { + m_navX = gyro; + m_isGyroAPigeon = false; + } + + /** + * Run in periodic if you are using a pigeon. Updates a delta angle so that it + * can calculate getRate(). Note + * that the getRate() method for a navX will likely be much more accurate than + * for a pigeon. + */ + public void updatePigeonDeltas() { + double currentPigeonAngle = getAngle(); + m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle; + m_lastPigeonAngle = currentPigeonAngle; + } + + /** + *

+ * NavX: + *

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

+ * Pigeon: + *

+ * Calibrate the gyro by collecting data at a range of tempuratures. Allow + * pigeon to cool, then boot + * into calibration mode. For faster calibration, use a heat lamp to heat up the + * pigeon. Once the pigeon + * has seen a reasonable range of tempuratures, it will exit calibration mode. + * It's important to + * make sure that the robot is not moving while the tempurature calculations are + * in progress, this + * is typically done when the robot is first turned on while it's sitting at + * rest before the + * competition starts. + */ + @Override + public void calibrate() { + if (m_isGyroAPigeon) + m_pigeon.enterCalibrationMode(CalibrationMode.Temperature); + else + m_navX.calibrate(); + } + + @Override + public void reset() { + if (m_isGyroAPigeon) + m_pigeon.setYaw(0); + else + m_navX.reset(); + } + + /** + * Get Yaw, Pitch, and Roll data. + * + * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data. + * Yaw is within [-368,640, +368,640] degrees. + * Pitch is within [-90,+90] degrees. + * Roll is within [-90,+90] degrees. + */ + private double[] getPigeonAngles() { + double[] angles = new double[3]; + m_pigeon.getYawPitchRoll(angles); + return angles; + } + + @Override + public double getAngle() { + if (m_isGyroAPigeon) { + return getPigeonAngles()[0]; + } else { + return m_navX.getAngle(); + } + } + + /** + * Gets an absolute heading of the robot + * + * @return heading from -180 to 180 degrees + */ + public double getHeading() { + return getHeading(getAngle()); + } + + /** + * Gets an absolute heading of the robot + * + * @return heading from -180 to 180 degrees + */ + public double getHeading(double angle) { + return Math.IEEEremainder(angle, 360); + } + + /** + * Returns the current pitch value (in degrees, from -90 to 90) + * reported by the sensor. Pitch is a measure of rotation around + * the Y Axis. + * + * @return The current pitch value in degrees (-90 to 90). + */ + public double getPitch() { + if (m_isGyroAPigeon) { + return MathUtil.clamp(getPigeonAngles()[1], -90, 90); + } else { + return MathUtil.clamp(m_navX.getPitch(), -90, 90); + } + } + + /** + * Returns the current roll value (in degrees, from -90 to 90) + * reported by the sensor. Roll is a measure of rotation around + * the X Axis. + * + * @return The current roll value in degrees (-90 to 90). + */ + public double getRoll() { + if (m_isGyroAPigeon) { + return MathUtil.clamp(getPigeonAngles()[2], -90, 90); + } else { + return MathUtil.clamp(m_navX.getRoll(), -90, 90); + } + } + + @Override + public double getRate() { + if (m_isGyroAPigeon) { + return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000; + } else { + return m_navX.getRate(); + } + } + + public PigeonIMU getPigeon() { + return m_pigeon; + } + + public AHRS getNavX() { + return m_navX; + } + + @Override + public void close() throws Exception { + + } + + // Begin old GyroBase class + private PIDSourceType m_pidSource = PIDSourceType.kDisplacement; + + /** + * Set which parameter of the gyro you are using as a process control variable. + * The Gyro class + * supports the rate and displacement parameters + * + * @param pidSource An enum to select the parameter. + */ + @Override + public void setPIDSourceType(PIDSourceType pidSource) { + m_pidSource = pidSource; + } + + @Override + public PIDSourceType getPIDSourceType() { + return m_pidSource; + } + + /** + * Get the output of the gyro for use with PIDControllers. May be the angle or + * rate depending on + * the set PIDSourceType + * + * @return the output according to the gyro + */ + @Override + public double pidGet() { + switch (m_pidSource) { + case kRate: + return getRate(); + case kDisplacement: + return getAngle(); + default: + return 0.0; + } + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Gyro"); + builder.addDoubleProperty("Value", this::getAngle, null); + } +} diff --git a/src/main/java/frc4388/utility/RobotTime.java b/src/main/java/frc4388/utility/RobotTime.java index 694f850..5bda50f 100644 --- a/src/main/java/frc4388/utility/RobotTime.java +++ b/src/main/java/frc4388/utility/RobotTime.java @@ -1,9 +1,6 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ +// 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; @@ -12,68 +9,68 @@ package frc4388.utility; *

All times are in milliseconds */ public class RobotTime { - private long m_currTime = System.currentTimeMillis(); - public long m_deltaTime = 0; + private long m_currTime = System.currentTimeMillis(); + public long m_deltaTime = 0; - private long m_startRobotTime = m_currTime; - public long m_robotTime = 0; - public long m_lastRobotTime = 0; + private long m_startRobotTime = m_currTime; + public long m_robotTime = 0; + public long m_lastRobotTime = 0; - private long m_startMatchTime = 0; - public long m_matchTime = 0; - public long m_lastMatchTime = 0; + private long m_startMatchTime = 0; + public long m_matchTime = 0; + public long m_lastMatchTime = 0; - public long m_frameNumber = 0; + public long m_frameNumber = 0; - /** - * Private constructor prevents other classes from instantiating - */ - private RobotTime(){} + /** + * Private constructor prevents other classes from instantiating + */ + private RobotTime(){} - private static RobotTime instance = null; + private static RobotTime instance = null; - /** - * Gets the instance of Robot Time. If there is no instance running one will be created. - * @return instance of Robot Time - */ - public static RobotTime getInstance() { - if (instance == null) { - instance = new RobotTime(); - } - return instance; + /** + * Gets the instance of Robot Time. If there is no instance running one will be created. + * @return instance of Robot Time + */ + public static RobotTime getInstance() { + if (instance == null) { + instance = new RobotTime(); } + return instance; + } - /** - * Call this once per periodic loop. - */ - public void updateTimes() { - m_lastRobotTime = m_robotTime; - m_lastMatchTime = m_matchTime; + /** + * Call this once per periodic loop. + */ + public void updateTimes() { + m_lastRobotTime = m_robotTime; + m_lastMatchTime = m_matchTime; - m_currTime = System.currentTimeMillis(); - m_robotTime = m_currTime - m_startRobotTime; - m_deltaTime = m_robotTime - m_lastRobotTime; - m_frameNumber++; + m_currTime = System.currentTimeMillis(); + m_robotTime = m_currTime - m_startRobotTime; + m_deltaTime = m_robotTime - m_lastRobotTime; + m_frameNumber++; - if (m_startMatchTime != 0) { - m_matchTime = m_currTime - m_startMatchTime; - } + if (m_startMatchTime != 0) { + m_matchTime = m_currTime - m_startMatchTime; } + } - /** - * Call this in both the auto and periodic inits - */ - public void startMatchTime() { - if (m_startMatchTime == 0) { - m_startMatchTime = m_currTime; - } + /** + * Call this in both the auto and periodic inits + */ + public void startMatchTime() { + if (m_startMatchTime == 0) { + m_startMatchTime = m_currTime; } + } - /** - * Call this in disabled init - */ - public void endMatchTime() { - m_startMatchTime = 0; - m_matchTime = 0; - } + /** + * Call this in disabled init + */ + public void endMatchTime() { + m_startMatchTime = 0; + m_matchTime = 0; + } } diff --git a/src/main/java/frc4388/utility/controller/IHandController.java b/src/main/java/frc4388/utility/controller/IHandController.java index 13aa763..319aebc 100644 --- a/src/main/java/frc4388/utility/controller/IHandController.java +++ b/src/main/java/frc4388/utility/controller/IHandController.java @@ -5,17 +5,17 @@ package frc4388.utility.controller; */ public interface IHandController { - public double getLeftXAxis(); + public double getLeftXAxis(); - public double getLeftYAxis(); + public double getLeftYAxis(); - public double getRightXAxis(); - - public double getRightYAxis(); + public double getRightXAxis(); - public double getLeftTriggerAxis(); + public double getRightYAxis(); - public double getRightTriggerAxis(); + public double getLeftTriggerAxis(); - public int getDpadAngle(); + public double getRightTriggerAxis(); + + public int getDpadAngle(); } diff --git a/src/main/java/frc4388/utility/controller/XboxController.java b/src/main/java/frc4388/utility/controller/XboxController.java index 8b8f0f8..25c496a 100644 --- a/src/main/java/frc4388/utility/controller/XboxController.java +++ b/src/main/java/frc4388/utility/controller/XboxController.java @@ -9,210 +9,210 @@ import edu.wpi.first.wpilibj.Joystick; */ 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 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 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; + 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_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 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 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 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 static final double DEADZONE = 0.1; - private Joystick m_stick; + private Joystick m_stick; - /** + /** * Add your docs here. */ - public XboxController(int portNumber){ - m_stick = new Joystick(portNumber); - } + 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); - } + 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; - } - } + /** + * 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 getAButton(){ + return m_stick.getRawButton(A_BUTTON); + } - public boolean getXButton(){ - return m_stick.getRawButton(X_BUTTON); - } + public boolean getXButton(){ + return m_stick.getRawButton(X_BUTTON); + } - public boolean getBButton(){ - return m_stick.getRawButton(B_BUTTON); - } + public boolean getBButton(){ + return m_stick.getRawButton(B_BUTTON); + } - public boolean getYButton(){ - return m_stick.getRawButton(Y_BUTTON); - } + public boolean getYButton(){ + return m_stick.getRawButton(Y_BUTTON); + } - public boolean getBackButton(){ - return m_stick.getRawButton(BACK_BUTTON); - } + public boolean getBackButton(){ + return m_stick.getRawButton(BACK_BUTTON); + } - public boolean getStartButton(){ - return m_stick.getRawButton(START_BUTTON); - } + public boolean getStartButton(){ + return m_stick.getRawButton(START_BUTTON); + } - public boolean getLeftBumperButton(){ - return m_stick.getRawButton(LEFT_BUMPER_BUTTON); - } + public boolean getLeftBumperButton(){ + return m_stick.getRawButton(LEFT_BUMPER_BUTTON); + } - public boolean getRightBumperButton(){ - return m_stick.getRawButton(RIGHT_BUMPER_BUTTON); - } + public boolean getRightBumperButton(){ + return m_stick.getRawButton(RIGHT_BUMPER_BUTTON); + } - public boolean getLeftJoystickButton(){ - return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON); - } + public boolean getLeftJoystickButton(){ + return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON); + } - public boolean getRightJoystickButton(){ - return m_stick.getRawButton(RIGHT_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 getLeftXAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS)); + } - public double getLeftYAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_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 getRightXAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS)); + } - public double getRightYAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_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 getLeftTriggerAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS)); + } - public double getRightTriggerAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_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); - } + /** + * 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(){ + public boolean getDPadLeft(){ return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE); - } + } - public boolean getDPadRight(){ + public boolean getDPadRight(){ return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE); - } + } - public boolean getDPadTop(){ + public boolean getDPadTop(){ return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE); - } + } - public boolean getDPadBottom(){ + public boolean getDPadBottom(){ return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE); - } + } - public boolean getLeftTrigger(){ - return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE); - } + public boolean getLeftTrigger(){ + return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE); + } - public boolean getRightTrigger(){ - return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE); - } + public boolean getRightTrigger(){ + return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE); + } - public boolean getRightAxisUpTrigger(){ - return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE); - } + public boolean getRightAxisUpTrigger(){ + return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE); + } - public boolean getRightAxisDownTrigger(){ - return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE); - } + public boolean getRightAxisDownTrigger(){ + return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE); + } - public boolean getRightAxisLeftTrigger(){ - return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE); - } + public boolean getRightAxisLeftTrigger(){ + return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE); + } - public boolean getRightAxisRightTrigger(){ - return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE); - } + public boolean getRightAxisRightTrigger(){ + return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE); + } - public boolean getLeftAxisUpTrigger(){ - return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE); - } + public boolean getLeftAxisUpTrigger(){ + return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE); + } - public boolean getLeftAxisDownTrigger(){ - return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE); - } + public boolean getLeftAxisDownTrigger(){ + return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE); + } - public boolean getLeftAxisLeftTrigger(){ - return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE); - } + public boolean getLeftAxisLeftTrigger(){ + return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE); + } - public boolean getLeftAxisRightTrigger(){ - return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE); - } + public boolean getLeftAxisRightTrigger(){ + return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE); + } } \ No newline at end of file diff --git a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java index 26372c2..cecf379 100644 --- a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java +++ b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java @@ -1,7 +1,6 @@ package frc4388.utility.controller; import edu.wpi.first.wpilibj2.command.button.Button; -import frc4388.utility.controller.XboxController; /** * Mapping for the Xbox controller triggers to allow triggers to be defined as @@ -9,61 +8,43 @@ import frc4388.utility.controller.XboxController; * 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; + 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; + 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; - } + /** + * 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() { - if (m_trigger == RIGHT_TRIGGER) { - return m_controller.getRightTrigger(); - } - else if (m_trigger == LEFT_TRIGGER) { - return m_controller.getLeftTrigger(); - } - else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) { - return m_controller.getRightAxisUpTrigger(); - } - else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) { - return m_controller.getRightAxisDownTrigger(); - } - else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) { - return m_controller.getRightAxisRightTrigger(); - } - else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) { - return m_controller.getRightAxisLeftTrigger(); - } - else if (m_trigger == LEFT_AXIS_UP_TRIGGER) { - return m_controller.getLeftAxisUpTrigger(); - } - else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) { - return m_controller.getLeftAxisDownTrigger(); - } - else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) { - return m_controller.getLeftAxisRightTrigger(); - } - else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) { - return m_controller.getLeftAxisLeftTrigger(); - } - return false; - } + /** {@inheritDoc} */ + @Override + public boolean get() { + switch (m_trigger) { + case RIGHT_TRIGGER: return m_controller.getRightTrigger(); + case LEFT_TRIGGER: return m_controller.getLeftTrigger(); + case RIGHT_AXIS_UP_TRIGGER: return m_controller.getRightAxisUpTrigger(); + case RIGHT_AXIS_DOWN_TRIGGER: return m_controller.getRightAxisDownTrigger(); + case RIGHT_AXIS_RIGHT_TRIGGER: return m_controller.getRightAxisRightTrigger(); + case RIGHT_AXIS_LEFT_TRIGGER: return m_controller.getRightAxisLeftTrigger(); + case LEFT_AXIS_UP_TRIGGER: return m_controller.getLeftAxisUpTrigger(); + case LEFT_AXIS_DOWN_TRIGGER: return m_controller.getLeftAxisDownTrigger(); + case LEFT_AXIS_RIGHT_TRIGGER: return m_controller.getLeftAxisRightTrigger(); + case LEFT_AXIS_LEFT_TRIGGER: return m_controller.getLeftAxisLeftTrigger(); + default: return false; + } + } } diff --git a/src/test/java/frc4388/mocks/MockPigeonIMU.java b/src/test/java/frc4388/mocks/MockPigeonIMU.java index ecddde7..d054a72 100644 --- a/src/test/java/frc4388/mocks/MockPigeonIMU.java +++ b/src/test/java/frc4388/mocks/MockPigeonIMU.java @@ -1,9 +1,6 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ +// 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.mocks; @@ -14,41 +11,41 @@ import com.ctre.phoenix.sensors.PigeonIMU; * Add your docs here. */ public class MockPigeonIMU extends PigeonIMU { - public int m_deviceNumber; - public double currentYaw; - public double currentPitch; - public double currentRoll; + public int m_deviceNumber; + public double currentYaw; + public double currentPitch; + public double currentRoll; - public MockPigeonIMU(int deviceNumber) { - super(deviceNumber); - m_deviceNumber = deviceNumber; - } + public MockPigeonIMU(int deviceNumber) { + super(deviceNumber); + m_deviceNumber = deviceNumber; + } - @Override - public ErrorCode setYaw(double angleDeg) { - currentYaw = angleDeg; - return ErrorCode.OK; - } + @Override + public ErrorCode setYaw(double angleDeg) { + currentYaw = angleDeg; + return ErrorCode.OK; + } - /** - * @param currentPitch the Pitch to set - */ - public void setCurrentPitch(double currentPitch) { - this.currentPitch = currentPitch; - } + /** + * @param currentPitch the Pitch to set + */ + public void setCurrentPitch(double currentPitch) { + this.currentPitch = currentPitch; + } - /** - * @param currentRoll the Roll to set - */ - public void setCurrentRoll(double currentRoll) { - this.currentRoll = currentRoll; - } + /** + * @param currentRoll the Roll to set + */ + public void setCurrentRoll(double currentRoll) { + this.currentRoll = currentRoll; + } - @Override - public ErrorCode getYawPitchRoll(double[] ypr_deg) { - ypr_deg[0] = currentYaw; - ypr_deg[1] = currentPitch; - ypr_deg[2] = currentRoll; - return ErrorCode.OK; - } + @Override + public ErrorCode getYawPitchRoll(double[] ypr_deg) { + ypr_deg[0] = currentYaw; + ypr_deg[1] = currentPitch; + ypr_deg[2] = currentRoll; + return ErrorCode.OK; + } } diff --git a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java index 8fcbf53..87ab85c 100644 --- a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java +++ b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java @@ -1,9 +1,6 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ +// 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; @@ -12,7 +9,7 @@ import static org.mockito.Mockito.mock; import org.junit.Test; -import edu.wpi.first.wpilibj.*; +import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.LEDConstants; import frc4388.utility.LEDPatterns; @@ -20,40 +17,40 @@ import frc4388.utility.LEDPatterns; * Add your docs here. */ public class LEDSubsystemTest { - @Test - public void testConstructor() { - // Arrange - Spark ledController = mock(Spark.class); + @Test + public void testConstructor() { + // Arrange + Spark ledController = mock(Spark.class); - // Act - LED led = new LED(ledController); + // Act + LED led = new LED(ledController); - // Assert - assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001); - } + // Assert + 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); + @Test + public void testPatterns() { + // Arrange + Spark ledController = mock(Spark.class); + LED led = new LED(ledController); - // Act - led.setPattern(LEDPatterns.RAINBOW_RAINBOW); + // Act + led.setPattern(LEDPatterns.RAINBOW_RAINBOW); - // Assert - assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001); + // Assert + assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001); - // Act - led.setPattern(LEDPatterns.BLUE_BREATH); + // Act + led.setPattern(LEDPatterns.BLUE_BREATH); - // Assert - assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001); + // Assert + assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001); - // Act - led.setPattern(LEDPatterns.SOLID_BLACK); + // Act + led.setPattern(LEDPatterns.SOLID_BLACK); - // Assert - assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001); - } + // Assert + assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001); + } } diff --git a/src/test/java/frc4388/utility/RobotGyroUtilityTest.java b/src/test/java/frc4388/utility/RobotGyroUtilityTest.java.old similarity index 91% rename from src/test/java/frc4388/utility/RobotGyroUtilityTest.java rename to src/test/java/frc4388/utility/RobotGyroUtilityTest.java.old index c6f0433..862da0e 100644 --- a/src/test/java/frc4388/utility/RobotGyroUtilityTest.java +++ b/src/test/java/frc4388/utility/RobotGyroUtilityTest.java.old @@ -1,9 +1,6 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ +// 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; diff --git a/src/test/java/frc4388/utility/RobotTimeUtilityTest.java b/src/test/java/frc4388/utility/RobotTimeUtilityTest.java index 2c31a34..cc96d41 100644 --- a/src/test/java/frc4388/utility/RobotTimeUtilityTest.java +++ b/src/test/java/frc4388/utility/RobotTimeUtilityTest.java @@ -1,9 +1,6 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ +// 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; @@ -15,90 +12,90 @@ import org.junit.*; * Add your docs here. */ public class RobotTimeUtilityTest { - RobotTime robotTime = RobotTime.getInstance(); + RobotTime robotTime = RobotTime.getInstance(); - @Test - public void testUpdateTimes() { - // Arrange - long lastTime; - robotTime.m_deltaTime = 0; - robotTime.m_robotTime = 0; - robotTime.m_lastRobotTime = 0; - robotTime.m_frameNumber = 0; - robotTime.endMatchTime(); - robotTime.m_lastMatchTime = 0; + @Test + public void testUpdateTimes() { + // Arrange + long lastTime; + robotTime.m_deltaTime = 0; + robotTime.m_robotTime = 0; + robotTime.m_lastRobotTime = 0; + robotTime.m_frameNumber = 0; + robotTime.endMatchTime(); + robotTime.m_lastMatchTime = 0; - // Assert - assertEquals(0, robotTime.m_deltaTime); - assertEquals(0, robotTime.m_robotTime); - assertEquals(0, robotTime.m_lastRobotTime); - assertEquals(0, robotTime.m_frameNumber); - lastTime = robotTime.m_robotTime; + // Assert + assertEquals(0, robotTime.m_deltaTime); + assertEquals(0, robotTime.m_robotTime); + assertEquals(0, robotTime.m_lastRobotTime); + assertEquals(0, robotTime.m_frameNumber); + lastTime = robotTime.m_robotTime; - // Act - wait(1); - robotTime.updateTimes(); + // Act + wait(1); + robotTime.updateTimes(); - // Assert - assertEquals(true, robotTime.m_deltaTime > 0); - assertEquals(true, robotTime.m_robotTime > 0); - assertEquals(lastTime, robotTime.m_lastRobotTime); - assertEquals(1, robotTime.m_frameNumber); - lastTime = robotTime.m_robotTime; + // Assert + assertEquals(true, robotTime.m_deltaTime > 0); + assertEquals(true, robotTime.m_robotTime > 0); + assertEquals(lastTime, robotTime.m_lastRobotTime); + assertEquals(1, robotTime.m_frameNumber); + lastTime = robotTime.m_robotTime; - // Act - wait(1); - robotTime.updateTimes(); + // Act + wait(1); + robotTime.updateTimes(); - // Assert - assertEquals(true, robotTime.m_deltaTime > 0); - assertEquals(true, robotTime.m_robotTime > 0); - assertEquals(lastTime, robotTime.m_lastRobotTime); - assertEquals(2, robotTime.m_frameNumber); - } + // Assert + assertEquals(true, robotTime.m_deltaTime > 0); + assertEquals(true, robotTime.m_robotTime > 0); + assertEquals(lastTime, robotTime.m_lastRobotTime); + assertEquals(2, robotTime.m_frameNumber); + } - @Test - public void testMatchTime() { - // Arrange - long lastTime; + @Test + public void testMatchTime() { + // Arrange + long lastTime; - // Assert - assertEquals(0, robotTime.m_matchTime); - assertEquals(0, robotTime.m_lastMatchTime); - lastTime = robotTime.m_matchTime; + // Assert + assertEquals(0, robotTime.m_matchTime); + assertEquals(0, robotTime.m_lastMatchTime); + lastTime = robotTime.m_matchTime; - // Act - robotTime.startMatchTime(); - wait(1); - robotTime.updateTimes(); + // Act + robotTime.startMatchTime(); + wait(1); + robotTime.updateTimes(); - // Assert - assertEquals(true, robotTime.m_matchTime > 0); - assertEquals(lastTime, robotTime.m_lastMatchTime); - lastTime = robotTime.m_matchTime; - - // Act - wait(1); - robotTime.updateTimes(); - robotTime.endMatchTime(); + // Assert + assertEquals(true, robotTime.m_matchTime > 0); + assertEquals(lastTime, robotTime.m_lastMatchTime); + lastTime = robotTime.m_matchTime; + + // Act + wait(1); + robotTime.updateTimes(); + robotTime.endMatchTime(); - // Assert - assertEquals(0, robotTime.m_matchTime); - assertEquals(lastTime, robotTime.m_lastMatchTime); - lastTime = robotTime.m_matchTime; + // Assert + assertEquals(0, robotTime.m_matchTime); + assertEquals(lastTime, robotTime.m_lastMatchTime); + lastTime = robotTime.m_matchTime; - // Act - wait(1); - robotTime.updateTimes(); + // Act + wait(1); + robotTime.updateTimes(); - // Assert - assertEquals(0, robotTime.m_matchTime); - assertEquals(lastTime, robotTime.m_lastMatchTime); - } + // Assert + assertEquals(0, robotTime.m_matchTime); + assertEquals(lastTime, robotTime.m_lastMatchTime); + } - private void wait(int millis) { - try { - Thread.sleep(millis); - } catch (Exception e) {} - } + private void wait(int millis) { + try { + Thread.sleep(millis); + } catch (Exception e) {} + } } diff --git a/template.config.js b/template.config.js deleted file mode 100644 index 54f2425..0000000 --- a/template.config.js +++ /dev/null @@ -1,31 +0,0 @@ -/** - * This file is a configuration file generated by the `Template` extension on `vscode` - * @see https://marketplace.visualstudio.com/items?itemName=yongwoo.template - */ -module.exports = { - // You can change the template path to another path - templateRootPath: "./.templates", - // After copying the template file the `replaceFileTextFn` function is executed - replaceFileTextFn: (fileText, templateName, utils) => { - // @see https://www.npmjs.com/package/change-case - const { changeCase } = utils; - // You can change the text in the file - return fileText - .replace(/__templateName__/gm, templateName) - .replace( - /__templateNameToPascalCase__/gm, - changeCase.pascalCase(templateName) - ) - .replace( - /__templateNameToParamCase__/gm, - changeCase.paramCase(templateName) - ); - }, - replaceFileNameFn: (fileName, templateName, utils) => { - const { path } = utils; - // @see https://nodejs.org/api/path.html#path_path_parse_path - const { base } = path.parse(fileName); - // You can change the file name - return base; - } -}; diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index 87f03cb..789d831 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,29 +1,30 @@ { "fileName": "Phoenix.json", "name": "CTRE-Phoenix", - "version": "5.19.4", + "version": "5.20.2", + "frcYear": 2022, "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ - "https://devsite.ctr-electronics.com/maven/release/" + "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix-frc2022-latest.json", "javaDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.19.4" + "version": "5.20.2" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.19.4" + "version": "5.20.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.19.4", + "version": "5.20.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -33,7 +34,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.19.4", + "version": "5.20.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -45,7 +46,19 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "simTalonSRX", - "version": "5.19.4", + "version": "5.20.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simTalonFX", + "version": "5.20.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -57,7 +70,31 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "simVictorSPX", - "version": "5.19.4", + "version": "5.20.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simPigeonIMU", + "version": "5.20.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simCANCoder", + "version": "5.20.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -71,52 +108,74 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.19.4", + "version": "5.20.2", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", - "sharedLibrary": false, + "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64", - "osxx86-64" + "linuxathena" ] }, { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.19.4", + "version": "5.20.2", "libName": "CTRE_Phoenix", "headerClassifier": "headers", - "sharedLibrary": false, + "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64", - "osxx86-64" + "linuxathena" ] }, { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.19.4", + "version": "5.20.2", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", - "sharedLibrary": false, + "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "linuxathena" ] }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.20.2", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.20.2", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.19.4", + "version": "5.20.2", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", - "sharedLibrary": false, + "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", @@ -124,68 +183,10 @@ "osxx86-64" ] }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "diagnostics", - "version": "5.19.4", - "libName": "CTRE_PhoenixDiagnostics", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64", - "osxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "canutils", - "version": "5.19.4", - "libName": "CTRE_PhoenixCanutils", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "platform-sim", - "version": "5.19.4", - "libName": "CTRE_PhoenixPlatform", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "core", - "version": "5.19.4", - "libName": "CTRE_PhoenixCore", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64", - "osxx86-64" - ] - }, { "groupId": "com.ctre.phoenix.sim", "artifactId": "simTalonSRX", - "version": "5.19.4", + "version": "5.20.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -196,10 +197,24 @@ "osxx86-64" ] }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simTalonFX", + "version": "5.20.2", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, { "groupId": "com.ctre.phoenix.sim", "artifactId": "simVictorSPX", - "version": "5.19.4", + "version": "5.20.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -209,6 +224,34 @@ "linuxx86-64", "osxx86-64" ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simPigeonIMU", + "version": "5.20.2", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simCANCoder", + "version": "5.20.2", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] } ] } \ No newline at end of file diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index 83de291..ab3d1d0 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -34,4 +34,5 @@ ] } ] -} \ No newline at end of file + } + \ No newline at end of file diff --git a/vendordeps/WPILibOldCommands.json b/vendordeps/WPILibOldCommands.json new file mode 100644 index 0000000..acc8879 --- /dev/null +++ b/vendordeps/WPILibOldCommands.json @@ -0,0 +1,37 @@ +{ + "fileName": "WPILibOldCommands.json", + "name": "WPILib-Old-Commands", + "version": "2020.0.0", + "uuid": "b066afc2-5c18-43c4-b758-43381fcb275e", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibOldCommands", + "artifactId": "wpilibOldCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibOldCommands", + "artifactId": "wpilibOldCommands-cpp", + "version": "wpilib", + "libName": "wpilibOldCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "linuxaarch64bionic", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxx86-64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/navx_frc.json b/vendordeps/navx_frc.json index dca1d82..0fb49a7 100644 --- a/vendordeps/navx_frc.json +++ b/vendordeps/navx_frc.json @@ -1,17 +1,17 @@ { "fileName": "navx_frc.json", "name": "KauaiLabs_navX_FRC", - "version": "3.1.413", + "version": "4.0.442", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", "mavenUrls": [ "https://repo1.maven.org/maven2/" ], - "jsonUrl": "https://www.kauailabs.com/dist/frc/2020/navx_frc.json", + "jsonUrl": "https://www.kauailabs.com/dist/frc/2022/navx_frc.json", "javaDependencies": [ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-java", - "version": "3.1.413" + "version": "4.0.442" } ], "jniDependencies": [], @@ -19,7 +19,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-cpp", - "version": "3.1.413", + "version": "4.0.442", "headerClassifier": "headers", "sourcesClassifier": "sources", "sharedLibrary": false,