Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Upgrade to 2022

Browse Source 
* Update license
* Remove arcade drive
* Correct indentation
* Disable RobotGyro (new gyro is untested)
This commit is contained in:
nathanrsxtn
2022-01-11 11:05:52 -07:00
parent 731310fbc8
commit 71563e6759
38 changed files with 1507 additions and 1541 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+5 -4
View File
@@ -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++ ### ### C++ ###
# Prerequisites # Prerequisites
@@ -151,11 +152,11 @@ gradle-app.setting
# gradle/wrapper/gradle-wrapper.properties # gradle/wrapper/gradle-wrapper.properties
# # VS Code Specific Java Settings # # VS Code Specific Java Settings
# DO NOT REMOVE .classpath and .project
.classpath .classpath
.project .project
.settings/ .settings/
bin/ bin/
imgui.ini
# Simulation GUI and other tools window save file
# End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode *-window.json
.templates/SubsystemTest.java
+3 -6
View File
@@ -1,9 +1,6 @@
/*----------------------------------------------------------------------------*/ // Copyright (c) FIRST and other WPILib contributors.
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ // Open Source Software; you can modify and/or share it under the terms of
/* Open Source Software - may be modified and shared by FRC teams. The code */ // the WPILib BSD license file in the root directory of this project.
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.subsystems; package frc4388.robot.subsystems;
.templates/UtilityTest.java
+3 -6
View File
@@ -1,9 +1,6 @@
/*----------------------------------------------------------------------------*/ // Copyright (c) FIRST and other WPILib contributors.
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ // Open Source Software; you can modify and/or share it under the terms of
/* Open Source Software - may be modified and shared by FRC teams. The code */ // the WPILib BSD license file in the root directory of this project.
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.utility; package frc4388.utility;
.vscode/settings.json
Vendored
+16 -2
View File
@@ -1,5 +1,6 @@
{ {
"java.configuration.updateBuildConfiguration": "automatic", "java.configuration.updateBuildConfiguration": "automatic",
"java.server.launchMode": "Standard",
"files.exclude": { "files.exclude": {
"**/.git": true, "**/.git": true,
"**/.svn": true, "**/.svn": true,
@@ -10,6 +11,19 @@
"**/.classpath": true, "**/.classpath": true,
"**/.project": true, "**/.project": true,
"**/.settings": 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"
} }
.wpilib/wpilib_preferences.json
+1 -1
View File
@@ -1,6 +1,6 @@
{ {
"enableCppIntellisense": false, "enableCppIntellisense": false,
"currentLanguage": "java", "currentLanguage": "java",
"projectYear": "2021", "projectYear": "2022",
"teamNumber": 4388 "teamNumber": 4388
} }
LICENSE
-24
View File
@@ -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.
README.md
-2
View File
@@ -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)
WPILib-License.md
+24
View File
@@ -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.
build.gradle
+49 -39
View File
@@ -1,6 +1,8 @@
import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO
plugins { plugins {
id "java" id "java"
id "edu.wpi.first.GradleRIO" version "2021.3.1" id "edu.wpi.first.GradleRIO" version "2022.1.1"
} }
sourceCompatibility = JavaVersion.VERSION_11 sourceCompatibility = JavaVersion.VERSION_11
@@ -9,67 +11,69 @@ targetCompatibility = JavaVersion.VERSION_11
def ROBOT_MAIN_CLASS = "frc4388.robot.Main" def ROBOT_MAIN_CLASS = "frc4388.robot.Main"
// Define my targets (RoboRIO) and artifacts (deployable files) // 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 { deploy {
targets { targets {
roboRIO("roborio") { roborio(getTargetTypeClass('RoboRIO')) {
// Team number is loaded either from the .wpilib/wpilib_preferences.json // Team number is loaded either from the .wpilib/wpilib_preferences.json
// or from command line. If not found an exception will be thrown. // or from command line. If not found an exception will be thrown.
// You can use getTeamOrDefault(team) instead of getTeamNumber if you // You can use getTeamOrDefault(team) instead of getTeamNumber if you
// want to store a team number in this file. // want to store a team number in this file.
team = frc.getTeamNumber() team = project.frc.getTeamNumber()
} debug = project.frc.getDebugOrDefault(false)
}
artifacts { artifacts {
frcJavaArtifact('frcJava') { // First part is artifact name, 2nd is artifact type
targets << "roborio" // getTargetTypeClass is a shortcut to get the class type using a string
// Debug can be overridden by command line, for use with VSCode
debug = frc.getDebugOrDefault(false) frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
} }
// Built in artifact to deploy arbitrary files to the roboRIO.
fileTreeArtifact('frcStaticFileDeploy') { // Static files artifact
// The directory below is the local directory to deploy frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
files = fileTree(dir: 'src/main/deploy') files = project.fileTree('src/main/deploy')
// Deploy to RoboRIO target, into /home/lvuser/deploy directory = '/home/lvuser/deploy'
targets << "roborio" }
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. // Set this to true to enable desktop support.
def includeDesktopSupport = true def includeDesktopSupport = true
// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
// Also defines JUnit 4. // Also defines JUnit 4.
dependencies { dependencies {
implementation wpi.deps.wpilib() implementation wpi.java.deps.wpilib()
nativeZip wpi.deps.wpilibJni(wpi.platforms.roborio) implementation wpi.java.vendor.java()
nativeDesktopZip wpi.deps.wpilibJni(wpi.platforms.desktop)
roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
implementation wpi.deps.vendor.java() roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio) roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)
nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop)
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' testImplementation 'junit:junit:4.12'
testCompile "org.mockito:mockito-core:2.+" testImplementation "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)
} }
// Simulation configuration (e.g. environment variables). // Simulation configuration (e.g. environment variables).
sim { wpi.sim.addGui().defaultEnabled = true
// Sets the websocket client remote host. wpi.sim.addDriverstation()
// envVar "HALSIMWS_HOST", "10.0.0.2"
}
// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') // 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 // in order to make them all available at runtime. Also adding the manifest so WPILib
@@ -77,4 +81,10 @@ sim {
jar { jar {
from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) 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)
gradle/wrapper/gradle-wrapper.jar
Vendored
BIN
View File
Binary file not shown.
gradle/wrapper/gradle-wrapper.properties
Vendored
+1 -1
View File
@@ -1,5 +1,5 @@
distributionBase=GRADLE_USER_HOME distributionBase=GRADLE_USER_HOME
distributionPath=permwrapper/dists 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 zipStoreBase=GRADLE_USER_HOME
zipStorePath=permwrapper/dists zipStorePath=permwrapper/dists
gradlew
Vendored Regular → Executable
+160 -109
View File
@@ -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"); # Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with 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 # Attempt to set APP_HOME
# Resolve links: $0 may be a link # Resolve links: $0 may be a link
PRG="$0" app_path=$0
# Need this for relative symlinks.
while [ -h "$PRG" ] ; do # Need this for daisy-chained symlinks.
ls=`ls -ld "$PRG"` while
link=`expr "$ls" : '.*-> \(.*\)$'` APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path
if expr "$link" : '/.*' > /dev/null; then [ -h "$app_path" ]
PRG="$link" do
else ls=$( ls -ld "$app_path" )
PRG=`dirname "$PRG"`"/$link" link=${ls#*' -> '}
fi case $link in #(
/*) app_path=$link ;; #(
*) app_path=$APP_HOME$link ;;
esac
done done
SAVED="`pwd`"
cd "`dirname \"$PRG\"`/" >/dev/null APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit
APP_HOME="`pwd -P`"
cd "$SAVED" >/dev/null
APP_NAME="Gradle" 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. # 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"' DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
# Use the maximum available, or set MAX_FD != -1 to use that value. # Use the maximum available, or set MAX_FD != -1 to use that value.
MAX_FD="maximum" MAX_FD=maximum
warn () { warn () {
echo "$*" echo "$*"
} } >&2
die () { die () {
echo echo
echo "$*" echo "$*"
echo echo
exit 1 exit 1
} } >&2
# OS specific support (must be 'true' or 'false'). # OS specific support (must be 'true' or 'false').
cygwin=false cygwin=false
msys=false msys=false
darwin=false darwin=false
nonstop=false nonstop=false
case "`uname`" in case "$( uname )" in #(
CYGWIN* ) CYGWIN* ) cygwin=true ;; #(
cygwin=true Darwin* ) darwin=true ;; #(
;; MSYS* | MINGW* ) msys=true ;; #(
Darwin* ) NONSTOP* ) nonstop=true ;;
darwin=true
;;
MINGW* )
msys=true
;;
NONSTOP* )
nonstop=true
;;
esac esac
CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
# Determine the Java command to use to start the JVM. # Determine the Java command to use to start the JVM.
if [ -n "$JAVA_HOME" ] ; then if [ -n "$JAVA_HOME" ] ; then
if [ -x "$JAVA_HOME/jre/sh/java" ] ; then if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
# IBM's JDK on AIX uses strange locations for the executables # IBM's JDK on AIX uses strange locations for the executables
JAVACMD="$JAVA_HOME/jre/sh/java" JAVACMD=$JAVA_HOME/jre/sh/java
else else
JAVACMD="$JAVA_HOME/bin/java" JAVACMD=$JAVA_HOME/bin/java
fi fi
if [ ! -x "$JAVACMD" ] ; then if [ ! -x "$JAVACMD" ] ; then
die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME 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." location of your Java installation."
fi fi
else 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. 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 Please set the JAVA_HOME variable in your environment to match the
@@ -105,79 +140,95 @@ location of your Java installation."
fi fi
# Increase the maximum file descriptors if we can. # Increase the maximum file descriptors if we can.
if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
MAX_FD_LIMIT=`ulimit -H -n` case $MAX_FD in #(
if [ $? -eq 0 ] ; then max*)
if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then MAX_FD=$( ulimit -H -n ) ||
MAX_FD="$MAX_FD_LIMIT" warn "Could not query maximum file descriptor limit"
fi esac
ulimit -n $MAX_FD case $MAX_FD in #(
if [ $? -ne 0 ] ; then '' | soft) :;; #(
warn "Could not set maximum file descriptor limit: $MAX_FD" *)
fi ulimit -n "$MAX_FD" ||
else warn "Could not set maximum file descriptor limit to $MAX_FD"
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" ;;
esac esac
fi fi
# Escape application args # Collect all arguments for the java command, stacking in reverse order:
save () { # * args from the command line
for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done # * the main class name
echo " " # * -classpath
} # * -D...appname settings
APP_ARGS=`save "$@"` # * --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 # For Cygwin or MSYS, switch paths to Windows format before running java
eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS" 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" "$@" exec "$JAVACMD" "$@"
gradlew.bat
Vendored
+7 -18
View File
@@ -29,6 +29,9 @@ if "%DIRNAME%" == "" set DIRNAME=.
set APP_BASE_NAME=%~n0 set APP_BASE_NAME=%~n0
set APP_HOME=%DIRNAME% 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. @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" set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
@@ -37,7 +40,7 @@ if defined JAVA_HOME goto findJavaFromJavaHome
set JAVA_EXE=java.exe set JAVA_EXE=java.exe
%JAVA_EXE% -version >NUL 2>&1 %JAVA_EXE% -version >NUL 2>&1
if "%ERRORLEVEL%" == "0" goto init if "%ERRORLEVEL%" == "0" goto execute
echo. echo.
echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 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_HOME=%JAVA_HOME:"=%
set JAVA_EXE=%JAVA_HOME%/bin/java.exe set JAVA_EXE=%JAVA_HOME%/bin/java.exe
if exist "%JAVA_EXE%" goto init if exist "%JAVA_EXE%" goto execute
echo. echo.
echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
@@ -61,28 +64,14 @@ echo location of your Java installation.
goto fail 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 :execute
@rem Setup the command line @rem Setup the command line
set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
@rem Execute Gradle @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 :end
@rem End local scope for the variables with windows NT shell @rem End local scope for the variables with windows NT shell
settings.gradle
+1 -1
View File
@@ -4,7 +4,7 @@ pluginManagement {
repositories { repositories {
mavenLocal() mavenLocal()
gradlePluginPortal() gradlePluginPortal()
String frcYear = '2021' String frcYear = '2022'
File frcHome File frcHome
if (OperatingSystem.current().isWindows()) { if (OperatingSystem.current().isWindows()) {
String publicFolder = System.getenv('PUBLIC') String publicFolder = System.getenv('PUBLIC')
src/main/java/frc4388/robot/Constants.java
+46 -60
View File
@@ -1,9 +1,6 @@
/*----------------------------------------------------------------------------*/ // Copyright (c) FIRST and other WPILib contributors.
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ // Open Source Software; you can modify and/or share it under the terms of
/* Open Source Software - may be modified and shared by FRC teams. The code */ // the WPILib BSD license file in the root directory of this project.
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot; package frc4388.robot;
@@ -19,62 +16,51 @@ import frc4388.utility.LEDPatterns;
* constants are needed, to reduce verbosity. * constants are needed, to reduce verbosity.
*/ */
public final class Constants { public final class Constants {
public static final class ArcadeDriveConstants { public static final class SwerveDriveConstants {
public static final int DRIVE_LEFT_FRONT_CAN_ID = 2; public static final double ROTATION_SPEED = 0.1;
public static final int DRIVE_RIGHT_FRONT_CAN_ID = 4; public static final double WHEEL_SPEED = 0.1;
public static final int DRIVE_LEFT_BACK_CAN_ID = 3; public static final double WIDTH = 22;
public static final int DRIVE_RIGHT_BACK_CAN_ID = 5; 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 DRIVE_PIGEON_ID = 6; // 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 int SMARTDASHBOARD_UPDATE_FRAME = 2; // 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 class SwerveDriveConstants { public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
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 PID constants public static final class OIConstants {
public static final int SWERVE_SLOT_IDX = 0; public static final int XBOX_DRIVER_ID = 0;
public static final int SWERVE_PID_LOOP_IDX = 1; public static final int XBOX_OPERATOR_ID = 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);
// 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;
}
} }
src/main/java/frc4388/robot/Robot.java
+10 -6
View File
@@ -75,12 +75,16 @@ public class Robot extends TimedRobot {
public void autonomousInit() { public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand(); m_autonomousCommand = m_robotContainer.getAutonomousCommand();
/* /*String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
* String autoSelected = SmartDashboard.getString("Auto Selector", switch (autoSelected) {
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand case "My Auto":
* = new MyAutoCommand(); break; case "Default Auto": default: autonomousCommand = new MyAutoCommand();
* autonomousCommand = new ExampleCommand(); break; } break;
*/ case "Default Auto":
default:
autonomousCommand = new ExampleCommand();
break;
}*/
// schedule the autonomous command (example) // schedule the autonomous command (example)
if (m_autonomousCommand != null) { if (m_autonomousCommand != null) {
src/main/java/frc4388/robot/RobotContainer.java
+80 -95
View File
@@ -1,9 +1,6 @@
/*----------------------------------------------------------------------------*/ // Copyright (c) FIRST and other WPILib contributors.
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ // Open Source Software; you can modify and/or share it under the terms of
/* Open Source Software - may be modified and shared by FRC teams. The code */ // the WPILib BSD license file in the root directory of this project.
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot; 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.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*; import frc4388.robot.Constants.*;
import frc4388.robot.subsystems.ArcadeDrive;
import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.LEDPatterns; import frc4388.utility.LEDPatterns;
@@ -28,105 +24,94 @@ import frc4388.utility.controller.XboxController;
* commands, and button mappings) should be declared here. * commands, and button mappings) should be declared here.
*/ */
public class RobotContainer { public class RobotContainer {
/* RobotMap */ /* RobotMap */
private final RobotMap m_robotMap = new RobotMap(); private final RobotMap m_robotMap = new RobotMap();
/* Subsystems */ /* Subsystems */
//private final ArcadeDrive m_robotArcadeDrive = new ArcadeDrive(m_robotMap.leftFrontMotor, m_robotMap.rightFrontMotor, private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
// m_robotMap.leftBackMotor, m_robotMap.rightBackMotor, m_robotMap.driveTrain, m_robotMap.gyroDrive); 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( private final LED m_robotLED = new LED(m_robotMap.LEDController);
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); /* 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); * The container for the robot. Contains subsystems, OI devices, and commands.
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); */
public RobotContainer() {
configureButtonBindings();
/** /* Default Commands */
* The container for the robot. Contains subsystems, OI devices, and commands. // drives the swerve drive with a two-axis input from the driver controller
*/ m_robotSwerveDrive.setDefaultCommand(
public RobotContainer() { new RunCommand(() -> m_robotSwerveDrive.driveWithInput(getDriverController().getLeftXAxis(),
configureButtonBindings(); getDriverController().getLeftYAxis(), getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
/* Default Commands */ // continually sends updates to the Blinkin LED controller to keep the lights on
// drives the arcade drive with a two-axis input from the driver controller m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
/*m_robotArcadeDrive.setDefaultCommand( }
new RunCommand(() -> m_robotArcadeDrive.driveWithInput(getDriverController().getLeftYAxis(),
getDriverController().getRightXAxis()), m_robotArcadeDrive));*/
// drives the swerve drive with a two-axis input from the driver controller /**
m_robotSwerveDrive.setDefaultCommand( * Use this method to define your button->command mappings. Buttons can be
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(getDriverController().getLeftXAxis(), * created by instantiating a {@link GenericHID} or one of its subclasses
getDriverController().getLeftYAxis(), getDriverController().getRightXAxis(), false), m_robotSwerveDrive)); * ({@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 /* Operator Buttons */
m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); // 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 * Use this to pass the autonomous command to the main {@link Robot} class.
* created by instantiating a {@link GenericHID} or one of its subclasses *
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then * @return the command to run in autonomous
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */
*/ public Command getAutonomousCommand() {
private void configureButtonBindings() { // no auto
/* Driver Buttons */ return new InstantCommand();
// 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));
/* Operator Buttons */ /**
// activates "Lit Mode" * Add your docs here.
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) */
.whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) public IHandController getDriverController() {
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); return m_driverXbox;
} }
/** /**
* Use this to pass the autonomous command to the main {@link Robot} class. * Add your docs here.
* */
* @return the command to run in autonomous public IHandController getOperatorController() {
*/ return m_operatorXbox;
public Command getAutonomousCommand() { }
// no auto
return new InstantCommand();
}
/** /**
* Add your docs here. * Add your docs here.
*/ */
public IHandController getDriverController() { public Joystick getOperatorJoystick() {
return m_driverXbox; return m_operatorXbox.getJoyStick();
} }
/** /**
* Add your docs here. * Add your docs here.
*/ */
public IHandController getOperatorController() { public Joystick getDriverJoystick() {
return m_operatorXbox; return m_driverXbox.getJoyStick();
} }
/**
* Add your docs here.
*/
public Joystick getOperatorJoystick() {
return m_operatorXbox.getJoyStick();
}
/**
* Add your docs here.
*/
public Joystick getDriverJoystick() {
return m_driverXbox.getJoyStick();
}
} }
src/main/java/frc4388/robot/RobotMap.java
+68 -112
View File
@@ -1,26 +1,15 @@
/*----------------------------------------------------------------------------*/ // Copyright (c) FIRST and other WPILib contributors.
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ // Open Source Software; you can modify and/or share it under the terms of
/* Open Source Software - may be modified and shared by FRC teams. The code */ // the WPILib BSD license file in the root directory of this project.
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot; 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.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.AbsoluteSensorRange;
import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.CANCoder;
import edu.wpi.first.wpilibj.Spark; import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import frc4388.robot.Constants.ArcadeDriveConstants;
import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.RobotGyro;
/** /**
* Defines and holds all I/O objects on the Roborio. This is useful for unit * 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 class RobotMap {
public RobotMap() { public RobotMap() {
configureLEDMotorControllers(); configureLEDMotorControllers();
//configureArcadeDriveMotorControllers(); configureSwerveMotorControllers();
configureSwerveMotorControllers(); }
}
/* LED Subsystem */ /* LED Subsystem */
public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
void configureLEDMotorControllers() { void configureLEDMotorControllers() {
} }
/* ArcadeDrive Subsystem */ /* Swerve Subsystem */
//public final WPI_TalonFX leftFrontMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_LEFT_FRONT_CAN_ID); public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
//public final WPI_TalonFX rightFrontMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
//public final WPI_TalonFX leftBackMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_LEFT_BACK_CAN_ID); public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
//public final WPI_TalonFX rightBackMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_RIGHT_BACK_CAN_ID); public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID);
//public final DifferentialDrive driveTrain = new DifferentialDrive(leftFrontMotor, rightFrontMotor); public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID);
//public final RobotGyro gyroDrive = new RobotGyro(new PigeonIMU(ArcadeDriveConstants.DRIVE_PIGEON_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 configureArcadeDriveMotorControllers() { 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);
// factory default values leftFrontSteerMotor.configFactoryDefault();
leftFrontMotor.configFactoryDefault(); leftFrontWheelMotor.configFactoryDefault();
rightFrontMotor.configFactoryDefault(); rightFrontSteerMotor.configFactoryDefault();
leftBackMotor.configFactoryDefault(); rightFrontWheelMotor.configFactoryDefault();
rightBackMotor.configFactoryDefault(); leftBackSteerMotor.configFactoryDefault();
leftBackWheelMotor.configFactoryDefault();
rightBackSteerMotor.configFactoryDefault();
rightBackWheelMotor.configFactoryDefault();
// set back motors as followers leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftBackMotor.follow(leftFrontMotor); leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackMotor.follow(rightFrontMotor); 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 neutral mode leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftFrontMotor.setNeutralMode(NeutralMode.Brake); leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontMotor.setNeutralMode(NeutralMode.Brake); rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftFrontMotor.setNeutralMode(NeutralMode.Brake); rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontMotor.setNeutralMode(NeutralMode.Brake); 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);
// flip input so forward becomes back, etc leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftFrontMotor.setInverted(false); leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontMotor.setInverted(false); rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftBackMotor.setInverted(InvertType.FollowMaster); rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackMotor.setInverted(InvertType.FollowMaster); leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
}*/ leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
/* Swerve Subsystem */ rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
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() { // config cancoder as remote encoder for swerve steer motors
leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); //leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); //leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); //rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
}
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);
}
} }
src/main/java/frc4388/robot/subsystems/ArcadeDrive.java
-85
View File
@@ -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);
}
}
src/main/java/frc4388/robot/subsystems/LED.java
+1 -1
View File
@@ -7,7 +7,7 @@
package frc4388.robot.subsystems; 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.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+140 -157
View File
@@ -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 */ /* 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 */ /* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */ /* the project. */
/*----------------------------------------------------------------------------*/ /*----------------------------------------------------------------------------*/
package frc4388.robot.subsystems; package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.CANCoder;
import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.wpilibj.kinematics.SwerveModuleState; import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.util.Units; import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains; import frc4388.utility.Gains;
import frc4388.utility.RobotGyro;
public class SwerveDrive extends SubsystemBase public class SwerveDrive extends SubsystemBase {
{ SwerveDriveKinematics m_kinematics;
SwerveDriveKinematics m_kinematics; private WPI_TalonFX m_leftFrontSteerMotor;
private WPI_TalonFX m_leftFrontSteerMotor; private WPI_TalonFX m_leftFrontWheelMotor;
private WPI_TalonFX m_leftFrontWheelMotor; private WPI_TalonFX m_rightFrontSteerMotor;
private WPI_TalonFX m_rightFrontSteerMotor; private WPI_TalonFX m_rightFrontWheelMotor;
private WPI_TalonFX m_rightFrontWheelMotor; private WPI_TalonFX m_leftBackSteerMotor;
private WPI_TalonFX m_leftBackSteerMotor; private WPI_TalonFX m_leftBackWheelMotor;
private WPI_TalonFX m_leftBackWheelMotor; private WPI_TalonFX m_rightBackSteerMotor;
private WPI_TalonFX m_rightBackSteerMotor; private WPI_TalonFX m_rightBackWheelMotor;
private WPI_TalonFX m_rightBackWheelMotor; private CANCoder m_leftFrontEncoder;
private CANCoder m_leftFrontEncoder; private CANCoder m_rightFrontEncoder;
private CANCoder m_rightFrontEncoder; private CANCoder m_leftBackEncoder;
private CANCoder m_leftBackEncoder; private CANCoder m_rightBackEncoder;
private CANCoder m_rightBackEncoder; double halfWidth = SwerveDriveConstants.WIDTH / 2.d;
double halfWidth = SwerveDriveConstants.WIDTH / 2.d; double halfHeight = SwerveDriveConstants.HEIGHT / 2.d;
double halfHeight = SwerveDriveConstants.HEIGHT / 2.d; public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
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 = private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
new Translation2d( public SwerveModule[] modules;
Units.inchesToMeters(halfHeight), public Gyro gyro; // TODO Add Gyro Lol
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 SwerveDrive(WPI_TalonFX leftFrontSteerMotor, WPI_TalonFX leftFrontWheelMotor,
public SwerveModule[] modules; WPI_TalonFX rightFrontSteerMotor, WPI_TalonFX rightFrontWheelMotor,
public RobotGyro gyro; //TODO Add Gyro Lol 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();
}
public SwerveDrive(WPI_TalonFX leftFrontSteerMotor,WPI_TalonFX leftFrontWheelMotor,WPI_TalonFX rightFrontSteerMotor,WPI_TalonFX rightFrontWheelMotor, // https://github.com/ZachOrr/MK3-Swerve-Example
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();
}
//https://github.com/ZachOrr/MK3-Swerve-Example
/**
* Method to drive the robot using joystick info. * Method to drive the robot using joystick info.
* *
* @param xSpeed Speed of the robot in the x direction (forward). * @param xSpeed Speed of the robot in the x direction (forward).
* @param ySpeed Speed of the robot in the y direction (sideways). * @param ySpeed Speed of the robot in the y direction (sideways).
* @param rot Angular rate of the robot. * @param rot Angular rate of the robot.
* @param fieldRelative Whether the provided x and y speeds are relative to the field. * @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) public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
{
/*var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s ); // var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s );
driveFromSpeeds(speeds);*/ // driveFromSpeeds(speeds);
double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND; double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
SwerveModuleState[] states = double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
kinematics.toSwerveModuleStates( SwerveModuleState[] states = kinematics.toSwerveModuleStates(
fieldRelative fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d())
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d()) : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot));
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot)); SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
SwerveDriveKinematics.normalizeWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC)); for (int i = 0; i < states.length; i++) {
for (int i = 0; i < states.length; i++) { SwerveModule module = modules[i];
SwerveModule module = modules[i]; SwerveModuleState state = states[i];
SwerveModuleState state = states[i]; module.setDesiredState(state);
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 // Converts a ChassisSpeed to SwerveModuleStates (targets)
SwerveModuleState leftFront = SwerveModuleState.optimize(moduleStates[0], Rotation2d.fromDegrees(m_leftFrontEncoder.getPosition())); public void driveFromSpeeds(ChassisSpeeds speeds) {
// Front right module state // https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html
SwerveModuleState rightFront = SwerveModuleState.optimize(moduleStates[1], Rotation2d.fromDegrees(m_rightFrontEncoder.getPosition())); // Convert to module states
// Back left module state SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds);
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 // Front left module state
setSwerveMotors(leftFront, leftBack, rightFront, rightBack); 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()));
//Sets steering motors to PID values // Set the motors
public void setSwerveMotors(SwerveModuleState leftFront, SwerveModuleState leftBack, SwerveModuleState rightFront, SwerveModuleState rightBack) setSwerveMotors(leftFront, leftBack, rightFront, rightBack);
{ }
/*//Set the Wheel motor speeds //Sets steering motors to PID values
m_leftFrontWheelMotor.set(m_leftFrontSteerMotor.get() + leftFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); public void setSwerveMotors(SwerveModuleState leftFront, SwerveModuleState leftBack, SwerveModuleState rightFront, SwerveModuleState rightBack)
m_rightFrontWheelMotor.set(m_rightFrontSteerMotor.get() + rightFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); {/*
m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + leftBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); //Set the Wheel motor speeds
m_rightBackWheelMotor.set(m_rightBackSteerMotor.get() + rightBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); m_leftFrontWheelMotor.set(m_leftFrontSteerMotor.get() + leftFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
m_rightFrontWheelMotor.set(m_rightFrontSteerMotor.get() + rightFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + leftBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
m_rightBackWheelMotor.set(m_rightBackSteerMotor.get() + rightBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
//PID //PID
m_leftFrontSteerMotor.set(TalonFXControlMode.Position, leftFront.angle.getDegrees() * 12000); m_leftFrontSteerMotor.set(TalonFXControlMode.Position, leftFront.angle.getDegrees() * 12000);
m_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees() * 12000); m_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees() * 12000);
m_leftBackSteerMotor.set(TalonFXControlMode.Position, leftBack.angle.getDegrees() * 12000); m_leftBackSteerMotor.set(TalonFXControlMode.Position, leftBack.angle.getDegrees() * 12000);
m_rightBackSteerMotor.set(TalonFXControlMode.Position, rightBack.angle.getDegrees()); m_rightBackSteerMotor.set(TalonFXControlMode.Position, rightBack.angle.getDegrees());
System.out.println("Target: " + leftFront.angle.getDegrees());*/ System.out.println("Target: " + leftFront.angle.getDegrees());
} */}
/*public void setSwerveGains(){ /*public void setSwerveGains(){
m_leftFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); 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_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_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_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_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.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_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_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_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_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.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_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_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_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_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.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_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_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_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); 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) // public void driveFieldRelative(double awayFromStation, double towardLeftBoundary, double rotate)
// { // {
// var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary, // var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary, rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/)
// rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/) // }
// }
} }
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+23 -24
View File
@@ -12,22 +12,20 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.CANCoderConfiguration; import com.ctre.phoenix.sensors.CANCoderConfiguration;
import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.kinematics.SwerveModuleState; import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.simulation.EncoderSim; import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.util.Units;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains; import frc4388.utility.Gains;
public class SwerveModule extends SubsystemBase { public class SwerveModule extends SubsystemBase {
private WPI_TalonFX driveMotor; private WPI_TalonFX driveMotor;
private WPI_TalonFX angleMotor; private WPI_TalonFX angleMotor;
private CANCoder canCoder; private CANCoder canCoder;
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
private static double kEncoderTicksPerRotation = 4096;
private static double kEncoderTicksPerRotation = 4096;
/** Creates a new SwerveModule. */ /** Creates a new SwerveModule. */
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder) { public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder) {
@@ -47,30 +45,32 @@ public class SwerveModule extends SubsystemBase {
angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
angleMotor.configAllSettings(angleTalonFXConfiguration); angleMotor.configAllSettings(angleTalonFXConfiguration);
/*TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration(); /*
* TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
driveTalonFXConfiguration.slot0.kP = kDriveP; *
driveTalonFXConfiguration.slot0.kI = kDriveI; * driveTalonFXConfiguration.slot0.kP = kDriveP;
driveTalonFXConfiguration.slot0.kD = kDriveD; * driveTalonFXConfiguration.slot0.kI = kDriveI;
driveTalonFXConfiguration.slot0.kF = kDriveF; * driveTalonFXConfiguration.slot0.kD = kDriveD;
* driveTalonFXConfiguration.slot0.kF = kDriveF;
driveMotor.configAllSettings(driveTalonFXConfiguration);*/ *
* driveMotor.configAllSettings(driveTalonFXConfiguration);
*/
CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration(); CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
//CANCODER CONFIG // CANCODER CONFIG
canCoder.configAllSettings(canCoderConfiguration); canCoder.configAllSettings(canCoderConfiguration);
} }
public Rotation2d getAngle() { public Rotation2d getAngle() {
// Note: This assumes the CANCoders are setup with the default feedback coefficient // Note: This assumes the CANCoders are setup with the default feedback coefficient and the sesnor value reports degrees.
// and the sesnor value reports degrees.
return Rotation2d.fromDegrees(canCoder.getAbsolutePosition()); return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
} }
/** /**
* Set the speed + rotation of the swerve module from a SwerveModuleState object * Set the speed + rotation of the swerve module from a SwerveModuleState object
* @param desiredState - A SwerveModuleState representing the desired new state of the module *
* @param desiredState - A SwerveModuleState representing the desired new state
* of the module
*/ */
public void setDesiredState(SwerveModuleState desiredState) { public void setDesiredState(SwerveModuleState desiredState) {
Rotation2d currentRotation = getAngle(); Rotation2d currentRotation = getAngle();
@@ -86,7 +86,6 @@ public class SwerveModule extends SubsystemBase {
double desiredTicks = currentTicks + deltaTicks; double desiredTicks = currentTicks + deltaTicks;
angleMotor.set(TalonFXControlMode.Position, desiredTicks); angleMotor.set(TalonFXControlMode.Position, desiredTicks);
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.SPEED_FEET_PER_SECOND_AT_FULL_POWER); driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.SPEED_FEET_PER_SECOND_AT_FULL_POWER);
} }
src/main/java/frc4388/utility/Gains.java
+49 -49
View File
@@ -6,55 +6,55 @@ package frc4388.utility;
/** Add your docs here. */ /** Add your docs here. */
public class Gains { public class Gains {
public double m_kP; public double m_kP;
public double m_kI; public double m_kI;
public double m_kD; public double m_kD;
public double m_kF; public double m_kF;
public int m_kIzone; public int m_kIzone;
public double m_kPeakOutput; public double m_kPeakOutput;
public double m_kmaxOutput; public double m_kmaxOutput;
public double m_kminOutput; public double m_kminOutput;
/** /**
* Creates Gains object for PIDs * Creates Gains object for PIDs
* @param kP The P value. * @param kP The P value.
* @param kI The I value. * @param kI The I value.
* @param kD The D value. * @param kD The D value.
* @param kF The F value. * @param kF The F value.
* @param kIzone The zone of the I 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. * @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) public Gains(double kP, double kI, double kD, double kF, int kIzone, double kPeakOutput)
{ {
m_kP = kP; m_kP = kP;
m_kI = kI; m_kI = kI;
m_kD = kD; m_kD = kD;
m_kF = kF; m_kF = kF;
m_kIzone = kIzone; m_kIzone = kIzone;
m_kPeakOutput = kPeakOutput; m_kPeakOutput = kPeakOutput;
m_kmaxOutput = m_kPeakOutput; m_kmaxOutput = m_kPeakOutput;
m_kminOutput = -m_kPeakOutput; m_kminOutput = -m_kPeakOutput;
} }
/** /**
* Creates Gains object for PIDs * Creates Gains object for PIDs
* @param kP The P value. * @param kP The P value.
* @param kI The I value. * @param kI The I value.
* @param kD The D value. * @param kD The D value.
* @param kF The F value. * @param kF The F value.
* @param kIzone The zone of the I 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 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. * @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) public Gains(double kP, double kI, double kD, double kF, int kIzone, double kMinOutput, double kMaxOutput)
{ {
m_kP = kP; m_kP = kP;
m_kI = kI; m_kI = kI;
m_kD = kD; m_kD = kD;
m_kF = kF; m_kF = kF;
m_kIzone = kIzone; m_kIzone = kIzone;
m_kminOutput = kMinOutput; m_kminOutput = kMinOutput;
m_kmaxOutput = kMaxOutput; m_kmaxOutput = kMaxOutput;
m_kPeakOutput = (Math.abs(m_kminOutput) > Math.abs(m_kmaxOutput)) ? Math.abs(m_kminOutput) : Math.abs(m_kmaxOutput); m_kPeakOutput = (Math.abs(m_kminOutput) > Math.abs(m_kmaxOutput)) ? Math.abs(m_kminOutput) : Math.abs(m_kmaxOutput);
} }
} }
src/main/java/frc4388/utility/RobotGyro.java
-180
View File
@@ -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;
}
/**
* <p>NavX:
* <p>Calibrate the gyro by running for a number of samples and computing the center value. Then use
* the center value as the Accumulator center value for subsequent measurements. It's important to
* make sure that the robot is not moving while the centering calculations are in progress, this
* is typically done when the robot is first turned on while it's sitting at rest before the
* competition starts.
*
* <p>Pigeon:
* <p>Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot
* into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon
* has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to
* make sure that the robot is not moving while the tempurature calculations are in progress, this
* is typically done when the robot is first turned on while it's sitting at rest before the
* competition starts.
*/
@Override
public void calibrate() {
if (m_isGyroAPigeon) {
m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
} else {
m_navX.calibrate();
}
}
@Override
public void reset() {
if (m_isGyroAPigeon) {
m_pigeon.setYaw(0);
} else {
m_navX.reset();
}
}
/**
* Get Yaw, Pitch, and Roll data.
*
* @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
* Yaw is within [-368,640, +368,640] degrees.
* Pitch is within [-90,+90] degrees.
* Roll is within [-90,+90] degrees.
*/
private double[] getPigeonAngles() {
double[] angles = new double[3];
m_pigeon.getYawPitchRoll(angles);
return angles;
}
@Override
public double getAngle() {
if (m_isGyroAPigeon) {
return getPigeonAngles()[0];
} else {
return m_navX.getAngle();
}
}
/**
* Gets an absolute heading of the robot
* @return heading from -180 to 180 degrees
*/
public double getHeading() {
return getHeading(getAngle());
}
/**
* Gets an absolute heading of the robot
* @return heading from -180 to 180 degrees
*/
public double getHeading(double angle) {
return Math.IEEEremainder(angle, 360);
}
/**
* Returns the current pitch value (in degrees, from -90 to 90)
* reported by the sensor. Pitch is a measure of rotation around
* the Y Axis.
* @return The current pitch value in degrees (-90 to 90).
*/
public double getPitch() {
if (m_isGyroAPigeon) {
return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
} else {
return MathUtil.clamp(m_navX.getPitch(), -90, 90);
}
}
/**
* Returns the current roll value (in degrees, from -90 to 90)
* reported by the sensor. Roll is a measure of rotation around
* the X Axis.
* @return The current roll value in degrees (-90 to 90).
*/
public double getRoll() {
if (m_isGyroAPigeon) {
return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
} else {
return MathUtil.clamp(m_navX.getRoll(), -90, 90);
}
}
@Override
public double getRate() {
if (m_isGyroAPigeon) {
return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
} else {
return m_navX.getRate();
}
}
public PigeonIMU getPigeon(){
return m_pigeon;
}
public AHRS getNavX(){
return m_navX;
}
@Override
public void close() throws Exception {
}
}
src/main/java/frc4388/utility/RobotGyro.java.old
+245
View File
@@ -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;
}
/**
* <p>
* NavX:
* <p>
* Calibrate the gyro by running for a number of samples and computing the
* center value. Then use
* the center value as the Accumulator center value for subsequent measurements.
* It's important to
* make sure that the robot is not moving while the centering calculations are
* in progress, this
* is typically done when the robot is first turned on while it's sitting at
* rest before the
* competition starts.
*
* <p>
* Pigeon:
* <p>
* Calibrate the gyro by collecting data at a range of tempuratures. Allow
* pigeon to cool, then boot
* into calibration mode. For faster calibration, use a heat lamp to heat up the
* pigeon. Once the pigeon
* has seen a reasonable range of tempuratures, it will exit calibration mode.
* It's important to
* make sure that the robot is not moving while the tempurature calculations are
* in progress, this
* is typically done when the robot is first turned on while it's sitting at
* rest before the
* competition starts.
*/
@Override
public void calibrate() {
if (m_isGyroAPigeon)
m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
else
m_navX.calibrate();
}
@Override
public void reset() {
if (m_isGyroAPigeon)
m_pigeon.setYaw(0);
else
m_navX.reset();
}
/**
* Get Yaw, Pitch, and Roll data.
*
* @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
* Yaw is within [-368,640, +368,640] degrees.
* Pitch is within [-90,+90] degrees.
* Roll is within [-90,+90] degrees.
*/
private double[] getPigeonAngles() {
double[] angles = new double[3];
m_pigeon.getYawPitchRoll(angles);
return angles;
}
@Override
public double getAngle() {
if (m_isGyroAPigeon) {
return getPigeonAngles()[0];
} else {
return m_navX.getAngle();
}
}
/**
* Gets an absolute heading of the robot
*
* @return heading from -180 to 180 degrees
*/
public double getHeading() {
return getHeading(getAngle());
}
/**
* Gets an absolute heading of the robot
*
* @return heading from -180 to 180 degrees
*/
public double getHeading(double angle) {
return Math.IEEEremainder(angle, 360);
}
/**
* Returns the current pitch value (in degrees, from -90 to 90)
* reported by the sensor. Pitch is a measure of rotation around
* the Y Axis.
*
* @return The current pitch value in degrees (-90 to 90).
*/
public double getPitch() {
if (m_isGyroAPigeon) {
return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
} else {
return MathUtil.clamp(m_navX.getPitch(), -90, 90);
}
}
/**
* Returns the current roll value (in degrees, from -90 to 90)
* reported by the sensor. Roll is a measure of rotation around
* the X Axis.
*
* @return The current roll value in degrees (-90 to 90).
*/
public double getRoll() {
if (m_isGyroAPigeon) {
return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
} else {
return MathUtil.clamp(m_navX.getRoll(), -90, 90);
}
}
@Override
public double getRate() {
if (m_isGyroAPigeon) {
return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
} else {
return m_navX.getRate();
}
}
public PigeonIMU getPigeon() {
return m_pigeon;
}
public AHRS getNavX() {
return m_navX;
}
@Override
public void close() throws Exception {
}
// Begin old GyroBase class
private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
/**
* Set which parameter of the gyro you are using as a process control variable.
* The Gyro class
* supports the rate and displacement parameters
*
* @param pidSource An enum to select the parameter.
*/
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the output of the gyro for use with PIDControllers. May be the angle or
* rate depending on
* the set PIDSourceType
*
* @return the output according to the gyro
*/
@Override
public double pidGet() {
switch (m_pidSource) {
case kRate:
return getRate();
case kDisplacement:
return getAngle();
default:
return 0.0;
}
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Gyro");
builder.addDoubleProperty("Value", this::getAngle, null);
}
}
src/main/java/frc4388/utility/RobotTime.java
+53 -56
View File
@@ -1,9 +1,6 @@
/*----------------------------------------------------------------------------*/ // Copyright (c) FIRST and other WPILib contributors.
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ // Open Source Software; you can modify and/or share it under the terms of
/* Open Source Software - may be modified and shared by FRC teams. The code */ // the WPILib BSD license file in the root directory of this project.
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.utility; package frc4388.utility;
@@ -12,68 +9,68 @@ package frc4388.utility;
* <p>All times are in milliseconds * <p>All times are in milliseconds
*/ */
public class RobotTime { public class RobotTime {
private long m_currTime = System.currentTimeMillis(); private long m_currTime = System.currentTimeMillis();
public long m_deltaTime = 0; public long m_deltaTime = 0;
private long m_startRobotTime = m_currTime; private long m_startRobotTime = m_currTime;
public long m_robotTime = 0; public long m_robotTime = 0;
public long m_lastRobotTime = 0; public long m_lastRobotTime = 0;
private long m_startMatchTime = 0; private long m_startMatchTime = 0;
public long m_matchTime = 0; public long m_matchTime = 0;
public long m_lastMatchTime = 0; public long m_lastMatchTime = 0;
public long m_frameNumber = 0; public long m_frameNumber = 0;
/** /**
* Private constructor prevents other classes from instantiating * Private constructor prevents other classes from instantiating
*/ */
private RobotTime(){} 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. * Gets the instance of Robot Time. If there is no instance running one will be created.
* @return instance of Robot Time * @return instance of Robot Time
*/ */
public static RobotTime getInstance() { public static RobotTime getInstance() {
if (instance == null) { if (instance == null) {
instance = new RobotTime(); instance = new RobotTime();
}
return instance;
} }
return instance;
}
/** /**
* Call this once per periodic loop. * Call this once per periodic loop.
*/ */
public void updateTimes() { public void updateTimes() {
m_lastRobotTime = m_robotTime; m_lastRobotTime = m_robotTime;
m_lastMatchTime = m_matchTime; m_lastMatchTime = m_matchTime;
m_currTime = System.currentTimeMillis(); m_currTime = System.currentTimeMillis();
m_robotTime = m_currTime - m_startRobotTime; m_robotTime = m_currTime - m_startRobotTime;
m_deltaTime = m_robotTime - m_lastRobotTime; m_deltaTime = m_robotTime - m_lastRobotTime;
m_frameNumber++; m_frameNumber++;
if (m_startMatchTime != 0) { if (m_startMatchTime != 0) {
m_matchTime = m_currTime - m_startMatchTime; m_matchTime = m_currTime - m_startMatchTime;
}
} }
}
/** /**
* Call this in both the auto and periodic inits * Call this in both the auto and periodic inits
*/ */
public void startMatchTime() { public void startMatchTime() {
if (m_startMatchTime == 0) { if (m_startMatchTime == 0) {
m_startMatchTime = m_currTime; m_startMatchTime = m_currTime;
}
} }
}
/** /**
* Call this in disabled init * Call this in disabled init
*/ */
public void endMatchTime() { public void endMatchTime() {
m_startMatchTime = 0; m_startMatchTime = 0;
m_matchTime = 0; m_matchTime = 0;
} }
} }
src/main/java/frc4388/utility/controller/IHandController.java
+7 -7
View File
@@ -5,17 +5,17 @@ package frc4388.utility.controller;
*/ */
public interface IHandController { public interface IHandController {
public double getLeftXAxis(); public double getLeftXAxis();
public double getLeftYAxis(); public double getLeftYAxis();
public double getRightXAxis(); public double getRightXAxis();
public double getRightYAxis(); public double getRightYAxis();
public double getLeftTriggerAxis(); public double getLeftTriggerAxis();
public double getRightTriggerAxis(); public double getRightTriggerAxis();
public int getDpadAngle(); public int getDpadAngle();
} }
src/main/java/frc4388/utility/controller/XboxController.java
+155 -155
View File
@@ -9,210 +9,210 @@ import edu.wpi.first.wpilibj.Joystick;
*/ */
public class XboxController implements IHandController public class XboxController implements IHandController
{ {
public static final int LEFT_X_AXIS = 0; public static final int LEFT_X_AXIS = 0;
public static final int LEFT_Y_AXIS = 1; public static final int LEFT_Y_AXIS = 1;
public static final int LEFT_TRIGGER_AXIS = 2; public static final int LEFT_TRIGGER_AXIS = 2;
public static final int RIGHT_TRIGGER_AXIS = 3; public static final int RIGHT_TRIGGER_AXIS = 3;
public static final int RIGHT_X_AXIS = 4; public static final int RIGHT_X_AXIS = 4;
public static final int RIGHT_Y_AXIS = 5; public static final int RIGHT_Y_AXIS = 5;
public static final int LEFT_RIGHT_DPAD_AXIS = 6; public static final int LEFT_RIGHT_DPAD_AXIS = 6;
public static final int TOP_BOTTOM_DPAD_AXIS = 6; public static final int TOP_BOTTOM_DPAD_AXIS = 6;
public static final int A_BUTTON = 1; public static final int A_BUTTON = 1;
public static final int B_BUTTON = 2; public static final int B_BUTTON = 2;
public static final int X_BUTTON = 3; public static final int X_BUTTON = 3;
public static final int Y_BUTTON = 4; public static final int Y_BUTTON = 4;
public static final int LEFT_BUMPER_BUTTON = 5; public static final int LEFT_BUMPER_BUTTON = 5;
public static final int RIGHT_BUMPER_BUTTON = 6; public static final int RIGHT_BUMPER_BUTTON = 6;
public static final int BACK_BUTTON = 7; public static final int BACK_BUTTON = 7;
public static final int START_BUTTON = 8; public static final int START_BUTTON = 8;
public static final int LEFT_JOYSTICK_BUTTON = 9; public static final int LEFT_JOYSTICK_BUTTON = 9;
public static final int RIGHT_JOYSTICK_BUTTON = 10; public static final int RIGHT_JOYSTICK_BUTTON = 10;
private static final double LEFT_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 RIGHT_DPAD_TOLERANCE = 0.9;
private static final double BOTTOM_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 TOP_DPAD_TOLERANCE = 0.9;
private static final double LEFT_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_TRIGGER_TOLERANCE = 0.5;
private static final double RIGHT_AXIS_UP_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_DOWN_TOLERANCE = 0.9;
private static final double RIGHT_AXIS_RIGHT_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_LEFT_TOLERANCE = -0.9;
private static final double LEFT_AXIS_UP_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_DOWN_TOLERANCE = 0.9;
private static final double LEFT_AXIS_RIGHT_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_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. * Add your docs here.
*/ */
public XboxController(int portNumber){ public XboxController(int portNumber){
m_stick = new Joystick(portNumber); m_stick = new Joystick(portNumber);
} }
/** /**
* Add your docs here. * Add your docs here.
*/ */
public Joystick getJoyStick() { public Joystick getJoyStick() {
return m_stick; return m_stick;
} }
/** /**
* Checks if the input falls within the deadzone. * Checks if the input falls within the deadzone.
* @param input from an axis on the controller * @param input from an axis on the controller
* @return true if input falls in deadzone, false if input falls outside deadzone * @return true if input falls in deadzone, false if input falls outside deadzone
*/ */
private boolean inDeadZone(double input){ private boolean inDeadZone(double input){
return (Math.abs(input) < DEADZONE); return (Math.abs(input) < DEADZONE);
} }
/** /**
* Updates an input to have a deadzone around the 0 position * Updates an input to have a deadzone around the 0 position
* @param input from an axis on the controller * @param input from an axis on the controller
* @return updated input * @return updated input
*/ */
private double getAxisWithDeadZoneCheck(double input){ private double getAxisWithDeadZoneCheck(double input){
if(inDeadZone(input)){ if(inDeadZone(input)){
return 0.0; return 0.0;
} else { } else {
return input; return input;
} }
} }
public boolean getAButton(){ public boolean getAButton(){
return m_stick.getRawButton(A_BUTTON); return m_stick.getRawButton(A_BUTTON);
} }
public boolean getXButton(){ public boolean getXButton(){
return m_stick.getRawButton(X_BUTTON); return m_stick.getRawButton(X_BUTTON);
} }
public boolean getBButton(){ public boolean getBButton(){
return m_stick.getRawButton(B_BUTTON); return m_stick.getRawButton(B_BUTTON);
} }
public boolean getYButton(){ public boolean getYButton(){
return m_stick.getRawButton(Y_BUTTON); return m_stick.getRawButton(Y_BUTTON);
} }
public boolean getBackButton(){ public boolean getBackButton(){
return m_stick.getRawButton(BACK_BUTTON); return m_stick.getRawButton(BACK_BUTTON);
} }
public boolean getStartButton(){ public boolean getStartButton(){
return m_stick.getRawButton(START_BUTTON); return m_stick.getRawButton(START_BUTTON);
} }
public boolean getLeftBumperButton(){ public boolean getLeftBumperButton(){
return m_stick.getRawButton(LEFT_BUMPER_BUTTON); return m_stick.getRawButton(LEFT_BUMPER_BUTTON);
} }
public boolean getRightBumperButton(){ public boolean getRightBumperButton(){
return m_stick.getRawButton(RIGHT_BUMPER_BUTTON); return m_stick.getRawButton(RIGHT_BUMPER_BUTTON);
} }
public boolean getLeftJoystickButton(){ public boolean getLeftJoystickButton(){
return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON); return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON);
} }
public boolean getRightJoystickButton(){ public boolean getRightJoystickButton(){
return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON); return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
} }
public double getLeftXAxis(){ public double getLeftXAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS)); return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS));
} }
public double getLeftYAxis(){ public double getLeftYAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS)); return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS));
} }
public double getRightXAxis(){ public double getRightXAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS)); return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS));
} }
public double getRightYAxis(){ public double getRightYAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS)); return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS));
} }
public double getLeftTriggerAxis(){ public double getLeftTriggerAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS)); return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS));
} }
public double getRightTriggerAxis(){ public double getRightTriggerAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS)); return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS));
} }
/** /**
* Get the angle input from the dpad. * 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. * @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc.
*/ */
public int getDpadAngle() { public int getDpadAngle() {
return m_stick.getPOV(0); return m_stick.getPOV(0);
} }
public boolean getDPadLeft(){ public boolean getDPadLeft(){
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE); 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); 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); 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); return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE);
} }
public boolean getLeftTrigger(){ public boolean getLeftTrigger(){
return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE); return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
} }
public boolean getRightTrigger(){ public boolean getRightTrigger(){
return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE); return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
} }
public boolean getRightAxisUpTrigger(){ public boolean getRightAxisUpTrigger(){
return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE); return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
} }
public boolean getRightAxisDownTrigger(){ public boolean getRightAxisDownTrigger(){
return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE); return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
} }
public boolean getRightAxisLeftTrigger(){ public boolean getRightAxisLeftTrigger(){
return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE); return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
} }
public boolean getRightAxisRightTrigger(){ public boolean getRightAxisRightTrigger(){
return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE); return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
} }
public boolean getLeftAxisUpTrigger(){ public boolean getLeftAxisUpTrigger(){
return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE); return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE);
} }
public boolean getLeftAxisDownTrigger(){ public boolean getLeftAxisDownTrigger(){
return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE); return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE);
} }
public boolean getLeftAxisLeftTrigger(){ public boolean getLeftAxisLeftTrigger(){
return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE); return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE);
} }
public boolean getLeftAxisRightTrigger(){ public boolean getLeftAxisRightTrigger(){
return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE); return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE);
} }
} }
src/main/java/frc4388/utility/controller/XboxTriggerButton.java
+36 -55
View File
@@ -1,7 +1,6 @@
package frc4388.utility.controller; package frc4388.utility.controller;
import edu.wpi.first.wpilibj2.command.button.Button; 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 * 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}. * exceeds a tolerance defined in {@link XboxController}.
*/ */
public class XboxTriggerButton extends Button { public class XboxTriggerButton extends Button {
public static final int RIGHT_TRIGGER = 0; public static final int RIGHT_TRIGGER = 0;
public static final int LEFT_TRIGGER = 1; public static final int LEFT_TRIGGER = 1;
public static final int RIGHT_AXIS_UP_TRIGGER = 2; public static final int RIGHT_AXIS_UP_TRIGGER = 2;
public static final int RIGHT_AXIS_DOWN_TRIGGER = 3; public static final int RIGHT_AXIS_DOWN_TRIGGER = 3;
public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4; public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4;
public static final int RIGHT_AXIS_LEFT_TRIGGER = 5; public static final int RIGHT_AXIS_LEFT_TRIGGER = 5;
public static final int LEFT_AXIS_UP_TRIGGER = 6; public static final int LEFT_AXIS_UP_TRIGGER = 6;
public static final int LEFT_AXIS_DOWN_TRIGGER = 7; public static final int LEFT_AXIS_DOWN_TRIGGER = 7;
public static final int LEFT_AXIS_RIGHT_TRIGGER = 8; public static final int LEFT_AXIS_RIGHT_TRIGGER = 8;
public static final int LEFT_AXIS_LEFT_TRIGGER = 9; public static final int LEFT_AXIS_LEFT_TRIGGER = 9;
private XboxController m_controller; private XboxController m_controller;
private int m_trigger; private int m_trigger;
/** /**
* Creates a Trigger-Button mapped to a specific Xbox controller and trigger * Creates a Trigger-Button mapped to a specific Xbox controller and trigger
*/ */
public XboxTriggerButton(XboxController controller, int trigger) { public XboxTriggerButton(XboxController controller, int trigger) {
m_controller = controller; m_controller = controller;
m_trigger = trigger; m_trigger = trigger;
} }
/** {@inheritDoc} */ /** {@inheritDoc} */
@Override @Override
public boolean get() { public boolean get() {
if (m_trigger == RIGHT_TRIGGER) { switch (m_trigger) {
return m_controller.getRightTrigger(); case RIGHT_TRIGGER: return m_controller.getRightTrigger();
} case LEFT_TRIGGER: return m_controller.getLeftTrigger();
else if (m_trigger == LEFT_TRIGGER) { case RIGHT_AXIS_UP_TRIGGER: return m_controller.getRightAxisUpTrigger();
return m_controller.getLeftTrigger(); case RIGHT_AXIS_DOWN_TRIGGER: return m_controller.getRightAxisDownTrigger();
} case RIGHT_AXIS_RIGHT_TRIGGER: return m_controller.getRightAxisRightTrigger();
else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) { case RIGHT_AXIS_LEFT_TRIGGER: return m_controller.getRightAxisLeftTrigger();
return m_controller.getRightAxisUpTrigger(); case LEFT_AXIS_UP_TRIGGER: return m_controller.getLeftAxisUpTrigger();
} case LEFT_AXIS_DOWN_TRIGGER: return m_controller.getLeftAxisDownTrigger();
else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) { case LEFT_AXIS_RIGHT_TRIGGER: return m_controller.getLeftAxisRightTrigger();
return m_controller.getRightAxisDownTrigger(); case LEFT_AXIS_LEFT_TRIGGER: return m_controller.getLeftAxisLeftTrigger();
} default: return false;
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;
}
} }
src/test/java/frc4388/mocks/MockPigeonIMU.java
+35 -38
View File
@@ -1,9 +1,6 @@
/*----------------------------------------------------------------------------*/ // Copyright (c) FIRST and other WPILib contributors.
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ // Open Source Software; you can modify and/or share it under the terms of
/* Open Source Software - may be modified and shared by FRC teams. The code */ // the WPILib BSD license file in the root directory of this project.
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.mocks; package frc4388.mocks;
@@ -14,41 +11,41 @@ import com.ctre.phoenix.sensors.PigeonIMU;
* Add your docs here. * Add your docs here.
*/ */
public class MockPigeonIMU extends PigeonIMU { public class MockPigeonIMU extends PigeonIMU {
public int m_deviceNumber; public int m_deviceNumber;
public double currentYaw; public double currentYaw;
public double currentPitch; public double currentPitch;
public double currentRoll; public double currentRoll;
public MockPigeonIMU(int deviceNumber) { public MockPigeonIMU(int deviceNumber) {
super(deviceNumber); super(deviceNumber);
m_deviceNumber = deviceNumber; m_deviceNumber = deviceNumber;
} }
@Override @Override
public ErrorCode setYaw(double angleDeg) { public ErrorCode setYaw(double angleDeg) {
currentYaw = angleDeg; currentYaw = angleDeg;
return ErrorCode.OK; return ErrorCode.OK;
} }
/** /**
* @param currentPitch the Pitch to set * @param currentPitch the Pitch to set
*/ */
public void setCurrentPitch(double currentPitch) { public void setCurrentPitch(double currentPitch) {
this.currentPitch = currentPitch; this.currentPitch = currentPitch;
} }
/** /**
* @param currentRoll the Roll to set * @param currentRoll the Roll to set
*/ */
public void setCurrentRoll(double currentRoll) { public void setCurrentRoll(double currentRoll) {
this.currentRoll = currentRoll; this.currentRoll = currentRoll;
} }
@Override @Override
public ErrorCode getYawPitchRoll(double[] ypr_deg) { public ErrorCode getYawPitchRoll(double[] ypr_deg) {
ypr_deg[0] = currentYaw; ypr_deg[0] = currentYaw;
ypr_deg[1] = currentPitch; ypr_deg[1] = currentPitch;
ypr_deg[2] = currentRoll; ypr_deg[2] = currentRoll;
return ErrorCode.OK; return ErrorCode.OK;
} }
} }
src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java
+31 -34
View File
@@ -1,9 +1,6 @@
/*----------------------------------------------------------------------------*/ // Copyright (c) FIRST and other WPILib contributors.
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ // Open Source Software; you can modify and/or share it under the terms of
/* Open Source Software - may be modified and shared by FRC teams. The code */ // the WPILib BSD license file in the root directory of this project.
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.subsystems; package frc4388.robot.subsystems;
@@ -12,7 +9,7 @@ import static org.mockito.Mockito.mock;
import org.junit.Test; import org.junit.Test;
import edu.wpi.first.wpilibj.*; import edu.wpi.first.wpilibj.motorcontrol.Spark;
import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.LEDConstants;
import frc4388.utility.LEDPatterns; import frc4388.utility.LEDPatterns;
@@ -20,40 +17,40 @@ import frc4388.utility.LEDPatterns;
* Add your docs here. * Add your docs here.
*/ */
public class LEDSubsystemTest { public class LEDSubsystemTest {
@Test @Test
public void testConstructor() { public void testConstructor() {
// Arrange // Arrange
Spark ledController = mock(Spark.class); Spark ledController = mock(Spark.class);
// Act // Act
LED led = new LED(ledController); LED led = new LED(ledController);
// Assert // Assert
assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001); assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001);
} }
@Test @Test
public void testPatterns() { public void testPatterns() {
// Arrange // Arrange
Spark ledController = mock(Spark.class); Spark ledController = mock(Spark.class);
LED led = new LED(ledController); LED led = new LED(ledController);
// Act // Act
led.setPattern(LEDPatterns.RAINBOW_RAINBOW); led.setPattern(LEDPatterns.RAINBOW_RAINBOW);
// Assert // Assert
assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001); assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001);
// Act // Act
led.setPattern(LEDPatterns.BLUE_BREATH); led.setPattern(LEDPatterns.BLUE_BREATH);
// Assert // Assert
assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001); assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001);
// Act // Act
led.setPattern(LEDPatterns.SOLID_BLACK); led.setPattern(LEDPatterns.SOLID_BLACK);
// Assert // Assert
assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001); assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001);
} }
} }
src/test/java/frc4388/utility/RobotGyroUtilityTest.java → src/test/java/frc4388/utility/RobotGyroUtilityTest.java.old
+3 -6
View File
@@ -1,9 +1,6 @@
/*----------------------------------------------------------------------------*/ // Copyright (c) FIRST and other WPILib contributors.
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ // Open Source Software; you can modify and/or share it under the terms of
/* Open Source Software - may be modified and shared by FRC teams. The code */ // the WPILib BSD license file in the root directory of this project.
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.utility; package frc4388.utility;
src/test/java/frc4388/utility/RobotTimeUtilityTest.java
+74 -77
View File
@@ -1,9 +1,6 @@
/*----------------------------------------------------------------------------*/ // Copyright (c) FIRST and other WPILib contributors.
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ // Open Source Software; you can modify and/or share it under the terms of
/* Open Source Software - may be modified and shared by FRC teams. The code */ // the WPILib BSD license file in the root directory of this project.
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.utility; package frc4388.utility;
@@ -15,90 +12,90 @@ import org.junit.*;
* Add your docs here. * Add your docs here.
*/ */
public class RobotTimeUtilityTest { public class RobotTimeUtilityTest {
RobotTime robotTime = RobotTime.getInstance(); RobotTime robotTime = RobotTime.getInstance();
@Test @Test
public void testUpdateTimes() { public void testUpdateTimes() {
// Arrange // Arrange
long lastTime; long lastTime;
robotTime.m_deltaTime = 0; robotTime.m_deltaTime = 0;
robotTime.m_robotTime = 0; robotTime.m_robotTime = 0;
robotTime.m_lastRobotTime = 0; robotTime.m_lastRobotTime = 0;
robotTime.m_frameNumber = 0; robotTime.m_frameNumber = 0;
robotTime.endMatchTime(); robotTime.endMatchTime();
robotTime.m_lastMatchTime = 0; robotTime.m_lastMatchTime = 0;
// Assert // Assert
assertEquals(0, robotTime.m_deltaTime); assertEquals(0, robotTime.m_deltaTime);
assertEquals(0, robotTime.m_robotTime); assertEquals(0, robotTime.m_robotTime);
assertEquals(0, robotTime.m_lastRobotTime); assertEquals(0, robotTime.m_lastRobotTime);
assertEquals(0, robotTime.m_frameNumber); assertEquals(0, robotTime.m_frameNumber);
lastTime = robotTime.m_robotTime; lastTime = robotTime.m_robotTime;
// Act // Act
wait(1); wait(1);
robotTime.updateTimes(); robotTime.updateTimes();
// Assert // Assert
assertEquals(true, robotTime.m_deltaTime > 0); assertEquals(true, robotTime.m_deltaTime > 0);
assertEquals(true, robotTime.m_robotTime > 0); assertEquals(true, robotTime.m_robotTime > 0);
assertEquals(lastTime, robotTime.m_lastRobotTime); assertEquals(lastTime, robotTime.m_lastRobotTime);
assertEquals(1, robotTime.m_frameNumber); assertEquals(1, robotTime.m_frameNumber);
lastTime = robotTime.m_robotTime; lastTime = robotTime.m_robotTime;
// Act // Act
wait(1); wait(1);
robotTime.updateTimes(); robotTime.updateTimes();
// Assert // Assert
assertEquals(true, robotTime.m_deltaTime > 0); assertEquals(true, robotTime.m_deltaTime > 0);
assertEquals(true, robotTime.m_robotTime > 0); assertEquals(true, robotTime.m_robotTime > 0);
assertEquals(lastTime, robotTime.m_lastRobotTime); assertEquals(lastTime, robotTime.m_lastRobotTime);
assertEquals(2, robotTime.m_frameNumber); assertEquals(2, robotTime.m_frameNumber);
} }
@Test @Test
public void testMatchTime() { public void testMatchTime() {
// Arrange // Arrange
long lastTime; long lastTime;
// Assert // Assert
assertEquals(0, robotTime.m_matchTime); assertEquals(0, robotTime.m_matchTime);
assertEquals(0, robotTime.m_lastMatchTime); assertEquals(0, robotTime.m_lastMatchTime);
lastTime = robotTime.m_matchTime; lastTime = robotTime.m_matchTime;
// Act // Act
robotTime.startMatchTime(); robotTime.startMatchTime();
wait(1); wait(1);
robotTime.updateTimes(); robotTime.updateTimes();
// Assert // Assert
assertEquals(true, robotTime.m_matchTime > 0); assertEquals(true, robotTime.m_matchTime > 0);
assertEquals(lastTime, robotTime.m_lastMatchTime); assertEquals(lastTime, robotTime.m_lastMatchTime);
lastTime = robotTime.m_matchTime; lastTime = robotTime.m_matchTime;
// Act // Act
wait(1); wait(1);
robotTime.updateTimes(); robotTime.updateTimes();
robotTime.endMatchTime(); robotTime.endMatchTime();
// Assert // Assert
assertEquals(0, robotTime.m_matchTime); assertEquals(0, robotTime.m_matchTime);
assertEquals(lastTime, robotTime.m_lastMatchTime); assertEquals(lastTime, robotTime.m_lastMatchTime);
lastTime = robotTime.m_matchTime; lastTime = robotTime.m_matchTime;
// Act // Act
wait(1); wait(1);
robotTime.updateTimes(); robotTime.updateTimes();
// Assert // Assert
assertEquals(0, robotTime.m_matchTime); assertEquals(0, robotTime.m_matchTime);
assertEquals(lastTime, robotTime.m_lastMatchTime); assertEquals(lastTime, robotTime.m_lastMatchTime);
} }
private void wait(int millis) { private void wait(int millis) {
try { try {
Thread.sleep(millis); Thread.sleep(millis);
} catch (Exception e) {} } catch (Exception e) {}
} }
} }
template.config.js
-31
View File
@@ -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;
}
};
vendordeps/Phoenix.json
+128 -85
View File
@@ -1,29 +1,30 @@
{ {
"fileName": "Phoenix.json", "fileName": "Phoenix.json",
"name": "CTRE-Phoenix", "name": "CTRE-Phoenix",
"version": "5.19.4", "version": "5.20.2",
"frcYear": 2022,
"uuid": "ab676553-b602-441f-a38d-f1296eff6537", "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
"mavenUrls": [ "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": [ "javaDependencies": [
{ {
"groupId": "com.ctre.phoenix", "groupId": "com.ctre.phoenix",
"artifactId": "api-java", "artifactId": "api-java",
"version": "5.19.4" "version": "5.20.2"
}, },
{ {
"groupId": "com.ctre.phoenix", "groupId": "com.ctre.phoenix",
"artifactId": "wpiapi-java", "artifactId": "wpiapi-java",
"version": "5.19.4" "version": "5.20.2"
} }
], ],
"jniDependencies": [ "jniDependencies": [
{ {
"groupId": "com.ctre.phoenix", "groupId": "com.ctre.phoenix",
"artifactId": "cci", "artifactId": "cci",
"version": "5.19.4", "version": "5.20.2",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -33,7 +34,7 @@
{ {
"groupId": "com.ctre.phoenix.sim", "groupId": "com.ctre.phoenix.sim",
"artifactId": "cci-sim", "artifactId": "cci-sim",
"version": "5.19.4", "version": "5.20.2",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -45,7 +46,19 @@
{ {
"groupId": "com.ctre.phoenix.sim", "groupId": "com.ctre.phoenix.sim",
"artifactId": "simTalonSRX", "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, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -57,7 +70,31 @@
{ {
"groupId": "com.ctre.phoenix.sim", "groupId": "com.ctre.phoenix.sim",
"artifactId": "simVictorSPX", "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, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -71,52 +108,74 @@
{ {
"groupId": "com.ctre.phoenix", "groupId": "com.ctre.phoenix",
"artifactId": "wpiapi-cpp", "artifactId": "wpiapi-cpp",
"version": "5.19.4", "version": "5.20.2",
"libName": "CTRE_Phoenix_WPI", "libName": "CTRE_Phoenix_WPI",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": false, "sharedLibrary": true,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"binaryPlatforms": [ "binaryPlatforms": [
"linuxathena", "linuxathena"
"windowsx86-64",
"linuxx86-64",
"osxx86-64"
] ]
}, },
{ {
"groupId": "com.ctre.phoenix", "groupId": "com.ctre.phoenix",
"artifactId": "api-cpp", "artifactId": "api-cpp",
"version": "5.19.4", "version": "5.20.2",
"libName": "CTRE_Phoenix", "libName": "CTRE_Phoenix",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": false, "sharedLibrary": true,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"binaryPlatforms": [ "binaryPlatforms": [
"linuxathena", "linuxathena"
"windowsx86-64",
"linuxx86-64",
"osxx86-64"
] ]
}, },
{ {
"groupId": "com.ctre.phoenix", "groupId": "com.ctre.phoenix",
"artifactId": "cci", "artifactId": "cci",
"version": "5.19.4", "version": "5.20.2",
"libName": "CTRE_PhoenixCCI", "libName": "CTRE_PhoenixCCI",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": false, "sharedLibrary": true,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"binaryPlatforms": [ "binaryPlatforms": [
"linuxathena" "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", "groupId": "com.ctre.phoenix.sim",
"artifactId": "cci-sim", "artifactId": "cci-sim",
"version": "5.19.4", "version": "5.20.2",
"libName": "CTRE_PhoenixCCISim", "libName": "CTRE_PhoenixCCISim",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": false, "sharedLibrary": true,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"binaryPlatforms": [ "binaryPlatforms": [
"windowsx86-64", "windowsx86-64",
@@ -124,68 +183,10 @@
"osxx86-64" "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", "groupId": "com.ctre.phoenix.sim",
"artifactId": "simTalonSRX", "artifactId": "simTalonSRX",
"version": "5.19.4", "version": "5.20.2",
"libName": "CTRE_SimTalonSRX", "libName": "CTRE_SimTalonSRX",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -196,10 +197,24 @@
"osxx86-64" "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", "groupId": "com.ctre.phoenix.sim",
"artifactId": "simVictorSPX", "artifactId": "simVictorSPX",
"version": "5.19.4", "version": "5.20.2",
"libName": "CTRE_SimVictorSPX", "libName": "CTRE_SimVictorSPX",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -209,6 +224,34 @@
"linuxx86-64", "linuxx86-64",
"osxx86-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"
]
} }
] ]
} }
vendordeps/WPILibNewCommands.json
+2 -1
View File
@@ -34,4 +34,5 @@
] ]
} }
] ]
} }
vendordeps/WPILibOldCommands.json
+37
View File
@@ -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"
]
}
]
}
vendordeps/navx_frc.json
+4 -4
View File
@@ -1,17 +1,17 @@
{ {
"fileName": "navx_frc.json", "fileName": "navx_frc.json",
"name": "KauaiLabs_navX_FRC", "name": "KauaiLabs_navX_FRC",
"version": "3.1.413", "version": "4.0.442",
"uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
"mavenUrls": [ "mavenUrls": [
"https://repo1.maven.org/maven2/" "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": [ "javaDependencies": [
{ {
"groupId": "com.kauailabs.navx.frc", "groupId": "com.kauailabs.navx.frc",
"artifactId": "navx-java", "artifactId": "navx-java",
"version": "3.1.413" "version": "4.0.442"
} }
], ],
"jniDependencies": [], "jniDependencies": [],
@@ -19,7 +19,7 @@
{ {
"groupId": "com.kauailabs.navx.frc", "groupId": "com.kauailabs.navx.frc",
"artifactId": "navx-cpp", "artifactId": "navx-cpp",
"version": "3.1.413", "version": "4.0.442",
"headerClassifier": "headers", "headerClassifier": "headers",
"sourcesClassifier": "sources", "sourcesClassifier": "sources",
"sharedLibrary": false, "sharedLibrary": false,